Integration: ROS 2 MoveIt2, URScript & Real Robot Deployment

This is the final post in the Traditional Manipulation Control series. Across 7 previous posts, we built knowledge from basic kinematics through trajectory types, MoveJ/MoveL, MoveC, blending, motion profiles, to spline and TOPPRA.

Now it is time to bring it all together into a practical workflow: design trajectories in Python, send them to MoveIt2, and execute on a real UR5e robot through ros2_control.

Overall Architecture

Python Script (trajectory design)
    |
    v
MoveIt2 (planning + collision checking)
    |
    v
ros2_control (hardware abstraction)
    |
    v
UR Driver (ur_robot_driver)
    |
    v
UR5e Controller (Real Robot)

Key Components

ROS 2 Environment Setup

# ROS 2 Humble (Ubuntu 22.04)
sudo apt install ros-humble-moveit
sudo apt install ros-humble-ur-robot-driver
sudo apt install ros-humble-ros2-controllers

# Workspace
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# Clone UR driver
git clone -b humble https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git

# Build
cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

MoveIt2 — Planning Framework

OMPL Planners vs Pilz Industrial Planner

MoveIt2 Python API

#!/usr/bin/env python3
"""
MoveIt2 trajectory planning example.
Requires ROS 2 Humble + MoveIt2 + ur_robot_driver.
"""
import rclpy
from rclpy.node import Node
from moveit_msgs.msg import (
    MotionPlanRequest, Constraints, JointConstraint,
    PositionConstraint, OrientationConstraint,
)
from moveit_msgs.srv import GetMotionPlan
from geometry_msgs.msg import PoseStamped
import numpy as np


class TrajectoryPlanner(Node):
    def __init__(self):
        super().__init__('trajectory_planner')
        self.plan_client = self.create_client(
            GetMotionPlan, '/plan_kinematic_path'
        )
        self.plan_client.wait_for_service(timeout_sec=10.0)
        self.get_logger().info('TrajectoryPlanner ready')

    def plan_joint_goal(self, joint_names, joint_values,
                        planner_id='PTP', velocity_scaling=0.5):
        """
        Plan MoveJ (joint space) trajectory.

        Args:
            joint_names: list of joint names
            joint_values: target joint angles (rad)
            planner_id: 'PTP' (Pilz) or 'RRTstar' (OMPL)
            velocity_scaling: 0.0 to 1.0
        """
        request = MotionPlanRequest()
        request.group_name = 'ur_manipulator'
        request.planner_id = planner_id
        request.num_planning_attempts = 5
        request.allowed_planning_time = 5.0
        request.max_velocity_scaling_factor = velocity_scaling
        request.max_acceleration_scaling_factor = velocity_scaling

        constraints = Constraints()
        for name, value in zip(joint_names, joint_values):
            jc = JointConstraint()
            jc.joint_name = name
            jc.position = value
            jc.tolerance_above = 0.001
            jc.tolerance_below = 0.001
            jc.weight = 1.0
            constraints.joint_constraints.append(jc)

        request.goal_constraints.append(constraints)

        future = self.plan_client.call_async(
            GetMotionPlan.Request(motion_plan_request=request)
        )
        rclpy.spin_until_future_complete(self, future)

        response = future.result()
        if response.motion_plan_response.error_code.val == 1:
            self.get_logger().info('Planning succeeded!')
            return response.motion_plan_response.trajectory
        else:
            self.get_logger().error(
                f'Planning failed: {response.motion_plan_response.error_code.val}'
            )
            return None

    def plan_cartesian_goal(self, target_pose, planner_id='LIN',
                            velocity_scaling=0.3):
        """Plan MoveL (Cartesian space) trajectory using Pilz LIN."""
        request = MotionPlanRequest()
        request.group_name = 'ur_manipulator'
        request.planner_id = planner_id
        request.num_planning_attempts = 5
        request.allowed_planning_time = 5.0
        request.max_velocity_scaling_factor = velocity_scaling
        request.max_acceleration_scaling_factor = velocity_scaling

        constraints = Constraints()
        pc = PositionConstraint()
        pc.header = target_pose.header
        pc.link_name = 'tool0'
        pc.weight = 1.0
        constraints.position_constraints.append(pc)

        oc = OrientationConstraint()
        oc.header = target_pose.header
        oc.link_name = 'tool0'
        oc.orientation = target_pose.pose.orientation
        oc.absolute_x_axis_tolerance = 0.001
        oc.absolute_y_axis_tolerance = 0.001
        oc.absolute_z_axis_tolerance = 0.001
        oc.weight = 1.0
        constraints.orientation_constraints.append(oc)

        request.goal_constraints.append(constraints)

        future = self.plan_client.call_async(
            GetMotionPlan.Request(motion_plan_request=request)
        )
        rclpy.spin_until_future_complete(self, future)

        response = future.result()
        if response.motion_plan_response.error_code.val == 1:
            self.get_logger().info('Cartesian planning succeeded!')
            return response.motion_plan_response.trajectory
        else:
            self.get_logger().error('Cartesian planning failed!')
            return None

