void copy_octet_base::construct()
{
Resource_impl::_started = false;
loadProperties();
serviceThread = 0;
sentEOS = false;
inputPortOrder.resize(0);;
outputPortOrder.resize(0);
PortableServer::ObjectId_var oid;
octet_in = new bulkio::InOctetPort("octet_in");
octet_in->setNewStreamListener(this, ©_octet_base::octet_in_newStreamCallback);
oid = ossie::corba::RootPOA()->activate_object(octet_in);
octet_out = new bulkio::OutOctetPort("octet_out");
oid = ossie::corba::RootPOA()->activate_object(octet_out);
registerInPort(octet_in);
inputPortOrder.push_back("octet_in");
registerOutPort(octet_out, octet_out->_this());
outputPortOrder.push_back("octet_out");
}
void multiply_const_ss_base::construct()
{
Resource_impl::_started = false;
loadProperties();
serviceThread = 0;
sentEOS = false;
inputPortOrder.resize(0);;
outputPortOrder.resize(0);
PortableServer::ObjectId_var oid;
short_in = new BULKIO_dataShort_In_i("short_in",&_sriListener );
oid = ossie::corba::RootPOA()->activate_object(short_in);
short_out = new BULKIO_dataShort_Out_i("short_out", this);
oid = ossie::corba::RootPOA()->activate_object(short_out);
registerInPort(short_in);
inputPortOrder.push_back("short_in");
registerOutPort(short_out, short_out->_this());
outputPortOrder.push_back("short_out");
}
void interleave_cc_1i_base::construct()
{
Resource_impl::_started = false;
loadProperties();
serviceThread = 0;
sentEOS = false;
inputPortOrder.resize(0);;
outputPortOrder.resize(0);
PortableServer::ObjectId_var oid;
complex_in = new bulkio::InFloatPort("complex_in");
complex_in->setNewStreamListener(this, &interleave_cc_1i_base::complex_in_newStreamCallback);
oid = ossie::corba::RootPOA()->activate_object(complex_in);
complex_out = new bulkio::OutFloatPort("complex_out");
oid = ossie::corba::RootPOA()->activate_object(complex_out);
registerInPort(complex_in);
inputPortOrder.push_back("complex_in");
registerOutPort(complex_out, complex_out->_this());
outputPortOrder.push_back("complex_out");
}
void streams_to_vector_ii_1i_base::construct()
{
Resource_impl::_started = false;
loadProperties();
serviceThread = 0;
sentEOS = false;
inputPortOrder.resize(0);;
outputPortOrder.resize(0);
PortableServer::ObjectId_var oid;
long_in = new bulkio::InLongPort("long_in");
long_in->setNewStreamListener(this, &streams_to_vector_ii_1i_base::long_in_newStreamCallback);
oid = ossie::corba::RootPOA()->activate_object(long_in);
long_out = new bulkio::OutLongPort("long_out");
oid = ossie::corba::RootPOA()->activate_object(long_out);
registerInPort(long_in);
inputPortOrder.push_back("long_in");
registerOutPort(long_out, long_out->_this());
outputPortOrder.push_back("long_out");
}
void chunks_to_symbols_sf_base::construct()
{
Resource_impl::_started = false;
loadProperties();
serviceThread = 0;
sentEOS = false;
inputPortOrder.resize(0);;
outputPortOrder.resize(0);
PortableServer::ObjectId_var oid;
short_in = new bulkio::InShortPort("short_in");
short_in->setNewStreamListener(this, &chunks_to_symbols_sf_base::short_in_newStreamCallback);
oid = ossie::corba::RootPOA()->activate_object(short_in);
float_out = new bulkio::OutFloatPort("float_out");
oid = ossie::corba::RootPOA()->activate_object(float_out);
registerInPort(short_in);
inputPortOrder.push_back("short_in");
registerOutPort(float_out, float_out->_this());
outputPortOrder.push_back("float_out");
}
void stream_to_streams_cc_5o_base::construct()
{
Resource_impl::_started = false;
loadProperties();
serviceThread = 0;
sentEOS = false;
inputPortOrder.resize(0);;
outputPortOrder.resize(0);
PortableServer::ObjectId_var oid;
complex_in = new bulkio::InFloatPort("complex_in");
complex_in->setNewStreamListener(this, &stream_to_streams_cc_5o_base::complex_in_newStreamCallback);
oid = ossie::corba::RootPOA()->activate_object(complex_in);
complex_out_0 = new bulkio::OutFloatPort("complex_out_0");
oid = ossie::corba::RootPOA()->activate_object(complex_out_0);
complex_out_1 = new bulkio::OutFloatPort("complex_out_1");
oid = ossie::corba::RootPOA()->activate_object(complex_out_1);
complex_out_2 = new bulkio::OutFloatPort("complex_out_2");
oid = ossie::corba::RootPOA()->activate_object(complex_out_2);
complex_out_3 = new bulkio::OutFloatPort("complex_out_3");
oid = ossie::corba::RootPOA()->activate_object(complex_out_3);
complex_out_4 = new bulkio::OutFloatPort("complex_out_4");
oid = ossie::corba::RootPOA()->activate_object(complex_out_4);
registerInPort(complex_in);
inputPortOrder.push_back("complex_in");
registerOutPort(complex_out_0, complex_out_0->_this());
outputPortOrder.push_back("complex_out_0");
registerOutPort(complex_out_1, complex_out_1->_this());
outputPortOrder.push_back("complex_out_1");
registerOutPort(complex_out_2, complex_out_2->_this());
outputPortOrder.push_back("complex_out_2");
registerOutPort(complex_out_3, complex_out_3->_this());
outputPortOrder.push_back("complex_out_3");
registerOutPort(complex_out_4, complex_out_4->_this());
outputPortOrder.push_back("complex_out_4");
}
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;
}
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 ReferenceForceUpdater::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("basePosIn", m_basePosIn);
addInPort("baseRpyIn",m_baseRpyIn);
addInPort("rpy",m_rpyIn);
// Set service provider to Ports
m_ReferenceForceUpdaterServicePort.registerProvider("service0", "ReferenceForceUpdaterService", m_ReferenceForceUpdaterService);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_ReferenceForceUpdaterServicePort);
// Get dt
RTC::Properties& prop = getProperties(); // get properties information from .wrl file
coil::stringTo(m_dt, prop["dt"].c_str());
// Make m_robot instance
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(), // load robot model for m_robot
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}
// 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);
}
// load virtual force sensors
readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
unsigned int nvforce = m_vfs.size();
for (unsigned int i=0; i<nvforce; i++){
for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
if (it->second.id == (int)i) fsensor_names.push_back(it->first);
}
}
// add ports for all force sensors (real force, input and output of ref_force)
unsigned int nforce = npforce + nvforce;
m_force.resize(nforce);
m_forceIn.resize(nforce);
m_ref_force_in.resize(nforce);
m_ref_force_out.resize(nforce);
m_ref_forceIn.resize(nforce);
m_ref_forceOut.resize(nforce);
std::cerr << "[" << m_profile.instance_name << "] create force sensor ports" << std::endl;
for (unsigned int i=0; i<nforce; i++){
// actual inport
m_forceIn[i] = new InPort<TimedDoubleSeq>(fsensor_names[i].c_str(), m_force[i]);
m_force[i].data.length(6);
registerInPort(fsensor_names[i].c_str(), *m_forceIn[i]);
// ref inport
m_ref_force_in[i].data.length(6);
for (unsigned int j=0; j<6; j++) m_ref_force_in[i].data[j] = 0.0;
m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+fsensor_names[i]+"In").c_str(), m_ref_force_in[i]);
registerInPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), *m_ref_forceIn[i]);
std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl;
// ref Outport
m_ref_force_out[i].data.length(6);
for (unsigned int j=0; j<6; j++) m_ref_force_out[i].data[j] = 0.0;
m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string("ref_"+fsensor_names[i]+"Out").c_str(), m_ref_force_out[i]);
registerOutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), *m_ref_forceOut[i]);
std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl;
}
// setting from conf file
// rleg,TARGET_LINK,BASE_LINK,x,y,z,rx,ry,rz,rth #<=pos + rot (axis+angle)
coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
if (end_effectors_str.size() > 0) {
size_t prop_num = 10;
size_t num = end_effectors_str.size()/prop_num;
for (size_t i = 0; i < num; i++) {
std::string ee_name, ee_target, ee_base;
coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
ee_trans eet;
for (size_t j = 0; j < 3; j++) {
coil::stringTo(eet.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
}
double tmpv[4];
for (int j = 0; j < 4; j++ ) {
coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
}
eet.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
eet.target_name = ee_target;
ee_map.insert(std::pair<std::string, ee_trans>(ee_name , eet));
ReferenceForceUpdaterParam rfu_param;
//set rfu param
rfu_param.update_count = round((1/rfu_param.update_freq)/m_dt);
if (( ee_name != "rleg" ) && ( ee_name != "lleg" ))
m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name , rfu_param));
ee_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
ref_force.push_back(hrp::Vector3::Zero());
//ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt)));
ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt, interpolator::LINEAR)));
if (( ee_name != "lleg" ) && ( ee_name != "rleg" )) transition_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(1, m_dt)));
std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl;
std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << std::endl;
std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl;
}
}
// check if the dof of m_robot match the number of joint in m_robot
unsigned int dof = m_robot->numJoints();
for ( unsigned int i = 0 ; i < dof; i++ ){
if ( (int)i != m_robot->joint(i)->jointId ) {
std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl;
return RTC::RTC_ERROR;
}
}
loop = 0;
transition_interpolator_ratio.reserve(transition_interpolator.size());
for (unsigned int i=0; i<transition_interpolator.size(); i++ ) transition_interpolator_ratio[i] = 0;
return RTC::RTC_OK;
}
RTC::ReturnCode_t RemoveForceSensorLinkOffset::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("rpy", m_rpyIn);
// Set OutPort buffer
// Set service provider to Ports
m_RemoveForceSensorLinkOffsetServicePort.registerProvider("service0", "RemoveForceSensorLinkOffsetService", m_service0);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_RemoveForceSensorLinkOffsetServicePort);
// </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;
}
int nforce = m_robot->numSensors(hrp::Sensor::FORCE);
m_force.resize(nforce);
m_forceOut.resize(nforce);
m_forceIn.resize(nforce);
for (size_t i = 0; i < nforce; i++) {
hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i);
m_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string("off_"+s->name).c_str(), m_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]);
registerOutPort(std::string("off_"+s->name).c_str(), *m_forceOut[i]);
m_forcemoment_offset_param.insert(std::pair<std::string, ForceMomentOffsetParam>(s->name, ForceMomentOffsetParam()));
}
return RTC::RTC_OK;
}
