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))
(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])
#print(tau_str)
#rbt.dyn.baseparms
# Save stuff
#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"
#print >> f_kin, "from offset_util import offset_and_reshape"
print >> f_kin, "import numpy as np"
print >> f_kin, "pi = np.pi\n\n\n"
fk_dict = "FK = {"
for i in range(len(rbt.geo.T)):
joint_fk_code = sympy.cse(rbt.geo.T[i])
fk_string = sympybotics.robotcodegen.robot_code_to_func(
'python', joint_fk_code, 'pose', 'joint_fk' + str(i).zfill(2), rbtdef)
fk_list_string = fk_string.split('\n')
''' get the all link dynamic parameters for identification,
and write to the file '''
DynParms_S = walkerX_rightLimb.dynparms()
filename = open(
"/home/meihui/work_ubt/walkerX/dynamics/modeling/dynamic_code/DynParms.txt",
'w')
for i in range(0, 7):
for j in range(0, len(DynParms_S) / 7):
filename.write(str(DynParms_S[i * (len(DynParms_S) / 7) + j]))
filename.write(' ')
filename.write('\n')
filename.close()
#生成动力学符号运算代码
RobotDynCode_walker_rightLimb = sympybotics.RobotDynCode(walkerX_rightLimb,
verbose=True)
#输出机器人正运动学矩阵
Matrix_T = RobotDynCode_walker_rightLimb.geo.T[-1]
# print(Matrix_T)
#输出机器人雅可比矩阵
Matrix_J = RobotDynCode_walker_rightLimb.kin.J[-1]
# print(Matrix_J)
###############
#输出C代码到TXT
###############
# 参数：目标编程语言，代码，函数输出，函数名，机器人模型