def callback_robotState(data): global cpt_1 cpt_1 += 1 if (cpt_1 % DOWNSAMPLING): return #read vector q from ros q = np.array(data.data) #construct qUrdf from sot convention and eventualy external freeflyer qUrdf = np.zeros(37) if READ_FREEFLYER_FROM_AA_SIGNAL: global ff_xzyquat qUrdf[:7] = ff_xzyquat else: qUrdf[:3] = q[:3] quatMat = se3.utils.rpyToMatrix(np.matrix(q[3:6]).T) quatVec = Quaternion(quatMat) qUrdf[3:7] = np.array(quatVec.coeffs()).squeeze() qUrdf[7:11] = q[18:22] # chest-head qUrdf[11:18] = q[29:] # larm qUrdf[18:25] = q[22:29] # rarm qUrdf[25:31] = q[12:18] # lleg qUrdf[31:] = q[6:12] # rleg global q_glob q_glob = qUrdf #update global configuration vector #~ embed() #~ tau = se3.rnea(v.robot.model,v.robot.data,qUrdf,np.matlib.zeros(37),np.matlib.zeros(37)) #~ print tau[-4] v.updateRobotConfig(q_glob)
def compute_freeflyer_state_from_fixed_body(jiminy_model, fixed_body_name, position, velocity=None, acceleration=None, use_theoretical_model=True): """ @brief Fill rootjoint data from articular data when a body is fixed parallel to world. @details 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 model The Jiminy model @param fixed_body_name The name of the body that is considered fixed parallel to world frame @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 Same as positionInOut but for velocity @pre Rootjoint must be a freeflyer @pre positionInOut.size() == model_->nq """ if use_theoretical_model: pnc_model = jiminy_model.pinocchio_model_th pnc_data = jiminy_model.pinocchio_data_th else: pnc_model = jiminy_model.pinocchio_model pnc_data = jiminy_model.pinocchio_data position[:6].fill(0) position[6] = 1.0 if velocity is not None: velocity[:6].fill(0) else: velocity = np.zeros((pnc_model.nv, )) if acceleration is not None: acceleration[:6].fill(0) else: acceleration = np.zeros((pnc_model.nv, )) pnc.forwardKinematics(pnc_model, pnc_data, position, velocity, acceleration) pnc.framesForwardKinematics(pnc_model, pnc_data, position) ff_M_fixed_body = get_body_world_transform(jiminy_model, fixed_body_name) w_M_ff = ff_M_fixed_body.inverse() base_link_translation = w_M_ff.translation base_link_quaternion = Quaternion(w_M_ff.rotation) position[:3] = base_link_translation position[3:7] = base_link_quaternion.coeffs() ff_v_fixed_body = get_body_world_velocity(jiminy_model, fixed_body_name) base_link_velocity = -ff_v_fixed_body velocity[:6] = base_link_velocity.vector ff_a_fixedBody = get_body_world_acceleration(jiminy_model, fixed_body_name) base_link_acceleration = -ff_a_fixedBody acceleration[:6] = base_link_acceleration.vector
def SE3ToViewerConfig(placement): q = [0] * 7 q[0:3] = placement.translation.T.tolist()[0] r = Quaternion(placement.rotation) q[6] = r.w q[3:6] = r.coeffs().transpose().tolist()[0][0:3] return q
def parseLine(line, i): # check that line starts with joint_states if line [0] != 'joint_states': raise RuntimeError('line {} does not start by keyword "joint_states"' .format(i)) # look for keyword ilg = irg = None # make sure that one and only one of 'left_gripper' and 'right_gripper' # is specified in the current line. try: ilg = line.index('left_gripper') except ValueError as exc: pass try: irg = line.index('right_gripper') except ValueError as exc: pass if irg is None and ilg is None: raise SyntaxError \ ('line {} contains neither "left_gripper" nor "right_gripper" tag.' .format(i+1)) if not irg is None and not ilg is None: raise SyntaxError \ ('line {} contains both "left_gripper" and "right_gripper" tags.' .format(i+1)) measurement = dict() if ilg: try: measurement['joint_states'] = map(float, line [1:ilg]) except ValueError as exc: raise SyntaxError\ ('line {}, tag "joint_states": could not convert list {} to array'. format(i+1, line [1:ilg])) try: v = map(float, line [ilg+1:]) p = Quaternion(x=v[3], y=v[4], z=v[5], w=v[6]) t = np.array(v[0:3]).reshape(3,1) measurement ['left_gripper'] = SE3(p,t) except ValueError as exc: raise SyntaxError\ ('line {}, tag "left_gripper": could not convert list {} to array'. format(i+1, line [ilg+1:])) if irg: try: measurement ['joint_states'] = map(float, line [1:irg]) except ValueError as exc: raise SyntaxError\ ('line {}, tag "joint_states": could not convert list {} to float'. format(i+1, line [1:irg])) try: v = map(float, line [irg+1:]) p = Quaternion(x=v[3], y=v[4], z=v[5], w=v[6]) t = np.array(v[0:3]).reshape(3,1) measurement ['right_gripper'] = SE3(p,t) except ValueError as exc: raise SyntaxError\ ('line {}, tag "right_gripper": could not convert list {} to float'. format(i+1, line [irg+1:])) return measurement
def SE3FromConfig(q): if isinstance(q, types.ListType): q = np.matrix(q).T placement = SE3.Identity() placement.translation = q[0:3] r = Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]) placement.rotation = r.matrix() return placement
def config_sot_to_urdf(q): qUrdf = mat_zeros(39) qUrdf[:3, 0] = q[:3, 0] quatMat = rpyToMatrix(q[3:6, 0]) quatVec = Quaternion(quatMat) qUrdf[3:7, 0] = quatVec.coeffs() qUrdf[7:, 0] = q[6:, 0] return qUrdf
def SE3FromConfig(q): if isinstance(q, list): q = np.array(q) placement = SE3.Identity() tr = np.array(q[0:3]) placement.translation = tr r = Quaternion(q[6], q[3], q[4], q[5]) placement.rotation = r.matrix() return placement
def compute_orientation_for_feet_placement(fb, pb, pId, moving, RF, prev_contactPhase, use_interpolated_orientation): """ Compute the rotation of the feet from the base orientation. :param fb: an instance of rbprm.Fullbody :param pb: the SL1M problem dictionary, containing all the phaseData :param pId: the Id of the current phase (SL1M index) :param moving: the Id of the moving feet :param RF: the Id of the right feet in the SL1M solver :param prev_contactPhase: the multicontact_api.ContactPhase of the previous phase :param use_interpolated_orientation: If False, the desired contact rotation is the current base orientation. If True, the desired contact rotation is the interpolation between the current base orientation and the one for the next phase :return: the rotation matrix of the new contact placement """ quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"]) if pId < len(pb["phaseData"]) - 1: quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"]) else: quat1 = Quaternion(pb["phaseData"][pId]["rootOrientation"]) if use_interpolated_orientation: rot = quat0.slerp(0.5, quat1) # check if feets do not cross : if moving == RF: qr = rot ql = Quaternion(prev_contactPhase.contactPatch(fb.lfoot).placement.rotation) else: ql = rot qr = Quaternion(prev_contactPhase.contactPatch(fb.rfoot).placement.rotation) rpy = matrixToRpy((qr * (ql.inverse())).matrix()) # rotation from the left foot pose to the right one if rpy[2] > 0: # yaw positive, feet are crossing rot = quat0 # rotation of the root, from the guide else: rot = quat0 # rotation of the root, from the guide return rot
def __call__(self, T, nu): """ Integrate se3 velocity from SE3 element T - T: instance of SE3 - nu: numpy array of dimension 6 (v,omega) """ q0 = np.array(7*[0.]) q0[0:3] = T.translation q0[3:7] = Quaternion(T.rotation).coeffs() q = integrate(self.model,q0,nu) return SE3(Quaternion(x=q[3], y=q[4], z=q[5], w=q[6]), q[0:3])
def SE3ToViewerConfig(placement): """ Convert a pinocchio.SE3 object to a python list of lenght 7 : translation + quaternion (x, y, z, w) :param placement: the pinocchio.SE3 object :return: a list of lenght 7 """ q = [0] * 7 q[0:3] = placement.translation.tolist() r = Quaternion(placement.rotation) q[6] = r.w q[3:6] = r.coeffs().tolist()[0:3] return q
def config_sot_to_urdf(q): # GEPETTO VIEWER Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36 # ROBOT VIEWER # Free flyer0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35 qUrdf = mat_zeros(37); qUrdf[:3,0] = q[:3,0]; quatMat = rpyToMatrix(q[3:6,0]); quatVec = Quaternion(quatMat); qUrdf[3:7,0] = quatVec.coeffs(); qUrdf[7:11,0] = q[18:22,0]; # chest-head qUrdf[11:18,0] = q[29:,0]; # larm qUrdf[18:25,0] = q[22:29,0]; # rarm qUrdf[25:31,0] = q[12:18,0]; # lleg qUrdf[31:,0] = q[6:12,0]; # rleg return qUrdf;
def config_sot_to_urdf(q): # GEPETTO VIEWER Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36 # ROBOT VIEWER # Free flyer0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35 qUrdf = mat_zeros(37); qUrdf[:3,0] = q[:3,0]; quatMat = rpyToMatrix(q[3:6,0]); quatVec = Quaternion(quatMat); qUrdf[3:7,0] = quatVec.coeffs(); qUrdf[7:11,0] = q[18:22,0]; # chest-head qUrdf[11:18,0] = q[29:,0]; # larm qUrdf[18:25,0] = q[22:29,0]; # rarm qUrdf[25:31,0] = q[12:18,0]; # lleg qUrdf[31:,0] = q[6:12,0]; # rleg return qUrdf;
def SE3FromConfig(q): """ Convert a vector of size >=7 to a pinocchio.SE3 object. Assume that the first 3 values of the vector are the translation part, followed by a quaternion(x,y,z,w) :param q: a list or a numpy array of size >=7 :return: a SE3 object """ if isinstance(q, list): q = np.array(q) placement = SE3.Identity() tr = np.array(q[0:3]) placement.translation = tr r = Quaternion(q[6], q[3], q[4], q[5]) placement.rotation = r.matrix() return placement
def refresh_observation(self) -> None: if not self.simulator.is_simulation_running: # Initialize observation chunks self.obs_chunks = [ self.system_state.q[2:], self.system_state.v, *[f.vector for f in self.system_state.f_external] ] # Initialize observation chunks sizes self.obs_chunks_sizes = [] idx_start = 0 for obs in self.obs_chunks: idx_end = idx_start + len(obs) self.obs_chunks_sizes.append([idx_start, idx_end]) idx_start = idx_end # Initialize previous torso position self.xpos_prev = self.system_state.q[0] # Update observation buffer for obs, size in zip(self.obs_chunks, self.obs_chunks_sizes): obs_idx = slice(*size) low = self.observation_space.low[obs_idx] high = self.observation_space.high[obs_idx] self._observation[obs_idx] = np.clip(obs, low, high) # Transform observed linear velocity to be in world frame self._observation[slice(*self.obs_chunks_sizes[1])][:3] = \ Quaternion(self.system_state.q[3:7]) * self.obs_chunks[1][:3]
class TrajectorySE3LinearInterp(RefTrajectory): def __init__(self, placement_init, placement_final, time_interval): RefTrajectory.__init__(self, "TrajectorySE3LinearInterp") self.placement_init = placement_init self.placement_final = placement_final self.t0 = time_interval[0] self.t1 = time_interval[1] self.length = self.t1 - self.t0 self.quat0 = Quaternion(self.placement_init.rotation) self.quat1 = Quaternion(self.placement_final.rotation) self.M = SE3.Identity() self.v = Motion.Zero() self.a = Motion.Zero() # constant velocity and null acceleration : self.v.linear = (placement_final.translation - placement_final.translation) / self.length self.v.angular = pin.log3(placement_final.rotation.T * placement_final.rotation) / self.length def __call__(self, t): return self.compute_for_normalized_time(t - self.t0) def compute_for_normalized_time(self, t): if t < 0: print "Trajectory called with negative time." return self.compute_for_normalized_time(0) elif t > self.length: print "Trajectory called after final time." return self.compute_for_normalized_time(self.t_total) u = t / self.length self.M = SE3.Identity() self.M.translation = u * self.placement_final.translation + ( 1. - u) * self.placement_init.translation self.M.rotation = (self.quat0.slerp(u, self.quat1)).matrix() return self.M, self.v, self.a
def set_transform(origin, a, b): from pinocchio import rpy, Quaternion length = np.linalg.norm(b - a) z = (b - a) / length R = Quaternion.FromTwoVectors(np.array([0, 0, 1]), z).matrix() origin.attrib['xyz'] = " ".join([str(v) for v in ((a + b) / 2)]) origin.attrib['rpy'] = " ".join([str(v) for v in rpy.matrixToRpy(R)])
def set_random_configs(self): """ randomly sample initial and goal configuration : """ self.q_init[:3] = [0, 0, 0.465] self.q_init[3:7] = [0, 0, 0, 1] self.q_init[-6] = self.v_init random.seed() alpha = random.uniform(0., 2. * np.pi) #alpha = 4. print("Test on a circle, alpha = ", alpha) self.q_goal = self.q_init[::] self.q_goal[:3] = [ self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465 ] # set final orientation to be along the circle : vx = np.matrix([1, 0, 0]).T v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T quat = Quaternion.FromTwoVectors(vx, v_goal) self.q_goal[3:7] = quat.coeffs().tolist() # set final velocity to fix the orientation : self.q_goal[-6] = self.v_goal * np.sin(alpha) self.q_goal[-5] = -self.v_goal * np.cos(alpha) self.v(self.q_init) print("initial root position : ", self.q_init) print("final root position : ", self.q_goal) self.ps.setInitialConfig(self.q_init) self.ps.addGoalConfig(self.q_goal) self.alpha = alpha # write problem in files : with open(self.status_filename, "w") as f: f.write("q_init= " + str(self.q_init) + "\n") f.write("q_goal= " + str(self.q_goal) + "\n")
def createPhaseFromRBPRMState(fb, stateId, t_init=-1): """ Create a multicontact_api ContactPhase object from an rbprm state :param fb: an instance of rbprm.FullBody :param stateId: the Id of the state :param t_init: the initial time of the contact phase :return: a multicontact_api.ContactPhase with the same contacts as the rbprm state """ q = fb.getConfigAtState(stateId) limbs_contact = fb.getAllLimbsInContact(stateId) cp = createPhaseFromConfig(fb, q, limbs_contact, t_init) if fb.cType == "_3_DOF": # update the contact normal from the data in fullbody for limbId in limbs_contact: eeName = fb.dict_limb_joint[limbId] [p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb( stateId, False, limbId) normal = np.array(n) print("New normal : ", normal) quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal) placement = cp.contactPatch(eeName).placement placement.rotation = quat.matrix() cp.contactPatch(eeName).placement = placement print("New placement : ", normal) return cp
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 __init__(self, placement_init, placement_final, time_interval): RefTrajectory.__init__(self, "TrajectorySE3LinearInterp") self.placement_init = placement_init self.placement_final = placement_final self.t0 = time_interval[0] self.t1 = time_interval[1] self.length = self.t1 - self.t0 self.quat0 = Quaternion(self.placement_init.rotation) self.quat1 = Quaternion(self.placement_final.rotation) self.M = SE3.Identity() self.v = Motion.Zero() self.a = Motion.Zero() # constant velocity and null acceleration : self.v.linear = (placement_final.translation - placement_final.translation) / self.length self.v.angular = pin.log3(placement_final.rotation.T * placement_final.rotation) / self.length
def computePoseFromSurface(surface): points = surface #normal = surface[1] normal = [0,0,1] center = np.zeros(3) for pt in points: center += np.array(pt) center /= len(points) center -= np.array(normal)*(WIDTH/2.) # rotation in rpy : q_rot = Quaternion() n_m = np.matrix(normal).T q_rot.setFromTwoVectors(Z_AXIS,n_m) rpy = matrixToRpy(q_rot.matrix()) pose = center.tolist() + rpy.T.tolist()[0] return pose
def quatConfigFromMatrix(m): """ Transform a rotation matrix to a list containing the quaternion representation (x, y, z, w) :param m: a rotation matrix :return: The Quaternion stored as a list """ quat = Quaternion(m) return quatToConfig(quat)
def transQuatToSE3(p): from pinocchio import SE3, Quaternion from numpy import matrix if len(p) != 7: raise ValueError("Cannot convert {} to SE3".format(p)) return SE3( Quaternion(p[6], p[3], p[4], p[5]).matrix(), matrix(p[0:3]).transpose())
def config_sot_to_urdf(q_sot): q_sot = np.array(q_sot) qUrdf = np.zeros(37) qUrdf[:3] = q_sot[:3] quatMat = se3.utils.rpyToMatrix(np.matrix(q_sot[3:6]).T) quatVec = Quaternion(quatMat) qUrdf[3:7] = np.array(quatVec.coeffs()).squeeze() qUrdf[7:11] = q_sot[18:22] # chest-head qUrdf[11:18] = q_sot[29:36] # larm qUrdf[18:25] = q_sot[22:29] # rarm qUrdf[25:31] = q_sot[12:18] # lleg qUrdf[31:37] = q_sot[6:12] # rleg return qUrdf
def rotationFromNormal(n): """ return a rotation matrix corresponding to the given contact normal :param n: the normal of the surface (as a numpy array) :return: a rotation matrix """ z_up = np.array([0., 0., 1.]) q = Quaternion.FromTwoVectors(z_up, n) return q.matrix()
def sot_2_pinocchio(q): # PINOCCHIO Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36 # SOT Free flyer 0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35 qPino = np.matlib.zeros((37, 1)) qPino[:3, 0] = q[:3] quatMat = rpyToMatrix(q[3:6]) quatVec = Quaternion(quatMat) qPino[3:7, 0] = quatVec.coeffs() qPino[7:11, 0] = q[18:22] # chest-head qPino[11:18, 0] = q[29:] # larm qPino[18:25, 0] = q[22:29] # rarm qPino[25:31, 0] = q[12:18] # lleg qPino[31:, 0] = q[6:12] # rleg return qPino
def contactPlacementFromRBPRMState(fb, stateId, eeName): # get limbName from effector name : fb.setCurrentConfig(fb.getConfigAtState(stateId)) placement = SE3FromConfig(fb.getJointPosition(eeName)) if fb.cType == "_3_DOF": limbId = list(fb.dict_limb_joint.keys())[list(fb.dict_limb_joint.values()).index(eeName)] [p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(stateId, False, limbId) normal = np.array(n) quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal) placement.rotation = quat.matrix() return placement
def sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb, z): it = 0 success = False n = [0, 0, 1] vz = np.matrix(n).T while not success and it < 10000: it += 1 # sample a random position for movingLimb and try to project s0 to this position qr = fullBody.shootRandomConfig() q1 = s0.q()[::] q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[ limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact) fullBody.setCurrentConfig(s1.q()) p = fullBody.getJointPosition( fullBody.dict_limb_joint[movingLimb])[0:3] p[0] += random.uniform(eff_x_range[0], eff_x_range[1]) p[1] += random.uniform(eff_y_range[0], eff_y_range[1]) p[2] = z s1, success = StateHelper.addNewContact(s1, movingLimb, p, n) # force root orientation : (align current z axis with vertical) if success: quat_1 = Quaternion(s1.q()[6], s1.q()[3], s1.q()[4], s1.q()[5]) v_1 = quat_1.matrix() * vz align = Quaternion.FromTwoVectors(v_1, vz) rot = align * quat_1 q_root = s1.q()[0:7] q_root[3:7] = rot.coeffs().T.tolist()[0] success = s1.projectToRoot(q_root) # check if new state is in static equilibrium if success: # check stability success = fullBody.isStateBalanced(s1.sId, 3) # check if transition is feasible according to CROC if success: #success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0) success = fullBody.isReachableFromState(s0.sId, s1.sId) return success, s1
def computeRootYawAngleBetwwenConfigs(q0, q1): quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5]) quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5]) v_angular = np.array(log3(quat0.matrix().T.dot(quat1.matrix()))) #print ("q_prev : ",q0) #print ("q : ",q1) #print ("v_angular = ",v_angular) return v_angular[2]
def plan_guide(self, root_goal): """ Plan a guide from the current last_phase root position to the given root position :param root_goal: list of size 3 or 7: translation and quaternion for the desired root position If the quaternion part is not specified, the final orientation is not constrained Store the Id of the new path in self.current_guide_id """ self.guide_planner.q_goal = self.guide_planner.q_init[::] self.guide_planner.q_goal[:3] = root_goal[:3] self.guide_planner.q_goal[3:7] = [0, 0, 0, 1] self.guide_planner.q_goal[-6:] = [0] * 6 last_phase = self.get_last_phase() if last_phase: logger.warning("Last phase not None") logger.warning("Last phase q_final : %s", last_phase.q_final[:7]) self.guide_planner.q_init[:7] = last_phase.q_final[:7] self.guide_planner.q_init[2] = self.guide_planner.rbprmBuilder.ref_height # FIXME #add small velocity in order to have smooth change of orientation at the beginning/end quat_init = Quaternion(self.guide_planner.q_init[6], self.guide_planner.q_init[3], self.guide_planner.q_init[4], self.guide_planner.q_init[5]) dir_init = quat_init * np.array([1, 0, 0]) self.guide_planner.q_init[-6] = dir_init[0] * V_INIT self.guide_planner.q_init[-5] = dir_init[1] * V_INIT if len(root_goal) >= 7: self.guide_planner.q_goal[3:7] = root_goal[3:7] quat_goal = Quaternion(self.guide_planner.q_goal[6], self.guide_planner.q_goal[3], self.guide_planner.q_goal[4], self.guide_planner.q_goal[5]) dir_goal = quat_goal * np.array([1, 0, 0]) self.guide_planner.q_goal[-6] = dir_goal[0] * V_GOAL self.guide_planner.q_goal[-5] = dir_goal[1] * V_GOAL logger.warning("Guide init = %s", self.guide_planner.q_init) logger.warning("Guide goal = %s", self.guide_planner.q_goal) self.guide_planner.ps.resetGoalConfigs() self.guide_planner.ps.clearRoadmap() self.current_root_goal = root_goal self.guide_planner.solve(True) self.current_guide_id = self.guide_planner.ps.numberPaths() - 1
def exportWaist(path, waist_t): filename = path + "/WaistOrientation.dat" with open(filename, 'w') as f: for waist in waist_t.T: quat = Quaternion(waist[0, 6], waist[0, 3], waist[0, 4], waist[0, 5]) #rot = matrixToRpy(quat.matrix()) # DEBUG rot = matrixToRpy(SE3.Identity().rotation) # DEBUG line = "" for i in range(3): line += str(rot[i, 0]) + " " for i in range(6): line += "0 " f.write(line.rstrip(" ") + "\n") print("Motion exported to : ", filename) return