def fk(self):
"""
:param model: model
:type model: rbdl.model
:return: 2D array of the joint locations
"""
x = []
y = []
point_local = np.array([0.0, 0.0, 0.0])
data = rbdl.CalcBodyToBaseCoordinates(self._model, self.q, self.b0,
point_local)
x.append(data[0])
y.append(data[2])
data = rbdl.CalcBodyToBaseCoordinates(self._model, self.q, self.b1,
point_local)
x.append(data[0])
y.append(data[2])
data = rbdl.CalcBodyToBaseCoordinates(self._model, self.q, self.b2,
point_local)
x.append(data[0])
y.append(data[2])
return (x, y)
def test_UpdateKinematicsConsistency(self):
contact_body_id = self.body_1
contact_point = np.array([0., -1., 0.])
self.q[0] = 0.1
self.q[1] = 0.2
self.q[2] = 0.3
self.q[3] = 0.4
self.q[4] = 0.5
self.q[5] = 0.6
rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot)
point1 = rbdl.CalcBodyToBaseCoordinates(self.model, self.q,
contact_body_id, contact_point)
rbdl.UpdateKinematicsCustom(self.model, self.q)
point2 = rbdl.CalcBodyToBaseCoordinates(self.model, self.q,
contact_body_id, contact_point)
self.qdot[0] = 1.1
self.qdot[1] = 1.2
self.qdot[2] = 1.3
self.qdot[3] = -1.4
self.qdot[4] = -1.5
self.qdot[5] = -1.6
rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot)
point_velocity1 = rbdl.CalcPointVelocity(self.model, self.q, self.qdot,
contact_body_id,
contact_point)
rbdl.UpdateKinematicsCustom(self.model, self.q, self.qdot)
point_velocity2 = rbdl.CalcPointVelocity(self.model, self.q, self.qdot,
contact_body_id,
contact_point)
self.qdot[0] = 10.1
self.qdot[1] = 10.2
self.qdot[2] = 10.3
self.qdot[3] = -10.4
self.qdot[4] = -10.5
self.qdot[5] = -10.6
rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot)
point_acceleration1 = rbdl.CalcPointAcceleration(
self.model, self.q, self.qdot, self.qddot, contact_body_id,
contact_point)
rbdl.UpdateKinematicsCustom(self.model, self.q, self.qdot, self.qddot)
point_acceleration2 = rbdl.CalcPointAcceleration(
self.model, self.q, self.qdot, self.qddot, contact_body_id,
contact_point)
assert_almost_equal(point1, point2)
assert_almost_equal(point_velocity1, point_velocity2)
assert_almost_equal(point_acceleration1, point_acceleration2)
def run(data):
q = data
# print(q)
j0_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link1, np.array([0., l1, 0.]))
j1_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link1, np.array([0., 0., 0.]))
j2_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link3, np.array([0., 0., 0.]))
j3_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link3, np.array([0., l3, 0.]))
link1.set_data([j0_p[1], j1_p[1]], [j0_p[2], j1_p[2]])
link2.set_data([j1_p[1], j2_p[1]], [j1_p[2], j2_p[2]])
link3.set_data([j2_p[1], j3_p[1]], [j2_p[2], j3_p[2]])
def run(data):
show_axis = 'yz'
q, fc = data
pA = rbdl.CalcBodyToBaseCoordinates(model, q, id_torso, np.array([0., 0., 0.2]))
pB = rbdl.CalcBodyToBaseCoordinates(model, q, id_torso, np.array([0., 0., -0.2]))
pC = rbdl.CalcBodyToBaseCoordinates(model, q, idl_link, np.array([0., 0., 0.]))
pD = rbdl.CalcBodyToBaseCoordinates(model, q, idl_thigh, np.array([0., 0., -0.5]))
pE = rbdl.CalcBodyToBaseCoordinates(model, q, idl_shank, np.array([0., 0., -0.5]))
pF = rbdl.CalcBodyToBaseCoordinates(model, q, idr_link, np.array([0., 0., 0.]))
pG = rbdl.CalcBodyToBaseCoordinates(model, q, idr_thigh, np.array([0., 0., -0.5]))
pH = rbdl.CalcBodyToBaseCoordinates(model, q, idr_shank, np.array([0., 0., -0.5]))
if show_axis=='yz':
AB.set_data([pA[1], pB[1]], [pA[2], pB[2]])
FC.set_data([pF[1], pC[1]], [pF[2], pC[2]])
CD.set_data([pC[1], pD[1]], [pC[2], pD[2]])
DE.set_data([pD[1], pE[1]], [pD[2], pE[2]])
FG.set_data([pF[1], pG[1]], [pF[2], pG[2]])
GH.set_data([pG[1], pH[1]], [pG[2], pH[2]])
else:
AB.set_data([pA[0], pB[0]], [pA[2], pB[2]])
FC.set_data([pF[0], pC[0]], [pF[2], pC[2]])
CD.set_data([pC[0], pD[0]], [pC[2], pD[2]])
DE.set_data([pD[0], pE[0]], [pD[2], pE[2]])
FG.set_data([pF[0], pG[0]], [pF[2], pG[2]])
GH.set_data([pG[0], pH[0]], [pG[2], pH[2]])
def run(data):
q = data
j0_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link1,
np.array([0., l1, 0.]))
j1_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link1,
np.array([0., 0., 0.]))
j2_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link3,
np.array([0., 0., 0.]))
j3_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link3,
np.array([0., l3, 0.]))
ax.plot([j0_p[1], j1_p[1]], [j0_p[2], j1_p[2]], 'r')
ax.plot([j1_p[1], j2_p[1]], [j1_p[2], j2_p[2]], 'b')
ax.plot([j2_p[1], j3_p[1]], [j2_p[2], j3_p[2]], 'g')
def __init__(self, model, q, qd, body_id, body_point_position,
update_kinematics=True):
self.body_id = body_id
self.body_point_position = body_point_position
self.pos = rbdl.CalcBodyToBaseCoordinates(model, q, body_id,
body_point_position,
update_kinematics)
self.rot = model.X_base[body_id].E.T
self.transform = homogeneous_matrix(rot=self.rot, pos=self.pos)
self.quaternion = tf_transformations.quaternion_from_matrix(
homogeneous_matrix(rot=self.rot)
)
self.rpy = np.array(tf_transformations.euler_from_matrix(
homogeneous_matrix(rot=self.rot), axes='sxyz'))
self.pose = np.concatenate((self.rpy, self.pos))
self.vel = rbdl.CalcPointVelocity6D(model, q, qd, body_id,
body_point_position,
update_kinematics)
self.angular_vel = self.vel[:3]
self.linear_vel = self.vel[-3:]
self.Jdqd = rbdl.CalcPointAcceleration6D(
model,
q, qd, np.zeros(model.dof_count),
body_id, body_point_position,
update_kinematics
)
self.temp = np.zeros((6, model.dof_count + 1)) # this is a bug
rbdl.CalcPointJacobian6D(model, q, body_id, body_point_position,
self.temp, update_kinematics)
self.J = self.temp[:6, :model.dof_count]
def test_InverseKinematics (self):
""" Checks whether inverse kinematics methods are consistent """
q = np.random.rand (self.model.q_size)
q_res = np.random.rand (self.model.q_size)
qCS_res = np.random.rand (self.model.q_size)
target_body_ids = np.array([self.body_3])
body_points = np.array([np.zeros(3)])
body_points[0][2] = 1.0
target_positions = np.array([np.zeros(3)])
target_positions[0][2] = 1.0
target_positions[0][0] = 2.0
ori_matrix = np.array([[0., 0., -1.], [0., 1., 0.], [1., 0., 0.]])
CS = rbdl.InverseKinematicsConstraintSet()
CS_size = CS.AddPointConstraint (target_body_ids[0],
body_points[0],
target_positions[0]
)
CS.AddOrientationConstraint (self.body_1, ori_matrix)
CS.dlambda = 0.9
CS.max_steps = 2000
CS.step_tol = 1.0e-8
rbdl.InverseKinematics ( self.model, q, target_body_ids,
body_points, target_positions, q_res,
CS.step_tol, CS.dlambda, CS.max_steps
)
rbdl.InverseKinematicsCS ( self.model, q, CS, qCS_res )
res_point = rbdl.CalcBodyToBaseCoordinates(self.model,
q_res, self.body_3, body_points[0])
res_point2 = rbdl.CalcBodyToBaseCoordinates(self.model,
qCS_res, self.body_3, body_points[0])
res_ori = rbdl.CalcBodyWorldOrientation (self.model,
qCS_res, self.body_1)
assert_almost_equal (target_positions[0], res_point)
assert_almost_equal (target_positions[0], res_point2)
assert_almost_equal (ori_matrix, res_ori)
def fk(self):
fk = {}
point_local = np.array([0.0, 0.0, 0.0])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.left_thigh, point_local)
fk["left_hip"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.left_shank, point_local)
fk["left_knee"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.left_foot, point_local)
fk["left_ankle"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.right_thigh, point_local)
fk["right_hip"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.right_shank, point_local)
fk["right_knee"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.right_foot, point_local)
fk["right_ankle"] = Point.Point(data[0], data[1], data[2])
q_left = self.get_left_leg().ankle.angle.z
q_right = self.get_right_leg().ankle.angle.z
fk["left_toe"] = Point.Point(0, 0, 0)
fk["left_toe"].x = fk["left_ankle"].x - 0.8 * (
8.0 / 100.0) * self._height * np.cos(q_left)
fk["left_toe"].y = fk["left_ankle"].y - 0.8 * (
8.0 / 100.0) * self._height * np.cos(q_left)
fk["left_toe"].z = fk["left_ankle"].z - 0.05 + 0.8 * (
8.0 / 100.0) * self._height * np.sin(q_left)
fk["left_heel"] = Point.Point(0, 0, 0)
fk["left_heel"].x = fk["left_ankle"].x + 0.2 * (
8.0 / 100.0) * self._height * np.cos(q_left)
fk["left_heel"].y = fk["left_ankle"].y + 0.2 * (
8.0 / 100.0) * self._height * np.cos(q_left)
fk["left_heel"].z = fk["left_ankle"].z - 0.05 + 0.2 * (
8.0 / 100.0) * self._height * np.sin(q_left)
fk["right_toe"] = Point.Point(0, 0, 0)
fk["right_toe"].x = fk["right_ankle"].x - 0.8 * (
8.0 / 100.0) * 1.57 * np.cos(q_right)
fk["right_toe"].y = fk["right_ankle"].y - 0.8 * (
8.0 / 100.0) * 1.57 * np.cos(q_right)
fk["right_toe"].z = fk["right_ankle"].z - 0.05 + 0.8 * (
8.0 / 100.0) * self._height * np.sin(q_right)
fk["right_heel"] = Point.Point(0, 0, 0)
fk["right_heel"].x = fk["right_ankle"].x + 0.2 * (
8.0 / 100.0) * self._height * np.cos(q_right)
fk["right_heel"].y = fk["right_ankle"].y + 0.2 * (
8.0 / 100.0) * self._height * np.cos(q_right)
fk["right_heel"].z = fk["right_ankle"].z - 0.05 + 0.2 * (
8.0 / 100.0) * self._height * np.sin(q_right)
return fk
def cb_joint_state(self, data):
self.joint_names = sort_data(data.name)
self.q = np.array(sort_data(data.position))
rbdl.CalcBodyToBaseCoordinates(self.model, self.q, 0, self.X, False)
rbdl.CalcBodySpatialJacobian(self.model, self.q, 0, self.X, self.J,
False)
rbdl.UpdateKinematics(self.model, self.q, self.qd, self.qdd)
self.calculate_torque()
self.calculate_acceleration()
def test_CoordinateTransformBodyBase(self):
"""
Checks whether CalcBodyToBaseCoordinates and CalcBaseToBodyCoordinates
give the right results.
"""
q = np.random.rand(self.model.q_size)
point_local = np.array([1., 2., 3.])
point_base = rbdl.CalcBodyToBaseCoordinates(self.model, q, self.body_3,
point_local)
point_local_2 = rbdl.CalcBaseToBodyCoordinates(self.model, q,
self.body_3, point_base)
assert_almost_equal(point_local, point_local_2)
def fk(self, body_name, q=None, body_offset=np.zeros(3),
update_kinematics=True, rotation_rep='quat'):
"""
Forward kinematics for a joint configuration.
:param body_name: Name of the body
:param q: Joint configuration. If it is not specified, the current
q value of the model is used.
:param body_offset: Body offset.
:param update_kinematics: Update model with the joint configuration.
:param rotation_rep: Representation used to represent the rotation of
the body: 'quat' or 'rpy'
:return: Pose of the requested body.
"""
if body_name == 'Waist' and not self.floating_base:
body_name = 'ROOT'
if q is None:
q = self.q
body_id = self.model.GetBodyId(body_name)
pos = rbdl.CalcBodyToBaseCoordinates(
self.model,
q,
body_id,
body_offset,
update_kinematics=update_kinematics
)
rot = rbdl.CalcBodyWorldOrientation(
self.model,
q,
body_id,
update_kinematics=update_kinematics
).T
if rotation_rep == 'rpy':
orient = np.array(
tf_transformations.euler_from_matrix(
homogeneous_matrix(rot=rot), axes='sxyz'
)
)
elif rotation_rep == 'quat':
orient = tf_transformations.quaternion_from_matrix(
homogeneous_matrix(rot=rot)
)
else:
raise TypeError("Wrong requested rotation representation: %s" %
rotation_rep)
return np.concatenate((orient, pos))
def get_pose(self, link, wrt_link=None, point=(0., 0., 0.)):
"""
Return the pose of the specified link with respect to the other given link.
Args:
link (int, str): unique link id, or name.
wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world, and
if -1 wrt to the base.
point (np.array[float[3]]): position of the point in link's local frame.
Returns:
np.array[float[7]]: pose (position and quaternion expressed as [x,y,z,w])
"""
link = self.get_link_id(link)
point = np.asarray(point)
position = rbdl.CalcBodyToBaseCoordinates(self.model, self._q, link, point, update_kinematics=False)
orientation = rbdl.CalcBodyWorldOrientation(self.model, self._q, link, update_kinematics=False)
orientation = get_quaternion_from_matrix(orientation)
return np.concatenate((position, orientation))
def fk(model, body_name, q=None, body_offset=np.zeros(3),
update_kinematics=True, rotation_rep='quat'):
"""
:param model:
:param body_name:
:param q:
:param body_offset:
:param update_kinematics:
:param rotation_rep:
:return:
"""
body_name = body_name.encode()
if q is None:
q = np.zeros(model.q_size)
body_id = model.GetBodyId(body_name)
pos = rbdl.CalcBodyToBaseCoordinates(model,
q,
body_id,
body_offset,
update_kinematics=update_kinematics)
rot = rbdl.CalcBodyWorldOrientation(model,
q,
body_id,
update_kinematics=update_kinematics).T
if rotation_rep == 'quat':
orient = tf_transformations.quaternion_from_matrix(
homogeneous_matrix(rot=rot))
elif rotation_rep == 'rpy':
orient = np.array(
tf_transformations.euler_from_matrix(homogeneous_matrix(rot=rot),
axes='sxyz'))
else:
return AttributeError(
"Wrong rotation representation %s. Only 'rpy' and 'quat' available." % rotation_rep)
return np.concatenate((orient, pos))
def __init__(self, urdf_file, floating_base=False):
self.floating_base = floating_base
self.model = rbdl.loadModel(urdf_file, verbose=False,
floating_base=floating_base)
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)
self.q_size = self.q.shape[0]
self.qdot_size = self.qdot.shape[0]
self.qddot_size = self.qddot.shape[0]
# Update model
self.update()
floating_base_body_id = 0
self.fb_origin_offset = rbdl.CalcBodyToBaseCoordinates(
self.model,
self.q * 0,
floating_base_body_id,
np.zeros(3),
False
)
def fwd_kinematics(model, q, body):
size = len(q)
q = q.reshape(size, )
point_local = np.array([0.0, 0.0, 0.0])
end_pos = rbdl.CalcBodyToBaseCoordinates(model, q, body, point_local)
return end_pos
print(xtrans)
# 三杆模型，第一个body，绕ry转动
body_1 = model.AppendBody(rbdl.SpatialTransform(), joint_rot_y, body)
body_2 = model.AppendBody(xtrans, joint_rot_y, body)
body_3 = model.AppendBody(xtrans, joint_rot_y, body)
# 获取模型中变量
q = np.zeros(model.q_size)
qdot = np.zeros(model.qdot_size)
qddot = np.zeros(model.qdot_size)
tau = np.zeros(model.qdot_size)
# 设置当前角度
q[0] = 1.3
q[1] = -0.5
q[2] = 3.2
# 计算运动学关系
point_local = np.array([1., 2., 3.]) # 一个局部坐标值
point_base = rbdl.CalcBodyToBaseCoordinates(
model, q, body_3, point_local) # 在body3的局部坐标系下的的点在全局中的坐标
point_local_2 = rbdl.CalcBaseToBodyCoordinates(model, q, body_3,
point_base) # 反过来
# 前向动力学
# 这个破模型当然计算得到的加速度全都是零了，真是傻了
rbdl.ForwardDynamics(model, q, qdot, tau, qddot)
print("qddot = " + str(qddot.transpose()))
G = np.zeros([3, model.qdot_size])
rbdl.CalcPointJacobian(model, q, body_3, point_local, G)
print("G = \n" + str(G))
def get_end_effector_pos(q):
point_local = np.array([0.0, 0.0, 0.0])
end_pos = rbdl.CalcBodyToBaseCoordinates(model, q, body_7, point_local)
return end_pos
# Construct the model
body_1 = model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, body)
body_2 = model.AppendBody (xtrans, joint_rot_y, body)
body_3 = model.AppendBody (xtrans, joint_rot_y, body)
# Create numpy arrays for the state
q = np.zeros (model.q_size)
qdot = np.zeros (model.qdot_size)
qddot = np.zeros (model.qdot_size)
tau = np.zeros (model.qdot_size)
# Modify the state
q[0] = 1.3
q[1] = -0.5
q[2] = 3.2
# Transform coordinates from local to global coordinates
point_local = np.array([1., 2., 3.])
point_base = rbdl.CalcBodyToBaseCoordinates (model, q, body_3, point_local)
point_local_2 = rbdl.CalcBaseToBodyCoordinates (model, q, body_3, point_base)
# Perform forward dynamics and print the result
rbdl.ForwardDynamics (model, q, qdot, tau, qddot)
print ("qddot = " + str(qddot.transpose()))
# Compute and print the jacobian (note: the output parameter
# of the Jacobian G must have proper size!)
G = np.zeros([3,model.qdot_size])
rbdl.CalcPointJacobian (model, q, body_3, point_local, G)
print ("G = \n" + str(G))
def jointStateCallback(data):
#global variables shared between functions
global pose_des
global vel_des
global q
global q_old
global qd
J = np.zeros((6, 7))
h = 0.01 #dt
K = (2 / h) * np.eye(6) #Stability for < 2/h
EE_LOCAL_POS = np.array((0.0, 0.0, 0.045)) #Local offset
k = 0.001 #Damped least square method
I = np.eye(6)
## JointState msg
## q = [gripper_left_joint, gripper_right_joint, iiwa_joint_1, iiwa_joint_2, iiwa_joint_3, iiwa_joint_4, iiwa_joint_5, iiwa_joint_6, iiwa_joint_7]
q = data.position[2:]
q = np.asarray(q)
q_old = q
qd = data.velocity[2:]
qd = np.asarray(qd)
#End-Effector Position
x = rbdl.CalcBodyToBaseCoordinates(model, q, 7, EE_LOCAL_POS, True) #x,y,z
#Orientation
R = rbdl.CalcBodyWorldOrientation(model, q, 7, True).T
#From rotation matrix to quaternion
quat = quaternion.from_rotation_matrix(R)
#Input as euler angles for orientation
quat_des = quaternion.from_euler_angles(pose_des[0], pose_des[1],
pose_des[2])
#Angular error
err_ang = quat_des * quat.conjugate()
err_ang = quaternion.as_float_array(err_ang)
#Linear error
err_lin = np.array(
([pose_des[3] - x[0], pose_des[4] - x[1], pose_des[5] - x[2]]))
#Total Error
err = np.array(([
err_ang[1], err_ang[2], err_ang[3], err_lin[0], err_lin[1], err_lin[2]
]))
# Jacobian (angular-linear)
rbdl.CalcPointJacobian6D(model, q, 7, EE_LOCAL_POS, J)
# Jacobian Inverse (Damped)
J_inv = J.T.dot(inv(J.dot(J.transpose()) + (k**2) * I))
#CLIK: Closed Loop Inverse Kinematics
q = q_old + h * J_inv.dot(vel_des + K.dot(err))
qd = J_inv.dot(vel_des + K.dot(err))
#rqt_plot for error
pub_error = rospy.Publisher('/iiwa/iiwa_position_controller/error',
Float64,
queue_size=10)
pub_error.publish(norm(err))
#POSITION CONTROLLER
pub = rospy.Publisher('/iiwa/iiwa_position_controller/command',
JointTrajectory,
queue_size=10)
#JointTrajectory msg generation
joints_str = JointTrajectory()
joints_str.header = Header()
joints_str.header.stamp = rospy.Time.now()
joints_str.joint_names = [
'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4',
'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'
]
#I'll use a single waypoint with the q[] values calculated with CLIK
point = JointTrajectoryPoint()
point.positions = [q[0], q[1], q[2], q[3], q[4], q[5], q[6]]
#point.velocities = [ qd[0], qd[1], qd[2], qd[3], qd[4], qd[5], qd[6] ]
point.time_from_start = rospy.Duration(1)
joints_str.points.append(point)
pub.publish(joints_str)
#Gripper Controller
pub_left = rospy.Publisher(
'/iiwa/gripper_left_position_controller/command',
Float64,
queue_size=10)
pub_right = rospy.Publisher(
'/iiwa/gripper_right_position_controller/command',
Float64,
queue_size=10)
#Open Position
pub_left.publish(0)
pub_right.publish(0)
rate.sleep()
q_old = q
cs.Bind(model)
q = np.zeros(model.q_size)
qdot = np.zeros(model.qdot_size)
qddot = np.zeros(model.qdot_size)
tau = np.zeros(model.qdot_size)
print("shape of q:", np.shape(q))
print("Size of q:", np.size(q))
# Testing the 4-bar linkage in the 0 degree angle for all the joints
q[0] = 0.
q[1] = 0.
q[2] = 0.
q[3] = 0.
q[4] = 0.
pos1 = rbdl.CalcBodyToBaseCoordinates(model, q, idB2, Xp.r)
pos2 = rbdl.CalcBodyToBaseCoordinates(model, q, idB5, Xs.r)
print 'Point Location wrt base: ', pos1
print 'Point Location wrt base: ', pos2
# Testing the 4-bar linkage in the with some non-zero angle
q[0] = np.pi
q[1] = -np.pi
q[2] = np.pi - q[0]
q[3] = -q[1]
q[4] = 0.
pos1 = rbdl.CalcBodyToBaseCoordinates(model, q, idB2, Xp.r)
pos2 = rbdl.CalcBodyToBaseCoordinates(model, q, idB5, Xs.r)
print 'Point Location wrt base: ', pos1
print 'Point Location wrt base: ', pos2
# Testing the 4-bar linkage in the with some non-zero angle
q_val = np.pi / 4
# print 'body', body_1
# print 'Inertia', body.mInertia
# print 'trans', trans
q = np.zeros(model.q_size)
qdot = np.zeros(model.qdot_size)
qddot = np.zeros(model.qdot_size)
tau = np.zeros(model.qdot_size)
print(np.shape(q))
print(np.size(q))
q[0] = 0 #-105*3.14/180# 3.14
#
# Giving an arbitrary location described in the local frame and printing it's
# location wrt the world frame
COM_L1 = np.array([0.0, 0.0, 0.0])
COM_L1_base = rbdl.CalcBodyToBaseCoordinates(model, q, body_1, COM_L1)
print 'COM_L1_base: ', COM_L1_base
# Giving an arbitrary location described in the local frame and printing it's
# location wrt the world frame
COM_L3 = np.array([0.0, 0.0, 0.0])
COM_L3_base = rbdl.CalcBodyToBaseCoordinates(model, q, body_3, COM_L3)
print 'COM_L3_base: ', COM_L3_base
rbdl.InverseDynamics(model, q, qdot, qddot, tau)
print 'G: ', tau
def get_G(q_):
q_ = np.asarray(q_)
def draw_model(self, bbmdl, q):
mdl = bbmdl.model
gl.glColor3f(0,1,1)
pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.roller, self.zero)
draw_line(pt0[0:2], (0,0))
gl.glColor3f(0,1,0)
pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.roller, self.zero, update_kinematics = False)
pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.roller, self.unit_y, update_kinematics = False)
draw_line(pt0[0:2], pt1[0:2])
gl.glColor3f(0,0,1)
pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.roller, self.zero, update_kinematics = False)
pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, self.zero, update_kinematics = False)
draw_line(pt0[0:2], pt1[0:2])
gl.glColor3f(1,.5,0)
pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, np.array([-self.l_board/2,self.r_roller,0]), update_kinematics = False)
pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, np.array([self.l_board/2,self.r_roller,0]), update_kinematics = False)
draw_line(pt0[0:2], pt1[0:2])
gl.glColor3f(1,0,0)
pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, self.zero, update_kinematics = False)
pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, self.unit_y, update_kinematics = False)
draw_line(pt0[0:2], pt1[0:2])
pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, self.zero, update_kinematics = False)
pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body, self.zero, update_kinematics = False)
draw_line(pt0[0:2], pt1[0:2])
gl.glColor3f(0,1,0)
pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body, self.zero, update_kinematics = False)
pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body, np.asarray([0,.1,0]), update_kinematics = False)
draw_line(pt0[0:2], pt1[0:2])
try:
gl.glColor3f(0,0,1)
pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body2, self.zero, update_kinematics = False)
pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body2, np.asarray([0,-self.h_body + self.r_roller + .05,0]), update_kinematics = False)
draw_line(pt0[0:2], pt1[0:2])
except AttributeError:
pass
