SymPyBotics

Symbolic Framework for Modeling and Identification of Robot Dynamics

Uses Sympy and Numpy libraries.

Citation:

Install

From git source:

git clone https://github.com/xu-yang16/SymPyBotics.git
cd sympybotics
python setup.py install

Test Code

import sympy
import sympybotics
rbtdef = sympybotics.RobotDef('Example Robot', [('-pi/2', 0, 0, 'q+pi/2'), ( 'pi/2', 0, 0, 'q-pi/2')], dh_convention='standard')
rbtdef.frictionmodel = {'Coulomb', 'viscous'} # options are None or a combination of 'Coulomb', 'viscous' and 'offset'
rbtdef.gravityacc = sympy.Matrix([0.0, 0.0, -9.81]) # optional, this is the default value
rbtdef.dynparms()
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)

rbt.geo.T[-1]
rbt.kin.J[-1]
tau_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.invdyn_code, 'tau_out', 'tau', rbtdef)
rbt.calc_base_parms()
rbt.dyn.baseparms

Modifications

1. robotmodel.py, dynamics\dyn_parm_dep.py, dynamics\dynamics.py: add import math
2. symcode\subexprs.py: change line 142 to sympy.utilities.iterables.iterable
3. __compatibility__.py: change line 7 and line 11 if pyversion is 3 to if pyversion == 3

Example

Definition of a 2 DOF example robot:

>>> import sympy
>>> import sympybotics
>>> rbtdef = sympybotics.RobotDef('Example Robot', # robot name
...                               [('-pi/2', 0, 0, 'q+pi/2'),  # list of tuples with Denavit-Hartenberg parameters
...                                ( 'pi/2', 0, 0, 'q-pi/2')], # (alpha, a, d, theta)
...                               dh_convention='standard' # either 'standard' or 'modified'
...                              )
>>> rbtdef.frictionmodel = {'Coulomb', 'viscous'} # options are None or a combination of 'Coulomb', 'viscous' and 'offset'
>>> rbtdef.gravityacc = sympy.Matrix([0.0, 0.0, -9.81]) # optional, this is the default value

>>> rbtdef.dynparms()
[L_1xx, L_1xy, L_1xz, L_1yy, L_1yz, L_1zz, l_1x, l_1y, l_1z, m_1, fv_1, fc_1, L_2xx, L_2xy, L_2xz, L_2yy, L_2yz, L_2zz, l_2x, l_2y, l_2z, m_2, fv_2, fc_2]

L is the link inertia tensor computed about the link frame; l is the link first moment of inertia; m is the link mass. These are the so-called barycentric parameters, with respect to which the dynamic model is linear.

Generation of geometric, kinematic and dynamic models:

>>> rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
generating geometric model
generating kinematic model
generating inverse dynamics code
generating gravity term code
generating coriolis term code
generating coriolis matrix code
generating inertia matrix code
generating regressor matrix code
generating friction term code
done

>>> rbt.geo.T[-1]
Matrix([
[-sin(q1)*sin(q2), -cos(q1),  sin(q1)*cos(q2), 0],
[ sin(q2)*cos(q1), -sin(q1), -cos(q1)*cos(q2), 0],
[         cos(q2),        0,          sin(q2), 0],
[               0,        0,                0, 1]])

>>> rbt.kin.J[-1]
Matrix([
[0,        0],
[0,        0],
[0,        0],
[0, -cos(q1)],
[0, -sin(q1)],
[1,        0]])

C function generation:

>>> tau_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.invdyn_code, 'tau_out', 'tau', rbtdef)

Doing print(tau_str), function code will be output:

void tau( double* tau_out, const double* parms, const double* q, const double* dq, const double* ddq )
{
  double x0 = sin(q[1]);
  double x1 = -dq[0];
  double x2 = -x1;
  double x3 = x0*x2;
  double x4 = cos(q[1]);
  double x5 = x2*x4;
  double x6 = parms[13]*x5 + parms[15]*dq[1] + parms[16]*x3;
  double x7 = parms[14]*x5 + parms[16]*dq[1] + parms[17]*x3;
  double x8 = -ddq[0];
  double x9 = -x4;
  double x10 = dq[1]*x1;
  double x11 = x0*x10 + x8*x9;
  double x12 = -x0*x8 - x10*x4;
  double x13 = 9.81*x0;
  double x14 = 9.81*x4;
  double x15 = parms[12]*x5 + parms[13]*dq[1] + parms[14]*x3;

  tau_out[0] = -parms[3]*x8 + x0*(parms[14]*x11 + parms[16]*ddq[1] + parms[17]*x12 - dq[1]*x15 - parms[19]*x14 + x5*x6) - x9*(parms[12]*x11 + parms[13]*ddq[1] + parms[14]*x12 + dq[1]*x7 + parms[19]*x13 - x3*x6);
  tau_out[1] = parms[13]*x11 + parms[15]*ddq[1] + parms[16]*x12 - parms[18]*x13 + parms[20]*x14 + x15*x3 - x5*x7;

  return;
}

Dynamic base parameters:

>>> rbt.calc_base_parms()
>>> rbt.dyn.baseparms
Matrix([
[L_1yy + L_2zz],
[         fv_1],
[         fc_1],
[L_2xx - L_2zz],
[        L_2xy],
[        L_2xz],
[        L_2yy],
[        L_2yz],
[         l_2x],
[         l_2z],
[         fv_2],
[         fc_2]])

Author

Cristóvão Duarte Sousa

License

New BSD license. See License File