def test_CalcPointJacobianNonSquare(self):
""" Computes point Jacobian and checks whether G * qdot is consistent
with CalcPointVelocity. """
self.model = rbdl.Model()
joint_trans_xyz = rbdl.Joint.fromJointType("JointTypeTranslationXYZ")
self.body_1 = self.model.AppendBody(rbdl.SpatialTransform(),
joint_trans_xyz, self.body)
self.body_4 = self.model.AppendBody(rbdl.SpatialTransform(),
joint_trans_xyz, self.body)
point_coords = np.array([0., 0., 1.])
q = np.zeros(self.model.q_size)
G = np.zeros([3, self.model.q_size])
rbdl.CalcPointJacobian(self.model, q, self.body_4, point_coords, G)
qdot = np.ones(self.model.qdot_size)
jac_point_vel = np.dot(G, qdot)
point_vel = rbdl.CalcPointVelocity(self.model, q, qdot, self.body_4,
point_coords)
assert_almost_equal(jac_point_vel, point_vel)
def get_model():
model = rbdl.Model()
m1 = 1
m2 = 1
l1 = 1
l2 = 1
model.gravity = np.array([0, -9.81, 0])
joint_rot_y = rbdl.Joint.fromJointType("JointTypeRevoluteZ")
xtrans = rbdl.SpatialTransform()
xtrans.r = np.array([0., -3., 0.])
link0 = rbdl.Body.fromMassComInertia(0,
np.array([0., 0, 0.]),
np.diag([0.0, 0.0, 0.0])
)
link1 = rbdl.Body.fromMassComInertia(m1,
np.array([0., -l1 * 0.5, 0.]),
np.diag([m1 * l1 * l1 / 3., m1 * l1 * l1 / 30., m1 * l1 * l1 / 3.])
)
link2 = rbdl.Body.fromMassComInertia(m2,
np.array([0., -l2 * 0.5, 0.]),
np.diag([m2 * l2 * l2 / 3., m2 * l2 * l2 / 30., m1 * l2 * l2 / 3.])
)
a = model.AppendBody(rbdl.SpatialTransform(), joint_rot_y, link0)
b = model.AppendBody(xtrans, joint_rot_y, link1)
#c = model.AppendBody(xtrans, joint_rot_y, link2)
return model
def setUp(self):
axis = np.asarray([
[0., 0., 0., 1., 0., 0.],
[0., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0., 1.],
[0., 0., 1., 0., 0., 0.],
[0., 1., 0., 0., 0., 0.],
[1., 0., 0., 0., 0., 0.],
])
self.model = rbdl.Model()
self.model.gravity = np.array ([0., -9.81, 0.])
joint_rot_y = rbdl.Joint.fromJointAxes (axis)
self.body = rbdl.Body.fromMassComInertia (1., np.array([0., 0.0, 0.0]), np.eye(3))
self.xtrans = rbdl.SpatialTransform()
self.xtrans.r = np.array([0., 0., 0.])
self.body_1 = self.model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, self.body)
self.q = np.zeros (self.model.q_size)
self.qdot = np.zeros (self.model.qdot_size)
self.qddot = np.zeros (self.model.qdot_size)
self.tau = np.zeros (self.model.qdot_size)
def dynamic_model(self, mass, height):
m1 = 1
m2 = 1
l1 = 0.5
l2 = 0.5
r1 = 0
r2 = -0.6
I1 = np.diag([0.06, 0.06, 0.002])
I2 = np.diag([0.06, 0.06, 0.002])
joint_rot_y = rbdl.Joint.fromJointType("JointTypeRevoluteY")
xtrans_1 = rbdl.SpatialTransform()
xtrans_1.r = np.array([0.0, 0.0, 0.0])
xtrans_2 = rbdl.SpatialTransform()
xtrans_2.r = np.array([0.0, 0.0, r2])
model = rbdl.Model()
link0 = rbdl.Body.fromMassComInertia(0, np.array([0., 0, 0.]),
np.diag([0.0, 0.0, 0.0]))
body1 = rbdl.Body.fromMassComInertia(m1, np.array([0.0, 0.0, -0.35]),
I1)
body2 = rbdl.Body.fromMassComInertia(m2, np.array([0.0, 0.0, -0.35]),
I2)
self.b0 = model.AppendBody(rbdl.SpatialTransform(), joint_rot_y, link0)
self.b1 = model.AppendBody(xtrans_2, joint_rot_y, body1)
self.b2 = model.AppendBody(xtrans_2, joint_rot_y, body2)
model.gravity = np.array([0, 0, -9.81])
return model
def dynamic_model(self):
# m1 = 1
# m2 = 1
# l1 = 0.5
# l2 = 0.5
# r1 = 0
# r2 = -0.6
# I1 = np.diag([0.06, 0.06, 0.002])
# I2 = np.diag([0.06, 0.06, 0.002])
# joint_rot_y = rbdl.Joint.fromJointType("JointTypeRevoluteY")
#
# xtrans_1 = rbdl.SpatialTransform()
# xtrans_1.r = np.array([0.0, 0.0, 0.0])
#
# xtrans_2 = rbdl.SpatialTransform()
# xtrans_2.r = np.array([0.0, 0.0, r2])
model = rbdl.Model()
parent_dist = {}
mass = {}
com = {}
inertia = {}
mass["head"] = 0
mass["top"] = 1
mass["bottom"] = 1
parent_dist["head"] = np.array([0.0, 0.0, 0.0])
parent_dist["top"] = np.array([0.0, 0.0, 0.0])
parent_dist["bottom"] = np.array([0.0, 0.0, -1.6])
inertia["head"] = np.diag([0.0, 0.0, 0.0])
inertia["top"] = np.diag([1.3117, 1.2876, 0.0274])
inertia["bottom"] = np.diag([1.3131, 1.2891, 0.0274])
com["head"] = np.array([0.0, 0.0, 0.0])
com["top"] = np.array([0.0062, -0.0107, -0.8037])
com["bottom"] = np.array([0.0088, -0.0091, -0.8046])
head = rbdl.Body.fromMassComInertia(mass["head"], com["head"],
inertia["head"])
top = rbdl.Body.fromMassComInertia(mass["top"], com["top"],
inertia["top"])
bottom = rbdl.Body.fromMassComInertia(mass["bottom"], com["bottom"],
inertia["bottom"])
xtrans = rbdl.SpatialTransform()
xtrans.r = np.array([0.0, 0.0, 0.0])
xtrans.E = np.eye(3)
self.head = model.AddBody(0, xtrans,
rbdl.Joint.fromJointType("JointTypeFixed"),
head, "head")
joint_rot_z = rbdl.Joint.fromJointType("JointTypeRevoluteX")
xtrans.r = parent_dist["top"]
self.top = model.AddBody(self.head, xtrans, joint_rot_z, top, "top")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["bottom"]
self.bottom = model.AddBody(self.top, xtrans, joint_rot_z, bottom,
"bottom")
model.gravity = np.array([0, 0, -9.81])
return model
def setUp(self):
self.model = rbdl.Model()
joint_rot_y = rbdl.Joint.fromJointType ("JointTypeFloatingBase")
self.body = rbdl.Body.fromMassComInertia (1., np.array([0., 0.0, 0.5]), np.eye(3) *
0.05)
self.xtrans = rbdl.SpatialTransform()
self.xtrans.r = np.array([0., 0., 0.])
self.body_1 = self.model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, self.body)
self.q = np.zeros (self.model.q_size)
self.qdot = np.zeros (self.model.qdot_size)
self.qddot = np.zeros (self.model.qdot_size)
self.tau = np.zeros (self.model.qdot_size)
def get_G(q):
# Create a new model
model = rbdl.Model()
qdot = np.zeros(num_of_joints).reshape(num_of_joints, -1)
qddot = np.zeros(num_of_joints).reshape(num_of_joints, -1)
tau = np.zeros(num_of_joints).reshape(num_of_joints, -1)
#This for loop iteratively computes the torque values of the entire system
for i in range(Num_Bodies):
# Creating of the transformation matrix between two adjacent bodies
trans = rbdl.SpatialTransform()
trans.E = np.eye(3)
trans.r = parent_dist[i]
# Using principal inertia values from yaml file
I_x = inertia[i][0]
I_y = inertia[i][1]
I_z = inertia[i][2]
# Creation of inertia Matrix
inertia_matrix = np.array([[I_x, 0, 0], [0, I_y, 0], [0, 0, I_z]])
# Creating each body of the robot
body = rbdl.Body.fromMassComInertia(mass_arr[i], com_pos[i],
inertia_matrix)
# Specifying joint Type
joint_type = rbdl.Joint.fromJointType(J_type[i][0])
# Adding body to the model to create the complete robot
append_body = model.AppendBody(trans, joint_type, body)
# RBDL inverse dynamics function
rbdl.InverseDynamics(model, q, qdot, qddot, tau)
print(tau)
def __init__(self, bb_params):
roller_rad = bb_params.r_roller
density = 100
self.model = rbdl.Model()
model = self.model
#define the roller
roller_mass = pi * roller_rad**2 * .81 * density
roller_moi = roller_mass * roller_rad**2
axis = np.asarray([[0., 0., -1., roller_rad, 0, 0]])
joint = rbdl.Joint.fromJointAxes(axis)
ytrans = rbdl.SpatialTransform()
ytrans.r = np.array([0, 0, 0])
body = rbdl.Body.fromMassComInertia(roller_mass, np.array([0., 0.,
0.]),
np.diag([1, 1, roller_moi]))
self.roller = model.AppendBody(rbdl.SpatialTransform(), joint, body)
#define the board (and junk fixed to it)
board_mass = 1
print("Masses: ", roller_mass, board_mass)
axis = np.asarray([[0., 0., 1., roller_rad, 0, 0]])
joint = rbdl.Joint.fromJointAxes(axis)
joint.mReversedPolarity = True
body = rbdl.Body.fromMassComInertia(board_mass, np.array([0, 0.5, 0]),
np.eye(3) * .1)
self.board = model.AppendBody(ytrans, joint, body)
body_height = bb_params.h_body
body_mass = 3
body_moi = body_mass * 0.5 * 0.3**2
if (bb_params.use_second_mass):
axis = np.asarray([[0, 0, 1.0, 0, 0, 0]])
joint = rbdl.Joint.fromJointAxes(axis)
body = rbdl.Body.fromMassComInertia(body_mass, np.array([0, .0,
0]),
np.eye(3) * body_moi)
ytrans.r = np.array([0, body_height, 0])
self.body = model.AppendBody(ytrans, joint, body)
## Second Spinner
axis = np.asarray([[0, 0, 1.0, 0, 0, 0]])
joint = rbdl.Joint.fromJointAxes(axis)
body = rbdl.Body.fromMassComInertia(
body_mass, np.array([0, -body_height + .05, 0]),
np.eye(3) * body_moi / 4.0)
ytrans.r = np.array([0, body_height, 0])
self.body2 = model.AddBody(self.board, ytrans, joint, body)
else:
axis = np.asarray([[0, 0, 1.0, 0, 0, 0]])
joint = rbdl.Joint.fromJointAxes(axis)
body = rbdl.Body.fromMassComInertia(body_mass, np.array([0, 0.,
0]),
np.eye(3) * body_moi)
ytrans.r = np.array([0, body_height, 0])
self.body = model.AppendBody(ytrans, joint, body)
# | G / b3
# ↓ / b2
# j0 b1 /
# Base⊙-------⊙ j1
# 新增：2018年6月11日
# 1. 添加对约束力的显示
# ---------------------------------------
import numpy as np
import rbdl
import matplotlib.pyplot as plt
import matplotlib.animation as animation
l1, l2, l3 = [0.5, 0.4, 0.6]
m1, m2, m3 = [1.0, 2.0, 3.0]
model = rbdl.Model()
model.gravity = np.array([0.0, 0.0, -9.81])
# 1. 创建三个body
ixx = m1 * l1 * l1 / 12
izz = ixx
b1 = rbdl.Body.fromMassComInertia(m1, np.array([0., l1 / 2, 0.]),
np.diag([ixx, 0.0, izz]))
ixx = m2 * l2 * l2 / 12
izz = ixx
b2 = rbdl.Body.fromMassComInertia(m2, np.array([0., 0., 0.]),
np.diag([ixx, 0.0, izz]))
ixx = m3 * l3 * l3 / 12
izz = ixx
b3 = rbdl.Body.fromMassComInertia(m3, np.array([0., l3 / 2, 0.]),
np.diag([ixx, 0.0, izz]))
# 2. 创建关节，平面浮动平台约束(q1, q2, q3)
m, R = 1.0, 0.0001
com = np.array([0.5,0.0,0.0])
I = 2./5.*m*R*R*np.array([[1.0,0.0,0.0],[0.0,1.0,0.0],[0.0,0.0,1.0]])
# Create body and virtual bodies
null_body = rbdl.Body()
null_body.mIsVirtual = True
body = rbdl.Body.fromMassComInertia( m, com, I)
xtrans = rbdl.SpatialTransform()
# Create Needed Joints
joint_rz = rbdl.Joint.fromJointType("JointTypeRevoluteZ")
joint_ry = rbdl.Joint.fromJointType("JointTypeRevoluteY")
joint_rx = rbdl.Joint.fromJointType("JointTypeRevoluteX")
joint_e = rbdl.Joint.fromJointType("JointTypeEulerZYX")
joint_s = rbdl.Joint.fromJointType("JointTypeSpherical")
# Create Spherical Joint by individual Revolute Joints q = [thz,thy,thx]
model1 = rbdl.Model()
model1.AddBody(0,xtrans, joint_rz, null_body)
model1.AppendBody(xtrans, joint_ry, null_body)
model1.AppendBody(xtrans, joint_rx, body)
model1.gravity = np.array([0.0,0.0,-1.0*10.0])
# Creaate Spherical Joints by Euler ZYX rotation E0s = Rz*Ry*Rx
model2 = rbdl.Model()
model2.AddBody(0,xtrans, joint_e, body)
model2.gravity = np.array([0.0,0.0,-1.0*10.0])
# Creaate Spherical Joints by Spherical with Quaternions q = [q0,q1,q2,q3]
model3 = rbdl.Model()
model3.AddBody(0,xtrans, joint_s, body)
model3.gravity = np.array([0.0,0.0,-1.0*10.0])
def QfromAxisAngle(v,th):
d = np.sqrt(np.dot(v,v))
def dynamic_model():
# add in mass and height params
model = rbdl.Model()
bodies = {}
mass = {}
com = {}
inertia = {}
bodies["right"] = {}
bodies["left"] = {}
segments = ["thigh", "shank", "foot"]
mass["hip"] = 2.37
mass["right_thigh"] = 2.11
mass["left_thigh"] = 2.11
mass["right_shank"] = 1.28
mass["left_shank"] = 1.28
mass["right_foot"] = 0.86
mass["left_foot"] = 0.86
parent_dist = {}
parent_dist["hip"] = np.array([0.0, 0.0, 0.0])
parent_dist["left_thigh"] = np.array([0.237, -0.124, -0.144])
parent_dist["left_shank"] = np.array([0.033, -0.03, -0.436])
parent_dist["left_foot"] = np.array([0.02, -0.027, -0.39])
parent_dist["right_thigh"] = np.array([-0.237, -0.124, -0.144])
parent_dist["right_shank"] = np.array([0.033, -0.03, -0.436])
parent_dist["right_foot"] = np.array([0.02, -0.027, -0.39])
inertia["hip"] = np.diag([0.0, 0.0, 0.0])
inertia["left_thigh"] = np.diag([0.0, 0.0, 0.07])
inertia["left_shank"] = np.diag([0.18, 0.18, 0.0])
inertia["left_foot"] = np.diag([0.07, 0.07, 0.0])
inertia["right_thigh"] = np.diag([0.0, 0.00, 0.07])
inertia["right_shank"] = np.diag([0.18, 0.18, 0.0])
inertia["right_foot"] = np.diag([0.07, 0.07, 0.0])
com["hip"] = np.array([0.00, -0.02, 0.18])
com["left_thigh"] = np.array([0.02, 0.01, -0.09])
com["left_shank"] = np.array([-0.02, -0.007, 0.06])
com["left_foot"] = np.array([0.08, -0.06, 0.04])
com["right_thigh"] = np.array([-0.02, 0.01, -0.09])
com["right_shank"] = np.array([0.02, -0.007, 0.06])
com["right_foot"] = np.array([0.08, -0.06, 0.04])
hip_body = rbdl.Body.fromMassComInertia(mass["hip"], com["hip"],
inertia["hip"])
for segs in segments:
bodies["right_" + segs] = rbdl.Body.fromMassComInertia(
mass["right_" + segs], com["right_" + segs],
inertia["right_" + segs])
bodies["left_" + segs] = rbdl.Body.fromMassComInertia(
mass["left_" + segs], com["left_" + segs], inertia["left_" + segs])
xtrans = rbdl.SpatialTransform()
xtrans.r = np.array([0.0, 0.0, 0.0])
xtrans.E = np.eye(3)
hip = model.AddBody(0, xtrans, rbdl.Joint.fromJointType("JointTypeFixed"),
hip_body, "hip")
joint_rot_z = rbdl.Joint.fromJointType("JointTypeRevoluteX")
xtrans.r = parent_dist["left_thigh"]
left_thigh = model.AddBody(hip, xtrans, joint_rot_z, bodies["left_thigh"],
"left_thigh")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["left_shank"]
left_shank = model.AddBody(left_thigh, xtrans, joint_rot_z,
bodies["left_shank"], "left_shank")
xtrans.r = parent_dist["left_foot"]
left_foot = model.AddBody(left_shank, xtrans, joint_rot_z,
bodies["left_foot"], "left_foot")
xtrans.r = parent_dist["right_thigh"]
right_thigh = model.AddBody(hip, xtrans, joint_rot_z,
bodies["right_thigh"], "right_thigh")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["right_shank"]
right_shank = model.AddBody(right_thigh, xtrans, joint_rot_z,
bodies["right_shank"], "right_shank")
xtrans.r = parent_dist["right_foot"]
right_foot = model.AddBody(right_shank, xtrans, joint_rot_z,
bodies["right_foot"], "right_foot")
model.gravity = np.array([0, 0, -9.81])
return model
def biped_model_create():
model = rbdl.Model()
model.gravity = np.array([0.0, 0.0, -9.81])
thigh_len = 0.5
shank_len = 0.5
# 1. 创建body,全部写下来是为了方便调试
# 左右腿一般是对称的
# 非精准的建模也是允许的
mass, com = 20.0, np.array([0., 0., 0.]) # 躯干
inertia = np.diag([0.37, 0.29, 0.128])
b_torso = rbdl.Body.fromMassComInertia(mass, com, inertia)
mass, com = 0.5, np.array([0., 0., 0.]) # 连接件1
inertia = np.diag([0.0003, 0.0003, 0.0003])
b_link1 = rbdl.Body.fromMassComInertia(mass, com, inertia)
mass, com = 2.0, np.array([0., 0., -thigh_len / 2]) # 大腿
inertia = np.diag([0.04208, 0.04208, 0.00083])
b_thigh = rbdl.Body.fromMassComInertia(mass, com, inertia)
mass, com = 2.0, np.array([0., 0., -shank_len / 2]) # 小腿
inertia = np.diag([0.04208, 0.04208, 0.00083])
b_shank = rbdl.Body.fromMassComInertia(mass, com, inertia)
# 允许存在误差，不对足端进行建模
# mass, com = 0.5, np.array([0., 0., 0.]) # 足--看能不能固定关节
# inertia = np.diag([0.0001, 0.0001, 0.0001])
# b_foot = rbdl.Body.fromMassComInertia(mass, com, inertia)
# 2. 创建运动副
space_free_type = np.diag([1, 1, 1, 1, 1, 1])
joint_float_space = rbdl.Joint.fromJointAxes(space_free_type)
joint_rot_x = rbdl.Joint.fromJointType("JointTypeRevoluteX")
joint_rot_y = rbdl.Joint.fromJointType("JointTypeRevoluteY")
# 3. 向模型中添加body
trans = rbdl.SpatialTransform() # 躯干
trans.r = np.array([0.0, 0.0, 0.0])
id_torso = model.AppendBody(trans, joint_float_space, b_torso)
trans.r = np.array([0.0, 0.12, -0.2]) # 左连接件
id_left_link1 = model.AddBody(id_torso, trans, joint_rot_x, b_link1)
trans.r = np.array([0.0, 0.0, 0.0]) # 左大腿
id_left_thigh = model.AddBody(id_left_link1, trans, joint_rot_y, b_thigh)
trans.r = np.array([0.0, 0.0, -thigh_len]) # 左小腿
id_left_shank = model.AddBody(id_left_thigh, trans, joint_rot_y, b_shank)
trans.r = np.array([0.0, -0.12, -0.2]) # 右连接件
id_right_link1 = model.AddBody(id_torso, trans, joint_rot_x, b_link1)
trans.r = np.array([0.0, 0.0, 0.0]) # 右大腿
id_right_thigh = model.AddBody(id_right_link1, trans, joint_rot_y, b_thigh)
trans.r = np.array([0.0, 0.0, -thigh_len]) # 右小腿
id_right_shank = model.AddBody(id_right_thigh, trans, joint_rot_y, b_shank)
# 4. 设计约束
cs_left = rbdl.ConstraintSet() # 左腿约束
c_point = np.array([0.0, 0.0, -0.55])
cs_left.AddConstraint(id_left_shank, c_point, np.array([1., 0., 0.]), 'ground_x'.encode('utf-8'))
cs_left.AddConstraint(id_left_shank, c_point, np.array([0., 1., 0.]), 'ground_y'.encode('utf-8'))
cs_left.AddConstraint(id_left_shank, c_point, np.array([0., 0., 1.]), 'ground_z'.encode('utf-8'))
cs_right = rbdl.ConstraintSet() # 右腿约束
c_point = np.array([0.0, 0.0, -0.55])
cs_right.AddConstraint(id_right_shank, c_point, np.array([1., 0., 0.]), 'ground_x'.encode('utf-8'))
cs_right.AddConstraint(id_right_shank, c_point, np.array([0., 1., 0.]), 'ground_y'.encode('utf-8'))
cs_right.AddConstraint(id_right_shank, c_point, np.array([0., 0., 1.]), 'ground_z'.encode('utf-8'))
lid = [id_torso,
id_left_link1,id_left_thigh, id_left_shank,
id_right_link1, id_right_thigh, id_right_shank]
# 添加部件的id_list
return model, cs_left, cs_right, lid
def model_generator():
mass_arr = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
COM_matrix = np.array([[0.001, 0.0, 0.06], [0.0, -0.017, 0.134],
[0.0, -0.074, 0.009], [0.0, 0.017, 0.134],
[-0.001, 0.081, 0.008], [0.0, -0.017, 0.129],
[0.0, 0.007, 0.068], [0.006, 0.0, 0.015],
[0., 0., 0.], [0., 0., 0.]])
inertia_matrix = dict()
inertia_matrix['base'] = np.diag([0., 0., 0.])
inertia_matrix['link1'] = np.diag([0.00818, 0.00737791, 0.00331532])
inertia_matrix['link2'] = np.diag([0.00820624, 0.00337541, 0.00741212])
inertia_matrix['link3'] = np.diag([0.00812617, 0.00739079, 0.0033597])
inertia_matrix['link4'] = np.diag([0.00819873, 0.00333248, 0.00737665])
inertia_matrix['link5'] = np.diag([0.00775252, 0.00696512, 0.00333736])
inertia_matrix['link6'] = np.diag([0.00300679, 0.0032974, 0.0031543])
inertia_matrix['link7'] = np.diag([0.00065849, 0.00065487, 0.00113144])
inertia_matrix['bar'] = np.diag(
[9.07819496e-04, 9.07819496e-04, 8.06572673e-05])
inertia_matrix['tip'] = np.diag([0.00014625, 0.00014625, 0.00017098])
joint_rot_z = rbdl.Joint.fromJointType("JointTypeRevoluteZ")
joint_fixed = rbdl.Joint.fromJointType("JointTypeFixed")
arm_model = rbdl.Model()
arm_model.gravity = [0., 0., -9.81]
trans0 = rbdl.SpatialTransform()
trans0.E = np.eye(3)
trans0.r = np.array([0.0, 0.0, 0.0])
trans1 = rbdl.SpatialTransform()
trans1.E = np.eye(3)
trans1.r = np.array([0.0, 0.0, 0.103])
trans2 = rbdl.SpatialTransform()
trans2.E = np.transpose(
np.array([1., 0., 0., 0., 0., 1., 0., -1., 0.]).reshape(3, 3))
trans2.r = np.array([0., 0.013, 0.209])
trans3 = rbdl.SpatialTransform()
trans3.E = np.transpose(
np.array([1., 0., 0., 0., 0., -1., 0., 1., 0.]).reshape(3, 3))
trans3.r = np.array([0., -0.194, -0.009])
trans4 = rbdl.SpatialTransform()
trans4.E = np.transpose(
np.array([1., 0., 0., 0., 0., -1., 0., 1., 0.]).reshape(3, 3))
trans4.r = np.array([0., -0.013, 0.202])
trans5 = rbdl.SpatialTransform()
trans5.E = np.transpose(
np.array([1., 0., 0., 0., 0., 1., 0., -1., 0.]).reshape(3, 3))
trans5.r = np.array([-0.002, 0.202, -0.008])
trans6 = rbdl.SpatialTransform()
trans6.E = np.transpose(
np.array([1., 0., 0., 0., 0., 1., 0., -1., 0.]).reshape(3, 3))
trans6.r = np.array([0.002, -0.052, 0.204])
trans7 = rbdl.SpatialTransform()
trans7.E = np.transpose(
np.array([1., 0., 0., 0., 0., -1., 0., 1., 0.]).reshape(3, 3))
trans7.r = np.array([-0.003, -0.05, 0.053])
trans8 = rbdl.SpatialTransform()
trans8.E = np.eye(3)
trans8.r = np.array([0.005, -0.0, 0.08])
trans9 = rbdl.SpatialTransform()
trans9.E = np.eye(3)
trans9.r = np.array([0.001, -0.0, 0.056])
obj1 = rbdl.Body.fromMassComInertia(mass_arr[0], COM_matrix[0],
inertia_matrix['base'])
obj2 = rbdl.Body.fromMassComInertia(mass_arr[1], COM_matrix[1],
inertia_matrix['link1'])
obj3 = rbdl.Body.fromMassComInertia(mass_arr[2], COM_matrix[2],
inertia_matrix['link2'])
obj4 = rbdl.Body.fromMassComInertia(mass_arr[3], COM_matrix[3],
inertia_matrix['link3'])
obj5 = rbdl.Body.fromMassComInertia(mass_arr[4], COM_matrix[4],
inertia_matrix['link4'])
obj6 = rbdl.Body.fromMassComInertia(mass_arr[5], COM_matrix[5],
inertia_matrix['link5'])
obj7 = rbdl.Body.fromMassComInertia(mass_arr[6], COM_matrix[6],
inertia_matrix['link6'])
obj8 = rbdl.Body.fromMassComInertia(mass_arr[7], COM_matrix[7],
inertia_matrix['link7'])
obj9 = rbdl.Body.fromMassComInertia(mass_arr[8], COM_matrix[8],
inertia_matrix['bar'])
obj10 = rbdl.Body.fromMassComInertia(mass_arr[9], COM_matrix[9],
inertia_matrix['tip'])
body1 = arm_model.AppendBody(trans0, joint_fixed, obj1)
body2 = arm_model.AppendBody(trans1, joint_rot_z, obj2)
body3 = arm_model.AppendBody(trans2, joint_rot_z, obj3)
body4 = arm_model.AppendBody(trans3, joint_rot_z, obj4)
body5 = arm_model.AppendBody(trans4, joint_rot_z, obj5)
body6 = arm_model.AppendBody(trans5, joint_rot_z, obj6)
body7 = arm_model.AppendBody(trans6, joint_rot_z, obj7)
body8 = arm_model.AppendBody(trans7, joint_fixed, obj8)
body9 = arm_model.AppendBody(trans8, joint_fixed, obj9)
body10 = arm_model.AppendBody(trans9, joint_fixed, obj10)
return arm_model, body10
def setUp(self):
# Make a triple-perpendicular-pendulum in absolute coordinates using
# 5 loop constraints to implement the first 2 pin joints
#
# The perpendicular pendulum pictured with joint angles of 0,0.
# The first joint rotates about the x axis, while the second
# joint rotates about the local y axis of link 1
#
# y
# |
# |___ x
# z / |
# | |
# / | | link1
# | |
# / | |
# axis1:z0| |__________
# (_____________) link 2
# | |
# |
#
# |
#
# | axis2:y1
#
self.model = rbdl.Model()
self.model.gravity = np.array([0.,-9.81,0.])
l1=1.
l2=1.
m1=1.
m2=1.
self.l1=l1
self.l2=l2
self.m1=m1
self.m2=m2
c1 = np.array([0.,-l1*0.5,0.])
J1 = np.array([[m1*l1*l1/3., 0., 0.],
[ 0., m1*l1*l1/30., 0.],
[ 0., 0., m1*l1*l1/3.]])
c2 = np.array([l2*0.5,0.,0.])
J2 = np.array([ [m2*l2*l2/30., 0., 0.],
[ 0., m2*l2*l2/3., 0.],
[ 0., 0., m2*l2*l2/3.]])
Xp1 = rbdl.SpatialTransform()
Xp1.r = np.array([0.,0.,0.])
Xp1.E = np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])
Xs1 = rbdl.SpatialTransform()
Xs1.r = np.array([0.,0.,0.])
Xs1.E = np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])
Xp2 = rbdl.SpatialTransform()
Xp2.r = np.array([0.,-l1,0.])
Xp2.E = np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])
Xs2 = rbdl.SpatialTransform()
Xs2.r = np.array([0.,0.,0.])
Xs2.E = np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])
axis = np.asarray([
[0., 0., 0., 1., 0., 0.],
[0., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0., 1.],
[0., 0., 1., 0., 0., 0.],
[0., 1., 0., 0., 0., 0.],
[1., 0., 0., 0., 0., 0.],
])
joint6Dof = rbdl.Joint.fromJointAxes (axis)
self.link1 = rbdl.Body.fromMassComInertia(m1,c1,J1)
self.link2 = rbdl.Body.fromMassComInertia(m2,c2,J2)
self.iLink1= self.model.AppendBody(rbdl.SpatialTransform(),joint6Dof,
self.link1)
self.iLink2= self.model.AppendBody(rbdl.SpatialTransform(),joint6Dof,
self.link2)
#point_coords = np.array ([0., 0., 1.])
self.q = np.zeros (self.model.q_size)
self.qd = np.zeros (self.model.qdot_size)
self.qdd = np.zeros (self.model.qdot_size)
self.tau = np.zeros (self.model.qdot_size)
assert(self.iLink1==6)
assert(self.iLink2==12)
self.cs = rbdl.ConstraintSet()
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,1,0,0]),False,0.1,"LoopGroundLink1",7)
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,0,1,0]),False)
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,0,0,1]),False)
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[1,0,0,0,0,0]),False)
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,1,0,0,0,0]),False)
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,1,0,0]),False,0.1,"LoopLink1Link2",11)
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,0,1,0]))
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,0,0,1]))
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[1,0,0,0,0,0]))
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,1,0,0,0]))
self.cs.Bind(self.model)
def dynamic_model(self, total_mass, height):
model = rbdl.Model()
return model
def Inverse_dynamics_calc_func(q_val, qdot_val, qddot_val):
# Create a new model
model = rbdl.Model()
print "No. of bodies =", len(q_val)
print "Q_values are", q_val
# print "Q_values are" , q_val[6]
# Creation of mass array from yaml file
mass = get_mass_array(data, Bodies)
# Number of bodies present in the robot
no_of_bodies, random_var = Bodies_count(data)
# Creation of joint type array from yaml file
joint_name_temp = np.array([["JointTypeFixed"]])
joint_type_name = "JointTypeRevoluteZ"
for count in range(0, int(no_of_bodies) - 1):
joint_name_temp = np.append(joint_name_temp, [[joint_type_name]],
axis=0)
joint_name = joint_name_temp
# joint_name = [["JointTypeFixed"], ["JointTypeRevoluteZ"], ["JointTypeFixed"], ["JointTypeFixed"], ["JointTypeFixed"], ["JointTypeFixed"], ["JointTypeFixed"]]
# The centre of mass array from yaml file
com_val = get_inertial_offset(data, Bodies)
# The inertia array from yaml file
inertia_val = get_inertia_values(data, Bodies)
# The distance vector array between two adjacent bodies from yaml file
r_val = get_parent_pivot(data, Joints)
q = q_val
qdot = qddot_val
qddot = qddot_val
tau = np.zeros(no_of_bodies - 1)
#This for loop iteratively computes the torque values of the entire system
for i in range(0, no_of_bodies - 1):
# Creating of the transformation matrix between two adjacent bodies
trans = rbdl.SpatialTransform()
trans.E = np.eye(3)
trans.r = r_val[i]
# Using principal inertia values from yaml file
I_x = inertia_val[i][0]
I_y = inertia_val[i][1]
I_z = inertia_val[i][2]
# count = count + 1
# Creation of inertia Matrix
inertia_matrix = np.array([[I_x, 0, 0], [0, I_y, 0], [0, 0, I_z]])
# Creating each body of the robot
body = rbdl.Body.fromMassComInertia(mass[i], com_val[i],
inertia_matrix)
# Specifying joint Type
joint_type = rbdl.Joint.fromJointType(joint_name[i][0])
# print(joint_type)
# Assigning the q, q dot, q dot dot values to the input values of inverse dynamics
# Adding body to the model to create the complete robot
append_body = model.AppendBody(trans, joint_type, body)
# RBDL inverse dynamics function
rbdl.InverseDynamics(model, q, qdot, qddot, tau)
# print("length of tau is ", len(tau))
return tau
def YamlToRBDLmodel(data, Bodies, Joints):
file_path = "/home/sonu/KUKA_7Dof/blender-kuka7dof.yaml"
data = yaml_loader(file_path)
model = rbdl.Model()
model.gravity = [0, 0, -9.81]
no_of_bodies, random_var = Bodies_count(data)
mass = get_mass_array(data, Bodies)
mass_arr = np.array([[1], [1], [1], [1], [1], [1], [1], [1]]) #good
com_val = get_inertial_offset(data, Bodies)
# print("COM val is\n", com_val)
inertia_val = get_inertia_values(data, Bodies)
# print(inertia_val)
parent_dist = get_parent_pivot(data, Joints)
# print(r_val)
J_type = get_joint_type(data, Joints)
joint_rot_z = rbdl.Joint.fromJointType("JointTypeRevoluteZ")
joint_fixed = rbdl.Joint.fromJointType("JointTypeFixed")
# Parsing child and parent axes to create the transformation matrix between two adjacent bodies
child_axes = get_child_axes(data, Joints)
parent_axes = get_parent_axes(data, Joints)
for i in range(0, no_of_bodies):
# Creating of the transformation matrix between two adjacent bodies
trans = rbdl.SpatialTransform()
print("\nParameters for body", i)
if i == 0:
trans.E = np.eye(3)
# print("R is\n", trans.E);
trans.r = [0.0, 0.0, 0.0]
# print("T is\n", trans.r);
print("T is\n", trans)
else:
# get parent and child axes of the pair
child_axis = dicttoVec(child_axes[i - 1][0])
parent_axis = dicttoVec(parent_axes[i - 1][0])
# Find the rotation matrix between child and parent
r_mat = rot_matrix_from_vecs(mathutils.Vector(child_axis),
mathutils.Vector(parent_axis))
r_mat_np = MatToNpArray(r_mat)
# print("R is", r_mat_np)
trans.E = r_mat_np
# print("R is\n", trans.E);
trans.r = parent_dist[i]
# print("T is\n", trans.r);
print("T is\n", trans)
# Creating Inertia matrix with just principle Inertia values
I_x = inertia_val[i][0]
I_y = inertia_val[i][1]
I_z = inertia_val[i][2]
inertia_matrix = np.array([[I_x, 0, 0], [0, I_y, 0], [0, 0, I_z]])
print("\nInertia matrix is\n", inertia_matrix)
print("\nCOM is\n", com_val[i])
print("\nMass of the body is\n", mass[i])
# Creating each body of the robot
body = rbdl.Body.fromMassComInertia(mass[i], com_val[i],
inertia_matrix)
# Specifying joint Type
if i == 0:
joint_type = joint_fixed
else:
joint_type = joint_rot_z
# joint_type = rbdl.Joint.fromJointType(joint_name[i][0])
print("joint type is", joint_type)
# Adding body to the model to create the complete robot
model.AppendBody(trans, joint_type, body)
# print(i)
return model
def dynamic_model(self):
model = rbdl.Model()
bodies = {}
mass = {}
com = {}
inertia = {}
bodies["right"] = {}
bodies["left"] = {}
segments = ["thigh", "shank", "foot"]
mass["hip"] = 5
mass["right_thigh"] = 2.11
mass["left_thigh"] = 2.11
mass["right_shank"] = 1.28
mass["left_shank"] = 1.28
mass["right_foot"] = 0.866
mass["left_foot"] = 0.866
parent_dist = {}
parent_dist["hip"] = np.array([0.0, 0.0, 0.0])
parent_dist["left_thigh"] = np.array([0.0061, -0.0263, -.6277])
parent_dist["left_shank"] = np.array([0.1445, 0.0231, -0.4364])
parent_dist["left_foot"] = np.array([0.0, 0.0268, -0.3881])
parent_dist["right_thigh"] = np.array([-0.0061, -0.0263, -0.6277])
parent_dist["right_shank"] = np.array([-0.1445, 0.0231, -0.4364])
parent_dist["right_foot"] = np.array([-0.0, 0.0268, -0.3881])
inertia["hip"] = np.diag([ 0.0,0.0,0.0])
inertia["left_thigh"] = np.diag([0.0, 0.0, 0.07])
inertia["left_shank"] = np.diag([0.0, 0.0, 0.05])
inertia["left_foot"] = np.diag([0.0, 0.0, 0.0])
inertia["right_thigh"] = np.diag([0.0, 0.00, 0.07])
inertia["right_shank"] = np.diag([0.0, 0.0, 0.05])
inertia["right_foot"] = np.diag([0.0, 0.0, 0.0])
com["hip"] = np.array([0.00, -0.02, 0.18])
com["left_thigh"] = np.array([0.02, 0.01, -0.09])
com["left_shank"] = np.array([-0.02, -0.007, 0.06])
com["left_foot"] = np.array([0.08, -0.06, 0.04])
com["right_thigh"] = np.array([-0.02, 0.01, -0.09])
com["right_shank"] = np.array([0.02, -0.007, 0.06])
com["right_foot"] = np.array([0.08, -0.06, 0.04])
com["left_thigh"] = np.array([.1009, 0.0088, -0.1974]) # needs to be fixed in blender
com["left_shank"] = np.array([-0.0305, 0.0174, -0.2118])
com["left_foot"] = np.array([-0.0052, -0.0017, -0.0235])
com["right_thigh"] = np.array([-.1009, 0.0088, -0.1974]) # needs to be fixed in blender
com["right_shank"] = np.array([0.0305, 0.0174, -0.2118])
com["right_foot"] = np.array([0.0052, -0.0017, -0.0235])
hip_body = rbdl.Body.fromMassComInertia(mass["hip"], com["hip"], inertia["hip"])
for segs in segments:
bodies["right_" + segs] = rbdl.Body.fromMassComInertia(mass["right_" + segs], com["right_" + segs], inertia["right_" + segs])
bodies["left_" + segs] = rbdl.Body.fromMassComInertia(mass["left_" + segs], com["left_" + segs], inertia["left_" + segs])
xtrans = rbdl.SpatialTransform()
xtrans.r = np.array([0.0, 0.0, 0.0])
xtrans.E = np.eye(3)
self.hip = model.AddBody(0, xtrans, rbdl.Joint.fromJointType("JointTypeFixed"), hip_body, "hip")
joint_rot_z = rbdl.Joint.fromJointType("JointTypeRevoluteX")
xtrans.r = parent_dist["left_thigh"]
self.left_thigh = model.AddBody(self.hip, xtrans, joint_rot_z, bodies["left_thigh"], "left_thigh")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["left_shank"]
self.left_shank = model.AddBody(self.left_thigh, xtrans, joint_rot_z, bodies["left_shank"], "left_shank")
xtrans.r = parent_dist["left_foot"]
self.left_foot = model.AddBody(self.left_shank, xtrans, joint_rot_z, bodies["left_foot"], "left_foot")
xtrans.r = parent_dist["right_thigh"]
self.right_thigh = model.AddBody(self.hip, xtrans, joint_rot_z, bodies["right_thigh"], "right_thigh")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["right_shank"]
self.right_shank = model.AddBody(self.right_thigh, xtrans, joint_rot_z, bodies["right_shank"], "right_shank")
xtrans.r = parent_dist["right_foot"]
self.right_foot = model.AddBody(self.right_shank, xtrans, joint_rot_z, bodies["right_foot"], "right_foot")
model.gravity = np.array([0, 0, -9.81])
return model
def dynamic_model_func(q_val, qdot_val, qddot_val, num_val):
# Create a new model
model = rbdl.Model()
# Creation of mass array from yaml file
mass = get_mass_array(data, Bodies)
# Number of bodies present in the robot
no_body, random_var = Bodies_count(data)
# Creation of joint type array from yaml file
joint_name_temp = np.array([["JointTypeFixed"]])
joint_type_name = "JointTypeRevoluteZ"
for count in range(0, int(no_body) - 1):
joint_name_temp = np.append(joint_name_temp, [[joint_type_name]],
axis=0)
joint_name = joint_name_temp
# The centre of mass array from yaml file
com_val = get_inertial_offset(data, Bodies)
# The inertia array from yaml file
inertia_val = get_inertia_values(data, Bodies)
# The distance vector array between two adjacent bodies from yaml file
r_val = get_parent_pivot(data, Joints)
for i in range(0, no_body):
# Creating of the transformation matrix between two adjacent bodies
trans = rbdl.SpatialTransform()
trans.E = np.eye(3)
trans.r = r_val[i]
# Using principal inertia values from yaml file
a = inertia_val[i][0]
b = inertia_val[i][1]
c = inertia_val[i][2]
# count = count + 1
# Creation of inertia Matrix
inertia_matrix = np.array([[a, 0, 0], [0, b, 0], [0, 0, c]])
# Creating each body of the robot
body = rbdl.Body.fromMassComInertia(mass[i], com_val[i],
inertia_matrix)
# Specifying joint Type
joint_type = rbdl.Joint.fromJointType(joint_name[i][0])
# Adding body to the model to create the complete robot
append_body = model.AppendBody(trans, joint_type, body)
# Initialize q, qdot, qddot and tau values
q = np.zeros(model.q_size)
qdot = np.zeros(model.qdot_size)
qddot = np.zeros(model.qdot_size)
tau = np.zeros(model.qdot_size)
# Assigning the q, qdot, qddot values to the input values of inverse dynamics
q[num_val] = q_val
qdot[num_val] = qdot_val
qddot[num_val] = qddot_val
# RBDL inverse dynamics function
rbdl.InverseDynamics(model, q, qdot, qddot, tau)
# print "tau is ", tau
return tau
def make_dynamic_model(self):
# add in mass and height params
model = rbdl.Model()
bodies = {}
mass = {}
com = {}
inertia = {}
bodies["right"] = {}
bodies["left"] = {}
segments = ["thigh", "shank", "foot"]
mass["hip"] = 2.37
mass["right_thigh"] = 2.11
mass["left_thigh"] = 2.11
mass["right_shank"] = 1.28
mass["left_shank"] = 1.28
mass["right_foot"] = 0.86
mass["left_foot"] = 0.86
parent_dist = {}
parent_dist["hip"] = np.array([0.0, 0.0, 0.0])
parent_dist["left_thigh"] = np.array([0.237, -0.124, -0.144])
parent_dist["left_shank"] = np.array([0.033, -0.03, -0.436])
parent_dist["left_foot"] = np.array([0.02, -0.027, -0.39])
parent_dist["right_thigh"] = np.array([-0.237, -0.124, -0.144])
parent_dist["right_shank"] = np.array([0.033, -0.03, -0.436])
parent_dist["right_foot"] = np.array([0.02, -0.027, -0.39])
inertia["hip"] = np.diag([0.0, 0.0, 0.0])
inertia["left_thigh"] = np.diag([0.0, 0.0, 0.07])
inertia["left_shank"] = np.diag([0.18, 0.18, 0.0])
inertia["left_foot"] = np.diag([0.07, 0.07, 0.0])
inertia["right_thigh"] = np.diag([0.0, 0.00, 0.07])
inertia["right_shank"] = np.diag([0.18, 0.18, 0.0])
inertia["right_foot"] = np.diag([0.07, 0.07, 0.0])
com["hip"] = np.array([0.00, -0.02, 0.18])
com["left_thigh"] = np.array([0.02, 0.01, -0.09])
com["left_shank"] = np.array([-0.02, -0.007, 0.06])
com["left_foot"] = np.array([0.08, -0.06, 0.04])
com["right_thigh"] = np.array([-0.02, 0.01, -0.09])
com["right_shank"] = np.array([0.02, -0.007, 0.06])
com["right_foot"] = np.array([0.08, -0.06, 0.04])
hip_body = rbdl.Body.fromMassComInertia(mass["hip"], com["hip"],
inertia["hip"])
for segs in segments:
bodies["right_" + segs] = rbdl.Body.fromMassComInertia(
mass["right_" + segs], com["right_" + segs],
inertia["right_" + segs])
bodies["left_" + segs] = rbdl.Body.fromMassComInertia(
mass["left_" + segs], com["left_" + segs],
inertia["left_" + segs])
xtrans = rbdl.SpatialTransform()
xtrans.r = np.array([0.0, 0.0, 0.0])
xtrans.E = np.eye(3)
self.hip = model.AddBody(0, xtrans,
rbdl.Joint.fromJointType("JointTypeFixed"),
hip_body, "hip")
joint_rot_z = rbdl.Joint.fromJointType("JointTypeRevoluteX")
xtrans.r = parent_dist["left_thigh"]
self.left_thigh = model.AddBody(self.hip, xtrans, joint_rot_z,
bodies["left_thigh"], "left_thigh")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["left_shank"]
self.left_shank = model.AddBody(self.left_thigh, xtrans, joint_rot_z,
bodies["left_shank"], "left_shank")
xtrans.r = parent_dist["left_foot"]
self.left_foot = model.AddBody(self.left_shank, xtrans, joint_rot_z,
bodies["left_foot"], "left_foot")
xtrans.r = parent_dist["right_thigh"]
self.right_thigh = model.AddBody(self.hip, xtrans, joint_rot_z,
bodies["right_thigh"], "right_thigh")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["right_shank"]
self.right_shank = model.AddBody(self.right_thigh, xtrans, joint_rot_z,
bodies["right_shank"], "right_shank")
xtrans.r = parent_dist["right_foot"]
self.right_foot = model.AddBody(self.right_shank, xtrans, joint_rot_z,
bodies["right_foot"], "right_foot")
model.gravity = np.array([0, 0, -9.81])
# constraint_set_right = rbdl.ConstraintSet()
# constraint_set_left = rbdl.ConstraintSet()
# constraint_set_both = rbdl.ConstraintSet()
#
# constraint_set_right.AddContactConstraint(id_r, heel_point, np.array([1., 0., 0.]), "right_heel_x")
# constraint_set_right.AddContactConstraint(id_r, heel_point, np.array([0., 1., 0.]), "right_heel_y")
#
# constraint_set_left.AddContactConstraint(id_l, heel_point, np.array([1., 0., 0.]), "left_heel_x")
# constraint_set_left.AddContactConstraint(id_l, heel_point, np.array([0., 1., 0.]), "left_heel_y")
#
# constraint_set_both.AddContactConstraint(id_r, heel_point, np.array([1., 0., 0.]), "right_heel_x")
# constraint_set_both.AddContactConstraint(id_r, heel_point, np.array([0., 1., 0.]), "right_heel_y")
# constraint_set_both.AddContactConstraint(id_r, heel_point, np.array([0., 0., 1.]), "right_heel_z")
#
# constraint_set_both.AddContactConstraint(id_l, heel_point, np.array([1., 0., 0.]), "left_heel_x")
# constraint_set_both.AddContactConstraint(id_l, heel_point, np.array([0., 1., 0.]), "left_heel_y")
# constraint_set_both.AddContactConstraint(id_l, heel_point, np.array([0., 0., 1.]), "left_heel_z")
#
# constraint_set_right.Bind(model)
# constraint_set_left.Bind(model)
# constraint_set_both.Bind(model)
x = []
y = []
return model
def dynamic_model(noise=0.0):
# add in mass and height params
model = rbdl.Model()
bodies = {}
mass = {}
com = {}
inertia = {}
bodies["right"] = {}
bodies["left"] = {}
segments = ["thigh", "shank", "foot"]
mass["hip"] = 45.32 + noise
mass["right_thigh"] = 13.934 + noise
mass["left_thigh"] = 13.934 + noise
mass["right_shank"] = 5.128 + noise
mass["left_shank"] = 5.128 + noise
mass["right_foot"] = 1.898 + noise
mass["left_foot"] = 1.898 + noise
parent_dist = {}
parent_dist["hip"] = np.array([0.0, 0.0, 0.0])
parent_dist["left_thigh"] = np.array([0.237, -0.124, -0.144])
parent_dist["left_shank"] = np.array([0.033, -0.03, -0.436])
parent_dist["left_foot"] = np.array([0.02, -0.027, -0.39])
parent_dist["right_thigh"] = np.array([-0.237, -0.124, -0.144])
parent_dist["right_shank"] = np.array([0.033, -0.03, -0.436])
parent_dist["right_foot"] = np.array([0.02, -0.027, -0.39])
inertia["hip"] = np.diag([5.9172, 6.1912, 1.4393])
inertia["left_thigh"] = np.diag([0.5435, 0.6347, 0.1249])
inertia["left_shank"] = np.diag([0.1365, 0.1843, 0.0562])
inertia["left_foot"] = np.diag([0.0312, 0.0276, 0.0405])
inertia["right_thigh"] = np.diag([0.5435, 0.6347, 0.1249])
inertia["right_shank"] = np.diag([0.1365, 0.1843, 0.0562])
inertia["right_foot"] = np.diag([0.0312, 0.0276, 0.0405])
com["hip"] = np.array([0.00, -0.0785, 0.2803])
com["left_thigh"] = np.array([-0.0833, -0.0198, -0.00678])
com["left_shank"] = np.array([-0.0968, -0.007, 0.06])
com["left_foot"] = np.array([-0.0733, -0.0583, 0.04])
com["right_thigh"] = np.array([-0.0833, -0.0198, -0.00678])
com["right_shank"] = np.array([-0.0968, -0.007, 0.06])
com["right_foot"] = np.array([-0.0733, -0.0583, 0.04])
hip_body = rbdl.Body.fromMassComInertia(mass["hip"], com["hip"],
inertia["hip"])
for segs in segments:
bodies["right_" + segs] = rbdl.Body.fromMassComInertia(
mass["right_" + segs], com["right_" + segs],
inertia["right_" + segs])
bodies["left_" + segs] = rbdl.Body.fromMassComInertia(
mass["left_" + segs], com["left_" + segs], inertia["left_" + segs])
xtrans = rbdl.SpatialTransform()
xtrans.r = np.array([0.0, 0.0, 0.0])
xtrans.E = np.eye(3)
hip = model.AddBody(0, xtrans, rbdl.Joint.fromJointType("JointTypeFixed"),
hip_body, "hip")
joint_rot_z = rbdl.Joint.fromJointType("JointTypeRevoluteX")
xtrans.r = parent_dist["left_thigh"]
left_thigh = model.AddBody(hip, xtrans, joint_rot_z, bodies["left_thigh"],
"left_thigh")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["left_shank"]
left_shank = model.AddBody(left_thigh, xtrans, joint_rot_z,
bodies["left_shank"], "left_shank")
xtrans.r = parent_dist["left_foot"]
left_foot = model.AddBody(left_shank, xtrans, joint_rot_z,
bodies["left_foot"], "left_foot")
xtrans.r = parent_dist["right_thigh"]
right_thigh = model.AddBody(hip, xtrans, joint_rot_z,
bodies["right_thigh"], "right_thigh")
xtrans.E = np.eye(3)
xtrans.r = parent_dist["right_shank"]
right_shank = model.AddBody(right_thigh, xtrans, joint_rot_z,
bodies["right_shank"], "right_shank")
xtrans.r = parent_dist["right_foot"]
right_foot = model.AddBody(right_shank, xtrans, joint_rot_z,
bodies["right_foot"], "right_foot")
model.gravity = np.array([0, 0, -9.81])
return model