URScript — Direct Robot Control

When you need direct control without MoveIt2 overhead, URScript is the way:

#!/usr/bin/env python3
"""Send URScript commands directly to UR robot via ROS 2."""
import rclpy
from rclpy.node import Node
from std_msgs.msg import String


class URScriptSender(Node):
    def __init__(self):
        super().__init__('urscript_sender')
        self.publisher = self.create_publisher(
            String, '/urscript_interface/script_command', 10
        )

    def send_movej(self, joints, a=1.4, v=1.05, r=0):
        """Send MoveJ via URScript."""
        joints_str = ', '.join([f'{j:.4f}' for j in joints])
        script = f'movej([{joints_str}], a={a}, v={v}, r={r})'
        msg = String()
        msg.data = script
        self.publisher.publish(msg)
        self.get_logger().info(f'Sent: {script}')

    def send_movel(self, pose, a=1.2, v=0.25, r=0):
        """Send MoveL via URScript."""
        pose_str = ', '.join([f'{p:.4f}' for p in pose])
        script = f'movel(p[{pose_str}], a={a}, v={v}, r={r})'
        msg = String()
        msg.data = script
        self.publisher.publish(msg)

    def send_movec(self, via_pose, end_pose, a=1.2, v=0.1, r=0):
        """Send MoveC via URScript."""
        via_str = ', '.join([f'{p:.4f}' for p in via_pose])
        end_str = ', '.join([f'{p:.4f}' for p in end_pose])
        script = f'movec(p[{via_str}], p[{end_str}], a={a}, v={v}, r={r})'
        msg = String()
        msg.data = script
        self.publisher.publish(msg)

ros2_control — Hardware Abstraction

follow_joint_trajectory Action

The standard way to execute trajectories on real robots:

#!/usr/bin/env python3
"""Execute trajectory via follow_joint_trajectory action."""
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
import numpy as np


class TrajectoryExecutor(Node):
    def __init__(self):
        super().__init__('trajectory_executor')
        self.action_client = ActionClient(
            self, FollowJointTrajectory,
            '/scaled_joint_trajectory_controller/follow_joint_trajectory'
        )
        self.action_client.wait_for_server(timeout_sec=10.0)

    def execute_trajectory(self, joint_names, positions, velocities,
                           timestamps):
        """
        Execute a trajectory on the robot.

        Args:
            joint_names: ['shoulder_pan_joint', ...]
            positions: list of joint position arrays
            velocities: list of joint velocity arrays
            timestamps: list of timestamps (seconds)
        """
        goal = FollowJointTrajectory.Goal()
        goal.trajectory.joint_names = joint_names

        for pos, vel, t in zip(positions, velocities, timestamps):
            point = JointTrajectoryPoint()
            point.positions = pos.tolist()
            point.velocities = vel.tolist()
            point.time_from_start = Duration(
                sec=int(t), nanosec=int((t % 1) * 1e9)
            )
            goal.trajectory.points.append(point)

        future = self.action_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, future)

        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('Goal rejected!')
            return False

        self.get_logger().info('Trajectory executing...')
        result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)

        result = result_future.result().result
        if result.error_code == FollowJointTrajectory.Result.SUCCESSFUL:
            self.get_logger().info('Trajectory completed successfully!')
            return True
        else:
            self.get_logger().error(f'Trajectory failed: {result.error_string}')
            return False

Complete Workflow: Design, Plan, Execute

#!/usr/bin/env python3
"""Complete workflow: Design trajectory in Python -> Execute on UR5e."""
import rclpy
import numpy as np
from scipy.interpolate import CubicSpline

