UR16e Motion Planning and Control

From an RRT detour to dynamic execution

Use the video controls to play, pause and seek.

The challenge is more than finding a path: interpolation and tracking errors can move a robot outside its planned corridor. This Python pipeline keeps timing on the checked joint-space edges, executes them with a nonlinear controller, then checks the reference and recorded motion independently.

Public GitHub repositoryTechnical report and commands

A consistent six-joint robot model

The supplied URDF defines the six revolute joints, inertial properties, visual meshes and collision meshes. Python loads that model directly, resolves its mesh paths and provides forward kinematics, Jacobians, numerical inverse kinematics, rigid-body dynamics and torque-driven simulation.

Frame checks reconcile standard DH with URDF tool0 expressed in the UR controller base frame. That base is rotated 180° around Z from the simulation's base_link. COM positions and inertia tensors are transformed into consistent frames instead of mixing hand-coded tables with URDF values.

Obstacle avoidance: plan first, then verify the motion

A joint-space RRT samples configurations, checks each extension along its edge, and connects a detour to the goal. Shortcut smoothing is checked again at finer spacing. Each remaining edge receives a quintic time law with zero velocity and acceleration at its endpoints, so the time parameterization follows the checked geometry instead of cutting corners.

All three found paths; the direct edge failed collision checking. Timings exclude smoothing and post-validation. The first seed's timed path was dynamically tracked. At 5 ms state samples, reference and actual minimum clearance were both 18.349 mm over the declared environment/self pairs; refined interval checks also found no collisions.

Matched torque-driven controller comparison

A separate six-second joint reference compares PD plus nominal gravity compensation with computed torque on the same nonlinear model. Both start with the same position offset and zero velocity, use the same 0.25 ms timestep, and share actuator bounds. This experiment contains no obstacles.

PD adds joint error feedback to gravity compensation. Computed torque uses nominal inverse dynamics to request desired acceleration plus feedback. Their gain units differ, so matching numerical gain values would not make the controllers equivalent.

Computed torque achieves a smaller nominal final error, but PD has lower pooled transient RMS in this configuration. Neither controller saturates in these converged runs.

Controller equations, gains and applied torques

PD: τ = Kp(qd−q) + Kd(vd−v) + gnom(q)
CT: τ = IDnom(q,v,ad + 64(qd−q) + 16(vd−v))

PD Kp=[180,180,120,50,30,20] Nm/rad; Kd=[30,35,22,8,5,3] Nm·s/rad. CT gains are acceleration-space gains, fixed across nominal and payload cases. An initial 1 ms stiff-wrist numerical failure and the corrected timestep/convergence checks remain in the report.

What changes with an unmodeled payload?

Add 1 kg and a declared rotational inertia at tool0 while keeping both controllers' nominal models and gains unchanged. The computed-torque pooled RMS increases to 0.186758 rad, with joint-5 peak error about 0.709 rad. PD also degrades, to 0.028008 rad pooled RMS.

The equations, gain conventions, payload application and matched settings were checked, and the Python command reproduced the saved results. The payload test measures model-mismatch sensitivity with the same gains and reference trajectory.

Verification beyond a dynamics round trip

An independent implementation builds mass from COM Jacobians and kinetic energy, gravity from potential-energy derivatives, and velocity terms from mass derivatives. Those quantities are compared with the rigid-body engine, so validation does not rely only on forward/inverse routines sharing the same mistake.

• Maximum independent mass-matrix discrepancy: 6.22e−15; sampled mass matrices are symmetric positive definite.
• Gravity discrepancy: 6.41e−9 Nm; Jacobian finite-difference residual: 1.66e−10.
• Local numerical FK–IK residuals, wrench signs and actual forward-step acceleration are checked.
• Seven tests pass; integrated timestep convergence is checked independently of the older nominal/payload study.

The checks compare model quantities across independently sampled configurations.

About the project

Completed during my software engineering internship at UAES. The Python implementation integrates URDF/frame reconciliation, dynamics corrections, independent checks, planning/control experiments, payload studies and mesh-based visualization. Original MATLAB/Simulink sources are retained in the repository.