icir_rpp

Vision-guided suction pick-and-place for a Franka Emika Panda with an OnRobot VGC10 vacuum gripper, built on MoveIt 2.

This repository implements a ROS 2 pick-and-place stack that repeatedly picks cylindrical objects out of a tote box and places them into a carrier tray. Grasp candidates are requested from an external perception server over a ROS 2 service; motion is planned and executed through MoveIt 2's MoveGroupInterface; and the grasp itself is closed-loop — the arm descends along a Cartesian path while the node watches the live vacuum reading from the VGC10 and stops the motion the instant suction is achieved. The same controller runs unmodified against the real Panda (franka_hardware) or a mock_components simulation, selected by a single launch argument.

A KUKA KR10 R1440-2 variant of the same task is also included.

---

System Overview

┌──────────────────────────────────────────────────────────────────────────┐
│                                icir_rpp                                   │
│                                                                           │
│   icir_rpp_server / external perception                                   │
│        │                                                                  │
│        │  /get_suction_points   (icir_rpp_msgs/srv/GetSuctionPoints)       │
│        │      → Pose[] suction_points, float64[] scores                    │
│        ▼                                                                  │
│   ┌──────────────────────────────────────────┐                            │
│   │  icir_rpp :: PickPlaceNode               │   /vg_command (String)     │
│   │  pick-and-place sequence + scene builder │ ─────────────────────────► │
│   │                                          │ ◄───────────────────────── │
│   └───────┬──────────────────────────────────┘   /vg_channelA_vacuum      │
│           │  MoveGroupInterface                        (Int32, 100 Hz)    │
│           ▼                                                     ▲         │
│   ┌───────────────┐                                    ┌────────┴───────┐ │
│   │  move_group   │  OMPL + Cartesian path             │ onrobot_vg_    │ │
│   │  (MoveIt 2)   │  + IterativeParabolic time param.  │ control        │ │
│   └───────┬───────┘                                    └────────┬───────┘ │
│           │  FollowJointTrajectory                              │ Modbus  │
│           ▼                                                     ▼  /TCP   │
│   ┌───────────────────────────────┐                    ┌────────────────┐ │
│   │ ros2_control                  │                    │ OnRobot VGC10  │ │
│   │  franka_hardware  |  mock     │                    │ vacuum gripper │ │
│   └───────────────────────────────┘                    └────────────────┘ │
│           │                                                               │
│           ▼                                                               │
│     Franka Emika Panda  /  KUKA KR10 R1440-2                              │
└──────────────────────────────────────────────────────────────────────────┘

Key Features

• Service-driven grasp loop — the controller polls a perception service for suction points and retries with the next-best candidate whenever planning or grasping fails
• Vacuum-feedback grasping — descent is executed asynchronously and aborted the moment the measured vacuum crosses a threshold, instead of moving to a fixed depth
• Sim/real parity — one node, one config; the hardware plugin is chosen by which move_group launch you start, and is_real:=true|false toggles the simulated object attach/detach
• Programmatic planning scene — nine safety walls, the carrier tray, and a tote-box STL mesh are inserted at every cycle; in simulation the target cylinder is spawned at a randomized pose within configurable ranges
• Explicit trajectory generation — joint-space and Cartesian waypoints are interpolated by hand and time-parameterized with IterativeParabolicTimeParameterization, giving direct control over approach speed (separate velocity scaling for the suction descent)

---

Hardware

Component	Model	Interface	Default address
Robot arm	Franka Emika Panda	franka_hardware/FrankaHardwareInterface (FCI)	192.168.10.102
Vacuum gripper	OnRobot VGC10	Modbus/TCP (unit 65)	192.168.10.209:502
Robot arm (alt.)	KUKA KR10 R1440-2	MoveIt config only	—

Vacuum on Channel A is read from holding register 258 in 1/1000 relative vacuum and published at 100 Hz.

---

Package Structure

