def display(robot, q): robot.realdisplay(q) pio.updateFramePlacements(robot.model, robot.viz.data) robot.viewer.gui.applyConfiguration( 'world/' + robot.model.frames[-2].name, list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-2]).flat)) robot.viewer.gui.applyConfiguration( 'world/' + robot.model.frames[-1].name, list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-1]).flat)) robot.viewer.gui.refresh()
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 setCameraTransform(self, translation=np.zeros(3), rotation=np.zeros(3), relative=False): # translation : [Px, Py, Pz], # rotation : [Roll, Pitch, Yaw] R_pnc = rpyToMatrix(np.array(rotation)) if Viewer.backend == 'gepetto-gui': H_abs = SE3(R_pnc, np.array(translation)) if relative: H_orig = XYZQUATToSe3( self._client.getCameraTransform(self._window_id)) H_abs = H_abs * H_orig self._client.setCameraTransform(self._window_id, se3ToXYZQUAT(H_abs).tolist()) else: if relative: raise RuntimeError( "'relative'=True not available with meshcat.") import meshcat.transformations as tf # Transformation of the camera object T_meshcat = tf.translation_matrix(translation) self._client.viewer[ "/Cameras/default/rotated/<object>"].set_transform(T_meshcat) # Orientation of the camera object Q_pnc = Quaternion(R_pnc).coeffs() Q_meshcat = np.roll(Q_pnc, shift=1) R_meshcat = tf.quaternion_matrix(Q_meshcat) self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def retrieve_freeflyer(trajectory_data, freeflyer_continuity=True): """ @brief Retrieves the freeflyer positions and velocities. The reference frame is the support foot. @param trajectory_data Sequence of States for which to retrieve the freeflyer. """ robot = trajectory_data['robot'] use_theoretical_model = trajectory_data['use_theoretical_model'] 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, use_theoretical_model) # 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 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 display(self, xs, fs=[], ps=[], dts=[], factor=1.): 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)
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 setCameraTransform(self, translation, rotation): # translation : [Px, Py, Pz], # rotation : [Roll, Pitch, Yaw] R_pnc = rpyToMatrix(np.array(rotation)) if Viewer.backend == 'gepetto-gui': T_pnc = np.array(translation) T_R = SE3(R_pnc, T_pnc) self._client.setCameraTransform(self._window_id, se3ToXYZQUAT(T_R).tolist()) else: import meshcat.transformations as tf # Transformation of the camera object T_meshcat = tf.translation_matrix(translation) self._client.viewer[ "/Cameras/default/rotated/<object>"].set_transform(T_meshcat) # Orientation of the camera object Q_pnc = Quaternion(R_pnc).coeffs() Q_meshcat = np.roll(Q_pnc, shift=1) R_meshcat = tf.quaternion_matrix(Q_meshcat) self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def compute_freeflyer_state_from_fixed_body(robot, position, velocity=None, acceleration=None, fixed_body_name=None, ground_profile=None, use_theoretical_model=True): """ @brief Fill rootjoint data from articular data when a body is fixed parallel to world. @details If 'fixed_body_name' is omitted, it will default to the contact point that ensures no contact points are going through the ground and a single one is touching it. @remark The hypothesis is that 'fixed_body_name' is fixed parallel to world frame. So this method computes the position of freeflyer rootjoint in the fixed body frame. @note This function modifies internal data. @param robot The jiminy robot @param[inout] position Must contain current articular data. The rootjoint data can contain any value, it will be ignored and replaced. The method fills in rootjoint data. @param[inout] velocity See position @param[inout] acceleration See position @param fixed_body_name The name of the body frame that is considered fixed parallel to world frame @param ground_profile The ground profile callback """ if not robot.has_freeflyer: raise ValueError("The robot does not have a freeflyer.") if use_theoretical_model: pnc_model = robot.pinocchio_model_th pnc_data = robot.pinocchio_data_th else: pnc_model = robot.pinocchio_model pnc_data = robot.pinocchio_data position[:6].fill(0.0) position[6] = 1.0 if velocity is not None and acceleration is not None: velocity[:6].fill(0.0) acceleration[:6].fill(0.0) pin.forwardKinematics(pnc_model, pnc_data, position, velocity, acceleration) elif velocity is not None: velocity[:6].fill(0.0) pin.forwardKinematics(pnc_model, pnc_data, position, velocity) else: pin.forwardKinematics(pnc_model, pnc_data, position) pin.framesForwardKinematics(pnc_model, pnc_data, position) if fixed_body_name is None: fixed_body_name = _compute_closest_contact_frame( robot, ground_profile, use_theoretical_model) 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 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
robot = loadTiago(initViewer=True) gv = robot.viewer.gui gv.setCameraTransform(0, [-8, -8, 2, .6, -0.25, -0.25, .7]) NQ = robot.model.nq NV = robot.model.nv IDX_TOOL = robot.model.getFrameId('frametool') IDX_BASIS = robot.model.getFrameId('framebasis') oMgoal = pio.SE3( pio.Quaternion(-0.4, 0.02, -0.5, 0.7).normalized().matrix(), np.matrix([.2, -.4, .7]).T) gv.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, .2) gv.applyConfiguration('world/framegoal', list(pio.se3ToXYZQUAT(oMgoal).flat)) DT = 1e-2 # Integration step. q0 = np.matrix([[ 0., 0., 0., 1., 0.18, 1.37, -0.24, -0.98, 0.98, 0., 0., 0., 0., -0.13, 0., 0., 0., 0. ]]).T q = q0.copy() herr = [] # Log the value of the error between tool and goal. # Loop on an inverse kinematics for 200 iterations. for i in range(200): # Integrate over 2 second of robot life pio.framesForwardKinematics(robot.model, robot.data, q) # Compute frame placements oMtool = robot.data.oMf[ IDX_TOOL] # Placement from world frame o to frame f oMtool
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)