The method of spring damper modeling of a joint

Overview

OpenHRP has plug-in called "BodyCustomizer" as structure which includes the program depending on a model in a dynamics calculation library dynamically.
If you use this plug-in, You can incorporate the spring damper model or analytical inverse kinematics of a joint to a model.
In this section we describe the method of spring damper modeling of a joint using sample project "springJoint.xml".

Creation of a model

With a sample, the model by which the joint of the spring damper model is defined between two boxes (link) is used.
The model file of the sample is (openHRP)/sample/model/springJoint.wrl.
You define a joint as usual similarly and set up the direction of displacement of a spring damper. Since it is a joint which is not controlled, JointID does not need to set up.
The name of SPRING_JOINT is attached to this joint.

DEF springJoint Humanoid {
  version "1.1"
  humanoidBody [
    DEF ROOT Joint {
        :
        :
        DEF SPRING_JOINT Joint {
            translation     0 0 0.05
            jointType "slide"
	        jointAxis 0 0 1
                :
                :

Creation of plug-in

Source code

Next, let's create plug-in for the upper models. The source code of a sample is (OpenHRP)/sample/example/customizer/customizer.cpp.

static const char** getTargetModelNames()
{
    static const char* names[] = { 
        "springJoint",
        0 };
	
    return names;
}

This function tells a dynamics calculation library about to which model plug-in is applied. It returns the arrangement which enumerated the names set up into the model file.
Note that it is not a name of the model loaded by GrxUI.
(In OpenHRP, you can attach a model name different from the name in a file, in order to identify the robot loaded from the same file.
When you want to specify plug-in as each of the same robot, you have to define a model file independently.）
Please put '0' into the end of arrangement.

static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName)
{
    Customizer* customizer = 0;
	
    string name(modelName);
    if(name == "springJoint"){
        customizer = new Customizer;
        customizer->bodyHandle = bodyHandle;
        customizer->springT = 1.0e3;    
        customizer->dampingT = 1.0e1;
        int jointIndex = bodyInterface->getLinkIndexFromName(bodyHandle, "SPRING_JOINT");
        if(jointIndex >=0 ){
            JointValSet& jointValSet = customizer->jointValSet;
            jointValSet.valuePtr = bodyInterface->getJointValuePtr(bodyHandle, jointIndex);
            jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(bodyHandle, jointIndex);
            jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(bodyHandle, jointIndex);
        }
    }

    return static_cast<BodyCustomizerHandle>(customizer);
}

This function is called to the beginning of plug-in. This performs initialization of a variable, etc. Please carry out initialization of a variable, etc.
How to access the variable of a dynamics calculation library from plug-in is also described.
The function offered from a dynamics calculation library to plug-in is as follows.

static void destroy(BodyCustomizerHandle customizerHandle)
{
    Customizer* customizer = static_cast<Customizer*>(customizerHandle);
    if(customizer){
        delete customizer;
    }
}

This function is called when plug-in is destroyed. Post-processing is performed.

extern "C" DLL_EXPORT
NS_HRPMODEL::BodyCustomizerInterface* getHrpBodyCustomizerInterface(NS_HRPMODEL::BodyInterface* bodyInterface_)
{
    bodyInterface = bodyInterface_;

    bodyCustomizerInterface.version = NS_HRPMODEL::BODY_CUSTOMIZER_INTERFACE_VERSION;
    bodyCustomizerInterface.getTargetModelNames = getTargetModelNames;
    bodyCustomizerInterface.create = create;
    bodyCustomizerInterface.destroy = destroy;
    bodyCustomizerInterface.initializeAnalyticIk = 0;
    bodyCustomizerInterface.calcAnalyticIk = 0;
    bodyCustomizerInterface.setVirtualJointForces = setVirtualJointForces;

    return &bodyCustomizerInterface;
}

This function tells a dynamics calculation library about the function which plug-in offers. It returns the structure which described the mounted function name.
Please input '0' into the function which is not mounted.

These four functions certainly need to be mounted in plug-in.

static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
{
    Customizer* customizer = static_cast<Customizer*>(customizerHandle);
    JointValSet& trans = customizer->jointValSet;
    *(trans.torqueForcePtr) = - customizer->springT * (*trans.valuePtr) - customizer->dampingT * (*trans.velocityPtr);
}

This function is called for every step by the integration loop of a dynamics calculation library. Please describe the code of formation of a spring damper in this function.

Installation

Please compile the created source code to create a library. Please be sure to attach a library name like ***Customizer.so(dll).
Please copy it to (OpenHRP3) / share/OpenHRP-3.1-/customizer.
(A sample is installed at the time of installation of OpenHRP.)
A dynamics calculation library loads all plug-in of the name of ***Customizer.so (dll) in this directory.

Execution of a sample