Commit 1d77cb63 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

com trajectory following now operational

parent ee2e2731
......@@ -6,7 +6,6 @@ from numpy.linalg import norm
import time
import hpp.corbaserver.rbprm.data.com_inequalities as ine
from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play
ineqPath = ine.__path__[0] +"/"
......@@ -250,7 +249,7 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
print "done. generating state trajectory ",state_id
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
print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
return paths_ids[-1], paths_ids[:-1], timeElapsed
def solve_effector_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
......@@ -264,6 +263,6 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
print "handling extra effector constraints"
refPathId = trackedEffectors[0]; path_start = trackedEffectors[1]; path_to = trackedEffectors[2]; effectorstracked = trackedEffectors[3]
paths_ids = [int(el) for el in fullBody.effectorRRTFromPath(state_id, refPathId, path_start, path_to, comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims, effectorstracked)]
print "done. computing final trajectory to display ",state_id
print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
return paths_ids[-1], paths_ids[:-1], timeElapsed
......@@ -2,6 +2,8 @@
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
from cwc import OptimError, cone_optimization
from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play
from numpy import append
#global variables
res = []
......@@ -68,25 +70,24 @@ def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times
interpassed = False
pRes = []
nRes = []
for path_id in path_ids:
for idx, path_id in enumerate(path_ids):
length = pp.client.problem.pathLength (path_id)
num_frames_required_fly = times[1] / dt_framerate
num_frames_required_support = times[0] / dt_framerate
dt_fly = float(length) / num_frames_required_fly
dt_support = float(length) / num_frames_required_support
dt_finals_fly = [dt_fly*i for i in range(int(num_frames_required_fly))] + [1]
dt_finals_support = [dt_support*i for i in range(int(num_frames_required_support))] + [1]
config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
interpassed = True
pRes+= [p[1] for t in dt_finals_fly]
nRes+= [N[1] for t in dt_finals_fly]
elif interpassed:
pRes+= [p[2] for t in dt_finals_support]
nRes+= [N[2] for t in dt_finals_support]
else:
pRes+= [p[0] for t in dt_finals_support]
nRes+= [N[0] for t in dt_finals_support]
num_frames_required = times[idx] / dt_framerate
print "dt_framerate", dt_framerate
print "num_frames_required", times[idx], " ", num_frames_required
dt = float(length) / num_frames_required
dt_finals = [dt*i for i in range(int(num_frames_required))] + [1]
#~ config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
#~ if(config_size_path > config_size):
#~ interpassed = True
pRes+= [p[idx] for t in dt_finals]
nRes+= [N[idx] for t in dt_finals]
#~ elif interpassed:
#~ pRes+= [p[2] for t in dt_finals_support]
#~ nRes+= [N[2] for t in dt_finals_support]
#~ else:
#~ pRes+= [p[0] for t in dt_finals_support]
#~ nRes+= [N[0] for t in dt_finals_support]
return pRes, nRes
......@@ -133,8 +134,8 @@ trackedEffectors = []):
global errorid
global stat_data
fail = 0
try:
#~ if(True):
#~ try:
if(True):
times = [];
dt = 1000;
distance = __getTimes(fullBody, configs, i, time_scale)
......@@ -143,7 +144,8 @@ trackedEffectors = []):
times2, dt2, dist2 = __getTimes(fullBody, configs, i+w, time_scale)
times += times2
dt = min(dt, dt2)
print 'time per path', times
time_per_path = [times[0]] + [times[1]] + [times [0]]
print 'time per path', times, time_per_path
print 'dt', dt
if(distance > 0.0001):
stat_data["num_trials"] += 1
......@@ -166,10 +168,10 @@ trackedEffectors = []):
#~ 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_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
Ps, Ns = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
Ps, Ns = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, frame_rate_andrea)
if(len(trajec) > 0):
new_traj = new_traj[1:]
new_traj_andrea = new_traj_andrea[1:]
......@@ -181,14 +183,16 @@ trackedEffectors = []):
#~ global contacts
#~ contacts += new_contacts
global pos
print "pos", len(pos), " ps, ", len(Ps)
pos += Ps
global normals
normals+= Ns
print len(trajec_mil), " ", len(pos), " ", len(normals)
assert(len(trajec_mil) == len(pos) and len(normals) == len(pos))
stat_data["num_success"] += 1
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
except hpperr as e:
#~ 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 "
......@@ -199,18 +203,18 @@ trackedEffectors = []):
#~ 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
except OptimError as e:
print "OptimError failed at id " + str(i) , e
stat_data["error_optim_fail"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
#~ 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
#~ except OptimError as e:
#~ print "OptimError failed at id " + str(i) , e
#~ 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
......@@ -241,8 +245,8 @@ trackedEffectors = []):
#~ print e
#~ errorid += [i]
#~ fail+=1
except:
print "unknown"
#~ except:
#~ print "unknown"
#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
#~ print "could not project com, trying to increase velocity "
#~ try:
......@@ -252,11 +256,11 @@ trackedEffectors = []):
#~ 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
#~ 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):
......
__24fps = 1. / 24.
__EPS = 0.0001
__EPS = 0.0000001
from numpy.linalg import norm
def __linear_interpolation(p0,p1,dist_p0_p1, val):
......@@ -53,6 +53,7 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
num_frames_required = total_time / dt_framerate
dt = float(length) / num_frames_required
#matches the extradof normalized
print "length ", length, "total tome ", total_time, "frame rate ", dt_framerate, "num_frames_required ", num_frames_required, "dt ", dt
dt_finals = [dt*i / length for i in range(int(num_frames_required))] + [1]
return[__find_q_t(robot, path_player, path_id, t) for t in dt_finals]
......@@ -61,12 +62,15 @@ def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, d
res = []
pp = path_player
activeid = 0
for path_id in path_ids:
for i, path_id in enumerate(path_ids):
config_size_path = len(path_player.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_paths[1], dt_framerate)
#~ if(i == 1 ):
print "a traj"
res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_paths[i], dt_framerate)
else:
res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_paths[0], dt_framerate)
print "a path"
res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_paths[i], dt_framerate)
activeid +=1
return res
......
......@@ -969,8 +969,6 @@ namespace hpp {
std::pair<stability::MatrixXX, stability::VectorX> cone = stability::ComputeCentroidalCone(fullBody, state, friction);
res->length ((_CORBA_ULong)cone.first.rows());
_CORBA_ULong size = (_CORBA_ULong) cone.first.cols()+1;
std::cout << "H cols" << cone.first.cols() << std::endl;
std::cout << "H rows" << cone.first.rows() << std::endl;
for(int i=0; i < cone.first.rows(); ++i)
{
double* dofArray = hpp::floatSeq::allocbuf(size);
......@@ -1010,8 +1008,7 @@ namespace hpp {
thirdState.contactBreaks(firstState, breaks);
if(breaks.size() > 1)
{
throw std::runtime_error ("too many contact breaks between states" + std::string(""+cId) +
"and " + std::string(""+(cId + 1)));
throw std::runtime_error ("too many contact breaks between states" + std::string(""+cId) + "and " + std::string(""+(cId + 1)));
}
if(breaks.size() == 1)
{
......@@ -1144,9 +1141,9 @@ namespace hpp {
CIT_Configuration cit = c.begin(); ++cit;
CIT_Configuration cdit = cd.begin();
CIT_Configuration cddit = cdd.begin();
for(;cit != c.end(); ++cit, ++cdit, ++cddit)
int i = 0;
for(;cit != c.end(); ++cit, ++cdit, ++cddit, ++i)
{
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);
......@@ -1181,8 +1178,7 @@ namespace hpp {
}
if(creations.size() > 1)
{
throw std::runtime_error ("too many contact creations between states" + std::string(""+cId) +
"and " + std::string(""+(cId + 1)));
throw std::runtime_error ("too many contact creations between states" + std::string(""+cId) + "and " + std::string(""+(cId + 1)));
}
hpp::floatSeqSeq *res;
......@@ -1237,8 +1233,7 @@ namespace hpp {
}
if(creations.size() > 1)
{
throw std::runtime_error ("too many contact creations between states" + std::string(""+cId) +
"and " + std::string(""+(cId + 1)));
throw std::runtime_error ("too many contact creations between states" + std::string(""+cId) + "and " + std::string(""+(cId + 1)));
}
hpp::floatSeqSeq *res;
......@@ -1451,26 +1446,11 @@ assert(s2 == s1 +1);
}
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());
......@@ -1485,14 +1465,19 @@ std::cout << "wtf2 " << (paths[cT2]->end() - paths[cT3]->initial()).norm() << s
//throw std::runtime_error ("could not project without collision at state " + s1 );
}
if(state1.configuration_ != s1Bis.configuration_)
{
core::PathPtr_t p1 = interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[cT1],
state1,s1Bis, numOptimizations,false);
resPath->appendPath(p1);
state1,s1Bis, numOptimizations,true);
// reduce path to remove extradof
core::SizeInterval_t interval(0, p1->initial().rows()-1);
core::SizeIntervals_t intervals;
intervals.push_back(interval);
PathPtr_t reducedPath = core::SubchainPath::create(p1,intervals);
resPath->appendPath(reducedPath);
pathsIds.push_back(AddPath(p1,problemSolver_));
}
{
core::PathPtr_t p2 =(*functor)(fullBody_,problemSolver_->problem(), paths[cT2],
s1Bis,s2Bis, numOptimizations,true);
......@@ -1505,15 +1490,20 @@ std::cout << "wtf2 " << (paths[cT2]->end() - paths[cT3]->initial()).norm() << s
resPath->appendPath(reducedPath);
}
if(s2Bis.configuration_ != state2.configuration_)
//if(s2Bis.configuration_ != state2.configuration_)
{
core::PathPtr_t p3= interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[cT3],
s2Bis,state2, numOptimizations,false);
resPath->appendPath(p3);
s2Bis,state2, numOptimizations,true);
// reduce path to remove extradof
core::SizeInterval_t interval(0, p3->initial().rows()-1);
core::SizeIntervals_t intervals;
intervals.push_back(interval);
PathPtr_t reducedPath = core::SubchainPath::create(p3,intervals);
resPath->appendPath(reducedPath);
pathsIds.push_back(AddPath(p3,problemSolver_));
}
pathsIds.push_back(AddPath(resPath,problemSolver_));
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(pathsIds.size());
for(std::size_t i=0; i< pathsIds.size(); ++i)
......
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