# Apply a body wrench in a local frame in a link

Hi everybody,

I am trying to apply a body wrench to a simple gazebo model written in SDF. I am running Ubuntu 14.4, with Gazebo 6 and ROS indigo.

I am applying this wrench by calling the rosservice /gazebo/apply_body_wrench as explained in http://answers.ros.org/question/11047... . I'd like to express the wrench in a local frame, defined within a link for example, and not in the world frame of Gazebo. So to test this, I made a MWE you can see below (model in SDF, correctly spawned in Gazebo):

<?xml version="1.0"?>
<sdf version="1.4">
<model name="my_robot_bw">

<pose>0 0 1 0 0 0</pose>

<gravity>false</gravity>

<frame name="frame1">
<pose>0 0 0 0 0 0.785398163</pose>
</frame>

<visual name="visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>

<collision name="collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>

</model>
</sdf>


A box is placed in the origin of the world frame on the ground (no gravity to see the apply force easily). Then I defined a local frame, named "frame1", in the link called "base" that is rotated by 45 degrees on the Z axis respect the wolrd frame. My goal is to apply a wrench to the base link always expressed in this "frame1", so in this case I'd like to apply a force in the frame1::X_axis and see the box moving at 45 degrees respect to gazebo world X and Y axes. So, once I started gazebo with rosrun and correctly spawned this simple model, I was able to call the rosservice with:

rosservice call /gazebo/apply_body_wrench '{body_name: "my_robot_bw::base", reference_frame: "my_robot_bw::base", reference_point: { x: 0, y: 0, z: 0 }, wrench: { force: { x: 1, y: 0, z: 0 }, torque: { x: 0, y: 0, z: 0 } }, start_time: 0, duration: -1 }'


Here two issues arise:

1) I get the error

[ERROR] [1457096195.411824276, 7.968000000]: wrench defined as [base]:[1.000000 0.000000 0.000000, 0.000000 0.000000 0.000000] --> applied as [base]:[1.000000 0.000000 0.000000, 0.000000 0.000000 0.000000]


which seems "normal", according to http://answers.ros.org/question/65077... when you set a reference frame other than empty, map, or world. It's something strange, but it's not a big problem I guess, as long as the service works.

2) The force is applied as expressed in the world frame, so the box starts moving along the world frame X axes, instead of the frame1 X axes. This is the main problem I am having. So, finally, is there something I am doing wrong? Or should I just always use wrench expressed in the world frame and rotate it by "myself" to make it behaves like if it were expressed in the local frame "frame1" ?

Thanks, Marco.

edit retag close merge delete

Sort by » oldest newest most voted

I have exactly the same problem as you. Anyone knows the solution?

The "apply_body_wrench" service just ignores the reference_frame I specified, and only applies wrench in the world frame.

more

Hi, at the moment I haven't solved this problem yet, but there are two "workaround":

1) Rotate the wrench in the world frame, by getting the state of the link and using it to rotate the wrench, and the apply it using 'apply_body_wrench'

( 2016-08-11 02:03:16 -0600 )edit

( 2016-08-11 02:04:41 -0600 )edit

Hi, thanks! I think your comments should work, and I'll try it later myself to see how it goes.

Just curious about why the native "gazebo/apply_body_wrench" service is buggy. This seems not to be a hard thing to fix.

( 2016-08-11 08:54:27 -0600 )edit