skidy - symbolic kinematics and dynamics generator

• skidy - symbolic kinematics and dynamics generator
  • 1. Installation
    • Install package from source
    • Dependencies
  • 2. Usage
    • 2.1. YAML and JSON
      • 2.1.1. Create robot model as YAML file
      • 2.1.2. Code generation using YAML
    • 2.2. Python
    • 2.3. URDF
  • 3. Unit testing
  • 4. Benchmarking
  • 5. License

Symbolic kinematics and dynamics model generation using Equations of Motion in closed form.

1. Installation

python3 -m pip install skidy

For installing the test dependencies use:

python3 -m pip install skidy[testing]

Install package from source

To install the package from source go to the root folder of the package:

cd /path/to/project

then use:

python3 -m pip install -e .

to install the package in editable mode or:

python3 -m pip install .

to install the package uneditable.

Note: The . is part of the command.

Dependencies

The project has the following dependencies:

• sympy (Version >= 1.8)

  python3 -m pip install --upgrade sympy

• numpy

  python3 -m pip install numpy

• urdf_parser_py (Version >= 0.0.4)

  python3 -m pip install --upgrade urdf_parser_py

• regex

  python3 -m pip install regex

• PyYAML

  python3 -m pip install PyYAML

• pylatex

  python3 -m pip install pylatex

• pydot

  python3 -m pip install pydot

For unit testing the following packages are additionally recommended:

• oct2py

  python3 -m pip install oct2py

  This requires a working octave installation on your system: Install on Ubuntu with:

  sudo apt install octave

• cython

  python3 -m pip install cython

• kinpy

  python3 -m pip install kinpy

• pinocchio

  python3 -m pip install pin

2. Usage

There are four ways to load your robot into the library:

1. using a YAML file
2. using a JSON file
3. directly in python
4. using URDF

For defining a robot with one of the first three options (YAML, JSON, python) the following parameters are required:

• 6D joint screw coordinates for any joint
• 4x4 body reference coordinates for any link
• 4x4 end-effector configuration w.r.t. last link body fixed frame in the chain

For generating the inverse dynamics the following two parameters are required:

• 3D gravity vector
• 6x6 Mass-inertia matrix for any link

Tree-like robot structures require the following graph description parameters additionally:

• parent link for any joint
• support graph for any joint
• (child links for any joint) -> currently not used

Using URDF the following three parameters are required:

• path to URDF file
• 4x4 end-effector configuration w.r.t. last link body fixed frame in the chain
• 3D gravity vector

2.1. YAML and JSON

NOTE: As JSON and YAML files represent the same data structure this documentation covers only YAML files. Use JSON accordingly and just replace yaml with json in all commands.

2.1.1. Create robot model as YAML file

There is a function to generate a template YAML file in which it is easy to modify the parameters for your robot. To generate your robot template use

python3 -m skidy --template [options] new_filename.yaml

or the python function skidy.generate_template_yaml(path, structure).

For [options] the option --structure is highly recommended. There you can define which joint types to use in the template. E.g. use --structure 'rrp' for a robot which has two revolute joints followed by one prismatic joint.

The command python3 -m skidy --template --structure 'rp' my_robot_template.yaml creates the following output file:

---
parent:
  - 0
  - 1

child:
  - [1]
  - []

support:
  - [1]
  - [1,2]

ee_parent: 2

gravity: [0,0,-g]

representation: spatial

joint_screw_coord:
  - type: revolute
    axis: [0,0,1]
    vec: [0,0,0]

  - type: prismatic
    axis: [0,0,1]

body_ref_config:
  - rotation:
      axis: [0,0,1]
      angle: 0
    translation: [0,0,0]

  - rotation:
      axis: [0,0,1]
      angle: 0
    translation: [0,0,0]

ee:
  - rotation:
      axis: [0,0,1]
      angle: 0
    translation: [0,0,0]

mass_inertia:
  - mass: m1
    frame: body
    inertia:
      Ixx: Ixx1
      Ixy: Ixy1
      Ixz: Ixz1
      Iyy: Iyy1
      Iyz: Iyz1
      Izz: Izz1
    com: [0,0,0]

  - mass: m2
    frame: body
    inertia:
      Ixx: Ixx2
      Ixy: Ixy2
      Ixz: Ixz2
      Iyy: Iyy2
      Iyz: Iyz2
      Izz: Izz2
    com: [0,0,0]

q: [q1,q2]
qd: [dq1,dq2]
q2d: [ddq1,ddq2]
WEE: [0,0,0,0,0,0]

assignments:
  g: 9.81

The code explained:

parent:
  - 0
  - 1

child:
  - [1]
  - []

support:
  - [1]
  - [1,2]

ee_parent: 2

