Commit d5809b31 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

cleaning

parent 0e2130bf
......@@ -110,7 +110,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
for w in range(1,use_window+1):
waypoint = end_com[:]
waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
print "waypoint_time", waypoint_time
#~ print "waypoint_time", waypoint_time
# trying not to apply constraint
#~ param_constraints += [("waypoint_reached_constraint",(waypoint_time, waypoint))]
p1, N1, init_com1, end_com1, t_end_phases1, cones1, COMConstraints1 = compute_state_info(fullBody,states, state_id+w, phase_dt[2*w:], mu, computeCones, limbsCOMConstraints)
......@@ -127,22 +127,21 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
print "end_phases", t_end_phases
timeelapsed = 0
#~ if (profile):
if (True):
if (profile):
start = time.clock()
print "init x", init_com + lastspeed.tolist()
#~ print "init x", init_com + lastspeed.tolist()
var_final, params = cone_optimization(p, N, [init_com + lastspeed.tolist(), end_com + [0,0,0]], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose,
constraints, param_constraints)
#~ if (profile):
if (True):
if (profile):
#~ if (True):
end = time.clock()
timeelapsed = (end - start) * 1000
print "solving time", timeelapsed
#~ print "solving time", timeelapsed
if(use_window > 0):
var_final['c'] = var_final['c'][:init_waypoint_time+1]
params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
lastspeed = var_final['dc'][init_waypoint_time]
print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
#~ print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
if (fullBody.projectStateToCOM(state_id+1, (var_final['c'][-1]).tolist())):
states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
else:
......@@ -224,11 +223,6 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
comPos = [c0] + [c.tolist() for c in final_state['c']]
comPosPerPhase = [[comPos[(int)(t_id/dt) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
comPosPerPhase[-1].append(comPos[-1])
print "(len(comPos)", len(comPos)
print "(len(0)", comPosPerPhase[0][0], comPosPerPhase[0][-1]
print "(len(1)", comPosPerPhase[1][0], comPosPerPhase[1][-1]
print "(len(2)", comPosPerPhase[2][0], comPosPerPhase[2][-1]
print "(len(sum)", len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2])
assert(len(comPos) == len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2]))
return comPosPerPhase, res[2] #res[2] is timeelapsed
......@@ -237,7 +231,6 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
print "done. generating state trajectory ",state_id
print "comePhaseLength", len(comPosPerPhase[0]), len(comPosPerPhase[1]), len(comPosPerPhase[2])
paths_ids = [int(el) for el in fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
print "done. computing final trajectory to display ",state_id
return paths_ids[-1], paths_ids[:-1], timeElapsed
......@@ -247,7 +240,6 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
print "done. generating state trajectory ",state_id
print "comePhaseLength", len(comPosPerPhase[0]), len(comPosPerPhase[1]), len(comPosPerPhase[2])
paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
print "done. computing final trajectory to display ",state_id
return paths_ids[-1], paths_ids[:-1], timeElapsed
......
......@@ -490,9 +490,7 @@ namespace hpp {
hpp::model::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s,com_target,succes);
if(succes)
{
std::cout << "updating config " << lastStatesComputed_[stateId].configuration_ << std::endl;
lastStatesComputed_[stateId].configuration_ = c;
std::cout << "updated config " << lastStatesComputed_[stateId].configuration_ << std::endl;
lastStatesComputedTime_[stateId].second.configuration_ = c;
return 1.;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment