/// Calculates The null pace for matrix M and places it in Vk
/// returns the number of columns in Vk
unsigned Utility::kernal( Ravelin::MatrixNd& M,Ravelin::MatrixNd& null_M){
unsigned size_null_space = 0;
Ravelin::MatrixNd U,V;
Ravelin::VectorNd S;
// SVD decomp to retrieve nullspace of Z'AZ
LA_.svd(M,U,S,V);
if(S.rows() != 0){
// Calculate the tolerance for ruling that a singular value is supposed to be zero
double ZERO_TOL = std::numeric_limits<double>::epsilon() * M.rows() * S[0];
// Count Zero singular values
for(int i = S.rows()-1;i>=0;i--,size_null_space++)
if(S[i] > ZERO_TOL)
break;
// get the nullspace
null_M.set_zero(V.rows(),size_null_space);
V.get_sub_mat(0,V.rows(),V.columns()-size_null_space,V.columns(),null_M);
}
return size_null_space;
}
void Robot::set_model_state(const Ravelin::VectorNd& q,const Ravelin::VectorNd& qd){
Ravelin::VectorNd set_q,set_qd;
if(get_data<Ravelin::VectorNd>("generalized_q", set_q)){
set_q.set_sub_vec(0,q);
_abrobot->set_generalized_coordinates_euler(set_q);
}
if(qd.rows() > 0)
if(get_data<Ravelin::VectorNd>("generalized_qd", set_qd)){
set_qd.set_sub_vec(0,qd);
_abrobot->set_generalized_velocity(Ravelin::DynamicBodyd::eSpatial,set_qd);
}
}
bool Utility::eval_cubic_spline(const Ravelin::VectorNd& coefs,const Ravelin::VectorNd& t_limits,double t,
double& X, double& Xd, double& Xdd){
int k=0;
while(t >= t_limits[k+1]){
k++;
if(k == t_limits.rows()-1)
return false;
}
// Spline always evaluates from t[0] = 0 in interval
// offset t to start of interval to find t in spline
t -= t_limits[0];
X = t*t*t*coefs[k*4] + t*t*coefs[k*4+1] + t*coefs[k*4+2] + coefs[k*4+3];
Xd = 3*t*t*coefs[k*4] + 2*t*coefs[k*4+1] + coefs[k*4+2];
Xdd = 6*t*coefs[k*4] + 2*coefs[k*4+1] ;
return true;
}
void loop(){
boost::shared_ptr<Pacer::Controller> ctrl(ctrl_weak_ptr);
static double start_time = t;
const std::vector<std::string>
eef_names_ = ctrl->get_data<std::vector<std::string> >("init.end-effector.id");
int NUM_FEET = eef_names_.size();
boost::shared_ptr<Ravelin::Pose3d> base_frame( new Ravelin::Pose3d(ctrl->get_data<Ravelin::Pose3d>("base_link_frame")));
Ravelin::VectorNd qd = ctrl->get_joint_generalized_value(Pacer::Controller::velocity);
Ravelin::VectorNd qdd = ctrl->get_joint_generalized_value(Pacer::Controller::acceleration);
int NUM_JOINT_DOFS = qd.rows();
// Initialize end effectors
Ravelin::VectorNd local_q = ctrl->get_generalized_value(Pacer::Controller::position);
#ifdef USE_OSG_DISPLAY
// 1 w/ base position
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Pose3d foot_pose(Ravelin::Matrix3d(link->get_pose()->q)*Ravelin::Matrix3d(0,0,-1, -1,0,0, 0,1,0),link->get_pose()->x,link->get_pose()->rpose);
foot_pose.update_relative_pose(Pacer::GLOBAL);
Utility::visualize.push_back( Pacer::VisualizablePtr( new Pacer::Pose(foot_pose,0.8)));
OUT_LOG(logERROR) << eef_names_[i] << "-orientation: " << t << " " << foot_pose.q;
}
#endif
// q w/o base position
local_q.set_sub_vec(NUM_JOINT_DOFS,Utility::pose_to_vec(Ravelin::Pose3d()));
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Origin3d xd,xdd;
//angular
Ravelin::Origin3d rpy,axd,axdd;
// Calc jacobian for AB at this EEF
Ravelin::MatrixNd J = ctrl->calc_link_jacobian(local_q,eef_names_[i]);
// Now that model state is set ffrom jacobian calculation
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
Ravelin::Pose3d foot_pose(Ravelin::Matrix3d(link->get_pose()->q)*Ravelin::Matrix3d(0,0,-1, -1,0,0, 0,1,0),link->get_pose()->x,link->get_pose()->rpose);
foot_pose.update_relative_pose(Pacer::GLOBAL);
// Ravelin::Origin3d x(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
bool new_var = ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.x",foot_pose.x);
ctrl->set_data<Ravelin::Quatd>(eef_names_[i]+".state.q",foot_pose.q);
J.block(0,3,0,NUM_JOINT_DOFS).mult(qd,xd);
J.block(0,3,0,NUM_JOINT_DOFS).mult(qdd,xdd);
J.block(3,6,0,NUM_JOINT_DOFS).mult(qd,axd);
J.block(3,6,0,NUM_JOINT_DOFS).mult(qdd,axdd);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.xd",xd);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.xdd",xdd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::position,foot_pose.x);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::velocity,xd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::acceleration,xdd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::load,Ravelin::Origin3d(0,0,0));
if(new_var){
ctrl->set_data<Ravelin::Quatd>(eef_names_[i]+".init.q",foot_pose.q);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".init.x",foot_pose.x);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".init.xd",xd);
}
ctrl->set_data<bool>(eef_names_[i]+".stance",false);
}
if(start_time == t){
local_q.segment(0,qd.rows()) = Ravelin::VectorNd::zero(qd.rows());
ctrl->set_model_state(local_q);
std::vector<Ravelin::Origin3d> x1(NUM_FEET) ,x2(NUM_FEET);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Origin3d x,base_x;
ctrl->get_data<Ravelin::Origin3d>(eef_names_[i]+".init.x",x);
base_x = x;
base_x[2] = 0;
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".base",base_x);
x2[i] = base_x;
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
x1[i] = Ravelin::Origin3d(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
}
std::fill(local_q.segment(0,qd.rows()).begin(),local_q.segment(0,qd.rows()).end(),M_PI);
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
x2[i] = Ravelin::Origin3d(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
// write in maximum reach from limb base
double reach = (x1[i]-x2[i]).norm();
ctrl->set_data<double>(eef_names_[i]+".reach",reach*0.95);
}
}
}
void Utility::check_finite(Ravelin::VectorNd& v){
for(int i=0;i<v.rows();i++)
if(!std::isfinite(v[i]))
v[i] = 0;
}
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);
}