def test_puma_dh_num_geo_kin_dyn():
pi = sympy.pi
q = sympybotics.robotdef.q
puma560_def = sympybotics.RobotDef(
'Puma 560 Robot',
[
(pi / 2, 0, 0, q),
(0, 0.4318, 0, q),
('-pi/2', '0.0203', '0.15005', 'q'), # test sympify
(pi / 2, 0, 0.4318, q),
(-pi / 2, 0, 0, q),
(0, 0, 0, q)
],
dh_convention='standard')
puma560_def.frictionmodel = None
puma560 = sympybotics.RobotDynCode(puma560_def)
q_test = [
0.7504516826728697, 0.8395156106908136, 0.16851233582594916,
0.3849629637427072, 0.5252993946810777, 0.6701207256444748
]
dq_test = [
0.24721855939629367, 0.9805915670454258, 0.9895299755642817,
0.7861135739668947, 0.273842245476577, 0.17182358900767503
]
ddq_test = [
0.707405815485141, 0.25295715193420953, 0.9763909835998361,
0.8412822676113918, 0.4867768296473465, 0.11480270540937143
]
dynparm_test = [
0.0, -0, -0, 0.34999999999999998, -0, 0.0, 0.0, 0.0, 0.0, 0,
1.03118515, 0.037980719999999996, 1.4401022999999999,
3.7274564059999999, -0.023751000000000001, 2.8425240559999998,
-6.33012, 0.10439999999999999, 3.9585, 17.4, 0.090474288000000014,
-0.0013739039999999998, 0.0068208000000000001, 0.111498032,
0.0047375999999999998, 0.015432319999999999, -0.09743999999999998,
-0.06767999999999999, 0.336, 4.8, 0.0020960200000000001, -0, -0,
0.0012999999999999999, -0, 0.0020960200000000001, 0.0,
0.015579999999999998, 0.0, 0.82, 0.00029999999999999997, -0, -0,
0.00040000000000000002, -0, 0.00029999999999999997, 0.0, 0.0, 0.0,
0.34, 0.00024216, -0, -0, 0.00024216, -0, 4.0000000000000003e-05, 0.0,
0.0, 0.0028799999999999997, 0.09
]
dynparm_test2 = [
0.47804562306292275, 0.5871876506259908, 0.9487349009746813,
0.35387185413632094, 0.28071604959871554, 0.368556182617345,
0.24355010647230801, 0.6753463418802456, 0.9728151452953864,
0.6620741264406734, 0.34669638996014096, 0.01593886435340608,
0.3521748260592804, 0.5384045845183812, 0.021600503502885116,
0.4654003203805651, 0.5202014161122065, 0.33744920539722967,
0.052363297799702835, 0.07051826001770234, 0.7389222546505236,
0.771434543548951, 0.3652539269897015, 0.2603059367721896,
0.4310648491411889, 0.7071252186366607, 0.16320122542325732,
0.44948421506462655, 0.48085540250421277, 0.08408482356412372,
0.923593157615906, 0.46852453511703684, 0.6670004526434297,
0.573634975268657, 0.12665814747855264, 0.3152822779549781,
0.21725524421221942, 0.5727451645276381, 0.7984025459787872,
0.7630923700903489, 0.27670044727992893, 0.919667661316697,
0.8828363383223908, 0.3618378476984413, 0.20690997158181246,
0.8321536435628591, 0.556153477173109, 0.3985225289975399,
0.7128303168401632, 0.433898343199971, 0.35116090526732047,
0.551636779637059, 0.03397585970570116, 0.28333059714010744,
0.4785548774382852, 0.16258401709779136, 0.8246056496848533,
0.23027686557606952, 0.26655111443159285, 0.6087446254045791
]
q_num_subs = dict(zip(puma560_def.q, q_test))
T = puma560.geo.T[-1]
T = T.subs(q_num_subs)
T = numpy.matrix(T).astype(numpy.float64)
J = puma560.kin.J[-1]
J = J.subs(q_num_subs)
J = numpy.matrix(J).astype(numpy.float64)
M_func_def = sympybotics.robotcodegen.robot_code_to_func(
'python', puma560.M_code, 'M', 'M_puma560', puma560_def)
c_func_def = sympybotics.robotcodegen.robot_code_to_func(
'python', puma560.c_code, 'c', 'c_puma560', puma560_def)
C_func_def = sympybotics.robotcodegen.robot_code_to_func(
'python', puma560.C_code, 'C', 'C_puma560', puma560_def)
g_func_def = sympybotics.robotcodegen.robot_code_to_func(
'python', puma560.g_code, 'g', 'g_puma560', puma560_def)
tau_func_def = sympybotics.robotcodegen.robot_code_to_func(
'python', puma560.invdyn_code, 'tau', 'tau_puma560', puma560_def)
H_func_def = sympybotics.robotcodegen.robot_code_to_func(
'python', puma560.H_code, 'H', 'H_puma560', puma560_def)
l = locals()
exec_(M_func_def, globals(), l)
exec_(c_func_def, globals(), l)
exec_(C_func_def, globals(), l)
exec_(g_func_def, globals(), l)
exec_(tau_func_def, globals(), l)
exec_(H_func_def, globals(), l)
tau_puma560 = l['tau_puma560']
g_puma560 = l['g_puma560']
c_puma560 = l['c_puma560']
C_puma560 = l['C_puma560']
M_puma560 = l['M_puma560']
H_puma560 = l['H_puma560']
tau = tau_puma560(dynparm_test, q_test, dq_test, ddq_test)
tau = numpy.matrix(tau).T.astype(numpy.float64)
g = g_puma560(dynparm_test, q_test)
g = numpy.matrix(g).T.astype(numpy.float64)
c = c_puma560(dynparm_test, q_test, dq_test)
c = numpy.matrix(c).T.astype(numpy.float64)
C = C_puma560(dynparm_test, q_test, dq_test)
C = numpy.matrix(C).reshape(puma560.dof, puma560.dof).astype(numpy.float64)
C_dq = C * numpy.matrix(dq_test).T
M = M_puma560(dynparm_test, q_test)
M = numpy.matrix(M).reshape(puma560.dof, puma560.dof).astype(numpy.float64)
H = H_puma560(q_test, dq_test, ddq_test)
H = numpy.matrix(H).reshape(puma560.dof,
puma560.dyn.n_dynparms).astype(numpy.float64)
tau_t2 = tau_puma560(dynparm_test2, q_test, dq_test, ddq_test)
tau_t2 = numpy.matrix(tau_t2).T.astype(numpy.float64)
g_t2 = g_puma560(dynparm_test2, q_test)
g_t2 = numpy.matrix(g_t2).T.astype(numpy.float64)
c_t2 = c_puma560(dynparm_test2, q_test, dq_test)
c_t2 = numpy.matrix(c_t2).T.astype(numpy.float64)
C_t2 = C_puma560(dynparm_test2, q_test, dq_test)
C_t2 = numpy.matrix(C_t2).reshape(puma560.dof,
puma560.dof).astype(numpy.float64)
C_dq_t2 = C_t2 * numpy.matrix(dq_test).T
M_t2 = M_puma560(dynparm_test2, q_test)
M_t2 = numpy.matrix(M_t2).reshape(puma560.dof,
puma560.dof).astype(numpy.float64)
T_pcorke = numpy.matrix([[
-0.655113870655343, -0.474277925274361, -0.588120962109346,
0.0540498757911011
],
[
0.524340309498464, 0.275038163391149,
-0.805866768463298, -0.154761559833806
],
[
0.543960528264717, -0.836310025215453,
0.0685017183295358, 0.568944734102513
], [0.0, 0.0, 0.0, 1.0]])
J_pcorke = numpy.matrix([[
0.154761559833806, -0.416115317270121, -0.181051500179202, 0.0, 0.0,
0.0
],
[
0.0540498757911011, -0.388002774727404,
-0.16881975145483, 0.0, 0.0, 0.0
],
[
-1.56125112837913e-17, -0.0660115669805396,
-0.354377730397368, 0.0, 0.0, 0.0
],
[
2.77555756156289e-17, 0.681969181663071,
0.681969181663071, -0.618588326585383,
0.778592276991744, -0.588120962109346
],
[
1.04083408558608e-16, -0.731380909828662,
-0.731380909828662, -0.576796808883883,
-0.541217849732837, -0.805866768463298
],
[
1.0, 5.72458747072346e-17,
5.72458747072346e-17, 0.533529683779323,
0.317611878460765, 0.0685017183295358
]])
tau_pcorke = numpy.matrix([[0.986185688341252], [16.1055550633721],
[-6.54839494827661], [0.00626452648190803],
[-0.0208972707132494], [5.59805923645945e-5]])
g_pcorke = numpy.matrix([[1.0295354054077e-15], [16.8135891283022],
[-7.29033633567802], [0.00449992182015992],
[-0.026719893613902], [0.0]])
c_pcorke = numpy.matrix([[-0.240237883763956], [-1.0843848843681],
[0.365583586254475], [-0.000123404998742911],
[0.00359951257553373], [1.1075729558421e-5]])
M_pcorke = numpy.matrix(
[[
2.05591694561963, -0.607149114401804, -0.0772414386669633,
0.00110264316988551, 0.000264135330891356, 2.74006873318143e-6
],
[
-0.607149114401804, 2.00048563124708, 0.306870038373922,
-0.000227846077158843, 0.000780985328855223, 7.53260796283046e-6
],
[
-0.0772414386669632, 0.306870038373922, 0.361368447500762,
-0.000267122764221431, 0.00156301630001611, 7.53260796283046e-6
],
[
0.00110264316988551, -0.000227846077158844, -0.000267122764221432,
0.00169083802882858, 1.45380752747483e-20, 3.4606953693396e-5
],
[
0.000264135330891356, 0.000780985328855223, 0.00156301630001611,
2.33135442795155e-20, 0.00064216, 2.44929359829471e-21
],
[
2.74006873318143e-6, 7.53260796283046e-6, 7.53260796283046e-6,
3.4606953693396e-5, 2.44929359829471e-21, 4.0e-5
]])
tau_assrt2 = numpy.matrix([[4.55384377546122], [-18.3482765770679],
[-18.5150406032816], [-3.48354572715293],
[-4.22434630683546], [-8.50580534103007]])
g_assrt2 = numpy.matrix([[-4.44089209850063e-16], [-16.5628257742538],
[-23.0524142097215], [-4.2536826692411],
[-8.37451830056579], [-7.9940463468124]])
c_assrt2 = numpy.matrix([[3.79031173486171], [-6.37866680030809],
[0.668430906458299], [0.0446113764650349],
[2.68800462309735], [-0.0871119983741941]])
M_assrt2 = numpy.matrix(
[[
2.80338554010218, -0.884144436072495, -1.36196693847736,
0.914512998462046, -0.799840112619873, -0.402048288616148
],
[
-0.884144436072495, 5.09045072475534, 3.75033319331688,
-0.822003775709123, 2.00576876282064, -0.136034063369629
],
[
-1.36196693847736, 3.75033319331688, 3.69036945486662,
-0.784876338891255, 1.91719457979014, 0.0657267986145439
],
[
0.914512998462046, -0.822003775709123, -0.784876338891255,
1.86573777350968, -1.06272697183618, 0.00496876753726561
],
[
-0.799840112619873, 2.00576876282064, 1.91719457979014,
-1.06272697183618, 1.2083737144556, -0.396167549434339
],
[
-0.402048288616148, -0.136034063369629, 0.0657267986145439,
0.00496876753726561, -0.396167549434339, 0.162584017097791
]])
assert_precision = 10
assert not numpy.any(numpy.round(T_pcorke - T, assert_precision))
assert not numpy.any(numpy.round(J_pcorke - J, assert_precision))
assert not numpy.any(numpy.round(tau_pcorke - tau, assert_precision))
assert not numpy.any(numpy.round(g_pcorke - g, assert_precision))
assert not numpy.any(numpy.round(c_pcorke - c, assert_precision))
assert not numpy.any(numpy.round(C_dq - c, assert_precision))
assert not numpy.any(numpy.round(M_pcorke - M, assert_precision))
assert not numpy.any(
numpy.round(tau_pcorke - H * numpy.matrix(dynparm_test).T,
assert_precision))
assert not numpy.any(numpy.round(tau_assrt2 - tau_t2, assert_precision))
assert not numpy.any(numpy.round(g_assrt2 - g_t2, assert_precision))
assert not numpy.any(numpy.round(c_assrt2 - c_t2, assert_precision))
assert not numpy.any(numpy.round(C_dq_t2 - c_t2, assert_precision))
assert not numpy.any(numpy.round(M_assrt2 - M_t2, assert_precision))
assert not numpy.any(
numpy.round(tau_assrt2 - H * numpy.matrix(dynparm_test2).T,
assert_precision))
'''
kind = '_rleg'
dh_params = [
(0, 0, 0, q+pi/2),
(pi/2, 0, 0, q+pi/2),
(pi/2, 0, 0, q+aThigh),
('0', -dThigh, 0, q-aTibia-aThigh),
('0', -dTibia, 0, q+aTibia),
(-pi/2, 0, 0, q),
]
'''
# We use the modified convention
print('Define the Robot')
rbtdef = sympybotics.RobotDef('THOR-OP 7DOF Arm',
dh_params,
dh_convention='modified')
print('Find the Dynamic parameters')
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
#print('DH Parameters')
#print(dh_params)
#print('Forward kinematics @ -1')
#print(rbt.geo.T[-1])
#print('Forward kinematics @ -2')
#print(rbt.geo.T[-2])
#print('Jacobian @ -1')
#print(rbt.kin.J[-1])
#print('Jacobian @ -2')
#print(rbt.kin.J[-2])
q = sympybotics.robotdef.q
# defining constants for the offset dh params from the body to the first joint
x_offset = 0
y_offset = 0
z_offset = 0
### First, create a robot object:
### list of tuples with Denavit-Hartenberg parameters (alpha, a, d, theta)
#Standard DH PARAMS
# alpha a d theta
dh_params = [(pi / 2, 0.10795, 0.0889, q + pi / 2), (0, 0.4572, 0.0, q),
(pi / 2, 0.06985, 0.0, q + pi / 2), (-pi / 2, 0.0, 0.4064, q),
(pi / 2, 0.0, 0.0, q), (0, 0.0, 0.254, q)]
rbtdef = sympybotics.RobotDef('Hal', dh_params, 'standard')
# defining which friction model to use
rbtdef.frictionmodel = {
'viscous'
} #'simple' # options are 'simple' and None, defaults to None
# printing the definition of which variables are used as dynamic parameters (i.e. mass, center of mass, etc.)
print(rbtdef.dynparms())
rbt = sympybotics.RobotDynCode(rbtdef)
arm = 'hal_arm'
offsets = str(x_offset) + ', ' + str(y_offset) + ', ' + str(z_offset)
f_kin = open('./' + arm + '_kinematics.py', 'w+')
print >> f_kin, "from math import sin, cos"
#!/usr/bin/python3 添加这行 可在linux终端下执行python代码的文件 直接./example1.py
# -*- coding: UTF-8 -*- 在python2中可支持中文字符
import sympy
import sympybotics
# 搭建CRP前六轴DH参数 1.1 和 1.2分别是a1和a2的长度 四个参数分别为 alpha, a, d, theta 0 0.1703 0.6155 0.3451 0.3599 0.5216
rbtdef = sympybotics.RobotDef('SixJoint', [('0', 0.00, 0.0, 'q1'),
('-pi/2', 0.1703, 0.0, 'q'),
('0', 0.6155, 0.0, 'q-pi/2'),
('-pi/2', 0.3451, 0.0, 'q'),
('pi/2', 0.3599, 0.0, 'q'),
('-pi/2', 0.5216, 0.0, 'q')],
dh_convention='modified')
# rbtdef.frictionmodel = {'Coulomb', 'viscous'} 加上摩擦系数和阻尼系数
# 设定重力加速度的值
rbtdef.gravityacc = sympy.Matrix([0.0, 0.0, -9.81])
# 显示动力学全参数
print(rbtdef.dynparms())
# 构建机器人模型
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
tau_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.invdyn_code,
'tau_out', 'tau', rbtdef)
print(tau_str)
# 向量Mregress中的元素分别表示各关节力矩值在标准参数XB下的系数 torque1=Mregress[0:29] torque2=Mregress[30:59] torque3=Mregress[60:89]
# 向量Mregress是标准参数30*1系数 而不是简化后的最小参数集 且未加库伦摩擦和粘性摩擦
# 计算并显示动力学模型的回归矩阵
Mregress = sympybotics.robotcodegen.robot_code_to_func('C', rbt.H_code,
# <codecell>
endeffector = T * np.array([0.0, 0.0, 0.0, 1.0])
endeffector
# <codecell>
import sympy
import sympybotics
# <codecell>
rbtdef = sympybotics.RobotDef(
'Example Robot', # robot name
[
(0, 0, 0, 0), # list of tuples with Denavit-Hartenberg parameters
('pi/2', 0, 0, 0)
], # (alpha, a, d, theta)
dh_convention='standard' # either 'standard' or 'modified'
)
# <codecell>
rbtdef.frictionmodel = {
'Coulomb', 'viscous'
} # options are None or a combination of 'Coulomb', 'viscous' and 'offset'
# <codecell>
rbtdef.dynparms()
# <codecell>
import string
import sympy
import sympybotics
q = sympybotics.robotdef.q
pi = sympy.pi
''' walkerX rightLimb model '''
walkerX_rightLimb = sympybotics.RobotDef(
'walkerX rightLimb', # robot name
[
(0, 0, 0, q), # list of tuples with Denavit-Hartenberg parameters
(-pi / 2, 0, 0, q + pi / 2),
(-pi / 2, 0, 0.215, q),
(-pi / 2, -0.0248, 0, q),
(pi / 2, 0.0248, 0.2055, q + pi / 2),
(-pi / 2, 0, 0, q + pi / 2),
(pi / 2, 0, 0, q)
], # (alpha, a, d, theta)
dh_convention='modified' # either 'standard' or 'modified'
)
''' set the friction model, including Coulomb and viscous '''
walkerX_rightLimb.frictionmodel = {
'Coulomb', 'viscous'
} #include 'Coulomb', 'viscous' and 'offset'
''' set the motor rotational inertia '''
walkerX_rightLimb.driveinertiamodel = 'simplified' # can be None or 'simplified'
''' get the all link dynamic parameters for identification,
and write to the file '''
DynParms_S = walkerX_rightLimb.dynparms()
# defining constants for the offset dh params from the body to the first joint
x_offset = 0.024645 + 0.055695 * math.cos(math.pi / 4)
y_offset = 0.219645 + 0.055695 * math.sin(math.pi / 4)
z_offset = 0.118588 + 0.011038
### First, create a robot object:
### list of tuples with Denavit-Hartenberg parameters (alpha, a, d, theta)
#Standard DH PARAMS
# alpha a d theta
dh_params = [(-pi / 2, 0.069, 0.27035, q + pi / 4),
(pi / 2, 0.0, 0.0, q + pi / 2), (-pi / 2, 0.069, 0.36435, q),
(pi / 2, 0.0, 0.0, q), (-pi / 2, 0.01, 0.37429, q),
(pi / 2, 0.0, 0.0, q), (0, 0.0, 0.229525, q)]
rbtdef = sympybotics.RobotDef('Baxter Robot', dh_params, 'standard')
# defining which friction model to use
rbtdef.frictionmodel = {
'viscous'
} #'simple' # options are 'simple', 'viscous', and None, defaults to None
# printing the definition of which variables are used as dynamic parameters (i.e. mass, center of mass, etc.)
print(rbtdef.dynparms())
rbt = sympybotics.RobotDynCode(rbtdef)
arm = 'baxter_left'
# this is an ugly way to write everything to python files that are then executable/callable.
# the same can be done for C code
f_handle = open('./' + arm + '_dynamics.py', 'w+')
import sympy
import sympybotics
l1 = 0.3
l2 = 0.2
rbtdef = sympybotics.RobotDef("two_link_planner", [('pi/2', 0, 0, 'q'),
(0, l1, 0, 'q')],
dh_convention='modified')
rbtdef.frictionmodel = {'Coulomb', 'viscous'}
rbtdef.gravityacc = sympy.Matrix([0.0, 0.0, -9.81])
print('dynamic parameters')
print(rbtdef.dynparms())
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
rbt.calc_base_parms()
print('minimum inertial parameters')
rbt.dyn.baseparms
print(rbt.dyn.baseparms)
print('observation matrix')
rbt.Hb_code
print(rbt.Hb_code)