Calling Joint->SetPosition() not moving joint

asked 2022-05-06 13:13:10 -0500

flyingsubs gravatar image

I have an example world file (at bottom of question). In it I have two links that have a box visual in them. I have a revolute joint connecting them. I have a plugin I wrote that in the update() call attempts to set the position of the joint:

gimbalYaw += .001;
std::cout << "In Update()"<<std::endl;
std::cout<< "Pos: "<<this->model->GetJoint("joint")->Position(0)<<std::endl;
this->model->GetJoint("joint")->SetPosition(0,gimbalYaw);

gimbalYaw is a class variable that just store the current position and increments. I can see that the joint position is being updated via the print out of Position(0). However, looking at the world in the GazeboClient the two boxes remain stationary. I expected for one of them to spin. Am I thinking about the joint wrong, or am I trying to set its rotation incorrectly? I would love to see a simple example of rotating a revolute joint being moved from a plugin to learn from.

The World file:

<?xml version="1.0"?> 
<sdf version="1.4">
  <world name="default">

  <model name="tt">
   <pose>0 0 2 0 0 0</pose>
    <static>true</static>
   <link name="linkA">
    <visual name="a">
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
    </visual>
   </link>
   <link name="linkB">
    <visual name="b">
        <pose>0 0 1 0 0 0</pose>
        <geometry>
          <box>
            <size>2 2 1</size>
          </box>
        </geometry>
    </visual>
   </link>
   <joint name="joint" type="revolute">
     <parent>linkA</parent>
      <child>linkB</child>
      <pose>0 0 4 0 0 0</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-10000000000000000</lower>
          <upper>10000000000000000</upper>
        </limit>
      </axis>
   </joint>
    <plugin name="GimbalModel"
        filename="libGimbalModel.so">
    </plugin>
  </model>

  </world>
</sdf>

I also expected the pose in the joint block to offset the blocks by 4 units, but that is also not happening. I am clearly understanding something about joints, I just don't know what.

edit retag flag offensive close merge delete