The parameters are generated to represent a serial robot by default. Modify parameters for tree-like structures. For serial robots these parameters are optional.

• parent: list of parent links for any joint. Use 0 for World.
• child: list of lists with child links for any link. Use empty list if no child is present.
• support: list of lists with all support links beginning with first link including current link for any link.
• ee_parent: parent joint index for end-effector link. Defaults to last link. For robots with more than one end-effector you can use a list of indices instead e.g.: ee_parent: [1,2]. Note, that this requires a list of end_effector transforms ee.

---

gravity: [0,0,-g]

Gravity vector. Note: you can always use symbolic variables instead of numeric values.

---

representation: spatial

Define whether the representation of the joint screw coordinates and the body reference configuration is w.r.t. world frame (representation: spatial) or in body fixed coordinates (representation: body_fixed)

---

joint_screw_coord:
  - type: revolute
    axis: [0,0,1]
    vec: [0,0,0]

  - type: prismatic
    axis: [0,0,1]

The joint screw coordinates can be defined either using the syntax which is used above, where type is the joint type (revolute or prismatic), axis is the joint axis and vec is a vector from the origin to the joint axis. Alternatively, you can directly use the 6D joint screw vectors instead:

joint_screw_coord:
  - [0,0,1,0,0,0]
  - [0,0,0,0,0,1]

---

body_ref_config:
  - rotation:
      axis: [0,0,1]
      angle: 0
    translation: [0,0,0]

  - rotation:
      axis: [0,0,1]
      angle: 0
    translation: [0,0,0]

The body reference configuration is a list of SE(3) transformation matrices. To define them you have several options:

1. Write down the whole matrix e.g.:

   body_ref_config:
     - [[cos(pi),-sin(pi),0, 0],
        [sin(pi), cos(pi),0, 0],
        [      0,       0,1,L1],
        [      0,       0,0, 1]]

2. Write rotation and translation separately:

   body_ref_config:
     - rotation:
         [[1,0,0],
          [0,1,0],
          [0,0,1]]
       translation: [0,0,L1]

3. Use axis angle representation for rotation: -> See code above.

4. For zero rotations or translations it is possible to omit the option:

   body_ref_config:
     - translation: [0,0,L1]

5. Use xyz_rpy coordinates to define Pose:

   body_ref_config:
     - xyzrpy: [0, 0, L1, 0, pi/2, 0]

6. Use roll pitch yaw (rpy) euler angles to define rotation:

   body_ref_config:
     - rotation:
         rpy: [0, pi/2, 0]
       translation: [0,0,L1]

7. Use quaternion [w,x,y,z] to define rotation:

   body_ref_config:
     - rotation:
         Q: [1, 0, 0, 0]
       translation: [0,0,L1]

---

ee:
  - rotation:
      axis: [0,0,1]
      angle: 0
    translation: [0,0,0]

End-effector representation w.r.t. last link body frame in the chain as SE(3) transformation matrix. Here you have the same syntax options as for the body reference configuration. For robots with more than one end-effector you can define more than one transform here. Note: in this case you have to define a list of parent joints in ee_parent. If you have just one end-effector, you can omit the top level list (indicated by the trailing -).

---

mass_inertia:
  - mass: m1
    frame: body
    inertia:
      Ixx: Ixx1
      Ixy: Ixy1
      Ixz: Ixz1
      Iyy: Iyy1
      Iyz: Iyz1
      Izz: Izz1
    com: [0,0,0]

  - mass: m2
    frame: body
    inertia:
      Ixx: Ixx2
      Ixy: Ixy2
      Ixz: Ixz2
      Iyy: Iyy2
      Iyz: Iyz2
      Izz: Izz2
    com: [0,0,0]

Mass-inertia matrices of all links. The keyword frame defines the frame where the inertia is defined in and accepts two values: frame: body and frame: com. When the inertia is w.r.t. the com frame, there is another keyword rotation available, which defines the rotation between the com frame and the body frame. The rotation can be defined like all other rotations above.

For the definition you have the following syntax options:

1. Write down whole matrix:

   mass_inertia:
     - [[   Ixx1,    Ixy1,    Ixz1,       0, -cz1*m1,  cy1*m1],
        [   Ixy1,    Iyy1,    Iyz1,  cz1*m1,       0, -cx1*m1],
        [   Ixz1,    Iyz1,    Izz1, -cy1*m1,  cx1*m1,       0],
        [      0,  cz1*m1, -cy1*m1,      m1,       0,       0],
        [-cz1*m1,       0,  cx1*m1,       0,      m1,       0],
        [ cy1*m1, -cx1*m1,       0,       0,       0,      m1]]

