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