Symbolic Framework for Modeling and Identification of Robot Dynamics
Uses Sympy and Numpy libraries.
Definition of a 2 DOF example robot:
>>>importsympy>>>importsympybotics>>>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)
generatinggeometricmodelgeneratingkinematicmodelgeneratinginversedynamicscodegeneratinggravitytermcodegeneratingcoriolistermcodegeneratingcoriolismatrixcodegeneratinginertiamatrixcodegeneratingregressormatrixcodegeneratingfrictiontermcodedone>>>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:
voidtau( double*tau_out, constdouble*parms, constdouble*q, constdouble*dq, constdouble*ddq )
{
doublex0=sin(q[1]);
doublex1=-dq[0];
doublex2=-x1;
doublex3=x0*x2;
doublex4=cos(q[1]);
doublex5=x2*x4;
doublex6=parms[13]*x5+parms[15]*dq[1] +parms[16]*x3;
doublex7=parms[14]*x5+parms[16]*dq[1] +parms[17]*x3;
doublex8=-ddq[0];
doublex9=-x4;
doublex10=dq[1]*x1;
doublex11=x0*x10+x8*x9;
doublex12=-x0*x8-x10*x4;
doublex13=9.81*x0;
doublex14=9.81*x4;
doublex15=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.baseparmsMatrix([
[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]])From git source:
git clone https://github.com/cdsousa/SymPyBotics.git
cd sympybotics
python setup.py install
New BSD license. See License File