2. Define mass, inertia and center of mass (com) separately:

   mass_inertia:
     - mass: m1
       frame: body
       inertia:
         [[Ixx1,Ixy1,Ixz1],
          [Ixy1,Iyy1,Iyz1],
          [Ixz1,Iyz1,Izz1]]
       com: [cx1,cy1,cz1]
   

3. Only define the 6 independent inertia parameters:

   mass_inertia:
     - mass: m1
       frame: body
       inertia: [Ixx1,Ixy1,Ixz1,Iyy1,Iyz1,Izz1]
       com: [cx1,cy1,cz1]
   

4. Define only necessary inertia parameters:

   mass_inertia:
     - mass: m1
       frame: body
       inertia: 
         Ixx: Ixx1
         Iyy: Iyy1
         Izz: Izz1
       com: [cx1,cy1,cz1]

   Supports the parameters Ixx, Ixy, Ixz, Iyy, Iyz and Izz. All parameters default to 0.

5. Define only one value which is internally multiplied by an identity matrix:

   mass_inertia:
     - mass: m1
       frame: com
       inertia: m1*L1**2
       com: [cx1,cy1,cz1]

6. Automatically generate symbols in inertia matrix:

   mass_inertia:
     - mass: m1
       frame: body
       inertia:
         index: 1
         pointmass: False
       com: [0,0,0]
   

   Here the parameter index is appended to the names Ixx etc. and generates an inertia matrix following the naming scheme used in the examples above. With the parameter pointmass: True the resulting inertia matrix looks like this:

   [[I1, 0, 0],
    [ 0,I1, 0],
    [ 0, 0,I1]]

---

q: [q1,q2]
qd: [dq1,dq2]
q2d: [ddq1,ddq2]
WEE: [0,0,0,0,0,0]

Define symbolic generalized vectors (q, qd, q2d) and the time varying wrench on the end-effector link (WEE). Where the generalized vectors are list of length n and the wrench is a list of length 6 with symbolic values e.g.: [Mx,My,Mz,Fx,Fy,Fz]. If the robot has more than one end-effector, one wrench per end_effector has to be defined in a list instead, e.g.:

WEE: 
  - [Mx1,My1,Mz1,Fx1,Fy1,Fz1]
  - [Mx2,My2,Mz2,Fx2,Fy2,Fz2]

Add jerk and jounce here, to activate the calculation of the 1st and 2nd order derivatives of the Equation of Motion. Optionally, you can define the time derivatives of the external wrench WDEE and W2DEE here accordingly. E.g.:

q: [q1,q2]
qd: [dq1,dq2]
q2d: [ddq1,ddq2]
q3d: [dddq1,dddq2]
q4d: [ddddq1,ddddq2]
WEE: [Mx,My,Mz,Fx,Fy,Fz]
WDEE: [dMx,dMy,dMz,dFx,dFy,dFz]
W2DEE: [ddMx,ddMy,ddMz,ddFx,ddFy,ddFz]

---

assignments:
  g: 9.81

Under the optional assignments keyword you can create a dictionary which maps the symbolic values to numeric values. These numeric values will be used as default values for the parameters in the generated code.

2.1.2. Code generation using YAML

To start the code generation process use:

python3 -m skidy [options] path/to/robot.yaml

In the options you have to specify what kind of code (python -p, Matlab -m, C++ -c, C -C, julia -j, cython --cython, latex -l) you'd like to generate and whether the equations should be simplified -s.

Use

python3 -m skidy -h

to get a description of all available options.

2.2. Python

As for YAML and JSON there is a function to auto-generate a python template file which makes it easier to define your own robot. To generate your robot template use

python3 -m skidy --template [options] new_filename.py

or the python function skidy.generate_template_python(path, structure).

For [options] the option --structure is highly recommended. There you can define which joint types to use in the template. E.g. use --structure 'rrp' for a robot which has two revolute joints followed by one prismatic joint.

The command python3 -m skidy --template --structure 'rp' my_robot_template.py creates the following output file:

from skidy import (SymbolicKinDyn,
                   transformation_matrix,
                   mass_matrix_mixed_data,
                   joint_screw,
                   SO3Exp,
                   inertia_matrix,
                   generalized_vectors)
from skidy.symbols import g, pi
import sympy

# Define symbols:
m1, m2 = sympy.symbols('m1 m2', real=True, const=True)
Ixx1, Ixx2 = sympy.symbols('Ixx1 Ixx2', real=True, const=True)
Ixy1, Ixy2 = sympy.symbols('Ixy1 Ixy2', real=True, const=True)
Ixz1, Ixz2 = sympy.symbols('Ixz1 Ixz2', real=True, const=True)
Iyy1, Iyy2 = sympy.symbols('Iyy1 Iyy2', real=True, const=True)
Iyz1, Iyz2 = sympy.symbols('Iyz1 Iyz2', real=True, const=True)
Izz1, Izz2 = sympy.symbols('Izz1 Izz2', real=True, const=True)

