# Manipulation (MoveIt 2)

## Table of contents
1. MoveIt 2 architecture
2. Move group configuration (SRDF, kinematics)
3. Planning scene management
4. Motion planning pipeline
5. Grasp generation and execution
6. MoveIt Servo (real-time jogging)
7. Integration with ros2_control
8. Collision-aware planning
9. Common failures and fixes

---

## 1. MoveIt 2 architecture

```
                    ┌──────────────────┐
                    │   Move Group     │ ← Central coordination node
                    │   Node           │
                    └────────┬─────────┘
                             │
         ┌───────────────────┼───────────────────┐
         ▼                   ▼                   ▼
┌──────────────┐   ┌──────────────┐   ┌──────────────┐
│   Planning    │   │  Planning    │   │  Trajectory  │
│   Pipeline    │   │  Scene       │   │  Execution   │
│  (OMPL/Pilz) │   │  Monitor     │   │  Manager     │
└──────────────┘   └──────────────┘   └──────────────┘
                                              │
                                    ┌─────────▼──────────┐
                                    │  ros2_control       │
                                    │  (JointTrajectory   │
                                    │   Controller)       │
                                    └────────────────────┘
```

**Key components:**
- **Move Group Node:** Central interface for planning and execution
- **Planning Pipeline:** Planner + post-processing (smoothing, time parameterization)
- **Planning Scene:** World model with robot state, collision objects, and attached objects
- **Trajectory Execution Manager:** Sends planned trajectories to controllers
- **MoveIt Servo:** Real-time Cartesian/joint jogging

## 2. Move group configuration

### SRDF (Semantic Robot Description Format)

The SRDF defines semantics not captured in URDF: planning groups, default
states, disabled collision pairs, and end effectors.

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

  <!-- Planning group: individual joints -->
  <group name="gripper">
    <joint name="finger_left_joint"/>
    <joint name="finger_right_joint"/>
  </group>

  <!-- Named states (for quick motion) -->
  <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="0"/>
    <joint name="joint_5" value="-1.57"/>
    <joint name="joint_6" value="0"/>
  </group_state>

  <group_state name="ready" group="arm">
    <joint name="joint_1" value="0"/>
    <joint name="joint_2" value="-0.785"/>
    <joint name="joint_3" value="0.785"/>
    <joint name="joint_4" value="0"/>
    <joint name="joint_5" value="-0.785"/>
    <joint name="joint_6" value="0"/>
  </group_state>

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

  <!-- Disable collision checking between adjacent links -->
  <disable_collisions link1="base_link" link2="link_1" reason="Adjacent"/>
  <disable_collisions link1="link_1" link2="link_2" reason="Adjacent"/>
  <!-- ... generated by MoveIt Setup Assistant -->
</robot>
```

### Kinematics configuration

```yaml
# kinematics.yaml
arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05   # seconds
  # For 6+ DOF arms, consider:
  # kinematics_solver: pick_ik/PickIkPlugin   # Faster for complex IK
```

### Joint limits override

```yaml
# joint_limits.yaml — override URDF limits for MoveIt
joint_limits:
  joint_1:
    has_velocity_limits: true
    max_velocity: 2.0             # rad/s
    has_acceleration_limits: true
    max_acceleration: 5.0         # rad/s²
  joint_2:
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
```

## 3. Planning scene management

### Adding collision objects (C++)

```cpp
#include <moveit/planning_scene_interface/planning_scene_interface.h>

