예제 #1
0
void RigidBody::step(double dt){
  FILE_LOG(logDEBUG)<<"entered RigidBody::step(double dt)"<<std::endl;

  // set com
  // NOTE: com is transform r in THIS case
  for(unsigned i=0;i<3;i++)
      com[i] = Tws(i,3);

  // NOTE: set accel for integration function
  vd_ = &vd;

  FILE_LOG(logDEBUG) << "q (before) = " << q << std::endl;
  FILE_LOG(logDEBUG) << "v (before) = " << v << std::endl;
  FILE_LOG(logDEBUG) << "vd (before) = " << vd << std::endl;

  // Set forces to apply
  // apply drag force
  {
      double k = 0.1;
      Ravelin::Vector3d drag;
      v.get_sub_vec(0,3,drag);

      drag = -drag *= (drag.dot(drag) * k);
      apply_force(drag,com);
  }

  // apply forces
  calc_fwd_dynamics();

  // Set up state
    Vec state;
    state.set_zero(q.rows() + v.rows());
    state.set_sub_vec(0,q);
    state.set_sub_vec(q.rows(),v);

  // Integrate state forward 1 step
    sim_->integrate(sim_->time,dt,&fn,state);

  // re-normalize quaternion state
    state.get_sub_vec(0,q.rows(),q);
    state.get_sub_vec(q.rows(),state.rows(),v);
    Vec e;
    q.get_sub_vec(3,7,e);
    e.normalize();
    q.set_sub_vec(3,e);

  FILE_LOG(logDEBUG) << "q (after) = " << q << std::endl;
  FILE_LOG(logDEBUG) << "v (after) = " << v << std::endl;
  FILE_LOG(logDEBUG) << "vd (after) = " << vd << std::endl;

  // Update graphics transform
  update();

  FILE_LOG(logDEBUG)<<"exited RigidBody::step(.)"<<std::endl;
  reset_accumulators();
}
예제 #2
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);
    }
}
예제 #3
0
Ravelin::Vector3d Utility::slerp( const Ravelin::Vector3d& v0,const Ravelin::Vector3d& v1,double t){
  double a = v0.dot(v1);
  return (v0*sin((1-t)*a)/sin(a) + v1*sin(t*a)/sin(a));
}
예제 #4
0
double Utility::distance_from_plane(const Ravelin::Vector3d& normal,const Ravelin::Vector3d& point, const Ravelin::Vector3d& x){
  Ravelin::Vector3d v;
  v = x - point;
  return normal.dot(v);
}
예제 #5
0
/// Gets the distance from a cylinder primitive
double PlanePrimitive::calc_signed_dist(shared_ptr<const CylinderPrimitive> pA, Point3d& pthis, Point3d& pcyl) const
{
  FILE_LOG(LOG_COLDET) << "PlanePrimitive::calc_signed_dist(.) entered" << std::endl;

  // Output Params
  // Set intitial value of distance to contact
  double d = std::numeric_limits<double>::infinity();

  // get the pose for the plane primitive
  boost::shared_ptr<const Ravelin::Pose3d> Pplane = pthis.pose;
  // get the pose for the cylinder
  boost::shared_ptr<const Ravelin::Pose3d> Pcyl = pcyl.pose;

  const double R = pA->get_radius();
  const double H = pA->get_height();
  const unsigned X = 0, Y = 1,Z = 2;

  // Cylinder to Plane frame Tranformation
  Ravelin::Transform3d pPc = Ravelin::Pose3d::calc_relative_pose(Pcyl,Pplane);

  // Cylinder axis (in plane frame) cN
  // Take Y column from Cylinder to Plane frame Tranformation
  Ravelin::Vector3d cN = Ravelin::Vector3d(
                           Ravelin::Matrix3d(pPc.q).get_column(Y),
                           Pplane);
  cN.normalize();

  // Plane Normal is Y axis in local frame
  Ravelin::Vector3d n(0,1,0,Pplane);

  // cylinder origin w.r.t. plane
  Point3d c0(pPc.x.data(),Pplane);

  double n_dot_cN = n.dot(cN);

  // Axial direction points toward plane parallel to axis of cylinder
  Ravelin::Vector3d axial_dir = cN;
  if(n_dot_cN > 0)
    axial_dir = -axial_dir;
  axial_dir.normalize();

  // if Cylinder is aligned with plane:
  // Return distance cylinder origin to
  // closest point on plane minus height
  if(fabs(n_dot_cN) > 1.0-1e-8){
    FILE_LOG(LOG_COLDET) << " -- Cylinder axis is perpendicular to Plane" << std::endl;

    // Measure from center of closest face to the plane
    Point3d x = (H/2.0)*axial_dir + c0;

    // Distance above plane
    d = x.dot(n);

    // Find point on plane that x is closest to
    pcyl =  Ravelin::Pose3d::transform_point(Moby::GLOBAL,x);
    Point3d pP = x - d*n;
    pthis =  Ravelin::Pose3d::transform_point(Moby::GLOBAL,pP);
  } else if(fabs(n_dot_cN) < 1e-8){
    FILE_LOG(LOG_COLDET) << " -- Cylinder axis is parallel to Plane"<< std::endl;

    // Measure from center of closest edge (on curved face) to the plane
    // Radial direction and Plane normal coincide in this case
    Point3d x = c0 - R*n + (H/2.0)*axial_dir;

    // Distance above plane
    d = x.dot(n);

    pcyl =  Ravelin::Pose3d::transform_point(Moby::GLOBAL,x);
    Point3d pP = x - d*n;
    pthis =  Ravelin::Pose3d::transform_point(Moby::GLOBAL,pP);
  } 
  else {
    FILE_LOG(LOG_COLDET) << " -- Cylinder edge is closest to plane"<< std::endl;

    //(axis_cylinder x (n_plane x axis_cylinder))
    // Radial direction points toward plane perpendicular to axis of cylinder
    Ravelin::Vector3d radial_dir =
        Ravelin::Vector3d::cross(
          axial_dir,
          Ravelin::Vector3d::cross(axial_dir,n)
        );
    radial_dir.normalize();
     
    //                                            (--(|)  
    // Through axis, toward plane "axial direction"^, ^on face, toward plane "radial direction"
    FILE_LOG(LOG_COLDET) << "axial_dir = " << axial_dir << std::endl;
    FILE_LOG(LOG_COLDET) << "p_axis = " << n << std::endl;
    FILE_LOG(LOG_COLDET) << "radial_dir = " << radial_dir << std::endl;

    // Measure from point to the plane
    Point3d x = c0 + (H/2.0)*axial_dir + R*radial_dir;
    FILE_LOG(LOG_COLDET) << "x_cyl (plane frame) = " << x << std::endl;
    
    // Distance above plane
    d = x.dot(n);

    Point3d pP = x - d*n;
    FILE_LOG(LOG_COLDET) << "x_plane (plane frame) = " << x << std::endl;

    pcyl =  Ravelin::Pose3d::transform_point(Moby::GLOBAL,x);
    pthis =  Ravelin::Pose3d::transform_point(Moby::GLOBAL,pP);
  }

  FILE_LOG(LOG_COLDET) << "Point cylinder (global frame): "<<  pcyl << std::endl;
  FILE_LOG(LOG_COLDET) << "Point plane (global frame): "<<  pthis << std::endl;
  FILE_LOG(LOG_COLDET) << "distance: "<<  d << std::endl;

  return d;
}