void check_final_state(boost::shared_ptr<Ravelin::ArticulatedBodyd>& rb){
boost::shared_ptr<RCArticulatedBodyd> robot
= boost::dynamic_pointer_cast<RCArticulatedBodyd>(rb);
Ravelin::VectorNd q;
robot->get_generalized_coordinates_euler(q);
Ravelin::Pose3d P = Utility::vec_to_pose(q.segment(q.size()-7,q.size()));
double x_progress = P.x[0];
// Progress from first version:
double last_x_progress = 1.01395;
#ifdef USE_GTEST
ASSERT_LE(last_x_progress,x_progress);
#else
if(last_x_progress>x_progress){
std::cerr << last_x_progress << " last progress > this progress" << x_progress;
exit(EXIT_FAILURE);
}
#endif
}
void Utility::calc_cubic_spline_coefs(const Ravelin::VectorNd& T_,const Ravelin::VectorNd& X,
const Ravelin::Vector2d& Xd, Ravelin::VectorNd& B){
static Ravelin::MatrixNd A;
// Spline always solves from t[0] = 0 in interval
static Ravelin::VectorNd T;
T = T_;
for(int i=0;i<T.rows();i++)
T[i] -= T_[0];
assert(T.rows() == X.rows());
int N = X.rows(); //n_control_points
// N_VIRTUAL_KNOTS= 2
// N_SPLINES = num_knots+N_VIRTUAL_KNOTS - 1
// N_CONSTRAINTS = num_knots*3 + num_knots + 2 + 2
// N_COEFS = 4*N_SPLINES
B.set_zero(4*(N+1));
A.set_zero(4*(N+1),4*(N+1));
// ---------- start constraint ----------
B[0] = 0;
B[1] = Xd[0]; // Velocity clamped spline
B[2] = X[0];
// xdd
A(0,0) = 6*T[0];
A(0,1) = 2;
// xd
A(1,0) = 3*T[0]*T[0];
A(1,1) = 2*T[0];
A(1,2) = 1;
// x
A(2,0) = T[0]*T[0]*T[0];
A(2,1) = T[0]*T[0];
A(2,2) = T[0];
A(2,3) = 1;
// ---------- start Virtual constraints ----------
double tv0 = T[0] + (T[1]-T[0])/1000.0;
// xdd
A(3,0) = 6*tv0;
A(3,1) = 2;
A(3,4) = -6*tv0;
A(3,5) = -2;
// xd
A(4,0) = 3*tv0*tv0;
A(4,1) = 2*tv0;
A(4,2) = 1;
A(4,4) = -3*tv0*tv0;
A(4,5) = -2*tv0;
A(4,6) = -1;
// x
A(5,0) = tv0*tv0*tv0;
A(5,1) = tv0*tv0;
A(5,2) = tv0;
A(5,3) = 1;
A(5,4) = -tv0*tv0*tv0;
A(5,5) = -tv0*tv0;
A(5,6) = -tv0;
A(5,7) = -1;
// ---------- End Virtual Constraint ----------
double tvN = T[N-1] - (T[N-1]-T[N-2])/1000.0;
// xddd
// A(A.rows()-6,A.columns()-8) = 6;
// A(A.rows()-6,A.columns()-4) = -6;
// xdd
A(A.rows()-6,A.columns()-8) = 6*tvN;
A(A.rows()-6,A.columns()-7) = 2;
A(A.rows()-6,A.columns()-4) = -6*tvN;
A(A.rows()-6,A.columns()-3) = -2;
// xd
A(A.rows()-5,A.columns()-8) = 3*tvN*tvN;
A(A.rows()-5,A.columns()-7) = 2*tvN;
A(A.rows()-5,A.columns()-6) = 1;
A(A.rows()-5,A.columns()-4) = -3*tvN*tvN;
A(A.rows()-5,A.columns()-3) = -2*tvN;
A(A.rows()-5,A.columns()-2) = -1;
// x
A(A.rows()-4,A.columns()-8) = tvN*tvN*tvN;
A(A.rows()-4,A.columns()-7) = tvN*tvN;
A(A.rows()-4,A.columns()-6) = tvN;
A(A.rows()-4,A.columns()-5) = 1;
A(A.rows()-4,A.columns()-4) = -tvN*tvN*tvN;
A(A.rows()-4,A.columns()-3) = -tvN*tvN;
A(A.rows()-4,A.columns()-2) = -tvN;
A(A.rows()-4,A.columns()-1) = -1;
// ---------- End constraint ----------
B[B.rows()-3] = 0;
B[B.rows()-2] = Xd[1]; // Velocity clamped spline
B[B.rows()-1] = X[N-1];
// xdd
A(A.rows()-3,A.columns()-4) = 6*T[N-1];
A(A.rows()-3,A.columns()-3) = 2;
// xddd
// A(A.rows()-3,A.columns()-4) = 6;
// xd
A(A.rows()-2,A.columns()-4) = 3*T[N-1]*T[N-1];
A(A.rows()-2,A.columns()-3) = 2*T[N-1];
A(A.rows()-2,A.columns()-2) = 1;
// x
A(A.rows()-1,A.columns()-4) = T[N-1]*T[N-1]*T[N-1];
A(A.rows()-1,A.columns()-3) = T[N-1]*T[N-1];
A(A.rows()-1,A.columns()-2) = T[N-1];
A(A.rows()-1,A.columns()-1) = 1;
// ---------- Fill in continuity conponents at each of N-2 knots ----------
for(int i=0;i<N-2;i++){
// Xdd continuity
// \ddot{P}_i - \ddot{P}_{i+1} = 0
A(5 + 4*i + 1,3 + 4*i + 1) = 6*T[i+1];
A(5 + 4*i + 1,3 + 4*i + 2) = 2;
A(5 + 4*i + 1,3 + 4*(i+1) + 1) = -6*T[i+1];
A(5 + 4*i + 1,3 + 4*(i+1) + 2) = -2;
// Xd continuity
// \dot{P}_i - \dot{P}_{i+1} = 0
A(5 + 4*i + 2,3 + 4*i + 1) = 3*T[i+1]*T[i+1];
A(5 + 4*i + 2,3 + 4*i + 2) = 2*T[i+1];
A(5 + 4*i + 2,3 + 4*i + 3) = 1;
A(5 + 4*i + 2,3 + 4*(i+1) + 1) = -3*T[i+1]*T[i+1];
A(5 + 4*i + 2,3 + 4*(i+1) + 2) = -2*T[i+1];
A(5 + 4*i + 2,3 + 4*(i+1) + 3) = -1;
// X continuity
// P_i - P_{i+1} = 0
A(5 + 4*i + 3,3 + 4*i + 1) = T[i+1]*T[i+1]*T[i+1];
A(5 + 4*i + 3,3 + 4*i + 2) = T[i+1]*T[i+1];
A(5 + 4*i + 3,3 + 4*i + 3) = T[i+1];
A(5 + 4*i + 3,3 + 4*i + 4) = 1;
A(5 + 4*i + 3,3 + 4*(i+1) + 1) = -T[i+1]*T[i+1]*T[i+1];
A(5 + 4*i + 3,3 + 4*(i+1) + 2) = -T[i+1]*T[i+1];
A(5 + 4*i + 3,3 + 4*(i+1) + 3) = -T[i+1];
A(5 + 4*i + 3,3 + 4*(i+1) + 4) = -1;
// P_i = X[i]
B[5 + 4*i + 4] = X[i+1];
A(5 + 4*i + 4,3 + 4*(i+1) + 1) = T[i+1]*T[i+1]*T[i+1];
A(5 + 4*i + 4,3 + 4*(i+1) + 2) = T[i+1]*T[i+1];
A(5 + 4*i + 4,3 + 4*(i+1) + 3) = T[i+1];
A(5 + 4*i + 4,3 + 4*(i+1) + 4) = 1;
}
workv_ = B;
// Solve linear system (A is corrupted and workv_ has result)
LA_.solve_fast(A,workv_);
// Exclude virtual points from returned spline coeficients
workv_.get_sub_vec(4,workv_.size()-4,B);
}