Cyclo Motion Controller

This guide shows how to run the Cyclo Motion Controller from cyclo_control on AI Worker.

cyclo_motion_controller is the software layer that acts like the robot's motion interpreter. You give it an easier command, such as an end-effector target pose or a joint target, and it computes the joint trajectories that the real robot should follow.

Its QP(Quadratic Programming)-based controller is especially useful because it does not only track the command, but also tries to keep the motion safe at the same time by considering limits and constraints such as joint range, joint velocity, and self-collision avoidance. In practice, you use it when you want to command the robot by target poses or joint commands while still relying on the controller to generate safe motion.

DANGER

⚠️ The controller only provides self-collision avoidance, and it is not guaranteed in all situations. Always operate the robot carefully and avoid fast or sudden movements.

Supported Controllers

• controller_type:=movel(Default): Generates interpolated arm motion from the current hand pose to the requested goal pose.
• controller_type:=movej: Receives raw joint trajectories and republishes safer filtered trajectories for the robot.

Understanding MoveL and MoveJ

MoveL and MoveJ are not just "send one target and hope the robot gets there." They are motion commands that tell the controller to generate an interpolated motion from the robot's current state to the requested goal over a given time.

• MoveL means "move in a Cartesian line." You command a target hand pose and an interpolation time, and the controller generates a smooth motion that keeps the end-effector on a straight (linear) path from the current pose to the goal.
• MoveJ means "move in joint space." You command target joint values and an interpolation time, and the controller generates a smooth motion from the current joint configuration toward those values. Because the interpolation happens in joint space, the end-effector path is generally not a straight line and may appear curved.

This is different from a simple pose or joint command that only describes the desired target state. MoveL and MoveJ are higher-level motion commands because they also imply a transition from the current state to the goal, including how long that transition should take.

Prerequisites

• Complete the hardware and software steps in the Setup Guide.
• In the default ai_worker Docker container environment, clone cyclo_control into ~/ros2_ws/src, then follow the installation steps in the README.
• Ensure the robot is on level ground with enough clearance to move both arms safely.
• Make sure the emergency stop button is always within reach during operation.
• Connect to the robot PC and make sure the battery is charged.

Bring Up the Robot

Start the AI Worker follower bringup first. Run this on the robot PC:

cd ~/ai_worker
./docker/container.sh enter
ros2 launch ffw_bringup ffw_sg2_follower_ai.launch.py

Keep that terminal running. Open a new terminal for the motion controller commands below.

Launch MoveL Controller

Open two new terminals and run the same environment setup in both:

cd ~/ai_worker
./docker/container.sh enter
source /opt/ros/jazzy/setup.bash
source ~/ros2_ws/install/setup.bash

1. In the first terminal, launch the default movel controller:

   ros2 launch cyclo_motion_controller_ros ai_worker_controller.launch.py controller_type:=movel

2. For marker-based control in RViz, relaunch it with start_interactive_marker:=true:

   ros2 launch cyclo_motion_controller_ros ai_worker_controller.launch.py controller_type:=movel start_interactive_marker:=true

3. For handover-style motions where the two grippers may intentionally come into contact, relaunch it with disable_gripper_collisions:=true:

   ros2 launch cyclo_motion_controller_ros ai_worker_controller.launch.py controller_type:=movel start_interactive_marker:=true disable_gripper_collisions:=true

4. If you use marker-based control, start RViz if it is not already running:

   rviz2

5. In RViz, set the fixed frame to base_link and add these displays:
   • RobotModel
   • TF
   • InteractiveMarkers

When start_interactive_marker:=true, the launch file starts two interactive markers:

• right_goal_marker, which publishes MoveL commands to /r_goal_move
• left_goal_marker, which publishes MoveL commands to /l_goal_move

You can use the movel controller in two ways:

• Move the interactive markers in RViz to send new right and left Cartesian goals.
• Publish MoveL commands directly to /r_goal_move and /l_goal_move.

TIP

The default movel controller plus interactive marker is usually the easiest way to test the motion controller because the interactive markers send MoveL commands directly and the controller handles inverse kinematics, constraint handling, and trajectory publishing.

