Support admittance control

[JohnTGZ]

Overview

This PR updates the robot_control_bridge to support Cartesian admittance control as the default Cartesian controller. It makes the following changes:

• Fix stiffness, damping and mass parameters to a default set of parameters that generalize well to all tasks (Freespace and contact). These cannot be changed at runtime via MotionUpdate or JointMotionUpdate messages
• Update limits (kinematic and joint) to match the actual robot setup in phase 2
• Add support for F/T sensor part in robot controller configs to enable admittance control
• Expose critical_mass as a config parameter
• Change default interpolation mode from LINEAR_INTERPOLATION to REFLEXXES.

TODO

• Fix stiffness, damping and mass parameters to a default set of parameters that generalize well to all tasks (Freespace and contact)
• Set default value for USE_FIXED_CONTROL_PARAMETERS to true
• Update changelog flowstate/aic_flowstate_ros_bridge/CHANGELOG.rst
• Update/remove test scripts

[kaushikbalasundar]

Based on how this script handles target_pose_stiffness and target_pose_damping, it seems like the test_joint_targets.launch.py and test_pose_targets.launch.py needs to provide a list as such: [target_pose_stiffness, target_pose_damping] as opposed to the six element list:

target_joint_stiffness": [
175.0,
175.0,
175.0,
125.0,
50.0,
50.0,
]

It might be good to re-verify on hardware that our chosen rotational damping parameters make sense because of this.

[JohnTGZ]

Good catch, sorry I didn't catch this. I will modify the script and we can test it again.

[kaushikbalasundar]

We use this pattern of setting diagonal elements using indices 0, 7, 14, 21, 28, 35 multiple times throughout this file. Consider adding a function to replace this?

inline void SetDiagonal6x6(google::protobuf::RepeatedField<double>* field, const std::vector<double>& diag) {
  field->Resize(36, 0.0);
  for (int i = 0; i < 6; ++i) {
    field->Set(i * 7, diag[i]);
  }
}

[kaushikbalasundar]

Some minor comments.

Could you verify if my understanding of how we use the target_stiffness and target_damping in test_robot_control_bridge.py is correct? If so, I'd like to reverify if the rotational stiffness and damping params make sense on hardware.

[JohnTGZ]

Some minor comments.

Could you verify if my understanding of how we use the target_stiffness and target_damping in test_robot_control_bridge.py is correct? If so, I'd like to reverify if the rotational stiffness and damping params make sense on hardware.

Yes what you mentioned is correct, it would only use the first 2 values of the array and ignore the rest. Thanks for catching this, we should test it again to verify.

I have fixed the issue and addressed your comments in commits e465546 and c2b021b