def generate_contact_sequence_sl1m(cfg): #RF,root_init,pb, coms, footpos, allfeetpos, res = runLPScript(cfg) RF, root_init, root_end, pb, coms, footpos, allfeetpos, res = runLPFromGuideScript(cfg) 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 if cfg.SL1M_USE_MIP: # if MIP is used, allfeetpos contains a list of all position and not just the new position feet_height_init = allfeetpos[0][0][2] else: feet_height_init = allfeetpos[0][2] logger.info("feet height initial = %s", 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) cs = build_cs_from_sl1m(cfg.SL1M_USE_MIP, fb, cfg.IK_REFERENCE_CONFIG, root_end, pb, RF, allfeetpos, cfg.SL1M_USE_ORIENTATION, cfg.SL1M_USE_INTERPOLATED_ORIENTATION, q_init=q_init) if cfg.DISPLAY_CS_STONES: displaySteppingStones(cs, v.client.gui, v.sceneName, fb) return cs, fb, v
def addPhaseFromConfig(fb, cs, q, limbsInContact, t_init = -1): """ Add a new contact phase from a wholebody configuration and a list of active contacts. Set the q_init, c_init and c_final from the joint positions, and add ContactPatch for each limb in contact, with the current placement corresponding to the wholebody configuration :param fb: an rbprm.Fullbody object :param cs: the current contact sequence :param q: a list of joint position :param limbsInContact: a list of limbs currently in contact :param t_init: if provided, the new COntactPhase will begin at this time """ multicontact_api.switchToNumpyArray() # FIXME : why is it required to add it again here ? cs.append(createPhaseFromConfig(fb, q, limbsInContact, t_init))
import numpy as np import mlp.config as cfg import multicontact_api from multicontact_api import ContactPhase, ContactSequence from mlp.utils.util import createFullbodyStatesFromCS from mlp.utils.cs_tools import connectPhaseTrajToFinalState, setInitialFromFinalValues, copyPhaseInitToFinal from mlp.utils.requirements import Requirements multicontact_api.switchToNumpyArray() class Inputs(Requirements): timings = True consistentContacts = True configurationValues = True class Outputs(Inputs): COMtrajectories = True COMvalues = True ## Produce a centroidal trajectory where the CoM only move when the contacts are fixed. ## It is then fixed during the swing phase where one contact is repositionned. ## The position reached by the CoM are given with 2-PAC ## The trajectory of the CoM is a quintic spline with initial and final velocity/acceleration constrained to 0 def getTargetCOMPosition(fullBody, id_state): print("call 2-pac for ids : "+str(id_state)+ " ; "+str(id_state+1)) tab = fullBody.isReachableFromState(id_state, id_state + 1, True, False) print("tab results : ",tab) success = tab[0] assert success, "2-pac failed for state id : " + str(id_state) if len(tab) == 7:
def addPhaseFromConfig(fb, cs, q, limbsInContact, t_init=-1): multicontact_api.switchToNumpyArray( ) # FIXME : why is it required to add it again here ? cs.append(createPhaseFromConfig(fb, q, limbsInContact, t_init))
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