def test_create_datas(self): collision_data = self.collision_model.createData() self.assertEqual(len(collision_data.oMg), self.collision_model.ngeoms) data_2, collision_data_2 = pin.createDatas(self.model, self.collision_model) self.assertTrue(self.model.check(data_2)) self.assertEqual(len(collision_data_2.oMg), self.collision_model.ngeoms)
def inverse_kinematics(self, x, y, z): """Calculate the joint values for a desired cartesian position""" model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( "/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf" ) data, collision_data, visual_data = pinocchio.createDatas( model, collision_model, visual_model) JOINT_ID = 5 oMdes = pinocchio.SE3(np.eye(3), np.array([x, y, x])) q = np.array(abel.sort_joints()[5:]) eps = 1e-1 IT_MAX = 1000 DT = 1e-4 damp = 1e-4 i = 0 while True: pinocchio.forwardKinematics(model, data, q) dMi = oMdes.actInv(data.oMi[JOINT_ID]) err = pinocchio.log(dMi).vector if norm(err) < eps: success = True break if i >= IT_MAX: success = False break J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID) v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err)) q = pinocchio.integrate(model, q, v * DT) if not i % 10: print('%d: error = %s' % (i, err.T)) i += 1 if success: print("Convergence achieved!") else: print( "\nWarning: the iterative algorithm has not reached convergence to the desired precision" ) print('\nresult: %s' % q.flatten().tolist()) print('\nfinal error: %s' % err.T) point = JointTrajectoryPoint() point.positions = [ 0, 0, 0, 0, 0, q[0], q[1], q[2], q[3], q[4], q[5], q[6], q[7], q[8], q[9] ] abel.move_all_joints(point) self.direct_kinematics()
def _config_robot(self, urdf_file, package_dir): if self._b_fixed_base: # Fixed based robot self._model, self._collision_model, self._visual_model = pin.buildModelsFromUrdf( urdf_file, package_dir) self._n_floating = 0 else: # Floating based robot self._model, self._collision_model, self._visual_model = pin.buildModelsFromUrdf( urdf_file, package_dir, pin.JointModelFreeFlyer()) self._n_floating = 6 self._data, self._collision_data, self._visual_data = pin.createDatas( self._model, self._collision_model, self._visual_model) self._n_q = self._model.nq self._n_q_dot = self._model.nv self._n_a = self._n_q_dot - self._n_floating passing_idx = 0 for j_id, j_name in enumerate(self._model.names): if j_name == 'root_joint' or j_name == 'universe': passing_idx += 1 else: self._joint_id[j_name] = j_id - passing_idx for f_id, frame in enumerate(self._model.frames): if frame.name == 'root_joint' or frame.name == 'universe': pass else: if f_id % 2 == 0: # Link link_id = int(f_id / 2 - 1) self._link_id[frame.name] = link_id else: # Joint pass assert len(self._joint_id) == self._n_a self._total_mass = sum( [inertia.mass for inertia in self._model.inertias]) self._joint_pos_limit = np.stack( [self._model.lowerPositionLimit, self._model.upperPositionLimit], axis=1)[self._n_floating:self._n_floating + self._n_a, :] self._joint_vel_limit = np.stack( [-self._model.velocityLimit, self._model.velocityLimit], axis=1)[self._n_floating:self._n_floating + self._n_a, :] self._joint_trq_limit = np.stack( [-self._model.effortLimit, self._model.effortLimit], axis=1)[self._n_floating:self._n_floating + self._n_a, :]
def direct_kinematics(self): """ Compute the direct kinematics for left and right hand (end-effector) by using the URDF model """ # Create data required by the algorithms model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( "/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf" ) data, collision_data, visual_data = pinocchio.createDatas( model, collision_model, visual_model) q = np.array(abel.sort_joints()[5:]) pinocchio.forwardKinematics(model, data, q) for name, oMi in zip(self.model.names, data.oMi): print(("{:<24} : {: .2f} {: .2f} {: .2f}".format( name, *oMi.translation.T.flat)))
# This path refers to Pinocchio source code but you can define your own directory here. pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") model_path = join(pinocchio_model_dir, "others/robots") if len(argv) < 2 else argv[1] mesh_dir = model_path urdf_model_path = join(model_path, "ur_description/urdf/ur5_robot.urdf") # Load the urdf model model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( urdf_model_path, mesh_dir) print('model name: ' + model.name) # Create data required by the algorithms data, collision_data, visual_data = pinocchio.createDatas( model, collision_model, visual_model) # Sample a random configuration q = pinocchio.randomConfiguration(model) print('q: %s' % q.T) # Perform the forward kinematics over the kinematic tree pinocchio.forwardKinematics(model, data, q) # Update Geometry models pinocchio.updateGeometryPlacements(model, data, collision_model, collision_data) pinocchio.updateGeometryPlacements(model, data, visual_model, visual_data) # Print out the placement of each joint of the kinematic tree print("\nJoint placements:")