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 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 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 pinocchio_2_sot(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 qSot = np.matlib.zeros((36, 1)) qSot[:3] = q[:3] quatMat = Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix() qSot[3:6] = matrixToRpy(quatMat) qSot[18:22] = q[7:11] # chest-head qSot[29:] = q[11:18] # larm qSot[22:29] = q[18:25] # rarm qSot[12:18] = q[25:31] # lleg qSot[6:12] = q[31:] # rleg return qSot.A.squeeze()
def exportFoot(path, name, ref): assert ref.shape[0] == 12, " feet trajectory must be of size 12 (3 translation + rotation matrice)" filename = path + "/" + name + ".dat" with open(filename, 'w') as f: for k in range(ref.shape[1]): placement = SE3FromVec(ref[:, k]) line = "" # 3D position : for i in range(3): line += str(placement.translation[i, 0]) + " " # orientation : rot = matrixToRpy(placement.rotation) #rot = matrixToRpy(SE3.Identity().rotation) # DEBUG for i in range(3): line += str(rot[i, 0]) + " " # TODO : velocity and acceleration : for i in range(12): line += "0 " f.write(line.rstrip(" ") + "\n") print("Motion exported to : ", filename) return
def generateContactSequence(): RF, root_init, pb, coms, footpos, allfeetpos, res = runLPScript() # load scene and robot fb, v = initScene(cfg.Robot, cfg.ENV_NAME, False) q_init = fb.referenceConfig[::] + [0] * 6 q_init[0:7] = root_init #q_init[2] += fb.referenceConfig[2] - 0.98 # 0.98 is in the _path script v(q_init) # init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact # FIXME : allow to customize that first phase cs = ContactSequenceHumanoid(0) addPhaseFromConfig(fb, v, cs, q_init, [fb.rLegId, fb.lLegId]) # loop over all phases of pb and add them to the cs : for pId in range( 2, len(pb["phaseData"]) ): # start at 2 because the first two ones are already done in the q_init prev_contactPhase = cs.contact_phases[-1] #n = normal(pb["phaseData"][pId]) phase = pb["phaseData"][pId] moving = phase["moving"] movingID = fb.lfoot if moving == RF: movingID = fb.rfoot pos = allfeetpos[pId] # array, desired position for the feet movingID pos[2] += 0.005 # FIXME it shouldn't be required !! # compute desired foot rotation : if USE_ORIENTATION: if pId < len(pb["phaseData"]) - 1: quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"]) quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"]) rot = quat0.slerp(0.5, quat1) # check if feets do not cross : if moving == RF: qr = rot ql = Quaternion( prev_contactPhase.LF_patch.placement.rotation) else: ql = rot qr = Quaternion( prev_contactPhase.RF_patch.placement.rotation) rpy = matrixToRpy((qr * (ql.inverse())).matrix( )) # rotation from the left foot pose to the right one if rpy[2, 0] > 0: # yaw positive, feet are crossing rot = Quaternion(phase["rootOrientation"] ) # rotation of the root, from the guide else: rot = Quaternion(phase["rootOrientation"] ) # rotation of the root, from the guide else: rot = Quaternion.Identity() placement = SE3() placement.translation = np.matrix(pos).T placement.rotation = rot.matrix() moveEffectorToPlacement(fb, v, cs, movingID, placement, initStateCenterSupportPolygon=True) # final phase : # fixme : assume root is in the middle of the last 2 feet pos ... q_end = fb.referenceConfig[::] + [0] * 6 p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2. for i in range(3): q_end[i] += p_end[i] setFinalState(cs, q=q_end) displaySteppingStones(cs, v.client.gui, v.sceneName, fb) return cs, fb, v
def generateContactSequence(): #RF,root_init,pb, coms, footpos, allfeetpos, res = runLPScript() RF, root_init, root_end, pb, coms, footpos, allfeetpos, res = runLPFromGuideScript( ) multicontact_api.switchToNumpyArray() # load scene and robot fb, v = initScene(cfg.Robot, cfg.ENV_NAME, True) q_init = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6 q_init[0:7] = root_init feet_height_init = allfeetpos[0][2] print("feet height initial = ", feet_height_init) q_init[2] = feet_height_init + cfg.IK_REFERENCE_CONFIG[2] q_init[2] += EPS_Z #q_init[2] = fb.referenceConfig[2] # 0.98 is in the _path script if v: v(q_init) # init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact # FIXME : allow to customize that first phase cs = ContactSequence(0) addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId]) # loop over all phases of pb and add them to the cs : for pId in range( 2, len(pb["phaseData"]) ): # start at 2 because the first two ones are already done in the q_init prev_contactPhase = cs.contactPhases[-1] #n = normal(pb["phaseData"][pId]) phase = pb["phaseData"][pId] moving = phase["moving"] movingID = fb.lfoot if moving == RF: movingID = fb.rfoot pos = allfeetpos[pId] # array, desired position for the feet movingID pos[2] += EPS_Z # FIXME it shouldn't be required !! # compute desired foot rotation : if cfg.SL1M_USE_ORIENTATION: 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 cfg.SL1M_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] > 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 else: rot = Quaternion.Identity() placement = SE3() placement.translation = np.array(pos).T placement.rotation = rot.matrix() cs.moveEffectorToPlacement(movingID, placement) # final phase : # fixme : assume root is in the middle of the last 2 feet pos ... q_end = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6 #p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2. #for i in range(3): # q_end[i] += p_end[i] q_end[0:7] = root_end feet_height_end = allfeetpos[-1][2] print("feet height final = ", feet_height_end) q_end[2] = feet_height_end + cfg.IK_REFERENCE_CONFIG[2] q_end[2] += EPS_Z fb.setCurrentConfig(q_end) com = fb.getCenterOfMass() setFinalState(cs, com, q=q_end) if cfg.DISPLAY_CS_STONES: displaySteppingStones(cs, v.client.gui, v.sceneName, fb) return cs, fb, v