Ejemplo n.º 1
0
RTC::ReturnCode_t Sensor::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("in", m_inIn);

  // Set OutPort buffer
  addOutPort("out", m_outOut);

  // Set service provider to Ports

  // Set service consumers to Ports

  // Set CORBA Service Ports

  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable

  // </rtc-template>
  return RTC::RTC_OK;
}
Ejemplo n.º 2
0
RTC::ReturnCode_t LeapRTC::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  addOutPort("frame", m_frameOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("debug", m_debug, "0");
  // </rtc-template>
  
  return RTC::RTC_OK;
}
RTC::ReturnCode_t CoordinateTransformer::onInitialize()
{
	// Registration: InPort/OutPort/Service
	// <rtc-template block="registration">
  // Set InPort buffers
  addInPort("SourceCoord", m_SourceCoordIn);
  
  // Set OutPort buffer
  addOutPort("DestinationCoord", m_DestinationCoordOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
	// </rtc-template>

	// <rtc-template block="bind_config">
	// Bind variables and configuration variable
	bindParameter("transX", m_transX, "0");
	bindParameter("transY", m_transY, "0");
	bindParameter("transZ", m_transZ, "0");
	bindParameter("rot_degX", m_rot_degX, "0");
	bindParameter("rot_degY", m_rot_degY, "0");
	bindParameter("rot_degZ", m_rot_degZ, "0");
	bindParameter("conf.default.scaleX", m_scaleX, "1");
	bindParameter("conf.default.scaleY", m_scaleY, "1");
	bindParameter("conf.default.scaleZ", m_scaleZ, "1");
	bindParameter("conf.default.mirrorXY", m_mirrorXY, "false");
	bindParameter("conf.default.mirrorYZ", m_mirrorYZ, "false");
	bindParameter("conf.default.mirrorZX", m_mirrorZX, "false");
	// </rtc-template>

	return RTC::RTC_OK;
}
Ejemplo n.º 4
0
RTC::ReturnCode_t PA10Controller::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable

  // Set InPort buffers
  addInPort("angle", m_angleIn);
  
  // Set OutPort buffer
  addOutPort("torque", m_torqueOut);
  
  // </rtc-template>

  Pgain = new double[DOF];
  Dgain = new double[DOF];

  gain.open(GAIN_FILE);
  if (gain.is_open()){
    for (int i=0; i<DOF; i++){
      gain >> Pgain[i];
      gain >> Dgain[i];
    }
    gain.close();
  }else{
Ejemplo n.º 5
0
RTC::ReturnCode_t DirectInput::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  addOutPort("lP", m_lPOut);
  addOutPort("lR", m_lROut);
  addOutPort("rglSlider0", m_rglSlider0Out);
  addOutPort("rglSlider1", m_rglSlider1Out);
  addOutPort("POV0", m_POV0Out);
  addOutPort("POV1", m_POV1Out);
  addOutPort("rgbButtons", m_rgbButtonsOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("debug", m_debug, "0");
  // </rtc-template>

  m_lP.data.length(3);
  m_lR.data.length(3);

  m_POV0.data.length(2);
  m_POV1.data.length(2);

  m_rgbButtons.data.length(8);

  return RTC::RTC_OK;
}
Ejemplo n.º 6
0
SeasonQRead::SeasonQRead() {
	addParameter(ADD_PARAMETERS(rain_folder));
	addOutPort(ADD_PARAMETERS(out));
	file = 0;
}
Ejemplo n.º 7
0
CycleNodeStart::CycleNodeStart() {
	addOutPort(ADD_PARAMETERS(out));
	addState("state", &out);
}
Ejemplo n.º 8
0
RTC::ReturnCode_t ThermoEstimator::onInitialize()
{
  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("debugLevel", m_debugLevel, "0");
  
  // </rtc-template>

  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("tauIn", m_tauInIn);
  addInPort("servoStateIn", m_servoStateInIn);

  // Set OutPort buffer
  addOutPort("tempOut", m_tempOutOut);
  addOutPort("servoStateOut", m_servoStateOutOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  // set parmeters
  RTC::Properties& prop = getProperties();
  coil::stringTo(m_dt, prop["dt"].c_str());

  m_robot = hrp::BodyPtr(new hrp::Body());

  RTC::Manager& rtcManager = RTC::Manager::instance();
  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
  int comPos = nameServer.find(",");
  if (comPos < 0){
    comPos = nameServer.length();
  }
  nameServer = nameServer.substr(0, comPos);
  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
                               CosNaming::NamingContext::_duplicate(naming.getRootContext())
        )){
    std::cerr << "failed to load model[" << prop["model"] << "]"
              << std::endl;
  }

  // init outport
  m_tempOut.data.length(m_robot->numJoints());
  m_servoStateIn.data.length(m_robot->numJoints());
  m_servoStateOut.data.length(m_robot->numJoints());
  
  // set temperature of environment
  if (prop["ambient_tmp"] != "") {
    coil::stringTo(m_ambientTemp, prop["ambient_tmp"].c_str());
  } else {
    m_ambientTemp = 25.0;
  }
  std::cerr <<  m_profile.instance_name << ": ambient temperature: " << m_ambientTemp << std::endl;
  
  // set motor heat parameters
  m_motorHeatParams.resize(m_robot->numJoints());
  coil::vstring motorHeatParamsFromConf = coil::split(prop["motor_heat_params"], ",");
  if (motorHeatParamsFromConf.size() != 2 * m_robot->numJoints()) {
    std::cerr <<  "[WARN]: size of motorHeatParams is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl;
  } else {
    std::cerr <<  "motorHeatParams is " << std::endl;
    for (int i = 0; i < m_robot->numJoints(); i++) {
      m_motorHeatParams[i].temperature = m_ambientTemp;
      std::cerr << motorHeatParamsFromConf[2 * i].c_str() << " " << motorHeatParamsFromConf[2 * i + 1].c_str() << std::endl;
      coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str());
      coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str());
    }
  }
  
  return RTC::RTC_OK;
}
Ejemplo n.º 9
0
RTC::ReturnCode_t HockeyArmController::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("ArmXY_in", m_armXY_inIn);
  
  // Set OutPort buffer
  addOutPort("ArmXY_out", m_ArmXY_outOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  m_AHServicePort.registerConsumer("AHCommonDataService", "AHCommon::AHCommonDataService", m_ahCommonDataService);
  
  // Set CORBA Service Ports
  addPort(m_AHServicePort);
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("arm_offset", m_arm_offset, "208.0");
  bindParameter("arm_len_1", m_arm_len_1, "560.0");
  bindParameter("arm_len_2", m_arm_len_2, "319.0");
  bindParameter("initial_enc_x", m_initial_enc_x, "1177");
  bindParameter("initial_enc_y", m_initial_enc_y, "1658");
  bindParameter("slow_limit_x", m_slow_limit_x, "5000");
  bindParameter("slow_limit_y", m_slow_limit_y, "5000");
  
  // </rtc-template>

  /* --- コンポーネント入出力の初期化 --- */
  // 入力の初期化
  m_armXY_in.data.length(2);
  m_armXY_in.data[0] = g_initXY.x;
  m_armXY_in.data[1] = g_initXY.y;
  // 出力の初期化
  m_ArmXY_out.data.length(2);
  m_ArmXY_out.data[0] = g_initXY.x;
  m_ArmXY_out.data[1] = g_initXY.y;
  m_ArmXY_outOut.write();

  /* --- A-IOボードのオープン --- */
  if (openDevices(1, g_opened[0])) return RTC::RTC_ERROR;
  if (openDevices(2, g_opened[1])) return RTC::RTC_ERROR;

  /* --- 位置決め制御インスタンスの初期化 --- */
  
  // X軸
  g_posController[0].init(1, ENCODER_EVAL, SLOW_LIMIT_X*ENCODER_EVAL, 3000);
  g_posController[0].start(true);
  g_posController[0].startMove();
  g_posController[0].servoOn();
  // Y軸
  g_posController[1].init(2, ENCODER_EVAL, SLOW_LIMIT_Y*ENCODER_EVAL, 3000);
  g_posController[1].start(true);
  g_posController[1].startMove();
  g_posController[1].servoOn();

  usleep(500000);

  /* --- clear encoder count with Z --- */
  std::cerr << "!!!" << std::endl;
  clearEncoderCountWithZ(g_posController[0], g_posController[1]);

  /* --- アームを初期位置に移動する --- */
  g_posController[0].moveTo(rad2pulse(g_initTh.x - M_PI/2, 2500*ENCODER_EVAL) * MOTOR_X_RATIO + INIT_ENC_X, 300);
  g_posController[1].moveTo(rad2pulse(g_initTh.y         , 2500*ENCODER_EVAL) * MOTOR_Y_RATIO + INIT_ENC_Y, 300);
  fprintf(stderr, "ARM init OK!\n");

  return RTC::RTC_OK;
}
Ejemplo n.º 10
0
Null::Null() {
	addOutPort("out", &out);
}
Ejemplo n.º 11
0
RTC::ReturnCode_t ImpedanceController::onInitialize()
{
    std::cout << "ImpedanceController::onInitialize()" << std::endl;
    bindParameter("debugLevel", m_debugLevel, "0");

    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers
    addInPort("qCurrent", m_qCurrentIn);
    addInPort("qRef", m_qRefIn);
    addInPort("rpy", m_rpyIn);
    addInPort("rpyRef", m_rpyRefIn);

    // Set OutPort buffer
    addOutPort("q", m_qOut);
  
    // Set service provider to Ports
    m_ImpedanceControllerServicePort.registerProvider("service0", "ImpedanceControllerService", m_service0);
  
    // Set service consumers to Ports
  
    // Set CORBA Service Ports
    addPort(m_ImpedanceControllerServicePort);
  
    // </rtc-template>
    // <rtc-template block="bind_config">
    // Bind variables and configuration variable
  
    // </rtc-template>

    RTC::Properties& prop = getProperties();
    coil::stringTo(m_dt, prop["dt"].c_str());

    m_robot = hrp::BodyPtr(new hrp::Body());

    RTC::Manager& rtcManager = RTC::Manager::instance();
    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
    int comPos = nameServer.find(",");
    if (comPos < 0){
        comPos = nameServer.length();
    }
    nameServer = nameServer.substr(0, comPos);
    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
                                 )){
      std::cerr << "failed to load model[" << prop["model"] << "] in "
                << m_profile.instance_name << std::endl;
      return RTC::RTC_ERROR;
    }

    coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
    int nvforce = virtual_force_sensor.size()/10;
    int nforce  = npforce + nvforce;
    m_force.resize(nforce);
    m_forceIn.resize(nforce);
    m_ref_force.resize(nforce);
    m_ref_forceOut.resize(nforce);
    for (unsigned int i=0; i<npforce; i++){
        hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i);
        m_forceIn[i] = new InPort<TimedDoubleSeq>(s->name.c_str(), m_force[i]);
        m_force[i].data.length(6);
        registerInPort(s->name.c_str(), *m_forceIn[i]);
        m_ref_force[i].data.length(6);
        for (unsigned int j=0; j<6; j++) m_ref_force[i].data[j] = 0.0;
        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string("ref_"+s->name).c_str(), m_ref_force[i]);
        registerOutPort(std::string("ref_"+s->name).c_str(), *m_ref_forceOut[i]);
        std::cerr << s->name << std::endl;
    }
    for (unsigned int i=0; i<nvforce; i++){
        std::string name = virtual_force_sensor[i*10+0];
        VirtualForceSensorParam p;
        hrp::dvector tr(7);
        for (int j = 0; j < 7; j++ ) {
          coil::stringTo(tr[j], virtual_force_sensor[i*10+3+j].c_str());
        }
        p.p = hrp::Vector3(tr[0], tr[1], tr[2]);
        p.R = Eigen::AngleAxis<double>(tr[6], hrp::Vector3(tr[3],tr[4],tr[5])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
        p.parent_link_name = virtual_force_sensor[i*10+2];
        m_sensors[name] = p;
        std::cerr << "virtual force sensor : " << name << std::endl;
        std::cerr << "                T, R : " << p.p[0] << " " << p.p[1] << " " << p.p[2] << std::endl << p.R << std::endl;
        std::cerr << "              parent : " << p.parent_link_name << std::endl;

        m_forceIn[i+npforce] = new InPort<TimedDoubleSeq>(name.c_str(), m_force[i+npforce]);
        m_force[i+npforce].data.length(6);
        registerInPort(name.c_str(), *m_forceIn[i+npforce]);
        m_ref_force[i+npforce].data.length(6);
        for (unsigned int j=0; j<6; j++) m_ref_force[i].data[j] = 0.0;
        m_ref_forceOut[i+npforce] = new OutPort<TimedDoubleSeq>(std::string("ref_"+name).c_str(), m_ref_force[i+npforce]);
        registerOutPort(std::string("ref_"+name).c_str(), *m_ref_forceOut[i+npforce]);
        std::cerr << name << std::endl;
    }
    for (unsigned int i=0; i<m_forceIn.size(); i++){
      abs_forces.insert(std::pair<std::string, hrp::Vector3>(m_forceIn[i]->name(), hrp::Vector3::Zero()));
      abs_moments.insert(std::pair<std::string, hrp::Vector3>(m_forceIn[i]->name(), hrp::Vector3::Zero()));
    }

    unsigned int dof = m_robot->numJoints();
    for ( int i = 0 ; i < dof; i++ ){
      if ( i != m_robot->joint(i)->jointId ) {
        std::cerr << "jointId is not equal to the index number" << std::endl;
        return RTC::RTC_ERROR;
      }
    }

    // allocate memory for outPorts
    m_q.data.length(dof);
    qrefv.resize(dof);
    loop = 0;

    return RTC::RTC_OK;
}
Ejemplo n.º 12
0
RTC::ReturnCode_t TorqueFilter::onInitialize()
{
  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("debugLevel", m_debugLevel, "0");
  
  // </rtc-template>

  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("qCurrent", m_qCurrentIn);
  addInPort("tauIn", m_tauInIn);

  // Set OutPort buffer
  addOutPort("tauOut", m_tauOutOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  RTC::Properties& prop = getProperties();
  coil::stringTo(m_dt, prop["dt"].c_str());

  m_robot = hrp::BodyPtr(new hrp::Body());

  RTC::Manager& rtcManager = RTC::Manager::instance();
  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
  int comPos = nameServer.find(",");
  if (comPos < 0){
    comPos = nameServer.length();
  }
  nameServer = nameServer.substr(0, comPos);
  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
                               CosNaming::NamingContext::_duplicate(naming.getRootContext())
        )){
    std::cerr << "failed to load model[" << prop["model"] << "] in "
              << m_profile.instance_name << std::endl;
    return RTC::RTC_ERROR;
  }

  // init outport
  m_tauOut.data.length(m_robot->numJoints());

  // set gravity compensation flag
  coil::stringTo(m_is_gravity_compensation, prop["gravity_compensation"].c_str());
  if (m_debugLevel > 0) {
    std::cerr <<  m_profile.instance_name << " : gravity compensation flag: " << m_is_gravity_compensation << std::endl;
  }
  
  // set torque offset
  // offset = -(gear torque in neutral pose)
  m_torque_offset.resize(m_robot->numJoints());
  coil::vstring torque_offset = coil::split(prop["torque_offset"], ",");
  if ( m_torque_offset.size() == torque_offset.size() && m_debugLevel > 0) {
    for(int i = 0; i < m_robot->numJoints(); i++){
        coil::stringTo(m_torque_offset[i], torque_offset[i].c_str());
        std::cerr << "[" <<  m_profile.instance_name << "]" << "offset[" << m_robot->joint(i)->name << "]: " << m_torque_offset[i] << std::endl;
    }
  } else {
    if (m_debugLevel > 0) {
      std::cerr << "[" <<  m_profile.instance_name << "]" << "Size of torque_offset is not correct." << std::endl;
      std::cerr << "[" <<  m_profile.instance_name << "]" <<  "joints: " << m_robot->numJoints() << std::endl;
      std::cerr << "[" <<  m_profile.instance_name << "]" <<  "offsets: " << torque_offset.size() << std::endl;
    }
  }
  
  // make filter
  // filter_dim, fb_coeffs[0], ..., fb_coeffs[filter_dim], ff_coeffs[0], ..., ff_coeffs[filter_dim]
  coil::vstring torque_filter_params = coil::split(prop["torque_filter_params"], ","); // filter values
  int filter_dim = 0;
  std::vector<double> fb_coeffs, ff_coeffs;
  bool use_default_flag = false;
  // check size of toruqe_filter_params
  if ( torque_filter_params.size() > 0 ) {
    coil::stringTo(filter_dim, torque_filter_params[0].c_str());
    if (m_debugLevel > 0) {
      std::cerr << "[" <<  m_profile.instance_name << "]" << "filter dim: " << filter_dim << std::endl;
      std::cerr << "[" <<  m_profile.instance_name << "]" << "torque filter param size: " << torque_filter_params.size() << std::endl;
    }
  } else {
    use_default_flag = true;
    if (m_debugLevel > 0) {
      std::cerr<< "[" <<  m_profile.instance_name << "]" << "There is no torque_filter_params. Use default values." << std::endl;
    }
  }
  if (!use_default_flag && ((filter_dim + 1) * 2 + 1 != torque_filter_params.size()) ) {
    if (m_debugLevel > 0) {
      std::cerr<< "[" <<  m_profile.instance_name << "]" << "Size of torque_filter_params is not correct. Use default values." << std::endl;
    }
    use_default_flag = true;
  }
  // define parameters
  if (use_default_flag) {
    // ex) 2dim butterworth filter sampling = 200[hz] cutoff = 5[hz]
    // octave$ [a, b] = butter(2, 5/200)
    // fb_coeffs[0] = 1.00000; <- b0
    // fb_coeffs[1] = 1.88903; <- -b1
    // fb_coeffs[2] = -0.89487; <- -b2
    // ff_coeffs[0] = 0.0014603; <- a0
    // ff_coeffs[1] = 0.0029206; <- a1
    // ff_coeffs[2] = 0.0014603; <- a2
    filter_dim = 2;
    fb_coeffs.resize(filter_dim+1);
    fb_coeffs[0] = 1.00000;
    fb_coeffs[1] = 1.88903;
    fb_coeffs[2] =-0.89487;
    ff_coeffs.resize(filter_dim+1);
    ff_coeffs[0] = 0.0014603;
    ff_coeffs[1] = 0.0029206;
    ff_coeffs[2] = 0.0014603;
  } else {
    fb_coeffs.resize(filter_dim + 1);
    ff_coeffs.resize(filter_dim + 1);
    for (int i = 0; i < filter_dim + 1; i++) {
      coil::stringTo(fb_coeffs[i], torque_filter_params[i + 1].c_str());
      coil::stringTo(ff_coeffs[i], torque_filter_params[i + (filter_dim + 2)].c_str());
    }
  }

  if (m_debugLevel > 0) {
    for (int i = 0; i < filter_dim + 1; i++) {
        std::cerr << "[" <<  m_profile.instance_name << "]" << "fb[" << i << "]: " << fb_coeffs[i] << std::endl;
        std::cerr << "[" <<  m_profile.instance_name << "]" << "ff[" << i << "]: " << ff_coeffs[i] << std::endl;
    }
  }
  
  // make filter instance
  for(int i = 0; i < m_robot->numJoints(); i++){
    m_filters.push_back(IIRFilter(filter_dim, fb_coeffs, ff_coeffs, std::string(m_profile.instance_name)));
  }
  
  return RTC::RTC_OK;
}
Ejemplo n.º 13
0
FlowReadSimple::FlowReadSimple() {
    data = new FlowReadSimple_Private();
    addOutPort(ADD_PARAMETERS(out));
    addParameter(ADD_PARAMETERS(rain_file));
}
Ejemplo n.º 14
0
FlowRead::FlowRead() {
	ctxt.dateformat = "dd.MM.yyyy hh:mm:ss";
	addOutPort(ADD_PARAMETERS(out));
	addParameter(ADD_PARAMETERS(filename));
	addParameter(ADD_PARAMETERS(ctxt.dateformat));
}
Ejemplo n.º 15
0
RTC::ReturnCode_t DirectInput::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  addOutPort("lP", m_lPOut);
  addOutPort("lR", m_lROut);
  addOutPort("rglSlider0", m_rglSlider0Out);
  addOutPort("rglSlider1", m_rglSlider1Out);
  addOutPort("POV0", m_POV0Out);
  addOutPort("POV1", m_POV1Out);
  addOutPort("rgbButtons", m_rgbButtonsOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("debug", m_debug, "0");
  // </rtc-template>

  m_lP.data.length(3);
  m_lR.data.length(3);

  m_POV0.data.length(2);
  m_POV1.data.length(2);

  m_rgbButtons.data.length(8);



	HWND g_hWnd;
	HANDLE g_hInstance;
#define BUFSIZE 1024
	char OldWindowTitle[BUFSIZE];
	char NewWindowTitle[BUFSIZE];

	//	std::cout << "[RTC::DirectInput] Getting Console WndTitle" << std::endl;
	::GetConsoleTitle(OldWindowTitle, BUFSIZE);
	//	std::cout << "[RTC::DirectInput] Title = " << OldWindowTitle << std::endl;
	wsprintf(NewWindowTitle, "ControllerComponent(%d/%d)",
		GetTickCount(), GetCurrentProcessId());
	//	::SetConsoleTitle(NewWindowTitle);

	Sleep(40);

	g_hWnd = FindWindow(NULL, OldWindowTitle);
	if (g_hWnd == NULL) {
	  std::cout << "[RTC::DirectInput] Failed To FindWindow." << std::endl;
	  return RTC::RTC_ERROR;
	}
	//	::SetConsoleTitle(OldWindowTitle);
	
	g_hInstance = ::GetModuleHandle(NULL); //GetWindowLong(g_hWnd, GWL_HINSTANCE);

	std::cout << "[RTC::DirectInput] Creating DirectInput8Manager" << std::endl;
	m_pDirectInputManager = new CDirectInput8Manager(g_hWnd);
	
	//Sleep(1000);


  return RTC::RTC_OK;
}
Ejemplo n.º 16
0
DWF::DWF() {
	addOutPort(ADD_PARAMETERS(out));
	addParameter(ADD_PARAMETERS(dwf_definition_file));
	addParameter(ADD_PARAMETERS(q_scale));
	q_scale = 1.0;
}
Ejemplo n.º 17
0
RTC::ReturnCode_t CollisionDetector::onInitialize()
{
    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
    // <rtc-template block="bind_config">
    // Bind variables and configuration variable
    bindParameter("debugLevel", m_debugLevel, "0");
  
    // </rtc-template>

    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers
    addInPort("qRef", m_qRefIn);
    addInPort("qCurrent", m_qCurrentIn);
    addInPort("servoStateIn", m_servoStateIn);

    // Set OutPort buffer
    addOutPort("q", m_qOut);
    addOutPort("beepCommand", m_beepCommandOut);
  
    // Set service provider to Ports
    m_CollisionDetectorServicePort.registerProvider("service0", "CollisionDetectorService", m_service0);
  
    // Set service consumers to Ports
  
    // Set CORBA Service Ports
    addPort(m_CollisionDetectorServicePort);
  
    // </rtc-template>

    //RTC::Properties& prop = getProperties();

    RTC::Manager& rtcManager = RTC::Manager::instance();
    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
    int comPos = nameServer.find(",");
    if (comPos < 0){
        comPos = nameServer.length();
    }
    nameServer = nameServer.substr(0, comPos);
    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());

    RTC::Properties& prop = getProperties();

    coil::stringTo(m_dt, prop["dt"].c_str());

    if ( prop["collision_viewer"] == "true" ) {
	m_use_viewer = true;
    }
