Skip to content

GitLab

Commit 0e2130bf authored by Steve Tonneau's avatar Steve Tonneau
Browse files

removing waypoint constraint in window planning

parent bdf9df88
Hide whitespace changes
Inline Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @ 0e2130bf
......@@ -111,6 +111,12 @@ module hpp
/// [z_inf, z_sup, y_inf, y_sup, x_inf, x_sup]
void boundSO3(in floatSeq limitszyx) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double projectStateToCOM(in unsigned short stateId, in floatSeq com) raises (Error);
/// Get Sample configuration by its id
/// \param sampleName name of the limb from which to retrieve a sample
/// \param sampleId id of the desired samples
......@@ -357,6 +363,13 @@ module hpp
/// \return projected configuration
floatSeq projectToCom(in double state, in floatSeq targetCom) raises (Error);
/// Retrieve the configuration at a given state
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the state does not exist.
/// \param state index of first state.
/// \return projected configuration
floatSeq getConfigAtState(in unsigned short state) raises (Error);
/// \param limb name of the limb for which the request aplies
/// \param state1 current state considered
/// \return whether the limb is in contact at this state
......
script/scenarios/ground_crouch_hyq_interp.py
View file @ 0e2130bf
......@@ -130,6 +130,6 @@ def play(frame_rate = 1./24.):
def saveAll(name):
saveAllData(fullBody, r, name)
#~ fullBody.exportAll(r, trajec, 'hole_hyq_t_var_04f_andrea');
#~ fullBody.exportAll(r, configs, 'obstacle_hyq_t_var_04f_contact_planning');
#~ fullBody.exportAll(r, configs, 'hole_hyq_t_var_04f_andrea_contact_planning');
#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @ 0e2130bf
......@@ -448,6 +448,21 @@ class FullBody (object):
def projectToCom(self, state, targetCom):
return self.client.rbprm.rbprm.projectToCom(state, targetCom)
## Returns the configuration at a given state
# Will fail if the index of the state does not exist.
# \param state index of state.
# \return state configuration
def getConfigAtState(self, state):
return self.client.rbprm.rbprm.getConfigAtState(state)
## Project a given state into a given COM position
# and update the state configuration.
# \param state index of first state.
# \param targetCom 3D vector for the com position
# \return whether the projection was successful
def projectStateToCOM(self, state, targetCom):
return self.client.rbprm.rbprm.projectStateToCOM(state, targetCom) > 0
## Given start and goal states
# generate a contact sequence over a list of configurations
#
......
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @ 0e2130bf
......@@ -92,8 +92,10 @@ def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, li
#~ print "num com constraints", len(COMConstraints)
return p, N, init_com, end_com, t_end_phases, cones, COMConstraints
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1],
reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
global lastspeed
use_window = max(0, min(use_window, (len(states) - 1) - (state_id + 2))) # can't use preview if last state is reached
assert( len(phase_dt) == 2 + use_window * 2 ), "phase_dt does not describe all phases"
......@@ -104,11 +106,13 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
p, N, init_com, end_com, t_end_phases, cones, COMConstraints = compute_state_info(fullBody,states, state_id, phase_dt[:2], mu, computeCones, limbsCOMConstraints)
if(use_window > 0):
init_waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
init_end_com = end_com[:]
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
param_constraints += [("waypoint_reached_constraint",(waypoint_time, waypoint))]
# 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)
p+=p1;
N+=N1;
......@@ -123,19 +127,30 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
print "end_phases", t_end_phases
timeelapsed = 0
if (profile):
#~ if (profile):
if (True):
start = time.clock()
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 (profile):
if (True):
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]
params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
global lastspeed
lastspeed = var_final['dc'][init_waypoint_time]
lastspeed = var_final['dc'][init_waypoint_time]
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:
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)
else:
lastspeed = np.array([0,0,0])
return var_final, params, timeelapsed
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None, use_window = 0):
......@@ -173,7 +188,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax = fig.add_subplot(111)
points = var_final['dc']
#~ print "points", points
ys = [norm(el) for el in points]
ys = [norm(el) * el[0] / abs(el[0]) for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
......@@ -184,7 +199,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['ddc']
ys = [norm(el) for el in points]
ys = [norm(el) * el[0] / abs(el[0]) for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
......@@ -210,9 +225,9 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
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)", len(comPosPerPhase[0])
print "(len(1)", len(comPosPerPhase[1])
print "(len(2)", len(comPosPerPhase[2])
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
......
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @ 0e2130bf
......@@ -133,8 +133,8 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
global errorid
global stat_data
fail = 0
#~ try:
if(True):
try:
#~ if(True):
times = [];
dt = 1000;
distance = __getTimes(fullBody, configs, i, time_scale)
......@@ -163,9 +163,9 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
global trajec_mil
frame_rate = 1./24.
frame_rate_andrea = 1./1000.
if(len(trajec) > 0):
frame_rate = 1./25.
frame_rate_andrea = 1./1001.
#~ if(len(trajec) > 0):
#~ frame_rate = 1./25.
#~ frame_rate_andrea = 1./1001.
new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, times, frame_rate)
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, times,frame_rate_andrea)
new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
......@@ -188,41 +188,41 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
stat_data["num_success"] += 1
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
#~ except hpperr as e:
#~ print "hpperr failed at id " + str(i) , e.strerror
#~ 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.strerror
#~ stat_data["error_optim_fail"] += 1
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
#~ except ValueError as e:
#~ print "ValueError failed at id " + str(i) , e
#~ stat_data["error_unknown"] += 1
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
#~ except IndexError as e:
#~ print "IndexError failed at id " + str(i) , e
#~ stat_data["error_unknown"] += 1
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
#~ except Exception as e:
#~ stat_data["error_unknown"] += 1
#~ stat_data["num_errors"] += 1
#~ print e
#~ errorid += [i]
#~ fail+=1
#~ except:
#~ stat_data["error_unknown"] += 1
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
except hpperr as e:
print "hpperr failed at id " + str(i) , e.strerror
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.strerror
stat_data["error_optim_fail"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except ValueError as e:
print "ValueError failed at id " + str(i) , e
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except IndexError as e:
print "IndexError failed at id " + str(i) , e
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except Exception as e:
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
print e
errorid += [i]
fail+=1
except:
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 @ 0e2130bf
......@@ -476,6 +476,34 @@ namespace hpp {
}
double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com) throw (hpp::Error)
{
try
{
if(lastStatesComputed_.size() <= stateId)
{
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);
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.;
}
return 0;
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error)
{
......@@ -1190,6 +1218,17 @@ assert(s2 == s1 +1);
}
}
core::Configuration_t project_or_throw(rbprm::RbPrmFullBodyPtr_t fulllBody, ProblemPtr_t problem, const State& state, const fcl::Vec3f& targetCom)
{
bool success(false);
core::Configuration_t res = rbprm::interpolation::projectOnCom(fulllBody, problem,state,targetCom, success);
if(!success)
{
throw std::runtime_error("could not project state on COM constraint");
}
return res;
}
hpp::floatSeq* RbprmBuilder::comRRTFromPos(double state1,
const hpp::floatSeqSeq& rootPositions1,
const hpp::floatSeqSeq& rootPositions2,
......@@ -1214,19 +1253,19 @@ assert(s2 == s1 +1);
std::vector<State> states;
states.push_back(state1);
State s1Bis(state1);
s1Bis.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s1Bis,paths[0]->end().head<3>());
s1Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Bis,paths[0]->end().head<3>());
states.push_back(s1Bis);
State s1Ter(s1Bis);
s1Ter.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s1Ter,paths[1]->initial().head<3>());
s1Ter.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Ter,paths[1]->initial().head<3>());
states.push_back(s1Ter);
State s2Bis(state2);
s2Bis.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s2Bis,paths[1]->end().head<3>());
s2Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Bis,paths[1]->end().head<3>());
states.push_back(s2Bis);
State s2Ter(s2Bis);
s2Ter.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s2Ter,paths[2]->initial().head<3>());
s2Ter.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Ter,paths[2]->initial().head<3>());
states.push_back(s2Ter);
states.push_back(state2);
......@@ -1325,19 +1364,19 @@ assert(s2 == s1 +1);
std::vector<State> states;
states.push_back(state1);
State s1Bis(state1);
s1Bis.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s1Bis,paths[0]->end().head<3>());
s1Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Bis,paths[0]->end().head<3>());
states.push_back(s1Bis);
State s1Ter(s1Bis);
s1Ter.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s1Ter,paths[1]->initial().head<3>());
s1Ter.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Ter,paths[1]->initial().head<3>());
states.push_back(s1Ter);
State s2Bis(state2);
s2Bis.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s2Bis,paths[1]->end().head<3>());
s2Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Bis,paths[1]->end().head<3>());
states.push_back(s2Bis);
State s2Ter(s2Bis);
s2Ter.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s2Ter,paths[2]->initial().head<3>());
s2Ter.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Ter,paths[2]->initial().head<3>());
states.push_back(s2Ter);
states.push_back(state2);
......@@ -1404,7 +1443,30 @@ assert(s2 == s1 +1);
}
model::Configuration_t config = dofArrayToConfig (std::size_t(3), targetCom);
fcl::Vec3f comTarget; for(int i =0; i<3; ++i) comTarget[i] = config[i];
model::Configuration_t res = interpolation::projectOnCom(fullBody_,problemSolver_->problem(), lastStatesComputed_[state],comTarget);
model::Configuration_t res = project_or_throw(fullBody_,problemSolver_->problem(), lastStatesComputed_[state],comTarget);
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(res.rows());
for(std::size_t i=0; i< res.rows(); ++i)
{
(*dofArray)[(_CORBA_ULong)i] = res[i];
}
return dofArray;
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
hpp::floatSeq* RbprmBuilder::getConfigAtState(unsigned short state) throw (hpp::Error)
{
try
{
if(lastStatesComputed_.size () < state)
{
throw std::runtime_error ("did not find a state at indicated index: " + std::string(""+(std::size_t)(state)));
}
model::Configuration_t res = lastStatesComputed_[state].configuration_;
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(res.rows());
for(std::size_t i=0; i< res.rows(); ++i)
......
src/rbprmbuilder.impl.hh
View file @ 0e2130bf
......@@ -164,6 +164,8 @@ namespace hpp {
const hpp::floatSeqSeq& rootPositions3,
unsigned short numOptimizations) 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);
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);
virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
......
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