void add_table(moveit::planning_interface::PlanningSceneInterface & psi)
{
  moveit_msgs::msg::CollisionObject table;
  table.id = "table";
  table.header.frame_id = "base_link";

  // Table shape
  shape_msgs::msg::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions = {1.0, 0.6, 0.02};  // x, y, z

  geometry_msgs::msg::Pose pose;
  pose.position.x = 0.5;
  pose.position.z = 0.4;
  pose.orientation.w = 1.0;

  table.primitives.push_back(primitive);
  table.primitive_poses.push_back(pose);
  table.operation = table.ADD;

  psi.applyCollisionObject(table);
}
```

### Attaching objects to the robot

```cpp
// Attach an object to the end effector (e.g., after grasping)
moveit_msgs::msg::AttachedCollisionObject attached;
attached.link_name = "tool0";
attached.object.id = "picked_part";
attached.object.header.frame_id = "tool0";
attached.touch_links = {"finger_left", "finger_right"};  // Allow contact

shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = {0.05, 0.05, 0.1};

geometry_msgs::msg::Pose pose;
pose.position.z = 0.05;
pose.orientation.w = 1.0;

attached.object.primitives.push_back(box);
attached.object.primitive_poses.push_back(pose);
attached.object.operation = attached.object.ADD;

psi.applyAttachedCollisionObject(attached);
```

### Using MoveGroupInterface (C++)

```cpp
#include <moveit/move_group_interface/move_group_interface.h>

auto move_group = moveit::planning_interface::MoveGroupInterface(node, "arm");

// Set planning parameters
move_group.setPlanningTime(5.0);           // Max planning time
move_group.setMaxVelocityScalingFactor(0.5);
move_group.setMaxAccelerationScalingFactor(0.5);
move_group.setNumPlanningAttempts(10);

// Move to named state
move_group.setNamedTarget("home");
auto result = move_group.move();

// Move to joint target
std::vector<double> joints = {0, -1.57, 1.57, 0, -1.57, 0};
move_group.setJointValueTarget(joints);
move_group.move();

// Move to pose target
geometry_msgs::msg::Pose target;
target.position.x = 0.4;
target.position.y = 0.0;
target.position.z = 0.5;
target.orientation.w = 1.0;
move_group.setPoseTarget(target);

// Plan then execute (preferred for safety)
moveit::planning_interface::MoveGroupInterface::Plan plan;
if (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS) {
  move_group.execute(plan);
}
```

### Cartesian path planning

```cpp
// Plan a Cartesian (straight-line) path
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(current_pose);  // Start

auto next_pose = current_pose;
next_pose.position.z -= 0.1;        // Move down 10cm
waypoints.push_back(next_pose);

next_pose.position.x += 0.2;        // Move forward 20cm
waypoints.push_back(next_pose);

moveit_msgs::msg::RobotTrajectory trajectory;
double fraction = move_group.computeCartesianPath(
  waypoints,
  0.01,          // eef_step: max step size in meters
  0.0,           // jump_threshold: 0 = disabled
  trajectory);

if (fraction > 0.95) {  // Successfully planned >95% of the path
  move_group.execute(trajectory);
}
```

## 4. Motion planning pipeline

### Planner selection

| Planner | Type | Best for |
|---|---|---|
| OMPL (RRTConnect) | Sampling-based | General 6-DOF planning, complex environments |
| OMPL (RRT*) | Optimal sampling | When path quality matters (longer planning time) |
| Pilz Industrial | Deterministic | Linear, circular motions in industrial settings |
| STOMP | Optimization | Smooth paths in cluttered environments |
| CHOMP | Optimization | Gradient-based, smooth trajectories |

### OMPL configuration

```yaml
# ompl_planning.yaml
planner_configs:
  RRTConnect:
    type: geometric::RRTConnect
    range: 0.0  # 0 = auto
  RRTstar:
    type: geometric::RRTstar
    range: 0.0
    goal_bias: 0.05
  PRM:
    type: geometric::PRM
    max_nearest_neighbors: 10

arm:
  planner_configs:
    - RRTConnect
    - RRTstar
  default_planner_config: RRTConnect
  projection_evaluator: joints(joint_1,joint_2)
  longest_valid_segment_fraction: 0.005