ros2 topic pub --once /r_goal_move robotis_interfaces/msg/MoveL "{
  pose: {
    header: {frame_id: 'base_link'},
    pose: {
      position: {x: 0.35, y: -0.20, z: 0.85},
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
    }
  },
  time_from_start: {sec: 2, nanosec: 0}
}"

ros2 topic pub --once /l_goal_move robotis_interfaces/msg/MoveL "{
  pose: {
    header: {frame_id: 'base_link'},
    pose: {
      position: {x: 0.35, y: 0.20, z: 0.85},
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
    }
  },
  time_from_start: {sec: 2, nanosec: 0}
}"

movel uses the time_from_start field as the interpolation duration for the Cartesian motion.

Launch MoveJ Controller

This controller is used to apply a safety filter to raw joint trajectory commands published for the follower arms.

ros2 launch cyclo_motion_controller_ros ai_worker_controller.launch.py controller_type:=movej

For handover-style motions where the two grippers may intentionally come into contact, relaunch it with disable_gripper_collisions:=true:

ros2 launch cyclo_motion_controller_ros ai_worker_controller.launch.py controller_type:=movej disable_gripper_collisions:=true

It subscribes to:

• /leader/joint_trajectory_command_broadcaster_right/raw_joint_trajectory
• /leader/joint_trajectory_command_broadcaster_left/raw_joint_trajectory

and republishes filtered outputs to:

• /leader/joint_trajectory_command_broadcaster_right/joint_trajectory
• /leader/joint_trajectory_command_broadcaster_left/joint_trajectory

Example commands:

ros2 topic pub --once /leader/joint_trajectory_command_broadcaster_right/raw_joint_trajectory trajectory_msgs/msg/JointTrajectory "{
  joint_names: ['arm_r_joint1', 'arm_r_joint2', 'arm_r_joint3', 'arm_r_joint4', 'arm_r_joint5', 'arm_r_joint6', 'arm_r_joint7', 'gripper_r_joint1'],
  points: [
    {
      positions: [0.3, -0.2, 0.1, 0.0, 0.2, -0.1, 0.0, 0.02],
      time_from_start: {sec: 3, nanosec: 0}
    }
  ]
}"

ros2 topic pub --once /leader/joint_trajectory_command_broadcaster_left/raw_joint_trajectory trajectory_msgs/msg/JointTrajectory "{
  joint_names: ['arm_l_joint1', 'arm_l_joint2', 'arm_l_joint3', 'arm_l_joint4', 'arm_l_joint5', 'arm_l_joint6', 'arm_l_joint7', 'gripper_l_joint1'],
  points: [
    {
      positions: [-0.3, 0.2, -0.1, 0.0, -0.2, 0.1, 0.0, 0.02],
      time_from_start: {sec: 3, nanosec: 0}
    }
  ]
}"

Launch Arguments

• controller_type: Selects movel, movej, vr, or leader.
• start_interactive_marker: Starts interactive markers for the movel controller.
• base_frame: Frame used for the interactive markers. Default: base_link.
• right_controlled_link, left_controlled_link: Controlled link names for the right and left markers.
• right_movel_topic, left_movel_topic: MoveL topics published by the right and left markers.
• disable_gripper_collisions: Disables collision checking only between the left and right grippers. Default: false.
• reactivate_service: Reactivation service used by the vr and leader modes. Default: /reactivate.
• config_file: Path to controller configuration yaml file.
• follower_urdf_path, follower_srdf_path: Override the follower robot model files.
• leader_urdf_path: Override the leader robot model file.

Key Topics and Services

Common

• /joint_states: Measured robot state required by all controller modes
• /reactivate: Service used to arm or re-arm the vr controller

MoveL

• /r_goal_move: Right arm MoveL command topic
• /l_goal_move: Left arm MoveL command topic
• /r_gripper_pose: Published current right gripper pose
• /l_gripper_pose: Published current left gripper pose

MoveJ

