cschindlbeck / urdf2casadi

Python library for getting a casadi function for the forward kinematics of a URDF chain

URDF2CASADI

A module for generating the forward kinematics of a robot from a URDF. It can generate the forward kinematics represented as a dual quaternion or a transformation matrix. urdf2casadi works both in python 2 and 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.

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 --user . 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 --user . in the folder (--user specifies 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