Forward Kinematics
Chain URDF link transforms from the base through each joint to recover the current end-effector pose in Cartesian space — the reference for both tracking error and the EE marker publish.
Real-Time Inverse Kinematics
for 6-DOF Robot Arms
A numerical IK solver designed to map Cartesian coordinate setpoints into joint positions. Operating directly as a ROS2 node which delivers smooth manipulator trajectories at a deterministic 50 Hz.
Fully coordinates joint parameters for multi-revolute configurations, managing link transformations efficiently.
Executes at a deterministic update period to provide rapid trajectory convergence and steady state monitoring.
Leverages robust numerical matrix inversion to resolve positional updates while minimizing total joint displacements.
Integrates directly with ROS2 communication frameworks, publishing to robot tf stacks without intermediate layers.
The robot_controller node runs a closed-loop IK solve at 50 Hz and
publishes live joint and marker state into the ROS2 graph for RViz.
Chain URDF link transforms from the base through each joint to recover the current end-effector pose in Cartesian space — the reference for both tracking error and the EE marker publish.
/marker
visualization_msgs/Marker
Blue arrow — EE pose from forward kinematics
Subtract the EE pose from the Cartesian setpoint. If inside the deadband, the node samples a new target; outside it, the residual drives the Jacobian solve.
/setpoint
visualization_msgs/Marker
Red arrow — Cartesian setpoint goal
Nudge each joint slightly and re-run FK to measure how the EE moves, assembling a 3×6 sensitivity matrix without analytical interpretations.
Project the Cartesian error through the Moore-Penrose inverse of the Jacobian Matrix so the joint update reduces tracking error while preferring small displacements.
Apply gain scaling and per-joint clamps before committing the new configuration, preventing overshoot and velocity spikes near singularities.
/joint_states
sensor_msgs/JointState
Six joint angles after the safety-governed update
Subscribes to /setpoint, /marker and /joint_states for live visualization
Select an algorithm step below to inspect its functional Python code from the active solver node.
# Click a tab on the left to inspect the implementationMathematical structures and parameters of the numerical Inverse Kinematics solver.
Determining the current spatial position of the manipulator requires chaining coordinate frame mappings from the base mount to the end-effector. This homogeneous transformation matrix is resolved using the configuration vector:
This chaining function uses dimensions extracted from the robot geometry parameters file (URDF file) to convert joint rotations into a single 4x4 matrix defining Cartesian frame coordinate properties.
The Jacobian matrix maps velocities between joints' rotation space and Cartesian movement space:
Because the controller node needs to translate a desired movement vector into state updates for the manipulator joints, this relationship was solved using the Moore-Penrose pseudo-inverse representation:
This mathematical mapping identifies the joints' state updates that satisfy positional setpoints while minimizing displacement magnitude, helping resolve redundant joint paths.
To ensure that this controller node can run on multiple designs without manual configuration, the columns of the Jacobian are evaluated numerically. The joint configurations are perturbed by a small interval $\epsilon = 0.01$:
This numerical approximation extracts directional sensitivities using only the Forward Kinematics function, making the solver compatible with arbitrary joint structure modifications.
Control loops operating near singular physical limits can generate excessive joint velocity updates. The node incorporates stability scaling and bounding rules to maintain controlled motion:
| Control Parameter | Operational Setting | Functional Purpose |
|---|---|---|
| Fixed-Gain Scaling | 0.01 factor | Reduces step magnitude to prevent target overshoot |
| Output Clamping | ±0.125 rad limit | Prevents angular speed spikes near singularities |
| Convergence Threshold | 0.01m deadband | Settles solver updates once target proximity is reached |