icir_rpp_ws/src/icir_rpp/
├── icir_rpp/                        # Main pick-and-place controller
│   ├── src/panda_vgc10/             # Modular Panda + VGC10 implementation
│   │   ├── main.cpp                 #   service loop: request grasps → run() → retry
│   │   ├── pick_place_node.cpp      #   node, parameters, callbacks, run() sequence
│   │   ├── motion_utils.cpp         #   joint / Cartesian motions, suction pick, place
│   │   ├── scene_utils.cpp          #   planning-scene objects, pick-pose offsets
│   │   └── suction_utils.cpp        #   gripper commands, vacuum wait helper
│   ├── src/kr10_r1440_2/            # KUKA KR10 R1440-2 variant
│   ├── launch/<robot>/              # move_group_*.launch.py + icir_rpp_*.launch.py pairs
│   ├── config/                      # Task parameters (my_parameter_*.yaml)
│   ├── meshes/tote_1.stl            # Tote-box collision mesh
│   ├── rviz/                        # RViz configurations per robot
│   └── script/                      # Standalone helper publishers
├── icir_rpp_server/                 # GetSuctionPoints service server (placeholder)
├── icir_rpp_msgs/                   # srv/GetSuctionPoints.srv
├── onrobot_vg_control/              # OnRobot VG10/VGC10 Modbus driver node
└── icir_rpp_resources/              # Robot descriptions + MoveIt configs
    ├── UPSTREAM.md                  #   fork provenance (ros-planning/moveit_resources)
    ├── icir_rpp_panda_description/  #   Panda URDF/meshes (+ VGC10 suction cup)
    ├── icir_rpp_panda_moveit_config/#   config/ (stock) and config_vgc10/ (this project)
    ├── icir_rpp_kuka_description/   #   KR10 R1440-2 URDF/meshes
    └── icir_rpp_kuka_moveit_config/ #   KR10 R1440-2 MoveIt config

The _vgc10 configuration adds the gripper chain panda_link8 → onrobot_vgc10_base_link → extender → suction → tip, exposed as the SRDF group vgc10 and the combined planning group panda_arm_hand_vgc10 used by the controller.

---

1. System Requirements

	Version
OS	Ubuntu 22.04
ROS	ROS 2 Humble
Build	colcon (--symlink-install recommended)

Dependencies

sudo apt install \
  ros-humble-moveit \
  ros-humble-moveit-visual-tools \
  ros-humble-ros2-control \
  ros-humble-ros2-controllers \
  ros-humble-joint-state-publisher \
  ros-humble-joint-state-publisher-gui

Requirement	Notes
franka_ros2 / libfranka	Real Panda only. Provides franka_hardware; must match the robot's system version.
pymodbus	Gripper driver only. The driver imports pymodbus.client.sync, so pymodbus 2.x is required (pip install 'pymodbus<3.0').

---

2. Installation

mkdir -p ~/icir_rpp_ws/src && cd ~/icir_rpp_ws/src
git clone <this-repository> icir_rpp

cd ~/icir_rpp_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash

Task parameters are read from the package share directory. With --symlink-install, editing icir_rpp/config/my_parameter_*.yaml in the source tree takes effect without rebuilding.

---

3. Setup

3.1 Network

[User PC] ──LAN── [Switch] ──┬── [Panda controller]  192.168.10.102
                             ├── [OnRobot VGC10]     192.168.10.209:502
                             └── [Camera / perception PC]

Override at launch if your addresses differ:

ros2 launch icir_rpp move_group_panda_vgc10_real.launch.py robot_ip:=192.168.10.102
ros2 launch onrobot_vg_control vg_control.launch.py ip:=192.168.10.209 port:=502

3.2 Franka preparation (real robot only)

1. Open Franka Desk at the robot IP
2. Unlock the joints
3. Activate FCI
4. Register the VGC10 as the end-effector (mass / centre of mass / inertia) so that the internal dynamics model is correct — see onrobot_vg_control/config/endeffector-config.json

---

4. Usage

