Update AdmittanceParameters for multi-tip

* Pass in a list of AdmittanceParameters to the
  FollowJointTrajectoryWithAdmittance action, one per tip.
* Move tip specific fields (absolute_force_torque_threshold) into
  the AdmittanceParameters msg.

Author: dyackzan

moveit_studio_msgs/moveit_pro_controllers_msgs/action/FollowJointTrajectoryWithAdmittance.action

@@ -14,17 +14,10 @@ float64[] path_tolerance
 # If empty, default tolerances from configs will be used.
 float64[] goal_tolerance
 
-# The absolute force or torque threshold value for each Cartesian axis at which
-# the controller will stop executing if exceeded.
-# If the absolute value of a measured FTS value exceeds the corresponding
-# threshold, the trajectory goal is aborted with 'error_code' set to
-# FORCE_TORQUE_THRESHOLD_EXCEEDED.
-# If empty, default threshold values from configs will be used.
-float64[] absolute_force_torque_threshold
-
-# "Joint Trajectory with Admittance Controller" control parameters.
-# These include stiffness, damping, path tolerances, etc.
-moveit_pro_controllers_msgs/AdmittanceParameters admittance_parameters
+# "Joint Trajectory with Admittance Controller" admittance parameters.
+# These include mass, stiffness, damping, selected Cartesian axes, joint damping, force/torque threshold and end
+# effector parameters for each tip.
+moveit_pro_controllers_msgs/AdmittanceParameters[] admittance_parameters
 
 ---
 int32 error_code

moveit_studio_msgs/moveit_pro_controllers_msgs/msg/AdmittanceParameters.msg

@@ -1,11 +1,27 @@
 # Admittance parameters.
-# These are the parameters of a mass-spring-damper model that determines
-# the admittance behavior on each of the Cartesian axes.
+# These are the parameters of a mass-spring-damper model that determine
+# the admittance behavior on each of the Cartesian axes of each tip.
+# Each of the individual mass, spring, and damper vectors have 6 elements, one for each Cartesian axis.
+# They are given in the order: [X, Y, Z, RX, RY, RZ].
 # Use carefully. Certain choices may lead to unstable behavior.
-float64[] mass
-float64[] stiffness
-float64[] damping
+float64[6] mass
+float64[6] stiffness
+float64[6] damping
+bool[6] selected_axes  # Axes in the control frame to control with admittance.
+# Joint space damping. If there are multiple tips and corresponding chains in the Admittance problem, and the chains
+# share joints, then the joint_damping must match in the AdmittanceParameters that correspond to the overlapping joints.
 float64 joint_damping
 
-geometry_msgs/Pose ee_pose_control   # Control frame w.r.t. end-effector frame.
-bool[] selected_axes                 # Axes in the control frame to control with admittance.
+# End-effector frame, mass, and center of gravity (CoG) position.
+string end_effector_frame
+float64 end_effector_mass
+float64 end_effector_cog[]  # X,Y,Z center of gravity in the end-effector frame. If empty, will default to [0.0, 0.0, 0.0].
+geometry_msgs/Pose ee_pose_control  # Control frame w.r.t. end-effector frame. If empty, will default to identity pose.
+
+# The absolute force or torque threshold value for each Cartesian axis at which
+# the controller will stop executing if exceeded.
+# If the absolute value of an axis of the measured FTS wrench exceeds the corresponding
+# threshold, the trajectory goal is aborted with 'error_code' set to
+# FORCE_TORQUE_THRESHOLD_EXCEEDED.
+# If empty, default threshold values from configs will be used.
+float64[] absolute_force_torque_threshold