def addPhaseFromConfig(fb, v, cs, q, limbsInContact): phase = ContactPhaseHumanoid() phase.reference_configurations.append(np.matrix((q)).T) if v: v(q) fb.setCurrentConfig(q) state = np.matrix(np.zeros(9)).T state[0:3] = np.matrix(fb.getCenterOfMass()).T phase.init_state = state.copy() phase.final_state = state.copy() # set Identity to each contact placement (otherwise it's uninitialized) phase.RF_patch.placement = SE3.Identity() phase.LF_patch.placement = SE3.Identity() phase.RH_patch.placement = SE3.Identity() phase.LH_patch.placement = SE3.Identity() for limb in limbsInContact: eeName = fb.dict_limb_joint[limb] q_j = fb.getJointPosition(eeName) patch = contactPatchForEffector(phase, eeName, fb) placement = SE3FromConfig(q_j) patch.placement = placement.act(fb.dict_offset[eeName]) patch.active = True cs.contact_phases.append(phase) if v: display_tools.displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
def rootOrientationFromFeetPlacement(phase, phase_next): #FIXME : extract only the yaw rotation qr = Quaternion(phase.RF_patch.placement.rotation) qr.x = 0 qr.y = 0 qr.normalize() ql = Quaternion(phase.LF_patch.placement.rotation) ql.x = 0 ql.y = 0 ql.normalize() q_rot = qr.slerp(0.5, ql) placement_init = SE3.Identity() placement_init.rotation = q_rot.matrix() if phase_next: if not isContactActive(phase, cfg.Robot.rfoot) and isContactActive( phase_next, cfg.Robot.rfoot): qr = Quaternion(phase_next.RF_patch.placement.rotation) qr.x = 0 qr.y = 0 qr.normalize() if not isContactActive(phase, cfg.Robot.lfoot) and isContactActive( phase_next, cfg.Robot.lfoot): ql = Quaternion(phase_next.LF_patch.placement.rotation) ql.x = 0 ql.y = 0 ql.normalize() q_rot = qr.slerp(0.5, ql) placement_end = SE3.Identity() placement_end.rotation = q_rot.matrix() return placement_init, placement_end
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 walk(fb, cs, distance, stepLength, gait, duration_ss = -1 , duration_ds = -1, first_half_step=True): """ Generate a walking motion from the last phase in the contact sequence. The contacts will be moved in the order of the 'gait' list. With the first one move only of half the stepLength TODO : make it generic ! it's currently limited to motion in the x direction :param fb: an rbprm.Fullbody object :param cs: a ContactSequence :param distance: the required distance the first and last contact placement along the X axis :param stepLength: the length of the steps :param gait: a list of limb names used as gait :param duration_ss: the duration of the single support phases :param duration_ds: the duration of the double support phases """ fb.usePosturalTaskContactCreation(True) prev_phase = cs.contactPhases[-1] for limb in gait: eeName = fb.dict_limb_joint[limb] assert prev_phase.isEffectorInContact(eeName), "All limbs in gait should be in contact in the first phase" isFirst = first_half_step reached = False firstContactReachedGoal = False remainingDistance = distance while remainingDistance >= 1e-6: for k, limb in enumerate(gait): #print("move limb : ",limb) eeName = fb.dict_limb_joint[limb] if isFirst: length = stepLength / 2. isFirst = False else: length = stepLength if k == 0 and first_half_step: if length > (remainingDistance + stepLength / 2.): length = remainingDistance + stepLength / 2. firstContactReachedGoal = True else: if length > remainingDistance: length = remainingDistance transform = SE3.Identity() #print("length = ",length) transform.translation = np.array([length, 0, 0]) cs.moveEffectorOf(eeName, transform, duration_ds, duration_ss) remainingDistance -= stepLength if first_half_step and not firstContactReachedGoal: transform = SE3.Identity() #print("last length = ", stepLength) transform.translation = np.array([stepLength / 2., 0, 0]) cs.moveEffectorOf(fb.dict_limb_joint[gait[0]], transform, duration_ds, duration_ss) q_end = fb.referenceConfig[::] + [0] * 6 q_end[0] += distance fb.setCurrentConfig(q_end) com = fb.getCenterOfMass() setFinalState(cs, com, q=q_end) fb.usePosturalTaskContactCreation(False)
def __init__(self, robot, frame_id, ref_trajectory, name="SE3 Task"): Task.__init__(self, robot, name) self._frame_id = frame_id self._ref_trajectory = ref_trajectory # set default value to M_ref self._M_ref = SE3.Identity() # mask over the desired euclidian axis self._mask = (np.ones(6)).astype(bool) self._gMl = SE3.Identity()
def jacobian(self, q): self.robot.mass(q) com_p = self.robot.com(q) cXi= SE3.Identity() oXi = self.robot.data.oMi[1] cXi.rotation = oXi.rotation cXi.translation = oXi.translation - com_p M_ff = self.robot.data.M[:6,:] M_com = cXi.inverse().np.T * M_ff L_dot = M_com[3:,:] wXc = SE3(eye(3),self.robot.position(q,1).inverse()*self.robot.com(q)) Jang = wXc.action.T[3:,:]*self.robot.mass(q)[:6,:] return self._coeff * L_dot[self._mask,:]
def __init__(self, debug: bool = False, **kwargs): # Get the urdf and mesh paths data_root_dir = resource_filename("gym_jiminy.envs", "data/bipedal_robots/cassie") urdf_path = os.path.join(data_root_dir, "cassie.urdf") # Initialize the walker environment super().__init__( **{ **dict(urdf_path=urdf_path, mesh_path=data_root_dir, simu_duration_max=SIMULATION_DURATION, step_dt=STEP_DT, reward_mixture=REWARD_MIXTURE, std_ratio=STD_RATIO, avoid_instable_collisions=False, debug=debug), **kwargs }) # Add missing pushrod close kinematic chain constraint M_pushrod_tarsus_right = SE3(np.eye(3), np.array([-0.12, 0.03, -0.005])) M_pushrod_hip_right = SE3(np.eye(3), np.array([0.0, 0.0, -0.045])) self.robot.add_frame("right_pushrod_tarsus", "right_tarsus", M_pushrod_tarsus_right) self.robot.add_frame("right_pushrod_hip", "hip_flexion_right", M_pushrod_hip_right) pushrod_right = DistanceConstraint("right_pushrod_tarsus", "right_pushrod_hip", 0.5012) self.robot.add_constraint("pushrod_right", pushrod_right) M_pushrod_tarsus_left = SE3(np.eye(3), np.array([-0.12, 0.03, 0.005])) M_pushrod_hip_left = SE3(np.eye(3), np.array([0.0, 0.0, 0.045])) self.robot.add_frame("left_pushrod_tarsus", "left_tarsus", M_pushrod_tarsus_left) self.robot.add_frame("left_pushrod_hip", "hip_flexion_left", M_pushrod_hip_left) pushrod_left = DistanceConstraint("left_pushrod_tarsus", "left_pushrod_hip", 0.5012) self.robot.add_constraint("pushrod_left", pushrod_left) # Replace knee to shin spring by fixed joint constraint right_spring_knee_to_shin = JointConstraint("knee_to_shin_right") self.robot.add_constraint("right_spring_knee_to_shin", right_spring_knee_to_shin) left_spring_knee_to_shin = JointConstraint("knee_to_shin_left") self.robot.add_constraint("left_spring_knee_to_shin", left_spring_knee_to_shin)
def dyn_value(self, t, q, v): g = self.robot.biais(q,0*v) b = self.robot.biais(q,v) b -= g; M = self.robot.data.mass[0]#(q) com_p = self.robot.com(q) cXi = SE3.Identity() oXi = self.robot.data.oMi[1] cXi.rotation = oXi.rotation cXi.translation = oXi.translation - com_p b_com = cXi.inverse().np.T * b[:6,0] b_angular = -b_com[3:,:] M_com = cXi.inverse().np.T * M[:6,:] L = M_com[3:,:] * v L_des, Ldot_des = self._ref_traj(t) L_error = L - L_des acc = Ldot_des - b_com[3:,:] self.a_des = acc self.drift = 0*self.a_des self._jacobian = self.jacobian(q) return self._jacobian[self._mask,:], self.drift[self._mask], self.a_des[self._mask]
def displayEffectorTrajectories(cs, viewer, Robot, suffixe="", colorAlpha=1, applyOffset=False): """ Display all the effector trajectories stored in the given ContactSequence. With colors for each effectors defined in Robot.dict_limb_color_traj :param cs: the ContactSequence storing the trajectories :param viewer: An instance of hpp.gepetto.Viewer :param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot) :param suffixe: an optionnal suffixe to the name of the node objects created :param colorAlpha: the transparency of the trajectories (must be between 0 and 1) """ effectors = cs.getAllEffectorsInContact() for pid, phase in enumerate(cs.contactPhases): for eeName in effectors: if phase.effectorHaveAtrajectory(eeName): color = Robot.dict_limb_color_traj[eeName] color[-1] = colorAlpha if applyOffset: offset = -Robot.dict_offset[eeName] else: offset = SE3.Identity() displaySE3Traj(phase.effectorTrajectory(eeName), viewer.client.gui, viewer.sceneName, eeName + "_traj_" + suffixe + str(pid), color, [phase.timeInitial, phase.timeFinal], offset)
def dyn_value(self, t, q, v): #(hg_ref, vhg_ref, ahg_ref) = self._ref_traj(t) vhg_ref = np.matrix([0., 0., 0., 0., 0., 0.]).T model = self.robot.model data = self.robot.data JMom = se3.ccrba(model, data, q, v) hg_prv = data.hg.vector.copy()[self._mask,:] #self.p_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:] #self.v_error = self.robot.fext[self._mask,:] - vhg_ref[self._mask,:] #self.v_error = self.robot.fext[self._mask,:] #self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:] #self.a_des = -self.kv*self.v_error #+model.gravity.vector[self._mask,:] self.a_des = self.kv*self.robot.fext[self._mask,:]#vhg_ref #+model.gravity.vector[self._mask,:] #self.drift = 0 * self.a_des #self.a_des = #*********************** p_com = data.com[0] cXi = SE3.Identity() oXi = self.robot.data.oMi[1] cXi.rotation = oXi.rotation cXi.translation = oXi.translation - p_com m_gravity = model.gravity.copy() model.gravity.setZero() b = se3.nle(model,data,q,v) model.gravity = m_gravity f_ff = se3.Force(b[:6]) f_com = cXi.act(f_ff) hg_drift = f_com.angular self.drift=f_com.np[self._mask,:] #************************ #print JMom.copy()[self._mask,:].shape #print self.__gain_matrix.shape self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix return self._jacobian, self.drift, self.a_des
def create_stairs_cs(): ENV_NAME = "multicontact/bauzil_stairs" fb, v = display_tools.initScene(Robot, ENV_NAME, False) cs = ContactSequence(0) # Create an initial contact phase : q_ref = fb.referenceConfig[::] + [0] * 6 q_ref[0:2] = [0.07, 1.2] addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId]) step_height = 0.1 step_width = 0.3 displacement = SE3.Identity() displacement.translation = array([step_width, 0, step_height]) cs.moveEffectorOf(fb.rfoot, displacement) cs.moveEffectorOf(fb.lfoot, displacement) q_end = q_ref[::] q_end[0] += step_width q_end[2] += step_height fb.setCurrentConfig(q_end) com = fb.getCenterOfMass() setFinalState(cs, array(com), q=q_end) return cs
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 dyn_value(self, t, q, v, update_geometry=False): g = self.robot.bias(q, 0 * v) b = self.robot.bias(q, v) b -= g M = self.robot.mass(q) com_p = self.robot.com(q) cXi = SE3.Identity() oXi = self.robot.data.oMi[1] cXi.rotation = oXi.rotation cXi.translation = oXi.translation - com_p b_com = cXi.actInv(se3.Force(b[:6, 0])) # b_com = cXi.actInv(b[:6,0]).vector b_com = b_com.angular # M_com = cXi.inverse().action.T * M[:6,:] # M_com = cXi.inverse().np.T * M[:6,:] M_com = self.robot.momentumJacobian(q, v, update_geometry) M_com = M_com[3:, :] L = M_com * v L_ref, dL_ref, ddL_ref = self._ref_traj(t) # acc = dL_ref - b_com[3:,:] dL_des = dL_ref - self.kp * (L - L_ref) return M_com[self._mask, :], b_com[self._mask, :], dL_des[self._mask, 0]
def displaySE3Traj(traj, gui, sceneName, name, color, time_interval, offset=SE3.Identity()): if name == None: name = "SE3_traj" rootName = name # add indices until the name is free list = gui.getNodeList() i = 0 while list.count(name) > 0: name = rootName + "_" + str(i) i += 1 path = [] dt = 0.01 t = time_interval[0] while t <= time_interval[1]: m = traj(t)[0] m = m.act(offset) path += m.translation.T.tolist() t += dt gui.addCurve(name, path, color) gui.addToGroup(name, sceneName) gui.refresh()
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 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 walk(fb, cs, distance, stepLength, gait, duration_ss=-1, duration_ds=-1): fb.usePosturalTaskContactCreation(True) prev_phase = cs.contactPhases[-1] for limb in gait: eeName = fb.dict_limb_joint[limb] assert prev_phase.isEffectorInContact( eeName ), "All limbs in gait should be in contact in the first phase" isFirst = True reached = False firstContactReachedGoal = False remainingDistance = distance while remainingDistance >= 1e-6: for k, limb in enumerate(gait): #print("move limb : ",limb) eeName = fb.dict_limb_joint[limb] if isFirst: length = stepLength / 2. isFirst = False else: length = stepLength if k == 0: if length > (remainingDistance + stepLength / 2.): length = remainingDistance + stepLength / 2. firstContactReachedGoal = True else: if length > remainingDistance: length = remainingDistance transform = SE3.Identity() #print("length = ",length) transform.translation = np.array([length, 0, 0]) cs.moveEffectorOf(eeName, transform, duration_ds, duration_ss) remainingDistance -= stepLength if not firstContactReachedGoal: transform = SE3.Identity() #print("last length = ", stepLength) transform.translation = np.array([stepLength / 2., 0, 0]) cs.moveEffectorOf(fb.dict_limb_joint[gait[0]], transform, duration_ds, duration_ss) q_end = fb.referenceConfig[::] + [0] * 6 q_end[0] += distance fb.setCurrentConfig(q_end) com = fb.getCenterOfMass() setFinalState(cs, com, q=q_end) fb.usePosturalTaskContactCreation(False)
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 computeWrenchRef(res): Mcom = SE3.Identity() for k, t in enumerate(res.t_t): Mcom.translation = res.c_reference[:, k] Fcom = Force.Zero() Fcom.linear = cfg.MASS * (res.ddc_reference[:, k] - cfg.GRAVITY) Fcom.angular = res.dL_reference[:, k] F0 = Mcom.act(Fcom) res.wrench_reference[:, k] = F0.vector return res
def computeInequalitiesAroundLine(fullBody, p_from, p_to, eeName, groupName, viewer): a = p_from.translation.tolist() b = p_to.translation.tolist() # size of the end effector (-x,x,-y,y,-z,z) size_diagonal = math.sqrt( cfg.Robot.dict_size[eeName][0]**2 + cfg.Robot.dict_size[eeName][1]**2) #TODO margin ?? #size = [size_diagonal/2., size_diagonal/2.,0.001] size = [ -cfg.Robot.dict_size[eeName][0] / 2., cfg.Robot.dict_size[eeName][0] / 2., -cfg.Robot.dict_size[eeName][1] / 2., cfg.Robot.dict_size[eeName][1] / 2., -0.001, 0.001 ] #size=[0,0,0] #FIXME debug """ size_r = np.array(size) # rotate size vector according to initial rotation of the effector : if VERBOSE : print "rotation init : ",p_from.rotation size_r = p_from.rotation.dot(size_r) """ points = large_col_free_box(fullBody.clientRbprm.rbprm, a, b, sizeObject=size) # Display the box before the size reduction : #if DISPLAY_CONSTRAINTS and not DISPLAY_JOINT_LEVEL: # display_box(viewer,points,groupName) """ pointsReduced = [] rot_init = p_from.rotation for i in range(len(box_points)): #pointsReduced += [points[i]-rot_init.dot((size_r*array(box_points[i]))/2.)] if VERBOSE : print "for point "+str(i)+" shift of "+str(-((size_r*np.array(box_points[i])))) pointsReduced += [points[i]-((size_r*np.array(box_points[i])))] """ # display the box after size reduction if DISPLAY_CONSTRAINTS and not DISPLAY_JOINT_LEVEL: display_box(viewer, points, groupName) # transform the points back to joint level pc = SE3.Identity( ) # take Identity rotation #FIXME probably not the best idea ... pointsJoint = [] for point in points: pc.translation = np.matrix(point).T pointsJoint += [ cfg.Robot.dict_offset[eeName].actInv(pc).translation.tolist() ] if DISPLAY_CONSTRAINTS and DISPLAY_JOINT_LEVEL: display_box(viewer, pointsJoint, groupName) H, h = to_ineq(pointsJoint) return H, h.reshape(-1, 1)
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 SE3FromVec(vect): if vect.shape[0] != 12 or vect.shape[1] != 1: raise ValueError("SE3FromVect take as input a vector of size 12") placement = SE3.Identity() placement.translation = vect[0:3] rot = placement.rotation rot[:, 0] = np.asarray(vect[3:6]).reshape(-1) rot[:, 1] = np.asarray(vect[6:9]).reshape(-1) rot[:, 2] = np.asarray(vect[9:12]).reshape(-1) placement.rotation = rot return placement
def __init__(self, robot, frame_id, ref_trajectory, name = "Task"): Task.__init__ (self, robot, name) self._frame_id = frame_id self._ref_trajectory = ref_trajectory # set default value to M_ref self._M_ref = SE3.Identity # mask over the desired euclidian axis self._mask = (np.ones(6)).astype(bool) # for local to global self._gMl = SE3.Identity() self.__gain_matrix = np.matrix(np.eye(robot.nv))
def __init__(self, robot, frame_id, op_point, target, ref_trajectory, name = "Task"): Task.__init__ (self, robot, name) self._frame_id = frame_id self._ref_trajectory = ref_trajectory self._op_point = op_point # set default value to M_ref self._M_ref = SE3.Identity # mask over the desired euclidian axis self._mask = (np.ones(6)).astype(bool) self._target = target # for local to global self._gMl = SE3.Identity()
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
class Robot: # data for talos (to avoid a depencie on talos-rbprm for this script) rfoot = 'leg_right_6_joint' lfoot = 'leg_left_6_joint' rhand = 'arm_right_7_joint' lhand = 'arm_left_7_joint' rLegOffset = [0., -0.00018, -0.102] lLegOffset = [0., -0.00018, -0.102] rArmOffset = [-0.01, 0., -0.154] lArmOffset = [-0.01, 0., -0.154] MRsole_offset = SE3.Identity() MRsole_offset.translation = np.matrix(rLegOffset).T MLsole_offset = SE3.Identity() MLsole_offset.translation = np.matrix(lLegOffset).T MRhand_offset = SE3.Identity() MRhand_offset.translation = np.matrix(rArmOffset).T MLhand_offset = SE3.Identity() MLhand_offset.translation = np.matrix(lArmOffset).T dict_offset = { rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset } MRsole_display = SE3.Identity() MLsole_display = SE3.Identity() MRhand_display = SE3.Identity() #MRhand_display.translation = np.matrix([0, 0., -0.11]) MLhand_display = SE3.Identity() #MLhand_display.translation = np.matrix([0, 0., -0.11]) dict_display_offset = { rfoot: MRsole_display, lfoot: MLsole_display, rhand: MRhand_display, lhand: MLhand_display } dict_limb_color_traj = { rfoot: [0, 1, 0, 1], lfoot: [1, 0, 0, 1], rhand: [0, 0, 1, 1], lhand: [0.9, 0.5, 0, 1] } dict_size = { rfoot: [0.2, 0.13], lfoot: [0.2, 0.13], rhand: [0.1, 0.1], lhand: [0.1, 0.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
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 computeBoxVertices(client, center, x_dir, dim, sizeObject): # compute the vertices of this box: #points = [ [dim[1],-dim[2],-dim[4]], [dim[1],-dim[2],dim[5]], [-dim[0],-dim[2],dim[5]], [-dim[0],-dim[2],-dim[4]], [dim[1],dim[3],-dim[4]], [dim[1],dim[3],dim[5]], [-dim[0],dim[3],dim[5]], [-dim[0],dim[3],-dim[4]] ] points = [] for sign in box_points: point = [] for i in range(3): if sign[i] < 0: # required because dimensions are not symetrical point += [-dim[i * 2]] else: point += [dim[i * 2 + 1]] points += [point] # transform this points to the position/orientation of the box : rot = rot_mat_x(client, x_dir.tolist()) t_c_w = SE3.Identity() # transform center of box in world frame t_c_w.translation = np.matrix(center).T t_c_w.rotation = rot pointsTransform = [] for p in points: t_p_c = SE3.Identity() # vertice position in box frame t_p_c.translation = np.matrix(p).T pointsTransform += [t_c_w.act(t_p_c).translation.T[0]] return pointsTransform
def createRandomState(fullBody, limbsInContact): extraDof = int(fullBody.client.robot.getDimensionExtraConfigSpace()) q0 = fullBody.referenceConfig[::] if extraDof > 0: q0 += [0] * extraDof qr = fullBody.shootRandomConfig() root = SE3.Identity() root.translation = np.matrix(qr[0:3]).T # sample random orientation along z : root = sampleRotationAlongZ(root) q0[0:7] = se3ToXYZQUATtuple(root) # apply random config to legs (FIXME : ID hardcoded for Talos) q0[7:19] = qr[7:19] fullBody.setCurrentConfig(q0) s0 = State(fullBody, q=q0, limbsIncontact=limbsInContact) return s0