Modify ComputeChunk for handling polynomials up to degree 5

src/Trajectory.cpp

@@ -150,7 +150,7 @@ void Trajectory::InitFromChunksList(const std::list<Chunk>&chunkslist0) {
     chunkcumulateddurationslist.resize(0);
     std::list<Chunk>::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<dReal>&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<dReal> coefficientsvector;
+    assert(currentchunk.degree <= 5);
+    std::vector<dReal> a, b, c, d, e, coefficientsvector;
     std::vector<Polynomial> polynomialsvector;
-    // current chunk : u0 + u1*s + u2*s^2
+    std::vector<std::vector<dReal> > 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<currentchunk.dimension; i++) {
-        u0 = currentchunk.polynomialsvector[i].coefficientsvector[0];
-        u1 = 0;
-        if(currentchunk.degree >= 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;
 }