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 test_CalcPointJacobian (self):
""" Computes point Jacobian and checks whether G * qdot is consistent
with CalcPointVelocity. """
q = np.zeros (self.model.q_size)
G = np.zeros ([3, self.model.q_size])
point_coords = np.array ([0., 0., 1.])
rbdl.CalcPointJacobian (
self.model,
q,
self.body_3,
point_coords,
G
)
qdot = np.ones(self.model.qdot_size)
point_vel = rbdl.CalcPointVelocity (
self.model,
q,
qdot,
self.body_3,
point_coords
)
jac_point_vel = np.dot (G, qdot)
assert_almost_equal (jac_point_vel, point_vel)
def get_com_jacobian(self, full=False):
"""
Get the CoM Jacobian.
Args:
full (bool): if True, it will return the jacobian as the concatenation of the angular and linear Jacobian.
Otherwise, it will just return the linear Jacobian.
Returns:
if full:
np.array[float[6,N]]: CoM Jacobian (concatenation of the angular and linear Jacobian, where N is the
number of DoFs)
else:
np.array[float[3,N]]: CoM Jacobian (only the linear part)
"""
jacobian = np.zeros((6, self.num_dofs)) if full else np.zeros((3, self.num_dofs))
mass = 0
for body_id in range(len(self.model.mBodies)):
body = self.model.mBodies[body_id]
jac = np.zeros(3, self.num_dofs)
rbdl.CalcPointJacobian(self.model, self._q, body_id, body.mCenterOfMass, jac, update_kinematics=False)
if full:
jacobian[3:] += body.mMass * jac
else:
jacobian += body.mMass * jac
mass += body.mMass
return jacobian / mass
def damped_least_squares_ik(q, x, xd, v, damping, dt, kp=1, kd=None):
err = position_pd(x, xd, v, kp=kp, kd=kd)
body_point = np.zeros(3)
rbdl.CalcPointJacobian(model,
q,
rbdl_link_id,
body_point,
J,
update_kinematics=True)
J_dagger = J.T.dot(
np.linalg.inv(J.dot(J.T) + damping * np.identity(3)))
dq = J_dagger.dot(err)
q = q + dq * dt
return q
def calculate_torque(self):
force1 = np.array([2.0, 2.0, 2.0])
force2 = np.array([3.0, 4.0, -2.0])
force3 = np.array([-1.0, 3.0, 5.0])
force4 = np.array([12.0, 22.0, 22.0])
forces = [force1, force2, force3, force4]
point1 = np.array([0.000, -0.00, -0.8])
point2 = np.array([0.000, -0.00, -0.8])
points = [point1, point2]
bodies = [self.top, self.bottom]
topJ = np.zeros((3, 2))
bottomJ = np.zeros((3, 2))
J = [topJ, bottomJ]
print(self.q)
for i, body in enumerate(bodies):
for force in forces:
rbdl.CalcPointJacobian(self.rbdl_model, np.array([0.0, 0.0]),
body, points[i], J[i])
J_t = np.transpose(J[i])
# print(J[i], '\n', J_t)
print(np.dot(J_t, np.transpose(force)))
# 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 get_end_effector_jacobian(q):
J = np.zeros([3, model.qdot_size])
point_local = np.array([0.0, 0.0, 0.0])
rbdl.CalcPointJacobian(model, q, body_7, point_local, J)
# rbdl.CalcBodySpatialJacobian(model, q, body_7, J, True)
return J
def Jacobian(model, q, body):
point_coords = np.array([0., 0., 0.])
J = np.zeros((3, 6))
q = q.reshape(6, )
rbdl.CalcPointJacobian(model, q, body, point_coords, J)
return J
