python 机器人工具——Sympybotics

Sympybotics机器人动力学符号推导工具箱

https://github.com/cdsousa/SymPyBoticshttps://github.com/cdsousa/SymPyBoticsSympybotic是一款机器人运动学和动力学的符号推导工具包.

其主要依赖于两个工具包：

SymPy是用于符号数学的Python库。 它旨在组建功能齐全的计算机代数系统（CAS），同时保持代码尽可能的简单，以使其易于理解和易于扩展。 SymPy完全用Python编写。

安装方式具体如下所示

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

测试例子1：

#!/usr/bin/python3

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

测试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
 
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
donerbt = 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]])

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