From 00e44432966d21a709dc77d20d4ac375edddc970 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Thu, 8 May 2014 17:29:29 +0800 Subject: [PATCH] Modify ComputeChunk for handling polynomials up to degree 5 --- src/Trajectory.cpp | 119 +++++++++++++++++++++++++++++---------------- 1 file changed, 77 insertions(+), 42 deletions(-) diff --git a/src/Trajectory.cpp b/src/Trajectory.cpp index 6029320..2d718ce 100644 --- a/src/Trajectory.cpp +++ b/src/Trajectory.cpp @@ -150,7 +150,7 @@ void Trajectory::InitFromChunksList(const std::list&chunkslist0) { chunkcumulateddurationslist.resize(0); std::list::iterator itchunk = chunkslist.begin(); while(itchunk != chunkslist.end()) { - assert(degree == itchunk->degree); + // assert(degree == itchunk->degree); dReal chunkduration = itchunk->duration; if(chunkduration > TINY) { chunkdurationslist.push_back(chunkduration); @@ -260,50 +260,81 @@ void Trajectory::Evaldd(dReal s, std::vector&qdd) const { void Trajectory::ComputeChunk(dReal t0, dReal tnext, dReal s, dReal sd, dReal sdd, const Chunk& currentchunk, Chunk& newchunk) { - assert(currentchunk.degree <= 3); - dReal a0, a1, a2, b0, b1, b2, b3, b4, c0, c1, c2, c3, c4, c5, c6, u0, u1, u2, u3; - std::vector coefficientsvector; + assert(currentchunk.degree <= 5); + std::vector a, b, c, d, e, coefficientsvector; std::vector polynomialsvector; - // current chunk : u0 + u1*s + u2*s^2 + std::vector > coeffsvects; + // current chunk : u0 + u1*s + u2*s^2 + u3*s^3 + u4*s^4 + u5*s^5 // profile : s + sd*t + 0.5*sdd*t^2 - // new chunk : v0 + v1*t + v2*t^2 + v3*t^3 + v4*t^4 + v5*t^5 + v6*t^6; - a0 = s + sd*t0 + 0.5*sdd*t0*t0; - a1 = sd + sdd*t0; - a2 = 0.5*sdd; - b0 = a0*a0; - b1 = 2*a0*a1; - b2 = 2*a0*a2+a1*a1; - b3 = 2*a1*a2; - b4 = a2*a2; - c0 = b0*a0; - c1 = b1*a0 + b0*a1; - c2 = b2*a0 + b1*a1 + b0*a2; - c3 = b3*a0 + b2*a1 + b1*a2; - c4 = b4*a0 + b3*a1 + b2*a2; - c5 = b4*a1 + b3*a2; - c6 = b4*a2; - for(int i=0; i= 1) - u1 = currentchunk.polynomialsvector[i].coefficientsvector[1]; - u2 = 0; - if(currentchunk.degree >= 2) - u2 = currentchunk.polynomialsvector[i].coefficientsvector[2]; - u3 = 0; - if(currentchunk.degree >= 3) - u3 = currentchunk.polynomialsvector[i].coefficientsvector[3]; - coefficientsvector.resize(0); - coefficientsvector.push_back(u3*c0 + u2*b0 + u1*a0 + u0); // v0 - coefficientsvector.push_back(u3*c1 + u2*b1 + u1*a1); // v1 - coefficientsvector.push_back(u3*c2 + u2*b2 + u1*a2); // v2 - coefficientsvector.push_back(u3*c3 + u2*b3); // v3 - coefficientsvector.push_back(u3*c4 + u2*b4); // v4 - coefficientsvector.push_back(u3*c5); // v5 - coefficientsvector.push_back(u3*c6); // v6 - polynomialsvector.push_back(Polynomial(coefficientsvector)); + // new chunk : v0 + v1*t + v2*t^2 + v3*t^3 + v4*t^4 + v5*t^5 + v6*t^6 + v7*t^ + v8*t^8 + v9*t^9 + v10*t^10; + + if (currentchunk.degree >= 1) { + a.resize(0); + a.push_back(s + sd*t0 + 0.5*sdd*t0*t0); + a.push_back(sd + sdd*t0); + a.push_back(0.5*sdd); + coeffsvects.push_back(a); } - newchunk = Chunk(tnext-t0, polynomialsvector); + if (currentchunk.degree >= 2) { + b.resize(0); + b.push_back(a[0]*a[0]); + b.push_back(2*a[0]*a[1]); + b.push_back(2*a[0]*a[2] + a[1]*a[1]); + b.push_back(2*a[1]*a[2]); + b.push_back(a[2]*a[2]); + coeffsvects.push_back(b); + } + if(currentchunk.degree >= 3) { + c.push_back(b[0]*a[0]); + c.push_back(b[1]*a[0] + b[0]*a[1]); + c.push_back(b[2]*a[0] + b[1]*a[1] + b[0]*a[2]); + c.push_back(b[3]*a[0] + b[2]*a[1] + b[1]*a[2]); + c.push_back(b[4]*a[0] + b[3]*a[1] + b[2]*a[2]); + c.push_back(b[4]*a[1] + b[3]*a[2]); + c.push_back(b[4]*a[2]); + coeffsvects.push_back(c); + } + if(currentchunk.degree >= 4) { + d.push_back(b[0]*b[0]); + d.push_back(2*b[0]*b[1]); + d.push_back(2*b[0]*b[2] + b[1]*b[1]); + d.push_back(2*(b[0]*b[3] + b[1]*b[2])); + d.push_back(2*(b[0]*b[4] + b[1]*b[3]) + b[2]*b[2]); + d.push_back(2*(b[1]*b[4] + b[2]*b[3])); + d.push_back(2*b[2]*b[4] + b[3]*b[3]); + d.push_back(2*b[3]*b[4]); + d.push_back(b[4]*b[4]); + coeffsvects.push_back(d); + } + if(currentchunk.degree >= 5) { + e.push_back(a[0]*d[0]); + e.push_back(d[1]*a[0] + d[0]*a[1]); + e.push_back(d[2]*a[0] + d[1]*a[1] + d[0]*a[2]); + e.push_back(d[3]*a[0] + d[2]*a[1] + d[1]*a[2]); + e.push_back(d[4]*a[0] + d[3]*a[1] + d[2]*a[2]); + e.push_back(d[5]*a[0] + d[4]*a[1] + d[3]*a[2]); + e.push_back(d[6]*a[0] + d[5]*a[1] + d[4]*a[2]); + e.push_back(d[7]*a[0] + d[6]*a[1] + d[5]*a[2]); + e.push_back(d[8]*a[0] + d[7]*a[1] + d[6]*a[2]); + e.push_back(d[8]*a[1] + d[7]*a[2]); + e.push_back(d[8]*a[2]); + coeffsvects.push_back(e); + } + + for(int i = 0; i < currentchunk.dimension; i++) { + coefficientsvector.resize(0); + coefficientsvector.push_back(currentchunk.polynomialsvector[i].coefficientsvector[0]); + for(int k = 1; k <= currentchunk.degree; k++){ + coefficientsvector.resize(2*k + 1, 0); + dReal u = currentchunk.polynomialsvector[i].coefficientsvector[k]; + int l = 2*k + 1; + for(int j = 0; j < l; j++) { + coefficientsvector[j] += u*coeffsvects[k - 1][j]; + } + } + polynomialsvector.push_back(Polynomial(coefficientsvector)); + } + newchunk = Chunk(tnext - t0, polynomialsvector); } @@ -400,6 +431,10 @@ int Trajectory::Reparameterize(Constraints& constraints, Trajectory& restrajecto sdcur = sdnext2; } + if (newchunkslist.size() < 1) { + return -1; + } + restrajectory = Trajectory(newchunkslist); return 1; }