int main(int argc, char** argv)
{
const char* addr = "tcp://192.168.1.128:3457";
if(argc <= 1) {
printf("USAGE: RealSense_ROS_Emitter tcp://SERVER_IP_ADDR:PORT\n");
printf(" Running on %s by default\n",addr);
}
File f;
if(!f.Open(addr,FILEREAD|FILEWRITE)) {
printf("Unable to open client to %s... did you run \"RealSenseClient %s\"?\n",addr,addr);
return 1;
}
//initialize ROS and publish messages
ros::init(argc, argv, "RealSense_ROS_Emitter", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
Loop loop(nh);
Timer timer;
double lastPrintTime = 0.0;
while(ros::ok()) {
if(!loop.ReadAndProcess(f)) {
printf("Abnormal termination\n");
return 1;
}
if(timer.ElapsedTime() > lastPrintTime + 1.0) {
double t = timer.ElapsedTime();
cout<<"Read rate: "<<double(loop.bytes_read)/(t-lastPrintTime)/1024/1024<<"mb/s, "<<float(loop.frames_read)/(t-lastPrintTime)<<" images/s"<<endl;
loop.bytes_read = 0;
loop.frames_read = 0;
lastPrintTime = t;
}
}
printf("Terminated due to ros::ok\n");
return 0;
}
void testTimer(int i) {
Timer t;
sleep(i);
t.Stop();
double diff = abs(t.ElapsedTime() - i * 1000000.0);
EXPECT_GE(std::max(t.ElapsedTime() / 20, 100.0), diff);
}
int main()
{
Timer timer;
cout << "sleeping for 1 second and reporting:\n";
timer.Start();
sleep(1);
timer.Pause();
cout << "timed %lf secs\n", timer.ElapsedTime();
timer.Clear();
cout << "sleeping 2 times for 1 second and reporting:\n";
timer.Start();
sleep(1);
timer.Pause();
sleep(1);
timer.Start();
sleep(1);
timer.Pause();
cout << "timed %lf secs\n", timer.ElapsedTime();
return 0;
}
bool SolveIK(RobotIKFunction& f,
Real tolerance,int& iters,int verbose)
{
if(verbose >= 1) {
printf("SolveIK(tol=%f,iters=%d):\n",tolerance,iters);
Timer timer;
RobotIKSolver solver(f);
solver.UseJointLimits(TwoPi);
solver.solver.verbose = verbose;
if(solver.Solve(tolerance,iters)) {
printf(" Succeeded! %f seconds\n", timer.ElapsedTime());
if(verbose >= 2) solver.PrintStats();
return true;
}
else {
printf(" Failed... %f seconds\n", timer.ElapsedTime());
if(verbose >= 2) solver.PrintStats();
return false;
}
}
else {
RobotIKSolver solver(f);
solver.UseJointLimits(TwoPi);
solver.solver.verbose = verbose;
return solver.Solve(tolerance,iters);
}
}
bool ROSWaitForUpdate(const char* topic,double timeout)
{
if(gSubscribers.count(topic) == 0) return false;
ROSSubscriberBase* s = gSubscribers[topic];
int oldNumMessages = s->numMessages;
Timer timer;
while(timer.ElapsedTime() < timeout) {
ros::spinOnce();
ros::Duration(Min(timeout-timer.ElapsedTime(),0.001)).sleep();
if(s->numMessages > oldNumMessages) return true;
}
return false;
}
virtual bool OnMessage(AnyCollection& message) {
cout<<message<<endl;
double t = timer.ElapsedTime();
printf("Time between messages: %g\n",t-lastTime);
lastTime = t;
return true;
}
void RigidObject::InitCollisions()
{
Timer timer;
geometry->InitCollisionData();
double t = timer.ElapsedTime();
if(t > 0.2)
printf("Initialized rigid object %s collision data structures in time %gs\n",geomFile.c_str(),t);
}
void TimeOptimizeTest1DParabolic()
{
Vector vmin(1,-1.0),vmax(1,1.0);
Vector amin(1,-1.0),amax(1,1.0);
//try the path from -1 to 1
int ns [] = {2,4,8,16,32,64,128,256,512,1024};
for(int i=0;i<10;i++) {
int numSegments = ns[i];
vector<Real> divs(numSegments+1);
vector<Vector> vmins(numSegments),vmaxs(numSegments);
vector<Vector> amins(numSegments),amaxs(numSegments);
vector<Vector> vs(numSegments+1);
for(int j=0;j<=numSegments;j++) {
divs[j] = Real(j)/Real(numSegments);
vs[j] = Vector(1,8.0*Abs(divs[j]-0.5));
if(j > 0) {
vmins[j-1] = vs[j-1];
vmaxs[j-1] = vs[j];
if(divs[j] < 0.5)
Swap(vmins[j-1],vmaxs[j-1]);
amins[j-1] = Vector(1,8.0*Sign(divs[j-1]-0.5));
amaxs[j-1] = Vector(1,8.0*Sign(divs[j-1]-0.5));
}
}
Timer timer;
TimeScaling timeScaling;
//timeScaling.ConditionMinTime(divs,vs,vmins,vmaxs,amins,amaxs);
bool res=timeScaling.SolveMinTimeArcLength(vmin,vmax,amin,amax,divs,vs,0.0,0.0);
double time=timer.ElapsedTime();
printf("Num segments: %d\n",numSegments);
printf("Result: %d\n",res);
if(res) {
printf("End time: %g\n",timeScaling.times.back());
printf("Solution time: %g s\n",time);
/*
for(size_t j=0;j<timeScaling.ds.size();j++)
Assert(vmin[0] <= timeScaling.ds[j]*vs[j][0] && timeScaling.ds[j]*vs[j][0] <= vmax[0]);
*/
}
#if PRINT_TIME_SCALING
if(i+1==10) {
cout<<"time,s,ds,val,vmin,vmax,amin,amax,dy,ddy"<<endl;
for(size_t i=0;i<timeScaling.times.size();i++) {
cout<<timeScaling.times[i]<<","<<timeScaling.params[i]<<","<<timeScaling.ds[i]<<","<<4.0*Pow(timeScaling.params[i]-0.5,2.0)*Sign(timeScaling.params[i]-0.5);
if(i<vmins.size()) {
Real dp=8.0*Abs(timeScaling.params[i]-0.5);
Real ddp=8.0*Sign(timeScaling.params[i]-0.5);
cout<<","<<vmins[i][0]<<","<<vmaxs[i][0]<<","<<amins[i][0]<<","<<amaxs[i][0];
cout<<","<<timeScaling.ds[i]*dp<<","<<timeScaling.TimeToParamAccel(i,timeScaling.times[i])*dp+Sqr(timeScaling.ds[i])*ddp<<endl;
}
else cout<<endl;
}
}
#endif // PRINT_TIME_SCALING
}
}
void SafeTrajClient::CalibrateLatency(int iters)
{
if(virtualController) return;
printf("SafeTrajClient: Calibrating latency... \n");
Timer timer;
for (int i=0;i<iters;i++)
this->t.Echo("");
this->latency = timer.ElapsedTime()/iters;
printf(" result %g\n",this->latency);
}
void CollisionPointCloud::InitCollisions()
{
Assert(points.size() > 0);
Timer timer;
bblocal.minimize();
for(size_t i=0;i<points.size();i++)
bblocal.expand(points[i]);
//set up the grid
Real res = gridResolution;
if(gridResolution <= 0) {
Vector3 dims = bblocal.bmax-bblocal.bmin;
Real maxdim = Max(dims.x,dims.y,dims.z);
Real mindim = Min(dims.x,dims.y,dims.z);
//default grid size: assume points are evenly distributed on a 2D manifold
//in space, try to get 50 points per grid cell
Real vol = dims.x*dims.y*dims.z;
//h^2 * n = vol
int ptsPerCell = 50;
Real h = Pow(vol / points.size() * ptsPerCell, 1.0/2.0);
if(h > mindim) {
//TODO: handle relatively flat point clouds
}
res = h;
}
grid.h.set(res);
for(size_t i=0;i<points.size();i++) {
Vector p(3,points[i]);
GridSubdivision::Index ind;
grid.PointToIndex(p,ind);
grid.Insert(ind,&points[i]);
}
printf("CollisionPointCloud::InitCollisions: %d points, res %g, time %gs\n",points.size(),res,timer.ElapsedTime());
//print stats
int nmax = 0;
for(GridSubdivision::HashTable::const_iterator i=grid.buckets.begin();i!=grid.buckets.end();i++)
nmax = Max(nmax,(int)i->second.size());
printf(" %d nonempty grid buckets, max size %d, avg %g\n",grid.buckets.size(),nmax,Real(points.size())/grid.buckets.size());
timer.Reset();
//initialize the octree, 10 points per cell
octree = new OctreePointSet(bblocal,10);
for(size_t i=0;i<points.size();i++)
octree->Add(points[i],(int)i);
printf(" octree initialized in time %gs, %d nodes\n",timer.ElapsedTime(),octree->Size());
/*
//TEST: method 2. Turns out to be much slower
timer.Reset();
octree = new OctreePointSet(bblocal,points.size());
octree->SplitToResolution(res);
for(size_t i=0;i<points.size();i++)
octree->Add(points[i],(int)i);
octree->Collapse(10);
printf(" octree 2 initialized in time %gs, %d nodes\n",timer.ElapsedTime(),octree->Size());
*/
}
void RobotWithGeometry::InitCollisions()
{
Timer timer;
for(size_t i=0;i<geometry.size();i++) {
if(!IsGeometryEmpty(i))
geometry[i]->InitCollisionData();
}
double t = timer.ElapsedTime();
if(t > 0.2)
printf("Initialized robot collision data structures in time %gs\n",t);
}
int main(int argc, const char *argv[])
{
string input_file("words.sorted");
leveldb::DB* db;
leveldb::Options options;
options.create_if_missing = true;
string idx_name = input_file + ".leveldb";
leveldb::Status status = leveldb::DB::Open(options, idx_name.c_str(), &db);
assert(status.ok());
vector<string> tokens;
loadTokens(input_file.c_str(), tokens);
char v[256];
for (int i = 0; i < tokens.size(); ++i) {
string token = tokens[i];
sprintf(v, "%d", (int)token.size());
string value(v);
db->Put(leveldb::WriteOptions(), token, value);
}
int count = tokens.size();
Timer t;
string value;
for (int i = 0; i < count; ++i) {
string token = tokens[i];
db->Get(leveldb::ReadOptions(), token, &value);
//int key_len = 0;
//sscanf(value.data(), "%d", &key_len);
//cout << key_len << " " << token << endl;
//assert(key_len == token.size());
}
t.Stop();
cout << "cpu time(s):" << t.ElapsedTimeCPU() / 1000000
<< " avg(us):" << t.ElapsedTimeCPU()/count << endl
<< "wall time(s):" << t.ElapsedTime() / 1000000
<< " avg(us):" << t.ElapsedTime()/count << endl;
delete db;
return 0;
}
virtual void Handle_Idle() {
if(simulate) {
Timer timer;
string res=uis[currentUI]->UpdateEvent();
sim.Advance(dt);
Refresh();
SleepIdleCallback(int(Max(0.0,dt-timer.ElapsedTime())*1000.0));
}
WorldViewProgram::Handle_Idle();
}
bool Service::WaitForMessage(AnyCollection& message,double timeout)
{
if(!worker) {
fprintf(stderr,"%s::WaitForMessage(): Not connected\n",Name());
return false;
}
Timer timer;
while(timer.ElapsedTime() < timeout) {
if(!worker->initialized) {
fprintf(stderr,"%s::WaitForMessage(): Abnormal disconnection\n",Name());
return false;
}
//read new messages
if(worker->UnreadCount() > 0) {
if(onlyProcessNewest) {
string str = worker->Newest();
stringstream ss(str);
AnyCollection msg;
if(!msg.read(ss)) {
fprintf(stderr,"%s::WaitForMessage(): Got an improperly formatted string\n",Name());
cout<<"String = \""<<str<<"\""<<endl;
return false;
}
if(!OnMessage(msg)) {
fprintf(stderr,"%s::WaitForMessage(): OnMessage returned false\n",Name());
return false;
}
message = msg;
return true;
}
else {
vector<string> msgs = worker->New();
for(size_t i=0;i<msgs.size();i++) {
stringstream ss(msgs[i]);
AnyCollection msg;
if(!msg.read(ss)) {
fprintf(stderr,"%s::WaitForMessage(): Got an improperly formatted string\n",Name());
return false;
}
if(!OnMessage(msg)) {
fprintf(stderr,"%s::WaitForMessage(): OnMessage returned false\n",Name());
return false;
}
}
message = msgs[0];
return true;
}
ThreadSleep(SSPP_MESSAGE_WAIT_TIME);
}
}
return false;
}
/* SpMV kernel implemented with Intel MKL */
void mkl_spmv(spx_index_t *rowptr, spx_index_t *colind, spx_value_t *values,
spx_index_t nrows, spx_index_t ncols, spx_index_t nnz,
spx_value_t *x, spx_value_t *y,
spx_value_t ALPHA, spx_value_t BETA)
{
/* 1. Matrix loading phase */
MKL_INT *pointerB, *pointerE;
char transa;
char matdescra[6];
transa = 'n';
matdescra[0] = 'g';
matdescra[1] = '-';
matdescra[2] = '-';
matdescra[3] = 'c';
pointerB = (MKL_INT *) malloc(sizeof(MKL_INT) * nrows);
pointerE = (MKL_INT *) malloc(sizeof(MKL_INT) * nrows);
for (int i = 0; i < nrows; i++) {
pointerB[i] = rowptr[i];
pointerE[i] = rowptr[i+1];
}
/* 2. SpMV benchmarking phase */
vector<double> mt(OUTER_LOOPS);
for (size_t i = 0; i < OUTER_LOOPS; i++) {
t.Clear();
t.Start();
for (size_t j = 0; j < LOOPS; j++) {
mkl_dcsrmv(&transa, &nrows, &ncols, &ALPHA, matdescra, values,
colind, pointerB, pointerE, x, &BETA, y);
}
t.Pause();
mt[i] = t.ElapsedTime();
}
sort(mt.begin(), mt.end());
double mt_median =
(OUTER_LOOPS % 2) ? mt[((OUTER_LOOPS+1)/2)-1]
: ((mt[OUTER_LOOPS/2-1] + mt[OUTER_LOOPS/2])/2);
double flops = (double)(LOOPS*nnz*2)/((double)1000*1000*mt_median);
cout << "m: " << MATRIX
<< " mt(median): " << mt_median
<< " flops: " << flops << endl;
/* 3. Cleanup */
free(pointerB);
free(pointerE);
}
virtual bool OnIdle() {
bool res=SimGUIBackend::OnIdle();
if(simulate) {
Timer timer;
string res=ui->UpdateEvent();
sim.Advance(dt);
SendRefresh();
SendPauseIdle(int(Max(0.0,dt-timer.ElapsedTime())*1000.0));
return true;
}
return res;
}
bool RunWhile(vector<Service*>& s,bool (*condition)())
{
if(s.empty()) return false;
for(size_t i=0;i<s.size();i++) {
if(!s[i]->OnStart()) {
for(size_t j=0;j<=i;j++)
s[j]->OnStop();
return false;
}
}
Timer timer;
vector<double> nextProcessTimes(s.size(),0.0);
while(condition()) {
double t = timer.ElapsedTime();
bool fired = false;
for(size_t i=0;i<s.size();i++) {
if(nextProcessTimes[i] <= t) {
fired = true;
int n=s[i]->Process();
if(n < 0) {
if(!s[i]->tolerateReadErrors) {
for(size_t j=0;j<s.size();j++)
s[j]->OnStop();
return false;
}
else nextProcessTimes[i] = t+s[i]->waitTime;
}
if(n == 0) {
if(s[i]->sleepTime<=0) nextProcessTimes[i] = t;
else nextProcessTimes[i] = t+s[i]->sleepTime;
}
}
}
if(fired) {
double minNextTime = nextProcessTimes[0];
for(size_t i=1;i<s.size();i++)
minNextTime = Min(minNextTime,nextProcessTimes[1]);
if(minNextTime == t)
ThreadYield();
else if(minNextTime > t)
ThreadSleep(minNextTime - t);
}
}
for(size_t j=0;j<s.size();j++)
s[j]->OnStop();
return true;
}
void TimeOptimizeTest2DCircleBezier()
{
CartesianCSpace space(2);
SphereConstraint sphereConstraint;
SmoothConstrainedInterpolator interp(&space,&sphereConstraint);
interp.ftol = 1e-8;
interp.xtol = 1e-2;
interp.maxNewtonIters = 100;
vector<Vector> pts;
pts.resize(5);
pts[0].resize(2);
pts[1].resize(2);
pts[2].resize(2);
pts[3].resize(2);
pts[4].resize(2);
pts[0](0)=1; pts[0](1)=0;
pts[1](0)=0; pts[1](1)=1;
pts[2](0)=-1; pts[2](1)=0;
pts[3](0)=0; pts[3](1)=-1;
pts[4](0)=1; pts[4](1)=0;
Timer timer;
TimeScaledBezierCurve curve;
MultiSmoothInterpolate(interp,pts,curve.path);
for(size_t i=0;i<curve.path.segments.size();i++)
Assert(curve.path.segments[i].space == &space);
printf("Num segments: %d\n",curve.path.segments.size());
Vector vmin(2,-1.0),vmax(2,1.0);
Vector amin(2,-1.0),amax(2,1.0);
bool res=curve.OptimizeTimeScaling(vmin,vmax,amin,amax);
if(!res) {
printf("Error optimizing path\n");
return;
}
printf("End time: %g\n",curve.EndTime());
printf("Solution time: %g s\n",timer.ElapsedTime());
Assert(curve.timeScaling.times.size()==curve.timeScaling.params.size());
#if PRINT_TIME_SCALING
cout<<"t,s,ds,x,y,dx,dy:"<<endl;
for(size_t i=0;i<curve.timeScaling.times.size();i++) {
Vector x,v;
curve.Eval(curve.timeScaling.times[i],x);
curve.Deriv(curve.timeScaling.times[i],v);
cout<<curve.timeScaling.times[i]<<","<<curve.timeScaling.params[i]<<","<<curve.timeScaling.ds[i]<<","<<x[0]<<","<<x[1]<<","<<v[0]<<","<<v[1]<<endl;
}
#endif
}
std::string MotionPlannerInterface::Plan(MilestonePath& path,const HaltingCondition& cond)
{
path.edges.clear();
bool foundPath = false;
Real lastCheckTime = 0, lastCheckValue = 0;
Timer timer;
for(int iters=0;iters<cond.maxIters;iters++) {
Real t=timer.ElapsedTime();
if(t > cond.timeLimit) {
if(foundPath) {
//get the final path
GetSolution(path);
}
return "timeLimit";
}
//check for cost improvements
if(foundPath && t > lastCheckTime + cond.costImprovementPeriod) {
GetSolution(path);
Real len = path.Length();
if(len < cond.costThreshold)
return "costThreshold";
if(lastCheckValue - len < cond.costImprovementThreshold)
return "costImprovementThreshold";
lastCheckTime = t;
lastCheckValue = len;
}
//do planning, check if a path is found
PlanMore();
if(!foundPath) {
if(IsSolved()) {
foundPath = true;
GetSolution(path);
if(cond.foundSolution) {
return "foundSolution";
}
lastCheckTime = t;
lastCheckValue = path.Length();
}
}
}
if(foundPath) {
//get the final path
GetSolution(path);
}
return "maxIters";
}
void TimeOptimizeTest1DLine()
{
Vector vmin(1,-1.0),vmax(1,1.0);
Vector amin(1,-1.0),amax(1,1.0);
//try the path from 0 to 1
int ns [] = {2,4,8,16,32,64,128,256,512,1024};
for(int i=0;i<10;i++) {
int numSegments = ns[i];
vector<Real> divs(numSegments+1);
vector<Vector> vmins(numSegments),vmaxs(numSegments);
vector<Vector> amins(numSegments),amaxs(numSegments);
vector<Vector> vs(numSegments+1);
for(int j=0;j<=numSegments;j++)
divs[j] = Real(j)/Real(numSegments);
//straight line path from 0 to 3
for(int j=0;j<numSegments;j++) {
vs[j] = Vector(1,3.0);
vmins[j] = Vector(1,3.0);
vmaxs[j] = Vector(1,3.0);
amins[j] = Vector(1,0.0);
amaxs[j] = Vector(1,0.0);
}
vs.back() = Vector(1,3.0);
Timer timer;
TimeScaling timeScaling;
timeScaling.ConditionMinTime(divs,vs,vmins,vmaxs,amins,amaxs);
bool res=timeScaling.SolveMinTime(vmin,vmax,amin,amax,divs,vs,vmins,vmaxs,amins,amaxs,0.0,0.0);
double time=timer.ElapsedTime();
printf("Num segments: %d\n",numSegments);
printf("Result: %d\n",res);
if(res) {
printf("End time: %g\n",timeScaling.times.back());
printf("Solution time: %g s\n",time);
for(size_t j=0;j<timeScaling.ds.size();j++)
Assert(vmin[0] <= timeScaling.ds[j] && timeScaling.ds[j] <= vmax[0]);
}
}
}
int main(){
// Here is the test code
int width, height;
unsigned char *srcImg;
char *str = (char *)"house.pgm";
if (ReadImagePGM(str, (char **)&srcImg, &width, &height) == 0){
printf("Failed opening <%s>\n", str);
return 1;
} //end-if
printf("Working on %dx%d image\n", width, height);
// EDLines Test below
Timer timer;
timer.Start();
int noLines;
LS *lines = DetectLinesByED(srcImg, width, height, &noLines);
timer.Stop();
printf("<%d> line segments detected in <%4.2lf> ms\n", noLines, timer.ElapsedTime());
// Dump the line segments to a file
if (noLines > 0){
FILE *fp = fopen("LineSegments.txt", "w");
fprintf(fp, "#Line segments are of the form: (sx, sy) (ex, ey)\n");
for (int i=0; i<noLines; i++){
fprintf(fp, "(%6.2lf %6.2lf) (%6.2lf %6.2lf)\n", lines[i].sx, lines[i].sy, lines[i].ex, lines[i].ey);
} //end-for
fclose(fp);
} //end-for
delete lines;
delete srcImg;
} //end-main
bool CEntropy::initializePaths(const ob::PlannerTerminationCondition &ptc){
assert(nPaths > 0);
paths.clear();
Timer timer;
while(ptc == false){
CEPath path(nStatesPath);
for(int k = 0; k < nStatesPath; k++){
CEState state(numD);
sampleRandomState(state);
path.states[k] = state;
}
if(this->isPathValid(path))
paths.push_back(path);
if(paths.size() == nPaths){
break;
}
}
initTime += timer.ElapsedTime();
if(paths.size() != nPaths){
return false;
}
return true;
}
void TimeOptimizeTest2DCircle()
{
Vector vmin(2,-1.0),vmax(2,1.0);
Vector amin(2,-1.0),amax(2,1.0);
//path param goes from 0 to 1
int ns [] = {2,4,8,16,32,64,128,256,512,1024};
for(int i=0;i<10;i++) {
int numSegments = ns[i];
vector<Real> divs(numSegments+1);
vector<Vector> vs(numSegments+1);
vector<Vector> vmins(numSegments),vmaxs(numSegments);
vector<Vector> amins(numSegments),amaxs(numSegments);
for(int j=0;j<=numSegments;j++)
divs[j] = Real(j)/Real(numSegments);
//circles
//p(s) = (sin(s*2pi/period),cos(s*2pi/period)
//p'(s) = 2pi/period*(cos(s*2pi/period),-sin(s*2pi/period))
//p''(s) = (2pi/period)^2*(-sin(s*2pi/period),-cos(s*2pi/period))
Real period = 1.0;
for(int j=0;j<numSegments;j++) {
Real umin = Real(j)/numSegments;
Real umax = Real(j+1)/numSegments;
Real xmin = umin*TwoPi/period;
Real xmax = umax*TwoPi/period;
Vector2 vmin(Cos(xmin),-Sin(xmin)),vmax(Cos(xmax),-Sin(xmax));
Vector2 amin(-Sin(xmin),-Cos(xmin)),amax(-Sin(xmax),-Cos(xmax));
if(vmin.x > vmax.x) Swap(vmin.x,vmax.x);
if(vmin.y > vmax.y) Swap(vmin.y,vmax.y);
if(amin.x > amax.x) Swap(amin.x,amax.x);
if(amin.y > amax.y) Swap(amin.y,amax.y);
if(xmax > TwoPi && xmin < TwoPi) { vmax.x=1; amin.y=-1; }
if(xmax > 2*TwoPi && xmin < 2*TwoPi) { vmax.x=1; amin.y=-1; }
if(xmax > Pi && xmin < Pi) { vmin.x=-1; amax.y=1; }
if(xmax > 3*Pi && xmin < 3*Pi) { vmin.x=-1; amax.y=1; }
if(xmax > Pi/2 && xmin < Pi/2) { vmin.y=-1; amin.x=-1; }
if(xmax > Pi/2+TwoPi && xmin < Pi/2+TwoPi) { vmin.y=-1; amin.x=-1; }
if(xmax > 3*Pi/2 && xmin < 3*Pi/2) { vmax.y=1; amax.x=1; }
if(xmax > 3*Pi/2+TwoPi && xmin < 3*Pi/2+TwoPi) { vmax.y=1; amax.x=1; }
Assert(vmin.x >= -1.0 && vmax.x <= 1.0);
Assert(vmin.y >= -1.0 && vmax.y <= 1.0);
Assert(amin.x >= -1.0 && amax.x <= 1.0);
Assert(amin.y >= -1.0 && amax.y <= 1.0);
Assert(vmin.x <= vmax.x);
Assert(vmin.y <= vmax.y);
Assert(amin.x <= amax.x);
Assert(amin.y <= amax.y);
vmax *= TwoPi/period;
vmin *= TwoPi/period;
amax *= Sqr(TwoPi/period);
amin *= Sqr(TwoPi/period);
vmins[j] = Vector(2,vmin);
vmaxs[j] = Vector(2,vmax);
amins[j] = Vector(2,amin);
amaxs[j] = Vector(2,amax);
vs[j] = Vector(2,Vector2(TwoPi/period*Cos(xmin),-TwoPi/period*Sin(xmin)));
if(j+1==numSegments)
vs[j+1] = Vector(2,Vector2(TwoPi/period*Cos(xmax),-TwoPi/period*Sin(xmax)));
}
Timer timer;
TimeScaling timeScaling;
bool res=timeScaling.SolveMinTime(vmin,vmax,amin,amax,divs,vs,vmins,vmaxs,amins,amaxs,0.0,0.0);
double time=timer.ElapsedTime();
printf("Num segments: %d\n",numSegments);
printf("Result: %d\n",res);
printf("End time: %g\n",timeScaling.times.back());
printf("Solution time: %g s\n",time);
if(i+1==10) {
cout<<"t,s,ds,x,y:"<<endl;
for(size_t i=0;i<timeScaling.times.size();i++)
cout<<timeScaling.times[i]<<","<<timeScaling.params[i]<<","<<timeScaling.ds[i]<<","<<Sin(timeScaling.params[i]*TwoPi/period)<<","<<Cos(timeScaling.params[i]*TwoPi/period)<<endl;
}
}
}
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;
}
void ContactOptimizeTest2(const char* robfile,const char* config1,const char* config2)
{
Robot robot;
if(!robot.Load(robfile)) {
printf("Unable to load robot file %s\n",robfile);
return;
}
Vector a,b;
ifstream ia(config1,ios::in);
ifstream ib(config2,ios::in);
ia >> a;
ib >> b;
if(!ia || !ib) {
printf("Unable to load config file(s)\n");
return;
}
ia.close();
ib.close();
printf("Automatically detecting contacts...\n");
robot.UpdateConfig(a);
ContactFormation formation;
GetFlatContacts(robot,5e-3,formation);
printf("Assuming friction 0.5\n");
for(size_t i=0;i<formation.contacts.size();i++) {
printf("%d contacts on link %d\n",formation.contacts[i].size(),formation.links[i]);
for(size_t j=0;j<formation.contacts[i].size();j++)
formation.contacts[i][j].kFriction = 0.5;
}
Stance stance;
LocalContactsToStance(formation,robot,stance);
MultiPath path;
path.sections.resize(1);
MultiPath::PathSection& section = path.sections[0];
path.SetStance(stance,0);
vector<IKGoal> ikGoals;
path.GetIKProblem(ikGoals);
RobotConstrainedInterpolator interp(robot,ikGoals);
//Real xtol = 5e-2;
Real xtol = 1e-1;
interp.ftol = 1e-6;
interp.xtol = xtol;
if(!interp.Project(a)) {
printf("Failed to project config a\n");
return;
}
if(!interp.Project(b)) {
printf("Failed to project config b\n");
return;
}
cout<<"Start: "<<a<<endl;
cout<<"Goal: "<<b<<endl;
vector<Vector> milestones,milestones2;
if(!interp.Make(a,b,milestones)) {
printf("Failed to interpolate\n");
return;
}
if(!interp.Make(b,a,milestones2)) {
printf("Failed to interpolate\n");
return;
}
milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end());
//milestones2 = milestones;
//milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end());
{
cout<<"Saving geometric path to temp.path"<<endl;
ofstream out("temp.path",ios::out);
for(size_t i=0;i<milestones.size();i++)
out<<Real(i)/Real(milestones.size()-1)<<" "<<milestones[i]<<endl;
out.close();
}
section.milestones = milestones;
vector<Real> divs(101);
for(size_t i=0;i<divs.size();i++)
divs[i] = Real(i)/(divs.size()-1);
Timer timer;
ContactTimeScaling scaling(robot);
scaling.frictionRobustness = 0.25;
scaling.torqueRobustness = 0.25;
scaling.forceRobustness = 0.05;
bool res=scaling.SetParams(path,divs);
if(!res) {
printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime());
printf("Saving to scaling_constraints.csv\n");
ofstream outc("scaling_constraints.csv",ios::out);
outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl;
for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++)
for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++)
outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl;
return;
}
printf("Contact scaling init successful, time %g\n",timer.ElapsedTime());
printf("Saving to scaling_constraints.csv\n");
ofstream outc("scaling_constraints.csv",ios::out);
outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl;
for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++)
for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++)
outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl;
res=scaling.Optimize();
if(!res) {
printf("Time scaling failed in time %g. Path may be dynamically infeasible.\n",timer.ElapsedTime());
return;
}
printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back());
scaling.Check(path);
/*
for(size_t i=0;i<scaling.traj.ds.size();i++) {
printf("time %g: rate %g\n",scaling.traj.times[i],scaling.ds[i]);
}
*/
{
cout<<"Saving dynamically optimized path to temp_opt.path"<<endl;
ofstream out("temp_opt.path",ios::out);
Real dt = scaling.traj.EndTime()/100;
Real t=0;
for(size_t i=0;i<=100;i++) {
Vector x;
scaling.traj.Eval(t,x);
out<<t<<"\t"<<x<<endl;
t += dt;
}
out.close();
}
}
void ContactOptimizeTest()
{
Robot robot;
if(!robot.Load("data/simple_2d_biped.rob")) {
printf("Unable to load data/simple_2d_biped.rob\n");
return;
}
MultiPath path;
path.sections.resize(1);
MultiPath::PathSection& section = path.sections[0];
section.holds.resize(2);
section.holds[0].contacts.resize(2);
section.holds[0].contacts[0].x.set(-0.4,-0.1,0);
section.holds[0].contacts[1].x.set(-0.4,0.1,0);
section.holds[0].contacts[0].n.set(0,0,1);
section.holds[0].contacts[1].n.set(0,0,1);
section.holds[0].contacts[0].kFriction = 0.5;
section.holds[0].contacts[1].kFriction = 0.5;
section.holds[1].contacts.resize(2);
section.holds[1].contacts[0].x.set(0.4,-0.1,0);
section.holds[1].contacts[1].x.set(0.4,0.1,0);
section.holds[1].contacts[0].n.set(0,0,1);
section.holds[1].contacts[1].n.set(0,0,1);
section.holds[1].contacts[0].kFriction = 0.5;
section.holds[1].contacts[1].kFriction = 0.5;
section.holds[0].link = 7;
section.holds[1].link = 9;
section.holds[0].SetupIKConstraint(Vector3(0.5,-0.1,0),Vector3(Zero));
section.holds[1].SetupIKConstraint(Vector3(0.5,-0.1,0),Vector3(Zero));
vector<IKGoal> ikGoals;
path.GetIKProblem(ikGoals);
RobotConstrainedInterpolator interp(robot,ikGoals);
Real xtol = 5e-2;
interp.ftol = 1e-4;
interp.xtol = xtol;
Vector a(7),b(7);
ifstream ia("simple_2d_biped/a.config",ios::in);
ifstream ib("simple_2d_biped/b.config",ios::in);
ia >> a;
ib >> b;
ia.close();
ib.close();
if(!interp.Project(a)) {
printf("Failed to project config a\n");
return;
}
if(!interp.Project(b)) {
printf("Failed to project config b\n");
return;
}
cout<<"Start: "<<a<<endl;
cout<<"Goal: "<<b<<endl;
vector<Vector> milestones,milestones2;
if(!interp.Make(a,b,milestones)) {
printf("Failed to interpolate\n");
return;
}
if(!interp.Make(b,a,milestones2)) {
printf("Failed to interpolate\n");
return;
}
milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end());
//milestones2 = milestones;
//milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end());
{
cout<<"Saving geometric path to temp.path"<<endl;
ofstream out("temp.path",ios::out);
for(size_t i=0;i<milestones.size();i++)
out<<Real(i)/Real(milestones.size()-1)<<" "<<milestones[i]<<endl;
out.close();
}
section.milestones = milestones;
vector<Real> divs(401);
for(size_t i=0;i<divs.size();i++)
divs[i] = Real(i)/(divs.size()-1);
Timer timer;
ContactTimeScaling scaling(robot);
scaling.frictionRobustness = 0.25;
scaling.torqueRobustness = 0.25;
scaling.forceRobustness = 0.1;
bool res=scaling.SetParams(path,divs);
if(!res) {
printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime());
return;
}
printf("Contact scaling init successful, time %g\n",timer.ElapsedTime());
ofstream outc("scaling_constraints.csv",ios::out);
outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl;
for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++)
for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++)
outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl;
res = scaling.Optimize();
if(!res) {
printf("Time scaling failed in time %g. Path may be dynamically infeasible.\n",timer.ElapsedTime());
return;
}
printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back());
printf("\n");
printf("Checking path for feasibility...\n");
scaling.Check(path);
/*
for(size_t i=0;i<scaling.traj.ds.size();i++) {
printf("time %g: rate %g\n",scaling.traj.times[i],scaling.ds[i]);
}
*/
{
cout<<"Saving dynamically optimized path to temp_opt.path"<<endl;
ofstream out("temp_opt.path",ios::out);
int numdivs = 200;
Real dt = scaling.traj.EndTime()/numdivs;
Real t=0;
for(size_t i=0;i<=numdivs;i++) {
Vector x;
scaling.traj.Eval(t,x);
out<<t<<"\t"<<x<<endl;
t += dt;
}
out.close();
}
}
void RunConstrainedInterpolateTest(const Config& a,const Config& b,CSpace* space,VectorFieldFunction* constraint)
{
ConstrainedInterpolator interp(space,constraint);
interp.ftol = 1e-8;
interp.xtol = 1e-2;
interp.maxNewtonIters = 100;
SmoothConstrainedInterpolator interp2(space,constraint);
interp2.ftol = interp.ftol;
interp2.xtol = interp.xtol;
interp2.maxNewtonIters = interp.maxNewtonIters;
//run tests
cout<<"Interpolating "<<a<<" -> "<<b<<endl;
double xtols[10] = {1e-3,2.5e-3,5e-3,1e-2,2.5e-2,5e-2,1e-1,2.5e-1,5e-1,1};
const static int N=10;
/*
cout<<"Descent interpolation"<<endl;
cout<<"xtol,success,time,edges,vertex error,edge error,smoothed error"<<endl;
for(int iter = 0; iter < N; iter++) {
interp.xtol = xtols[iter];
cout<<xtols[iter]<<",";
int numTrials = 10;
bool res;
vector<Config> path;
Timer timer;
for(int j=0;j<numTrials;j++) {
path.resize(0);
res=true;
path.push_back(a);
Vector temp,dir;
while(true) {
dir=b-path.back();
if(dir.norm() < interp.xtol) {
path.push_back(b);
break;
}
interp2.ProjectVelocity(path.back(),dir);
Real n = dir.norm();
if(n < 1e-7) {
res = false;
break;
}
dir *= interp.xtol / n;
temp = path.back() + dir;
interp2.Project(temp);
path.push_back(temp);
}
if(!res) { numTrials = j+1; break; }
}
if(res) {
cout<<"1,"<<timer.ElapsedTime()/numTrials<<","<<path.size()<<",";
Real maxerr = 0.0;
Real maxerrmid = 0.0;
Real maxerrsmooth = 0.0;
Vector val;
Vector x;
for(size_t i=0;i<path.size();i++) {
(*interp.constraint)(path[i],val);
maxerr = Max(maxerr,val.norm());
if(i+1 < path.size()) {
int numdivs = Max(10000/(int)path.size(),2);
for(int j=1;j<numdivs;j++) {
space->Interpolate(path[i],path[i+1],Real(j)/Real(numdivs),x);
(*interp.constraint)(x,val);
maxerrmid = Max(maxerrmid,val.norm());
}
//do smoothed
GeneralizedCubicBezierCurve curve(space);
curve.x0 = path[i];
curve.x3 = path[i+1];
if(i == 0) curve.x1 = curve.x0 + (path[i+1]-path[i])/3.0;
else curve.x1 = curve.x0 + 0.5*(path[i+1]-path[i-1])/3.0;
if(i+2 >= path.size()) curve.x2 = curve.x3 - (path[i+1]-path[i])/3.0;
else curve.x2 = curve.x3 - 0.5*(path[i+2]-path[i])/3.0;
for(int j=1;j<numdivs;j++) {
curve.Eval(Real(j)/Real(numdivs),x);
(*interp.constraint)(x,val);
maxerrsmooth = Max(maxerrsmooth,val.norm());
}
}
}
cout<<maxerr<<","<<maxerrmid<<","<<maxerrsmooth<<endl;
}
else {
cout<<"0,"<<timer.ElapsedTime()<<endl;
}
}
cout<<"Linear interpolation"<<endl;
cout<<"xtol,success,time,edges,vertex error,edge error,smoothed error"<<endl;
for(int iter = 0; iter < N; iter++) {
interp.xtol = xtols[iter];
cout<<xtols[iter]<<",";
int numTrials = 10;
bool res;
vector<Config> path;
Timer timer;
for(int j=0;j<numTrials;j++) {
path.resize(0);
res=interp.Make(a,b,path);
if(!res) { numTrials = j+1; break; }
}
if(res) {
cout<<"1,"<<timer.ElapsedTime()/numTrials<<","<<path.size()<<",";
Real maxerr = 0.0;
Real maxerrmid = 0.0;
Real maxerrsmooth = 0.0;
Vector val;
Vector x;
for(size_t i=0;i<path.size();i++) {
(*interp.constraint)(path[i],val);
maxerr = Max(maxerr,val.norm());
if(i+1 < path.size()) {
int numdivs = Max(10000/(int)path.size(),2);
for(int j=1;j<numdivs;j++) {
space->Interpolate(path[i],path[i+1],Real(j)/Real(numdivs),x);
(*interp.constraint)(x,val);
maxerrmid = Max(maxerrmid,val.norm());
}
//do smoothed
GeneralizedCubicBezierCurve curve(space);
curve.x0 = path[i];
curve.x3 = path[i+1];
if(i == 0) curve.x1 = curve.x0 + (path[i+1]-path[i])/3.0;
else curve.x1 = curve.x0 + 0.5*(path[i+1]-path[i-1])/3.0;
if(i+2 >= path.size()) curve.x2 = curve.x3 - (path[i+1]-path[i])/3.0;
else curve.x2 = curve.x3 - 0.5*(path[i+2]-path[i])/3.0;
for(int j=1;j<numdivs;j++) {
curve.Eval(Real(j)/Real(numdivs),x);
(*interp.constraint)(x,val);
maxerrsmooth = Max(maxerrsmooth,val.norm());
}
}
}
cout<<maxerr<<","<<maxerrmid<<","<<maxerrsmooth<<endl;
}
else {
cout<<"0,"<<timer.ElapsedTime()<<endl;
}
}
*/
cout<<"Smooth interpolation"<<endl;
cout<<"xtol,success,time,edges,vertex error,edge error"<<endl;
for(int iter = 0; iter < N; iter++) {
interp2.xtol = xtols[iter];
cout<<xtols[iter]<<",";
GeneralizedCubicBezierSpline cpath;
int numTrials = 10;
bool res;
Timer timer;
for(int j=0;j<numTrials;j++) {
cpath.segments.clear();
cpath.durations.clear();
res=interp2.Make(a,b,cpath);
if(!res) { numTrials = j+1; break; }
}
if(res) {
cout<<"1,"<<timer.ElapsedTime()/numTrials<<","<<cpath.segments.size()<<",";
Real maxerr = 0.0;
Real maxerrmid = 0.0;
for(size_t i=0;i<cpath.segments.size();i++) {
if(i > 0) Assert(cpath.segments[i].x0 == cpath.segments[i-1].x3);
Vector val;
(*interp.constraint)(cpath.segments[i].x0,val);
maxerr = Max(maxerr,val.norm());
(*interp.constraint)(cpath.segments[i].x3,val);
maxerr = Max(maxerr,val.norm());
Vector x;
int numdivs = Max(10000/(int)cpath.segments.size(),2);
for(int j=1;j<numdivs;j++) {
cpath.segments[i].Eval(Real(j)/Real(numdivs),x);
(*interp.constraint)(x,val);
maxerrmid = Max(maxerrmid,val.norm());
}
}
cout<<maxerr<<","<<maxerrmid<<endl;
}
else {
cout<<"0,"<<timer.ElapsedTime()<<endl;
}
}
}
void TimeOptimizeTest1DSine()
{
Vector vmin(1,-1.0),vmax(1,1.0);
Vector amin(1,-1.0),amax(1,1.0);
//try the path from 0 to 1
int ns [] = {2,4,8,16,32,64,128,256,512,1024,2028};
for(int i=0;i<11;i++) {
int numSegments = ns[i];
vector<Real> divs(numSegments+1);
vector<Vector> vs(numSegments+1);
vector<Vector> vmins(numSegments),vmaxs(numSegments);
vector<Vector> amins(numSegments),amaxs(numSegments);
for(int j=0;j<=numSegments;j++)
divs[j] = Real(j)/Real(numSegments);
//sine wave
//p(s) = sin(s*2pi/period)
//p'(s) = 2pi/period*cos(s*2pi/period)
//p''(s) = -(2pi/period)^2*sin(s*2pi/period)
Real period = 1.0;
for(int j=0;j<numSegments;j++) {
Real umin = Real(j)/numSegments;
Real umax = Real(j+1)/numSegments;
Real xmin = umin*TwoPi/period;
Real xmax = umax*TwoPi/period;
Real vmin=Cos(xmin),vmax=Cos(xmax);
Real amin=-Sin(xmin),amax=-Sin(xmax);
if(vmin > vmax) Swap(vmin,vmax);
if(amin > amax) Swap(amin,amax);
if(xmax > TwoPi && xmin < TwoPi) vmax=1;
if(xmax > 2*TwoPi && xmin < 2*TwoPi) vmax=1;
if(xmax > Pi && xmin < Pi) vmin=-1;
if(xmax > 3*Pi && xmin < 3*Pi) vmin=-1;
if(xmax > Pi/2 && xmin < Pi/2) amin=-1;
if(xmax > Pi/2+TwoPi && xmin < Pi/2+TwoPi) amin=-1;
if(xmax > 3*Pi/2 && xmin < 3*Pi/2) amax=1;
if(xmax > 3*Pi/2+TwoPi && xmin < 3*Pi/2+TwoPi) amax=1;
Assert(vmax - vmin <= 1.0/numSegments*TwoPi);
Assert(amax - amin <= 1.0/numSegments*TwoPi/period);
Assert(vmin >= -1.0 && vmax <= 1.0);
Assert(amin >= -1.0 && amax <= 1.0);
Assert(vmin <= vmax);
Assert(amin <= amax);
vmax *= TwoPi/period;
vmin *= TwoPi/period;
amax *= Sqr(TwoPi/period);
amin *= Sqr(TwoPi/period);
vs[j] = Vector(1,TwoPi/period*Cos(xmin));
if(j+1==numSegments)
vs[j+1] = Vector(1,TwoPi/period*Cos(xmax));
vmins[j] = Vector(1,vmin);
vmaxs[j] = Vector(1,vmax);
amins[j] = Vector(1,amin);
amaxs[j] = Vector(1,amax);
}
Timer timer;
TimeScaling timeScaling;
timeScaling.ConditionMinTime(divs,vs,vmins,vmaxs,amins,amaxs);
bool res=timeScaling.SolveMinTime(vmin,vmax,amin,amax,divs,vs,vmins,vmaxs,amins,amaxs,0.0,0.0);
//bool res=timeScaling.SolveMinTimeArcLength(vmin,vmax,amin,amax,divs,vs,vmins,vmaxs,amins,amaxs,0.0,0.0);
double time=timer.ElapsedTime();
printf("Num segments: %d\n",numSegments);
printf("Result: %d\n",res);
if(res) {
printf("End time: %g\n",timeScaling.times.back());
printf("Solution time: %g s\n",time);
}
if(i+1==11) {
cout<<"time,s,ds,val,vmin,vmax,amin,amax,dy,ddy"<<endl;
for(size_t i=0;i<timeScaling.times.size();i++) {
cout<<timeScaling.times[i]<<","<<timeScaling.params[i]<<","<<timeScaling.ds[i]<<","<<Sin(timeScaling.params[i]*TwoPi/period);
if(i<vmins.size()) {
Real dp=Cos(timeScaling.params[i]*TwoPi/period)*TwoPi/period;
Real ddp=-Sin(timeScaling.params[i]*TwoPi/period)*Sqr(TwoPi/period);
cout<<","<<vmins[i][0]<<","<<vmaxs[i][0]<<","<<amins[i][0]<<","<<amaxs[i][0];
cout<<","<<timeScaling.ds[i]*dp<<","<<timeScaling.TimeToParamAccel(i,timeScaling.times[i])*dp+Sqr(timeScaling.ds[i])*ddp<<endl;
}
else cout<<endl;
}
}
}
}
void TimeOptimizeTestNLinkPlanar()
{
const char* fn = "data/planar%d.rob";
int ns [] = {5,10,15,20,25,30,40,50,60,70,80,90,100};
Real tolscales [] = {3.95,3.55,2.73,2,2,2,1.8,1.6,1.42,1,1,1,1};
int num = 13;
char buf[256];
for(int i=0;i<num;i++) {
int n=ns[i];
printf("Testing optimize %d\n",n);
sprintf(buf,fn,n);
Robot robot;
if(!robot.Load(buf)) {
fprintf(stderr,"Unable to load robot %s\n",buf);
return;
}
int ee = robot.links.size()-1;
Vector3 localpt(1,0,0);
Real len = robot.links.size();
//make a half circle
Config a(robot.q.n,Pi/robot.links.size()),b;
a[0] *= 0.5;
robot.UpdateConfig(a);
IKGoal goal,goaltemp;
goal.link = ee;
goal.localPosition = localpt;
goal.SetFixedPosition(robot.links[ee].T_World*localpt);
//cout<<"Goal position "<<goal.endPosition<<endl;
goaltemp = goal;
goaltemp.link = ee/2;
goaltemp.SetFixedPosition(goal.endPosition*0.5);
//cout<<"Middle goal position "<<goaltemp.endPosition<<endl;
vector<IKGoal> onegoal(1),bothgoals(2);
onegoal[0] = goal;
bothgoals[0] = goal;
bothgoals[1] = goaltemp;
int iters=100;
bool res=SolveIK(robot,bothgoals,1e-3,iters);
if(!res) {
fprintf(stderr,"Couldn't solve for target robot config\n");
return;
}
b = robot.q;
Timer timer;
GeneralizedCubicBezierSpline path;
RobotSmoothConstrainedInterpolator interp(robot,onegoal);
interp.xtol = 1e-3*tolscales[i];
if(!interp.Make(a,b,path)) {
fprintf(stderr,"Couldn't interpolate for target robot config\n");
return;
}
printf("Solved for path with tol %g in time %g\n",interp.xtol,timer.ElapsedTime());
{
sprintf(buf,"trajopt_a_%d.config",n);
ofstream out(buf);
out<<a<<endl;
}
{
sprintf(buf,"trajopt_b_%d.config",n);
ofstream out(buf);
out<<b<<endl;
}
{
sprintf(buf,"trajopt_interp_%d.xml",n);
vector<Real> times;
vector<Config> configs;
path.GetPiecewiseLinear(times,configs);
MultiPath mpath;
mpath.SetTimedMilestones(times,configs);
mpath.SetIKProblem(onegoal);
mpath.Save(buf);
}
{
//unroll joints
for(size_t i=0;i<path.segments.size();i++) {
for(int j=0;j<path.segments[i].x0.n;j++) {
if(path.segments[i].x0[j] > Pi)
path.segments[i].x0[j] -= TwoPi;
if(path.segments[i].x1[j] > Pi)
path.segments[i].x1[j] -= TwoPi;
if(path.segments[i].x2[j] > Pi)
path.segments[i].x2[j] -= TwoPi;
if(path.segments[i].x3[j] > Pi)
path.segments[i].x3[j] -= TwoPi;
}
}
sprintf(buf,"trajopt_interp_%d.spline",n);
ofstream out(buf,ios::out);
out.precision(10);
path.Save(out);
}
TimeScaledBezierCurve curve;
//curve.path = path;
{
sprintf(buf,"trajopt_interp_%d.spline",n);
ifstream in(buf,ios::in);
curve.path.Load(in);
Assert(curve.path.segments.size() == path.durations.size());
for(size_t i=0;i<curve.path.durations.size();i++) {
Assert(FuzzyEquals(curve.path.durations[i],path.durations[i]));
if(!curve.path.segments[i].x0.isEqual(path.segments[i].x0,Epsilon)) {
printf("Error on segment %d\n",i);
cout<<path.segments[i].x0<<endl;
cout<<curve.path.segments[i].x0<<endl;
cout<<"Distance: "<<path.segments[i].x0.distance(curve.path.segments[i].x0)<<endl;
}
Assert(curve.path.segments[i].x0.isEqual(path.segments[i].x0,Epsilon));
Assert(curve.path.segments[i].x1.isEqual(path.segments[i].x1,Epsilon));
Assert(curve.path.segments[i].x2.isEqual(path.segments[i].x2,Epsilon));
Assert(curve.path.segments[i].x3.isEqual(path.segments[i].x3,Epsilon));
}
}
Vector vmin(robot.q.n,-Pi),vmax(robot.q.n,Pi);
Vector amin(robot.q.n,-Pi),amax(robot.q.n,Pi);
timer.Reset();
if(!curve.OptimizeTimeScaling(vmin,vmax,amin,amax)) {
fprintf(stderr,"Optimize failed in time %g\n",timer.ElapsedTime());
return;
}
printf("Solved time optimization with %d segments in time %g\n",curve.path.segments.size(),timer.ElapsedTime());
//output optimized path
{
Real dt = 0.5;
vector<Real> times;
vector<Config> configs;
/*curve.GetDiscretizedPath(dt,configs);
times.resize(configs.size());
for(size_t j=0;j<times.size();j++)
times[j] = j*dt;
*/
curve.GetPiecewiseLinear(times,configs);
MultiPath mpath;
mpath.SetIKProblem(onegoal);
mpath.SetTimedMilestones(times,configs);
sprintf(buf,"trajopt_opt_%d.xml",n);
mpath.Save(buf);
}
}
}
/* SpMV kernel implemented with pOSKI */
void poski_spmv(spx_index_t *Aptr, spx_index_t *Aind, spx_value_t *Aval,
spx_index_t nrows, spx_index_t ncols, spx_index_t nnz,
spx_value_t *x, spx_value_t *y,
spx_value_t ALPHA, spx_value_t BETA)
{
poski_Init();
/* 1. Matrix loading phase */
poski_threadarg_t *poski_thread = poski_InitThreads();
poski_ThreadHints(poski_thread, NULL, POSKI_THREADPOOL, NR_THREADS);
// poski_ThreadHints(poski_thread, NULL, POSKI_PTHREAD, NR_THREADS);
poski_partitionarg_t *partitionMat = poski_partitionMatHints(OneD, NR_THREADS,
KERNEL_MatMult,
OP_NORMAL);
poski_mat_t A_tunable =
poski_CreateMatCSR(Aptr, Aind, Aval, nrows, ncols, nnz, SHARE_INPUTMAT,
poski_thread, partitionMat, 2, INDEX_ZERO_BASED,
MAT_GENERAL);
/* 2. Vector loading */
poski_partitionvec_t *partitionVecX =
poski_PartitionVecHints(A_tunable, KERNEL_MatMult, OP_NORMAL, INPUTVEC);
poski_vec_t x_view = poski_CreateVec(x, ncols, STRIDE_UNIT, partitionVecX);
poski_partitionvec_t *partitionVecY =
poski_PartitionVecHints(A_tunable, KERNEL_MatMult, OP_NORMAL, OUTPUTVEC);
poski_vec_t y_view = poski_CreateVec(y, nrows, STRIDE_UNIT, partitionVecY);
/* 3. Tuning phase */
t.Clear();
t.Start();
poski_TuneHint_MatMult(A_tunable, OP_NORMAL, ALPHA, x_view, BETA,
y_view, ALWAYS_TUNE_AGGRESSIVELY);
poski_TuneMat(A_tunable);
t.Pause();
double pt = t.ElapsedTime();
/* 4. SpMV benchmarking phase */
vector<double> mt(OUTER_LOOPS);
for (size_t i = 0; i < OUTER_LOOPS; i++) {
t.Clear();
t.Start();
for (size_t j = 0; j < LOOPS; j++) {
poski_MatMult(A_tunable, OP_NORMAL, ALPHA, x_view, BETA, y_view);
}
t.Pause();
mt[i] = t.ElapsedTime();
}
sort(mt.begin(), mt.end());
double mt_median =
(OUTER_LOOPS % 2) ? mt[((OUTER_LOOPS+1)/2)-1]
: ((mt[OUTER_LOOPS/2-1] + mt[OUTER_LOOPS/2])/2);
double flops = (double)(LOOPS*nnz*2)/((double)1000*1000*mt_median);
cout << "m: " << MATRIX
<< " pt: " << pt
<< " mt(median): " << mt_median
<< " flops: " << flops << endl;
/* 4. Cleanup */
poski_DestroyPartitionVec(partitionVecX);
poski_DestroyPartitionVec(partitionVecY);
poski_DestroyVec(x_view);
poski_DestroyVec(y_view);
poski_DestroyPartitionMat(partitionMat);
poski_DestroyMat(A_tunable);
poski_DestroyThreads(poski_thread);
poski_Close();
}