#ifdef USE_HRPSYSUTIL
    m_glbody = new GLbody();
    m_robot = hrp::BodyPtr(m_glbody);
#else
    m_robot = hrp::BodyPtr(new hrp::Body());
#endif // USE_HRPSYSUTIL
    //
    OpenHRP::BodyInfo_var binfo;
    binfo = hrp::loadBodyInfo(prop["model"].c_str(),
			      CosNaming::NamingContext::_duplicate(naming.getRootContext()));
    if (CORBA::is_nil(binfo)){
	std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
		  << std::endl;
	return RTC::RTC_ERROR;
    }
#ifdef USE_HRPSYSUTIL
    if (!loadBodyFromBodyInfo(m_robot, binfo, true, GLlinkFactory)) {
#else
    if (!loadBodyFromBodyInfo(m_robot, binfo, true, hrplinkFactory)) {
#endif // USE_HRPSYSUTIL
      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
                << m_profile.instance_name << std::endl;
      return RTC::RTC_ERROR;
    }
#ifdef USE_HRPSYSUTIL
    loadShapeFromBodyInfo(m_glbody, binfo);
#endif // USE_HRPSYSUTIL
    if ( prop["collision_model"] == "AABB" ) {
        convertToAABB(m_robot);
    } else if ( prop["collision_model"] == "convex hull" ||
                prop["collision_model"] == "" ) { // set convex hull as default
        convertToConvexHull(m_robot);
    }
    setupVClipModel(m_robot);

    if ( prop["collision_pair"] != "" ) {
	std::cerr << "[" << m_profile.instance_name << "] prop[collision_pair] ->" << prop["collision_pair"] << std::endl;
	std::istringstream iss(prop["collision_pair"]);
	std::string tmp;
	while (getline(iss, tmp, ' ')) {
	    size_t pos = tmp.find_first_of(':');
	    std::string name1 = tmp.substr(0, pos), name2 = tmp.substr(pos+1);
            if ( m_robot->link(name1)==NULL ) {
                std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name1 << std::endl;
		std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
		for (unsigned int i=0; i < m_robot->numLinks(); i++) {
		  std::cerr << " " << m_robot->link(i)->name;
		}
		std::cerr << std::endl;
                continue;
            }
            if ( m_robot->link(name2)==NULL ) {
                std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name2 << std::endl;
		std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
		for (unsigned int i=0; i < m_robot->numLinks(); i++) {
		  std::cerr << " " << m_robot->link(i)->name;
		}
		std::cerr << std::endl;
                continue;
            }
	    std::cerr << "[" << m_profile.instance_name << "] check collisions between " << m_robot->link(name1)->name << " and " <<  m_robot->link(name2)->name << std::endl;
	    m_pair[tmp] = new CollisionLinkPair(new VclipLinkPair(m_robot->link(name1), m_VclipLinks[m_robot->link(name1)->index],
                                                                  m_robot->link(name2), m_VclipLinks[m_robot->link(name2)->index], 0));
	}
    }

    if ( prop["collision_loop"] != "" ) {
        coil::stringTo(m_collision_loop, prop["collision_loop"].c_str());
        std::cerr << "[" << m_profile.instance_name << "] set collision_loop: " << m_collision_loop << std::endl;
    }
#ifdef USE_HRPSYSUTIL
    if ( m_use_viewer ) {
      m_scene.addBody(m_robot);
      GLlink::drawMode(GLlink::DM_COLLISION);
    }
#endif // USE_HRPSYSUTIL

    // true (1) do not move when collide,
    // false(0) move even if collide
    m_curr_collision_mask.resize(m_robot->numJoints()); // collision_mask used to select output                0: passthough reference data, 1 output safe data
    std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0);
    m_init_collision_mask.resize(m_robot->numJoints()); // collision_mask defined in .conf as [collisoin_mask] 0: move even if collide, 1: do not move when collide
    std::fill(m_init_collision_mask.begin(), m_init_collision_mask.end(), 1);
    if ( prop["collision_mask"] != "" ) {
	std::cerr << "[" << m_profile.instance_name << "] prop[collision_mask] ->" << prop["collision_mask"] << std::endl;
        coil::vstring mask_str = coil::split(prop["collision_mask"], ",");
        if (mask_str.size() == m_robot->numJoints()) {
            for (size_t i = 0; i < m_robot->numJoints(); i++) {coil::stringTo(m_init_collision_mask[i], mask_str[i].c_str()); }
            for (size_t i = 0; i < m_robot->numJoints(); i++) {
                if ( m_init_collision_mask[i] == 0 ) {
                    std::cerr << "[" << m_profile.instance_name << "] CollisionDetector will not control " << m_robot->joint(i)->name << std::endl;
                }
            }
        }else{
            std::cerr << "[" << m_profile.instance_name << "] ERROR size of collision_mask is differ from robot joint number .. " << mask_str.size()  << ", " << m_robot->numJoints() << std::endl;
        }
    }

    if ( prop["use_limb_collision"] != "" ) {
        std::cerr << "[" << m_profile.instance_name << "] prop[use_limb_collision] -> " << prop["use_limb_collision"] << std::endl;
        if ( prop["use_limb_collision"] == "true" ) {
            m_use_limb_collision = true;
            std::cerr << "[" << m_profile.instance_name << "] Enable use_limb_collision" << std::endl;
        }
    }

    // setup collision state
    m_state.angle.length(m_robot->numJoints());
    m_state.collide.length(m_robot->numLinks());

    // allocate memory for outPorts
    m_q.data.length(m_robot->numJoints());
    m_recover_time = 0;
    m_safe_posture = true;
    m_have_safe_posture = false;
    i_dt = 1.0;
    default_recover_time = 2.5/m_dt;
    m_recover_jointdata = new double[m_robot->numJoints()];
    m_lastsafe_jointdata = new double[m_robot->numJoints()];
    m_interpolator = new interpolator(m_robot->numJoints(), i_dt);
    m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator");
    m_link_collision = new bool[m_robot->numLinks()];

    for(unsigned int i=0; i<m_robot->numJoints(); i++){
      m_q.data[i] = 0;
    }

    m_servoState.data.length(m_robot->numJoints());
    for(unsigned int i = 0; i < m_robot->numJoints(); i++) {
        m_servoState.data[i].length(1);
        int status = 0;
        status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
        status |= 1<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
        status |= 1<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
        status |= 0<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
        status |= 0<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
        m_servoState.data[i][0] = status;
    }

    collision_beep_freq = static_cast<int>(1.0/(3.0*m_dt)); // 3 times / 1[s]
    m_beepCommand.data.length(bc.get_num_beep_info());
    return RTC::RTC_OK;
}



