Ejemplo n.º 1
0
int main(int argc, char ** argv)
{
  struct sigaction sa;
  bzero(&sa, sizeof(sa));
  sa.sa_handler = handle;
  if (0 != sigaction(SIGINT, &sa, 0)) {
    err(EXIT_FAILURE, "sigaction");
  }
  
  // Before we attempt to read any tasks and skills from the YAML
  // file, we need to inform the static type registry about custom
  // additions such as the HelloGoodbyeSkill.
  Factory::addSkillType<uta_opspace::HelloGoodbyeSkill>("uta_opspace::HelloGoodbyeSkill");
  
  ros::init(argc, argv, "wbc_m3_ctrl_servo", ros::init_options::NoSigintHandler);
  parse_options(argc, argv);
  ros::NodeHandle node("~");
  
  controller.reset(new ControllerNG("wbc_m3_ctrl::servo"));
  param_cbs.reset(new ParamCallbacks());
  Servo servo;
  try {
    if (verbose) {
      warnx("initializing param callbacks");
    }
    registry.reset(factory->createRegistry());
    registry->add(controller);
    param_cbs->init(node, registry, 1, 100);
    
    if (verbose) {
      warnx("starting servo with %lld Hz", servo_rate);
    }
    actual_servo_rate = servo_rate;
    servo.start(servo_rate);
  }
  catch (std::runtime_error const & ee) {
    errx(EXIT_FAILURE, "failed to start servo: %s", ee.what());
  }
  
  warnx("started servo RT thread");
  ros::Time dbg_t0(ros::Time::now());
  ros::Time dump_t0(ros::Time::now());
  ros::Duration dbg_dt(0.1);
  ros::Duration dump_dt(0.05);
  
  while (ros::ok()) {
    ros::Time t1(ros::Time::now());
    if (verbose) {
      if (t1 - dbg_t0 > dbg_dt) {
	dbg_t0 = t1;
	servo.skill->dbg(cout, "\n\n**************************************************", "");
	controller->dbg(cout, "--------------------------------------------------", "");
	cout << "--------------------------------------------------\n";
	jspace::pretty_print(model->getState().position_, cout, "jpos", "  ");
	jspace::pretty_print(model->getState().velocity_, cout, "jvel", "  ");
	jspace::pretty_print(model->getState().force_, cout, "jforce", "  ");
	jspace::pretty_print(controller->getCommand(), cout, "gamma", "  ");
	Vector gravity;
	model->getGravity(gravity);
	jspace::pretty_print(gravity, cout, "gravity", "  ");
	cout << "servo rate: " << actual_servo_rate << "\n";
      }
    }
    if (t1 - dump_t0 > dump_dt) {
      dump_t0 = t1;
      controller->qhlog(*servo.skill, rt_get_cpu_time_ns() / 1000);
    }
    ros::spinOnce();
    usleep(10000);		// 100Hz-ish
  }
  
  warnx("shutting down");
  servo.shutdown();
}
Ejemplo n.º 2
0
int main(int argc, char ** argv)
{
  struct sigaction sa;
  bzero(&sa, sizeof(sa));
  sa.sa_handler = handle;
  if (0 != sigaction(SIGINT, &sa, 0)) {
    err(EXIT_FAILURE, "sigaction");
  }
  
  // Before we attempt to read any tasks and skills from the YAML
  // file, we need to inform the static type registry about custom
  // additions such as the HelloGoodbyeSkill.
  Factory::addSkillType<uta_opspace::RigidTf>("uta_opspace::RigidTf");
  Factory::addSkillType<uta_opspace::SurfaceMotion>("uta_opspace::SurfaceMotion");
  Factory::addSkillType<uta_opspace::SurfaceOriMotion>("uta_opspace::SurfaceOriMotion");
  Factory::addSkillType<uta_opspace::AttachSurface>("uta_opspace::AttachSurface");
  Factory::addSkillType<uta_opspace::KnownAttachSurface>("uta_opspace::KnownAttachSurface");
  Factory::addSkillType<uta_opspace::JointMultiPos>("uta_opspace::JointMultiPos");
  
  
  ros::init(argc, argv, "wbc_fri_servo", ros::init_options::NoSigintHandler);
  parse_options(argc, argv);
  ros::NodeHandle node("~");

  jspace::Vector command(Vector::Zero(7));
  pos_prev = Vector::Zero(7);
  //Need to setup the transform here
  //Should be changed to a parameter in TAO
  jspace::Matrix R(Matrix::Identity(3,3));
  R(0,0) = 0.9221;
  R(0,1) = -0.0576;
  R(0,2) = -0.3827;
  R(1,0) = 0.3791;
  R(1,1) = -0.0643;
  R(1,2) = 0.9231;
  R(2,0) = -0.0778;
  R(2,1) = -0.9963;
  R(2,2) = -0.0374;
  jspace::Matrix d(Matrix::Zero(3,1));
  /*d(0,0) = -1.0877;
  d(1,0) = 2.8646;
  d(2,0) = 0.0528;*/
  d(0,0) = -1.0877;
  d(1,0) = 3.0146;
  d(2,0) = 0.0428;
  
  controller.reset(new ControllerNG("wbc_fri::servo"));
  param_cbs.reset(new ParamCallbacks());
  Servo servo;
  if (verbose) {
    warnx("initializing param callbacks");
  }
  registry.reset(factory->createRegistry());
  registry->add(controller);
  param_cbs->init(node, registry, 1, 100);

  //Initial message
  ros::Publisher torque_pub = node.advertise<JointEfforts>("/JointEffortCommand", 1000);
  ros::Publisher jointimp_pub = node.advertise<FriJointImpedance>("/FriJointImpedance", 1000);
  ros::Subscriber state_sub = node.subscribe("/JointState", 1000, stateCallback);
  ros::Subscriber mass_sub = node.subscribe("/MassMatrix", 1000, massCallback);
  ros::Subscriber fristate_sub = node.subscribe("/FriJointState", 1000, friCallback);
  JointEfforts torque_msg;
  FriJointImpedance jointimp_msg;

  for (size_t ii(0); ii<7; ++ii) {
	torque_msg.efforts.push_back(0.0);
	jointimp_msg.stiffness[ii] = 0.0;
	jointimp_msg.damping[ii] = 0.0;
  }
  torque_pub.publish(torque_msg);
  jointimp_pub.publish(jointimp_msg);



  //UDP camera- with option of no camera
  Position3d rawCamData[56];
  jspace::Matrix camData;

  //EDIT ME to actual camera address
  char local_cam_port [10];
  sprintf(local_cam_port, "%d", CLIENT_PORT+2);
  char cam_port [10];
  sprintf(cam_port, "%d", CS8C_PORT+2);
  UdpOSI cameraUDP("50000", "192.168.1.39", cam_port, 0);
  int bytes =0;
  if (cam) {
    bytes = cameraUDP.recvPacket((char*)rawCamData, sizeof(rawCamData));
    if (bytes<1) {
      fprintf(stderr, "Error receiving data from camera");
	 servo.cleanup();
	 return 0;
    }
    camData = Matrix::Zero(bytes/sizeof(Position3d),3);
    for (size_t ii=0; ii < bytes/sizeof(Position3d); ++ii){
      camData(ii,0) = rawCamData[ii].x*1e-3;
      camData(ii,1) = rawCamData[ii].y*1e-3;
      camData(ii,2) = rawCamData[ii].z*1e-3;
    } 
    Matrix temp(R * camData.transpose() + d*Matrix::Ones(1,bytes/sizeof(Position3d)));
    state.camData_ = temp.transpose();
    cout <<"Recieved initial camera data\n";
    }  

  /*
  //Visualization
  visinfo info;
  UdpOSI visUDP( "57860", const_cast<char*>(CS8C_IPADDR), "50000", 0);*/
  

  int status = servo.init(state);
  if (0!= status) {
    fprintf(stderr, "init callback returned %d\n", status);
    servo.cleanup();
    return 0;
  }

  ros::Time dbg_t0(ros::Time::now());
  ros::Time dump_t0(ros::Time::now());
  ros::Duration dbg_dt(0.1);
  ros::Duration dump_dt(0.05);
 //ros::Rate loop_rate(10);
  ros::Time begin(ros::Time::now());
  while (ros::ok()) {

    //UDP camera in and out
    if (cam) {
      bytes = cameraUDP.recvPacket((char*)rawCamData, sizeof(rawCamData));
      if (bytes <1) {
	fprintf(stderr,"Error: no camera data recieved");
	servo.cleanup();
	return 0;
      }    
      for (size_t ii=0; ii < bytes/sizeof(Position3d); ++ii){
	camData(ii,0) = rawCamData[ii].x*1e-3;
	camData(ii,1) = rawCamData[ii].y*1e-3;
	camData(ii,2) = rawCamData[ii].z*1e-3;
      }
      Matrix temp(R * camData.transpose() + d*Matrix::Ones(1,bytes/sizeof(Position3d)));
      state.camData_ = temp.transpose();
    }  

    /*
    //Vis out
    for (size_t ii(0); ii<6 ; ++ii) {
      info.q[ii] = state.position_[ii];
    }
    Skill::task_table_t const * tasks(servo.skill->getTaskTable());
    info.R[0] = 0.1;
    info.T[0] = 0.4;//No longer automatic-> should not be manual
    if (cam) {
      for (size_t ii(0); ii < state.camData_.rows(); ++ii) {
	for (size_t jj(0); jj < state.camData_.cols(); ++jj) {
	  info.sp[ii][jj] = state.camData_(ii,jj);
	}
      }
    }

    info.eedes[0] = 0.0;
    info.eedes[1] = 0.0;
    info.eedes[2] = 0.0;
    
    info.cp[0] = 0.0;
    info.cp[1] = 0.0;
    info.cp[2] = 0.0;


    bytes = visUDP.sendPacket((char*)&info, sizeof(info));
     */

    status = servo.update(state, command);
    if (0 != status) {
      fprintf(stderr, "update callback returned %d\n", status);
      servo.cleanup();
      return 0;
    }

    for (size_t ii(0); ii<7; ++ii) {
      if (isnan(command[ii])) {
	torque_msg.efforts[ii] = 0;
      }
      else {
          torque_msg.efforts[ii] = command[ii];
      }
    }

    torque_pub.publish(torque_msg);
    jointimp_pub.publish(jointimp_msg);
    ros::Time t1(ros::Time::now());
    if (verbose) {
      if (t1 - dbg_t0 > dbg_dt) {
	dbg_t0 = t1;
	servo.skill->dbg(cout, "\n\n**************************************************", "");
	controller->dbg(cout, "--------------------------------------------------", "");
	cout << "--------------------------------------------------\n";
	jspace::pretty_print(model->getState().position_, cout, "jpos", "  ");
	jspace::pretty_print(controller->getCommand(), cout, "gamma", "  ");
	jspace::pretty_print(model->getState().camData_, cout, "cam", "  ");
      }
    }
    if (t1 - dump_t0 > dump_dt) {
      dump_t0 = t1;
      controller->qhlog(*servo.skill, ros::Duration(t1-begin).toSec()*1000);
    }
    ros::spinOnce();
    //loop_rate.sleep();
  }
  
  warnx("shutting down");
  ros::shutdown();
  servo.cleanup();
  //cameraUDP.~UdpOSI();
}