Add messages for motion primitives

control_msgs/CMakeLists.txt

@@ -24,6 +24,9 @@ set(msg_files
   msg/JointTolerance.msg
   msg/JointTrajectoryControllerState.msg
   msg/MecanumDriveControllerState.msg
+  msg/MotionArgument.msg
+  msg/MotionPrimitive.msg
+  msg/MotionPrimitiveSequence.msg
   msg/MultiDOFCommand.msg
   msg/MultiDOFStateStamped.msg
   msg/PidState.msg
@@ -35,10 +38,11 @@ set(msg_files
 )
 
 set(action_files
-  action/ParallelGripperCommand.action
+  action/ExecuteMotionPrimitiveSequence.action
   action/FollowJointTrajectory.action
   action/GripperCommand.action
   action/JointTrajectory.action
+  action/ParallelGripperCommand.action
   action/PointHead.action
   action/SingleJointPosition.action
 )

control_msgs/action/ExecuteMotionPrimitiveSequence.action

@@ -0,0 +1,13 @@
+# A list of MotionPrimitive messages that are executed in sequence. They don't have to be of the
+# same type, as long as the robot supports executing them one after another.
+MotionPrimitiveSequence trajectory
+---
+int32 SUCCESSFUL = 0
+int32 INVALID_GOAL = -1
+int32 OLD_HEADER_TIMESTAMP = -3
+
+int32 error_code
+string error_string
+---
+# Index of the currently executed motion primitive in the sequence.
+uint8 current_primitive_index

control_msgs/msg/MotionArgument.msg

@@ -0,0 +1,4 @@
+# Key Value pair to be used in MotionPrimitive messages
+
+string argument_name
+float64 argument_value

control_msgs/msg/MotionPrimitive.msg

@@ -0,0 +1,23 @@
+int8 UNKNOWN=-1
+int8 LINEAR_JOINT=0 # Often referred as PTP
+int8 LINEAR_CARTESIAN=50 # Often referred as LIN
+int8 CIRCULAR_CARTESIAN=51 # Often referred as CIRC
+# potentially more, such as spline motion
+
+int8 type -1 # one of the above
+
+float64 blend_radius
+
+# MotionArguments are meant to be used as vendor-specific or use-case-specific arguments to
+# MotionPrimitive messages. This is to avoid bloating the MotionPrimitive message with all possible
+# uses cases but rather add specific data using as a list of key-value pairs, which is what this
+# message represents.
+MotionArgument[] additional_arguments # (max) velocity, (max) acceleration, efficiency
+
+# Targets should be either specified through joint configurations or Cartesian poses.
+# Depending on the motion type and implementation, there might be multiple poses allowed.
+# For example, circular motions are often specified as via and target
+# Usually one of these is empty, in some edge cases even both can be empty, when all input is given
+# in the more flexible MotionArgument[] field.
+geometry_msgs/PoseStamped[] poses
+float64[] joint_positions

control_msgs/msg/MotionPrimitiveSequence.msg

@@ -0,0 +1 @@
+MotionPrimitive[] motions