Exemple #1
0
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());
}
Exemple #2
0
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;
}
Exemple #4
0
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;
}
Exemple #5
0
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;
}
Exemple #6
0
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;
}
Exemple #7
0
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);
}
Exemple #10
0
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;
}
Exemple #11
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;
}
Exemple #12
0
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;
}