Each robot has a pair of launch files: one brings up move_group + RViz + ros2_control, the other starts the pick-and-place controller. Run them in separate terminals.

4.1 Simulation

Terminal 1 — MoveIt, RViz, mock hardware:

ros2 launch icir_rpp move_group_panda_vgc10.launch.py

Terminal 2 — grasp service (returns a fixed example pose):

ros2 run icir_rpp_server pick_pose_server.py

Terminal 3 — pick-and-place controller:

ros2 launch icir_rpp icir_rpp_panda_vgc10.launch.py is_real:=false

Leave is_sim_time at its default (true). That is the setting the real-robot experiments were validated with. is_sim_time:=false no longer crashes the node, but it is untested.

With is_real:=false the node spawns a cylinder at a random pose inside the configured x_range / y_range / z_range / roll_range / pitch_range, derives the pick pose from that cylinder, and attaches/detaches it in the planning scene as it is carried.

There is no vacuum sensor in simulation, so suctionPick() runs the descent to completion and logs a "Suction vacuum timeout" warning before continuing. This is expected.

Alternative — the MoveIt demo scene (motion planning only, no task node):

ros2 launch icir_rpp_panda_moveit_config demo_vgc10.launch.py

4.2 Real Robot

Unlock the robot and activate FCI in Franka Desk first.

Terminal 1 — MoveIt, RViz, franka_hardware, and the VGC10 driver (included automatically):

ros2 launch icir_rpp move_group_panda_vgc10_real.launch.py \
    robot_ip:=192.168.10.102 use_fake_hardware:=false

Terminal 2 — perception service supplying suction points:

ros2 run icir_rpp_server pick_pose_server.py     # replace with the real perception node

Terminal 3 — pick-and-place controller:

ros2 launch icir_rpp icir_rpp_panda_vgc10.launch.py is_real:=true

With is_real:=true no cylinder is spawned; the pick pose comes straight from the service response, and the pre-pick pose is derived by backing off suction_offset along the grasp frame's local −Z axis.

Convenience scripts for the two real-robot terminals live at the workspace root: run_real1_moveit.sh and run_real2_demo.sh.

Dry run — the real-robot launch path (effort controllers, franka broadcaster, VG driver) without a robot attached:

ros2 launch icir_rpp move_group_panda_vgc10_real.launch.py use_fake_hardware:=true

4.3 KUKA KR10 R1440-2

# terminal 1
ros2 launch icir_rpp move_group_kr10_r1440_2_vgc10.launch.py
# terminal 2
ros2 launch icir_rpp icir_rpp_kr10_r1440_2_vgc10.launch.py

4.4 Manual gripper control

ros2 topic pub -1 /vg_command std_msgs/String "data: 'grip'"
ros2 topic pub -1 /vg_command std_msgs/String "data: 'release'"
ros2 topic echo /vg_channelA_vacuum

---

5. Pick-and-Place Sequence

PickPlaceNode::run() executes the following steps; any failure releases the vacuum, returns to the start pose, and reports failure to the main loop, which then requests the next grasp candidate.

#	Step	Implementation
1	clearScene	Remove the simulated cylinder, detach anything attached, clear markers
2	goToStartPose	Joint-space goal [0, −0.785, 0, −2.356, 0, 1.571, 0] (skipped if the previous cycle already ended at home)
3	addAllCollisionObjects	9 safety walls + carrier box + tote-box mesh (+ random cylinder in simulation), then compute pick_pose_ / pick_pose_pre_
4	planAndPickPre	OMPL plan to the pre-pick pose under a panda_joint3 ±45° path constraint, looped 5×. See Known Issues about the retry flag and the constraint
5	suctionPick	Publish grip, descend along a Cartesian path at suction_velocity, asyncExecute, and stop as soon as the vacuum exceeds vacuum_thr (3 s timeout)
6	attachObject	Simulation only — attach cylinder1 to the link_attach link
7	upAfterPick	Cartesian lift to (0.555, 0.0, 0.15) with SLERP orientation blending, 20 waypoints
8	placeObject	Cartesian move above the carrier, then publish release
9	detachObject	Simulation only
10	goToStartPoseJoint	20-step joint interpolation, time-parameterized with IPTP

