Skip to content
GitLab
Commit ee2e2731 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

[BUG FIX] wrong index computation in integrate for generateComTrajectory

parent 1b30767e
Hide whitespace changes
Inline Side-by-side
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @ ee2e2731
......@@ -124,19 +124,22 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
start = time.clock()
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)
print "end_com ", end_com , "computed end come", var_final['c'][-1], var_final['c_end']
#~ print "end_com ", end_com , "computed end come", var_final['c'][-1], var_final['c_end']
if (profile):
end = time.clock()
timeelapsed = (end - start) * 1000
#~ print "solving time", timeelapsed
if(use_window > 0):
var_final['c'] = var_final['c'][:init_waypoint_time+1]
var_final['dc'] = var_final['dc'][:init_waypoint_time+1]
var_final['ddc'] = var_final['ddc'][:init_waypoint_time+1]
params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
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())):
print "PROJECTED", init_end_com, var_final['c'][-1]
states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
lastspeed = var_final['dc'][init_waypoint_time]
print "init speed", lastspeed
else:
print "reached com is not good, restarting problem with 0 window"
return gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt[:2], reduce_ineq, verbose, limbsCOMConstraints, profile, use_window = 0)
......@@ -204,18 +207,21 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
plt.show()
return var_final, params, elapsed
def __cVarPerPhase(var, dt, t, final_state, addValue, isAcceleration = False):
def __cVarPerPhase(var, dt, t, final_state, addValue):
varVals = addValue + [v.tolist() for v in final_state[var]]
varPerPhase = [[varVals[(int)(t_id/dt) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
varPerPhase = [[varVals[(int)(round(t_id/dt)) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
varPerPhase[2].append(varVals[-1])
if(not isAcceleration):
assert len(varVals) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), "incorrect num of c or dc"
if(not var == "ddc"):
assert len(varVals) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), mess
varPerPhase[0].append(varPerPhase[1][0]) # duplicate position
varPerPhase[1].append(varPerPhase[2][0])
if(isAcceleration): #acceleration: remove first
if var == "dc":
varPerPhase[2] = varPerPhase[2][:-1] # not relevant for computation
else:
varPerPhase[0].append(varPerPhase[1][0]) # end pos of state is the same as the previous one
varPerPhase[1].append(varPerPhase[2][0])
if var == "ddc": #acceleration: remove first
varPerPhase = [v[1:] for v in varPerPhase]
assert len(final_state[var]) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), "incorrect num of ddc"
assert len(final_state[var]) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), "incorrect num of ddc"
return varPerPhase
def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True,
......@@ -232,8 +238,8 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
dc0 = res[1]["x_init"][3:7]
comPosPerPhase = __cVarPerPhase('c' , dt, t, final_state, [c0])
comVelPerPhase = __cVarPerPhase('dc' , dt, t, final_state, [dc0])
comAccPerPhase = __cVarPerPhase('ddc', dt, t, final_state, [[0,0,0]], isAcceleration = True)
comAccPerPhase = __cVarPerPhase('ddc', dt, t, final_state, [[0,0,0]])
#now compute com trajectorirs
comTrajIds = [fullBody.generateComTraj(comPosPerPhase[i], comVelPerPhase[i], comAccPerPhase[i], dt) for i in range(0,3)]
return comTrajIds, res[2] #res[2] is timeelapsed
......
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @ ee2e2731
......@@ -189,22 +189,22 @@ trackedEffectors = []):
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
except hpperr as e:
#~ print "hpperr failed at id " + str(i) , e.strerror
#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
#~ print "could not project com, trying to increase velocity "
#~ try:
#~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
#~ except:
#~ if ((len(configs) - 1) - (i + 3) > 0):
#~ print "could not project com, trying to increase velocity more "
#~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
#~ else:
print "In hpperr and window != 0"
print "hpperr failed at id " + str(i) , e.strerror
if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
print "could not project com, trying to increase velocity "
try:
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
except:
if ((len(configs) - 1) - (i + 3) > 0):
print "could not project com, trying to increase velocity more "
step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
else:
print "In hpperr and window != 0"
print "hpperr failed at id " + str(i) , e.strerror
stat_data["error_com_proj"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
stat_data["error_com_proj"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except OptimError as e:
print "OptimError failed at id " + str(i) , e
stat_data["error_optim_fail"] += 1
......@@ -223,40 +223,40 @@ trackedEffectors = []):
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
except Exception as e:
print e
if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
print "could not project com, trying to increase velocity "
try:
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
except:
print "faile twice"
if ((len(configs) - 1) - (i + 3) > 0):
print "could not project com, trying to increase velocity more "
step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
else:
print "In Exception and window != 0"
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
print e
errorid += [i]
fail+=1
#~ except Exception as e:
#~ print e
#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
#~ print "could not project com, trying to increase velocity "
#~ try:
#~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
#~ except:
#~ print "faile twice"
#~ if ((len(configs) - 1) - (i + 3) > 0):
#~ print "could not project com, trying to increase velocity more "
#~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
#~ else:
#~ print "In Exception and window != 0"
#~ stat_data["error_unknown"] += 1
#~ stat_data["num_errors"] += 1
#~ print e
#~ errorid += [i]
#~ fail+=1
except:
print "unknown"
if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
print "could not project com, trying to increase velocity "
try:
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
except:
if ((len(configs) - 1) - (i + 3) > 0):
print "could not project com, trying to increase velocity more "
step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
else:
print "In unknown and window != 0"
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
#~ print "could not project com, trying to increase velocity "
#~ try:
#~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
#~ except:
#~ if ((len(configs) - 1) - (i + 3) > 0):
#~ print "could not project com, trying to increase velocity more "
#~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
#~ else:
print "In unknown and window != 0"
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
return fail
def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False):
......
src/rbprmbuilder.impl.cc
View file @ ee2e2731
......@@ -568,7 +568,7 @@ namespace hpp {
}
double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com) throw (hpp::Error)
double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target) throw (hpp::Error)
{
try
{
......@@ -576,7 +576,6 @@ namespace hpp {
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
model::Configuration_t com_target = dofArrayToConfig (3, com);
State s = lastStatesComputed_[stateId];
bool succes (false);
hpp::model::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s,com_target,succes);
......@@ -594,6 +593,12 @@ namespace hpp {
}
}
double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com) throw (hpp::Error)
{
model::Configuration_t com_target = dofArrayToConfig (3, com);
return projectStateToCOMEigen(stateId, com_target);
}
hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error)
{
......@@ -1131,7 +1136,7 @@ namespace hpp {
throw std::runtime_error("generateComTraj requires at least 2 configurations to generate path");
}
core::PathVectorPtr_t res = core::PathVector::create(3, 3);
if(cdd.size() != c.size()-1 || cd.size() != c.size())
if(cdd.size() != c.size()-1 || cdd.size() != cd.size())
{
std::cout << c.size() << " " << cd.size() << " " << cdd.size() << std::endl;
throw std::runtime_error("in generateComTraj, positions and accelerations vector should have the same size");
......@@ -1141,6 +1146,7 @@ namespace hpp {
CIT_Configuration cddit = cdd.begin();
for(;cit != c.end(); ++cit, ++cdit, ++cddit)
{
std::cout << "is trajectory ok ?" << ((*cit) - (*(cit-1) + dt * (*cdit) + dt * dt * (*cddit) * 0.5)).norm() <<std::endl;
res->appendPath(interpolation::ComTrajectory::create(*(cit-1),*cit,*cdit,*cddit,dt));
}
return problemSolver_->addPath(res);
......@@ -1443,16 +1449,33 @@ assert(s2 == s1 +1);
{
throw std::runtime_error("in comRRTFromPos, at least one com trajectory is not present in problem solver");
}
const State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
model::Configuration_t oldConf = state1.configuration_;
if (projectStateToCOMEigen(s1,paths[cT1]->initial().head<3>()) == 0)
{
throw std::runtime_error("can not project com on initial state");
}
std::cout << "com projected ? " << (oldConf - state1.configuration_).norm() << std::endl;
oldConf = state2.configuration_;
if (projectStateToCOMEigen(s2,paths[cT3]->end().head<3>()) == 0)
{
throw std::runtime_error("can not project com on initial state");
}
std::cout << "com2 projected ? " << (oldConf - state2.configuration_).norm() << std::endl;
State s1Bis(state1);
std::cout << "wtf" << (paths[cT1]->end() - paths[cT1]->end()).norm() << std::endl;
s1Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Bis,paths[cT1]->end().head<3>());
State s2Bis(state2);
s2Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Bis,paths[cT2]->end().head<3>());
std::cout << "wtf2 " << (paths[cT2]->end() - paths[cT3]->initial()).norm() << std::endl;
core::PathVectorPtr_t resPath = core::PathVector::create(fullBody_->device_->configSize(), fullBody_->device_->numberDof());
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
fullBody_->device_->currentConfiguration(s1Bis.configuration_);
if(!(problemSolver_->problem()->configValidations()->validate(s1Bis.configuration_, rport)
......
src/rbprmbuilder.impl.hh
View file @ ee2e2731
......@@ -191,6 +191,7 @@ namespace hpp {
const hpp::Names_t& trackedEffectors) throw (hpp::Error);
virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error);
virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
double projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target)throw (hpp::Error);
virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
......
Supports Markdown
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