def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=None): if cs_initGuess: print "WARNING : in current implementation of timeopt.generateCentroidalTrajectory the initial guess is ignored. (TODO)" q_init = cs.contact_phases[0].reference_configurations[0].copy() num_phases = cs.size() ee_ids = [ timeopt.EndeffectorID.RF, timeopt.EndeffectorID.LF, timeopt.EndeffectorID.RH, timeopt.EndeffectorID.LH ] effectors_phases, size = extractAllEffectorsPhasesFromCS( cs, cs_initGuess, ee_ids) #print "final dic : ",effectors_phases print "final number of phases : ", size # initialize timeopt problem : tp = timeopt.problem(size) com_init = cs.contact_phases[0].init_state[:3] vel_init = cs.contact_phases[0].init_state[3:6] com_end = cs.contact_phases[-1].final_state[:3] com_init[2] += cfg.COM_SHIFT_Z com_end[2] += cfg.COM_SHIFT_Z tp.setInitialCOM(com_init) tp.setInitialLMOM(vel_init * cfg.MASS) tp.setFinalCOM(com_end) p0 = cs.contact_phases[0] for ee in ee_ids: patch = getPhasePatchforEE(p0, ee) tp.setInitialPose(isContactEverActive(cs, ee), patch.placement.translation, patch.placement.rotation, ee) tp.setMass(cfg.MASS) #FIXME tp.getMass() #add all effector phases to the problem : i = 0 for ee in effectors_phases.keys(): for phase in effectors_phases[ee]: tp.setPhase( i, timeopt.phase(ee, phase[0], phase[1], phase[2].translation, phase[2].rotation)) i += 1 if cfg.USE_WP_COST: addCOMviapoints(tp, cs_initGuess, viewer) cfg_path = cfg.TIME_OPT_CONFIG_PATH + '/' + cfg.TIMEOPT_CONFIG_FILE print "set configuration file for time-optimization : ", cfg_path tp.setConfigurationFile(cfg_path) tp.setTimeoptSolver(cfg_path) tStart = time.time() tp.solve() tTimeOpt = time.time() - tStart print "timeopt problem solved in : " + str(tTimeOpt) + " s" print "write results in cs" cs_result = fillCSFromTimeopt(cs, cs_initGuess, tp) if cfg.TIME_SHIFT_COM > 0: cs_result = addInitAndGoalShift(cs_result) return cs_result
rot = np.matrix(np.zeros((3, 3))) rot[0, 0] = 1.0 rot[1, 1] = 1.0 rot[2, 2] = 1.0 tp.setInitialCOM(cs.contact_phases[0].init_state[:3]) tp.setInitialPose(True, np.matrix((0.086, 0.15,-0.92)).transpose(), rot, timeopt.EndeffectorID.RF) tp.setInitialPose(True, np.matrix((-0.086, 0.15,-0.92)).transpose(), rot, timeopt.EndeffectorID.LF) tp.setInitialPose(True, np.matrix((0.4, 0.3, 0.0)).transpose(), rot, timeopt.EndeffectorID.RH) tp.setInitialPose(True, np.matrix((-0.4, 0.3, 0.0)).transpose(), rot, timeopt.EndeffectorID.LH) tp.setMass(88.0); tp.getMass(); tp.setFinalCOM(cs.contact_phases[num_phases-1].final_state[0:3]) for i in range(len(RF)): tp.setPhase(i, timeopt.phase(timeopt.EndeffectorID.RF, RF_time[i][0, 0], RF_time[i][0, 1], RF[i].translation, RF[i].rotation)) for i in range(len(LF)): tp.setPhase(i + len(RF), timeopt.phase(timeopt.EndeffectorID.LF, LF_time[i][0, 0], LF_time[i][0, 1], LF[i].translation, LF[i].rotation)) print "set configuration file for time-optimization" import os cfg_path=str(os.path.dirname(os.path.abspath(__file__))) + '/../config/' + 'cfg_timeopt_demo01.yaml' tp.setConfigurationFile(cfg_path) tp.setTimeoptSolver(cfg_path) tp.solve() state=[]; s_time=0.0; cnt = 0; for k in range(tp.getTrajectorySize()):
timeopt.EndeffectorID.RF) tp.setInitialPose(True, np.matrix((-0.086, 0.15, -0.92)).transpose(), rot, timeopt.EndeffectorID.LF) tp.setInitialPose(True, np.matrix((0.4, 0.3, 0.0)).transpose(), rot, timeopt.EndeffectorID.RH) tp.setInitialPose(True, np.matrix((-0.4, 0.3, 0.0)).transpose(), rot, timeopt.EndeffectorID.LH) tp.setMass(60.0) tp.setFinalCOM(np.matrix((0.2, 1.2, 0.4)).transpose()) tp.setPhase( 0, timeopt.phase(timeopt.EndeffectorID.RF, 0.0, 1.0, np.matrix((0.086, 0.15, -0.92)).transpose(), rot)) tp.setPhase( 1, timeopt.phase(timeopt.EndeffectorID.RF, 2.0, 4.5, np.matrix((0.500, 0.45, -0.76)).transpose(), rot)) tp.setPhase( 2, timeopt.phase(timeopt.EndeffectorID.RF, 6.0, 9.9, np.matrix((0.450, 0.98, -0.27)).transpose(), rot)) tp.setPhase( 3, timeopt.phase(timeopt.EndeffectorID.LF, 0.0, 2.5, np.matrix((-0.086, 0.15, -0.92)).transpose(), rot)) tp.setPhase( 4, timeopt.phase(timeopt.EndeffectorID.LF, 4.0, 6.5,
timeopt.EndeffectorID.LF) tp.setInitialPose(True, np.matrix((0.4, 0.3, 0.0)).transpose(), rot, timeopt.EndeffectorID.RH) tp.setInitialPose(True, np.matrix((-0.4, 0.3, 0.0)).transpose(), rot, timeopt.EndeffectorID.LH) tp.setMass(60.0) tp.getMass() tp.setFinalCOM(cs.contact_phases[num_phases - 1].final_state[0:3]) for i in range(len(RF)): tp.setPhase( i, timeopt.phase(timeopt.EndeffectorID.RF, RF_time[i][0, 0], RF_time[i][0, 1], RF[i].translation, RF[i].rotation)) for i in range(len(LF)): tp.setPhase( i + len(RF), timeopt.phase(timeopt.EndeffectorID.LF, LF_time[i][0, 0], LF_time[i][0, 1], LF[i].translation, LF[i].rotation)) print "set configuration file for time-optimization" import os cfg_path = str(os.path.dirname( os.path.abspath(__file__))) + '/../config/' + 'cfg_momSc_demo03.yaml' tp.setConfigurationFile(cfg_path) tp.setTimeoptSolver(cfg_path) tp.solve() state = []