def generateContactSequence(): fb, v = display_tools.initScene(cfg.Robot, cfg.ENV_NAME) cs = ContactSequenceHumanoid(0) filename = cfg.CONTACT_SEQUENCE_PATH + "/" + cfg.DEMO_NAME + ".cs" print "Import contact sequence binary file : ", filename cs.loadFromBinary(filename) display_tools.displayWBconfig( v, cs.contact_phases[0].reference_configurations[0]) return cs, fb, v
def loadMotionFromFiles(gui, path, npzFilename, csFilename): # load cs from file : cs = ContactSequenceHumanoid(0) cs.loadFromBinary(path + csFilename) displaySteppingStones(cs, gui, sceneName, Robot) colors = [[0., 0., 1., 1.], [0., 1., 0., 1.]] displayCOMTrajectory(cs, gui, sceneName, colors) #extract data from npz archive : res = wb_res.loadFromNPZ(path + npzFilename) displayFeetTrajFromResult(gui, sceneName, res, Robot) plot.plotALLFromWB(cs, res) return res, cs
def addInitAndGoalShift(cs_in): cs = cs_com = ContactSequenceHumanoid(cs_in.size() + 2) # copy all phases but leave one phase at the beginning and at the end : for k in range(cs_in.size()): phase = cs_in.contact_phases[k].copy() # shift times to take in account the new phase : for i in range(len(phase.time_trajectory)): phase.time_trajectory[i] += cfg.TIME_SHIFT_COM cs.contact_phases[k + 1] = phase # now add new first phase : phase = cs.contact_phases[0] s_final = cs.contact_phases[1].init_state s_init = s_final.copy() s_init[2] -= cfg.COM_SHIFT_Z phase.init_state = s_init phase.final_state = s_final generateLinearInterpTraj(phase, cfg.TIME_SHIFT_COM, 0) copyPhaseContacts(cs.contact_phases[1], phase) phase.reference_configurations = cs.contact_phases[ 1].reference_configurations # add the new final phase phase = cs.contact_phases[-1] s_init = cs.contact_phases[-2].final_state s_final = s_init.copy() s_final[2] -= cfg.COM_SHIFT_Z phase.init_state = s_init phase.final_state = s_final generateLinearInterpTraj(phase, cfg.TIME_SHIFT_COM, cs.contact_phases[-2].time_trajectory[-1]) copyPhaseContacts(cs.contact_phases[-2], phase) phase.reference_configurations = cs.contact_phases[ -2].reference_configurations return cs
def generateCentroidalTrajectory(cs, cs_initGuess=None, fb=None, viewer=None): if cs_initGuess: print "WARNING : in centroidal.croc, initial guess is ignored." if not fb: raise ValueError("CROC called without fullBody object.") beginId, endId = createFullbodyStatesFromCS(cs, fb) print "beginid = ", beginId print "endId = ", endId cs_result = ContactSequenceHumanoid(cs) if (2 * (endId - beginId) + 1) != cs.size(): raise NotImplemented( "Current implementation of CROC require to have 2 contact phases per States in fullBody" ) # for each phase in the cs, create a corresponding FullBody State and call CROC, # then discretize the solution and fill the cs struct # Make the assumption that the CS was created with the generateContactSequence method from the same fb object id_phase = 0 for id_state in range(beginId, endId): print "id_state = ", str(id_state) print "id_phase = ", str(id_phase) # First, compute the bezier curves between the states : pid = fb.isDynamicallyReachableFromState(id_state, id_state + 1, True, numPointsPerPhases=0) if len(pid) != 4: print "# ERROR : Cannot compute CROC between states " + str( id_state) + " , " + str(id_state + 1) return cs c = fb.getPathAsBezier(int(pid[0])) # Now split and write the results in the correct contactPhases if id_phase == 0: append = False t = 0. else: append = True t = cs_result.contact_phases[id_phase].time_trajectory[-1] assert c.min() == 0, "bezier curve should start at t=0." # First phase (second half of the trajectory of this phase if it's not the initial phase of the CS) start = 0 end = fb.client.problem.pathLength(int(pid[1])) writeTrajInPhase(cs_result.contact_phases[id_phase], c, t, start, end, append) #Second phase id_phase += 1 start = end end += fb.client.problem.pathLength(int(pid[2])) t = cs_result.contact_phases[id_phase - 1].time_trajectory[-1] writeTrajInPhase(cs_result.contact_phases[id_phase], c, t, start, end) # Third phase (only the first half of the trajectory, exept if it's the final contact phase of the CS) id_phase += 1 start = end end += fb.client.problem.pathLength(int(pid[3])) t = cs_result.contact_phases[id_phase - 1].time_trajectory[-1] assert abs(end - c.max()) < 1e-10, "Error in computation of time interval" writeTrajInPhase(cs_result.contact_phases[id_phase], c, t, start, end) return cs_result
import numpy as np import pinocchio from crocoddyl import (ActionModelImpact, CallbackDDPVerbose, CallbackSolverDisplay, ShootingProblem, SolverDDP, StatePinocchio, a2m, m2a) from crocoddyl.locomotion import ContactSequenceWrapper, createMultiphaseShootingProblem from multicontact_api import ContactSequenceHumanoid, CubicHermiteSpline np.set_printoptions(linewidth=400, suppress=True) robot = conf.robot rmodel = robot.model rdata = robot.data rmodel.defaultState = np.concatenate([m2a(robot.q0), np.zeros(rmodel.nv)]) # ----------------Load Contact Phases and PostProcess----------------------- cs = ContactSequenceHumanoid(0) cs.loadFromXML(conf.MUSCOD_CS_OUTPUT_FILENAME, conf.CONTACT_SEQUENCE_XML_TAG) csw = ContactSequenceWrapper(cs, conf.contact_patches) csw.createCentroidalPhi(rmodel, rdata) csw.createEESplines(rmodel, rdata, conf.X_init, 0.005) # ----------------Define Problem---------------------------- models = createMultiphaseShootingProblem(rmodel, rdata, csw, conf.DT) # disp = lambda xs: disptraj(robot, xs) problem = ShootingProblem(m2a(conf.X_init[0]), models[:-1], models[-1]) # Set contacts in the data elements. Ugly. # This is defined for IAMEuler. If using IAMRK4, differential is a list. so we need to change. for d in problem.runningDatas:
from multicontact_api import ContactSequenceHumanoid, ContactPhaseHumanoid import mlp.viewer.display_tools as display_tools from mlp.utils.cs_tools import * import mlp.config as cfg from pinocchio import SE3 from mlp.utils.util import rotatePlacement from hpp.corbaserver.rbprm.talos import Robot # change robot here from mlp.utils.cs_tools import moveEffectorToPlacement import numpy as np from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper ENV_NAME = "multicontact/stairsAirbus_noRamp" fb, v = display_tools.initScene(Robot, ENV_NAME, False) cs = ContactSequenceHumanoid(0) #Create an initial contact phase : q_ref = [ 0, 0, 0.9832773, 0, 0.0, 0.0, 1.0, #Free flyer 1.45, 0.0, -0.611354, 1.059395, -0.448041, -0.001708, #Left Leg
def fillCSFromTimeopt(cs, cs_initGuess, tp): cs_com = ContactSequenceHumanoid(cs) # extract infos from tp to fill cs.contact_phases struct u = [0] * 6 x = [0] * 9 MASS = tp.getMass() p_id = 0 # phase id in cs k_id = 1 # id in the current phase # tp.getTime(0) == dt !! not 0 p0 = cs_com.contact_phases[0] init_state = p0.init_state init_state[0:3] = tp.getInitialCOM() p0.init_state = init_state state = p0.init_state appendOrReplace(p0.time_trajectory, 0, 0.) appendOrReplace(p0.state_trajectory, 0, state) appendOrReplace(p0.control_trajectory, 0, np.matrix(np.zeros(6)).T) for k in range(tp.getTrajectorySize()): appendOrReplace(cs_com.contact_phases[p_id].time_trajectory, k_id, tp.getTime(k)) #extract x and u from tp : if k == 0: u[0:3] = ((tp.getLMOM(k) / MASS) / tp.getTime(k)).tolist() # acceleration u[3:6] = ((tp.getAMOM(k)) / (tp.getTime(k))).tolist() # angular momentum variation else: u[0:3] = ( ((tp.getLMOM(k) / MASS) - (tp.getLMOM(k - 1) / MASS)) / (tp.getTime(k) - tp.getTime(k - 1))).tolist() #acceleration u[3:6] = ((tp.getAMOM(k) - tp.getAMOM(k - 1)) / (tp.getTime(k) - tp.getTime(k - 1)) ).tolist() # angular momentum variation #print "control = ",np.matrix(u) x[0:3] = tp.getCOM(k).tolist() # position x[3:6] = (tp.getLMOM(k) / MASS).tolist() # velocity x[6:9] = tp.getAMOM(k).tolist() # angular momentum appendOrReplace(cs_com.contact_phases[p_id].control_trajectory, k_id, np.matrix(u)) appendOrReplace(cs_com.contact_phases[p_id].state_trajectory, k_id, np.matrix(x)) if k > 0 and isNewPhase(tp, k - 1, k, cs_com, cs_initGuess, p_id) and p_id < cs_com.size() - 1: #last k of current phase, first k of next one (same state_traj and time) # set final state of current phase : cs_com.contact_phases[p_id].final_state = np.matrix(x) # first k of the current phase p_id += 1 k_id = 0 cs_com.contact_phases[p_id].init_state = np.matrix(x) appendOrReplace(cs_com.contact_phases[p_id].time_trajectory, k_id, tp.getTime(k)) appendOrReplace(cs_com.contact_phases[p_id].control_trajectory, k_id, np.matrix(u)) appendOrReplace(cs_com.contact_phases[p_id].state_trajectory, k_id, np.matrix(x)) k_id += 1 # timeopt solution is not guarantee to end at the desired final state. # so we add a final phase here, with a smooth motion from the final state atteined by timeopt to the desired one connectFinalPhase(cs_com.contact_phases[-1]) return cs_com
def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=None): if cs_initGuess: print "WARNING : in centroidal.geometric, initial guess is ignored." cs_result = ContactSequenceHumanoid(cs) p0 = cs_result.contact_phases[0] com_z = p0.init_state[2, 0] previous_phase = None # first, compute the new init/final position for each state : in the center of the support polygon for pid in range(1, cs_result.size() - 1): phase = cs_result.contact_phases[pid] state = phase.init_state com_x = 0. com_y = 0. if phase.LF_patch.active: com_x += phase.LF_patch.placement.translation[0, 0] com_y += phase.LF_patch.placement.translation[1, 0] if phase.RF_patch.active: com_x += phase.RF_patch.placement.translation[0, 0] com_y += phase.RF_patch.placement.translation[1, 0] if phase.LH_patch.active: com_x += phase.LH_patch.placement.translation[0, 0] com_y += phase.LH_patch.placement.translation[1, 0] if phase.RH_patch.active: com_x += phase.RH_patch.placement.translation[0, 0] com_y += phase.RH_patch.placement.translation[1, 0] com_x /= phase.numActivePatches() com_y /= phase.numActivePatches() # test : take com height from config found from planning : com_z = cs_result.contact_phases[pid].init_state[2, 0] state[0] = com_x state[1] = com_y state[2] = com_z phase.init_state = state #print "phase : "+str(pid)+" com Init = "+str(com_x)+","+str(com_y)+","+str(com_z) if previous_phase != None: previous_phase.final_state = phase.init_state.copy() previous_phase = cs_result.contact_phases[pid] # then, generate a straight line from init_state to final_state for each phase : t_total = 0. for pid in range(cs_result.size()): phase = cs_result.contact_phases[pid] duration = computePhaseDuration(cs_result, pid) com0 = phase.init_state[0:3] com1 = phase.final_state[0:3] vel = (com1 - com0) / duration am = np.matrix(np.zeros(3)).T t = 0. while t < duration - 0.0001: u = t / duration state = np.matrix(np.zeros(9)).T com = com0 * (1. - u) + com1 * (u) state[0:3] = com state[3:6] = vel state[6:9] = am phase.state_trajectory.append(state) phase.time_trajectory.append(t_total) t += cfg.SOLVER_DT t_total += cfg.SOLVER_DT state[0:3] = com1 state[3:6] = vel state[6:9] = am phase.state_trajectory.append(state) phase.time_trajectory.append(t_total) return cs_result
def contactSequenceFromRBPRMConfigs(fb, beginId, endId): print "generate contact sequence from planning : " n_states = endId - beginId + 1 # There could be either contact break, creation or repositionning between each adjacent states. # But there should be only contacts break or creation between each adjacent contactPhases cs = ContactSequenceHumanoid(0) prev_phase = None phaseId = 0 # for each contact state we must create 2 phase (one with all the contact and one with the next replaced contact(s) broken) for stateId in range(beginId, endId + 1): if VERBOSE: print "current state id = ", stateId # %%%%%%%%% add phase with all the contacts in the rbprm State: %%%%%%%%%%%%% phase = ContactPhaseHumanoid() current_config = fb.getConfigAtState(stateId) fb.setCurrentConfig(current_config) # Determine the contact changes which are going to occur : if stateId < endId: next_variations = fb.getContactsVariations(stateId, stateId + 1) else: next_variations = [] if VERBOSE: print "variations : ", next_variations assert len( next_variations ) <= 1, "Several changes of contacts in adjacent states, not implemented yet !" if len(next_variations) == 0: if VERBOSE: print "no variations !" movingLimb = None contact_break = False contact_create = False else: movingLimb = next_variations[0] contact_break = fb.isLimbInContact(movingLimb, stateId) contact_create = fb.isLimbInContact(movingLimb, stateId + 1) if contact_break and contact_create: contact_reposition = True else: contact_reposition = False if VERBOSE: print "movingLimb = ", movingLimb print "break = " + str(contact_break) + " ; create = " + str( contact_create) + " ; reposition = " + str(contact_reposition) if movingLimb or stateId == endId: # add a ContactPhase corresponding to the current state # Build the phase contact patches (placement and activity) setContactActivityFromRBRMState(phase, fb, stateId) if not prev_phase: setContactPlacementFromRBPRMState(phase, fb, stateId) else: copyPhaseContactPlacements(prev_phase, phase) # now we change the contacts that have moved : previous_variations = fb.getContactsVariations( stateId - 1, stateId) setContactPlacementFromRBPRMState(phase, fb, stateId, previous_variations) # assign current wholebody config as reference in phase : phase.reference_configurations.append( np.matrix((current_config)).T) # retrieve the COM position for init and final state init_state = np.matrix(np.zeros(9)).T init_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose() init_state[3:6] = np.matrix(current_config[-6:-3]).transpose() phase.init_state = init_state final_state = init_state.copy() if not contact_reposition and stateId < endId: # in the case of contact reposition, the CoM motion will take place in the next intermediate phase. For the current phase the init and final state are equals current_config = fb.getConfigAtState(stateId + 1) fb.setCurrentConfig(current_config) final_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose() final_state[3:6] = np.matrix(current_config[-6:-3]).transpose() phase.final_state = final_state # add phase to contactSequence : cs.contact_phases.append(phase) phaseId += 1 prev_phase = phase if VERBOSE: print "add a phase at id : ", phaseId - 1 if contact_reposition: # %%%%%% create intermediate state, by removing the contact repositionned betwen stateId and stateId+1 %%%%%%%% phase = ContactPhaseHumanoid() # copy previous placement : copyPhaseContacts(prev_phase, phase) # find the contact to break : patch = contactPatchForEffector(phase, fb.dict_limb_joint[movingLimb]) patch.active = False # assign current wholebody config as reference in phase : phase.reference_configurations.append( np.matrix((current_config)).T) # retrieve the COM position for init and final state phase.init_state = prev_phase.final_state.copy() final_state = phase.init_state.copy() current_config = fb.getConfigAtState(stateId + 1) fb.setCurrentConfig(current_config) final_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose() final_state[3:6] = np.matrix(current_config[-6:-3]).transpose() phase.final_state = final_state.copy() # add phase to contactSequence : cs.contact_phases.append(phase) phaseId += 1 prev_phase = phase if VERBOSE: print "add an intermediate contact phase at id : ", phaseId - 1 # end adding intermediate contact phase # end if movingLimb or stateId == endId # end for each stateId # assign contact models : # only used by muscod ?? But we need to fill it otherwise the serialization fail for k, phase in enumerate(cs.contact_phases): RF_patch = phase.RF_patch cm = RF_patch.contactModel cm.mu = cfg.MU cm.ZMP_radius = 0.01 RF_patch.contactModelPlacement = SE3.Identity() LF_patch = phase.LF_patch cm = LF_patch.contactModel cm.mu = cfg.MU cm.ZMP_radius = 0.01 LF_patch.contactModelPlacement = SE3.Identity() LH_patch = phase.LH_patch cm = LH_patch.contactModel cm.mu = cfg.MU cm.ZMP_radius = 0.01 LH_patch.contactModelPlacement = SE3.Identity() RH_patch = phase.RH_patch cm = RH_patch.contactModel cm.mu = cfg.MU cm.ZMP_radius = 0.01 RH_patch.contactModelPlacement = SE3.Identity() return cs
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 contactSequenceFromRBPRMConfigs(fb, configs, beginId, endId): print "generate contact sequence from planning : " global i_sphere i_sphere = 0 #print "MR offset",MRsole_offset #print "ML offset",MLsole_offset n_double_support = len(configs) # config only contains the double support stance n_steps = n_double_support * 2 - 1 # Notice : what we call double support / simple support are in fact the state with all the contacts and the state without the next moving contact cs = ContactSequenceHumanoid(n_steps) unusedPatch = cs.contact_phases[0].LF_patch.copy() unusedPatch.placement = SE3.Identity() unusedPatch.active = False # for each contact state we must create 2 phase (one with all the contact and one with the next replaced contact(s) broken) for stateId in range(beginId, endId + 1): # %%%%%%%%% all the contacts : %%%%%%%%%%%%% cs_id = (stateId - beginId) * 2 config_id = stateId - beginId phase_d = cs.contact_phases[cs_id] fb.setCurrentConfig(configs[config_id]) """ init_guess_for_phase = init_guess_provided if init_guess_for_phase: c_init_guess = curves_initGuess[config_id] t_init_guess = timings_initGuess[config_id] init_guess_for_phase = isinstance(c_init_guess,bezier) if init_guess_for_phase: print "bezier curve provided for config id : "+str(config_id) """ # compute MRF and MLF : the position of the contacts q_rl = fb.getJointPosition(fb.rfoot) q_ll = fb.getJointPosition(fb.lfoot) q_rh = fb.getJointPosition(fb.rhand) q_lh = fb.getJointPosition(fb.lhand) # feets MRF = SE3.Identity() MLF = SE3.Identity() MRF.translation = np.matrix(q_rl[0:3]).T MLF.translation = np.matrix(q_ll[0:3]).T if not cfg.FORCE_STRAIGHT_LINE: rot_rl = quatFromConfig(q_rl) rot_ll = quatFromConfig(q_ll) MRF.rotation = rot_rl.matrix() MLF.rotation = rot_ll.matrix() # apply the transform ankle -> center of contact MRF *= fb.MRsole_offset MLF *= fb.MLsole_offset # hands MRH = SE3() MLH = SE3() MRH.translation = np.matrix(q_rh[0:3]).T MLH.translation = np.matrix(q_lh[0:3]).T rot_rh = quatFromConfig(q_rh) rot_lh = quatFromConfig(q_lh) MRH.rotation = rot_rh.matrix() MLH.rotation = rot_lh.matrix() MRH *= fb.MRhand_offset MLH *= fb.MLhand_offset phase_d.RF_patch.placement = MRF phase_d.LF_patch.placement = MLF phase_d.RH_patch.placement = MRH phase_d.LH_patch.placement = MLH # initial state : Set all new contacts patch (either with placement computed below or unused) if stateId == beginId: # FIXME : for loop ? how ? if fb.isLimbInContact(fb.rLegId, stateId): phase_d.RF_patch.active = True else: phase_d.RF_patch.active = False if fb.isLimbInContact(fb.lLegId, stateId): phase_d.LF_patch.active = True else: phase_d.LF_patch.active = False if fb.isLimbInContact(fb.rArmId, stateId): phase_d.RH_patch.active = True else: phase_d.RH_patch.active = False if fb.isLimbInContact(fb.lArmId, stateId): phase_d.LH_patch.active = True else: phase_d.LH_patch.active = False else: # we need to copy the unchanged patch from the last simple support phase (and not create a new one with the same placement) phase_d.RF_patch = phase_s.RF_patch phase_d.RF_patch.active = fb.isLimbInContact(fb.rLegId, stateId) phase_d.LF_patch = phase_s.LF_patch phase_d.LF_patch.active = fb.isLimbInContact(fb.lLegId, stateId) phase_d.RH_patch = phase_s.RH_patch phase_d.RH_patch.active = fb.isLimbInContact(fb.rArmId, stateId) phase_d.LH_patch = phase_s.LH_patch phase_d.LH_patch.active = fb.isLimbInContact(fb.lArmId, stateId) # now we change the contacts that have moved : variations = fb.getContactsVariations(stateId - 1, stateId) if len(variations) != 1: print "Several contact changes between states " + str( stateId - 1) + " and " + str(stateId) + " : " + str(variations) assert len( variations ) == 1, "Several changes of contacts in adjacent states, not implemented yet !" for var in variations: # FIXME : for loop in variation ? how ? if var == fb.lLegId: phase_d.LF_patch.placement = MLF if var == fb.rLegId: phase_d.RF_patch.placement = MRF if var == fb.lArmId: phase_d.LH_patch.placement = MLH if var == fb.rArmId: phase_d.RH_patch.placement = MRH # retrieve the COM position for init and final state (equal for double support phases) init_state = np.matrix(np.zeros(9)).T init_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose() init_state[3:6] = np.matrix(configs[config_id][-6:-3]).transpose() final_state = init_state.copy() #phase_d.time_trajectory.append((fb.getDurationForState(stateId))*cfg.DURATION_n_CONTACTS/cfg.SPEED) phase_d.init_state = init_state phase_d.final_state = final_state phase_d.reference_configurations.append( np.matrix((configs[config_id])).T) #print "done for double support" if stateId < endId: # %%%%%% simple support : %%%%%%%% phase_s = cs.contact_phases[cs_id + 1] # copy previous placement : phase_s.RF_patch = phase_d.RF_patch phase_s.LF_patch = phase_d.LF_patch phase_s.RH_patch = phase_d.RH_patch phase_s.LH_patch = phase_d.LH_patch # find the contact to break : variations = fb.getContactsVariations(stateId, stateId + 1) if len(variations) != 1: print "Several contact changes between states " + str( stateId) + " and " + str(stateId + 1) + " : " + str(variations) assert len( variations ) == 1, "Several changes of contacts in adjacent states, not implemented yet !" for var in variations: if var == fb.lLegId: phase_s.LF_patch.active = False if var == fb.rLegId: phase_s.RF_patch.active = False if var == fb.lArmId: phase_s.LH_patch.active = False if var == fb.rArmId: phase_s.RH_patch.active = False # retrieve the COM position for init and final state phase_s.reference_configurations.append( np.matrix((configs[config_id])).T) init_state = phase_d.init_state.copy() final_state = phase_d.final_state.copy() fb.setCurrentConfig(configs[config_id + 1]) final_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose() final_state[3:6] = np.matrix(configs[config_id + 1][-6:-3]).transpose() #phase_s.time_trajectory.append((fb.getDurationForState(stateId))*(1-cfg.DURATION_n_CONTACTS)/cfg.SPEED) phase_s.init_state = init_state.copy() phase_s.final_state = final_state.copy() #print "done for single support" # assign contact models : # only used by muscod ?? But we need to fill it otherwise the serialization fail for k, phase in enumerate(cs.contact_phases): RF_patch = phase.RF_patch cm = RF_patch.contactModel cm.mu = cfg.MU cm.ZMP_radius = 0.01 RF_patch.contactModelPlacement = SE3.Identity() LF_patch = phase.LF_patch cm = LF_patch.contactModel cm.mu = cfg.MU cm.ZMP_radius = 0.01 LF_patch.contactModelPlacement = SE3.Identity() LH_patch = phase.LH_patch cm = LH_patch.contactModel cm.mu = cfg.MU cm.ZMP_radius = 0.01 LH_patch.contactModelPlacement = SE3.Identity() RH_patch = phase.RH_patch cm = RH_patch.contactModel cm.mu = cfg.MU cm.ZMP_radius = 0.01 RH_patch.contactModelPlacement = SE3.Identity() return cs
def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=None): if cs_initGuess: print "WARNING : in centroidal.quasiStatic, initial guess is ignored." if not fullBody: raise ValueError("quasiStatic called without fullBody object.") beginId, endId = createFullbodyStatesFromCS(cs, fullBody) print "beginid = ", beginId print "endId = ", endId cs_result = ContactSequenceHumanoid(cs) def getPhaseDuration(sid, pid): if sid == endId: duration = cfg.DURATION_FINAL if pid == 0: duration = cfg.DURATION_INIT elif cs.contact_phases[pid].numActivePatches() == 1: duration = cfg.DURATION_SS elif cs.contact_phases[pid].numActivePatches() == 2: duration = cfg.DURATION_DS elif cs.contact_phases[pid].numActivePatches() == 3: duration = cfg.DURATION_TS elif cs.contact_phases[pid].numActivePatches() == 4: duration = cfg.DURATION_QS return duration # for each phase in the cs, create a corresponding FullBody State and call CROC, # then discretize the solution and fill the cs struct # Make the assumption that the CS was created with the generateContactSequence method from the same fb object id_phase = 0 for id_state in range(beginId, endId + 1): print "id_state = ", str(id_state) print "id_phase = ", str(id_phase) phase_fixed = cs_result.contact_phases[ id_phase] # phase where the CoM move and the contacts are fixed # set initial state to be the final one of the previous phase : if id_phase > 1: phase_fixed.init_state = phase_swing.final_state.copy() current_t = phase_swing.time_trajectory[-1] else: current_t = 0. # compute 'optimal' position of the COM to go before switching phase: if id_state == endId: c = phase_fixed.final_state[0:3] else: c = getTargetCOMPosition(fullBody, id_state) # fill phase trajectories to connect to 'c' moveToCOMPosition(phase_fixed, c, current_t, getPhaseDuration(id_state, id_phase)) id_phase += 1 if id_state < endId: current_t = phase_fixed.time_trajectory[-1] phase_swing = cs_result.contact_phases[ id_phase] # phase where the CoM is fixed and an effector move # in swing phase, com do not move : phase_swing.init_state = phase_fixed.final_state.copy() phase_swing.final_state = phase_fixed.final_state.copy() fillPhaseTrajWithZeros(phase_swing, current_t, getPhaseDuration(id_state, id_phase)) id_phase += 1 return cs_result
def generateCentroidalTrajectory(cs,cs_initGuess = None, fullBody = None, viewer = None): cs_res = ContactSequenceHumanoid(0) filename = cfg.CONTACT_SEQUENCE_PATH + "/"+cfg.DEMO_NAME+"_COM.cs" cs_res.loadFromBinary(filename) print "Import contact sequence binary file : ",filename return cs_res