Esempio n. 1
0
/// 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;
}
Esempio n. 2
0
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);
  }
}
Esempio n. 3
0
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
}
Esempio n. 4
0
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;
}
Esempio n. 5
0
void loop(){
boost::shared_ptr<Pacer::Controller> ctrl(ctrl_weak_ptr);
  
  const unsigned X=0,Y=1,Z=2,THETA=5;
    // World frame
    boost::shared_ptr<Ravelin::Pose3d>
        environment_frame(new Ravelin::Pose3d());
    Utility::visualize.push_back(Pacer::VisualizablePtr( new Pacer::Pose(*environment_frame.get())));

      boost::shared_ptr<Ravelin::Pose3d>
        base_horizontal_frame(new Ravelin::Pose3d(ctrl->get_data<Ravelin::Pose3d>("base_horizontal_frame")));

    Ravelin::Vector3d com(base_horizontal_frame->x.data());

  OUT_LOG(logERROR) << "x = " << base_horizontal_frame->x ;
  
  Ravelin::VectorNd base_xd;
  ctrl->get_base_value(Pacer::Robot::velocity,base_xd);
  OUT_LOG(logERROR) << "xd = " << base_xd ;

    /////////////////////
    /// SET VARIABLES ///

    // Command robot to walk in a direction
    Ravelin::VectorNd command;
    command.set_zero(6);

    double max_forward_speed = ctrl->get_data<double>(plugin_namespace+".max-forward-speed");
    double max_strafe_speed  = ctrl->get_data<double>(plugin_namespace+".max-strafe-speed");
    double max_turn_speed    = ctrl->get_data<double>(plugin_namespace+".max-turn-speed");

    // if FALSE, drive like a car (x and theta)
    // Q: if TRUE, turn toward waypoint while stepping in direction of waypoint
    bool HOLONOMIC = false;

    /////////////////////////////////////
    /// Assign WAYPOINTS in the plane ///
    static std::vector<Point> waypoints;
  
    std::vector<double> waypoints_vec = ctrl->get_data<std::vector<double> >(plugin_namespace+".waypoints");
  
    // if (waypoints not inititalized) OR (waypoints changed) OR (only one waypoint)
    // update waypoints
    if(waypoints.empty() || waypoints_vec.size()/2 != waypoints.size() || waypoints_vec.size() == 2){
      waypoints.clear();
      for(int i=0;i<waypoints_vec.size();i+=2)
        waypoints.push_back(Point(waypoints_vec[i],waypoints_vec[i+1]));
    }
        
    /////////////////////////////
    /// CHOOSE NEXT WAYPOINT  ///

    int num_waypoints = waypoints.size();
  
    // if waypoints are empty
    // do not do anything (do not update SE2 command)
    if(num_waypoints != 0){
      Ravelin::Vector3d goto_point;

      static int waypoint_index = 0;
      static Ravelin::Vector3d
      next_waypoint(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
      if(waypoints.size() == 1){
        waypoint_index = 0;
        next_waypoint = Ravelin::Vector3d(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
      }
      double distance_to_wp = (Ravelin::Origin3d(next_waypoint.data()) - Ravelin::Origin3d(com[X],com[Y],0)).norm();

      double max_dist = ctrl->get_data<double>(plugin_namespace+".tolerance");
      if( distance_to_wp < max_dist){
      OUT_LOG(logDEBUG1) << "waypoint reached, incrementing waypoint.";
      OUTLOG(next_waypoint,"this_wp",logDEBUG1);
      OUT_LOG(logDEBUG1) << "this waypoint: " << next_waypoint;
      OUT_LOG(logDEBUG1) << "robot position: " << com;

      waypoint_index = (waypoint_index+1)% num_waypoints;
      if(waypoint_index == 0)
        exit(0);
      next_waypoint = Ravelin::Vector3d(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
      }

      OUT_LOG(logDEBUG1) << "num_wps = " << num_waypoints;
      OUT_LOG(logDEBUG1) << "distance_to_wp = " << distance_to_wp;
      OUT_LOG(logDEBUG1) << "waypoint_index = " << waypoint_index;
      Utility::visualize.push_back(Pacer::VisualizablePtr(new Pacer::Ray(next_waypoint,com,Ravelin::Vector3d(1,0.5,0))));
      OUT_LOG(logDEBUG1) << "next_wp" << next_waypoint;

      for(int i=0;i<num_waypoints;i++){
          Ravelin::Vector3d wp(waypoints[i].first,waypoints[i].second,0,environment_frame);
          OUT_LOG(logDEBUG1) << "\twp" << wp;
      Utility::visualize.push_back(Pacer::VisualizablePtr( new Pacer::Point(wp,Ravelin::Vector3d(1,0.5,0),0.1)));
      }

      goto_point = next_waypoint;

      ///////////////////////
      /// GO TO WAYPOINT  ///

      // Find direction to waypoint from robot position
      Ravelin::Vector3d goto_direction =
        Ravelin::Vector3d(goto_point[X],goto_point[Y],0,environment_frame)
        - Ravelin::Vector3d(com[X],com[Y],0,environment_frame);
      goto_direction = Ravelin::Pose3d::transform_vector(base_horizontal_frame,goto_direction);
      goto_direction.normalize();

      double angle_to_goal = atan2(goto_direction[Y],goto_direction[X]);
      OUT_LOG(logDEBUG) << "angle_to_goal = " << angle_to_goal;

      // If robot is facing toward goal already, walk in that direction
      if(fabs(angle_to_goal) < M_PI_4){
        if(HOLONOMIC){
          command[Y] = goto_direction[Y]*max_strafe_speed;
        }
        command[X] = goto_direction[X]*max_forward_speed;
        command[THETA] = angle_to_goal;
      } else {
        command[THETA] = Utility::sign(angle_to_goal)*max_turn_speed;
        if(HOLONOMIC){
          command[X] = goto_direction[X]*max_forward_speed;
          command[Y] = goto_direction[Y]*max_strafe_speed;
        } else {
          command[X] = 0;
          command[Y] = 0;
        }
      }
    
//      double slow_down_tol = 0.1;
//      if(distance_to_wp < slow_down_tol)
//        command *= distance_to_wp*slow_down_tol+0.05;
    
      Ravelin::Origin3d command_SE2(command[X],command[Y],command[THETA]);
      ctrl->set_data<Ravelin::Origin3d>("SE2_command",command_SE2);
    }
}
Esempio n. 6
0
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);
    }
  }
}
Esempio n. 7
0
void Utility::check_finite(Ravelin::VectorNd& v){
  for(int i=0;i<v.rows();i++)
    if(!std::isfinite(v[i]))
      v[i] = 0;
}
Esempio n. 8
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);
}