bush_customizer.cpp
Go to the documentation of this file.
00001 #include <cmath>
00002 #include <cstring>
00003 #include <boost/function.hpp>
00004 #include <boost/bind.hpp>
00005 #include <fstream>
00006 #include <coil/stringutil.h>
00007 
00008 //#define CNOID_BODY_CUSTOMIZER
00009 #ifdef CNOID_BODY_CUSTOMIZER
00010 #include <cnoid/BodyCustomizerInterface>
00011 #else
00012 #include <hrpModel/BodyCustomizerInterface.h>
00013 #endif
00014 
00015 #include <iostream>
00016 
00017 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
00018 #define DLL_EXPORT __declspec(dllexport)
00019 #else 
00020 #define DLL_EXPORT 
00021 #endif /* Windows */
00022 
00023 #if defined(HRPMODEL_VERSION_MAJOR) && defined(HRPMODEL_VERSION_MINOR)
00024 #if HRPMODEL_VERSION_MAJOR >= 3 && HRPMODEL_VERSION_MINOR >= 1
00025 #include <hrpUtil/EigenTypes.h>
00026 #define NS_HRPMODEL hrp
00027 #endif
00028 #endif
00029 
00030 #ifdef CNOID_BODY_CUSTOMIZER
00031 #define NS_HRPMODEL cnoid
00032 cnoid::Matrix3 trans(const cnoid::Matrix3& M) { return M.transpose(); }
00033 double dot(const cnoid::Vector3& a, const cnoid::Vector3& b) { return a.dot(b); }
00034 typedef cnoid::Matrix3 Matrix33;
00035 #endif
00036 
00037 
00038 #ifndef NS_HRPMODEL
00039 #define NS_HRPMODEL OpenHRP
00040 typedef OpenHRP::vector3 Vector3;
00041 typedef OpenHRP::matrix33 Matrix33;
00042 #endif
00043 
00044 using namespace std;
00045 using namespace boost;
00046 using namespace NS_HRPMODEL;
00047 
00048 static BodyInterface* bodyInterface = 0;
00049 
00050 static BodyCustomizerInterface bodyCustomizerInterface;
00051 
00052 struct JointValSet
00053 {
00054   double* valuePtr;
00055   double* velocityPtr;
00056   double* torqueForcePtr;
00057 };
00058 
00059 struct BushCustomizerParam
00060 {
00061   JointValSet jointValSets;
00062   std::string name;
00063   // Spring coefficient for bush. For slide joint, [N/m], for rotate joint, [Nm/rad]
00064   double spring;
00065   // Damping coefficient for bush. For slide joint, [N/(m/s)], for rotate joint, [Nm/(rad/s)]
00066   double damping;
00067   int index;
00068 };
00069 
00070 struct BushCustomizer
00071 {
00072   BodyHandle bodyHandle;
00073   bool hasVirtualBushJoints;
00074   std::vector<BushCustomizerParam> params;
00075 };
00076 
00077 // Parameters should be specified by configure file using BUSH_CUSTOMIZER_CONFIG_PATH environment.
00078 
00079 // Robot model name using bush
00080 static std::string robot_model_name;
00081 
00082 // Bush configuration parameters such as:
00083 //   bush_config: joint1,spring1,damping1 joint2,spring2,damping2 joint3,spring3,damping3 ...
00084 //   joint* should be included in VRML file.
00085 static coil::vstring bush_config;
00086 
00087 static const char** getTargetModelNames()
00088 {
00089   static const char* names[] = {
00090     robot_model_name.c_str(),
00091     0 };
00092   return names;
00093 }
00094 
00095 static void getVirtualbushJoints(BushCustomizer* customizer, BodyHandle body)
00096 {
00097   std::cerr << "[Bush customizer] Bush params" << std::endl;
00098   customizer->hasVirtualBushJoints = true;
00099   for (size_t i = 0; i < bush_config.size(); i++) {
00100     // tmp_config <= temp bush config, e.g., "joint*,spring*,damping*"
00101     coil::vstring tmp_config = coil::split(bush_config[i], ",");
00102     // Check size
00103     if ( tmp_config.size() != 3 ) {
00104       std::cerr << "[Bush customizer]   Parameter size mismatch (" << i << ") (" << tmp_config.size() << ")" << std::endl;
00105       return;
00106     }
00107     int bushIndex = bodyInterface->getLinkIndexFromName(body, tmp_config[0].c_str());
00108     if(bushIndex < 0){
00109       std::cerr << "[Bush customizer]   No such joint name (" << tmp_config[0] << ")" << std::endl;
00110       customizer->hasVirtualBushJoints = false;
00111     } else {
00112       BushCustomizerParam p;
00113       p.index = bushIndex;
00114       p.name = tmp_config[0];
00115       p.spring = atof(tmp_config[1].c_str());
00116       p.damping= atof(tmp_config[2].c_str());
00117       p.jointValSets.valuePtr = bodyInterface->getJointValuePtr(body, bushIndex);
00118       p.jointValSets.velocityPtr = bodyInterface->getJointVelocityPtr(body, bushIndex);
00119       p.jointValSets.torqueForcePtr = bodyInterface->getJointForcePtr(body, bushIndex);
00120       customizer->params.push_back(p);
00121       std::cerr << "[Bush customizer]   name = " << p.name << ", index = " << p.index << ", spring = " << p.spring << ", damping = " << p.damping << std::endl;
00122     }
00123   }
00124 }
00125 
00126 static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName)
00127 {
00128   BushCustomizer* customizer = 0;
00129 
00130   std::cerr << "[Bush customizer] Create " << std::string(modelName) << std::endl;
00131   customizer = new BushCustomizer;
00132 
00133   customizer->bodyHandle = bodyHandle;
00134   //customizer->hasVirtualBushJoints = false;
00135   // customizer->springT  = 5.0e5; // N/m
00136   // customizer->dampingT = 1.0e3; // N/(m/s)
00137   // customizer->springR  = 1e3; // Nm / rad
00138   // customizer->dampingR = 2.5e1;   // Nm / (rad/s)
00139 
00140   getVirtualbushJoints(customizer, bodyHandle);
00141 
00142   return static_cast<BodyCustomizerHandle>(customizer);
00143 }
00144 
00145 
00146 static void destroy(BodyCustomizerHandle customizerHandle)
00147 {
00148   BushCustomizer* customizer = static_cast<BushCustomizer*>(customizerHandle);
00149   if(customizer){
00150     delete customizer;
00151   }
00152 }
00153 
00154 static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
00155 {
00156   BushCustomizer* customizer = static_cast<BushCustomizer*>(customizerHandle);
00157 
00158   if(customizer->hasVirtualBushJoints){
00159     for(int i=0; i < customizer->params.size(); ++i){
00160       BushCustomizerParam& param = customizer->params[i];
00161       *(param.jointValSets.torqueForcePtr) = - param.spring * (*param.jointValSets.valuePtr) - param.damping * (*param.jointValSets.velocityPtr);
00162       //std::cerr << "Bush " << i << " " << 0 << " " << *(param.jointValSets.torqueForcePtr) << " = " << *(param.jointValSets.valuePtr) << " + " << *(param.jointValSets.velocityPtr) << std::endl;
00163     }
00164   }
00165 }
00166 
00167 extern "C" DLL_EXPORT
00168 NS_HRPMODEL::BodyCustomizerInterface* getHrpBodyCustomizerInterface(NS_HRPMODEL::BodyInterface* bodyInterface_)
00169 {
00170   bodyInterface = bodyInterface_;
00171   char* tmpenv = getenv("BUSH_CUSTOMIZER_CONFIG_PATH");
00172   if (tmpenv) {
00173     std::ifstream ifs(tmpenv);
00174     if (ifs.fail() ) {
00175       std::cerr << "[Bush customizer] Could not open [" << tmpenv << "]" << std::endl;
00176     } else {
00177       std::string tmpstr;
00178       std::cerr << "[Bush customizer] Open [" << tmpenv << "]" << std::endl;
00179       while (std::getline(ifs,tmpstr)) {
00180         coil::vstring config_params = coil::split(tmpstr, ":");
00181         if (config_params[0] == "robot_model_name") {
00182           robot_model_name = config_params[1];
00183           std::cerr << "[Bush customizer]   robot_model_name = [" << robot_model_name << "]" << std::endl;
00184         } else if ( config_params[0] == "bush_config" ) {
00185           bush_config = coil::split(config_params[1], " ");
00186           std::cerr << "[Bush customizer]   bush_config = [" << config_params[1] << "]" << std::endl;
00187         }
00188       }
00189     }
00190   }
00191 
00192   bodyCustomizerInterface.version = NS_HRPMODEL::BODY_CUSTOMIZER_INTERFACE_VERSION;
00193   bodyCustomizerInterface.getTargetModelNames = getTargetModelNames;
00194   bodyCustomizerInterface.create = create;
00195   bodyCustomizerInterface.destroy = destroy;
00196   bodyCustomizerInterface.initializeAnalyticIk = NULL;
00197   bodyCustomizerInterface.calcAnalyticIk = NULL;
00198   bodyCustomizerInterface.setVirtualJointForces = setVirtualJointForces;
00199 
00200   return &bodyCustomizerInterface;
00201 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:15