# Define connectivity graph
parent = [0,
          1]

child = [[1],
         []]

support = [[1],
           [1,2]]

ee_parent = 2

# gravity vector
gravity = sympy.Matrix([0,0,g])

# representation of joint screw coordinates and body reference configurations
representation = 'spatial' # alternative: 'body_fixed'

# joint screw coordinates (6x1 sympy.Matrix per joint)
joint_screw_coord = []
joint_screw_coord.append(joint_screw(axis=[0,0,1], vec=[0,0,0], revolute=True))
joint_screw_coord.append(joint_screw(axis=[0,0,1], revolute=False))

# body reference configurations (4x4 SE3 Pose (sympy.Matrix) per link)
body_ref_config = []
body_ref_config.append(transformation_matrix(r=SO3Exp(axis=[0,0,1],angle=0),t=[0,0,0]))
body_ref_config.append(transformation_matrix(r=SO3Exp(axis=[0,0,1],angle=0),t=[0,0,0]))

# end-effector configuration w.r.t. last link body fixed frame in the chain (4x4 SE3 Pose (sympy.Matrix))
ee = transformation_matrix(r=SO3Exp(axis=[0,0,1],angle=0),t=[0,0,0])

# mass_inertia parameters (6x6 sympy.Matrix per link)
Mb = []
Mb.append(mass_matrix_mixed_data(m1, inertia_matrix(Ixx1,Ixy1,Ixz1,Iyy1,Iyz1,Izz1), sympy.Matrix([0,0,0]), frame="body"))
Mb.append(mass_matrix_mixed_data(m2, inertia_matrix(Ixx2,Ixy2,Ixz2,Iyy2,Iyz2,Izz2), sympy.Matrix([0,0,0]), frame="body"))

q, qd, q2d = generalized_vectors(len(body_ref_config), startindex=1)
WEE = sympy.zeros(6,1)

skd = SymbolicKinDyn(gravity_vector=gravity,
                     ee=ee,
                     body_ref_config=body_ref_config,
                     joint_screw_coord=joint_screw_coord,
                     config_representation=representation,
                     Mb=Mb,
                     parent=parent,
                     child=child,
                     support=support,
                     ee_parent=ee_parent,
                     )

# run Calculations
skd.closed_form_kinematics_body_fixed(q, qd, q2d, simplify=True)
skd.closed_form_inv_dyn_body_fixed(q, qd, q2d, WEE=WEE, simplify=True)

# Generate Code
skd.generate_code(python=True, cpp=False, C=False, Matlab=False, latex=False,
                 folder="./generated_code", use_global_vars=True,
                 name="plant", project="Project")

The code explained:

from skidy import (SymbolicKinDyn,
                   transformation_matrix,
                   mass_matrix_mixed_data,
                   joint_screw,
                   SO3Exp,
                   inertia_matrix,
                   generalized_vectors)

The class SymbolicKinDyn is the main object for calculating the kinematic and dynamic equations of your robot and generate the code. Additionally, we import several helper functions for defining the matrices which are useful for the robot definition:

• transformation_matrix: Create SE(3) transformation matrix from SO(3) rotation and translation vector.
• mass_matrix_mixed_data: Create 6x6 mass-inertia matrix from mass, 3x3 inertia matrix and 3x1 center of mass vector.
• joint_screw: create 6x1 joint screw vector from joint axis and vector from origin to joint axis.
• SO3Exp: Exponential mapping of SO(3) to generate rotation matrix from rotation angle and rotation axis.
• inertia_matrix: generate 3x3 inertia matrix from 6 independent parameters (Ixx, Ixy, ...).
• generalized_vectors: generate symbolic generalized vectors q, qd and q2d of predefined length n.

from skidy.symbols import g, pi

The package skidy.symbols includes the most common used symbolic variables, which can be used for defining your robot.

import sympy

The whole library used sympy objects for all symbolic equations etc. Hence, we need sympy to create additional symbolic variables and matrices later.

---

# Define symbols:
m1, m2 = sympy.symbols('m1 m2', real=True, const=True)
Ixx1, Ixx2 = sympy.symbols('Ixx1 Ixx2', real=True, const=True)
Ixy1, Ixy2 = sympy.symbols('Ixy1 Ixy2', real=True, const=True)
Ixz1, Ixz2 = sympy.symbols('Ixz1 Ixz2', real=True, const=True)
Iyy1, Iyy2 = sympy.symbols('Iyy1 Iyy2', real=True, const=True)
Iyz1, Iyz2 = sympy.symbols('Iyz1 Iyz2', real=True, const=True)
Izz1, Izz2 = sympy.symbols('Izz1 Izz2', real=True, const=True)

