Esempio n. 1
0
bool CheckBounds(Robot& robot,const TimeScaledBezierCurve& traj,const vector<Real>& times)
{
  Vector v,a,maxv,maxa;
  Vector oldv,diffa,maxdiffa;
  bool res=true;
  for(size_t i=0;i<times.size();i++) {
    //double-checking velocity and acceleration bounds
    traj.Deriv(times[i],v);
    traj.Accel(times[i],a);
    if(i==0) { maxv=v; maxa=a; }
    else {
      for(int j=0;j<v.n;j++) {
	maxv[j] = Max(maxv[j],Abs(v[j]));
	maxa[j] = Max(maxa[j],Abs(a[j]));
      }
    }
    for(int j=0;j<v.n;j++) {
      if(Abs(v[j]) > robot.velMax[j]+1e-3) {
	printf("Exceeded vel max %s=%g at time %g\n",robot.LinkName(j).c_str(),v(j),times[i]);
	res = false;
      }
      if(Abs(a[j]) > robot.accMax[j]+1e-3) {
	printf("Exceeded accel max %s=%g at time %g\n",robot.LinkName(j).c_str(),a(j),times[i]);
	res = false; 
      }
    }
    if(!oldv.empty()) {
      diffa = (v-oldv)/(times[i]-times[i-1]);
      for(int j=0;j<v.n;j++) {
	if(Abs(diffa[j]) > robot.accMax[j]+1e-3) {
	  printf("Diff accel max %s=%g at time %g\n",robot.LinkName(j).c_str(),diffa(j),times[i]);
	  res = false;
	}
      }	    
    }
    oldv = v;
  }
  cout<<"Max vel "<<maxv<<endl;
  cout<<"Max accel "<<maxa<<endl;
  cout<<"End vel "<<v<<endl;
  cout<<"End accel "<<a<<endl;
  return res;
}
Esempio n. 2
0
void SaveLimits(Robot& robot,const TimeScaledBezierCurve& traj,Real dt,const char* fn)
{
  Real T=traj.EndTime();
  int numdivs = (int)Ceil(T/dt);
  printf("Saving time-scaled values to %s\n",fn);
  ofstream out(fn,ios::out);
  out<<"t";
  for(size_t i=0;i<robot.links.size();i++)
    out<<",q["<<robot.linkNames[i]<<"]";
  for(size_t i=0;i<robot.links.size();i++)
    out<<",v["<<robot.linkNames[i]<<"]";
  for(size_t i=0;i<robot.links.size();i++)
    out<<",a["<<robot.linkNames[i]<<"]";
  out<<",activeVLimit,activeAlimit,vsaturation,asaturation";
  out<<endl;

  Vector q,v,a,maxv,maxa;
  for(int i=0;i<=numdivs;i++) {
    //double-checking velocity and acceleration bounds
    Real t = Real(i)/Real(numdivs)*T;
    traj.Eval(t,q);
    traj.Deriv(t,v);
    traj.Accel(t,a);
    out<<t;
    for(int j=0;j<q.n;j++)
      out<<","<<q(j);
    for(int j=0;j<v.n;j++)
      out<<","<<v(j);
    for(int j=0;j<a.n;j++)
      out<<","<<a(j);
    Real maxVSat=0,maxASat=0;
    int maxVSatInd=0,maxASatInd=0;
    for(int j=0;j<v.n;j++)
      if(Abs(v(j))/robot.velMax(j) > maxVSat) {
	maxVSat = Abs(v(j))/robot.velMax(j);
	maxVSatInd = j;
      }
    for(int j=0;j<a.n;j++)
      if(Abs(a(j))/robot.accMax(j) > maxASat) {
	maxASat = Abs(a(j))/robot.accMax(j);
	maxASatInd = j;
      }
    out<<","<<robot.linkNames[maxVSatInd];
    out<<","<<robot.linkNames[maxASatInd];
    out<<","<<maxVSat<<","<<maxASat<<endl;
  }
  out<<endl;
  for(size_t i=0;i<traj.timeScaling.times.size();i++) {
    //double-checking velocity and acceleration bounds
    Real t = traj.timeScaling.times[i];
    traj.Eval(t,q);
    traj.Deriv(t,v);
    traj.Accel(t,a);
    out<<t;
    for(int j=0;j<q.n;j++)
      out<<","<<q(j);
    for(int j=0;j<v.n;j++)
      out<<","<<v(j);
    for(int j=0;j<a.n;j++)
      out<<","<<a(j);
    Real maxSat=0;
    int maxSatInd=0;
    bool maxSatA=false;
    for(int j=0;j<v.n;j++)
      if(Abs(v(j))/robot.velMax(j) > maxSat) {
	maxSat = Abs(v(j))/robot.velMax(j);
	maxSatInd = j;
      }
    for(int j=0;j<a.n;j++)
      if(Abs(a(j))/robot.accMax(j) > maxSat) {
	maxSat = Abs(a(j))/robot.accMax(j);
	maxSatInd = j;
	maxSatA=true;
      }
    if(maxSatA)
      out<<",a["<<robot.linkNames[maxSatInd]<<"]";
    else
      out<<",v["<<robot.linkNames[maxSatInd]<<"]";
    out<<","<<maxSat<<endl;
  }
}