RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onInitialize()
{
m_SequencePlayerServicePort.registerConsumer("service0", "SequencePlayerService", m_service0);
addPort(m_SequencePlayerServicePort);
RTC::Properties& prop = getProperties();
body = 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());
std::string modelfile = m_pManager->getConfig()["model"];
if (!loadBodyFromModelLoader(body, modelfile.c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext())))
{
std::cerr << "[HrpsysJointTrajectoryBridge] failed to load model[" << prop["model"] << "]" << std::endl;
}
bool ret = false;
while (!ret)
{
try
{
bodyinfo = hrp::loadBodyInfo(modelfile.c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()));
ret = loadBodyFromBodyInfo(body, bodyinfo);
}
catch (CORBA::SystemException& ex)
{
std::cerr << "[HrpsysJointTrajectoryBridge] loadBodyInfo(): CORBA::SystemException " << ex._name() << std::endl;
sleep(1);
}
catch (...)
{
std::cerr << "[HrpsysJointTrajectoryBridge] loadBodyInfo(): failed to load model[" << modelfile << "]"
<< std::endl;
;
sleep(1);
}
}
if (body == NULL)
{
return RTC::RTC_ERROR;
}
body->calcForwardKinematics();
ROS_INFO_STREAM("[HrpsysJointTrajectoryBridge] @Initilize name : " << getInstanceName() << " done");
return RTC::RTC_OK;
}
RTC::ReturnCode_t CollisionDetector::onActivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << 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());
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
if ( prop["collision_viewer"] == "true" ) {
m_use_viewer = true;
}
m_glbody = new GLbody();
m_robot = hrp::BodyPtr(m_glbody);
//
OpenHRP::BodyInfo_var binfo;
binfo = hrp::loadBodyInfo(prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()));
if (CORBA::is_nil(binfo)){
std::cerr << "failed to load model[" << prop["model"] << "]"
<< std::endl;
return RTC::RTC_ERROR;
}
if (!loadBodyFromBodyInfo(m_robot, binfo, true, GLlinkFactory)) {
std::cerr << "failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}
loadShapeFromBodyInfo(m_glbody, binfo);
convertToConvexHull(m_robot);
//convertToAABB(m_robot);
setupVClipModel(m_robot);
if ( prop["collision_pair"] != "" ) {
std::cerr << "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);
std::cerr << "check collisions between " << m_robot->link(name1)->name << " and " << m_robot->link(name2)->name << std::endl;
m_pair[tmp] = 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 ( m_pair.size() == 0 ) {
std::cerr << "failed to setup collisions" << std::endl;
return RTC::RTC_ERROR;
}
if ( m_use_viewer ) {
m_scene.addBody(m_robot);
}
// allocate memory for outPorts
m_q.data.length(m_robot->numJoints());
m_recover_time = 0;
m_safe_posture = true;
i_dt = 1.0;
default_recover_time = 2.5/m_dt;
m_recover_jointdata = (double *)malloc(sizeof(double)*m_robot->numJoints());
m_interpolator = new interpolator(m_robot->numJoints(), i_dt);
return RTC::RTC_OK;
}
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;
}