Apply a body wrench in a local frame in a link

edit

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> 

  <link name="base">

    <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>

  </link>

</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.