How to control one joint while a second one swings?
I have a model with 2 revolute joints spawn from an urdf, and a gazebo-ros plugin to interact with it.
I want to command the position of one joint, and the other should swing freely, but wish to know its position.
So I have a std::vector<gazebo::physics::JointPtr> sim_joints_;
properly initialized with the sdf joints, and in the plugin Update()
I read the two states like
for(unsigned int j=0; j < 2; j++)
{
joint_position_[j] += angles::shortest_angular_distance(joint_position_[j],
sim_joints_[j]->GetAngle(0).Radian());
}
then publish all joint_position_
's, update the joint_position_command
for joint j=1, and finally write
sim_joints_[0]->Update();
sim_joints_[1]->SetPosition(0, joint_position_command);
Joint j=1 moves according to the command, but joint j=0 does not swing. If I don't write anything, i.e. delete the SetPosition()
, or if I use sim_joints_[j=0,1]->Update()
, both joints swing freely.
Seems like writing the position to any joint makes the rest joints to freeze.
Am I missing something? How can achieve the desired behavior?