MoveIt2 Robot Arm Guide 2026

<?xml version="1.0"?>
<robot name="my_robot">
  <!-- Planning group for the arm -->
  <group name="arm">
    <chain base_link="base_link" tip_link="tool0"/>
  </group>

  <!-- Planning group for the gripper -->
  <group name="gripper">
    <joint name="finger_joint_1"/>
    <joint name="finger_joint_2"/>
  </group>

  <!-- Named poses -->
  <group_state name="home" group="arm">
    <joint name="joint_1" value="0"/>
    <joint name="joint_2" value="-1.57"/>
    <joint name="joint_3" value="1.57"/>
    <joint name="joint_4" value="-1.57"/>
    <joint name="joint_5" value="0"/>
    <joint name="joint_6" value="0"/>
  </group_state>

  <!-- End-effector -->
  <end_effector name="eef" parent_link="tool0"
    group="gripper" parent_group="arm"/>

  <!-- Disable self-collision for adjacent links -->
  <disable_collisions link1="link_1" link2="link_2" reason="Adjacent"/>
</robot>

arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  # For better performance, use bio_ik:
  # kinematics_solver: bio_ik/BioIKKinematics
  # kinematics_solver_attempts: 10

gripper:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005

arm:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - RRTConnectkConfigDefault      # fastest, good for most tasks
    - RRTstarkConfigDefault         # asymptotically optimal
    - BITstarkConfigDefault         # best quality, slower
    - PRMkConfigDefault             # good for repeated queries
  longest_valid_segment_fraction: 0.005

planner_configs:
  RRTConnectkConfigDefault:
    type: geometric::RRTConnect
    range: 0.0

  BITstarkConfigDefault:
    type: geometric::BITstar
    stop_on_solution_improvement: false

import rclpy
from rclpy.node import Node
from moveit.planning import MoveItPy
from moveit.core.robot_state import RobotState
import time

def main():
    rclpy.init()
    node = Node("moveit_py_demo")

    # Initialize MoveItPy — connects to move_group
    robot = MoveItPy(node_name="moveit_py")
    arm = robot.get_planning_component("arm")

    # Move to named pose "home" (defined in SRDF)
    arm.set_start_state_to_current_state()
    arm.set_goal_state(configuration_name="home")

    plan_result = arm.plan()
    if plan_result:
        robot.execute(plan_result.trajectory, controllers=[])
        print("Moved to home pose!")
    else:
        print("Planning failed!")

    time.sleep(2.0)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

from geometry_msgs.msg import PoseStamped

# Set pose goal for end-effector
pose_goal = PoseStamped()
pose_goal.header.frame_id = "base_link"
pose_goal.pose.position.x = 0.4
pose_goal.pose.position.y = 0.0
pose_goal.pose.position.z = 0.4
pose_goal.pose.orientation.x = 0.0
pose_goal.pose.orientation.y = 0.0
pose_goal.pose.orientation.z = 0.0
pose_goal.pose.orientation.w = 1.0

arm.set_start_state_to_current_state()
arm.set_goal_state(
    pose_stamped_msg=pose_goal,
    pose_link="tool0"  # link to achieve the pose
)

plan_result = arm.plan()
if plan_result:
    robot.execute(plan_result.trajectory, controllers=[])

from moveit.core.kinematic_constraints import construct_joint_constraint
from moveit_msgs.msg import RobotTrajectory
import copy

# Build waypoints for a straight-line forward motion
waypoints = []

# Start from current pose
arm.set_start_state_to_current_state()
current_pose = arm.get_current_pose("tool0").pose

# Move 10cm forward in X
for i in range(1, 6):
    wpose = copy.deepcopy(current_pose)
    wpose.position.x += 0.02  # 2cm per step
    waypoints.append(copy.deepcopy(wpose))

# Compute Cartesian path — eef_step=1mm resolution
(plan, fraction) = arm.compute_cartesian_path(
    waypoints,
    eef_step=0.001,           # 1mm resolution
    avoid_collisions=True,
)

print(f"Cartesian path coverage: {fraction * 100:.1f}%")
if fraction > 0.95:           # >95% path achieved
    robot.execute(plan, controllers=[])

from moveit.planning import PlanningSceneMonitor
from shape_msgs.msg import SolidPrimitive
from moveit_msgs.msg import CollisionObject
from geometry_msgs.msg import Pose

# Get the planning scene interface
psm = robot.get_planning_scene_monitor()

with psm.read_write() as scene:
    # Add a box obstacle (e.g., a table)
    collision_object = CollisionObject()
    collision_object.header.frame_id = "base_link"
    collision_object.id = "table"

    box = SolidPrimitive()
    box.type = SolidPrimitive.BOX
    box.dimensions = [0.6, 0.8, 0.05]  # 60x80x5cm

    box_pose = Pose()
    box_pose.position.x = 0.5
    box_pose.position.y = 0.0
    box_pose.position.z = -0.025
    box_pose.orientation.w = 1.0

    collision_object.primitives = [box]
    collision_object.primitive_poses = [box_pose]
    collision_object.operation = CollisionObject.ADD

    scene.apply_collision_object(collision_object)
    scene.current_state.update()

print("Table added to planning scene. MoveIt2 will now avoid it.")

# Install UR ROS 2 driver
sudo apt install ros-jazzy-ur -y

# For UR10e at IP 192.168.1.100:
ros2 launch ur_robot_driver ur_control.launch.py \
  ur_type:=ur10e \
  robot_ip:=192.168.1.100 \
  use_fake_hardware:=false \
  launch_rviz:=false

# In a second terminal, launch MoveIt2:
ros2 launch ur_moveit_config ur_moveit.launch.py \
  ur_type:=ur10e \
  use_fake_hardware:=false

# Install franka_ros2
sudo apt install ros-jazzy-franka-ros2 -y

# On a real Franka FR3:
ros2 launch franka_fr3_moveit_config moveit.launch.py \
  robot_ip:=192.168.1.10

# This launches: move_group, RViz2, franka_robot_state_broadcaster
# FCI (Franka Control Interface) must be unlocked via Desk web UI first