Torso joint is suddenly going down

[jontromanab]

Hello,

I am trying to execute trajectories on both the arm and arm_torso group. I noticed something weird. When I am planning with arm_torso or arm individually, the robot is doing everything perfectly. But when I am changing the joint group to arm just after executing a trajectory on arm_torso, the robot suddenly drops its torso_lift_joint to '0'. It is happening with MoveIT, as well as executing custom trajectories. (If you just change the group to 'arm' from 'arm_torso' and execute any trajectory in MoveIT, the bug will show up)

I have seen this behaviour in Gazebo. I have a real Fetch robot, but I can not examine this on that (I really don't want to hurt it)

Please have a look. I will search further for solutions. Thank you

[mikeferguson]

So, I finally realized what is going on here, but forgot to post:

• When you send a command to arm_with_torso, it commands all 7 joints of the arm, and the torso lift joint. When the trajectory ends, the controller continues to publish joint position commands to all of the 8 joints since "stop_with_action" parameter is set to false.
• When you then send a command to the arm group alone, the arm_with_torso controller will stop running since you switch to the arm controller. This means nothing is commanding the torso joint to hold position, hence the drop.

The real fix here would be to have a multi-group controller that does both the arm, arm_with_torso, and torso groups and knows to continue controlling the other joints when switching to a different group.

The workaround for now would be to publish a position command to the torso group when you switch to doing arm alone (or always plan with the torso if you are using MoveIt)

@erelson this was the ticket we talked about a while ago that I said I had finally figured out.