customizer.cpp
Go to the documentation of this file.
00001 #include <hrpModel/BodyCustomizerInterface.h>
00002 
00003 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
00004 #define DLL_EXPORT __declspec(dllexport)
00005 #else 
00006 #define DLL_EXPORT 
00007 #endif /* Windows */
00008 
00009 #if defined(HRPMODEL_VERSION_MAJOR) && defined(HRPMODEL_VERSION_MINOR)
00010 #if HRPMODEL_VERSION_MAJOR >= 3 && HRPMODEL_VERSION_MINOR >= 1
00011 #define NS_HRPMODEL hrp
00012 #endif
00013 #endif
00014 
00015 #ifndef NS_HRPMODEL
00016 #define NS_HRPMODEL OpenHRP
00017 #endif
00018 
00019 using namespace std;
00020 using namespace NS_HRPMODEL;
00021 
00022 static BodyInterface* bodyInterface = 0;
00023 
00024 static BodyCustomizerInterface bodyCustomizerInterface;
00025 
00026 struct JointValSet
00027 {
00028     double* valuePtr;
00029     double* velocityPtr;
00030     double* torqueForcePtr;
00031 };
00032 
00033 struct Customizer
00034 {
00035     BodyHandle bodyHandle;
00036     JointValSet jointValSet;
00037 
00038     double springT;
00039     double dampingT;
00040 };
00041 
00042 static const char** getTargetModelNames()
00043 {
00044     static const char* names[] = { 
00045         "springJoint",
00046         0 };
00047         
00048     return names;
00049 }
00050 
00051 static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName)
00052 {
00053     Customizer* customizer = 0;
00054         
00055     string name(modelName);
00056     if(name == "springJoint"){
00057         customizer = new Customizer;
00058         customizer->bodyHandle = bodyHandle;
00059         customizer->springT = 1.0e3;    
00060         customizer->dampingT = 1.0e1;
00061         int jointIndex = bodyInterface->getLinkIndexFromName(bodyHandle, "SPRING_JOINT");
00062         if(jointIndex >=0 ){
00063             JointValSet& jointValSet = customizer->jointValSet;
00064             jointValSet.valuePtr = bodyInterface->getJointValuePtr(bodyHandle, jointIndex);
00065             jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(bodyHandle, jointIndex);
00066             jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(bodyHandle, jointIndex);
00067         }
00068     }
00069 
00070     return static_cast<BodyCustomizerHandle>(customizer);
00071 }
00072 
00073 
00074 static void destroy(BodyCustomizerHandle customizerHandle)
00075 {
00076     Customizer* customizer = static_cast<Customizer*>(customizerHandle);
00077     if(customizer){
00078         delete customizer;
00079     }
00080 }
00081 
00082 static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
00083 {
00084     Customizer* customizer = static_cast<Customizer*>(customizerHandle);
00085     JointValSet& trans = customizer->jointValSet;
00086     *(trans.torqueForcePtr) = - customizer->springT * (*trans.valuePtr) - customizer->dampingT * (*trans.velocityPtr);
00087 }
00088 
00089 
00090 extern "C" DLL_EXPORT
00091 NS_HRPMODEL::BodyCustomizerInterface* getHrpBodyCustomizerInterface(NS_HRPMODEL::BodyInterface* bodyInterface_)
00092 {
00093     bodyInterface = bodyInterface_;
00094 
00095     bodyCustomizerInterface.version = NS_HRPMODEL::BODY_CUSTOMIZER_INTERFACE_VERSION;
00096     bodyCustomizerInterface.getTargetModelNames = getTargetModelNames;
00097     bodyCustomizerInterface.create = create;
00098     bodyCustomizerInterface.destroy = destroy;
00099     bodyCustomizerInterface.initializeAnalyticIk = 0;
00100     bodyCustomizerInterface.calcAnalyticIk = 0;
00101     bodyCustomizerInterface.setVirtualJointForces = setVirtualJointForces;
00102 
00103     return &bodyCustomizerInterface;
00104 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53