# Step 1: Design trajectory (offline, in Python)
waypoints = np.array([
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
    [np.pi/6, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, 0],
    [np.pi/6, -np.pi/3, np.pi/3, -np.pi/2, np.pi/3, 0],
    [np.pi/6, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, 0],
    [np.pi/2, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, 0],
    [np.pi/2, -np.pi/3, np.pi/3, -np.pi/2, np.pi/3, 0],
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
])

segment_times = [1.0, 0.5, 0.3, 0.5, 1.0, 0.5, 1.0]
t_waypoints = np.cumsum([0] + segment_times)

# Step 2: Cubic spline interpolation
dt = 0.008  # 125 Hz (UR controller rate)
t_dense = np.arange(0, t_waypoints[-1], dt)

joint_names = [
    'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
    'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]

positions = np.zeros((len(t_dense), 6))
velocities = np.zeros((len(t_dense), 6))

for j in range(6):
    cs = CubicSpline(t_waypoints, waypoints[:, j], bc_type='clamped')
    positions[:, j] = cs(t_dense)
    velocities[:, j] = cs(t_dense, 1)

# Step 3: Execute on robot
def main():
    rclpy.init()
    executor = TrajectoryExecutor()

    success = executor.execute_trajectory(
        joint_names=joint_names,
        positions=positions,
        velocities=velocities,
        timestamps=t_dense.tolist()
    )

    if success:
        print("Pick-and-place cycle completed!")
    else:
        print("Execution failed — check robot status")

    executor.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Safety Considerations

Velocity and Acceleration Limits

# Safety limits for UR5e
UR5E_LIMITS = {
    'joint_velocity': [2.094, 2.094, 3.142, 3.142, 3.142, 3.142],
    'joint_acceleration': [10.0] * 6,
    'tcp_velocity': 1.0,    # m/s (cobot mode)
    'tcp_force': 150,       # N (collision detection)
}

def validate_trajectory(positions, velocities, accelerations, dt, limits):
    """Validate trajectory against safety limits."""
    violations = []

    for i in range(len(positions)):
        t = i * dt
        for j in range(6):
            if abs(velocities[i, j]) > limits['joint_velocity'][j]:
                violations.append(
                    f"t={t:.3f}: Joint {j+1} velocity "
                    f"{velocities[i, j]:.3f} exceeds limit"
                )
            if i > 0 and abs(accelerations[i, j]) > limits['joint_acceleration'][j]:
                violations.append(
                    f"t={t:.3f}: Joint {j+1} acceleration exceeds limit"
                )

    if violations:
        print(f"SAFETY: {len(violations)} violations found!")
        for v in violations[:5]:
            print(f"  {v}")
        return False
    else:
        print("SAFETY: All limits satisfied")
        return True

Deployment Checklist

1. Simulation test — Run full trajectory in MuJoCo or Gazebo
2. Validate limits — Check velocity, acceleration, workspace bounds
3. Dry run — Execute at 10% speed (velocity_scaling=0.1)
4. Collision check — MoveIt2 collision detection with scene objects
5. Emergency stop — Always have E-stop within reach
6. Teach pendant — Ready to override with teach pendant

Summary Workflow

1. Design trajectory (Python + numpy/scipy)
    |
2. Validate (check limits, workspace, singularity)
    |
3. Simulate (MuJoCo/Gazebo — verify visually)
    |
4. MoveIt2 plan (collision checking, optimization)
    |
5. Execute at 10% speed (safety dry run)
    |
6. Execute at full speed (production)

References

• Coleman, D. et al. "Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study." Journal of Software Engineering for Robotics, 2014. arXiv:1404.3785
• Pilz GmbH. Pilz Industrial Motion Planner for MoveIt. GitHub
• Scherzinger, S. et al. "Forward-Looking Integration of ros2_control for Real-Time Robot Control." ROSCon, 2022. ros2_control Docs
• Universal Robots. ROS 2 Driver Documentation. GitHub

Series Summary

Across 8 posts, the Traditional Manipulation Control series covered all essential knowledge:

This is a solid foundation for advancing to modern techniques: AI-based manipulation, imitation learning, and diffusion policy.

Related Posts

• Grasping & Manipulation Basics — Next step: AI manipulation
• ROS 2 Control Hardware Interface — Detailed ros2_control guide
• MoveIt2 Motion Planning — Detailed MoveIt2 configuration