Commit 727b52ea by Steve Tonneau

### ongoing: instead of inverting whole matrix solving problems one by one

parent e9882b85
 ... ... @@ -68,8 +68,8 @@ struct cubic_zero_vel : public exact_cubic x, Eigen::Ref h1, Eigen::Ref h2) const ... ... @@ -102,6 +102,41 @@ struct cubic_zero_vel : public exact_cubic b = (h1)^-1 * h2 * x return b; }*/ private: template void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) { const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second; const point_t& b0 = constraints.init_vel , c0 = constraints.init_acc; const num_t& init_t = wayPointsBegin->first, end_t = - wayPointsNext->first; const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt; const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3; subSplines.push_back(create_cubic (a0,b0,c0,d0,init_t, end_t)); constraints.init_vel = subSplines.back().derivate(end_t, 1); constraints.init_acc = subSplines.back().derivate(end_t, 2); } template void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) { const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second; const point_t& b0 = constraints.init_vel, b1 = constraints.end_vel, c0 = constraints.init_acc, c1 = constraints.end_acc; const num_t& init_t = wayPointsBegin->first, end_t = - wayPointsNext->first; const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt; //solving a system of four linear eq with 4 unknows: d0, e0 const num_t alpha_0 = a1 - a0 + b0 *dt + c0 * dt_2; const num_t alpha_1 = b1 - b0 + b0 *dt + c0 * dt_2; /*subSplines.push_back(create_cubic (a0,b0,c0,d0,e0, init_t, end_t)); constraints.init_vel = subSplines.back().derivate(end_t, 1); constraints.init_acc = subSplines.back().derivate(end_t, 2);*/ //solving } template ... ... @@ -112,9 +147,20 @@ struct cubic_zero_vel : public exact_cubic(it, next, cons, subSplines); } compute_end_spline(it, next,cons, subSplines); return subSplines; // then solving last step /*// refer to the paper to understand all this. MatrixX h1 = MatrixX::Zero(size, size); MatrixX h2 = MatrixX::Zero(size, size); MatrixX h3 = MatrixX::Zero(size, size); ... ... @@ -173,7 +219,7 @@ struct cubic_zero_vel : public exact_cubic(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first)); return subSplines; return subSplines;*/ } ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!