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;
}
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;
}
}