Symbolic Framework for Modeling and Identification of Robot Dynamics
Uses Sympy and Numpy libraries.
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]])From git source:
git clone https://github.com/cdsousa/SymPyBotics.git
cd sympybotics
python setup.py install
New BSD license. See License File
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent