Overview
This project implements a modular Motion Control and Task Execution framework for the KUKA KR210 in ROS 2 Humble. It focuses on robot kinematics, structured task logic, and scalable communication patterns suitable for industrial-grade manipulation tasks.
The stack includes:
Forward Kinematics (FK) Deterministic computation of end-effector pose from joint states using DH parameters. Inverse Kinematics (IK) A hybrid solver (analytical + numerical fallback) enabling 6-DoF pose targeting with singularity checks and joint-limit enforcement. Behavior Tree Task Planning High-level robot behaviors (pick, place, move, home, error recovery) expressed using a modular BT architecture. Action Server for Synchronous Motion Control Implements a ROS 2 Action interface to ensure safe, confirmable, feedback-rich motion sequences. RViz + Gazebo Integration Full visualization and simulation pipeline for trajectory validation. Modular Nodes kinematics_node: FK/IK computation services motion_action_server: Sends joint trajectories to controllers task_planner_bt: Orchestrates sequences via behavior trees trajectory_generator: Smooth cubic and quintic interpolators state_monitor: Joint/TF health and safety checks URDF + SRDF Robot Model Accurate KR210 geometry, inertia, and kinematic structure for simulation and planning (MoveIt-compatible). Features 1. Kinematics Forward Kinematics Implemented using modified DH parameters Produces 4×4 transformation matrices for each link Validated against MoveIt and Gazebo TF outputs Inverse Kinematics Analytical solutions for shoulder/elbow/wrist configurations Branch selection (elbow-up/down, wrist-flip) Collision-aware numerical IK fallback using gradient descent Singularities detected through Jacobian determinant checks 2. Task Planning with Behavior Trees
Task execution is implemented as a Behavior Tree (BT) using an extensible node library:
BT Nodes Implemented
MoveToPose (calls IK → Action Server) MoveToJointTargets CheckGripperStatus PickObject PlaceObject ReturnHome RetryOnFailure / Fallback nodes for robustness
Advantages of using BTs
Reactive and modular Easy to extend with new tasks Fail-safe execution flow Debuggable using Groot 3. Action Server for Synchronous Control
A ROS 2 Action Server manages all robot motion commands.
Action Server Responsibilities
Receives goal poses or joint targets Computes IK (or accepts joint input directly) Sends trajectory to controllers Provides real-time feedback (current pose, remaining error) Supports goal cancellation / preemption Ensures deterministic robot behavior
This gives the system clean decoupling between planning (BT) and motion execution (Actions).
- Simulation & Visualization Gazebo: Full KR210 + world environment RViz2: Joint/TF visualization, path overlays MoveIt 2 (optional): Benchmarking against planner solutions
- Safety & Monitoring Joint-limit enforcement Collision checks in IK Velocity/acceleration bounds during trajectory generation Emergency stop topic monitored by state_monitor Automatic recovery behaviors in BT