Sympybotics

  Sympybotic是一款使用python语言利用Sympy和Numpy包的开源机器人运动学和动力学的符号推导工具包，在机器人动力学参数辨识中可以用来建立机器人动力学模型，根据所建模型推导动力学最小惯性参数集和观测矩阵，并可以转换为C代码，直接用于实际辨识实验 安装：  

git clone https://github.com/cdsousa/SymPyBotics.git
cd sympybotics
python setup.py install

安装成功后，README文件中已经有一个二连杆的例子以供学习使用

  Example 定义2连杆机构模型  

>>> 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 为惯性张量 l 为质心位置 m 质量   生成运动学、动力学模型  

>>> 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代码  

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

  打印输出  

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;
}

  最小惯性矩阵  

>>> 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]])

  随意定义一个六自由度机器人模型，建立MDH模型，并将观测矩阵和最小惯性参数集输出至txt文件  

import sympy
import numpy
import sympybotics
# 建立机器人模型
rbtdef = sympybotics.RobotDef('Example Robot', # robot name
                              [('0', 0, 0.29, 'q'),  # list of tuples with Denavit-Hartenberg parameters
                               ( 'pi/2', 0, 0, 'q'),
                               ('0',0.32,0,'q'),
                               ('-pi/2',0,0.42,'q'),
                               ('pi/2',0,0,'q'),
                               ('-pi/2',0,0.18,'q')], # (alpha, a, d, theta)
                              dh_convention='modified' # either 'standard' or 'modified'
                              )
# 设定重力加速度的值（沿z轴负方向）
rbtdef.gravityacc=sympy.Matrix([0.0, 0.0, -9.81])
# 设定摩擦力 库伦摩擦与粘滞摩擦
rbtdef.frictionmodel = {'Coulomb', 'viscous'}
# 显示动力学全参数
print(rbtdef.dynparms())
#构建机器人动力学模型
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
# 转换为C代码
tau_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.invdyn_code, 'tau_out', 'tau', rbtdef)
print(tau_str) #打印
#计算并显示动力学模型的回归观测矩阵，转换为C代码
rbt.calc_base_parms()
rbt.dyn.baseparms
print(rbt.dyn.baseparms)# 打印最小参数集P
rbt.Hb_code
print(rbt.Hb_code)#打印观测矩阵
Yr = sympybotics.robotcodegen.robot_code_to_func('C', rbt.Hb_code, 'H', 'Hb_code', rbtdef)
print(Yr) #打印显示转换为C代码后的观测矩阵Yr
#把动力学全参数模型，关节力矩模型，观测矩阵和最小惯性参数集结果保存为txt
data=open("D:\data.txt",'w+')
print(rbt.dyn.dynparms,tau_str,Yr,rbt.dyn.baseparms,file=data)
data.close()