bush_customizer.cpp
Go to the documentation of this file.
1 #include <cmath>
2 #include <cstring>
3 #include <boost/function.hpp>
4 #include <boost/bind.hpp>
5 #include <fstream>
6 #include <coil/stringutil.h>
7 
8 //#define CNOID_BODY_CUSTOMIZER
9 #ifdef CNOID_BODY_CUSTOMIZER
10 #include <cnoid/BodyCustomizerInterface>
11 #else
13 #endif
14 
15 #include <iostream>
16 
17 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
18 #define DLL_EXPORT __declspec(dllexport)
19 #else
20 #define DLL_EXPORT
21 #endif /* Windows */
22 
23 #if defined(HRPMODEL_VERSION_MAJOR) && defined(HRPMODEL_VERSION_MINOR)
24 #if HRPMODEL_VERSION_MAJOR >= 3 && HRPMODEL_VERSION_MINOR >= 1
25 #include <hrpUtil/EigenTypes.h>
26 #define NS_HRPMODEL hrp
27 #endif
28 #endif
29 
30 #ifdef CNOID_BODY_CUSTOMIZER
31 #define NS_HRPMODEL cnoid
32 cnoid::Matrix3 trans(const cnoid::Matrix3& M) { return M.transpose(); }
33 double dot(const cnoid::Vector3& a, const cnoid::Vector3& b) { return a.dot(b); }
34 typedef cnoid::Matrix3 Matrix33;
35 #endif
36 
37 
38 #ifndef NS_HRPMODEL
39 #define NS_HRPMODEL OpenHRP
40 typedef OpenHRP::vector3 Vector3;
41 typedef OpenHRP::matrix33 Matrix33;
42 #endif
43 
44 using namespace std;
45 using namespace boost;
46 using namespace NS_HRPMODEL;
47 
48 static BodyInterface* bodyInterface = 0;
49 
50 static BodyCustomizerInterface bodyCustomizerInterface;
51 
53 {
54  double* valuePtr;
55  double* velocityPtr;
56  double* torqueForcePtr;
57 };
58 
60 {
62  std::string name;
63  // Spring coefficient for bush. For slide joint, [N/m], for rotate joint, [Nm/rad]
64  double spring;
65  // Damping coefficient for bush. For slide joint, [N/(m/s)], for rotate joint, [Nm/(rad/s)]
66  double damping;
67  int index;
68 };
69 
71 {
74  std::vector<BushCustomizerParam> params;
75 };
76 
77 // Parameters should be specified by configure file using BUSH_CUSTOMIZER_CONFIG_PATH environment.
78 
79 // Robot model name using bush
80 static std::string robot_model_name;
81 
82 // Bush configuration parameters such as:
83 // bush_config: joint1,spring1,damping1 joint2,spring2,damping2 joint3,spring3,damping3 ...
84 // joint* should be included in VRML file.
85 static coil::vstring bush_config;
86 
87 static const char** getTargetModelNames()
88 {
89  static const char* names[] = {
90  robot_model_name.c_str(),
91  0 };
92  return names;
93 }
94 
95 static void getVirtualbushJoints(BushCustomizer* customizer, BodyHandle body)
96 {
97  std::cerr << "[Bush customizer] Bush params" << std::endl;
98  customizer->hasVirtualBushJoints = true;
99  for (size_t i = 0; i < bush_config.size(); i++) {
100  // tmp_config <= temp bush config, e.g., "joint*,spring*,damping*"
101  coil::vstring tmp_config = coil::split(bush_config[i], ",");
102  // Check size
103  if ( tmp_config.size() != 3 ) {
104  std::cerr << "[Bush customizer] Parameter size mismatch (" << i << ") (" << tmp_config.size() << ")" << std::endl;
105  return;
106  }
107  int bushIndex = bodyInterface->getLinkIndexFromName(body, tmp_config[0].c_str());
108  if(bushIndex < 0){
109  std::cerr << "[Bush customizer] No such joint name (" << tmp_config[0] << ")" << std::endl;
110  customizer->hasVirtualBushJoints = false;
111  } else {
113  p.index = bushIndex;
114  p.name = tmp_config[0];
115  p.spring = atof(tmp_config[1].c_str());
116  p.damping= atof(tmp_config[2].c_str());
117  p.jointValSets.valuePtr = bodyInterface->getJointValuePtr(body, bushIndex);
118  p.jointValSets.velocityPtr = bodyInterface->getJointVelocityPtr(body, bushIndex);
119  p.jointValSets.torqueForcePtr = bodyInterface->getJointForcePtr(body, bushIndex);
120  customizer->params.push_back(p);
121  std::cerr << "[Bush customizer] name = " << p.name << ", index = " << p.index << ", spring = " << p.spring << ", damping = " << p.damping << std::endl;
122  }
123  }
124 }
125 
126 static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName)
127 {
128  BushCustomizer* customizer = 0;
129 
130  std::cerr << "[Bush customizer] Create " << std::string(modelName) << std::endl;
131  customizer = new BushCustomizer;
132 
133  customizer->bodyHandle = bodyHandle;
134  //customizer->hasVirtualBushJoints = false;
135  // customizer->springT = 5.0e5; // N/m
136  // customizer->dampingT = 1.0e3; // N/(m/s)
137  // customizer->springR = 1e3; // Nm / rad
138  // customizer->dampingR = 2.5e1; // Nm / (rad/s)
139 
140  getVirtualbushJoints(customizer, bodyHandle);
141 
142  return static_cast<BodyCustomizerHandle>(customizer);
143 }
144 
145 
146 static void destroy(BodyCustomizerHandle customizerHandle)
147 {
148  BushCustomizer* customizer = static_cast<BushCustomizer*>(customizerHandle);
149  if(customizer){
150  delete customizer;
151  }
152 }
153 
154 static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
155 {
156  BushCustomizer* customizer = static_cast<BushCustomizer*>(customizerHandle);
157 
158  if(customizer->hasVirtualBushJoints){
159  for(int i=0; i < customizer->params.size(); ++i){
160  BushCustomizerParam& param = customizer->params[i];
161  *(param.jointValSets.torqueForcePtr) = - param.spring * (*param.jointValSets.valuePtr) - param.damping * (*param.jointValSets.velocityPtr);
162  //std::cerr << "Bush " << i << " " << 0 << " " << *(param.jointValSets.torqueForcePtr) << " = " << *(param.jointValSets.valuePtr) << " + " << *(param.jointValSets.velocityPtr) << std::endl;
163  }
164  }
165 }
166 
167 extern "C" DLL_EXPORT
168 NS_HRPMODEL::BodyCustomizerInterface* getHrpBodyCustomizerInterface(NS_HRPMODEL::BodyInterface* bodyInterface_)
169 {
170  bodyInterface = bodyInterface_;
171  char* tmpenv = getenv("BUSH_CUSTOMIZER_CONFIG_PATH");
172  if (tmpenv) {
173  std::ifstream ifs(tmpenv);
174  if (ifs.fail() ) {
175  std::cerr << "[Bush customizer] Could not open [" << tmpenv << "]" << std::endl;
176  } else {
177  std::string tmpstr;
178  std::cerr << "[Bush customizer] Open [" << tmpenv << "]" << std::endl;
179  while (std::getline(ifs,tmpstr)) {
180  coil::vstring config_params = coil::split(tmpstr, ":");
181  if (config_params[0] == "robot_model_name") {
182  robot_model_name = config_params[1];
183  std::cerr << "[Bush customizer] robot_model_name = [" << robot_model_name << "]" << std::endl;
184  } else if ( config_params[0] == "bush_config" ) {
185  bush_config = coil::split(config_params[1], " ");
186  std::cerr << "[Bush customizer] bush_config = [" << config_params[1] << "]" << std::endl;
187  }
188  }
189  }
190  }
191 
193  bodyCustomizerInterface.getTargetModelNames = getTargetModelNames;
196  bodyCustomizerInterface.initializeAnalyticIk = NULL;
197  bodyCustomizerInterface.calcAnalyticIk = NULL;
198  bodyCustomizerInterface.setVirtualJointForces = setVirtualJointForces;
199 
200  return &bodyCustomizerInterface;
201 }
JointValSet::velocityPtr
double * velocityPtr
Definition: bush_customizer.cpp:55
BushCustomizerParam::name
std::string name
Definition: bush_customizer.cpp:62
i
png_uint_32 i
Definition: png.h:2732
hrp::BodyHandle
void * BodyHandle
Definition: Body.h:40
JointValSet::torqueForcePtr
double * torqueForcePtr
Definition: bush_customizer.cpp:56
JointValSet::valuePtr
double * valuePtr
Definition: bush_customizer.cpp:54
getHrpBodyCustomizerInterface
DLL_EXPORT NS_HRPMODEL::BodyCustomizerInterface * getHrpBodyCustomizerInterface(NS_HRPMODEL::BodyInterface *bodyInterface_)
Definition: bush_customizer.cpp:168
robot_model_name
static std::string robot_model_name
Definition: bush_customizer.cpp:80
BushCustomizer::hasVirtualBushJoints
bool hasVirtualBushJoints
Definition: bush_customizer.cpp:73
NS_HRPMODEL
#define NS_HRPMODEL
Definition: bush_customizer.cpp:39
boost
Modifications controlling boost library behavior.
Definition: ColladaUtil.h:306
create
static BodyCustomizerHandle create(BodyHandle bodyHandle, const char *modelName)
Definition: bush_customizer.cpp:126
b
long b
Definition: jpegint.h:371
BushCustomizerParam::spring
double spring
Definition: bush_customizer.cpp:64
BushCustomizerParam::jointValSets
JointValSet jointValSets
Definition: bush_customizer.cpp:61
BushCustomizerParam::index
int index
Definition: bush_customizer.cpp:67
destroy
static void destroy(BodyCustomizerHandle customizerHandle)
Definition: bush_customizer.cpp:146
BushCustomizerParam::damping
double damping
Definition: bush_customizer.cpp:66
bodyCustomizerInterface
static BodyCustomizerInterface bodyCustomizerInterface
Definition: bush_customizer.cpp:50
BushCustomizer::params
std::vector< BushCustomizerParam > params
Definition: bush_customizer.cpp:74
setVirtualJointForces
static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
Definition: bush_customizer.cpp:154
DLL_EXPORT
#define DLL_EXPORT
Definition: bush_customizer.cpp:20
BushCustomizer::bodyHandle
BodyHandle bodyHandle
Definition: bush_customizer.cpp:72
trans
png_infop png_bytep * trans
Definition: png.h:2432
BodyCustomizerInterface.h
The definitions of the body customizer interface for increasing binary compatibility.
hrp::dot
double dot(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:19
JointValSet
Definition: bush_customizer.cpp:52
BushCustomizer
Definition: bush_customizer.cpp:70
getTargetModelNames
static const char ** getTargetModelNames()
Definition: bush_customizer.cpp:87
EigenTypes.h
bodyInterface
static BodyInterface * bodyInterface
Definition: bush_customizer.cpp:48
BushCustomizerParam
Definition: bush_customizer.cpp:59
test.a
int a
Definition: test.py:1
Matrix33
OpenHRP::matrix33 Matrix33
Definition: bush_customizer.cpp:41
bush_config
static coil::vstring bush_config
Definition: bush_customizer.cpp:85
hrp::BODY_CUSTOMIZER_INTERFACE_VERSION
static const int BODY_CUSTOMIZER_INTERFACE_VERSION
Definition: BodyCustomizerInterface.h:58
getVirtualbushJoints
static void getVirtualbushJoints(BushCustomizer *customizer, BodyHandle body)
Definition: bush_customizer.cpp:95
hrp::BodyCustomizerHandle
void * BodyCustomizerHandle
Definition: Body.h:42
Vector3
OpenHRP::vector3 Vector3
Definition: bush_customizer.cpp:40


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:02