JointPathExC.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/Manager.h>
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpModel/Body.h>
00013 #include <hrpModel/Link.h>
00014 #include <hrpModel/Sensor.h>
00015 #include <hrpModel/ModelLoaderUtil.h>
00016 #include "JointPathEx.h"
00017 
00018 
00019 extern "C" {
00020     hrp::JointPathExPtr jpe;
00021     hrp::BodyPtr m_robot(new hrp::Body());
00022     static std::string print_prefix("[jpe]");
00023 
00024     int initializeOpenHRPModel (char* _filename)
00025     {
00026         int rtmargc=0;
00027         char** argv=NULL;
00028         //std::vector<char *> rtmargv;
00029         //rtmargv.push_back(argv[0]);
00030         rtmargc++;
00031 
00032         RTC::Manager* manager;
00033         //manager = RTC::Manager::init(rtmargc, rtmargv.data());
00034         manager = RTC::Manager::init(rtmargc, argv);
00035 
00036         std::string nameServer = manager->getConfig()["corba.nameservers"];
00037         int comPos = nameServer.find(",");
00038         if (comPos < 0){
00039             comPos = nameServer.length();
00040         }
00041         nameServer = nameServer.substr(0, comPos);
00042         RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
00043         std::string filename(_filename);
00044         if (!loadBodyFromModelLoader(m_robot, filename.c_str(),
00045                                      CosNaming::NamingContext::_duplicate(naming.getRootContext()),
00046                                      true)){
00047             std::cerr << print_prefix << " Failed to load model[" << filename << "]" << std::endl;
00048             return 1;
00049         } else {
00050             std::cerr << print_prefix << " Success to load model[" << filename << "]" << std::endl;
00051         }
00052         return 0;
00053     };
00054 
00055     int initializeJointPathExInstance (char* root_link_name, char* target_link_name)
00056     {
00057         jpe = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(root_link_name), m_robot->link(target_link_name), 0.002, false, std::string("test")));
00058         if ( !jpe ) {
00059             std::cerr << print_prefix << " Fail to joint path from " << root_link_name << " to " << target_link_name << std::endl;
00060         } else {
00061             std::cerr << print_prefix << " Success to joint path from " << root_link_name << " to " << target_link_name << " (dof = " << jpe->numJoints() << std::endl;
00062         }
00063         return 0;
00064     }
00065 
00066     int _setJointAngles (double* ja)
00067     {
00068         for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
00069             m_robot->joint(i)->q = ja[i];
00070         }
00071         m_robot->calcForwardKinematics();
00072         return 0;
00073     }
00074 
00075     int _getJointAngles (double* ja)
00076     {
00077         for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
00078             ja[i] = m_robot->joint(i)->q;
00079         }
00080     }
00081 
00082     int _calcInverseKinematics2Loop (double* _vel_p, double* _vel_r)
00083     {
00084         hrp::dvector qrefv = hrp::dvector::Zero(jpe->numJoints());
00085         hrp::Vector3 vel_p, vel_r;
00086         for (size_t i = 0; i < 3; i++) {
00087             vel_p[i] = _vel_p[i];
00088             vel_r[i] = _vel_r[i];
00089         }
00090         jpe->calcInverseKinematics2Loop(vel_p, vel_r, 1.0, 0.002, 0.0, &qrefv);
00091         return 0;
00092     }
00093 };


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55