Create symbolic variables which can be used in the equations for the robot definition later. The most common symbols are also already present in the skidy.symbols package and may be imported from there instead.

---

# Define connectivity graph
parent = [0,
          1]

child = [[1],
         []]

support = [[1],
           [1,2]]

ee_parent = 2

Connectivity graph of the robot. The parameters are generated to represent a serial robot by default. Modify parameters for tree-like structures. For serial robots these parameters are optional.

• parent: list of parent links for any joint. Use 0 for World.
• child: list of lists with child links for any link. Use empty list if no child is present.
• support: list of lists with all support links beginning with first link including current link for any link.
• ee_parent: parent joint index for end-effector link. Defaults to last link. For robots with more than one end-effector you can use a list of indices instead e.g.: ee_parent = [1,2]. Note, that this requires a list of end_effector transforms in ee.

---

# gravity vector
gravity = sympy.Matrix([0,0,g])

Gravity vector as sympy.Matrix. Note that we can use symbolic variables here.

---

# representation of joint screw coordinates and body reference configurations
representation = 'spatial' # alternative: 'body_fixed'

Define whether the representation of the joint screw coordinates and the body reference configuration is w.r.t. world frame (representation = 'spatial') or in body fixed coordinates (representation = 'body_fixed').

---

# joint screw coordinates (6x1 sympy.Matrix per joint)
joint_screw_coord = []
joint_screw_coord.append(joint_screw(axis=[0,0,1], vec=[0,0,0], revolute=True))
joint_screw_coord.append(joint_screw(axis=[0,0,1], revolute=False))

The joint screw coordinates can be defined either using the syntax which is used above, where axis is the joint axis, vec is a vector from the origin to the joint axis and revolute has to be True for revolute joints and False for prismatic joints. Note that prismatic joints don't need the parameter vec. Alternatively, you can directly use the 6D joint screw vectors instead:

joint_screw_coord = []
joint_screw_coord.append(sympy.Matrix([0,0,1,0,0,0]))
joint_screw_coord.append(sympy.Matrix([0,0,0,0,0,1]))

---

# body reference configurations (4x4 SE3 Pose (sympy.Matrix) per link)
body_ref_config = []
body_ref_config.append(transformation_matrix(r=SO3Exp(axis=[0,0,1],angle=0),t=[0,0,0]))
body_ref_config.append(transformation_matrix(r=SO3Exp(axis=[0,0,1],angle=0),t=[0,0,0]))

The body reference configuration is a list of SE(3) transformation matrices. To define them you have several options:

1. Write down the whole matrix e.g.:

   body_ref_config.append(
       sympy.Matrix([[sympy.cos(pi/2),-sympy.sin(pi/2),0, 0],
                     [sympy.sin(pi/2), sympy.cos(pi/2),0, 0],
                     [              0,               0,1,L1],
                     [              0,               0,0, 1]])
   )

   Note: this example assumes you have defined the symbolic variable L1 before.

2. Write rotation and translation separately:

   body_ref_config.append(
       transformation_matrix(
           r=sympy.Matrix([[1,0,0],
                           [0,1,0],
                           [0,0,1]]),
           t=sympy.Matrix([0,0,L1])
       )
   )

3. Use axis angle representation for rotation: -> See code above.

4. For zero rotations or translations it is possible to omit the option:

   body_ref_config.append(transformation_matrix(t=[0,0,0]))

5. Use xyz_rpy coordinates to define Pose:

   body_ref_config.append(xyz_rpy_to_matrix([0, 0, L1, 0, pi/2, 0]))

   Note that you have to import the function using from skidy import xyz_rpy_to_matrix.

6. Use roll pitch yaw (rpy) euler angles to define rotation:

   body_ref_config.append(
       transformation_matrix(
           r=rpy_to_matrix([0, pi/2, 0]),
           t=sympy.Matrix([0,0,L1])
       )
   )

   Note that you have to import the function using from skidy import rpy_to_matrix.

7. Use quaternion [w,x,y,z] to define rotation:

   body_ref_config.append(
       transformation_matrix(
           r=quaternion_to_matrix([1,0,0,0]),
           t=sympy.Matrix([0,0,L1])
       )
   )

   Note that you have to import the function using from skidy import quaternion_to_matrix.

---

# end-effector configuration w.r.t. last link body fixed frame in the chain (4x4 SE3 Pose (sympy.Matrix))
ee = transformation_matrix(r=SO3Exp(axis=[0,0,1],angle=0),t=[0,0,0])

End-effector representation w.r.t. last link body frame in the chain as SE(3) transformation matrix. Here you have the same syntax options as for the body reference configuration. For robots with more than one end-effector you can use a list of transforms instead. This requires a list of parent joint indices for the parameter ee_parent.

---

