PyObject* IKSolver::solve(int iters,double tol)
{
RobotIKFunction f(*robot.robot);
vector<IKGoal> goals(objectives.size());
for(size_t i=0;i<objectives.size();i++)
goals[i] = objectives[i].goal;
f.UseIK(goals);
if(activeDofs.empty()) GetDefaultIKDofs(*robot.robot,goals,f.activeDofs);
else f.activeDofs.mapping = activeDofs;
RobotIKSolver solver(f);
if(useJointLimits) {
if(qmin.empty())
solver.UseJointLimits();
else
solver.UseJointLimits(Vector(qmin),Vector(qmax));
}
solver.solver.verbose = 0;
bool res = solver.Solve(tol,iters);
robot.robot->UpdateGeometry();
PyObject* tuple = PyTuple_New(2);
PyTuple_SetItem(tuple,0,PyBool_FromLong(res));
PyTuple_SetItem(tuple,1,PyInt_FromLong(iters));
return tuple;
}
void IKSolver::getActiveDofs(std::vector<int>& out)
{
if(activeDofs.empty()) {
vector<IKGoal> goals(objectives.size());
for(size_t i=0;i<objectives.size();i++)
goals[i] = objectives[i].goal;
ArrayMapping map;
GetDefaultIKDofs(*robot.robot,goals,map);
out = map.mapping;
}
else {
out = activeDofs;
}
}
void IKSolver::getJacobian(std::vector<std::vector<double> >& out)
{
RobotIKFunction f(*robot.robot);
vector<IKGoal> goals(objectives.size());
for(size_t i=0;i<objectives.size();i++)
goals[i] = objectives[i].goal;
f.UseIK(goals);
if(activeDofs.empty()) GetDefaultIKDofs(*robot.robot,goals,f.activeDofs);
else f.activeDofs.mapping = activeDofs;
Vector x(f.activeDofs.Size());
Matrix J;
f.GetState(x);
J.resize(f.NumDimensions(),x.n);
f.Jacobian(x,J);
//copy to out
copy(J,out);
}
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;
}
