UR5 Kinematic Simulation

An implementation of numerical inverse kinematics numerical method, based on the body frame for a UR5 manipulator, validated in CoppeliaSim.

This repository contains a small MATLAB project for simulating inverse kinematics of a UR5 manipulator using an iterative body-frame method.

This project comes from the Modern Robotic Course 2: Robot Kinematics, so this simulation is based on the use of its theory and the MR library code.

The main idea of this project is to start, from an initial joint configuration, using the Newton-Raphson methodology update this joint configuration until the end-effector reaches a desired pose specified by the matrix T_sd considering a orientation and pose tolerance.

Project Purpose

This repository implements a body-frame inverse kinematic numerical approach which is based on:

• a set of screw-axis vectors for the robot joints (B),
• the home configuration of the end-effector (M),
• a desired target pose (Tsd)
• and the iterative update using the body Jacobian and its pseudoinverse.

Method

The solver implements the body-frame Newton-Raphson iteration (Modern Robotics, Algorithm 6.2):

$$\theta_{i+1} = \theta_i + J_b^\dagger(\theta_i) \cdot V_b$$

where:

• $J_b(\theta_i)$ is the Jacobian evaluated at the current joint configuration,
• $J_b^\dagger$ is its Moore-Penrose pseudoinverse,
• $V_b$ is the body-frame twist error.

At each iteration, the forward kinematics FKinBody computes the current end-effector pose, the error twist $V_b$ is derived via the matrix logarithm, and the joint angles are updated until both the angular and linear components of $V_b$ fall below the specified tolerances (tol_rot, tol_pos) or maxiter is reached.

Visual Preview

In the following section, a couple of media files that showcase the performance of this methodology in Coppelia Simulator:

From the code testing located in this repository, the following simulation is presented, visualizing the behavior of the manipulator during the different iterations provided by the numerical methodology:

Watch the simulation video

Repository Contents

• IKinBodyIterates.m - Main function of this repository, performs the iterative process of inverse kinematic and creates the file iterates.csv and log.txt.
• iterates.csv - An output file that contains the joint-angle values from each iteration that can be read in Coppelia SIM.
• log.txt - Log generated by the script during the execution, contain the entire information provided in the command window.
• Scene2_UR5_csv.ttt - A prepared scene for Coppelia SIM that allows to read different iterations of joint configurations in a .csv file.
• Testing.m - An example script that defines the robot parameters, target pose, and initial guess, to then runs the solver.
• screenshot.png and video.mp4 - Visual assets for the demonstration.

Prerequisites

This code was developed using Matlab 2025a, and for its execution you must have the Modern Robotics (MR) library installed, because this project uses functions such as:

• FKinBody
• MatrixLog6
• TransInv
• se3ToVec

How to Run

1. Open MATLAB.
2. Navigate to this repository folder.
3. Run the example script:

run('Testing.m')

4. Open Coppelia SIM
5. Open scene Scene2_UR5_csv.ttt
6. load the .csv file extracted from the example script inside the simulation.

What the Script Does

The main work of this repository comes from Testing.m where it's defines:

• Link geometry values,
• the home end-effector transform M,
• the joint screw axes based on the body-frame B,
• the target pose T,
• the initial guess theta0,
• the maximum number of iterations allowed,
• and the tolerances for rotation and position.

It then calls:

[thetalist, success] = IKinBodyIterates(B, M, T, theta0,...
                                        tol_rot, tol_pos, maxiter)

Output

When the script runs successfully, it will:

• print the iteration history in the command window,
• display the end-effector pose and error twist at each step,
• save the joint-angle values to iterates.csv,
• and create a log in log.txt.

Notes

If you want to test this code in different situations, the following values inside Testing.m can be modify:

• B – joint screw axes,
• M – home end-effector pose,
• T – desired target pose,
• theta0 – initial joint-angle guess,
• maxiter - maximum number of iterations allowed,
• tol_rot and tol_pos – convergence tolerances.

Author

Sebastián Muñoz Jirón - Mechatronics Engineer, M.Sc. in Engineering Sciences (Mechanical Engineering), Universidad de Santiago de Chile.

LinkedIn · GitHub

License

This project is licensed under the MIT License - see the LICENSE file for details.