8. Commanding the robot (sim and real)

Part I: The ROS 2 stack · §8 of 43

All of the methods below work the same way with mock and real hardware. On the real robot, start with small, slow motions (§12).

8.1 Trajectory action (recommended)

Action servers:

Action	Type
`/left_joint_trajectory_controller/follow_joint_trajectory`	`control_msgs/action/FollowJointTrajectory`
`/right_joint_trajectory_controller/follow_joint_trajectory`	same
`/left_gripper_controller/follow_joint_trajectory`	same
`/right_gripper_controller/follow_joint_trajectory`	same

What happens during one FollowJointTrajectory goal. The result is SUCCEEDED when time runs out, whether or not the arm got there: all tolerances are 0, which means not checked.

sequenceDiagram
  participant C as your client (CLI / rclpy)
  participant T as joint_trajectory_controller
  participant H as hardware interface
  C->>T: send_goal (all 7 joints, points, time_from_start)
  T-->>C: goal accepted
  loop every control cycle until the last time_from_start
    T->>H: position command
    H-->>T: joint state
  end
  T-->>C: result SUCCEEDED (not checked against the real position)

Left arm, all joints to 0.15 rad in 3 s:

ros2 action send_goal /left_joint_trajectory_controller/follow_joint_trajectory \
  control_msgs/action/FollowJointTrajectory \
  '{trajectory: {joint_names: [openarm_left_joint1, openarm_left_joint2, openarm_left_joint3, openarm_left_joint4, openarm_left_joint5, openarm_left_joint6, openarm_left_joint7],
    points: [{positions: [0.15, 0.15, 0.15, 0.15, 0.15, 0.15, 0.15], time_from_start: {sec: 3}}]}}'

Right arm, multi-waypoint (out to the “hands up” pose, then home):

ros2 action send_goal /right_joint_trajectory_controller/follow_joint_trajectory \
  control_msgs/action/FollowJointTrajectory \
  '{trajectory: {joint_names: [openarm_right_joint1, openarm_right_joint2, openarm_right_joint3, openarm_right_joint4, openarm_right_joint5, openarm_right_joint6, openarm_right_joint7],
    points: [
      {positions: [0, 0, 0, 1.0, 0, 0, 0], time_from_start: {sec: 3}},
      {positions: [0, 0, 0, 2.0, 0, 0, 0], time_from_start: {sec: 6}},
      {positions: [0, 0, 0, 0,   0, 0, 0], time_from_start: {sec: 10}}]}}' --feedback

Gripper (metres, 0 = closed, 0.044 = open):

# open
ros2 action send_goal /left_gripper_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory \
  '{trajectory: {joint_names: [openarm_left_finger_joint1], points: [{positions: [0.044], time_from_start: {sec: 1}}]}}'
# close
ros2 action send_goal /left_gripper_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory \
  '{trajectory: {joint_names: [openarm_left_finger_joint1], points: [{positions: [0.0], time_from_start: {sec: 1}}]}}'

Rules:

Include all 7 joints of the arm (allow_partial_joints_goal: false).
Order of joint_names is free, but positions must match it.
Check interpolation_method first (§4). With splines (zeus), time_from_start sets the speed: make it long enough, since a short time means a fast, violent move on the real robot. With none (upstream default), the joint holds still for the whole time_from_start and then jumps to the target, whatever the time: never use none for single-point or sparse goals on hardware.
Cancel a running goal: Ctrl+C on ros2 action send_goal cancels it. The controller then holds the current position (decelerate_on_cancel: false, so it stops immediately rather than ramping down).
SUCCEEDED only means “time ran out” (tolerances are 0 = disabled). Check /joint_states.
Check each target against the per-arm limits, and remember ”+” isn’t “forward”: see the direction map (§3). On zeus, openarm_jmove.py does these checks for you (§8.7).

8.2 Trajectory topic (fire-and-forget)

Each JTC also listens on /<controller>/joint_trajectory (trajectory_msgs/msg/JointTrajectory):

ros2 topic pub --once /left_joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory \
  '{joint_names: [openarm_left_joint1, openarm_left_joint2, openarm_left_joint3, openarm_left_joint4, openarm_left_joint5, openarm_left_joint6, openarm_left_joint7],
    points: [{positions: [0, 0, 0, 0, 0, 0, 0], time_from_start: {sec: 3}}]}'

No feedback and no result. A new message replaces the running trajectory. Useful for streaming from a teleop node.

8.3 Python (rclpy) action client

Save as move_arm.py and run with python3 move_arm.py in a sourced shell:

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from builtin_interfaces.msg import Duration
 
 
class ArmCommander(Node):
    def __init__(self, side: str):
        super().__init__(f"{side}_arm_commander")
        self.side = side
        self.joints = [f"openarm_{side}_joint{i}" for i in range(1, 8)]
        self.client = ActionClient(
            self, FollowJointTrajectory,
            f"/{side}_joint_trajectory_controller/follow_joint_trajectory")
 
    def move(self, waypoints, seconds_per_point=3.0):
        """waypoints: list of 7-element joint position lists (rad)."""
        goal = FollowJointTrajectory.Goal()
        goal.trajectory.joint_names = self.joints
        for k, q in enumerate(waypoints, start=1):
            t = seconds_per_point * k
            goal.trajectory.points.append(JointTrajectoryPoint(
                positions=list(q),
                time_from_start=Duration(sec=int(t), nanosec=int((t % 1) * 1e9))))
 
        self.client.wait_for_server()
        send_future = self.client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, send_future)
        handle = send_future.result()
        if not handle.accepted:
            self.get_logger().error("Goal rejected")
            return False
        result_future = handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)
        res = result_future.result().result
        self.get_logger().info(f"error_code={res.error_code} {res.error_string}")
        return res.error_code == 0
 
 