```

### Pilz industrial planner (deterministic motions)

```cpp
// Linear motion (LIN)
move_group.setPlanningPipelineId("pilz_industrial_motion_planner");
move_group.setPlannerId("LIN");
move_group.setPoseTarget(target_pose);
move_group.move();

// Circular motion (CIRC) — requires interim and goal pose
// Set via path constraints or specialized API
```

## 5. Grasp generation and execution

### Pick and place pattern

```cpp
void pick(moveit::planning_interface::MoveGroupInterface & arm,
          moveit::planning_interface::MoveGroupInterface & gripper)
{
  // 1. Open gripper
  gripper.setNamedTarget("open");
  gripper.move();

  // 2. Move to pre-grasp pose (above object)
  geometry_msgs::msg::Pose pre_grasp = object_pose;
  pre_grasp.position.z += 0.15;  // 15cm above
  arm.setPoseTarget(pre_grasp);
  arm.move();

  // 3. Approach (Cartesian down)
  std::vector<geometry_msgs::msg::Pose> approach = {pre_grasp, object_pose};
  moveit_msgs::msg::RobotTrajectory traj;
  arm.computeCartesianPath(approach, 0.01, 0.0, traj);
  arm.execute(traj);

  // 4. Close gripper
  gripper.setNamedTarget("closed");
  gripper.move();

  // 5. Attach object to end effector in planning scene
  psi.applyAttachedCollisionObject(attached_object);

  // 6. Retreat (Cartesian up)
  std::vector<geometry_msgs::msg::Pose> retreat = {object_pose, pre_grasp};
  arm.computeCartesianPath(retreat, 0.01, 0.0, traj);
  arm.execute(traj);
}
```

### MoveIt Task Constructor (MTC)

MTC provides a structured approach to multi-stage manipulation tasks like pick-and-place,
bin picking, and assembly. Each task is composed of stages (compute IK, approach, grasp,
retreat, place) that can be configured independently.

```cpp
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages.h>
#include <moveit/task_constructor/solvers.h>

auto task = std::make_unique<moveit::task_constructor::Task>();
// Load robot model via RobotModelLoader (node->getRobotModel() does not exist)
auto robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>(node);
task->setRobotModel(robot_model_loader->getModel());

// Pipeline planner for free-space motion
auto pipeline = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(node);

// Cartesian planner for approach/retreat
auto cartesian = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
cartesian->setMaxVelocityScalingFactor(0.1);

// Stage 1: Current state
task->add(std::make_unique<moveit::task_constructor::stages::CurrentState>("current"));

// Stage 2: Open gripper
auto open = std::make_unique<moveit::task_constructor::stages::MoveTo>("open_gripper", pipeline);
open->setGroup("gripper");
open->setGoal("open");
task->add(std::move(open));

// Stage 3: Move to pick approach
auto approach = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach", cartesian);
approach->setGroup("arm");
geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world";
direction.vector.z = -0.1;  // Approach downward
approach->setDirection(direction);
task->add(std::move(approach));

// ... more stages: close gripper, retreat, move to place, open gripper
```

Install: `sudo apt install ros-jazzy-moveit-task-constructor-core`

### pick_ik solver

pick_ik is a modern inverse kinematics solver that uses gradient descent optimization.
It often finds solutions faster than KDL for complex kinematic chains and handles
singularities better.

```yaml
# kinematics.yaml — planning group name at top level
arm:
  kinematics_solver: pick_ik/PickIkPlugin
  kinematics_solver_timeout: 0.05
  position_threshold: 0.001
