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,10 @@
+MotionPrimitiveSequence trajectory
+---
+int32 SUCCESSFUL = 0
+int32 INVALID_GOAL = -1
+int32 OLD_HEADER_TIMESTAMP = -3
+
+int32 error_code
+string error_string
+---
+uint8 current_primitive_index # How far are we in the actual trajectory?

control_msgs/msg/MotionArgument.msg

@@ -0,0 +1,2 @@
+string argument_name
+float64 argument_value

control_msgs/msg/MotionPrimitive.msg

@@ -0,0 +1,17 @@
+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
+
+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
+geometry_msgs/PoseStamped[] poses
+float64[] joint_positions

control_msgs/msg/MotionPrimitiveSequence.msg

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