URDF2CASADI

A module for generating the forward kinematics and dynamics of a robot from a URDF. It can generate the forward kinematics represented as a dual quaternion or a transformation matrix. urdf2casadi works in python 3, and any platform that supports CasADi and urdf_parser_py.

Other libraries

This module is implemented in Python, and was intended to explore a CasADi approach to forward kinematics and rigid body dynamics algorithms based on URDFs. For a more real-time control applicable alternative, consider the Pinocchio library.

Differences to the original codebase

• new function to get inertia data only
• add patch to get Jacobian
• refactoring
• update dependencies to newer versions

Caveats

• maybe non-diagonal inertial values are not correctly handled
• external forces are not handled correct (i_X_0 is missed in some function invocations)

Installation

With ROS:

1. Get ROS (actually anything that installs urdfdom_py/urdf_parser_py will do).
2. Get CasADi (e.g. pip install casadi).
3. Run pip install . in the folder.

Without ROS:

1. Change the urdfdom-py to urdf-parser-py in requirements.txt (line 3) and in setup.py (line 20).
2. Get CasADi (e.g. pip install casadi).
3. Run pip install . in the folder (with additional option --user you can specify that it is a local install).

Install with poetry:

1. git clone urdf2casadi
2. Install poetry as described on the website.
3. Run poetry install in the directory.

Usage example

import casadi as cs
from urdf2casadi import urdfparser as u2c
urdf_path = "../urdf/ur5_mod.urdf"
root_link = "base_link"
end_link = "tool0"
robot_parser = u2c.URDFparser()
robot_parser.from_file(urdf_path)
# Also supports .from_server for ros parameter server, or .from_string if you have the URDF as a string.
fk_dict = robot_parser.get_forward_kinematics(root_link, end_link)
print(fk_dict.keys())
# should give ['q', 'upper', 'lower', 'dual_quaternion_fk', 'joint_names', 'T_fk', 'joint_list', 'quaternion_fk']
forward_kinematics = fk_dict["T_fk"]
print(forward_kinematics([0.3, 0.3, 0.3, 0., 0.3, 0.7]))

Citation

The results were published in "Robot Dynamics with URDF & CasADi" at ICCMA 2019. [Preprint]

@inproceedings{urdf2casadi,
  title={Robot Dynamics with URDF \& CasADi},
  author={Johannessen, Lill Maria Gjerde and Arbo, Mathias Hauan and Gravdahl, Jan Tommy},
  booktitle={2019 7th International Conference on Control, Mechatronics and Automation (ICCMA)},
  year={2019},
  organization={IEEE}
}

Todo/Implementation status

• Forward kinematics with SE(3) matrix
• Forward kinematics of rotation with quaternion
• Dual Quaternions as alternative to SE(3) matrices
• Dynamics from links and their inertia tags
• Denavit Hartenberg?
• Move numerical to a test folder
• Examples