bool DiscretizeConstrainedMultiPath(Robot& robot,const MultiPath& path,MultiPath& out,Real xtol)
{
if(path.settings.contains("resolution")) {
//see if the resolution is high enough to just interpolate directly
Real res=path.settings.as<Real>("resolution");
if(res <= xtol) {
out = path;
return true;
}
}
vector<GeneralizedCubicBezierSpline> paths;
if(!InterpolateConstrainedMultiPath(robot,path,paths,xtol))
return false;
out = path;
{
out.settings.set("resolution",xtol);
out.settings.set("program","DiscretizeConstrainedMultiPath");
}
Real tofs = (path.HasTiming() ? path.sections[0].times.front() : 0);
for(size_t i=0;i<out.sections.size();i++) {
out.sections[i].velocities.resize(0);
paths[i].GetPiecewiseLinear(out.sections[i].times,out.sections[i].milestones);
//shift section timing
Real tscale = (path.HasTiming() ? path.sections[i].times.back()-path.sections[i].times.front() : 1.0) / out.sections[i].times.back();
for(size_t j=0;j<out.sections[i].times.size();j++)
out.sections[i].times[j] = tofs + out.sections[i].times[j]*tscale;
tofs = out.sections[i].times.back();
}
return true;
}
void EvaluateMultiPath(Robot& robot,const MultiPath& path,Real t,Config& q,Real xtol,Real contactol,int numIKIters)
{
RobotCSpace space(robot);;
RobotGeodesicManifold manifold(robot);
GeneralizedCubicBezierCurve curve(&space,&manifold);
Real duration,param;
int seg=path.Evaluate(t,curve,duration,param,MultiPath::InterpLinear);
if(seg < 0) seg = 0;
if(seg >= path.sections.size()) seg = (int)path.sections.size()-1;
curve.Eval(param,q);
//solve for constraints
bool solveIK = false;
if(!path.settings.contains("resolution"))
solveIK = true;
else {
Real res = path.settings.as<Real>("resolution");
if(res > xtol) solveIK=true;
}
if(solveIK) {
vector<IKGoal> ik;
path.GetIKProblem(ik,seg);
if(!ik.empty()) {
swap(q,robot.q);
robot.UpdateFrames();
int iters=numIKIters;
bool res=SolveIK(robot,ik,contactol,iters,0);
if(!res) printf("Warning, couldn't solve IK problem at sec %d, time %g\n",seg,t);
swap(q,robot.q);
}
}
}
Real EuropeanMultiPathPricer::operator()(const MultiPath& multiPath)
const {
Size n = multiPath.pathSize();
QL_REQUIRE(n>0, "the path cannot be empty");
Size numAssets = multiPath.assetNumber();
QL_REQUIRE(numAssets>0, "there must be some paths");
Size j;
// calculate the final price of each asset
Array finalPrice(numAssets, 0.0);
for (j = 0; j < numAssets; j++)
finalPrice[j] = multiPath[j].back();
return (*payoff_)(finalPrice) * discount_;
}
Real EverestMultiPathPricer::operator()(const MultiPath& multiPath) const {
Size n = multiPath.pathSize();
QL_REQUIRE(n>0, "the path cannot be empty");
Size numAssets = multiPath.assetNumber();
QL_REQUIRE(numAssets>0, "there must be some paths");
// We search the yield min
Real minYield = multiPath[0].back() / multiPath[0].front() - 1.0;
for (Size j=1; j<numAssets; ++j) {
Rate yield = multiPath[j].back() / multiPath[j].front() - 1.0;
minYield = std::min(minYield, yield);
}
return (1.0 + minYield + guarantee_) * notional_ * discount_;
}
inline Real EuropeanHestonPathPricer::operator()(
const MultiPath& multiPath) const {
const Path& path = multiPath[0];
const Size n = multiPath.pathSize();
QL_REQUIRE(n>0, "the path cannot be empty");
return payoff_(path.back()) * discount_;
}
Real PagodaMultiPathPricer::operator()(const MultiPath& multiPath) const {
Size numAssets = multiPath.assetNumber();
Size numSteps = multiPath.pathSize();
Real averagePerformance = 0.0;
for (Size i = 1; i < numSteps; i++) {
for (Size j = 0; j < numAssets; j++) {
averagePerformance +=
multiPath[j].front() *
(multiPath[j][i]/multiPath[j][i-1] - 1.0);
}
}
averagePerformance /= numAssets;
return discount_ * fraction_
* std::max<Real>(0.0, std::min(roof_, averagePerformance));
}
bool ContactTimeScaling::Check(const MultiPath& path)
{
Robot& robot = cspace.robot;
//test at the colocation points
Assert(traj.timeScaling.times.size()==paramDivs.size());
Vector x,dx,ddx;
bool feasible = true;
for(size_t i=0;i<paramDivs.size();i++) {
int seg=paramSections[i];
Real t=traj.timeScaling.times[i];
traj.Eval(t,x);
traj.Deriv(t,dx);
traj.Accel(t,ddx);
for(int j=0;j<dx.n;j++)
if(Abs(dx[j]) > robot.velMax[j]*(1+Epsilon)) {
printf("Vel at param %d (time %g/%g) is infeasible\n",i,t,traj.timeScaling.times.back());
printf(" |%g| > %g at link %s\n",dx[j],robot.velMax[j],robot.LinkName(j).c_str());
feasible = false;
}
for(int j=0;j<ddx.n;j++)
if(Abs(ddx[j]) > robot.accMax[j]*(1+Epsilon)) {
printf("Acc at param %d (time %g/%g) is infeasible\n",i,t,traj.timeScaling.times.back());
printf(" |%g| > %g at link %s\n",ddx[j],robot.accMax[j],robot.LinkName(j).c_str());
feasible = false;
}
ContactFormation formation;
Stance stance;
path.GetStance(stance,seg);
ToContactFormation(stance,formation);
if(formation.contacts.empty()) continue;
robot.UpdateConfig(x);
robot.dq = dx;
TorqueSolver solver(robot,formation);
solver.SetGravity(Vector3(0,0,-9.8));
solver.SetDynamics(ddx);
bool res=solver.Solve();
if(!res) {
printf("TorqueSolver was not able to compute a solution at param %d (time %g/%g)\n",i,t,traj.timeScaling.times.back());
feasible = false;
continue;
}
for(int j=0;j<solver.t.n;j++) {
if(Abs(solver.t(j)) > robot.torqueMax(j)*(1+Epsilon)) {
printf("Torque at param %d (time %g/%g) is infeasible\n",i,t,traj.timeScaling.times.back());
printf(" |%g| > %g at link %s\n",solver.t(j),robot.torqueMax(j),robot.LinkName(j).c_str());
feasible = false;
}
}
}
return feasible;
}
Array AmericanBasketPathPricer::state(const MultiPath& path,
Size t) const {
QL_REQUIRE(path.assetNumber() == assetNumber_, "invalid multipath");
Array tmp(assetNumber_);
for (Size i=0; i<assetNumber_; ++i) {
tmp[i] = path[i][t]*scalingValue_;
}
return tmp;
}
/*
Extract the relevant information from the whole path
*/
LongstaffSchwartzMultiPathPricer::PathInfo
LongstaffSchwartzMultiPathPricer::transformPath(const MultiPath& multiPath)
const {
const Size numberOfAssets = multiPath.assetNumber();
const Size numberOfTimes = timePositions_.size();
Matrix path(numberOfAssets, numberOfTimes, Null<Real>());
for (Size i = 0; i < numberOfTimes; ++i) {
const Size pos = timePositions_[i];
for (Size j = 0; j < numberOfAssets; ++j)
path[j][i] = multiPath[j][pos];
}
PathInfo info(numberOfTimes);
payoff_->value(path, forwardTermStructures_, info.payments, info.exercises, info.states);
return info;
}
void ContactOptimizeTest2(const char* robfile,const char* config1,const char* config2)
{
Robot robot;
if(!robot.Load(robfile)) {
printf("Unable to load robot file %s\n",robfile);
return;
}
Vector a,b;
ifstream ia(config1,ios::in);
ifstream ib(config2,ios::in);
ia >> a;
ib >> b;
if(!ia || !ib) {
printf("Unable to load config file(s)\n");
return;
}
ia.close();
ib.close();
printf("Automatically detecting contacts...\n");
robot.UpdateConfig(a);
ContactFormation formation;
GetFlatContacts(robot,5e-3,formation);
printf("Assuming friction 0.5\n");
for(size_t i=0;i<formation.contacts.size();i++) {
printf("%d contacts on link %d\n",formation.contacts[i].size(),formation.links[i]);
for(size_t j=0;j<formation.contacts[i].size();j++)
formation.contacts[i][j].kFriction = 0.5;
}
Stance stance;
LocalContactsToStance(formation,robot,stance);
MultiPath path;
path.sections.resize(1);
MultiPath::PathSection& section = path.sections[0];
path.SetStance(stance,0);
vector<IKGoal> ikGoals;
path.GetIKProblem(ikGoals);
RobotConstrainedInterpolator interp(robot,ikGoals);
//Real xtol = 5e-2;
Real xtol = 1e-1;
interp.ftol = 1e-6;
interp.xtol = xtol;
if(!interp.Project(a)) {
printf("Failed to project config a\n");
return;
}
if(!interp.Project(b)) {
printf("Failed to project config b\n");
return;
}
cout<<"Start: "<<a<<endl;
cout<<"Goal: "<<b<<endl;
vector<Vector> milestones,milestones2;
if(!interp.Make(a,b,milestones)) {
printf("Failed to interpolate\n");
return;
}
if(!interp.Make(b,a,milestones2)) {
printf("Failed to interpolate\n");
return;
}
milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end());
//milestones2 = milestones;
//milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end());
{
cout<<"Saving geometric path to temp.path"<<endl;
ofstream out("temp.path",ios::out);
for(size_t i=0;i<milestones.size();i++)
out<<Real(i)/Real(milestones.size()-1)<<" "<<milestones[i]<<endl;
out.close();
}
section.milestones = milestones;
vector<Real> divs(101);
for(size_t i=0;i<divs.size();i++)
divs[i] = Real(i)/(divs.size()-1);
Timer timer;
ContactTimeScaling scaling(robot);
scaling.frictionRobustness = 0.25;
scaling.torqueRobustness = 0.25;
scaling.forceRobustness = 0.05;
bool res=scaling.SetParams(path,divs);
if(!res) {
printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime());
printf("Saving to scaling_constraints.csv\n");
ofstream outc("scaling_constraints.csv",ios::out);
outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl;
for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++)
for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++)
outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl;
return;
}
printf("Contact scaling init successful, time %g\n",timer.ElapsedTime());
printf("Saving to scaling_constraints.csv\n");
ofstream outc("scaling_constraints.csv",ios::out);
outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl;
for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++)
for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++)
outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl;
res=scaling.Optimize();
if(!res) {
printf("Time scaling failed in time %g. Path may be dynamically infeasible.\n",timer.ElapsedTime());
return;
}
printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back());
scaling.Check(path);
/*
for(size_t i=0;i<scaling.traj.ds.size();i++) {
printf("time %g: rate %g\n",scaling.traj.times[i],scaling.ds[i]);
}
*/
{
cout<<"Saving dynamically optimized path to temp_opt.path"<<endl;
ofstream out("temp_opt.path",ios::out);
Real dt = scaling.traj.EndTime()/100;
Real t=0;
for(size_t i=0;i<=100;i++) {
Vector x;
scaling.traj.Eval(t,x);
out<<t<<"\t"<<x<<endl;
t += dt;
}
out.close();
}
}
/************************************************************************
Expands the single multi path into all possible sequence variants.
Since this turns out to be the time-limiting process for long de nvoo,
this is implemented using a quick branch and bound that works on
edge variant indices. The SeqPaths are created only for the final
set of sequences.
*************************************************************************/
void PrmGraph::fast_expand_multi_path_for_combo_scores(
AllScoreModels *model,
const MultiPath& multi_path,
vector<SeqPath>& seq_paths,
score_t min_score,
score_t forbidden_pair_penalty,
int max_num_paths)
{
const vector<AA_combo>& aa_edge_comobos = config->get_aa_edge_combos();
vector<score_t> max_attainable_scores;
const int num_edges = multi_path.edge_idxs.size();
max_attainable_scores.resize(num_edges+1,0);
int num_path_variants=1;
int e;
for (e=num_edges-1; e<num_edges; e++)
{
const int e_idx = multi_path.edge_idxs[e];
const MultiEdge& edge = multi_edges[e_idx];
num_path_variants *= edge.variant_ptrs.size();
}
if (num_path_variants == 0)
{
cout << "Error: had an edge with 0 variants!" <<endl;
exit(1);
}
// re-check the max attainable scores, this time compute all combo scores along the path
// this will give a more accurate max attainable score, and also allow quick computation
// of the expanded scores
max_attainable_scores.clear();
max_attainable_scores.resize(num_edges+1,0);
vector<score_t> max_node_scores;
max_node_scores.resize(num_edges+1,NEG_INF);
for (e=0; e<=num_edges; e++)
{
const int n_edge_idx = (e>0 ? multi_path.edge_idxs[e-1] : -1);
const int c_edge_idx = (e<num_edges ? multi_path.edge_idxs[e] : -1);
const int node_idx = (n_edge_idx>=0 ? multi_edges[n_edge_idx].c_idx : multi_edges[c_edge_idx].n_idx);
const int num_n_vars = (e>0 ? multi_edges[n_edge_idx].variant_ptrs.size() : 1);
const int num_c_vars = (e<num_edges ? multi_edges[c_edge_idx].variant_ptrs.size() : 1);
int j,k;
for (j=0; j<num_n_vars; j++)
for (k=0; k<num_c_vars; k++)
{
score_t combo_score = model->get_node_combo_score(this,node_idx,n_edge_idx,j,c_edge_idx,k);
if (max_node_scores[e]<combo_score)
max_node_scores[e]=combo_score;
}
}
max_attainable_scores[num_edges]=max_node_scores[num_edges];
for (e=num_edges-1; e>=0; e--)
{
const int e_idx = multi_path.edge_idxs[e];
const MultiEdge& edge = multi_edges[e_idx];
max_attainable_scores[e]= max_attainable_scores[e+1] +
edge.max_variant_score + max_node_scores[e];
}
score_t max_path_score = max_attainable_scores[0] - multi_path.num_forbidden_nodes * forbidden_pair_penalty;
score_t min_delta_allowed = min_score - max_path_score;
if (min_delta_allowed>0)
return;
// in this expansion method, all edges are stored (not only ones with more than one variant)
// perform expansion using a heap and condensed path representation
vector<int> var_positions; // holds the edge idxs
vector<int> num_vars;
vector<int> var_edge_positions;
vector< vector< score_t > > var_scores;
var_scores.clear();
var_positions.clear();
num_vars.clear();
int num_aas_in_multipath = 0;
for (e=0; e<num_edges; e++)
{
const int e_idx = multi_path.edge_idxs[e];
const MultiEdge& edge = multi_edges[e_idx];
const int num_vars_in_edge = edge.variant_ptrs.size();
vector<score_t> scores;
num_aas_in_multipath+=edge.num_aa;
int i;
for (i=0; i<num_vars_in_edge; i++)
scores.push_back(edge.variant_scores[i]);
var_scores.push_back(scores);
var_positions.push_back(e);
num_vars.push_back(num_vars_in_edge);
var_edge_positions.push_back(num_aas_in_multipath-edge.num_aa);
}
// create the SeqPaths from the edge_combos...
const int num_positions = num_aas_in_multipath+1;
SeqPath template_path; // holds common elements to all paths
template_path.n_term_aa = multi_path.n_term_aa;
template_path.c_term_aa = multi_path.c_term_aa;
template_path.n_term_mass = multi_path.n_term_mass;
template_path.c_term_mass = multi_path.c_term_mass;
template_path.charge = charge;
template_path.multi_path_rank = multi_path.original_rank;
template_path.path_score = 0;
template_path.prm_ptr = (PrmGraph *)this;
template_path.positions.clear();
template_path.positions.resize(num_positions);
template_path.num_forbidden_nodes = multi_path.num_forbidden_nodes;
template_path.path_score -= template_path.num_forbidden_nodes * forbidden_pair_penalty;
int pos_idx=0;
for (e=0; e<multi_path.edge_idxs.size(); e++)
{
const int e_idx = multi_path.edge_idxs[e];
const MultiEdge& edge = get_multi_edge(e_idx);
PathPos& pos = template_path.positions[pos_idx++];
pos.breakage = edge.n_break;
pos.edge_idx = e_idx;
pos.mass = edge.n_break->mass;
pos.node_idx = edge.n_idx;
int *variant_ptr = edge.variant_ptrs[0];
const int num_aa = *variant_ptr++;
int *aas = variant_ptr;
if (edge.variant_ptrs.size() == 1)
{
pos.variant_ptr = edge.variant_ptrs[0];
pos.edge_variant_score = edge.variant_scores[0];
pos.aa = aas[0];
}
if (edge.num_aa == 1)
continue;
int j;
for (j=1; j<edge.num_aa; j++)
{
PathPos& pos = template_path.positions[pos_idx++];
pos.breakage = NULL;
pos.aa = aas[j];
}
}
if (pos_idx != num_aas_in_multipath)
{
cout << "Error: mismatches between positions and multipath length!" << endl;
exit(1);
}
PathPos& last_pos = template_path.positions[num_positions-1];
const int last_e_idx = multi_path.edge_idxs[e-1];
const MultiEdge& last_edge = get_multi_edge(last_e_idx);
last_pos.breakage = last_edge.c_break;
last_pos.edge_idx =-1;
last_pos.mass = last_edge.c_break->mass;
last_pos.node_idx = last_edge.c_idx;
const int num_multi_var_edges = var_positions.size();
vector<var_combo> combo_heap;
vector<int> var_idxs;
var_idxs.resize(num_multi_var_edges,0);
const int last_var_pos = num_multi_var_edges-1;
const int last_var_val = num_vars[last_var_pos];
const score_t needed_var_score = min_score; // this is only the penalty for forbidden pairs
while (1)
{
score_t idxs_score = 0;
int k;
for (k=0; k<var_idxs.size(); k++)
idxs_score += var_scores[k][var_idxs[k]]; // this is the score for the edge variants (usually 0)
const vector<PathPos>& positions = template_path.positions;
score_t path_score = idxs_score + template_path.path_score;
// compute the node scores based on the edge variants
// N-term node score
vector<score_t> node_scores;
node_scores.resize(var_idxs.size()+1,NEG_INF);
node_scores[0] = model->get_node_combo_score(this,positions[0].node_idx,-1,0,
positions[0].edge_idx,var_idxs[0]);
path_score += node_scores[0];
// middle nodes score
int prev_edge_idx = positions[0].edge_idx;
for (k=1; k<var_idxs.size(); k++)
{
const int p=var_edge_positions[k];
const PathPos& pos = positions[p];
node_scores[k] = model->get_node_combo_score(this,pos.node_idx,prev_edge_idx,var_idxs[k-1],
pos.edge_idx,var_idxs[k]);
path_score += node_scores[k];
prev_edge_idx = pos.edge_idx;
}
// C-term node score
node_scores[k] = model->get_node_combo_score(this,last_pos.node_idx,prev_edge_idx,var_idxs[k-1],-1,0);
path_score += node_scores[k];
if (path_score>=needed_var_score)
{
if (combo_heap.size()<max_num_paths)
{
var_combo v;
v.path_score = path_score;
v.var_idxs = var_idxs;
v.node_scores = node_scores;
combo_heap.push_back(v);
if (combo_heap.size()== max_num_paths)
make_heap(combo_heap.begin(),combo_heap.end());
}
else
{
const score_t score = path_score;
if (score>combo_heap[0].path_score)
{
pop_heap(combo_heap.begin(),combo_heap.end());
var_combo& combo = combo_heap[max_num_paths-1];
combo.path_score = path_score;
combo.var_idxs = var_idxs;
combo.node_scores = node_scores;
push_heap(combo_heap.begin(),combo_heap.end());
}
}
}
int j=0;
while (j<num_multi_var_edges)
{
var_idxs[j]++;
if (var_idxs[j]==num_vars[j])
{
var_idxs[j++]=0;
}
else
break;
}
if (j==num_multi_var_edges)
break;
}
seq_paths.clear();
seq_paths.resize(combo_heap.size(),template_path);
int i;
for (i=0; i<combo_heap.size(); i++)
{
const var_combo& combo = combo_heap[i];
SeqPath& seq_path = seq_paths[i];
// fill in info that changes with multi-variant edges
int j;
for (j=0; j<num_multi_var_edges; j++)
{
const int var_pos = var_positions[j];
const int e_idx = multi_path.edge_idxs[var_pos];
const MultiEdge& edge = get_multi_edge(e_idx);
const int pos_idx = var_edge_positions[j];
if (seq_path.positions[pos_idx].edge_idx != e_idx)
{
cout << "POS: " << pos_idx << endl;
cout << "Error: mismatch in pos_idx and e_idx of a multipath!" << endl;
cout << "looking for " << e_idx << endl;
cout << "edge idxs:" << endl;
int k;
for (k=0; k<seq_path.positions.size(); k++)
cout << k << "\t" << seq_path.positions[k].edge_idx << "\tnidx: " <<
seq_path.positions[k].node_idx << endl;
cout << endl;
multi_path.print(config);
this->print();
exit(1);
}
PathPos& pos = seq_path.positions[pos_idx];
const int variant_idx = combo.var_idxs[j];
int *variant_ptr = edge.variant_ptrs[variant_idx];
const int num_aa = *variant_ptr++;
int *aas = variant_ptr;
if (num_aa != edge.num_aa)
{
cout << "Error: edge and variant mixup!" << endl;
exit(1);
}
pos.node_score = combo.node_scores[j];
pos.edge_variant_score = edge.variant_scores[variant_idx];
pos.variant_ptr = edge.variant_ptrs[variant_idx];
pos.aa = aas[0];
if (edge.num_aa == 1)
continue;
int k;
for (k=1; k<edge.num_aa; k++)
{
PathPos& pos = seq_path.positions[pos_idx+k];
pos.aa = aas[k];
}
}
seq_path.positions[seq_path.positions.size()-1].node_score = combo.node_scores[num_multi_var_edges];
seq_path.path_score=combo.path_score;
float pen = seq_path.adjust_complete_sequence_penalty(this);
seq_path.make_seq_str(config);
}
}
bool ContactTimeScaling::SetParams(const MultiPath& path,const vector<Real>& paramDivs,int numFCEdges)
{
Robot& robot = cspace.robot;
CustomTimeScaling::SetPath(path,paramDivs);
CustomTimeScaling::SetDefaultBounds();
CustomTimeScaling::SetStartStop();
ContactFormation formation;
int oldSection = -1;
LinearProgram_Sparse lp;
NewtonEulerSolver ne(robot);
//kinetic energy, coriolis force, gravity
Vector3 gravity(0,0,-9.8);
Vector C,G;
//coefficients of time scaling
Vector a,b,c;
bool feasible=true;
for(size_t i=0;i<paramDivs.size();i++) {
Assert(paramSections[i] >= 0 && paramSections[i] < (int)path.sections.size());
if(paramSections[i] != oldSection) {
//reconstruct LP for the contacts in this section
Stance stance;
path.GetStance(stance,paramSections[i]);
ToContactFormation(stance,formation);
for(size_t j=0;j<formation.contacts.size();j++)
for(size_t k=0;k<formation.contacts[j].size();k++) {
Assert(formation.contacts[j][k].kFriction > 0);
Assert(frictionRobustness < 1.0);
formation.contacts[j][k].kFriction *= (1.0-frictionRobustness);
}
//now formulate the LP. Variable 0 is dds, variable 1 is ds^2
//rows 1-n are torque max
//rows n+1 - 2n are acceleration max
//rows 2n+1 + 2n+numFCEdges*nc are the force constraints
//vel max is encoded in the velocity variable
int n = (int)robot.links.size();
int nc = formation.numContactPoints();
#if TEST_NO_CONTACT
nc = 0;
#endif // TEST_NO_CONTACT
lp.Resize(n*2+numFCEdges*nc,2+3*nc);
lp.A.setZero();
lp.c.setZero();
//fill out wrench matrix FC*f <= 0
#if !TEST_NO_CONTACT
SparseMatrix FC;
GetFrictionConePlanes(formation,numFCEdges,FC);
lp.A.copySubMatrix(n*2,2,FC);
for(int j=0;j<FC.m;j++)
lp.p(n*2+j) = -forceRobustness;
#endif // !TEST_NO_CONTACT
lp.l(0) = 0.0;
lp.l(1) = -Inf;
oldSection = paramSections[i];
}
//configuration specific
robot.UpdateConfig(xs[i]);
robot.dq = dxs[i];
ne.CalcResidualTorques(C);
robot.GetGravityTorques(gravity,G);
ne.MulKineticEnergyMatrix(dxs[i],a);
ne.MulKineticEnergyMatrix(ddxs[i],b);
b += C;
c = G;
//|a dds + b ds^2 + c - Jtf| <= torquemax*scale+shift
for(int j=0;j<a.n;j++) {
lp.A(j,0) = b(j);
lp.A(j,1) = a(j);
Real tmax = robot.torqueMax(j)*torqueLimitScale+torqueLimitShift;
if(tmax < 0) tmax=0;
lp.p(j) = tmax-c(j);
lp.q(j) = -tmax-c(j);
}
#if TEST_NO_CONTACT
lp.p.set(Inf);
lp.q.set(-Inf);
#else
//fill out jacobian transposes
int ccount=0;
for(size_t l=0;l<formation.links.size();l++) {
int link = formation.links[l];
int target = (formation.targets.empty() ? -1 : formation.targets[l]);
for(size_t j=0;j<formation.contacts[l].size();j++,ccount++) {
Vector3 p=formation.contacts[l][j].x;
//if it's a self-contact, then transform to world
if(target >= 0)
p = robot.links[target].T_World*p;
Vector3 v;
int k=link;
while(k!=-1) {
robot.links[k].GetPositionJacobian(robot.q[k],p,v);
if(v.x != 0.0) lp.A(k,2+ccount*3)=-v.x;
if(v.y != 0.0) lp.A(k,2+ccount*3+1)=-v.y;
if(v.z != 0.0) lp.A(k,2+ccount*3+2)=-v.z;
k=robot.parents[k];
}
k = target;
while(k!=-1) {
robot.links[k].GetPositionJacobian(robot.q[k],p,v);
if(v.x != 0.0) lp.A(k,2+ccount*3)+=v.x;
if(v.y != 0.0) lp.A(k,2+ccount*3+1)+=v.y;
if(v.z != 0.0) lp.A(k,2+ccount*3+2)+=v.z;
k=robot.parents[k];
}
}
}
Assert(ccount == formation.numContactPoints());
#endif //TEST_NO_CONTACT
//fill out acceleration constraint |ddx*ds^2 + dx*dds| <= amax
for(int j=0;j<a.n;j++) {
lp.q(a.n+j) = -robot.accMax(j);
lp.p(a.n+j) = robot.accMax(j);
lp.A(a.n+j,0) = ddxs[i][j];
lp.A(a.n+j,1) = dxs[i][j];
}
//compute upper bounds from vel and acc max
lp.u(0) = Inf; lp.u(1) = Inf;
for(int j=0;j<a.n;j++) {
if(dxs[i][j] < 0)
lp.u(0) = Min(lp.u(0),Sqr(robot.velMin(j)/dxs[i][j]));
else
lp.u(0) = Min(lp.u(0),Sqr(robot.velMax(j)/dxs[i][j]));
}
//expand polytope
Geometry::PolytopeProjection2D proj(lp);
Geometry::UnboundedPolytope2D poly;
proj.Solve(poly);
if(poly.vertices.empty()) {
//problem is infeasible?
printf("Problem is infeasible at segment %d\n",i);
cout<<"x = "<<xs[i]<<endl;
cout<<"dx = "<<dxs[i]<<endl;
cout<<"ddx = "<<ddxs[i]<<endl;
lp.Print(cout);
getchar();
feasible=false;
}
/*
if(i == 49) {
cout<<"x = "<<xs[i]<<endl;
cout<<"dx = "<<dxs[i]<<endl;
cout<<"ddx = "<<ddxs[i]<<endl;
lp.Print(cout);
cout<<"Result: "<<endl;
for(size_t j=0;j<poly.planes.size();j++)
cout<<poly.planes[j].normal.x<<" ds^2 + "<<poly.planes[j].normal.y<<" dds <= "<<poly.planes[j].offset<<endl;
cout<<"Vertices: "<<endl;
for(size_t j=0;j<poly.vertices.size();j++) {
cout<<poly.vertices[j]<<endl;
}
getchar();
}
*/
ds2ddsConstraintNormals[i].resize(poly.planes.size());
ds2ddsConstraintOffsets[i].resize(poly.planes.size());
for(size_t j=0;j<poly.planes.size();j++) {
ds2ddsConstraintNormals[i][j] = poly.planes[j].normal;
ds2ddsConstraintOffsets[i][j] = poly.planes[j].offset;
if(saveConstraintNames) {
stringstream ss;
ss<<"projected_constraint_plane_"<<j;
ds2ddsConstraintNames[i].push_back(ss.str());
}
}
}
//done!
return feasible;
}
/** @brief Completely interpolates, optimizes, and time-scales the
* given MultiPath to satisfy its contact constraints.
*
* Arguments
* - robot: the robot
* - path: the MultiPath containing the milestones / stances of the motion
* - interpTol: the tolerance that must be met for contact constraints for the
* interpolated path.
* - numdivs: the number of grid points used for time-scaling
* - traj (out): the output timed trajectory. (Warning, the spaces and manifolds in
* traj.path.segments will be bogus pointers.)
* - torqueRobustness: a parameter in [0,1] determining the robustness margin
* added to the torque constraint. The robot's torques are scaled by a factor
* of (1-torqueRobustness).
* - frictionRobustness: a parameter in [0,1] determinining the robustness margin
* added to the friction constraint. The friction coefficients are scaled by
* a factor of (1-frictionRobustness). Helps the path avoid slipping.
* - forceRobustness: a minimum normal contact force that must be applied
* *at each contact point* (in Newtons). Helps the trajectory avoid contact
* separation.
* - savePath: if true, saves the interpolated MultiPath to disk.
* - saveConstraints: if true, saves the time scaling convex program constraints
* to disk.
*
* Return value is true if interpolation / time scaling was successful.
* Failure indicates that the milestones could not be interpolated, or
* the path is not dynamically feasible. For example, the milestones or
* interpolated path might violate stability constraints.
*/
bool ContactOptimizeMultipath(Robot& robot,const MultiPath& path,
Real interpTol,int numdivs,
TimeScaledBezierCurve& traj,
Real torqueRobustness=0.0,
Real frictionRobustness=0.0,
Real forceRobustness=0.0,
bool savePath = true,
bool saveConstraints = false)
{
Assert(torqueRobustness < 1.0);
Assert(frictionRobustness < 1.0);
Timer timer;
MultiPath ipath;
bool res=DiscretizeConstrainedMultiPath(robot,path,ipath,interpTol);
if(!res) {
printf("Could not discretize path, failing\n");
return false;
}
int ns = 0;
for(size_t i=0;i<ipath.sections.size();i++)
ns += ipath.sections[i].milestones.size()-1;
printf("Interpolated at resolution %g to %d segments, time %g\n",interpTol,ns,timer.ElapsedTime());
if(savePath)
{
cout<<"Saving interpolated geometric path to temp.xml"<<endl;
ipath.Save("temp.xml");
}
vector<Real> divs(numdivs);
Real T = ipath.Duration();
for(size_t i=0;i<divs.size();i++)
divs[i] = T*Real(i)/(divs.size()-1);
timer.Reset();
ContactTimeScaling scaling(robot);
//CustomTimeScaling scaling(robot);
scaling.frictionRobustness = frictionRobustness;
scaling.torqueLimitScale = 1.0-torqueRobustness;
scaling.forceRobustness = forceRobustness;
res=scaling.SetParams(ipath,divs);
/*
scaling.SetPath(ipath,divs);
scaling.SetDefaultBounds();
scaling.SetStartStop();
bool res=true;
*/
if(!res) {
printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime());
if(saveConstraints) {
printf("Saving to scaling_constraints.csv\n");
ofstream outc("scaling_constraints.csv",ios::out);
outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl;
for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++)
for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++)
outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl;
}
return false;
}
printf("Contact scaling init successful, time %g\n",timer.ElapsedTime());
if(saveConstraints) {
printf("Saving to scaling_constraints.csv\n");
ofstream outc("scaling_constraints.csv",ios::out);
outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl;
for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++)
for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++)
outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl;
}
res=scaling.Optimize();
if(!res) {
printf("Time scaling failed in time %g. Path may be dynamically infeasible.\n",timer.ElapsedTime());
return false;
}
printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back());
//scaling.Check(ipath);
traj = scaling.traj;
return true;
}
void ContactOptimizeMultipath(const char* robfile,const char* pathfile,const char* settingsfile = NULL)
{
ContactOptimizeSettings settings;
if(settingsfile != NULL) {
if(!settings.read(settingsfile)) {
printf("Unable to read settings file %s\n",settingsfile);
return;
}
}
Real xtol=Real(settings["xtol"]);
int numdivs = int(settings["numdivs"]);
bool ignoreForces = bool(settings["ignoreForces"]);
Real torqueRobustness = Real(settings["torqueRobustness"]);
Real frictionRobustness = Real(settings["frictionRobustness"]);
Real forceRobustness = Real(settings["forceRobustness"]);
string outputPath;
settings["outputPath"].as(outputPath);
Real outputDt = Real(settings["outputDt"]);
Robot robot;
if(!robot.Load(robfile)) {
printf("Unable to load robot file %s\n",robfile);
return;
}
MultiPath path;
if(!path.Load(pathfile)) {
printf("Unable to load path file %s\n",pathfile);
return;
}
//double check friction
for(size_t i=0;i<path.sections.size();i++) {
Stance s;
path.GetStance(s,i);
bool changed = false;
int k=0;
int numContacts = 0;
for(Stance::iterator h=s.begin();h!=s.end();h++,k++) {
numContacts += (int)h->second.contacts.size();
for(size_t j=0;j<h->second.contacts.size();j++)
if(h->second.contacts[j].kFriction <= 0) {
if(!changed)
printf("Warning, contact %d of hold %d has invalid friction %g, setting to 0.5\n",j,k,h->second.contacts[j].kFriction);
h->second.contacts[j].kFriction = 0.5;
changed = true;
}
}
path.SetStance(s,i);
if(numContacts == 0 && !ignoreForces && robot.joints[0].type == RobotJoint::Floating) {
printf("Warning, no contacts given in stance %d for floating-base robot\n",i);
printf("Should set ignoreForces = true in trajopt.settings if you wish\n");
printf("to ignore contact force constraints.\n");
printf("Press enter to continue...\n");
getchar();
}
}
TimeScaledBezierCurve opttraj;
if(ignoreForces) {
bool res=GenerateAndTimeOptimizeMultiPath(robot,path,xtol,outputDt);
if(!res) {
printf("Time optimization failed\n");
return;
}
Assert(path.HasTiming());
cout<<"Saving dynamically optimized path to "<<outputPath<<endl;
ofstream out(outputPath.c_str(),ios::out);
for(size_t i=0;i<path.sections.size();i++) {
for(size_t j=0;j<path.sections[i].milestones.size();j++)
out<<path.sections[i].times[j]<<"\t"<<path.sections[i].milestones[j]<<endl;
}
out.close();
}
else {
bool res=ContactOptimizeMultipath(robot,path,xtol,
numdivs,opttraj,
torqueRobustness,
frictionRobustness,
forceRobustness);
if(!res) {
printf("Time optimization failed\n");
return;
}
RobotCSpace cspace(robot);
for(size_t i=0;i<opttraj.path.segments.size();i++) {
opttraj.path.segments[i].space = &cspace;
opttraj.path.segments[i].manifold = &cspace;
}
vector<Config> milestones;
opttraj.GetDiscretizedPath(outputDt,milestones);
cout<<"Saving dynamically optimized path to "<<outputPath<<endl;
ofstream out(outputPath.c_str(),ios::out);
for(size_t i=0;i<milestones.size();i++) {
out<<i*outputDt<<"\t"<<milestones[i]<<endl;
}
out.close();
printf("Plotting vel/acc constraints to trajopt_plot.csv...\n");
opttraj.Plot("trajopt_plot.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
}
}
int main(int argc,const char** argv)
{
if(argc <= 3) {
printf("USAGE: ContactPlan [options] world_file configs stance\n");
printf("OPTIONS:\n");
printf("-o filename: the output linear path or multipath (default contactplan.xml)\n");
printf("-p settings: set the planner configuration file\n");
printf("-opt: do optimal planning (do not terminate on the first found solution)\n");
printf("-n iters: set the default number of iterations per step (default 1000)\n");
printf("-t time: set the planning time limit (default infinity)\n");
printf("-m margin: set support polygon margin (default 0)\n");
printf("-r robotindex: set the robot index (default 0)\n");
return 0;
}
Srand(time(NULL));
int robot = 0;
const char* outputfile = "contactplan.xml";
HaltingCondition termCond;
string plannerSettings;
int i;
//parse command line arguments
for(i=1;i<argc;i++) {
if(argv[i][0]=='-') {
if(0==strcmp(argv[i],"-n")) {
termCond.maxIters = atoi(argv[i+1]);
i++;
}
else if(0==strcmp(argv[i],"-t")) {
termCond.timeLimit = atof(argv[i+1]);
i++;
}
else if(0==strcmp(argv[i],"-opt")) {
termCond.foundSolution = false;
}
else if(0==strcmp(argv[i],"-p")) {
if(!GetFileContents(argv[i+1],plannerSettings)) {
printf("Unable to load planner settings file %s\n",argv[i+1]);
return 1;
}
i++;
}
else if(0==strcmp(argv[i],"-r")) {
robot = atoi(argv[i+1]);
i++;
}
else if(0==strcmp(argv[i],"-m")) {
gSupportPolygonMargin = atof(argv[i+1]);
i++;
}
else if(0==strcmp(argv[i],"-o")) {
outputfile = argv[i+1];
i++;
}
else {
printf("Invalid option %s\n",argv[i]);
return 1;
}
}
else break;
}
if(i+3 < argc) {
printf("USAGE: ContactPlan [options] world_file configs stance\n");
return 1;
}
if(i+3 > argc) {
printf("Warning: extra arguments provided\n");
}
const char* worldfile = argv[i];
const char* configsfile = argv[i+1];
const char* stancefile = argv[i+2];
//Read in the world file
XmlWorld xmlWorld;
RobotWorld world;
if(!xmlWorld.Load(worldfile)) {
printf("Error loading world XML file %s\n",worldfile);
return 1;
}
if(!xmlWorld.GetWorld(world)) {
printf("Error loading world file %s\n",worldfile);
return 1;
}
vector<Config> configs;
{
//Read in the configurations specified in configsfile
ifstream in(configsfile);
if(!in) {
printf("Error opening configs file %s\n",configsfile);
return false;
}
while(in) {
Config temp;
in >> temp;
if(in) configs.push_back(temp);
}
if(configs.size() < 2) {
printf("Configs file does not contain 2 or more configs\n");
return 1;
}
in.close();
}
Stance stance;
{
//read in the stance specified by stancefile
ifstream in(stancefile,ios::in);
in >> stance;
if(!in) {
printf("Error loading stance file %s\n",stancefile);
return 1;
}
in.close();
}
//If the stance has no contacts, use ContactPlan. Otherwise, use StancePlan
bool ignoreContactForces = false;
if(NumContactPoints(stance)==0) {
printf("No contact points in stance, planning without stability constraints\n");
ignoreContactForces = true;
}
//set up the command line, store it into the MultiPath settings
string cmdline;
cmdline = argv[0];
for(int i=1;i<argc;i++) {
cmdline += " ";
cmdline += argv[i];
}
MultiPath path;
path.settings["robot"] = world.robots[robot].name;
path.settings["command"] = cmdline;
//begin planning
bool feasible = true;
Config qstart = world.robots[robot].robot->q;
for(size_t i=0;i+1<configs.size();i++) {
MilestonePath mpath;
bool res = false;
if(ignoreContactForces)
res = ContactPlan(world,robot,configs[i],configs[i+1],stance,mpath,termCond,plannerSettings);
else
res = StancePlan(world,robot,configs[i],configs[i+1],stance,mpath,termCond,plannerSettings);
if(!res) {
printf("Planning from stance %d to %d failed\n",i,i+1);
path.sections.resize(path.sections.size()+1);
path.SetStance(stance,path.sections.size()-1);
path.sections.back().milestones[0] = configs[i];
path.sections.back().milestones[1] = configs[i+1];
break;
}
else {
path.sections.resize(path.sections.size()+1);
path.sections.back().milestones.resize(mpath.NumMilestones());
path.SetStance(stance,path.sections.size()-1);
for(int j=0;j<mpath.NumMilestones();j++)
path.sections.back().milestones[j] = mpath.GetMilestone(j);
qstart = path.sections.back().milestones.back();
}
}
if(feasible)
printf("Path planning success! Saving to %s\n",outputfile);
else
printf("Path planning failure. Saving placeholder path to %s\n",outputfile);
const char* ext = FileExtension(outputfile);
if(ext && 0==strcmp(ext,"path")) {
printf("Converted to linear path format\n");
LinearPath lpath;
Convert(path,lpath);
ofstream f(outputfile,ios::out);
lpath.Save(f);
f.close();
}
else
path.Save(outputfile);
return 0;
}
bool InterpolateConstrainedMultiPath(Robot& robot,const MultiPath& path,vector<GeneralizedCubicBezierSpline>& paths,Real xtol)
{
//sanity check -- make sure it's a continuous path
if(!path.IsContinuous()) {
fprintf(stderr,"InterpolateConstrainedMultiPath: path is discontinuous\n");
return false;
}
if(path.sections.empty()) {
fprintf(stderr,"InterpolateConstrainedMultiPath: path is empty\n");
return false;
}
if(path.settings.contains("resolution")) {
//see if the resolution is high enough to just interpolate directly
Real res=path.settings.as<Real>("resolution");
if(res <= xtol) {
printf("Direct interpolating trajectory with res %g\n",res);
//just interpolate directly
RobotCSpace space(robot);
RobotGeodesicManifold manifold(robot);
paths.resize(path.sections.size());
for(size_t i=0;i<path.sections.size();i++) {
if(path.sections[i].times.empty()) {
SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,paths[i].segments,
&space,&manifold);
//uniform timing
paths[i].durations.resize(paths[i].segments.size());
Real dt=1.0/Real(paths[i].segments.size());
for(size_t j=0;j<paths[i].segments.size();j++)
paths[i].durations[j] = dt;
}
else {
SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,path.sections[i].times,paths[i].segments,
&space,&manifold);
//get timing from path
paths[i].durations.resize(paths[i].segments.size());
for(size_t j=0;j<paths[i].segments.size();j++)
paths[i].durations[j] = path.sections[i].times[j+1]-path.sections[i].times[j];
}
}
return true;
}
}
printf("Discretizing constrained trajectory at res %g\n",xtol);
RobotCSpace cspace(robot);
RobotGeodesicManifold manifold(robot);
//create transition constraints and derivatives
vector<vector<IKGoal> > stanceConstraints(path.sections.size());
vector<vector<IKGoal> > transitionConstraints(path.sections.size()-1);
vector<Config> transitionDerivs(path.sections.size()-1);
for(size_t i=0;i<path.sections.size();i++)
path.GetIKProblem(stanceConstraints[i],i);
for(size_t i=0;i+1<path.sections.size();i++) {
//put all nonredundant constraints into transitionConstraints[i]
transitionConstraints[i]=stanceConstraints[i];
for(size_t j=0;j<stanceConstraints[i+1].size();j++) {
bool res=AddGoalNonredundant(stanceConstraints[i+1][j],transitionConstraints[i]);
if(!res) {
fprintf(stderr,"Conflict between goal %d of stance %d and stance %d\n",j,i+1,i);
fprintf(stderr," Link %d\n",stanceConstraints[i+1][j].link);
return false;
}
}
const Config& prev=path.sections[i].milestones[path.sections[i].milestones.size()-2];
const Config& next=path.sections[i+1].milestones[1];
manifold.InterpolateDeriv(prev,next,0.5,transitionDerivs[i]);
transitionDerivs[i] *= 0.5;
//check for overshoots a la MonotonicInterpolate
Vector inslope,outslope;
manifold.InterpolateDeriv(prev,path.sections[i].milestones.back(),1.0,inslope);
manifold.InterpolateDeriv(path.sections[i].milestones.back(),next,0.0,outslope);
for(int j=0;j<transitionDerivs[i].n;j++) {
if(Sign(transitionDerivs[i][j]) != Sign(inslope[j]) || Sign(transitionDerivs[i][j]) != Sign(outslope[j])) transitionDerivs[i][j] = 0;
else {
if(transitionDerivs[i][j] > 0) {
if(transitionDerivs[i][j] > 3.0*outslope[j])
transitionDerivs[i][j] = 3.0*outslope[j];
if(transitionDerivs[i][j] > 3.0*inslope[j])
transitionDerivs[i][j] = 3.0*inslope[j];
}
else {
if(transitionDerivs[i][j] < 3.0*outslope[j])
transitionDerivs[i][j] = 3.0*outslope[j];
if(transitionDerivs[i][j] < 3.0*inslope[j])
transitionDerivs[i][j] = 3.0*inslope[j];
}
}
}
//project "natural" derivative onto transition manifold
RobotIKFunction f(robot);
f.UseIK(transitionConstraints[i]);
GetDefaultIKDofs(robot,transitionConstraints[i],f.activeDofs);
Vector temp(f.activeDofs.Size()),dtemp(f.activeDofs.Size()),dtemp2;
f.activeDofs.InvMap(path.sections[i].milestones.back(),temp);
f.activeDofs.InvMap(transitionDerivs[i],dtemp);
Matrix J;
f.PreEval(temp);
f.Jacobian(temp,J);
RobustSVD<Real> svd;
if(!svd.set(J)) {
fprintf(stderr,"Unable to set SVD of transition constraints %d\n",i);
return false;
}
svd.nullspaceComponent(dtemp,dtemp2);
dtemp -= dtemp2;
f.activeDofs.Map(dtemp,transitionDerivs[i]);
}
//start constructing path
paths.resize(path.sections.size());
for(size_t i=0;i<path.sections.size();i++) {
paths[i].segments.resize(0);
paths[i].durations.resize(0);
Vector dxprev,dxnext;
if(i>0)
dxprev.setRef(transitionDerivs[i-1]);
if(i<transitionDerivs.size())
dxnext.setRef(transitionDerivs[i]);
if(stanceConstraints[i].empty()) {
SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,paths[i].segments,&cspace,&manifold);
DiscretizeSpline(paths[i],xtol);
//Note: discretizeSpline will fill in the spline durations
}
else {
RobotSmoothConstrainedInterpolator interp(robot,stanceConstraints[i]);
interp.xtol = xtol;
if(!MultiSmoothInterpolate(interp,path.sections[i].milestones,dxprev,dxnext,paths[i])) {
/** TEMP - test no inter-section smoothing**/
//if(!MultiSmoothInterpolate(interp,path.sections[i].milestones,paths[i])) {
fprintf(stderr,"Unable to interpolate section %d\n",i);
return false;
}
}
//set the time scale if the input path is timed
if(!path.sections[i].times.empty()) {
//printf("Time scaling section %d to duration %g\n",i,path.sections[i].times.back()-path.sections[i].times.front());
paths[i].TimeScale(path.sections[i].times.back()-path.sections[i].times.front());
}
}
return true;
}
void TimeOptimizeTestNLinkPlanar()
{
const char* fn = "data/planar%d.rob";
int ns [] = {5,10,15,20,25,30,40,50,60,70,80,90,100};
Real tolscales [] = {3.95,3.55,2.73,2,2,2,1.8,1.6,1.42,1,1,1,1};
int num = 13;
char buf[256];
for(int i=0;i<num;i++) {
int n=ns[i];
printf("Testing optimize %d\n",n);
sprintf(buf,fn,n);
Robot robot;
if(!robot.Load(buf)) {
fprintf(stderr,"Unable to load robot %s\n",buf);
return;
}
int ee = robot.links.size()-1;
Vector3 localpt(1,0,0);
Real len = robot.links.size();
//make a half circle
Config a(robot.q.n,Pi/robot.links.size()),b;
a[0] *= 0.5;
robot.UpdateConfig(a);
IKGoal goal,goaltemp;
goal.link = ee;
goal.localPosition = localpt;
goal.SetFixedPosition(robot.links[ee].T_World*localpt);
//cout<<"Goal position "<<goal.endPosition<<endl;
goaltemp = goal;
goaltemp.link = ee/2;
goaltemp.SetFixedPosition(goal.endPosition*0.5);
//cout<<"Middle goal position "<<goaltemp.endPosition<<endl;
vector<IKGoal> onegoal(1),bothgoals(2);
onegoal[0] = goal;
bothgoals[0] = goal;
bothgoals[1] = goaltemp;
int iters=100;
bool res=SolveIK(robot,bothgoals,1e-3,iters);
if(!res) {
fprintf(stderr,"Couldn't solve for target robot config\n");
return;
}
b = robot.q;
Timer timer;
GeneralizedCubicBezierSpline path;
RobotSmoothConstrainedInterpolator interp(robot,onegoal);
interp.xtol = 1e-3*tolscales[i];
if(!interp.Make(a,b,path)) {
fprintf(stderr,"Couldn't interpolate for target robot config\n");
return;
}
printf("Solved for path with tol %g in time %g\n",interp.xtol,timer.ElapsedTime());
{
sprintf(buf,"trajopt_a_%d.config",n);
ofstream out(buf);
out<<a<<endl;
}
{
sprintf(buf,"trajopt_b_%d.config",n);
ofstream out(buf);
out<<b<<endl;
}
{
sprintf(buf,"trajopt_interp_%d.xml",n);
vector<Real> times;
vector<Config> configs;
path.GetPiecewiseLinear(times,configs);
MultiPath mpath;
mpath.SetTimedMilestones(times,configs);
mpath.SetIKProblem(onegoal);
mpath.Save(buf);
}
{
//unroll joints
for(size_t i=0;i<path.segments.size();i++) {
for(int j=0;j<path.segments[i].x0.n;j++) {
if(path.segments[i].x0[j] > Pi)
path.segments[i].x0[j] -= TwoPi;
if(path.segments[i].x1[j] > Pi)
path.segments[i].x1[j] -= TwoPi;
if(path.segments[i].x2[j] > Pi)
path.segments[i].x2[j] -= TwoPi;
if(path.segments[i].x3[j] > Pi)
path.segments[i].x3[j] -= TwoPi;
}
}
sprintf(buf,"trajopt_interp_%d.spline",n);
ofstream out(buf,ios::out);
out.precision(10);
path.Save(out);
}
TimeScaledBezierCurve curve;
//curve.path = path;
{
sprintf(buf,"trajopt_interp_%d.spline",n);
ifstream in(buf,ios::in);
curve.path.Load(in);
Assert(curve.path.segments.size() == path.durations.size());
for(size_t i=0;i<curve.path.durations.size();i++) {
Assert(FuzzyEquals(curve.path.durations[i],path.durations[i]));
if(!curve.path.segments[i].x0.isEqual(path.segments[i].x0,Epsilon)) {
printf("Error on segment %d\n",i);
cout<<path.segments[i].x0<<endl;
cout<<curve.path.segments[i].x0<<endl;
cout<<"Distance: "<<path.segments[i].x0.distance(curve.path.segments[i].x0)<<endl;
}
Assert(curve.path.segments[i].x0.isEqual(path.segments[i].x0,Epsilon));
Assert(curve.path.segments[i].x1.isEqual(path.segments[i].x1,Epsilon));
Assert(curve.path.segments[i].x2.isEqual(path.segments[i].x2,Epsilon));
Assert(curve.path.segments[i].x3.isEqual(path.segments[i].x3,Epsilon));
}
}
Vector vmin(robot.q.n,-Pi),vmax(robot.q.n,Pi);
Vector amin(robot.q.n,-Pi),amax(robot.q.n,Pi);
timer.Reset();
if(!curve.OptimizeTimeScaling(vmin,vmax,amin,amax)) {
fprintf(stderr,"Optimize failed in time %g\n",timer.ElapsedTime());
return;
}
printf("Solved time optimization with %d segments in time %g\n",curve.path.segments.size(),timer.ElapsedTime());
//output optimized path
{
Real dt = 0.5;
vector<Real> times;
vector<Config> configs;
/*curve.GetDiscretizedPath(dt,configs);
times.resize(configs.size());
for(size_t j=0;j<times.size();j++)
times[j] = j*dt;
*/
curve.GetPiecewiseLinear(times,configs);
MultiPath mpath;
mpath.SetIKProblem(onegoal);
mpath.SetTimedMilestones(times,configs);
sprintf(buf,"trajopt_opt_%d.xml",n);
mpath.Save(buf);
}
}
}
void ContactOptimizeTest()
{
Robot robot;
if(!robot.Load("data/simple_2d_biped.rob")) {
printf("Unable to load data/simple_2d_biped.rob\n");
return;
}
MultiPath path;
path.sections.resize(1);
MultiPath::PathSection& section = path.sections[0];
section.holds.resize(2);
section.holds[0].contacts.resize(2);
section.holds[0].contacts[0].x.set(-0.4,-0.1,0);
section.holds[0].contacts[1].x.set(-0.4,0.1,0);
section.holds[0].contacts[0].n.set(0,0,1);
section.holds[0].contacts[1].n.set(0,0,1);
section.holds[0].contacts[0].kFriction = 0.5;
section.holds[0].contacts[1].kFriction = 0.5;
section.holds[1].contacts.resize(2);
section.holds[1].contacts[0].x.set(0.4,-0.1,0);
section.holds[1].contacts[1].x.set(0.4,0.1,0);
section.holds[1].contacts[0].n.set(0,0,1);
section.holds[1].contacts[1].n.set(0,0,1);
section.holds[1].contacts[0].kFriction = 0.5;
section.holds[1].contacts[1].kFriction = 0.5;
section.holds[0].link = 7;
section.holds[1].link = 9;
section.holds[0].SetupIKConstraint(Vector3(0.5,-0.1,0),Vector3(Zero));
section.holds[1].SetupIKConstraint(Vector3(0.5,-0.1,0),Vector3(Zero));
vector<IKGoal> ikGoals;
path.GetIKProblem(ikGoals);
RobotConstrainedInterpolator interp(robot,ikGoals);
Real xtol = 5e-2;
interp.ftol = 1e-4;
interp.xtol = xtol;
Vector a(7),b(7);
ifstream ia("simple_2d_biped/a.config",ios::in);
ifstream ib("simple_2d_biped/b.config",ios::in);
ia >> a;
ib >> b;
ia.close();
ib.close();
if(!interp.Project(a)) {
printf("Failed to project config a\n");
return;
}
if(!interp.Project(b)) {
printf("Failed to project config b\n");
return;
}
cout<<"Start: "<<a<<endl;
cout<<"Goal: "<<b<<endl;
vector<Vector> milestones,milestones2;
if(!interp.Make(a,b,milestones)) {
printf("Failed to interpolate\n");
return;
}
if(!interp.Make(b,a,milestones2)) {
printf("Failed to interpolate\n");
return;
}
milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end());
//milestones2 = milestones;
//milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end());
{
cout<<"Saving geometric path to temp.path"<<endl;
ofstream out("temp.path",ios::out);
for(size_t i=0;i<milestones.size();i++)
out<<Real(i)/Real(milestones.size()-1)<<" "<<milestones[i]<<endl;
out.close();
}
section.milestones = milestones;
vector<Real> divs(401);
for(size_t i=0;i<divs.size();i++)
divs[i] = Real(i)/(divs.size()-1);
Timer timer;
ContactTimeScaling scaling(robot);
scaling.frictionRobustness = 0.25;
scaling.torqueRobustness = 0.25;
scaling.forceRobustness = 0.1;
bool res=scaling.SetParams(path,divs);
if(!res) {
printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime());
return;
}
printf("Contact scaling init successful, time %g\n",timer.ElapsedTime());
ofstream outc("scaling_constraints.csv",ios::out);
outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl;
for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++)
for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++)
outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl;
res = scaling.Optimize();
if(!res) {
printf("Time scaling failed in time %g. Path may be dynamically infeasible.\n",timer.ElapsedTime());
return;
}
printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back());
printf("\n");
printf("Checking path for feasibility...\n");
scaling.Check(path);
/*
for(size_t i=0;i<scaling.traj.ds.size();i++) {
printf("time %g: rate %g\n",scaling.traj.times[i],scaling.ds[i]);
}
*/
{
cout<<"Saving dynamically optimized path to temp_opt.path"<<endl;
ofstream out("temp_opt.path",ios::out);
int numdivs = 200;
Real dt = scaling.traj.EndTime()/numdivs;
Real t=0;
for(size_t i=0;i<=numdivs;i++) {
Vector x;
scaling.traj.Eval(t,x);
out<<t<<"\t"<<x<<endl;
t += dt;
}
out.close();
}
}
/************************************************************************
Expands the single multi path into all possible sequence variants.
Since this turns out to be the time-limiting process for long de nvoo,
this is implemented using a quick branch and bound that works on
edge variant indices. The SeqPaths are created only for the final
set of sequences.
*************************************************************************/
void PrmGraph::fast_expand_multi_path(const MultiPath& multi_path,
vector<SeqPath>& seq_paths,
score_t min_score,
score_t forbidden_pair_penalty,
int max_num_paths) const
{
const vector<AA_combo>& aa_edge_comobos = config->get_aa_edge_combos();
vector<score_t> max_attainable_scores;
const int num_edges = multi_path.edge_idxs.size();
max_attainable_scores.resize(num_edges+1,0);
int num_path_variants=1;
int e;
for (e=num_edges-1; e>=0; e--)
{
const int e_idx = multi_path.edge_idxs[e];
const MultiEdge& edge = multi_edges[e_idx];
max_attainable_scores[e]= max_attainable_scores[e+1] +
edge.max_variant_score + edge.c_break->score;
num_path_variants *= edge.variant_ptrs.size();
}
const score_t max_path_score = multi_edges[multi_path.edge_idxs[0]].n_break->score +
max_attainable_scores[0];
const score_t min_delta_allowed = min_score - max_path_score;
if (min_delta_allowed>0)
return; // this multipath won't help much
if (num_path_variants == 0)
{
cout << "Error: had an edge with 0 variants!" <<endl;
exit(1);
}
// perform expansion using a heap and condensed path representation
vector<int> var_positions; // holds the edge idxs
vector<int> num_vars;
vector<int> var_edge_positions;
vector< vector< score_t > > var_scores;
var_scores.clear();
var_positions.clear();
num_vars.clear();
int num_aas_in_multipath = 0;
for (e=0; e<num_edges; e++)
{
const int e_idx = multi_path.edge_idxs[e];
const MultiEdge& edge = multi_edges[e_idx];
const int num_vars_in_edge = edge.variant_ptrs.size();
vector<score_t> scores;
num_aas_in_multipath+=edge.num_aa;
if (num_vars_in_edge>1)
{
int i;
for (i=0; i<num_vars_in_edge; i++)
scores.push_back(edge.variant_scores[i]);
var_scores.push_back(scores);
var_positions.push_back(e);
num_vars.push_back(num_vars_in_edge);
var_edge_positions.push_back(num_aas_in_multipath-edge.num_aa);
// cout << e << "\t" << e_idx << "\t" << num_vars_in_edge << "\t" << edge.num_aa << endl;
}
}
// create the SeqPaths from the edge_combos...
const int num_positions = num_aas_in_multipath+1;
SeqPath template_path; // holds common elements to all paths
template_path.n_term_aa = multi_path.n_term_aa;
template_path.c_term_aa = multi_path.c_term_aa;
template_path.n_term_mass = multi_path.n_term_mass;
template_path.c_term_mass = multi_path.c_term_mass;
template_path.multi_path_rank = multi_path.original_rank;
template_path.path_score = 0;
template_path.prm_ptr = (PrmGraph *)this;
template_path.positions.clear();
template_path.positions.resize(num_positions);
template_path.num_forbidden_nodes = multi_path.num_forbidden_nodes;
template_path.path_score -= template_path.num_forbidden_nodes * forbidden_pair_penalty;
int pos_idx=0;
for (e=0; e<multi_path.edge_idxs.size(); e++)
{
const int e_idx = multi_path.edge_idxs[e];
const MultiEdge& edge = get_multi_edge(e_idx);
PathPos& pos = template_path.positions[pos_idx++];
pos.breakage = edge.n_break;
pos.edge_idx = e_idx;
pos.mass = edge.n_break->mass;
pos.node_score = edge.n_break->score;
pos.node_idx = edge.n_idx;
template_path.path_score += pos.node_score;
int *variant_ptr = edge.variant_ptrs[0];
const int num_aa = *variant_ptr++;
int *aas = variant_ptr;
if (edge.variant_ptrs.size() == 1)
{
pos.edge_variant_score = edge.variant_scores[0];
pos.aa = aas[0];
template_path.path_score += pos.edge_variant_score;
}
if (edge.num_aa == 1)
continue;
int j;
for (j=1; j<edge.num_aa; j++)
{
PathPos& pos = template_path.positions[pos_idx++];
pos.breakage = NULL;
pos.aa = aas[j];
}
}
if (pos_idx != num_aas_in_multipath)
{
cout << "Error: mismatches between positions and multipath length!" << endl;
exit(1);
}
PathPos& last_pos = template_path.positions[num_positions-1];
const int last_e_idx = multi_path.edge_idxs[e-1];
const MultiEdge& last_edge = get_multi_edge(last_e_idx);
last_pos.breakage = last_edge.c_break;
last_pos.edge_idx =-1;
last_pos.mass = last_edge.c_break->mass;
last_pos.node_idx = last_edge.c_idx;
last_pos.node_score = last_edge.c_break->score;
template_path.path_score += last_pos.node_score;
const int num_multi_var_edges = var_positions.size();
if (num_multi_var_edges == 0)
{
seq_paths.resize(1);
seq_paths[0]=template_path;
return;
}
vector<var_combo> combo_heap;
vector<int> idxs;
idxs.resize(num_multi_var_edges,0);
const int last_var_pos = num_multi_var_edges-1;
const int last_var_val = num_vars[last_var_pos];
const score_t needed_var_score = min_score - template_path.path_score;
while (1)
{
score_t idxs_score = 0;
int k;
for (k=0; k<idxs.size(); k++)
idxs_score += var_scores[k][idxs[k]];
if (idxs_score>=needed_var_score)
{
if (combo_heap.size()<max_num_paths)
{
var_combo v;
v.path_score = template_path.path_score + idxs_score;
v.var_idxs = idxs;
combo_heap.push_back(v);
if (combo_heap.size()== max_num_paths)
make_heap(combo_heap.begin(),combo_heap.end());
}
else
{
const score_t score = template_path.path_score + idxs_score;
if (score>combo_heap[0].path_score)
{
pop_heap(combo_heap.begin(),combo_heap.end());
combo_heap[max_num_paths-1].path_score = score;
combo_heap[max_num_paths-1].var_idxs = idxs;
push_heap(combo_heap.begin(),combo_heap.end());
}
}
}
int j=0;
while (j<num_multi_var_edges)
{
idxs[j]++;
if (idxs[j]==num_vars[j])
{
idxs[j++]=0;
}
else
break;
}
if (j==num_multi_var_edges)
break;
}
seq_paths.clear();
seq_paths.resize(combo_heap.size(),template_path);
int i;
for (i=0; i<combo_heap.size(); i++)
{
const var_combo& combo = combo_heap[i];
SeqPath& seq_path = seq_paths[i];
// fill in info that changes with multi-variant edges
int j;
for (j=0; j<num_multi_var_edges; j++)
{
const int var_pos = var_positions[j];
const int e_idx = multi_path.edge_idxs[var_pos];
const MultiEdge& edge = get_multi_edge(e_idx);
const int pos_idx = var_edge_positions[j];
if (seq_path.positions[pos_idx].edge_idx != e_idx)
{
cout << "POS: " << pos_idx << endl;
cout << "Error: mismatch in pos_idx and e_idx of a multipath!" << endl;
cout << "looking for " << e_idx << endl;
cout << "edge idxs:" << endl;
int k;
for (k=0; k<seq_path.positions.size(); k++)
cout << k << "\t" << seq_path.positions[k].edge_idx << "\tnidx: " <<
seq_path.positions[k].node_idx << endl;
cout << endl;
multi_path.print(config);
this->print();
exit(1);
}
PathPos& pos = seq_path.positions[pos_idx];
const int variant_idx = combo.var_idxs[j];
int *variant_ptr = edge.variant_ptrs[variant_idx];
const int num_aa = *variant_ptr++;
int *aas = variant_ptr;
if (num_aa != edge.num_aa)
{
cout << "Error: edge and variant mixup!" << endl;
exit(1);
}
pos.edge_variant_score = edge.variant_scores[variant_idx];
pos.aa = aas[0];
// pos.edge_var_idx = variant_idx;
if (edge.num_aa == 1)
continue;
int k;
for (k=1; k<edge.num_aa; k++)
{
PathPos& pos = seq_path.positions[pos_idx+k];
pos.aa = aas[k];
}
}
//seq_path.path_score = max_path_score + combo.path_delta;
seq_path.path_score=combo.path_score;
seq_path.make_seq_str(config);
}
}