# mass_inertia parameters (6x6 sympy.Matrix per link)
Mb = []
Mb.append(mass_matrix_mixed_data(m1, inertia_matrix(Ixx1,Ixy1,Ixz1,Iyy1,Iyz1,Izz1), sympy.Matrix([0,0,0]), frame='body'))
Mb.append(mass_matrix_mixed_data(m2, inertia_matrix(Ixx2,Ixy2,Ixz2,Iyy2,Iyz2,Izz2), sympy.Matrix([0,0,0]), frame='body'))

Mass-inertia matrices of all links. The mass inertia matrix has to be w.r.t. the body-fixed frame. But with the argument frame = 'com' you can define the inertia matrix w.r.t. the center of mass frame instead. In this case, there is another argument R in the function mass_matrix_mixed_data which defines the rotation matrix of the COM frame to the body-fixed frame.

For the definition you have the following syntax options:

1. Write down whole matrix:

   Mb.append(
       sympy.Matrix([[   Ixx1,    Ixy1,    Ixz1,       0, -cz1*m1,  cy1*m1],
                     [   Ixy1,    Iyy1,    Iyz1,  cz1*m1,       0, -cx1*m1],
                     [   Ixz1,    Iyz1,    Izz1, -cy1*m1,  cx1*m1,       0],
                     [      0,  cz1*m1, -cy1*m1,      m1,       0,       0],
                     [-cz1*m1,       0,  cx1*m1,       0,      m1,       0],
                     [ cy1*m1, -cx1*m1,       0,       0,       0,      m1]])
   )

2. Define mass, inertia matrix and center of mass separately:

   Mb.append(
       mass_matrix_mixed_data(
           m1,
           sympy.Matrix([[Ixx1,Ixy1,Ixz1],
                         [Ixy1,Iyy1,Iyz1],
                         [Ixz1,Iyz1,Izz1]]),
           sympy.Matrix([cx1,cy1,cz1])
       )
   )

3. Only define the 6 independent inertia parameters:

   Mb.append(
       mass_matrix_mixed_data(
           m1,
           inertia_matrix(Ixx1,Ixy1,Ixz1,Iyy1,Iyz1,Izz1),
           sympy.Matrix([cx1,cy1,cz1])
       )
   )

4. Automatically generate symbols in inertia matrix:

   Mb.append(
       mass_matrix_mixed_data(
           m1,
           symbolic_inertia_matrix(index=1, pointmass=False),
           sympy.Matrix([cx1,cy1,cz1])
       )
   )

   where symbolic_inertia_matrix(index=1, pointmass=False) auto generates the variables Ixx1, Ixy1, etc. and creates a sympy.Matrix from it. With the parameter pointmass=True the resulting inertia matrix looks like this instead:

   sympy.Matrix([[I1, 0, 0],
                 [ 0,I1, 0],
                 [ 0, 0,I1]])

   Note that you have to import the function using from skidy import symbolic_inertia_matrix.

---

q, qd, q2d = generalized_vectors(len(body_ref_config), startindex=1)

Generate the generalized vectors (joint positions q, joint velocities qd and joint accelerations q2d). The symbols are auto generated starting at index startindex. The degrees of freedom in this case are taken from the length of body_ref_config.

The equivalent code would be:

q1, q2 = sympy.symbols("q1 d2", real=True, constant=False)
dq1, dq2 = sympy.symbols("dq1 dd2", real=True, constant=False)
ddq1, ddq2 = sympy.symbols("ddq1 ddq2", real=True, constant=False)
q = sympy.Matrix([q1,q2])
qd = sympy.Matrix([dq1,dq2])
q2d = sympy.Matrix([ddq1,ddq2])

To generate jerk and jounce as well, use:

q, qd, q2d, q3d, q4d = generalized_vectors(len(body_ref_config), startindex=1, derivatives=True)

---

WEE = sympy.zeros(6,1)

Define the time varying wrench on the end-effector link. Should be a (6,1) sympy.Matrix with symbolic variables, e.g.: sympy.Matrix([Mx,My,Mz,Fx,Fy,Fz]).

Optionally, you can define the time derivatives WDEE and W2DEE here too. E.g.:

WEE = sympy.Matrix([Mx,My,Mz,Fx,Fy,Fz])
WDEE = sympy.Matrix([dMx,dMy,dMz,dFx,dFy,dFz])
W2DEE = sympy.Matrix([ddMx,ddMy,ddMz,ddFx,ddFy,ddFz])

If your robot has more than one end-effector, you have to use one external wrench per end-effector in a list, e.g.:

WEE = [sympy.Matrix([Mx1,My1,Mz1,Fx1,Fy1,Fz1]),
       sympy.Matrix([Mx2,My2,Mz2,Fx2,Fy2,Fz2])]

---

