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 = []