def test_xyzquat_operator(self): # Setup mul = Multiply_poseQuaternion_vector("mul") mul_inv = MultiplyInv_poseQuaternion_vector("mul_inv") # Create a Random SE3 object. xyzq_rand = pinocchio.SE3ToXYZQUAT(pinocchio.SE3.Random()) xyzq_out = pinocchio.SE3ToXYZQUAT(pinocchio.SE3.Random()) # mul.sin1.value = xyzq_rand mul.sin2.value = xyzq_out # mul_inv.sin1.value = xyzq_rand plug(mul.sout, mul_inv.sin2) # mul_inv.sout.recompute(1) out = mul_inv.sout.value # debug print print() print("mul.sin1.value =", mul.sin1.value) print("mul.sin2.value =", mul.sin2.value) print("mul.sout.value =", mul.sout.value) print("mul_inv.sin1.value =", mul_inv.sin1.value) print("mul_inv.sin2.value =", mul_inv.sin2.value) print("mul_inv.sout.value =", mul_inv.sout.value) # Testing np.testing.assert_allclose(out, xyzq_out, rtol=1e-4)
def test_no_update_no_select(self): entity = CreateWorldFrame("") # Random input identity_frame = pinocchio.SE3.Identity() random_frame = pinocchio.SE3.Random() entity.frame_sin.value = pinocchio.SE3ToXYZQUAT(random_frame) # checkout that the output is not changed entity.world_frame_sout.recompute(1) np.testing.assert_almost_equal(entity.world_frame_sout.value, pinocchio.SE3ToXYZQUAT(identity_frame))
def _build_from_mesh(self, model_wrapper, mesh, bounds=None): model = model_wrapper.model geom_model = model_wrapper.geom_model # save geom_obj placement geom_obj = mesh.geom_obj() placement = geom_obj.placement.copy() geom_obj.placement = pin.SE3.Identity() geom_obj.q_placement = placement q_freeflyer = pin.SE3ToXYZQUAT(placement).copy() # create a freeflyer joint and add it to the model free_flyer = pin.JointModelFreeFlyer() universe_joint = 0 joint_name = f"freeflyer_{mesh.name}" if bounds is not None: parent_joint = model.addJoint( universe_joint, free_flyer, pin.SE3.Identity(), joint_name=joint_name, max_effort=np.array([1000]), max_velocity=np.array([1000]), min_config=bounds[0], max_config=bounds[1], ) else: parent_joint = model.addJoint( universe_joint, free_flyer, pin.SE3.Identity(), joint_name ) # add geom_obj to the geom_model geom_obj.parentJoint = parent_joint geom_model.addGeometryObject(geom_obj)
def compute(m): tq_vec = pin.SE3ToXYZQUAT(m) tq_tup = pin.SE3ToXYZQUATtuple(m) mm_vec = pin.XYZQUATToSE3(tq_vec) mm_tup = pin.XYZQUATToSE3(tq_tup) mm_lis = pin.XYZQUATToSE3(list(tq_tup)) return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
def attach_to_link(model, link, gripper=None, handle=None, contact=None): """ Attach a gripper, handle or contact to a different link. - model: a pinocchio.Model that represents the kinematic chain. - link: the link onto which to attach. - gripper, handle, contact: exactly one of them should be provided """ import pinocchio if int(gripper is None) + int(handle is None) + int(contact is None) != 2: raise ValueError( "Exactly one of {gripper, handle, contact} should be provided") srdf = ( contact if handle is None else handle) if gripper is None else gripper oid = model.getFrameId(srdf['link']) nid = model.getFrameId(link) if oid >= model.nframes or nid >= model.nframes: raise ValueError("Could not find one of the frames") if oid == nid: return of = model.frames[oid] nf = model.frames[nid] if of.parent != nf.parent: raise RuntimeError("The frames are not attached to the same joint") srdf['link'] = nf.name if contact is not None: # Change srdf['points'] from old link to new link raise NotImplementedError( "Need to change srdf['points'] from old link to new link") else: olMf = pinocchio.XYZQUATToSE3(srdf["position"]) jMnl = nf.placement jMol = of.placement nlMf = jMnl.inverse() * jMol * olMf srdf["position"] = pinocchio.SE3ToXYZQUAT(nlMf)
def test_double_convertion(self): # Setup # Create a Random SE3 object. pin_se3_pose_rot_mat = pinocchio.SE3.Random() # Convert the rotation in quaternion. init_se3_pose_quat = np.array( pinocchio.SE3ToXYZQUAT(pin_se3_pose_rot_mat)) # Create the small graph. quat2rpy = PoseQuaternionToPoseRPY("") rpy2quat = PoseRPYToPoseQuaternion("") plug(quat2rpy.sout, rpy2quat.sin) # Feed the graph. quat2rpy.sin.value = init_se3_pose_quat # Evaluate the graph. rpy2quat.sout.recompute(1) # Extract the result. final_se3_pose_quat = rpy2quat.sout.value # Make the quaternions w positive for testing: if init_se3_pose_quat[-1] < 0.0: init_se3_pose_quat[-4:] *= -1 if final_se3_pose_quat[-1] < 0.0: final_se3_pose_quat[-4:] *= -1 # Testing np.testing.assert_allclose(init_se3_pose_quat, final_se3_pose_quat, rtol=1e-4)
def test_against_pinocchio(self): """ Test against pinocchio conversion tools """ # Setup # Create a Random SE3 object. pin_se3_pose_rot_mat = pinocchio.SE3.Random() # Convert the rotation in roll-pitch-yaw. pin_rpy = pinocchio.rpy.matrixToRpy(pin_se3_pose_rot_mat.rotation).T pin_se3_pose_rpy = np.array( np.hstack([pin_se3_pose_rot_mat.translation, pin_rpy])) # Convert the rotation in quaternion. pin_se3_pose_quat = pinocchio.SE3ToXYZQUAT(pin_se3_pose_rot_mat) # Create the entity and convert the se3(pose, quat) -> se3(pose, rpy). pq2pr = PoseQuaternionToPoseRPY("") pq2pr.sin.value = np.array(pin_se3_pose_quat) pq2pr.sout.recompute(1) entity_se3_pose_rpy = pq2pr.sout.value # Testing np.testing.assert_allclose(pin_se3_pose_rpy, entity_se3_pose_rpy, rtol=1e-6)
def test_se3ToXYZQUAT_XYZQUATToSe3(self): m = pin.SE3.Identity() m.translation = np.matrix('1. 2. 3.').T m.rotation = np.matrix( '1. 0. 0.;0. 0. -1.;0. 1. 0.') # rotate('x', pi / 2) self.assertApprox( pin.SE3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]) self.assertApprox( pin.XYZQUATToSE3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
def test_use_all(self): entity = CreateWorldFrame("") entity.update() entity.set_which_dofs(np.array(6 * [1])) # Random input random_frame = pinocchio.SE3.Random() entity.frame_sin.value = pinocchio.SE3ToXYZQUAT(random_frame) # checkout that the output is not changed entity.world_frame_sout.recompute(1) np.testing.assert_almost_equal(entity.world_frame_sout.value, entity.frame_sin.value)
def test_full_offset_xyz_null(self): entity = CreateWorldFrame("") entity.update() entity.set_which_dofs(np.array([0, 0, 0, 1, 1, 1])) # Random input random_frame = pinocchio.SE3.Random() entity.frame_sin.value = pinocchio.SE3ToXYZQUAT(random_frame) # checkout that the output is not changed entity.world_frame_sout.recompute(1) np.testing.assert_almost_equal( entity.world_frame_sout.value, 3 * [0] + entity.frame_sin.value[3:].tolist())
def compute_freeflyer(trajectory_data: TrajectoryDataType, freeflyer_continuity: bool = True) -> None: """Compute the freeflyer positions and velocities. .. warning:: This function modifies the internal robot data. :param trajectory_data: Sequence of States for which to retrieve the freeflyer. :param freeflyer_continuity: Whether or not to enforce the continuity in position of the freeflyer. Optional: True by default. """ robot = trajectory_data['robot'] contact_frame_prev = None w_M_ff_offset = pin.SE3.Identity() w_M_ff_prev = None for s in trajectory_data['evolution_robot']: # Compute freeflyer using contact frame as reference frame s.contact_frame = compute_freeflyer_state_from_fixed_body( robot, s.q, s.v, s.a, s.contact_frame, None) # Move freeflyer to ensure continuity over time, if requested if freeflyer_continuity: # Extract the current freeflyer transform w_M_ff = pin.XYZQUATToSE3(s.q[:7]) # Update the internal buffer of the freeflyer transform if contact_frame_prev is not None \ and contact_frame_prev != s.contact_frame: w_M_ff_offset = w_M_ff_offset * w_M_ff_prev * w_M_ff.inverse() contact_frame_prev = s.contact_frame w_M_ff_prev = w_M_ff # Add the appropriate offset to the freeflyer w_M_ff = w_M_ff_offset * w_M_ff s.q[:7] = pin.SE3ToXYZQUAT(w_M_ff)
def writeToMessage(self, t, q, v=None, tau=None, p=dict(), pd=dict(), f=dict(), s=dict()): # Filling the time information self._msg.header.stamp = rospy.Time(t) self._msg.time = t if v is None: v = np.zeros(self._model.nv) if tau is None: tau = np.zeros(self._model.njoints - 2) # Filling the centroidal state pinocchio.centerOfMass(self._model, self._data, q, v) c = self._data.com[0] cd = self._data.vcom[0] # Center of mass self._msg.centroidal.com_position.x = c[0] self._msg.centroidal.com_position.y = c[1] self._msg.centroidal.com_position.z = c[2] self._msg.centroidal.com_velocity.x = cd[0] self._msg.centroidal.com_velocity.y = cd[1] self._msg.centroidal.com_velocity.z = cd[2] # Base self._msg.centroidal.base_orientation.x = q[3] self._msg.centroidal.base_orientation.y = q[4] self._msg.centroidal.base_orientation.z = q[5] self._msg.centroidal.base_orientation.w = q[6] self._msg.centroidal.base_angular_velocity.x = v[3] self._msg.centroidal.base_angular_velocity.y = v[4] self._msg.centroidal.base_angular_velocity.z = v[5] # Momenta momenta = pinocchio.computeCentroidalMomentum(self._model, self._data) momenta_rate = pinocchio.computeCentroidalMomentumTimeVariation( self._model, self._data) self._msg.centroidal.momenta.linear.x = momenta.linear[0] self._msg.centroidal.momenta.linear.y = momenta.linear[1] self._msg.centroidal.momenta.linear.z = momenta.linear[2] self._msg.centroidal.momenta.angular.x = momenta.angular[0] self._msg.centroidal.momenta.angular.y = momenta.angular[1] self._msg.centroidal.momenta.angular.z = momenta.angular[2] self._msg.centroidal.momenta_rate.linear.x = momenta_rate.linear[0] self._msg.centroidal.momenta_rate.linear.y = momenta_rate.linear[1] self._msg.centroidal.momenta_rate.linear.z = momenta_rate.linear[2] self._msg.centroidal.momenta_rate.angular.x = momenta_rate.angular[0] self._msg.centroidal.momenta_rate.angular.y = momenta_rate.angular[1] self._msg.centroidal.momenta_rate.angular.z = momenta_rate.angular[2] # Filling the joint state njoints = self._model.njoints - 2 for j in range(njoints): self._msg.joints[j].position = q[7 + j] self._msg.joints[j].velocity = v[6 + j] self._msg.joints[j].effort = tau[j] # Filling the contact state names = list(p.keys()) + list(pd.keys()) + list(f.keys()) + list( s.keys()) names = list(dict.fromkeys(names)) self._msg.contacts = [None] * len(names) if len(names) != 0 and (len(p.keys()) == 0 or len(pd.keys()) == 0): pinocchio.forwardKinematics(self._model, self._data, q, v) for i, name in enumerate(names): contact_msg = ContactState() contact_msg.name = name frame_id = self._model.getFrameId(name) # Retrive the contact position if name in p: pose = pinocchio.SE3ToXYZQUAT(p[name]) else: oMf = pinocchio.updateFramePlacement(self._model, self._data, frame_id) pose = pinocchio.SE3ToXYZQUAT(oMf) # Retrieve the contact velocity if name in pd: ovf = pd[name] else: ovf = pinocchio.getFrameVelocity( self._model, self._data, frame_id, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED) # Storing the contact position and velocity inside the message contact_msg.pose.position.x = pose[0] contact_msg.pose.position.y = pose[1] contact_msg.pose.position.z = pose[2] contact_msg.pose.orientation.x = pose[3] contact_msg.pose.orientation.y = pose[4] contact_msg.pose.orientation.z = pose[5] contact_msg.pose.orientation.w = pose[6] contact_msg.velocity.linear.x = ovf.linear[0] contact_msg.velocity.linear.y = ovf.linear[1] contact_msg.velocity.linear.z = ovf.linear[2] contact_msg.velocity.angular.x = ovf.angular[0] contact_msg.velocity.angular.y = ovf.angular[1] contact_msg.velocity.angular.z = ovf.angular[2] # Retrieving and storing force data if name in f: contact_info = f[name] ctype = contact_info[0] force = contact_info[1] contact_msg.type = ctype contact_msg.wrench.force.x = force.linear[0] contact_msg.wrench.force.y = force.linear[1] contact_msg.wrench.force.z = force.linear[2] contact_msg.wrench.torque.x = force.angular[0] contact_msg.wrench.torque.y = force.angular[1] contact_msg.wrench.torque.z = force.angular[2] if name in s: terrain_info = s[name] norm = terrain_info[0] friction = terrain_info[1] contact_msg.surface_normal.x = norm[0] contact_msg.surface_normal.y = norm[1] contact_msg.surface_normal.z = norm[2] contact_msg.friction_coefficient = friction self._msg.contacts[i] = contact_msg return copy.deepcopy(self._msg)
def display(self, xs, fs=[], ps=[], dts=[], factor=1.): numpy_conversion = False if libcrocoddyl_pywrap.getNumpyType() == np.matrix: numpy_conversion = True libcrocoddyl_pywrap.switchToNumpyMatrix() warnings.warn( "Numpy matrix supports will be removed in future release", DeprecationWarning, stacklevel=2) if ps: for key, p in ps.items(): self.robot.viewer.gui.setCurvePoints( self.frameTrajGroup + "/" + key, p) if not dts: dts = [0.] * len(xs) S = 1 if self.rate <= 0 else max(len(xs) / self.rate, 1) for i, x in enumerate(xs): if not i % S: if fs: self.activeContacts = { k: False for k, c in self.activeContacts.items() } for f in fs[i]: key = f["key"] pose = f["oMf"] wrench = f["f"] # Display the contact forces R = rotationMatrixFromTwoVectors( self.x_axis, wrench.linear) forcePose = pinocchio.SE3ToXYZQUATtuple( pinocchio.SE3(R, pose.translation)) forceMagnitud = np.linalg.norm( wrench.linear) / self.totalWeight forceName = self.forceGroup + "/" + key self.robot.viewer.gui.setVector3Property( forceName, "Scale", [1. * forceMagnitud, 1., 1.]) self.robot.viewer.gui.applyConfiguration( forceName, forcePose) self.robot.viewer.gui.setVisibility(forceName, "ON") # Display the friction cones position = pose position.rotation = rotationMatrixFromTwoVectors( self.z_axis, f["nsurf"]) frictionName = self.frictionGroup + "/" + key self._setConeMu(key, f["mu"]) self.robot.viewer.gui.applyConfiguration( frictionName, list( np.array(pinocchio.SE3ToXYZQUAT( position)).squeeze())) self.robot.viewer.gui.setVisibility(frictionName, "ON") self.activeContacts[key] = True for key, c in self.activeContacts.items(): if c == False: self.robot.viewer.gui.setVisibility( self.forceGroup + "/" + key, "OFF") self.robot.viewer.gui.setVisibility( self.frictionGroup + "/" + key, "OFF") self.robot.display(x[:self.robot.nq]) time.sleep(dts[i] * factor) if numpy_conversion: numpy_conversion = False libcrocoddyl_pywrap.switchToNumpyMatrix()
def compute_freeflyer_state_from_fixed_body( robot: jiminy.Model, position: np.ndarray, velocity: Optional[np.ndarray] = None, acceleration: Optional[np.ndarray] = None, fixed_body_name: Optional[str] = None, ground_profile: Optional[Callable[ [np.ndarray], Tuple[float, np.ndarray]]] = None, use_theoretical_model: Optional[bool] = None) -> str: """Fill rootjoint data from articular data when a body is fixed and aligned with world frame. This method computes the position of freeflyer in the fixed body frame. If **fixed_body_name** is omitted, it will default to either: - the set of three contact points - a single collision body In such a way that no contact points nor collision bodies are going through the ground and at least contact points or a collision body are touching it. `None` is returned if their is no contact frame or if the robot does not have a freeflyer. .. warning:: This function modifies the internal robot data. .. warning:: The method fills in freeflyer data instead of returning an updated copy for efficiency. :param robot: Jiminy robot. :param position: Must contain current articular data. The freeflyer data can contain any value, it will be ignored and replaced. :param velocity: See position. :param acceleration: See position. :param fixed_body_name: Name of the body frame that is considered fixed parallel to world frame. Optional: It will be infered from the set of contact points and collision bodies. :param ground_profile: Ground profile callback. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching the robot's state. Must be False if `fixed_body_name` is not speficied. Optional: True by default if `fixed_body_name` is specified, False otherwise. :returns: Name of the contact frame, if any. """ # Early return if no freeflyer if not robot.has_freeflyer: return None # Handling of default arguments if use_theoretical_model is None: use_theoretical_model = fixed_body_name is not None position[:6].fill(0.0) position[6] = 1.0 if velocity is not None: velocity[:6].fill(0.0) if acceleration is not None: acceleration[:6].fill(0.0) update_quantities( robot, position, velocity, acceleration, update_physics=False, use_theoretical_model=use_theoretical_model) if fixed_body_name is None: if use_theoretical_model: raise RuntimeError( "Cannot infer contact transform for theoretical model.") w_M_ff = compute_transform_contact(robot, ground_profile) else: ff_M_fixed_body = get_body_world_transform( robot, fixed_body_name, use_theoretical_model, copy=False) if ground_profile is not None: ground_translation = np.zeros(3) ground_translation[2], normal = ground_profile( ff_M_fixed_body.translation) ground_rotation = pin.Quaternion.FromTwoVectors( np.array([0.0, 0.0, 1.0]), normal).matrix() w_M_ground = pin.SE3(ground_rotation, ground_translation) else: w_M_ground = pin.SE3.Identity() w_M_ff = w_M_ground.act(ff_M_fixed_body.inverse()) position[:7] = pin.SE3ToXYZQUAT(w_M_ff) if fixed_body_name is not None: if velocity is not None: ff_v_fixed_body = get_body_world_velocity( robot, fixed_body_name, use_theoretical_model) base_link_velocity = - ff_v_fixed_body velocity[:6] = base_link_velocity.vector if acceleration is not None: ff_a_fixedBody = get_body_world_acceleration( robot, fixed_body_name, use_theoretical_model) base_link_acceleration = - ff_a_fixedBody acceleration[:6] = base_link_acceleration.vector return fixed_body_name
def XYZRPYToXYZQuat(xyzrpy): """Convert [X,Y,Z,Roll,Pitch,Yaw] to [X,Y,Z,Qx,Qy,Qz,Qw]. """ return pin.SE3ToXYZQUAT(XYZRPYToSE3(xyzrpy))
def test_se3ToXYZQUAT_XYZQUATToSe3(self): m = pin.SE3.Identity() m.translation = np.array([1., 2., 3.]) m.rotation = np.array([[1., 0., 0.],[0., 0., -1.],[0., 1., 0.]]) # rotate('x', pi / 2) self.assertApprox(pin.SE3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]) self.assertApprox(pin.XYZQUATToSE3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
gn.maxIter = 100 gn.verbose = False penalty = mno.Penalty(variables.space.nq, variables.space.nv, constraints.dimension()) penalty.etol2 = 1e-12 penalty.fxtol2 = 1e-10 penalty.maxIter = 40 penalty.verbose = True # Compute initial guess X0 = variables.space.neutral for k in range(variables.nplanes): variables.plane(X0, k)[0] = 1 # Assume w is the object base link variables.tag(X0, 0, False)[:] = pinocchio.SE3ToXYZQUAT(wMt0) unknown_tags = set(range(1, variables.ntags)) unknown_cameras = set(range(variables.ncameras)) unhandled_images = images[:] while len(unknown_tags) > 0: if len(unhandled_images) == 0: raise RuntimeError("some tags were not seen") for im, image in enumerate(images): if image not in unhandled_images: continue cMw = None for tag, cMi in zip(image.tags, image.guess_cMis): if tag in unknown_tags: continue # compute camera pose cMw = cMi * variables.tag(X0, tag).inverse() variables.camera(X0, im, False)[:] = pinocchio.SE3ToXYZQUAT(cMw) unhandled_images.remove(image)
gn.verbose = False penalty = mno.Penalty(variables.space.nq, variables.space.nv, constraints.dimension()) penalty.etol2 = 1e-12 penalty.fxtol2 = 1e-10 penalty.maxIter = 40 penalty.verbose = True # Compute initial guess X0 = variables.space.neutral for k in range(variables.nplanes): variables.plane(X0, k)[0] = 1 # Assume w is the object base link variables.tag(X0, 0, False)[:] = pinocchio.SE3ToXYZQUAT(wMt0) unknown_tags = set(range(1, variables.ntags)) unknown_cameras = set(range(variables.ncameras)) unhandled_images = images[:] while len(unknown_tags) > 0: if len(unhandled_images) == 0: raise RuntimeError("some tags were not seen") for im, image in enumerate(images): if image not in unhandled_images: continue cMw = None for tag, cMi in zip(image.tags, image.guess_cMis): if tag in unknown_tags: continue # compute camera pose cMw = cMi * variables.tag(X0, tag).inverse() variables.camera(X0, im, False)[:] = pinocchio.SE3ToXYZQUAT(cMw) unhandled_images.remove(image)