skd = SymbolicKinDyn(gravity_vector=gravity,
                     ee=ee,
                     body_ref_config=body_ref_config,
                     joint_screw_coord=joint_screw_coord,
                     config_representation=representation,
                     Mb=Mb,
                     parent=parent,
                     child=child,
                     support=support,
                     )

Initialize class with all defined parameters.

---

# run Calculations
skd.closed_form_kinematics_body_fixed(q, qd, q2d, simplify=True)
skd.closed_form_inv_dyn_body_fixed(q, qd, q2d, WEE=WEE, simplify=True)

Generate forward kinematics and inverse dynamics equations. Both functions share the following arguments:

• simplify: generated expressions are simplified. Note that the simplification takes a lot of time for robots with more than 2 revolute joints in a chain.
• cse: Use common subexpression elimination to shorten equations. Note that the equations are not human-readable afterwards.
• parallel: use parallel computation.

skd.closed_form_kinematics_body_fixed generates the following equations and saves them as class parameters:

• body_acceleration
• body_acceleration_ee
• body_jacobian_matrix
• body_jacobian_matrix_dot
• body_jacobian_matrix_ee
• body_jacobian_matrix_ee_dot
• body_twist_ee
• forward_kinematics
• hybrid_acceleration
• hybrid_acceleration_ee
• hybrid_jacobian_matrix
• hybrid_jacobian_matrix_dot
• hybrid_jacobian_matrix_ee
• hybrid_jacobian_matrix_ee_dot
• hybrid_twist_ee

and skd.closed_form_inv_dyn_body_fixed generates the following equations and saves them as class parameters:

• coriolis_centrifugal_matrix
• generalized_mass_inertia_matrix
• gravity_vector
• inverse_dynamics

with provided generalized jerk and jounce vectors, skd.closed_form_inv_dyn_body_fixed additionally generates the following derivatives of the equation of motion:

• coriolis_centrifugal_matrix_dot
• generalized_mass_inertia_matrix_dot
• gravity_vector_dot
• inverse_dynamics_dot
• coriolis_centrifugal_matrix_ddot
• generalized_mass_inertia_matrix_ddot
• gravity_vector_ddot
• inverse_dynamics_ddot

skd.closed_form_inv_dyn_body_fixed takes the wrench WEE (6x1 sympy.Matrix) (tau,F) on the end-effector link and its time derivatives WDEE and W2DEE as optional additional arguments.

To calculate the 1st and 2nd order time derivative of the EOM use e.g.:

skd.closed_form_inv_dyn_body_fixed(q, qd, q2d, q3d, q4d, WEE, WDEE, W2DEE, simplify=True)

---

# Generate Code
skd.generate_code(python=True, cpp=False, C=False, Matlab=False, latex=False,
                 folder="./generated_code", use_global_vars=True,
                 name="plant", project="Project")

Generate Python, Matlab, C++, C (C99), Julia, Cython and/or LaTeX code from the generated equations. Note that this can take some time, especially for non-simplified equations and complex robots.

2.3. URDF

There are two ways to use URDF files. First, you can convert your URDF file to YAML or JSON using:

skidy --convert my_robot.urdf my_robot.yaml

Here, you can add end-effector transforms, and modify whatever you want.

Second, you can load your URDF using python. There is a function to generate a template python file, which loads your URDF. In the python file it is necessary to define:

1. the URDF path
2. the gravity vector
3. end-effector configuration w.r.t. last link body fixed frame in the chain

To generate the python template file use:

python -m skidy --template --urdf my_urdf_template.py

or the python function skidy.generate_template_python(path, urdf=True).

This generates the following output:

from skidy import (SymbolicKinDyn,
                   transformation_matrix,
                   SO3Exp,
                   generalized_vectors)
from skidy.symbols import g, pi
import sympy

# Define symbols:
lee = sympy.symbols('lee', real=True, const=True)

urdfpath = '/path/to/robot.urdf' # TODO: change me!

# gravity vector
gravity = sympy.Matrix([0,0,g])

# end-effector configuration w.r.t. last link body fixed frame in the chain
ee = transformation_matrix(r=SO3Exp(axis=[0,0,1],angle=0),t=[0,0,0])

skd = SymbolicKinDyn(gravity_vector=gravity,
                     ee=ee,
                     )

skd.load_from_urdf(path = urdfpath,
                   symbolic=False, 
                   cse=False, 
                   simplify_numbers=True,  
                   tolerance=0.0001, 
                   max_denominator=8, 
                   )

q, qd, q2d = generalized_vectors(len(skd.body_ref_config), startindex=1)
WEE = sympy.zeros(6,1)

# run Calculations
skd.closed_form_kinematics_body_fixed(q, qd, q2d, simplify=True)
skd.closed_form_inv_dyn_body_fixed(q, qd, q2d, WEE=WEE, simplify=True)

