diff --git a/control_msgs/CMakeLists.txt b/control_msgs/CMakeLists.txt index 6207cc0..ba855e9 100644 --- a/control_msgs/CMakeLists.txt +++ b/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 ) diff --git a/control_msgs/action/FollowJointWrenchTrajectory.action b/control_msgs/action/FollowJointWrenchTrajectory.action new file mode 100644 index 0000000..16a0a50 --- /dev/null +++ b/control_msgs/action/FollowJointWrenchTrajectory.action @@ -0,0 +1,59 @@ +# 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. +# - TRAJECTORY_ABORTED: Indicates that the trajectory was aborted during +# execution (e.g., another trajectory was sent for execution, or a +# problem occurred). +string error_string + +--- +std_msgs/Header header +string[] joint_names +control_msgs/JointWrenchTrajectoryPoint desired +control_msgs/JointWrenchTrajectoryPoint actual +control_msgs/JointWrenchTrajectoryPoint error +# the currently used point from trajectory.points array +int32 index diff --git a/control_msgs/msg/JointWrenchTrajectory.msg b/control_msgs/msg/JointWrenchTrajectory.msg new file mode 100644 index 0000000..92a10c3 --- /dev/null +++ b/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 diff --git a/control_msgs/msg/JointWrenchTrajectoryPoint.msg b/control_msgs/msg/JointWrenchTrajectoryPoint.msg new file mode 100644 index 0000000..034f6da --- /dev/null +++ b/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 JointTrajectoryPoint. +trajectory_msgs/JointTrajectoryPoint point + +# The wrench is expressed in this reference frame. +string wrench_frame +geometry_msgs/Wrench wrench