def cb_joint_state(self, data): self.joint_names = sort_data(data.name) q_pred = np.array(sort_data(data.position)) q = [] qd = [] qdd = [] for i, _ in enumerate(self.joint_spaces): self.joint_spaces[i].update_data(q_pred[i]) q.append(self.joint_spaces[i].q) qd.append(self.joint_spaces[i].qd) qdd.append(self.joint_spaces[i].qdd) self.q = np.array(q) self.qd = np.array(qd) self.qdd = np.array(qdd) a = rbdl.CalcBaseToBodyCoordinates(self.model, self.q, 0, np.array([0.6,0.9,1.]), False) rbdl.CalcBodySpatialJacobian(self.model, self.q, 0, self.X, self.J, False) print(a) 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)
# 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))