From b96cd0588c6b2e187f4785afb10a4bba8ca1634d Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Mon, 14 Jul 2025 07:53:00 -0600 Subject: [PATCH] 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. --- ...FollowJointTrajectoryWithAdmittance.action | 15 +++------- .../msg/AdmittanceParameters.msg | 30 ++++++++++++++----- .../msg/TipAdmittanceParameters.msg | 0 3 files changed, 27 insertions(+), 18 deletions(-) create mode 100644 moveit_studio_msgs/moveit_pro_controllers_msgs/msg/TipAdmittanceParameters.msg diff --git a/moveit_studio_msgs/moveit_pro_controllers_msgs/action/FollowJointTrajectoryWithAdmittance.action b/moveit_studio_msgs/moveit_pro_controllers_msgs/action/FollowJointTrajectoryWithAdmittance.action index cd0db38..89bc53b 100644 --- a/moveit_studio_msgs/moveit_pro_controllers_msgs/action/FollowJointTrajectoryWithAdmittance.action +++ b/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 diff --git a/moveit_studio_msgs/moveit_pro_controllers_msgs/msg/AdmittanceParameters.msg b/moveit_studio_msgs/moveit_pro_controllers_msgs/msg/AdmittanceParameters.msg index bfda634..f8b0860 100644 --- a/moveit_studio_msgs/moveit_pro_controllers_msgs/msg/AdmittanceParameters.msg +++ b/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[3] end_effector_cog # X,Y,Z center of gravity in the end-effector frame. +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 diff --git a/moveit_studio_msgs/moveit_pro_controllers_msgs/msg/TipAdmittanceParameters.msg b/moveit_studio_msgs/moveit_pro_controllers_msgs/msg/TipAdmittanceParameters.msg new file mode 100644 index 0000000..e69de29