Implementing simple systems (e.g. pendulum, spring-damper,...)


#1

Hello there,

I am trying to implement a simple model at the NRP to try out a neural controller we are developing with simple known physics. Ideally, I would like to implement a coupled oscillator; so two bodies connected by a spring damper to each other and to fixed points in the environment. However, I am struggling with finding a good way to implement it.

I implemented two masses from a .sdf-file, but am struggeling with implementing the springs…
Can I use SMACH for that?
Do you have any suggestions on how to deal with that?

How can I apply external forces to each mass of the system, similar to the ones I can manually set to interact with the environment, but I would like to set it at specified points and times triggered by my controller?

I am sorry, if this might be a bit of a stupid question, but I have not too much experience with ROS and would appreciate a hint in the right direction.

Thank you and best regards,
Annika


#2

Dear Annika,

Regarding the question about springs modelling, you may find this Gazebo post useful.

Regarding the question as how to apply external forces, you can use the ROS service named /gazebo/apply_body_wrench.
Here are two examples in the context of the NRP, one used in SMACH StateMachine script, the other in a Transfer Function script: $HBP/Experiments/tutorial_baseball_exercise/throw_ball.exd and $HBP/dvs_robot_head/move_target.py.

Best regards,
Luc


#3

Dear Luc,

Thank you for the reply.

Is there a NRP project that already uses the ROS topic /gazebo/apply_body_wrench because I already struggeled on getting that to properly working a while back?

I had already read the Gazebo post about the springs, and I have implemented a “spring” as a prismatic joint with stiffness and damping, but I struggeled in connecting two bodies with a spring, because each of the masses needs to be an individual body I can apply force on…
Therefore, my idea was to realize the springs through SMACH, but I am unsure if this is possible or a useful idea?

Thank you very much again for your help,
Annika


#4

Hi Annika,

From what I see in the apply_body_wrench documentation http://docs.ros.org/jade/api/gazebo_msgs/html/srv/ApplyBodyWrench.html
you can indeed apply the wrench per body, as long as you have defined two separate models in the sdf. Then connecting the two bodies with a joint should now be hard. If you could provide some more specifics about the problems that you encountered with the service, it would be easier to track down the problem.

Best,
Manos


#5

Hello Manos,

I have managed to create a force topic by adding a plugin in the .sdf file. Through that I can command forces to a body via ros pub and that is working. However, I have trouble sending the same data through the NRP.

Through the ros terminal I can send commands with

rostopic pub /force1 geometry_msgs/Wrench ‘{force: {x: 0.0, y: 0.0, z: -30.0}, torque: {x: 0.0,y: 0.0,z: 0.0}}’

When I create a Subscriber in the NRP:

from geometry_msgs.msg import Wrench
@nrp.MapRobotSubscriber(“forces”, Topic("/force1", Wrench))

I can also read out the data I publish through ros. However, when creating a Publisher in the NRP:

@nrp.MapRobotPublisher(“forces”, Topic("/force1", Wrench))

I cannot manage to send the corresponding data through this way.
How do I need to address the topic? I tried various ways like

forces.send_message([0.0, 0.0, 0.0], [0.0, 0.0, 0.0])

but always get the error Invalid number of arguments, args should be ['force', 'torque'] args

I would appreciate if you could tell me what I am doing wrong and how I properly address the topic through the NRP and send commands?

Thank you very much,
Annika


#6

Hi Annika,

That is because the expected type of the send message argument is Wrench. You could try something like this

from geometry_msgs.msg import Wrench, Vector3
@nrp.MapRobotPublisher(“forces”, Topic("/force1", Wrench))
def apply_wrench (t,forces):
force = Vector3(0.0 ,0.0 ,0.0)
torque = Vector3(0.0 ,0.0 ,0.0)
wrench = Wrench(force, torque)
forces.send_message(wrench)

Hope it helps!

Best,
Manos


#7

Hey Manos,

Thank your for the quick reply.

I have actually tried also sending it like this, but I always get the error

global name ‘Vector3’ is not defined
global name ‘Wrench’ is not defined

even though I do import it like you also said with

from geometry_msgs.msg import Wrench, Vector3

Any idea why this could be?

Greetings,
Annika


#8

Try this:

from geometry_msgs.msg import Wrench, Vector3
@nrp.MapRobotPublisher(“forces”, Topic("/force1", Wrench))
def apply_wrench (t,forces):
from geometry_msgs.msg import Wrench, Vector3
force = Vector3(0.0 ,0.0 ,0.0)
torque = Vector3(0.0 ,0.0 ,0.0)
wrench = Wrench(force, torque)
forces.send_message(wrench)

For some weird reason the import outside of the function does not work, I will take a look at why and get back to you.

Best,
Manos


#9

Hey,

That worked! THANK YOU!!!

What would you suggest is the best way to couple two objects created from different sdf files in this scenario? Through a transfer function? Or use SMACH?

Thank you again for your help,
Annika


#10

Hi Annika,

I am not sure I get the question right, but if you mean two object models coming from two different sdfs and these are just environment objects and not robots, then you can just create a new world sdf and add the two models there. If they are robots, you could add them to the .bibi file without the need to have a single sdf. If you want to manipulate objects in the environment (i.e. changing colours, moving objects around at certain times etc) then the most convenient way would indeed be a SMACH. If you could elaborate a bit more on the scenario we could provide more details. I would take a look at the SMACH tutorial here https://developer.humanbrainproject.eu/docs/projects/HBP%20Neurorobotics%20Platform/1.2/nrp/user_manual/tutorials/experiment/state_machines.html to see the different functionalities that SMACHs provide.

Best,
Manos