# Generate Code
skd.generate_code(python=True, cpp=False, C=False, Matlab=False, latex=False,
                 folder="./generated_code", use_global_vars=True,
                 name="plant", project="Project")

The code explained:

from skidy import (SymbolicKinDyn,
                   transformation_matrix,
                   SO3Exp,
                   generalized_vectors)

The class SymbolicKinDyn is the main object for calculating the kinematic and dynamic equations of your robot and generate the code. Additionally, we import several helper functions for defining the matrices which are useful for the robot definition:

• transformation_matrix: Create SE(3) transformation matrix from SO(3) rotation and translation vector.
• SO3Exp: Exponential mapping of SO(3) to generate rotation matrix from rotation angle and rotation axis.
• generalized_vectors: generate symbolic generalized vectors q, qd and q2d of predefined length n.

from skidy.symbols import g, pi

The package skidy.symbols includes the most common used symbolic variables, which can be used for defining your robot.

import sympy

The whole library used sympy objects for all symbolic equations etc. Hence, we need sympy to create additional symbolic variables and matrices later.

---

# Define symbols:
lee = sympy.symbols('lee', real=True, const=True)

Create symbolic variables which can be used in the equations for the robot definition later. The most common symbols are also already present in the skidy.symbols package and may be imported from there instead.

---

urdfpath = '/path/to/robot.urdf' # TODO: change me!

Enter the path to your URDF file here.

---

# gravity vector
gravity = sympy.Matrix([0,0,g])

Gravity vector as sympy.Matrix. Note that we can use symbolic variables here.

---

# end-effector configuration w.r.t. last link body fixed frame in the chain
ee = transformation_matrix(r=SO3Exp(axis=[0,0,1],angle=0),t=[0,0,0])

End-effector representation w.r.t. last link body frame in the chain as SE(3) transformation matrix. Look up the chapter Python for all available syntax options.

---

skd = SymbolicKinDyn(gravity_vector=gravity,
                     ee=ee,
                     )

Initialize class with the two defined parameters.

---

skd.load_from_urdf(path = urdfpath,
                   symbolic=False, 
                   cse=False, 
                   simplify_numbers=True,  
                   tolerance=0.0001, 
                   max_denominator=8, 
                   )

Load the URDF file. Here you can specify the following options:

1. symbolic: symbolify values in urdf file (bool).
2. cse: use common subexpression elimination to shorten equations (bool).
3. simplify_numbers: round numbers if close to common fractions like 1/2 etc and replace eg 3.1416 by pi (bool).
4. tolerance: tolerance for simplify numbers.
5. max_denominator: define max denominator for simplify numbers to avoid simplification to something like 13/153. Use 0 to deactivate.

---

q, qd, q2d = generalized_vectors(len(skd.body_ref_config), startindex=1)

Generate the generalized vectors (joint positions q, joint velocities qd and joint accelerations q2d). The symbols are auto generated starting at index startindex. The degrees of freedom in this case are taken from the length of the parameter skd.body_ref_config, which was generated by the function load_from_urdf.

---

WEE = sympy.zeros(6,1)

Define the time varying wrench on the end-effector link. Should be a (6,1) sympy.Matrix with symbolic variables.

---

# run Calculations
skd.closed_form_kinematics_body_fixed(q, qd, q2d, simplify=True)
skd.closed_form_inv_dyn_body_fixed(q, qd, q2d, WEE=WEE, simplify=True)

Generate forward kinematics and inverse dynamics equations. See chapter Python for more information.

---

# Generate Code
skd.generate_code(python=True, cpp=False, C=False, Matlab=False, latex=False,
                 folder="./generated_code", use_global_vars=True,
                 name="plant", project="Project")

Generate Python, Matlab, C++, C (C99), Julia, Cython and/or LaTeX code from the generated equations. Note that this can take time, especially for non-simplified equations and complex robots.

3. Unit testing

To run the unit tests use:

python3 ./test/unit_testing.py

4. Benchmarking

For benchmarking the project the script benchmarking/benchmarking.py was used. This script loads 4 robots with increasing complexity (1 to 4 revolute joint in a chain with planar task space) and takes the execution time of the functions closed_form_kinematics_body_fixed(), closed_form_inv_dyn_body_fixed() and generate_code(). Additionally, the arguments parallel, simplify and cse have been altered. The results are shown in the following table:

arguments	parallel	serial
simplify
simplify + cse
no simplify
no simplify + cse

5. License

This work has been released under the BSD 3-Clause License. Details and terms of use are specified in the LICENSE file within this repository. Note that we do not publish third-party software, hence software packages from other developers are released under their very own terms and conditions. If you install third-party software packages along with this repo ensure that you follow each individual license agreement.