Runtime Interfaces

Type	Name	Message	Direction
Service	/get_suction_points	icir_rpp_msgs/srv/GetSuctionPoints	client (icir_rpp) → server (icir_rpp_server)
Topic	/vg_command	std_msgs/String (grip / release)	icir_rpp → onrobot_vg_control
Topic	/vg_channelA_vacuum	std_msgs/Int32 (1/1000 vacuum)	onrobot_vg_control → icir_rpp
Topic	/pick_pose	geometry_msgs/Pose	optional manual override (see script/pick_pose_publisher.py)
Topic	planning_scene	moveit_msgs/PlanningScene	icir_rpp → move_group (object colours)

The service loop is selected by the #define is_srv at the top of main.cpp. Commenting it out switches the node to the /pick_pose topic interface instead.

---

6. Task Parameters

Edit icir_rpp/config/my_parameter_panda.yaml — no rebuild required with --symlink-install.

Parameter	Default	Description
planning_group	panda_arm_hand_vgc10	MoveIt planning group
link_attach	tip	Link the picked object is attached to
max_velocity / max_acceleration	0.6	Scaling for transit motions
suction_velocity / suction_acceleration	0.02	Scaling for the grasp descent
vacuum_thr	200.0	Vacuum level counted as a successful grasp
suction_offset	0.03 m	Stand-off distance of the pre-pick pose along local −Z
carrier_position_*, carrier_box_*	—	Place-target tray pose and size
tote_box_position_*, tote_box_mesh_scale_factor	—	Tote-box mesh pose and scale
cylinder_height / cylinder_radius	0.08 / 0.02 m	Simulated object dimensions
x_range, y_range, z_range, roll_range, pitch_range, yaw_range	—	Randomization ranges for the simulated object

All parameters are required: a missing key raises std::invalid_argument at construction. The YAML's top-level key must match the node name set in the launch file (icir_rpp_panda_vgc10_node, icir_rpp_kr10_r1440_2_vgc10).

Joint limits. The task node time-parameterizes trajectories itself with IterativeParabolicTimeParameterization, so joint_limits.yaml only takes effect if the launch file passes moveit_config.joint_limits in the node's parameters=[...] list — configuring it on MoveItConfigsBuilder alone is not enough. MoveIt reads the limits from the robot_description_planning.joint_limits.* parameters of that node. The Panda task launch deliberately does not pass it — see Known Issues.

---

7. Sim vs. Real

Two independent axes decide the behaviour.

Hardware plugin — chosen by which move_group launch you start:

	Simulation	Real
Launch	move_group_panda_vgc10.launch.py	move_group_panda_vgc10_real.launch.py
xacro	panda_vgc10.urdf.xacro	panda_vgc10_real.urdf.xacro
Plugin	mock_components/GenericSystem	franka_hardware/FrankaHardwareInterface
Command interface	position	effort (+ per-joint PID gains)
update_rate	100 Hz	1000 Hz
Controllers	ros2_controllers_vgc10.yaml	ros2_controllers_vgc10_real.yaml

Task behaviour — the is_real argument of icir_rpp_panda_vgc10.launch.py, independent of the above:

is_real	Behaviour
false	Spawn a random cylinder, derive the pick pose from it, attach/detach in the scene
true	No cylinder; pick pose comes from the service, pre-pick derived via suction_offset

---

8. Troubleshooting

PackageNotFoundError: package '…_description' not found

• MoveItConfigsBuilder resolves the URDF package from <moveit_config>/.setup_assistant in its constructor, before any .robot_description() override applies. The file must name a package that exists. Rebuild the moveit_config package after editing.

Service [get_suction_points] not available

• Start icir_rpp_server's pick_pose_server.py (or your perception node) before the controller. The node waits 10 s at start-up and then keeps retrying in the main loop.

