[BUG] Arm falls when torso is moved

[fetchrobotics]

[BUG] Arm falls when torso is moved

[mikeferguson]

These are somewhat related, but not entirely.

The original issue is actually pretty simple, but there's not an easy catch all fix for robot_controllers: the first command is given through the "arm_with_torso_controller". This controller controls the 7 joints in the arm and the torso. The second command is given to the torso_controller. When the second command arrives, the drivers have to stop the arm_with_torso_controller (since they both want to command the torso), and thus the arm is no longer commanded. Gravity compensation isn't perfect (especially as robots age and joints reduce their stiction) and so the arm slowly descends.

The issue @ba2603 is seeing is really just that gazebo simulation is even less good than the real world. The twist controller stops after a set time, reverting to just gravity compensation. Gravity compensation is almost certainly still running, but the simulation fails to actually hold the arm in place.

In both cases, the best way to keep then arm where it is, is to call the arm_controller/follow_joint_trajectory with a single trajectory point equal to the current pose of the arm. This will then hold the arm in place rigidly.

[erelson]

Hi @jemi622, this is something that has always existed, not a bug with the robot's software/hardware: It sounds like your code is not telling the arm joints to hold their goal positions after they're reached. Instead, in between commands, the robot is just using the default gravity compensation controller. To get a better idea of what to do, you can probably start from the arm teleop code, and the code that "stops" motors from holding position:

• https://github.com/fetchrobotics/fetch_ros/blob/melodic-devel/fetch_teleop/src/joystick_teleop.cpp
• https://github.com/fetchrobotics/fetch_robots/blob/melodic-devel/fetch_bringup/scripts/controller_reset.py#L85

[ba2603]

@erelson something like this happens when I try to use the joystick_teleop.cpp for Gazebo. The torso works just fine, actually. However, the arm can be moved with the CartesianTwistController, but the script stops the controller 0.5s after the deadman/control buttons are released. At that point, the arm begins to fall.

What has happened? I noticed that the topic /query_controller_states isn’t published, so the “disable motor position holding” button does not work. It seems like the gravity compensation stops working after using the CartesianTwistController. Maybe I am misunderstanding your comment.

Is this because the teleop CPP file wasn’t meant to be used on Gazebo?

Thank you for your time. Please let me know if this isn’t relevant enough to the original post.

[ba2603]

@mikeferguson that makes sense! Thank you for your prompt response. I appreciate your time, especially given the circumstances of the last few weeks.

I will definitely give it a try. I'm thinking to modify the

virtual bool stop()

method in the ArmTeleop class to publish that single point. If I run into trouble with the torso, I will use the arm_with_torso_controller instead to make sure that stays fixed as well. Perhaps this will turn into a PR for the sake of the uncertain gravity compensation.

All the best, and I hope you and your family stay well!