RTC::ReturnCode_t ForwardKinematics::onInitialize() { std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl; // <rtc-template block="bind_config"> // Bind variables and configuration variable coil::Properties& ref = getProperties(); bindParameter("sensorAttachedLink", m_sensorAttachedLinkName, ref["conf.default.sensorAttachedLink"].c_str()); // </rtc-template> // Registration: InPort/OutPort/Service // <rtc-template block="registration"> // Set InPort buffers addInPort("q", m_qIn); addInPort("sensorRpy", m_sensorRpyIn); addInPort("qRef", m_qRefIn); addInPort("basePosRef", m_basePosRefIn); addInPort("baseRpyRef", m_baseRpyRefIn); // Set OutPort buffer // Set service provider to Ports m_ForwardKinematicsServicePort.registerProvider("service0", "ForwardKinematicsService", m_service0); addPort(m_ForwardKinematicsServicePort); m_service0.setComp(this); // Set service consumers to Ports // Set CORBA Service Ports // </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()); m_refBody = hrp::BodyPtr(new hrp::Body()); if (!loadBodyFromModelLoader(m_refBody, 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; } m_actBody = hrp::BodyPtr(new hrp::Body()); if (!loadBodyFromModelLoader(m_actBody, 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; } m_refLink = m_refBody->rootLink(); m_actLink = m_actBody->rootLink(); return RTC::RTC_OK; }
int initializeOpenHRPModel (char* _filename) { int rtmargc=0; char** argv=NULL; //std::vector<char *> rtmargv; //rtmargv.push_back(argv[0]); rtmargc++; RTC::Manager* manager; //manager = RTC::Manager::init(rtmargc, rtmargv.data()); manager = RTC::Manager::init(rtmargc, argv); std::string nameServer = manager->getConfig()["corba.nameservers"]; int comPos = nameServer.find(","); if (comPos < 0){ comPos = nameServer.length(); } nameServer = nameServer.substr(0, comPos); RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str()); std::string filename(_filename); if (!loadBodyFromModelLoader(m_robot, filename.c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()), true)){ std::cerr << print_prefix << " Failed to load model[" << filename << "]" << std::endl; return 1; } else { std::cerr << print_prefix << " Success to load model[" << filename << "]" << std::endl; } return 0; };
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 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; }
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 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; }
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; }
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; }
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; }
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; }
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; }