Feature/joint wrench msgs

control_msgs/CMakeLists.txt

@@ -23,6 +23,8 @@ set(msg_files
   msg/JointJog.msg
   msg/JointTolerance.msg
   msg/JointTrajectoryControllerState.msg
+  msg/JointWrenchTrajectory.msg
+  msg/JointWrenchTrajectoryPoint.msg
   msg/MecanumDriveControllerState.msg
   msg/MultiDOFCommand.msg
   msg/MultiDOFStateStamped.msg
@@ -39,6 +41,7 @@ set(action_files
   action/FollowJointTrajectory.action
   action/GripperCommand.action
   action/JointTrajectory.action
+  action/FollowJointWrenchTrajectory.action
   action/PointHead.action
   action/SingleJointPosition.action
 )

control_msgs/action/FollowJointWrenchTrajectory.action

@@ -0,0 +1,54 @@
+# The joint trajectory to follow
+control_msgs/JointWrenchTrajectory trajectory
+
+# Tolerances for the trajectory.  If the measured joint values fall
+# outside the tolerances the trajectory goal is aborted.  Any
+# tolerances that are not specified (by being omitted or set to 0) are
+# set to the defaults for the action server (often taken from the
+# parameter server).
+
+# Tolerances applied to the joints as the trajectory is executed.  If
+# violated, the goal aborts with error_code set to
+# PATH_TOLERANCE_VIOLATED.
+control_msgs/JointTolerance[] path_tolerance
+
+# To report success, the joints must be within goal_tolerance of the
+# final trajectory value.  The goal must be achieved by time the
+# trajectory ends plus goal_time_tolerance.  (goal_time_tolerance
+# allows some leeway in time, so that the trajectory goal can still
+# succeed even if the joints reach the goal some time after the
+# precise end time of the trajectory).
+#
+# If the joints are not within goal_tolerance after "trajectory finish
+# time" + goal_time_tolerance, the goal aborts with error_code set to
+# GOAL_TOLERANCE_VIOLATED
+control_msgs/JointTolerance[] goal_tolerance
+builtin_interfaces/Duration goal_time_tolerance
+
+---
+int32 error_code
+int32 SUCCESSFUL = 0
+int32 INVALID_GOAL = -1
+int32 INVALID_JOINTS = -2
+int32 OLD_HEADER_TIMESTAMP = -3
+int32 PATH_TOLERANCE_VIOLATED = -4
+int32 GOAL_TOLERANCE_VIOLATED = -5
+int32 TRAJECTORY_ABORTED = -6
+
+# Human readable description of the error code. Contains complementary
+# information that is especially useful when execution fails, for instance:
+# - INVALID_GOAL: The reason for the invalid goal (e.g., the requested
+#   trajectory requires a command that differ significantly from the
+#   robot's current state).
+# - INVALID_JOINTS: The mismatch between the expected controller joints
+#   and those provided in the goal.
+# - PATH_TOLERANCE_VIOLATED and GOAL_TOLERANCE_VIOLATED: Which joint
+#   violated which tolerance, and by how much.
+string error_string
+
+---
+std_msgs/Header header
+string[] joint_names
+control_msgs/JointWrenchTrajectoryPoint desired
+control_msgs/JointWrenchTrajectoryPoint actual
+control_msgs/JointWrenchTrajectoryPoint error

control_msgs/msg/JointWrenchTrajectory.msg

@@ -0,0 +1,10 @@
+# The header is used to specify the reference time for the trajectory durations
+std_msgs/Header header
+
+# The names of the active joints in each trajectory point. These names are
+# ordered and must correspond to the values in each trajectory point.
+string[] joint_names
+
+# Array of trajectory points, which describe the joint positions, velocities,
+# accelerations and/or efforts, and task space wrenches at each time point.
+control_msgs/JointWrenchTrajectoryPoint[] points

control_msgs/msg/JointWrenchTrajectoryPoint.msg

@@ -0,0 +1,7 @@
+# A joint-wrench trajectory point defines joint positions, joint velocities, joint accelerations, joint efforts, and a task space wrench, typically used by controllers.
+# Note: The time_from_start field is included in the joint trajectory point.
+trajectory_msgs/JointTrajectoryPoint point
+
+# The wrench is expressed in this reference frame.
+string wrench_frame
+geometry_msgs/Wrench wrench