def main():
    rclpy.init()
    left = ArmCommander("left")
    left.move([[0, 0, 0, 0.5, 0, 0, 0],
               [0, 0, 0, 0.0, 0, 0, 0]])
    left.destroy_node()
    rclpy.shutdown()
 
 
if __name__ == "__main__":
    main()

To move both arms at the same time, create two clients and send both goals before waiting on either result.

8.4 Reading state

ros2 topic echo /joint_states                                   # name/position/velocity/effort, all 16 joints
ros2 topic echo /left_joint_trajectory_controller/controller_state  # reference vs feedback vs error
ros2 topic echo /dynamic_joint_states                           # every state interface
ros2 run tf2_ros tf2_echo world openarm_left_link7              # Cartesian pose of a link

On the real robot, /joint_states effort for arm joints is real motor torque (Nm). For the gripper, velocity and effort are always 0.

8.5 Forward position controller (streaming, advanced)

Launch with robot_controller:=forward_position_controller, then:

ros2 topic pub --once /left_forward_position_controller/commands std_msgs/msg/Float64MultiArray \
  '{data: [0, 0, 0, 0.2, 0, 0, 0]}'

The motor setpoint jumps to the value instantly: there is no interpolation. With the stiff MIT gains this is a fast, violent step on the real robot. Only use it from your own node streaming small increments at a high, steady rate (e.g. ≥100 Hz, a few mrad per step). Never use it with ros2 topic pub on hardware.

8.6 Switching controllers at runtime

ros2 control list_controllers
ros2 control load_controller left_forward_position_controller --set-state inactive
ros2 control switch_controllers --deactivate left_joint_trajectory_controller --activate left_forward_position_controller
# and back
ros2 control switch_controllers --deactivate left_forward_position_controller --activate left_joint_trajectory_controller

Two controllers can’t claim the same command interface at once. Always deactivate one and activate the other in a single switch_controllers call.

8.7 Helper scripts on zeus

In Prandium/robot_scripts/ on the laptop = /root/scripts/ in the container (§5). Each prints what it will do and asks y/N before anything moves, and Ctrl+C cancels the motion (the arm then holds where it is). They handle Ctrl+C themselves (rclpy.init(signal_handler_options=SignalHandlerOptions.NO)), because by default rclpy shuts its connection down on Ctrl+C and the goal could then no longer be cancelled.

Script	Usage	What it does
`openarm_launch.sh`	`bash /root/scripts/openarm_launch.sh real\|sim`	Starts the bringup with zeus’s settings (§6).
`openarm_stop.sh`	`bash /root/scripts/openarm_stop.sh`	Safe stop of the real robot (§12.4).
`openarm_jmove.py`	`python3 /root/scripts/openarm_jmove.py <left\|right> <joint 1-7> <target_rad> [seconds=10]`	Moves one joint; the others keep their current positions. Refuses targets outside that arm’s limits (0.05 rad margin; exactly 0, the home pose, is always allowed), steps over 0.35 rad from the current position, average speeds over 0.1 rad/s, durations under 5 s. Prints the final position and error.
`openarm_jsweep.py`	`python3 /root/scripts/openarm_jsweep.py <left\|right\|both> [amplitude=0.3] [seconds=5]`	Tests joints 1–7 in turn: to the test position, 1 s hold, back. Safe directions (below). Same checks; stops if a joint misses its target by more than 0.1 rad. Prints the expected direction for each joint.
`openarm_demo.py`	`python3 /root/scripts/openarm_demo.py [time_scale]`	~55 s choreography for both arms and grippers (below).

openarm_jsweep.py test directions, chosen so nothing swings toward the body:

Joint	Left	Right
1	−0.3 (forward)	+0.3 (forward)
2	−0.3 (outward)	+0.3 (outward)
3, 4, 5, 7	+0.3	+0.3
6	+0.3 (outward)	−0.3 (outward)

openarm_demo.py choreography (times at time_scale 1):

Time (s)	Stage
0 → 6	both forearms come up in front (“ready”: joint4 = 1.0)
6 → 14	grippers open, close, open, close
14 → 25	forearms twist (joint5 ±0.6, mirrored), then back to centre
25 → 43	right hand up (joint1 +0.4, joint4 1.6), two waves (joint3 ±0.35)
43 → 55	back to “ready”, then down to zero

Every waypoint is checked against the per-arm limits and every segment against an average speed of 0.25 rad/s (the fastest segment is 0.24); the arms must start within 0.1 rad of zero; time_scale must be ≥ 1 (only slower). Waypoints have zero velocity and acceleration, so the controller makes smooth quintic moves. The poses were checked with forward kinematics: the hands stay at least 30 cm apart, in front of the torso, and never lower than when hanging. Run it in sim first; on the real robot start with time_scale 1.5. Going faster than 1 means raising MAX_SPEED and the minimum scale in the script; with an unclamped base, do it in steps.

Prandium Docs

Explorer