RTC::ReturnCode_t CollisionDetector::onFinalize()
{
    delete[] m_recover_jointdata;
    delete[] m_lastsafe_jointdata;
    delete m_interpolator;
    delete[] m_link_collision;
    return RTC::RTC_OK;
}

/*
  RTC::ReturnCode_t CollisionDetector::onStartup(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t CollisionDetector::onShutdown(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/

RTC::ReturnCode_t CollisionDetector::onActivated(RTC::UniqueId ec_id)
{
    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
    m_have_safe_posture = false;
    return RTC::RTC_OK;
}
Ejemplo n.º 18
0
ConstSource::ConstSource() {
	addParameter(ADD_PARAMETERS(const_flow))
                .setUnit("m^3/s, g/m^3");
	addOutPort(ADD_PARAMETERS(out));
}
Ejemplo n.º 19
0
RTC::ReturnCode_t StateHolder::onInitialize()
{
  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  
  // </rtc-template>

  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
    addInPort("currentQIn", m_currentQIn);
    addInPort("qIn", m_qIn);
    addInPort("tqIn", m_tqIn);
    addInPort("basePosIn", m_basePosIn);
    addInPort("baseRpyIn", m_baseRpyIn);
    addInPort("zmpIn", m_zmpIn);
    addInPort("optionalDataIn", m_optionalDataIn);
  
  // Set OutPort buffer
    addOutPort("qOut", m_qOut);
    addOutPort("tqOut", m_tqOut);
    addOutPort("basePosOut", m_basePosOut);
    addOutPort("baseRpyOut", m_baseRpyOut);
    addOutPort("baseTformOut", m_baseTformOut);
    addOutPort("basePoseOut", m_basePoseOut);
    addOutPort("zmpOut", m_zmpOut);
    addOutPort("optionalDataOut", m_optionalDataOut);
  
  // Set service provider to Ports
  m_StateHolderServicePort.registerProvider("service0", "StateHolderService", m_service0);
  m_TimeKeeperServicePort.registerProvider("service1", "TimeKeeperService", m_service1);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  addPort(m_StateHolderServicePort);
  addPort(m_TimeKeeperServicePort);

  // </rtc-template>

  RTC::Properties& prop = getProperties();
  coil::stringTo(m_dt, prop["dt"].c_str());
  std::cout << "StateHolder: dt = " << m_dt << std::endl;
  RTC::Manager& rtcManager = RTC::Manager::instance();
  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
  int comPos = nameServer.find(",");
  if (comPos < 0){
      comPos = nameServer.length();
  }
  nameServer = nameServer.substr(0, comPos);
  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
  OpenHRP::BodyInfo_var binfo;
  binfo = hrp::loadBodyInfo(prop["model"].c_str(),
                            CosNaming::NamingContext::_duplicate(naming.getRootContext()));
  OpenHRP::LinkInfoSequence_var lis = binfo->links();
  const OpenHRP::LinkInfo& li = lis[0];
  hrp::Vector3 p, axis;
  p << li.translation[0], li.translation[1], li.translation[2];
  axis << li.rotation[0], li.rotation[1], li.rotation[2];
  hrp::Matrix33 R = hrp::rodrigues(axis, li.rotation[3]);
  hrp::Vector3 rpy = hrp::rpyFromRot(R);
  
  m_baseTform.data.length(12);
  double *T = m_baseTform.data.get_buffer();
  T[0] = R(0,0); T[1] = R(0,1); T[ 2] = R(0,2); T[ 3] = p[0];
  T[4] = R(0,0); T[5] = R(0,1); T[ 6] = R(0,2); T[ 7] = p[1];
  T[8] = R(0,0); T[9] = R(0,1); T[10] = R(0,2); T[11] = p[2];
  m_basePos.data.x = m_basePose.data.position.x = p[0];
  m_basePos.data.y = m_basePose.data.position.y = p[1];
  m_basePos.data.z = m_basePose.data.position.z = p[2];
  m_baseRpy.data.r = m_basePose.data.orientation.r = rpy[0];
  m_baseRpy.data.p = m_basePose.data.orientation.p = rpy[1];
  m_baseRpy.data.y = m_basePose.data.orientation.y = rpy[2];
  m_zmp.data.x = m_zmp.data.y = m_zmp.data.z = 0.0;

  // Setting for wrench data ports (real + virtual)
  std::vector<std::string> fsensor_names;
  //   find names for real force sensors
  for ( int k = 0; k < lis->length(); k++ ) {
    OpenHRP::SensorInfoSequence& sensors = lis[k].sensors;
    for ( int l = 0; l < sensors.length(); l++ ) {
      if ( std::string(sensors[l].type) == "Force" ) {
        fsensor_names.push_back(std::string(sensors[l].name));
      }
    }
  }
  int npforce = fsensor_names.size();
  //   find names for virtual force sensors
  coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
  int nvforce = virtual_force_sensor.size()/10;
  for (unsigned int i=0; i<nvforce; i++){
    fsensor_names.push_back(virtual_force_sensor[i*10+0]);
  }
  //   add ports for all force sensors
  int nforce  = npforce + nvforce;
  m_wrenches.resize(nforce);
  m_wrenchesIn.resize(nforce);
  m_wrenchesOut.resize(nforce);
  for (unsigned int i=0; i<nforce; i++){
    m_wrenchesIn[i] = new InPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"In").c_str(), m_wrenches[i]);
    m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Out").c_str(), m_wrenches[i]);
    m_wrenches[i].data.length(6);
    m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
    m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
    registerInPort(std::string(fsensor_names[i]+"In").c_str(), *m_wrenchesIn[i]);
    registerOutPort(std::string(fsensor_names[i]+"Out").c_str(), *m_wrenchesOut[i]);
  }

  
  return RTC::RTC_OK;
}
RTC::ReturnCode_t SequencePlayer::onInitialize()
{
    std::cout << "SequencePlayer::onInitialize()" << std::endl;
    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers
    addInPort("qInit", m_qInitIn);
    addInPort("basePosInit", m_basePosInitIn);
    addInPort("baseRpyInit", m_baseRpyInitIn);
  
    // Set OutPort buffer
    addOutPort("qRef", m_qRefOut);
    addOutPort("zmpRef", m_zmpRefOut);
    addOutPort("accRef", m_accRefOut);
    addOutPort("basePos", m_basePosOut);
    addOutPort("baseRpy", m_baseRpyOut);
  
    // Set service provider to Ports
    m_SequencePlayerServicePort.registerProvider("service0", "SequencePlayerService", m_service0);
  
    // Set service consumers to Ports
  
    // Set CORBA Service Ports
    addPort(m_SequencePlayerServicePort);
  
    // </rtc-template>
    // <rtc-template block="bind_config">
    // Bind variables and configuration variable
  
    // </rtc-template>

    RTC::Properties& prop = getProperties();
    double dt;
    coil::stringTo(dt, prop["dt"].c_str());

    m_robot = new Body();

    RTC::Manager& rtcManager = RTC::Manager::instance();
    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
    int comPos = nameServer.find(",");
    if (comPos < 0){
        comPos = nameServer.length();
    }
    nameServer = nameServer.substr(0, comPos);
    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
                                 )){
        std::cerr << "failed to load model[" << prop["model"] << "]" 
                  << std::endl;
    }

    unsigned int dof = m_robot->numJoints();

    m_seq = new seqplay(dof, dt);

    m_qInit.data.length(dof);
    for (unsigned int i=0; i<dof; i++) m_qInit.data[i] = 0.0;
    m_basePosInit.data.x = m_basePosInit.data.y = m_basePosInit.data.z = 0.0; 
    m_baseRpyInit.data.r = m_baseRpyInit.data.p = m_baseRpyInit.data.y = 0.0;

    // allocate memory for outPorts
    m_qRef.data.length(dof);

    return RTC::RTC_OK;
}
Ejemplo n.º 21
0
RTC::ReturnCode_t TorqueController::onInitialize()
{
  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("debugLevel", m_debugLevel, "0");
  
  // </rtc-template>

  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("tauCurrent", m_tauCurrentInIn);
  addInPort("tauMax", m_tauMaxInIn);
  addInPort("qCurrent", m_qCurrentInIn);
  addInPort("qRef", m_qRefInIn); // for naming rule of hrpsys_config.py

  // Set OutPort buffer
  addOutPort("q", m_qRefOutOut); // for naming rule of hrpsys_config.py
  
  // Set service provider to Ports
  m_TorqueControllerServicePort.registerProvider("service0", "TorqueControllerService", m_service0);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  addPort(m_TorqueControllerServicePort);
  
  // </rtc-template>

  // read property settings
  RTC::Properties& prop = getProperties();
  // get dt
  coil::stringTo(m_dt, prop["dt"].c_str()); 
  // make rtc manager settings
  RTC::Manager& rtcManager = RTC::Manager::instance();
  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
  int comPos = nameServer.find(",");
  if (comPos < 0){
    comPos = nameServer.length();
  }
  nameServer = nameServer.substr(0, comPos);
  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
  // set robot model
  m_robot = hrp::BodyPtr(new hrp::Body());
  std::cerr << prop["model"].c_str() << std::endl;
  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
                               CosNaming::NamingContext::_duplicate(naming.getRootContext())
        )){
    std::cerr << "failed to load model[" << prop["model"] << "]"
              << std::endl;
  }
  // make torque controller settings
  coil::vstring motorTorqueControllerParamsFromConf = coil::split(prop["torque_controller_params"], ",");
  hrp::dvector tdcParamKe(m_robot->numJoints()), tdcParamKd(m_robot->numJoints()), tdcParamKi(m_robot->numJoints()), tdcParamT(m_robot->numJoints());
  if (motorTorqueControllerParamsFromConf.size() == 2 * m_robot->numJoints()) {
    std::cerr << "use TwoDofController" << std::endl;
    for (int i = 0; i < m_robot->numJoints(); i++) { // use TwoDofController
      coil::stringTo(tdcParamKe[i], motorTorqueControllerParamsFromConf[2 * i].c_str());
      coil::stringTo(tdcParamT[i], motorTorqueControllerParamsFromConf[2 * i + 1].c_str());
      m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamT[i], m_dt));
    }
    if (m_debugLevel > 0) {
      std::cerr << "torque controller parames:" << std::endl;
      for (int i = 0; i < m_robot->numJoints(); i++) {
        std::cerr << m_robot->joint(i)->name << ":" << tdcParamKe[i] << " " << tdcParamT[i] << " " << m_dt << std::endl;
      }
    }
  } else if (motorTorqueControllerParamsFromConf.size() == 3 * m_robot->numJoints()) { // use TwoDofControllerPDModel
    std::cerr << "use TwoDofControllerPDModel" << std::endl;
    for (int i = 0; i < m_robot->numJoints(); i++) { // use TwoDofControllerPDModel
      coil::stringTo(tdcParamKe[i], motorTorqueControllerParamsFromConf[3 * i].c_str());
      coil::stringTo(tdcParamKd[i], motorTorqueControllerParamsFromConf[3 * i + 1].c_str());
      coil::stringTo(tdcParamT[i], motorTorqueControllerParamsFromConf[3 * i + 2].c_str());
      m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamKd[i], tdcParamT[i], m_dt));
    }
    if (m_debugLevel > 0) {
      std::cerr << "torque controller parames:" << std::endl;
      for (int i = 0; i < m_robot->numJoints(); i++) {
        std::cerr << m_robot->joint(i)->name << ":" << tdcParamKe[i] << " " << tdcParamKd[i] << " " << tdcParamT[i] << " " << m_dt << std::endl;
      }
    }
  } else if (motorTorqueControllerParamsFromConf.size() == 4 * m_robot->numJoints()) { // use TwoDofControllerDynamicsModel
    std::cerr << "use TwoDofControllerDynamicsModel" << std::endl;
    for (int i = 0; i < m_robot->numJoints(); i++) { // use TwoDofControllerDynamicsModel
      coil::stringTo(tdcParamKe[i], motorTorqueControllerParamsFromConf[4 * i].c_str());
      coil::stringTo(tdcParamKd[i], motorTorqueControllerParamsFromConf[4 * i + 1].c_str());
      coil::stringTo(tdcParamKi[i], motorTorqueControllerParamsFromConf[4 * i + 2].c_str());
      coil::stringTo(tdcParamT[i], motorTorqueControllerParamsFromConf[4 * i + 3].c_str());
      m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamKd[i], tdcParamKi[i], tdcParamT[i], m_dt));
    }
    if (m_debugLevel > 0) {
      std::cerr << "torque controller parames:" << std::endl;
      for (int i = 0; i < m_robot->numJoints(); i++) {
        std::cerr << m_robot->joint(i)->name << ":" << tdcParamKe[i] << " " << tdcParamKd[i] << " " << tdcParamKi[i] << " " << tdcParamT[i] << " " << m_dt << std::endl;
      }
    }
  } else { // default
    std::cerr << "[WARNING] torque_controller_params is not correct number, " << motorTorqueControllerParamsFromConf.size() << ". Use default controller." << std::endl;
    for (int i = 0; i < m_robot->numJoints(); i++) {
      tdcParamKe[i] = 400.0;
      tdcParamT[i] = 0.04;
      m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamT[i], m_dt));
    }
    if (m_debugLevel > 0) {
      std::cerr << "torque controller parames:" << std::endl;
      for (int i = 0; i < m_robot->numJoints(); i++) {
        std::cerr << m_robot->joint(i)->name << ":" << tdcParamKe[i] << " " << tdcParamT[i] << " " << m_dt << std::endl;
      }
    }
  }

  // allocate memory for outPorts
  m_qRefOut.data.length(m_robot->numJoints());
  return RTC::RTC_OK;
}
Ejemplo n.º 22
0
RTC::ReturnCode_t AriaRTC::onInitialize()
{
    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers
    addInPort("targetVelocity", m_targetVelocityIn);
    addInPort("poseUpdate", m_poseUpdateIn);

    // Set OutPort buffer
    addOutPort("currentVelocity", m_currentVelocityOut);
    addOutPort("currentPose", m_currentPoseOut);
    addOutPort("range", m_rangeOut);
    addOutPort("bumper", m_bumperOut);
    addOutPort("sonar", m_sonarOut);

    // Set service provider to Ports

    // Set service consumers to Ports

    // Set CORBA Service Ports

    // </rtc-template>

    // <rtc-template block="bind_config">
    // Bind variables and configuration variable
    bindParameter("debug", m_debug, "0");
    bindParameter("robotPort", m_robotPort, "COM1");
    bindParameter("robotPortType", m_robotPortType, "serial");
    bindParameter("robotTcpPort", m_robotTcpPort, "8101");
    bindParameter("robotTcpAddress", m_robotTcpAddress, "localhost");
    bindParameter("laserPort", m_laserPort, "COM2");
    bindParameter("laserPortType", m_laserPortType, "serial");
    bindParameter("laserType", m_laserType, "none");
    bindParameter("laserTcpPort", m_laserTcpPort, "8102");
    bindParameter("bumperSendingPolicy", m_bumperSendingPolicy, "event");
    bindParameter("commandTimeout", m_commandTimeout, "3");
    bindParameter("odometryUpdateInterval", m_odometryUpdateInterval, "0.5");
    bindParameter("initial_pose_x", m_initial_pose_x, "0.0");
    bindParameter("initial_pose_y", m_initial_pose_y, "0.0");
    bindParameter("initial_pose_z", m_initial_pose_z, "0.0");
    bindParameter("transvelmax", m_transvelmax, "0.75");
    bindParameter("rotvelmax", m_rotvelmax, "1.75");
    bindParameter("rotacc", m_rotacc, "1.75");
    bindParameter("rotdecel", m_rotdecel, "1.75");
    bindParameter("transacc", m_transacc, "0.3");
    bindParameter("transdecel", m_transdecel, "0.3");

    bindParameter("update_gain", m_update_gain, "false");
    bindParameter("gain_rotkp", m_gain_rotkp, "40");
    bindParameter("gain_rotkv", m_gain_rotkv, "20");
    bindParameter("gain_rotki", m_gain_rotki, "0");
    bindParameter("gain_transkp", m_gain_transkp, "40");
    bindParameter("gain_transkv", m_gain_transkv, "30");
    bindParameter("gain_transki", m_gain_transki, "0");

    /*
    bindParameter("update_flash", m_update_flash, "false");
    bindParameter("flash_revcount", m_flash_revcount, "0");
    bindParameter("flash_driftfactor", m_flash_driftfactor, "0");
    bindParameter("flash_ticksmm", m_flash_ticksmm, "0");
    */


    // </rtc-template>


    ssr::MobileRobot::init();
    return RTC::RTC_OK;
}
Ejemplo n.º 23
0
RTC::ReturnCode_t GraspController::onInitialize()
{
  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("debugLevel", m_debugLevel, "0");
  
  // </rtc-template>

  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("qRef", m_qRefIn); // for naming rule of hrpsys_config.py
  addInPort("qCurrent", m_qCurrentIn);
  addInPort("qIn", m_qIn);
  
  // Set OutPort buffer
  addOutPort("q", m_qOut); // for naming rule of hrpsys_config.py
  
  // Set service provider to Ports
  m_GraspControllerServicePort.registerProvider("service0", "GraspControllerService", m_service0);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  addPort(m_GraspControllerServicePort);
  
  // </rtc-template>

  m_robot = hrp::BodyPtr(new hrp::Body());
  RTC::Properties& prop = getProperties();
  coil::stringTo(m_dt, prop["dt"].c_str());

  RTC::Manager& rtcManager = RTC::Manager::instance();
  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
  int comPos = nameServer.find(",");
  if (comPos < 0){
      comPos = nameServer.length();
  }
  nameServer = nameServer.substr(0, comPos);
  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
                               CosNaming::NamingContext::_duplicate(naming.getRootContext())
          )){
      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" 
                << std::endl;
      return RTC::RTC_ERROR;
  }
  // <name> : <joint1>, <direction1>, <joint2>, <direction2>, <joint1>, <direction2>, <name> : <joint1>, <direction1>
  // check num of grasp
  coil::vstring grasp_joint_params = coil::split(prop["grasp_joint_groups"], ",");
  std::string grasp_name;
  GraspJoint grasp_joint;
  std::vector<GraspJoint> grasp_joints;
  for(int i = 0, f = 0; i < grasp_joint_params.size(); i++ ){
    coil::vstring grasp_joint_group_names = coil::split(grasp_joint_params[i], ":");
    if ( grasp_joint_group_names.size() > 1 ) {
      if ( grasp_name != "" ) {
        GraspParam grasp_param;
        grasp_param.time = 0;
        grasp_param.joints = grasp_joints;
        grasp_param.time = 1; // stop
        m_grasp_param[grasp_name] = grasp_param;
        grasp_joints.clear();
      }
      // initialize
      grasp_name = grasp_joint_group_names[0];
      if ( !! m_robot->link(grasp_joint_group_names[1]) ) {
        grasp_joint.id = m_robot->link(std::string(grasp_joint_group_names[1].c_str()))->jointId;
      }
      f = 0;
      i++;
    }
    if ( f == 0 ) {
      coil::stringTo(grasp_joint.dir,grasp_joint_params[i].c_str());
      grasp_joints.push_back(grasp_joint);
      f = 1 ;
    } else {
      if ( !! m_robot->link(grasp_joint_params[i]) ) {
        grasp_joint.id = m_robot->link(grasp_joint_params[i])->jointId;
      }
      f = 0 ;
    }
  }
  // finalize
  if ( grasp_name != "" ) {
    GraspParam grasp_param;
    grasp_param.time = 0;
    grasp_param.joints = grasp_joints;
    grasp_param.time = 1; // stop
    m_grasp_param[grasp_name] = grasp_param;
  }
  //
  if ( m_debugLevel ) {
    std::map<std::string, GraspParam >::iterator it = m_grasp_param.begin();
    while ( it != m_grasp_param.end() ) {
      std::cerr << "[" << m_profile.instance_name << "] " << it->first << " : ";
      for ( int i = 0 ; i < it->second.joints.size(); i++ ) {
        std::cerr << "id = " << it->second.joints[i].id << ", dir = " << it->second.joints[i].dir << ", ";
      }
      std::cerr << std::endl;
      it++;
    }
  }


  return RTC::RTC_OK;
}
Ejemplo n.º 24
0
RTC::ReturnCode_t SequencePlayer::onInitialize()
{
    std::cout << "SequencePlayer::onInitialize()" << std::endl;
    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers
    addInPort("qInit", m_qInitIn);
    addInPort("basePosInit", m_basePosInitIn);
    addInPort("baseRpyInit", m_baseRpyInitIn);
    addInPort("zmpRefInit", m_zmpRefInitIn);
  
    // Set OutPort buffer
    addOutPort("qRef", m_qRefOut);
    addOutPort("tqRef", m_tqRefOut);
    addOutPort("zmpRef", m_zmpRefOut);
    addOutPort("accRef", m_accRefOut);
    addOutPort("basePos", m_basePosOut);
    addOutPort("baseRpy", m_baseRpyOut);
    addOutPort("optionalData", m_optionalDataOut);
  
    // Set service provider to Ports
    m_SequencePlayerServicePort.registerProvider("service0", "SequencePlayerService", m_service0);
  
    // Set service consumers to Ports
  
    // Set CORBA Service Ports
    addPort(m_SequencePlayerServicePort);
  
    // </rtc-template>
    // <rtc-template block="bind_config">
    // Bind variables and configuration variable
  
    bindParameter("debugLevel", m_debugLevel, "0");
    // </rtc-template>

    RTC::Properties& prop = getProperties();
    coil::stringTo(dt, prop["dt"].c_str());

    m_robot = hrp::BodyPtr(new Body());

    RTC::Manager& rtcManager = RTC::Manager::instance();
    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
    int comPos = nameServer.find(",");
    if (comPos < 0){
        comPos = nameServer.length();
    }
    nameServer = nameServer.substr(0, comPos);
    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
                                 )){
        std::cerr << "failed to load model[" << prop["model"] << "]" 
                  << std::endl;
    }

    unsigned int dof = m_robot->numJoints();


    // Setting for wrench data ports (real + virtual)
    std::vector<std::string> fsensor_names;
    //   find names for real force sensors
    unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
    for (unsigned int i=0; i<npforce; i++){
      fsensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
    }
    //   find names for virtual force sensors
    coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
    unsigned int nvforce = virtual_force_sensor.size()/10;
    for (unsigned int i=0; i<nvforce; i++){
      fsensor_names.push_back(virtual_force_sensor[i*10+0]);
    }
    //   add ports for all force sensors
    unsigned int nforce  = npforce + nvforce;
    m_wrenches.resize(nforce);
    m_wrenchesOut.resize(nforce);
    for (unsigned int i=0; i<nforce; i++){
      m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Ref").c_str(), m_wrenches[i]);
      m_wrenches[i].data.length(6);
      registerOutPort(std::string(fsensor_names[i]+"Ref").c_str(), *m_wrenchesOut[i]);
    }

    if (prop.hasKey("seq_optional_data_dim")) {
      coil::stringTo(optional_data_dim, prop["seq_optional_data_dim"].c_str());
    } else {
      optional_data_dim = 1;
    }

    m_seq = new seqplay(dof, dt, nforce, optional_data_dim);

    m_qInit.data.length(dof);
    for (unsigned int i=0; i<dof; i++) m_qInit.data[i] = 0.0;
    Link *root = m_robot->rootLink();
    m_basePosInit.data.x = root->p[0]; m_basePosInit.data.y = root->p[1]; m_basePosInit.data.z = root->p[2];
    hrp::Vector3 rpy = hrp::rpyFromRot(root->R);
    m_baseRpyInit.data.r = rpy[0]; m_baseRpyInit.data.p = rpy[1]; m_baseRpyInit.data.y = rpy[2];
    m_zmpRefInit.data.x = 0; m_zmpRefInit.data.y = 0; m_zmpRefInit.data.z = 0;

    // allocate memory for outPorts
    m_qRef.data.length(dof);
    m_tqRef.data.length(dof);
    m_optionalData.data.length(optional_data_dim);

    return RTC::RTC_OK;
}