void Field::write(const H5::CommonFG &loc, const H5::H5Location &parent) const {
assert(invariant());
auto group = loc.createGroup(name());
H5::createAttribute(group, "type", project()->enumtype, "Field");
H5::createAttribute(group, "name", name());
// H5::createHardLink(group, "project", parent, ".");
H5::createHardLink(group, "..", parent, ".");
H5::createSoftLink(group, "project", "..");
// H5::createHardLink(group, "configuration", parent,
// "configurations/" + configuration->name());
H5::createSoftLink(group, "configuration",
"../configurations/" + configuration()->name());
H5::createHardLink(group, "project/configurations/" +
configuration()->name() + "/fields",
name(), group, ".");
// H5::createHardLink(group, "manifold", parent,
// "manifolds/" + manifold->name());
H5::createSoftLink(group, "manifold", "../manifolds/" + manifold()->name());
H5::createHardLink(group,
"project/manifolds/" + manifold()->name() + "/fields",
name(), group, ".");
// H5::createHardLink(group, "tangentspace", parent,
// "tangentspaces/" + tangentspace->name());
H5::createSoftLink(group, "tangentspace",
"../tangentspaces/" + tangentspace()->name());
H5::createHardLink(group, "project/tangentspaces/" + tangentspace()->name() +
"/fields",
name(), group, ".");
// H5::createHardLink(group, "tensortype", parent,
// "tensortypes/" + tensortype->name());
H5::createSoftLink(group, "tensortype",
"../tensortypes/" + tensortype()->name());
H5::createGroup(group, "discretefields", discretefields());
}
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);
}
}
}
ostream &SubDiscretization::output(ostream &os, int level) const {
os << indent(level) << "SubDiscretization " << quote(name()) << ": Manifold "
<< quote(manifold()->name()) << " parent Discretization "
<< quote(parent_discretization()->name()) << " child Discretization "
<< quote(child_discretization()->name()) << " factor=" << factor()
<< " offset=" << offset() << "\n";
return os;
}
vector<vector<complex<double> > > fChannel(
vector<vector<complex<double> > > symbolsIn, vector<int> delay,
vector<complex<double> > beta, vector<struct DOAStruct> DOA, double SNR,
vector<vector<double> > array)
{
vector<vector<complex<double> > > out;
// compute the standard deviation for gaussian random numbers
double stdDev = sqrt(2/ pow(10.0,0.1*SNR) );
// find length of longest message
int longestMsg = 0;
for (int i = 0; i < symbolsIn.size(); i++)
if (symbolsIn[i].size() > longestMsg) longestMsg = symbolsIn[i].size();
// compute array manifold vector
int nOutputs = array.size();
int nInputs = symbolsIn.size(); // number of inputs
vector<vector<complex<double> > > manifold(nOutputs,
vector<complex<double> >(nInputs, complex<double>(0.0, 0.0)));
for (int i = 0; i < nOutputs; i++)
{
for (int k = 0; k < nInputs; k++)
{
double scaled_phase = 0.0;
scaled_phase += cos(DOA[k].azimuth) * cos(DOA[i].elevation) * array[i][0];
scaled_phase += sin(DOA[k].azimuth) * cos(DOA[i].elevation) * array[i][1];
scaled_phase += sin(DOA[k].elevation) * array[i][2];
manifold[i][k] = complex<double> (cos(3.14159*scaled_phase),
sin(3.14159*scaled_phase));
}
}
int outLength = longestMsg;
out.resize(nOutputs);
for (int i = 0; i < nOutputs; i++)
out[i].resize(outLength);
for (int i = 0; i < outLength; i++)
{
for (int k = 0; k < nOutputs; k++)
{
complex<double> tmp(0.0, 0.0);
for (int b = 0; b < nInputs; b++)
{
tmp += beta[b]*manifold[k][b]*symbolsIn[b][(i+symbolsIn[b].size()-delay[k]) % symbolsIn[b].size()];
}
tmp += whiteGaussianNoise(0.0, stdDev);
out[k][i] = tmp;
}
}
return out;
}
ostream &Field::output(ostream &os, int level) const {
os << indent(level) << "Field " << quote(name()) << ": Configuration "
<< quote(configuration()->name()) << " Manifold "
<< quote(manifold()->name()) << " TangentSpace "
<< quote(tangentspace()->name()) << " TensorType "
<< quote(tensortype()->name()) << "\n";
for (const auto &df : discretefields())
df.second->output(os, level + 1);
return os;
}
shared_ptr<DiscreteField>
Field::createDiscreteField(const string &name,
const shared_ptr<Configuration> &configuration,
const shared_ptr<Discretization> &discretization,
const shared_ptr<Basis> &basis) {
assert(configuration->project().get() == project().get());
assert(discretization->manifold().get() == manifold().get());
assert(basis->tangentspace().get() == tangentspace().get());
auto discretefield = DiscreteField::create(
name, shared_from_this(), configuration, discretization, basis);
checked_emplace(m_discretefields, discretefield->name(), discretefield,
"Field", "discretefields");
assert(discretefield->invariant());
return discretefield;
}
void Field::merge(const shared_ptr<Field> &field) {
assert(project()->name() == field->project()->name());
assert(m_configuration->name() == field->configuration()->name());
assert(m_manifold->name() == field->manifold()->name());
assert(m_tangentspace->name() == field->tangentspace()->name());
assert(m_tensortype->name() == field->tensortype()->name());
for (const auto &iter : field->discretefields()) {
const auto &discretefield = iter.second;
if (!m_discretefields.count(discretefield->name()))
createDiscreteField(
discretefield->name(), project()->configurations().at(
discretefield->configuration()->name()),
manifold()->discretizations().at(
discretefield->discretization()->name()),
tangentspace()->bases().at(discretefield->basis()->name()));
m_discretefields.at(discretefield->name())->merge(discretefield);
}
}
void SubDiscretization::write(const H5::CommonFG &loc,
const H5::H5Location &parent) const {
assert(invariant());
auto group = loc.createGroup(name());
H5::createAttribute(group, "type", manifold()->project()->enumtype,
"SubDiscretization");
H5::createAttribute(group, "name", name());
// H5::createHardLink(group, "manifold", parent, ".");
H5::createHardLink(group, "..", parent, ".");
H5::createSoftLink(group, "manifold", "..");
// H5::createHardLink(group, "parent_discretization", parent,
// string("discretizations/") +
// parent_discretization->name());
H5::createSoftLink(group, "parent_discretization",
string("../discretizations/") +
parent_discretization()->name());
H5::createHardLink(group, string("manifold/discretizations/") +
parent_discretization()->name() +
"/child_discretizations",
name(), group, ".");
// H5::createHardLink(group, "child_discretization", parent,
// string("discretizations/") +
// child_discretization->name());
H5::createSoftLink(group, "child_discretization",
string("../discretizations/") +
child_discretization()->name());
H5::createHardLink(group, string("manifold/discretizations/") +
child_discretization()->name() +
"/parent_discretizations",
name(), group, ".");
auto tmp_factor = factor();
std::reverse(tmp_factor.begin(), tmp_factor.end());
H5::createAttribute(group, "factor", tmp_factor);
auto tmp_offset = offset();
std::reverse(tmp_offset.begin(), tmp_offset.end());
H5::createAttribute(group, "offset", tmp_offset);
}
Raytracer::Raytracer(): texture(0)
{
prepareTargetBuffer(128,128);
std::shared_ptr<Clump3d> clump(new Clump3d);
//
// Cube
//
std::shared_ptr<MeshGeometry3d> geom1(new MeshGeometry3d());
cube(geom1->getMesh());
geom1->meshChanged();
geom1->setColor(GAL::P4d(1.0, 0.0, 0.0, 1.0));
std::shared_ptr<MeshGeometry3d> geom5(new MeshGeometry3d());
cube(geom5->getMesh());
geom5->meshChanged();
geom5->setColor(GAL::P4d(0.0, 0.7, 1.0, 1.0));
//
// Manifold
//
std::shared_ptr<MeshGeometry<Vertex3d> > geom3(new MeshGeometry<Vertex3d>());
manifold(geom3->getMesh());
geom3->meshChanged();
geom3->setColor(GAL::P4d(1.0, 1.0, 0.0, 1.0));
//
// Sphere
//
std::shared_ptr<SphereGeometry3d> geom2(new SphereGeometry3d(0.5));
geom2->setColor(GAL::P4d(0.0, 1.0, 1.0, 1.0));
std::shared_ptr<SphereGeometry3d> geom4(new SphereGeometry3d(0.25));
geom4->setColor(GAL::P4d(1.0, 0.8, 0.0, 1.0));
clump->addGeometry(geom1);
clump->addGeometry(geom2);
clump->addGeometry(geom3);
clump->addGeometry(geom4);
clump->addGeometry(geom5);
geom1->setTranslation(GAL::P3d(-1,-1,0));
geom2->setTranslation(GAL::P3d(0,1,0));
geom3->setTranslation(GAL::P3d(1,0.5,0));
geom3->setLocalTransform(GAL::EulerRotationX(90.0), 0);
geom4->setTranslation(GAL::P3d(1.2,1,0.8));
geom5->setTranslation(GAL::P3d(0,1,-3));
geom5->setLocalTransform(GAL::EulerRotationY(10.0), 0);
scene.addClump(clump);
std::shared_ptr<Light3d> light1(new Light3d());
light1->setPosition(GAL::P3d(1,4,-1));
light1->setDiffuseColor(GAL::P3d(0.7, 0.7, 0.7));
light1->setSpecularColor(GAL::P3d(1.0, 1.0, 1.0));
light1->setShadow(true);
light1->setSoftShadowWidth(0.05);
scene.addLight(light1);
std::shared_ptr<Light3d> light2(new Light3d());
light2->setPosition(GAL::P3d(-1,4,3));
light2->setDiffuseColor(GAL::P3d(0.7, 0.7, 0.7));
light2->setSpecularColor(GAL::P3d(1.0, 1.0, 1.0));
light2->setShadow(true);
light2->setSoftShadowWidth(0.05);
scene.addLight(light2);
camera.setFrustum(-1, 1, -1, 1, -1, -100);
camera.setTranslation(GAL::P3d(1,2,3));
camera.setLocalTransform(GAL::EulerRotationX(30.0) * GAL::EulerRotationY(15.0), 0);
camera.setFSAA(true);
camera.setRecursionDepth(3);
}
bool GenerateAndTimeOptimizeMultiPath(Robot& robot,MultiPath& multipath,Real xtol,Real dt)
{
Timer timer;
vector<GeneralizedCubicBezierSpline > paths;
if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol))
return false;
printf("Generated interpolating path in time %gs\n",timer.ElapsedTime());
RobotCSpace cspace(robot);
RobotGeodesicManifold manifold(robot);
for(size_t i=0;i<multipath.sections.size();i++) {
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].space = &cspace;
paths[i].segments[j].manifold = &manifold;
}
for(int iters=0;iters<gNumTimescaleBisectIters;iters++)
paths[i].Bisect();
}
#if SAVE_INTERPOLATING_CURVES
int index=0;
printf("Saving sections, element %d to section_x_bezier.csv\n",index);
for(size_t i=0;i<paths.size();i++) {
{
stringstream ss;
ss<<"section_"<<i<<"_bezier.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"duration,x0,x1,x2,x3"<<endl;
for(size_t j=0;j<paths[i].segments.size();j++) {
out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_vel.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"v(0),v(0.5),v(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Deriv(0,temp);
temp /= paths[i].durations[j];
out<<temp[index];
paths[i].segments[j].Deriv(0.5,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
paths[i].segments[j].Deriv(1,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
out<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_acc.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"a(0),a(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Accel(0,temp);
temp /= Sqr(paths[i].durations[j]);
out<<temp[index];
paths[i].segments[j].Accel(1,temp);
temp /= Sqr(paths[i].durations[j]);
out<<","<<temp[index];
out<<endl;
}
out.close();
}
}
#endif //SAVE_INTERPOLATING_CURVES
#if SAVE_LORES_INTERPOLATING_CURVES
paths.clear();
if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*2.0))
return false;
for(size_t i=0;i<multipath.sections.size();i++) {
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].space = &cspace;
paths[i].segments[j].manifold = &manifold;
}
}
for(size_t i=0;i<paths.size();i++) {
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_x2.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"duration,x0,x1,x2,x3"<<endl;
for(size_t j=0;j<paths[i].segments.size();j++) {
out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_vel_x2.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"v(0),v(0.5),v(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Deriv(0,temp);
temp /= paths[i].durations[j];
out<<temp[index];
paths[i].segments[j].Deriv(0.5,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
paths[i].segments[j].Deriv(1,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
out<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_acc_x2.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"a(0),a(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Accel(0,temp);
temp /= Sqr(paths[i].durations[j]);
out<<temp[index];
paths[i].segments[j].Accel(1,temp);
temp /= Sqr(paths[i].durations[j]);
out<<","<<temp[index];
out<<endl;
}
out.close();
}
}
paths.clear();
if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*4.0))
return false;
for(size_t i=0;i<multipath.sections.size();i++) {
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].space = &cspace;
paths[i].segments[j].manifold = &manifold;
}
}
for(size_t i=0;i<paths.size();i++) {
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_x4.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"duration,x0,x1,x2,x3"<<endl;
for(size_t j=0;j<paths[i].segments.size();j++) {
out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_vel_x4.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"v(0),v(0.5),v(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Deriv(0,temp);
temp /= paths[i].durations[j];
out<<temp[index];
paths[i].segments[j].Deriv(0.5,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
paths[i].segments[j].Deriv(1,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
out<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_acc_x4.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"a(0),a(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Accel(0,temp);
temp /= Sqr(paths[i].durations[j]);
out<<temp[index];
paths[i].segments[j].Accel(1,temp);
temp /= Sqr(paths[i].durations[j]);
out<<","<<temp[index];
out<<endl;
}
out.close();
}
}
paths.clear();
if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*8.0))
return false;
for(size_t i=0;i<multipath.sections.size();i++) {
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].space = &cspace;
paths[i].segments[j].manifold = &manifold;
}
}
for(size_t i=0;i<paths.size();i++) {
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_x8.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"duration,x0,x1,x2,x3"<<endl;
for(size_t j=0;j<paths[i].segments.size();j++) {
out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_vel_x8.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"v(0),v(0.5),v(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Deriv(0,temp);
temp /= paths[i].durations[j];
out<<temp[index];
paths[i].segments[j].Deriv(0.5,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
paths[i].segments[j].Deriv(1,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
out<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_acc_x8.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"a(0),a(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Accel(0,temp);
temp /= Sqr(paths[i].durations[j]);
out<<temp[index];
paths[i].segments[j].Accel(1,temp);
temp /= Sqr(paths[i].durations[j]);
out<<","<<temp[index];
out<<endl;
}
out.close();
}
}
#endif //SAVE_LORES_INTERPOLATING_CURVES
//concatenate sections into a single curve
TimeScaledBezierCurve traj;
vector<int> edgeToSection,sectionEdges(1,0);
for(size_t i=0;i<multipath.sections.size();i++) {
traj.path.Concat(paths[i]);
for(size_t j=0;j<paths[i].segments.size();j++)
edgeToSection.push_back((int)i);
sectionEdges.push_back(sectionEdges.back()+(int)paths[i].segments.size());
}
timer.Reset();
bool res=traj.OptimizeTimeScaling(robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
if(!res) {
printf("Failed to optimize time scaling\n");
return false;
}
else {
printf("Optimized into a path with duration %g, (took %gs)\n",traj.EndTime(),timer.ElapsedTime());
}
double T = traj.EndTime();
int numdivs = (int)Ceil(T/dt);
printf("Discretizing at time resolution %g\n",T/numdivs);
numdivs++;
Vector x,v;
int sCur = -1;
for(int i=0;i<numdivs;i++) {
Real t=T*Real(i)/Real(numdivs-1);
int trajEdge = traj.timeScaling.TimeToSegment(t);
if(trajEdge == (int)edgeToSection.size()) trajEdge--; //end of path
Assert(trajEdge < (int)edgeToSection.size());
int s=edgeToSection[trajEdge];
if(s < sCur) {
fprintf(stderr,"Strange: edge index is going backward? %d -> %d\n",sCur,s);
fprintf(stderr," time %g, division %d, traj segment %d\n",t,i,trajEdge);
}
Assert(s - sCur >=0);
while(sCur < s) {
//close off the current section and add a new one
Real switchTime=traj.timeScaling.times[sectionEdges[sCur+1]];
Assert(switchTime <= t);
traj.Eval(switchTime,x);
traj.Deriv(switchTime,v);
if(sCur >= 0) {
multipath.sections[sCur].times.push_back(switchTime);
multipath.sections[sCur].milestones.push_back(x);
multipath.sections[sCur].velocities.push_back(v);
}
multipath.sections[sCur+1].milestones.resize(0);
multipath.sections[sCur+1].velocities.resize(0);
multipath.sections[sCur+1].times.resize(0);
multipath.sections[sCur+1].milestones.push_back(x);
multipath.sections[sCur+1].velocities.push_back(v);
multipath.sections[sCur+1].times.push_back(switchTime);
sCur++;
}
if(t == multipath.sections[s].times.back()) continue;
traj.Eval(t,x);
traj.Deriv(t,v);
multipath.sections[s].times.push_back(t);
multipath.sections[s].milestones.push_back(x);
multipath.sections[s].velocities.push_back(v);
}
#if DO_TEST_TRIANGULAR
timer.Reset();
TimeScaledBezierCurve trajTri;
Real Ttrap = 0;
printf("%d paths?\n",paths.size());
for(size_t i=0;i<paths.size();i++)
Ttrap += OptimalTriangularTimeScaling(paths[i],robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax,trajTri);
printf("Optimal trapezoidal time scaling duration %g, calculated in %gs\n",Ttrap,timer.ElapsedTime());
printf("Saving plot to opt_tri_multipath.csv\n");
trajTri.Plot("opt_tri_multipath.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
#endif // DO_TEST_TRAPEZOIDAL
#if DO_CHECK_BOUNDS
CheckBounds(robot,traj,dt);
#endif // DO_CHECK_BOUNDS
#if DO_SAVE_PLOT
printf("Saving plot to opt_multipath.csv\n");
traj.Plot("opt_multipath.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
#endif //DO_SAVE_PLOT
#if DO_SAVE_LIMITS
SaveLimits(robot,traj,dt,"opt_multipath_limits.csv");
#endif // DO_SAVE_LIMITS
{
multipath.settings.set("resolution",xtol);
multipath.settings.set("program","GenerateAndTimeOptimizeMultiPath");
}
return true;
}
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;
}
bool TimeOptimizePath(Robot& robot,const vector<Real>& oldtimes,const vector<Config>& oldconfigs,Real dt,vector<Real>& newtimes,vector<Config>& newconfigs)
{
//make a smooth interpolator
Vector dx0,dx1,temp;
RobotCSpace cspace(robot);
RobotGeodesicManifold manifold(robot);
TimeScaledBezierCurve traj;
traj.path.durations.resize(oldconfigs.size()-1);
for(size_t i=0;i+1<oldconfigs.size();i++) {
traj.path.durations[i] = oldtimes[i+1]-oldtimes[i];
if(!(traj.path.durations[i] > 0)) {
fprintf(stderr,"TimeOptimizePath: input path does not have monotonically increasing time: segment %d range [%g,%g]\n",i,oldtimes[i],oldtimes[i+1]);
return false;
}
Assert(traj.path.durations[i] > 0);
}
traj.path.segments.resize(oldconfigs.size()-1);
for(size_t i=0;i+1<oldconfigs.size();i++) {
traj.path.segments[i].space = &cspace;
traj.path.segments[i].manifold = &manifold;
traj.path.segments[i].x0 = oldconfigs[i];
traj.path.segments[i].x3 = oldconfigs[i+1];
if(i > 0) {
InterpolateDerivative(robot,oldconfigs[i],oldconfigs[i+1],dx0);
InterpolateDerivative(robot,oldconfigs[i],oldconfigs[i-1],temp);
dx0 -= temp*(traj.path.durations[i]/traj.path.durations[i-1]);
dx0 *= 0.5;
}
else dx0.resize(0);
if(i+2 < oldconfigs.size()) {
InterpolateDerivative(robot,oldconfigs[i+1],oldconfigs[i+2],dx1);
InterpolateDerivative(robot,oldconfigs[i+1],oldconfigs[i],temp);
dx1 *= traj.path.durations[i]/traj.path.durations[i+1];
dx1 -= temp;
dx1 *= 0.5;
}
else dx1.resize(0);
traj.path.segments[i].SetNaturalTangents(dx0,dx1);
}
Timer timer;
bool res=traj.OptimizeTimeScaling(robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
if(!res) {
printf("Failed to optimize time scaling\n");
return false;
}
else {
printf("Optimized into a path with duration %g, (took %gs)\n",traj.EndTime(),timer.ElapsedTime());
}
double T = traj.EndTime();
int numdivs = (int)Ceil(T/dt);
printf("Discretizing at time resolution %g\n",T/numdivs);
numdivs++;
newtimes.resize(numdivs);
newconfigs.resize(numdivs);
for(int i=0;i<numdivs;i++) {
newtimes[i] = T*Real(i)/Real(numdivs-1);
traj.Eval(newtimes[i],newconfigs[i]);
}
#if DO_CHECK_BOUNDS
CheckBounds(robot,traj,newtimes);
#endif //DO_CHECKBOUNDS
return true;
}
