• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 小磊在路上 1344次浏览 0个评论

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

 


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明Sympybotics机器人动力学符号推导工具箱
喜欢 (0)

您必须 登录 才能发表评论!

加载中……