void GetFlatStance(RobotWithGeometry& robot,Real tol,Stance& s,Real kFriction)
{
ContactFormation formation;
GetFlatContacts(robot,tol,formation);
LocalContactsToStance(formation,robot,s);
for(Stance::iterator i=s.begin();i!=s.end();i++)
for(size_t j=0;j<i->second.contacts.size();j++)
i->second.contacts[j].kFriction = kFriction;
}
bool ConstraintChecker::HasContact(const Robot& robot,const Stance& stance,Real dist)
{
Vector res;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const IKGoal& g=i->second.ikConstraint;
res.resize(IKGoal::NumDims(g.posConstraint)+IKGoal::NumDims(g.rotConstraint));
EvalIKError(g,robot.links[i->first].T_World,res);
if(res.maxAbsElement() > dist) return false;
}
return true;
}
void LocalContactsToStance(const ContactFormation& contacts,const RobotKinematics3D& robot,Stance& stance)
{
stance.clear();
for(size_t i=0;i<contacts.contacts.size();i++) {
Hold h;
LocalContactsToHold(contacts.contacts[i],contacts.links[i],robot,h);
if(!contacts.targets.empty())
h.ikConstraint.destLink = contacts.targets[i];
stance.insert(h);
}
}
bool ConstraintChecker::HasSupportPolygon(const Robot& robot,const Stance& stance,const Vector3& gravity,int numFCEdges)
{
vector<ContactPoint> cps(NumContactPoints(stance));
int k=0;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const Hold& h=i->second;
for(size_t j=0;j<h.contacts.size();j++,k++)
cps[k]=h.contacts[j];
}
Vector3 com = robot.GetCOM();
vector<Vector3> f;
return TestCOMEquilibrium(cps,gravity,numFCEdges,com,f);
}
Real ConstraintChecker::ContactDistance(const Robot& robot,const Stance& stance)
{
Real maxErr = 0;
Vector res;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const IKGoal& g=i->second.ikConstraint;
res.resize(IKGoal::NumDims(g.posConstraint)+IKGoal::NumDims(g.rotConstraint));
EvalIKError(g,robot.links[i->first].T_World,res);
Real m=res.maxAbsElement();
if(m > maxErr) maxErr = m;
}
return maxErr;
}
void Grasp::SetStance(const Stance& s)
{
constraints.resize(s.size());
contacts.resize(0);
contactLinks.resize(0);
int k=0;
for(Stance::const_iterator i=s.begin();i!=s.end();i++,k++) {
constraints[k] = i->second.ikConstraint;
contacts.insert(contacts.end(),i->second.contacts.begin(),i->second.contacts.end());
contactLinks.resize(contacts.size(),i->first);
}
fixedDofs.resize(0);
fixedValues.resize(0);
}
bool ConstraintChecker::HasContactVelocity(const Robot& robot,const Stance& stance,Real maxErr)
{
Vector res;
Vector3 dw,dv;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const IKGoal& g=i->second.ikConstraint;
res.resize(IKGoal::NumDims(g.posConstraint)+IKGoal::NumDims(g.rotConstraint));
robot.GetWorldAngularVelocity(g.link,robot.dq,dw);
robot.GetWorldVelocity(Vector3(Zero),g.link,robot.dq,dv);
EvalIKGoalDeriv(g,robot.links[i->first].T_World,dw,dv,res);
if(res.maxAbsElement() > maxErr) return false;
}
return true;
}
bool ConstraintChecker::HasSupportPolygon_Robust(const Robot& robot,const Stance& stance,const Vector3& gravity,Real robustnessFactor,int numFCEdges)
{
vector<ContactPoint> cps(NumContactPoints(stance));
int k=0;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const Hold& h=i->second;
for(size_t j=0;j<h.contacts.size();j++,k++)
cps[k]=h.contacts[j];
}
Vector3 com = robot.GetCOM();
EquilibriumTester eq;
eq.Setup(cps,gravity,numFCEdges,com);
eq.SetRobustnessFactor(robustnessFactor);
return eq.TestCurrent();
}
void Grasp::GetStance(Stance& s) const
{
s.clear();
for(size_t i=0;i<constraints.size();i++)
s[constraints[i].link].ikConstraint = constraints[i];
for(size_t i=0;i<contacts.size();i++)
s[contactLinks[i]].contacts.push_back(contacts[i]);
}
bool ConstraintChecker::HasEnvCollision(Robot& robot,Terrain& env,const Stance& stance, const vector<int>& ignoreList)
{
robot.UpdateGeometry();
robot.InitMeshCollision(*env.geometry);
vector<bool> fixed(robot.links.size(), false);
for (Stance::const_iterator i = stance.begin(); i != stance.end(); i++)
fixed[i->first] = true;
for (int i = 0; i < ignoreList.size(); i++)
fixed[ignoreList[i]] = true;
for (size_t i = 0; i < robot.links.size(); i++) {
if (!fixed[i]) {
if (robot.MeshCollision(i)) {
return true;
}
}
}
return false;
}
/** @brief Performs path planning in collision-free space for the
* given robot at the stance s between start configuration qstart and
* destination qgoal
* Stability is *not* tested. If you wish to maintain stability use
* StancePlan
*
* The output is given in path. cond controls the number of iterations/time
* for planning.
*
* The constraint specifications are given in WorldPlannerSettings. If you
* have custom requirements, you will need to set them up.
*/
bool ContactPlan(RobotWorld& world,int robot,Config& qstart,Config& qgoal,const Stance& s,MilestonePath& path,
const HaltingCondition& cond,const string& plannerSettings="")
{
WorldPlannerSettings settings;
settings.InitializeDefault(world);
//do more constraint setup here (modifying the settings object) if desired,
//e.g., set collision margins, edge collision checking resolution, etc.
ContactCSpace cspace(world,robot,&settings);
for(Stance::const_iterator i=s.begin();i!=s.end();i++)
cspace.AddContact(i->second.ikConstraint);
//sanity checking
if(!cspace.CheckContact(qstart)) {
printf("Start configuration does not meet contact constraint, error %g\n",cspace.ContactDistance(qstart));
if(!cspace.SolveContact()) {
printf(" Solving for contact failed.\n");
return false;
}
else {
printf(" IK problem was solved, using new start configuration\n");
qstart = cspace.GetRobot()->q;
}
}
if(!cspace.CheckContact(qgoal)) {
printf("Goal configuration does not meet contact constraint, error %g\n",cspace.ContactDistance(qgoal));
if(!cspace.SolveContact()) {
printf(" Solving for contact failed.\n");
return false;
}
else {
printf(" IK problem was solved, using new goal configuration\n");
qgoal = cspace.GetRobot()->q;
}
}
if(!cspace.IsFeasible(qstart)) {
cout<<"Start configuration is infeasible, violated constraints:"<<endl;
vector<bool> infeasible;
cspace.CheckObstacles(qstart,infeasible);
for(size_t i=0;i<infeasible.size();i++)
if(infeasible[i]) cout<<" "<<cspace.ObstacleName(i)<<endl;
return false;
}
if(!cspace.IsFeasible(qgoal)) {
cout<<"Goal configuration is infeasible, violated constraints:"<<endl;
vector<bool> infeasible;
cspace.CheckObstacles(qgoal,infeasible);
for(size_t i=0;i<infeasible.size();i++)
if(infeasible[i]) cout<<" "<<cspace.ObstacleName(i)<<endl;
return false;
}
MotionPlannerFactory factory;
if(!plannerSettings.empty())
factory.LoadJSON(plannerSettings);
//do more planner setup here if desired, e.g., change planner type,
//perturbation size, connection radius, etc
//make the planner and do the planning
Timer timer;
MotionPlannerInterface* planner = factory.Create(&cspace,qstart,qgoal);
cout<<" Create time: "<<timer.ElapsedTime()<<endl;
string res = planner->Plan(path,cond);
cout<<"Planner terminated with condition "<<res<<endl;
printf(" Stats: SolveContact %d, %gs. IsFeasible %d, %gs\n",cspace.numSolveContact,cspace.solveContactTime,cspace.numIsFeasible,cspace.isFeasibleTime);
delete planner;
return !path.edges.empty();
}
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);
}
}
