Exemplo n.º 1
0
void controller_callback(Moby::ControlledBodyPtr dbp, double t, void*)
{
  double p =  0.001*cos(t*(2.0*M_PI)*500.0);
  double v =  -0.001*sin(t*1000.0) *(2.0*M_PI)*500.0;
  double Kp = 1e3
      ,Kv = 1e1
              ;

  for(int i=0;i<slide->get_joints().size();i++){
    if(slide->get_joints()[i]->num_dof() == 0) continue;
    static Ravelin::VectorNd U(1);
    U[0] = Kp*(p - slide->get_joints()[i]->q[0]) + Kv*(v - slide->get_joints()[i]->qd[0]);
    std::cout << "x = [ " << slide->get_joints()[i]->q[0] << " , " << slide->get_joints()[i]->qd[0] << " ];" << std::endl;
    std::cout << "x* = [ " << p << " , " << v << " ];" << std::endl;
    std::cout << "U = [ " << U[0] << " ];" << std::endl;
    slide->get_joints()[i]->add_force(U);
  }
}
void init(void* separator, const std::map<std::string, Moby::BasePtr>& read_map, double time)
{
  const unsigned Z = 2;

  // get a reference to the TimeSteppingSimulator instance
  for (std::map<std::string, Moby::BasePtr>::const_iterator i = read_map.begin();
       i !=read_map.end(); i++)
  {
    // Find the simulator reference
    if (!sim)
      sim = boost::dynamic_pointer_cast<TimeSteppingSimulator>(i->second);
    if (i->first == "ur10_schunk_hybrid")
      robot = boost::dynamic_pointer_cast<RCArticulatedBody>(i->second);
  }

  assert(robot);
  robot->controller = &controller; 
  
  // sets the starting velocity for the robot joints (do not modify this)
  const std::vector<shared_ptr<Jointd> >& joints = robot->get_joints();
    robot->get_generalized_coordinates_euler(tempQ);
  for (unsigned i=0; i< joints.size(); i++)
    if (joints[i]->joint_id.find("fixed") == std::string::npos && 
        joints[i]->joint_id != "world_joint")
      q_init[joints[i]->joint_id] = tempQ[joints[i]->get_coord_index()];


  // setup the constraint callback function
  //sim->constraint_callback_fn = &constraint_callback_fn;
  
  const double PERIOD = 5.0;
  const double AMP = 0.5;
  const double SMALL_AMP = AMP * 0.1;
  std::map<std::string, double> qd_init;
  qd_init["shoulder_pan_joint"] = AMP*PERIOD;
  qd_init["shoulder_lift_joint"] = SMALL_AMP*PERIOD*2.0;
  qd_init["elbow_joint"] = AMP*PERIOD*2.0/3.0;
  qd_init["wrist_1_joint"] = AMP*PERIOD*1.0/7.0;
  qd_init["wrist_2_joint"] = AMP*PERIOD*2.0/11.0;
  qd_init["wrist_3_joint"] = AMP*PERIOD*3.0/13.0;
  qd_init["l_finger_actuator"] = 0.0;
  qd_init["r_finger_actuator"] = 0.0;
  VectorNd qd;
  robot->get_generalized_velocity(DynamicBodyd::eEuler, qd);
  for (unsigned i=0; i< joints.size(); i++)
    qd[joints[i]->get_coord_index()] = qd_init[joints[i]->joint_id];
  robot->set_generalized_velocity(DynamicBodyd::eEuler, qd);
  
  // read gains from the gains file
  std::ifstream in("gains.dat");
  if (in.fail())
  {
    std::cerr << "Failed to open 'gains.dat' for reading!" << std::endl;
    exit(-1);
  }
  char c;bool stringbool=true; bool P=true; bool I=false; bool D = false;
  bool done = false;
  std::string gainsString;
  std::string jointString; 
  std::string Num;
  std::string pNum; 
  std::string iNum; 
  std::string dNum;
  int i = 0;
  std::stringstream ss;
  while(in.get(c)){
   ss << c;
  }
  gainsString = ss.str();
  
  
  // overwrite any output files
  for (std::map<std::string, double>::const_iterator i = q_init.begin(); i != q_init.end(); i++)
  {
    const std::string& joint_name = i->first;
    std::string fname1 = joint_name + "_desiredPID.txt";
    std::string fname2 = joint_name + "_statePID.txt";
    std::ofstream out1(fname1.c_str());
    std::ofstream out2(fname2.c_str());
    out1.close();
    out2.close();
  }



  std::string lineString;
  // read the step size
  char* stepsize_str = getenv("STEP_SIZE");
  //std::cout << stepsize_str<<std::endl;
  //float stepsize_str = getenv("STEP_SIZE");
  
  if (!stepsize_str)
    throw std::runtime_error("No STEP_SIZE variable specified!");
  step_size = std::atof(stepsize_str);
  //std::cout << step_size << std::endl;

  for (int i = 0; i < gainsString.length(); i++){
     char c = gainsString[i];//this char is the one that is iterated through in the text file
     if ( (c >64 && c < 91) || (c > 96 && c < 123) || c == 95 || (c>47 && c<58)){//corresponds to the available
        jointString += c;  //ASCII values for letters and numbers, and hyphens
     }else if (c == 9){//if a tab is found, means that a number is coming
	while (true){i++; c=gainsString[i]; if (c!=9){break;}}//skip all tabs until a number is found
        while (true){
	   if (c == 9){//if a another tab is found that means its the next number
	      while (true){i++; c=gainsString[i]; if (c!=9){break;}}//skip thru tabs
	      if (P){P = false; I = true;}//if its time for p gain, we know i gain is next so set bool
	      else if (I){I = false; D = true;}
	      else if (D){
		D = false; P = true;
	      }
	   }else if (c == '\n'){
		double pDouble= atof(pNum.c_str());//if a new line is found we know we have obtained all values
	   	double iDouble= atof(iNum.c_str());//so turn the strings into doubles
	   	double dDouble= atof(dNum.c_str());
	   	Ravelin::Origin3d gainValues(pDouble,iDouble,dDouble);//created an O3D of gains
	   	PID_gains.insert(std::pair<std::string,Origin3d>(jointString,gainValues));//insert into map
		jointString = "";//reset all found values to begin finding the next row
		pNum = "";               
		iNum = "";		
		dNum = "";
		D = false; P = true;
		break;
	   }
	   if (P){
	        pNum += c;//if its time for a p gain to be entered, add the sucker in
	   }else if (D){
		dNum += c;
	   }else if (I){
		iNum += c;
	   }

	   i++;
	   c = gainsString[i];
        }
     }
  }
}