• /leader/joint_trajectory_command_broadcaster_right/raw_joint_trajectory: Raw right-arm moveJ input
• /leader/joint_trajectory_command_broadcaster_left/raw_joint_trajectory: Raw left-arm moveJ input
• /leader/joint_trajectory_command_broadcaster_right/joint_trajectory: Filtered or tracked right-arm output trajectory
• /leader/joint_trajectory_command_broadcaster_left/joint_trajectory: Filtered or tracked left-arm output trajectory

Troubleshooting

• If the markers do not appear in RViz after setting start_interactive_marker:=true, check that the InteractiveMarkers display is added and that the base_frame is set correctly.(default set to base_link)
• If the movel controller does not move, check the terminal log to see whether /joint_states is updating and whether MoveL commands are arriving on /r_goal_move and /l_goal_move.
• If movej does not respond, confirm the raw trajectory topics are receiving commands and that each command includes correct joint names.

Controller Parameters

The main parameters live in cyclo_motion_controller_ros/config/ai_worker_config.yaml. The file is divided by controller name, so you usually tune only the block that matches the controller mode you are running.

ai_worker_movel_controller

• control_frequency, time_step: Main control loop speed.
• trajectory_time: Time field used when publishing output joint trajectories.
• kp_position, kp_orientation: Cartesian tracking gains for end-effector position and orientation.
• weight_position, weight_orientation: Relative importance of position and orientation tracking in the QP solve.
• weight_damping: Regularization term that discourages unnecessarily large joint velocities.
• collision_buffer, collision_safe_distance: Safety margins used for collision avoidance.
• slack_penalty: Cost applied when the solver must relax constraints. Larger values enforce constraints more strictly.
• cbf_alpha: Responsiveness of the barrier constraint. Larger values make the constraint act more like a full brake near a cliff, affecting the control input later and more abruptly as the motion gets very close to the constraint boundary.
• joint_states_topic: Source of measured robot state.
• right_movel_topic, left_movel_topic: Right and left MoveL command input topics.
• right_traj_topic, left_traj_topic: Right and left arm trajectory output topics.
• lift_topic, lift_vel_bound: Lift command path settings. When lift_vel_bound is 0, the controller does not consider lift joint motion.
• r_gripper_pose_topic, l_gripper_pose_topic: Published current gripper pose topics.
• r_gripper_name, l_gripper_name: Link names used to compute the gripper pose.
• right_gripper_joint, left_gripper_joint: Gripper joint names preserved in published trajectories.
• controller_error_topic: Published controller error topic.

ai_worker_movej_controller

• control_frequency, time_step: Main control loop speed.
• trajectory_time: Time field used when publishing output joint trajectories.
• kp_joint: Joint-space tracking gain for the moveJ target.
• weight_tracking: Relative importance of joint tracking in the QP solve.
• weight_damping: Regularization term that discourages unnecessarily large joint velocities.
• collision_buffer, collision_safe_distance: Safety margins used for collision avoidance.
• slack_penalty: Cost applied when the solver must relax constraints. Larger values enforce constraints more strictly.
• cbf_alpha: Responsiveness of the barrier constraint. Larger values make the constraint act more like a full brake near a cliff, affecting the control input later and more abruptly as the motion gets very close to the constraint boundary.
• joint_states_topic: Source of measured robot state.
• right_traj_topic, left_traj_topic: Raw right and left joint trajectory input topics.
• right_traj_filtered_topic, left_traj_filtered_topic: Filtered right and left output trajectory topics.
• right_gripper_joint, left_gripper_joint: Gripper joint names preserved from the input command.

Safety and Usage Tips

DANGER

⚠️ The controller only provides self-collision avoidance, and it is not guaranteed in all situations. Operate the robot carefully and avoid fast or sudden movements.

• Keep people, cables, and nearby objects clear before sending commands.
• Start with small motions first to confirm the model, frames, and topics are configured correctly.

• Always stay within reach of the emergency stop during operation.
• Stop the controller immediately with Ctrl+C if it is safe to do so and the robot behaves unexpectedly.