Forward and Inverse Kinematics
Learning Objectives
- Understand the mathematical relationship between joint angles and end-effector positions
- Perform forward kinematics calculations to determine end-effector pose from joint angles
- Solve inverse kinematics problems to determine joint angles for desired end-effector poses
- Identify the differences between analytical and numerical inverse kinematics solutions
- Recognize the practical applications of forward and inverse kinematics in robotics
Prerequisites: This chapter builds on the ROS 2 concepts covered in Module 1: The Robotic Nervous System and the robot structure fundamentals from Module 2: Robot Links, Joints, and Coordinate Frames. If you're new to ROS 2 or robot structure concepts, we recommend reviewing those chapters first.
Introduction to Kinematics
Kinematics is the study of motion without considering the forces that cause it. In robotics, kinematics deals with the relationship between joint angles and the position and orientation of the robot's end-effector. Understanding kinematics is fundamental to controlling robot movement and planning robot trajectories.
Key Concepts: Forward kinematics (calculating end-effector pose from joint angles) and inverse kinematics (calculating joint angles from desired end-effector pose).
Forward Kinematics: From Joint Angles to End-Effector Pose
Forward kinematics is the process of calculating the position and orientation of a robot's end-effector given the joint angles. This is typically a straightforward calculation that follows the kinematic chain from the base to the end-effector.
Mathematical Foundation
The forward kinematics problem involves transforming coordinates from joint space to Cartesian space. For each joint in the kinematic chain, we apply a transformation matrix that depends on the joint's type and current angle (or position).
Denavit-Hartenberg Convention
The Denavit-Hartenberg (DH) convention provides a systematic method for assigning coordinate frames to robot joints. Each joint is described by four parameters:
- a: Link length (distance along x-axis)
- α: Link twist (angle about x-axis)
- d: Link offset (distance along z-axis)
- θ: Joint angle (angle about z-axis)
Using these parameters, we can construct transformation matrices for each joint:
T_i = [cos(θ_i) -sin(θ_i)*cos(α_i) sin(θ_i)*sin(α_i) a_i*cos(θ_i)]
[sin(θ_i) cos(θ_i)*cos(α_i) -cos(θ_i)*sin(α_i) a_i*sin(θ_i)]
[0 sin(α_i) cos(α_i) d_i ]
[0 0 0 1 ]
Homogeneous Transformation Matrices
The complete forward kinematics solution is obtained by multiplying all the individual transformation matrices:
T_total = T_1 * T_2 * T_3 * ... * T_n
Where T_total represents the complete transformation from the base frame to the end-effector frame.
Practical Forward Kinematics Example
For a simple 2-DOF planar manipulator with revolute joints:
- Define the joint angles θ1 and θ2
- Define the link lengths L1 and L2
- Calculate the end-effector position (x, y):
x = L1 * cos(θ1) + L2 * cos(θ1 + θ2)
y = L1 * sin(θ1) + L2 * sin(θ1 + θ2)
Implementation in Robotics
Forward kinematics is typically implemented using:
- ROS tf2 library: For coordinate frame transformations
- KDL (Kinematics and Dynamics Library): For general kinematic calculations
- Custom matrix libraries: For specialized applications
Inverse Kinematics: From End-Effector Pose to Joint Angles
Inverse kinematics is the process of calculating the joint angles required to achieve a desired end-effector position and orientation. This is typically more complex than forward kinematics and may have multiple solutions or no solution at all.
Mathematical Foundation
The inverse kinematics problem involves finding joint angles θ = [θ1, θ2, ..., θn]T such that the end-effector achieves a desired pose T_desired. This is expressed as:
f(θ) = T_desired
Where f(θ) represents the forward kinematics function.
Analytical vs. Numerical Solutions
Analytical Solutions
Analytical solutions involve deriving closed-form equations for the joint angles. These are exact solutions but are only possible for robots with specific geometries:
- Geometric methods: Using geometric relationships and trigonometry
- Algebraic methods: Solving systems of polynomial equations
- Pieper's solution: For robots with 6 DOF where 3 joint axes intersect
Numerical Solutions
Numerical methods are iterative approaches that work for more general robot configurations:
- Jacobian-based methods: Using the robot's Jacobian matrix to iteratively approach the solution
- Gradient descent: Minimizing the error between current and desired poses
- Cyclic Coordinate Descent (CCD): Adjusting one joint at a time to minimize error
Jacobian-Based Inverse Kinematics
The Jacobian matrix J(θ) relates joint velocities to end-effector velocities:
v = J(θ) * θ_dot
Where v is the end-effector velocity and θ_dot is the joint velocity vector.
To solve for joint angles, we can use the pseudoinverse of the Jacobian:
θ_dot = J#(θ) * v
Where J# is the pseudoinverse of the Jacobian.
Common Inverse Kinematics Challenges
Multiple Solutions
Many robots have redundant DOF, leading to multiple valid joint configurations for the same end-effector pose.
Singularities
At singular configurations, the Jacobian becomes non-invertible, making it impossible to find a unique solution.
No Solution
If the desired pose is outside the robot's workspace, no solution exists.
Joint Limits
Solutions must respect physical joint limits, which may exclude mathematically valid solutions.
Implementation Approaches
Closed-Form Solutions
For simple robots (e.g., 6-DOF arms with spherical wrist), closed-form solutions can be derived using geometric or algebraic methods.
Iterative Methods
For complex robots, iterative methods like Newton-Raphson or gradient descent are used:
θ_{k+1} = θ_k + α * J#(θ_k) * (x_desired - f(θ_k))
Where α is a step size parameter and k is the iteration number.
Practical Implementation Examples
Using KDL for Kinematics
The Kinematics and Dynamics Library (KDL) provides tools for both forward and inverse kinematics:
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainiksolver.hpp>
// Create a chain for your robot
KDL::Chain chain;
// Add segments to the chain
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame(KDL::Vector(0, 0, 0))));
// Forward kinematics solver
KDL::ChainFkSolverPos_recursive fksolver(chain);
// Inverse kinematics solver
KDL::ChainIkSolverPos_NR iksolver(chain, fksolver, iksolvervel);
// Solve for joint positions
KDL::JntArray joint_pos_in, joint_pos_out;
KDL::Frame pose_out;
fksolver.JntToCart(joint_pos_in, pose_out);
Using MoveIt! for Inverse Kinematics
MoveIt! provides high-level tools for motion planning including inverse kinematics:
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/kinematics_base/kinematics_base.h>
// Load robot model
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
// Create kinematic state
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
// Get end-effector pose
const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform("end_effector");
// Solve inverse kinematics
std::vector<double> joint_values;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
Python Implementation Example
import numpy as np
from scipy.optimize import fsolve
def forward_kinematics(joint_angles, link_lengths):
"""Calculate end-effector position for a 2-DOF planar manipulator"""
theta1, theta2 = joint_angles
L1, L2 = link_lengths
x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
return np.array([x, y])
def inverse_kinematics(target_pos, link_lengths):
"""Solve inverse kinematics for a 2-DOF planar manipulator"""
L1, L2 = link_lengths
x, y = target_pos
r = np.sqrt(x**2 + y**2)
# Check if target is reachable
if r > L1 + L2:
raise ValueError("Target position is outside the workspace")
# Calculate second joint angle
cos_theta2 = (r**2 - L1**2 - L2**2) / (2 * L1 * L2)
sin_theta2 = np.sqrt(1 - cos_theta2**2)
theta2 = np.arctan2(sin_theta2, cos_theta2)
# Calculate first joint angle
k1 = L1 + L2 * np.cos(theta2)
k2 = L2 * np.sin(theta2)
theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
return np.array([theta1, theta2])
Kinematics in ROS 2
TF2 for Coordinate Transformations
The tf2 library in ROS 2 handles coordinate frame transformations, which are essential for kinematics:
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
// Create a transform listener
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_{tf_buffer_};
// Get transform between frames
geometry_msgs::msg::TransformStamped transform;
try {
transform = tf_buffer_.lookupTransform("base_link", "end_effector",
tf2::TimePointZero);
} catch (const tf2::TransformException & ex) {
RCLCPP_ERROR(node_->get_logger(), "Could not get transform: %s", ex.what());
}
Using the Robot State Publisher
The robot_state_publisher node uses joint positions to publish the robot's forward kinematics:
<node pkg="robot_state_publisher" exec="robot_state_publisher" name="robot_state_publisher">
<param name="robot_description" value="$(var robot_description)"/>
<param name="publish_frequency" value="50.0"/>
</node>
Workspace Analysis
Reachable Workspace
The reachable workspace is the set of all positions that the end-effector can reach, regardless of orientation. For a 2-DOF planar manipulator with link lengths L1 and L2:
- Maximum reach: L1 + L2
- Minimum reach: |L1 - L2|
- Workspace area: π * [(L1 + L2)² - (L1 - L2)²]
Dexterous Workspace
The dexterous workspace is the set of positions where the end-effector can achieve any orientation. This is typically a subset of the reachable workspace.
Singularity Analysis
Singularities occur when the robot loses one or more degrees of freedom. At these configurations, the Jacobian matrix becomes singular (non-invertible).
Types of Singularities
- Boundary singularities: Occur at the edge of the workspace
- Interior singularities: Occur within the workspace
- Wrist singularities: Involving wrist joints (for 6-DOF arms)
Detecting Singularities
Singularities can be detected by examining the determinant of the Jacobian matrix:
import numpy as np
def is_singular(jacobian, threshold=1e-6):
"""Check if the Jacobian is singular"""
det = np.linalg.det(jacobian)
return abs(det) < threshold
Kinematics Constraints and Optimization
Joint Limit Constraints
Robot joints have physical limits that must be respected:
def check_joint_limits(joint_angles, joint_limits):
"""Check if joint angles are within limits"""
for i, (angle, (min_limit, max_limit)) in enumerate(zip(joint_angles, joint_limits)):
if angle < min_limit or angle > max_limit:
return False, f"Joint {i} exceeds limits: {angle} not in [{min_limit}, {max_limit}]"
return True, "All joints within limits"
Optimization with Constraints
When multiple solutions exist, optimization techniques can be used to select the best configuration:
- Minimize joint displacement from current configuration
- Avoid joint limits
- Optimize for manipulability
- Consider obstacle avoidance
Troubleshooting Guide for Kinematics Issues
Forward Kinematics Problems
Issue: Calculated end-effector pose doesn't match expected values Solutions:
- Verify DH parameters are correctly defined for your robot
- Check coordinate frame conventions (right-hand rule)
- Validate transformation matrix calculations
- Confirm joint angle units (radians vs degrees)
- Verify parent-child relationships in the kinematic chain
- Test with simple configurations where the answer is known
Inverse Kinematics Problems
Issue: No solution found for desired end-effector pose Solutions:
- Check if the target pose is within the robot's workspace
- Verify joint limits aren't preventing a solution
- Try different initial joint configurations
- Use numerical methods if analytical solutions fail
- Consider approximate solutions if exact solutions aren't required
Issue: Multiple valid solutions - which one to choose? Solutions:
- Implement optimization criteria (closest to current configuration)
- Use joint centering to avoid limits
- Consider obstacle avoidance in configuration space
- Apply task-specific preferences (e.g., elbow up vs down)
Singularity Issues
Issue: Robot becomes uncontrollable at certain configurations Solutions:
- Detect singularities using Jacobian determinant
- Implement singularity avoidance strategies
- Use damped least squares method instead of pseudoinverse
- Plan paths that avoid singular configurations
- Add small perturbations to move away from singularities
Numerical Convergence Problems
Issue: Inverse kinematics solution doesn't converge Solutions:
- Adjust step size parameters in iterative algorithms
- Improve initial guess for joint angles
- Use better conditioned Jacobian formulations
- Implement line search methods to ensure convergence
- Check for numerical stability in matrix operations
Advanced Topics in Kinematics
Redundant Manipulators
Robots with more degrees of freedom than required for a task are called redundant. This provides additional flexibility but requires additional constraints to solve inverse kinematics:
θ_dot = J# * x_dot + (I - J# * J) * y
Where y is an arbitrary vector that can be chosen to optimize secondary objectives.
Kinematic Calibration
Kinematic parameters may not exactly match the physical robot. Calibration involves determining actual DH parameters through measurement:
- Open-loop methods: Measure end-effector position with external sensors
- Closed-loop methods: Use robot's own sensors to determine parameters
- Optimization-based methods: Minimize error between calculated and measured poses
Kinematics for Different Robot Types
Serial Manipulators
- Most common robot type
- Single kinematic chain from base to end-effector
- Well-studied kinematic solutions
Parallel Manipulators
- Multiple kinematic chains connecting base to end-effector
- Higher rigidity and precision
- More complex kinematic solutions
Mobile Manipulators
- Combine mobile base with manipulator arm
- Extended kinematic models include base pose
- More complex workspace analysis
Hands-On Exercises for Kinematics
Exercise 1: Forward Kinematics Implementation
Objective: Implement forward kinematics for a simple 2-DOF planar manipulator.
Instructions:
- Define link lengths for your manipulator (e.g., L1 = 1.0m, L2 = 0.8m)
- Implement the forward kinematics equations in Python or C++
- Test with various joint angle combinations
- Visualize the robot configuration and end-effector position
- Verify your calculations match geometric expectations
Expected Outcome: A working forward kinematics implementation with visualization.
Exercise 2: Inverse Kinematics Solution
Objective: Solve inverse kinematics for the same 2-DOF manipulator.
Instructions:
- Implement the inverse kinematics equations for your 2-DOF robot
- Handle both possible solutions (elbow up and elbow down)
- Test with various end-effector positions within the workspace
- Verify solutions by applying forward kinematics to the result
- Check that joint limits are respected
Expected Outcome: A complete inverse kinematics solver with multiple solution handling.
Exercise 3: Workspace Analysis
Objective: Analyze and visualize the workspace of your robot.
Instructions:
- Calculate the theoretical workspace boundaries
- Generate random joint configurations and plot resulting end-effector positions
- Identify areas with multiple solutions vs. single solutions
- Visualize the workspace in 2D or 3D
- Compare theoretical vs. actual workspace considering joint limits
Expected Outcome: A comprehensive workspace analysis with visualizations.
Verification Steps for Kinematics Implementation
Forward Kinematics Verification
- Known Configuration Test: Test with joint angles where end-effector position is known (e.g., all zeros)
- Geometric Validation: Verify results match geometric calculations
- Continuity Check: Ensure small changes in joint angles result in small changes in end-effector position
- Range Validation: Confirm end-effector positions are within expected workspace
- Coordinate Frame Verification: Check that orientation calculations are correct
Inverse Kinematics Verification
- Round-Trip Test: Solve IK for a pose, then FK for the resulting joint angles - should match original pose
- Workspace Check: Verify that unreachable poses are properly detected
- Multiple Solutions: Test configurations with multiple valid solutions
- Singularities: Test behavior near singular configurations
- Joint Limits: Confirm solutions respect physical joint constraints
Performance Testing
- Computation Time: Measure execution time for real-time applications
- Convergence Rate: For iterative methods, measure iterations to convergence
- Robustness: Test with various starting configurations
- Accuracy: Measure how closely solutions achieve desired poses
- Stability: Test behavior with noisy input data
"Try This" Experiments
Experiment 1: Humanoid Arm Kinematics
- Find a humanoid robot model (like the ROS 2 example models)
- Identify the kinematic chain for one arm
- Calculate the number of degrees of freedom
- Determine the workspace of the arm
- Consider how the torso and other joints affect the overall workspace
Experiment 2: Jacobian Analysis
- Implement the Jacobian calculation for a simple robot
- Visualize how the Jacobian changes with different configurations
- Identify where singularities occur
- Observe how manipulability changes across the workspace
- Relate mathematical properties to physical robot behavior
Experiment 3: Real Robot Comparison
- If you have access to a physical robot, compare simulation to reality
- Measure actual end-effector positions vs. calculated positions
- Identify sources of error (calibration, flexibility, etc.)
- Consider how to improve model accuracy
- Document differences between theoretical and actual kinematics
Real-World Connections
Forward and inverse kinematics are fundamental to many robotics applications:
- Industrial Robotics: Precise positioning for assembly and welding tasks
- Medical Robotics: Surgical robots require accurate kinematic control
- Agricultural Robotics: Manipulation tasks like harvesting require kinematic solutions
- Service Robotics: Mobile manipulators use kinematics for object interaction
- Research Robotics: New robot designs require kinematic analysis for control
Summary
Forward and inverse kinematics form the mathematical foundation for robot control and motion planning. Forward kinematics calculates end-effector pose from joint angles, while inverse kinematics determines joint angles for desired end-effector poses. Understanding both analytical and numerical methods for solving these problems is essential for effective robot programming. The next section will cover joint constraints and motion limits that affect how robots can actually move.