```

Install: `sudo apt install ros-jazzy-pick-ik`

## 6. MoveIt Servo (real-time jogging)

MoveIt Servo enables real-time Cartesian and joint-space velocity control,
useful for teleoperation, visual servoing, and force-guided motions.

### Configuration

```yaml
# servo_params.yaml — Humble (Servo API changed significantly in Jazzy)
# Jazzy+ uses a redesigned parameter structure; check the MoveIt Servo
# migration guide when upgrading.
moveit_servo:
  ros__parameters:
    ## Humble parameters:
    use_gazebo: false
    planning_frame: "base_link"
    ee_frame_name: "tool0"
    robot_link_command_frame: "base_link"
    command_in_type: "speed_units"  # or "unitless"
    scale:
      linear: 0.3      # m/s per unit
      rotational: 0.5   # rad/s per unit
      joint: 0.5        # rad/s per unit
    publish_period: 0.01  # 100 Hz
    check_collisions: true
    collision_check_rate: 50.0
    self_collision_proximity_threshold: 0.01
    scene_collision_proximity_threshold: 0.02
    joint_limit_margin: 0.1  # radians from limit

    ## Jazzy+ key differences:
    #  - `use_gazebo` removed
    #  - `publish_period` → `publish_frequency: 100.0`
    #  - `command_in_type` removed; use topic type directly
    #  - See moveit2/moveit_servo/config/ for current defaults
```

### Sending Servo commands

```python
# Twist command for Cartesian jogging
from geometry_msgs.msg import TwistStamped

twist = TwistStamped()
twist.header.frame_id = 'base_link'
twist.header.stamp = node.get_clock().now().to_msg()
twist.twist.linear.x = 0.1    # Move forward at 0.1 m/s
twist.twist.angular.z = 0.05  # Rotate slowly

servo_twist_pub.publish(twist)  # Publish to /servo_node/delta_twist_cmds
```

## 7. Integration with ros2_control

MoveIt 2 sends trajectories to ros2_control's `JointTrajectoryController`:

```yaml
# controllers.yaml for MoveIt
controller_manager:
  ros__parameters:
    update_rate: 500

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

arm_controller:
  ros__parameters:
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
```

### MoveIt controller configuration

```yaml
# moveit_controllers.yaml
moveit_simple_controller_manager:
  controller_names:
    - arm_controller

  arm_controller:
    type: FollowJointTrajectory
    action_ns: follow_joint_trajectory
    default: true
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
```

## 8. Collision-aware planning

### Allowed collision matrix (ACM)

The ACM defines which link pairs should skip collision checking (adjacent
links, always-in-contact parts). Generate it with MoveIt Setup Assistant:

```bash
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```

### Adding obstacles from sensor data

```yaml
# Octomap integration — build 3D collision map from depth camera
move_group_node:
  ros__parameters:
    octomap_resolution: 0.05
    octomap_frame: "map"
    point_cloud_topic: "/camera/depth/points"
```

### Padding and scaling

```yaml
# For safety margin around the robot body
robot_description_planning:
  default_robot_padding: 0.02   # 2cm padding on all links
  default_robot_scale: 1.0      # No scaling
```

## 9. Common failures and fixes

| Symptom | Cause | Fix |
|---|---|---|
| "No motion plan found" | Start or goal in collision, or unreachable | Check planning scene in RViz, verify IK solution exists |
| Trajectory execution fails | Controller rejects trajectory (velocity/accel limits) | Set `setMaxVelocityScalingFactor(0.3)`, tune joint_limits.yaml |
| Robot jerks at trajectory start | Current state doesn't match trajectory start | Ensure `joint_state_broadcaster` is running and publishing current state |
| IK solver times out | Complex pose, kinematic singularity | Try different IK solver (pick_ik), increase timeout, check if pose is reachable |
| Collision object not appearing | Wrong frame_id or stale planning scene | Verify `header.frame_id` matches TF tree, check planning scene monitor topic |
| Cartesian path returns low fraction | Obstacles in the way, or step too large | Reduce `eef_step`, add intermediate waypoints, check for collisions |
| MoveIt Servo stops unexpectedly | Near joint limit or collision | Increase `joint_limit_margin`, check collision scene |
| Planning takes too long | Complex environment, too many collision objects | Simplify collision geometry, use convex decomposition, increase planning time |
