customizer.cpp
Go to the documentation of this file.
2 
3 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
4 #define DLL_EXPORT __declspec(dllexport)
5 #else
6 #define DLL_EXPORT
7 #endif /* Windows */
8 
9 #if defined(HRPMODEL_VERSION_MAJOR) && defined(HRPMODEL_VERSION_MINOR)
10 #if HRPMODEL_VERSION_MAJOR >= 3 && HRPMODEL_VERSION_MINOR >= 1
11 #define NS_HRPMODEL hrp
12 #endif
13 #endif
14 
15 #ifndef NS_HRPMODEL
16 #define NS_HRPMODEL OpenHRP
17 #endif
18 
19 using namespace std;
20 using namespace NS_HRPMODEL;
21 
22 static BodyInterface* bodyInterface = 0;
23 
24 static BodyCustomizerInterface bodyCustomizerInterface;
25 
26 struct JointValSet
27 {
28  double* valuePtr;
29  double* velocityPtr;
30  double* torqueForcePtr;
31 };
32 
33 struct Customizer
34 {
37 
38  double springT;
39  double dampingT;
40 };
41 
42 static const char** getTargetModelNames()
43 {
44  static const char* names[] = {
45  "springJoint",
46  0 };
47 
48  return names;
49 }
50 
51 static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName)
52 {
53  Customizer* customizer = 0;
54 
55  string name(modelName);
56  if(name == "springJoint"){
57  customizer = new Customizer;
58  customizer->bodyHandle = bodyHandle;
59  customizer->springT = 1.0e3;
60  customizer->dampingT = 1.0e1;
61  int jointIndex = bodyInterface->getLinkIndexFromName(bodyHandle, "SPRING_JOINT");
62  if(jointIndex >=0 ){
63  JointValSet& jointValSet = customizer->jointValSet;
64  jointValSet.valuePtr = bodyInterface->getJointValuePtr(bodyHandle, jointIndex);
65  jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(bodyHandle, jointIndex);
66  jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(bodyHandle, jointIndex);
67  }
68  }
69 
70  return static_cast<BodyCustomizerHandle>(customizer);
71 }
72 
73 
74 static void destroy(BodyCustomizerHandle customizerHandle)
75 {
76  Customizer* customizer = static_cast<Customizer*>(customizerHandle);
77  if(customizer){
78  delete customizer;
79  }
80 }
81 
82 static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
83 {
84  Customizer* customizer = static_cast<Customizer*>(customizerHandle);
85  JointValSet& trans = customizer->jointValSet;
86  *(trans.torqueForcePtr) = - customizer->springT * (*trans.valuePtr) - customizer->dampingT * (*trans.velocityPtr);
87 }
88 
89 
90 extern "C" DLL_EXPORT
91 NS_HRPMODEL::BodyCustomizerInterface* getHrpBodyCustomizerInterface(NS_HRPMODEL::BodyInterface* bodyInterface_)
92 {
93  bodyInterface = bodyInterface_;
94 
96  bodyCustomizerInterface.getTargetModelNames = getTargetModelNames;
99  bodyCustomizerInterface.initializeAnalyticIk = 0;
100  bodyCustomizerInterface.calcAnalyticIk = 0;
101  bodyCustomizerInterface.setVirtualJointForces = setVirtualJointForces;
102 
103  return &bodyCustomizerInterface;
104 }
JointValSet jointValSet
Definition: customizer.cpp:36
static const char ** getTargetModelNames()
Definition: customizer.cpp:42
png_infop png_charpp name
Definition: png.h:2382
double springT
Definition: customizer.cpp:38
static const int BODY_CUSTOMIZER_INTERFACE_VERSION
static void destroy(BodyCustomizerHandle customizerHandle)
Definition: customizer.cpp:74
void * BodyHandle
Definition: Body.h:40
png_infop png_bytep * trans
Definition: png.h:2435
static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
Definition: customizer.cpp:82
static BodyCustomizerInterface bodyCustomizerInterface
Definition: customizer.cpp:24
static BodyCustomizerHandle create(BodyHandle bodyHandle, const char *modelName)
Definition: customizer.cpp:51
void * BodyCustomizerHandle
Definition: Body.h:42
#define DLL_EXPORT
Definition: customizer.cpp:6
double * valuePtr
double * velocityPtr
BodyHandle bodyHandle
Definition: customizer.cpp:35
The definitions of the body customizer interface for increasing binary compatibility.
double * torqueForcePtr
static BodyInterface * bodyInterface
Definition: customizer.cpp:22
double dampingT
Definition: customizer.cpp:39
DLL_EXPORT NS_HRPMODEL::BodyCustomizerInterface * getHrpBodyCustomizerInterface(NS_HRPMODEL::BodyInterface *bodyInterface_)
Definition: customizer.cpp:91


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