Gripper node fails with ImportError: pymodbus.client.sync

• pymodbus 3.x moved that module. Install pymodbus<3.0.

Cartesian path generation failed: xx% complete

• The straight-line descent or lift is blocked or out of reach. Check the pick pose against the tote-box and wall collision objects, and verify suction_offset.

Missing required parameter: …

• The YAML top-level key no longer matches the node name= in the launch file. They must be identical.

getCurrentState() returned nullptr (/joint_states 미수신)

• move_group / ros2_control is not running, or use_sim_time is true while nothing publishes /clock. The node now reports this and exits instead of segfaulting.

Velocity/acceleration limits appear to be ignored

• Expected: the task node does not receive robot_description_planning, so it falls back to the URDF limits. This is deliberate — see Known Issues.

Known Issues

The items below are known defects that are deliberately left in place. The real-robot experiments were validated against exactly this behaviour, and fixing any of them changes what the arm actually does, which would require re-running those experiments. Each is marked with a NOTE: comment at the source location.

• joint_limits_vgc10.yaml never reaches the task node — icir_rpp_panda_vgc10.launch.py omits moveit_config.joint_limits, so the node's RobotModel uses the URDF limits instead (j1–j4 2.3925 vs 2.1750, j5–j7 2.8710 vs 2.6100, and no acceleration limits at all, which makes IPTP fall back to its internal default). Passing it changes the speed profile of every trajectory, including the suction descent. The real-robot move_group has the same problem for a different reason: move_group_panda_vgc10_real.launch.py:120 passes the raw joint_limits_yaml dict, so the parameters land as joint_limits.* instead of robot_description_planning.joint_limits.* and MoveIt never reads them. Only the simulation move_group launch gets the limits, via moveit_config.to_dict().
• planAndPickPre() retry flag is inverted — success = plan(...) != SUCCESS, so the loop does not stop at the first successful plan; it runs all 5 iterations and executes the last one. Fixing it would execute a different (also valid) OMPL trajectory.
• The vacuum counter is cumulative, not consecutive — suctionVacuumCallback never resets vacuum_success_counter_ when the reading drops below vacuum_thr, so five scattered samples above the threshold are enough to signal success. Resetting it would shift the grasp-success timing.
• suction_vacuum_ is never reset between cycles — suctionPick()'s suction_cv_.wait_for(lock, 3s, pred) evaluates its predicate before waiting, and suction_vacuum_ still holds the last value published by the previous cycle. If the vacuum has not vented below vacuum_thr by then, the wait returns true immediately and the cycle reports a successful grasp before the arm has even descended. Only shows up on the real robot over repeated cycles (there is no vacuum topic in simulation). Resetting it at the top of suctionPick() is a one-line fix, but it changes when a grasp is declared successful.
• suctionPick() does not stop the descent on timeout — move_group_.stop() is called only in the success branch, so on a vacuum timeout asyncExecute keeps descending while the sequence moves on to upAfterPick, which then calls setStartStateToCurrentState() on a moving arm. Stopping it would change what the arm does whenever a grasp fails.
• The panda_joint3 path constraint is never cleared — planAndPickPre() sets it and does not call clearPathConstraints(), so it stays applied to the goToStartPose() recovery plans for the rest of the cycle. Clearing it would change those recovery trajectories.
• init_joint_model_group_() runs before the executor thread starts — it works because the default is_sim_time:=true freezes the node clock at 0, which makes MoveIt's state-staleness check pass trivially. A null check was added so it now reports and exits instead of segfaulting, but the call position is unchanged.

---

License

Apache-2.0 — see LICENSE.

The packages under icir_rpp_resources/ are derived from ros-planning/moveit_resources; see icir_rpp_resources/UPSTREAM.md for the fork point and the list of modifications. onrobot_vg_control is a ROS 2 port of the OnRobot VG ROS 1 driver.

---

Contact

Jinseong Park js.park@cnu.ac.kr