Body Handler¶

What is a Body Handler ¶

A body handler is a mechanism for implementing processes specific to an individual body model in C++ and incorporating them into the model. You implement the processes as a class inheriting the BodyHandler class and build it as a shared library. Then the shared library is loaded when the model is loaded, according to the description in the model file, and the handler is incorporated into the body model.

Examples of model-specific processes that can be implemented as body handlers include the following.

Analytical (non-iterative) inverse kinematics calculations for the mechanism of a specific robot
Processes that make multiple joints move in conjunction with each other, for closed link mechanisms and the like
Customization of how joint displacements are displayed and entered on the GUI

The body handler is an extension mechanism that works at the level of the Body library, and its feature is that functions can be added on a per-model basis without modifying the code of Choreonoid itself or its plugins. The incorporated processes are commonly reflected in the various processes that handle the model, such as model operations on the GUI and simulations.

Specification in Model Files ¶

A body handler is specified with the body_handlers key in the file of a body model.

body_handlers: MyRobotHandler

For the value, write the file name of the shared library of the handler, omitting the extension (.so, .dll). It is also possible to specify multiple handlers by writing them as a list.

The library file is searched for in the “bodyhandler” directory under the plugin directory of Choreonoid

[Choreonoid installation destination]/lib/choreonoid-x.y/bodyhandler

(x.y is the version number of Choreonoid). It is also possible to specify an absolute path.

When the handler is successfully loaded, messages such as the following are output to the message view when the model is loaded.

Body handler HRP4CHandler was found at "/usr/local/lib/choreonoid-2.5/bodyhandler/HRP4CHandler.so".
Body handler HRP4CHandler has been loaded to HRP-4C.

Standard Handler Interfaces ¶

The functions of a body handler are provided by implementing the virtual functions of interface classes that inherit BodyHandler. The Body library provides the following interfaces as standard, which are used by the corresponding processes of Choreonoid.

CustomJointPathHandler¶

This is an interface for providing a custom JointPath object for the joint path between specific links. It is mainly used to incorporate analytical inverse kinematics calculations specialized for the mechanism of the target model. The incorporated inverse kinematics is reflected in posture operations by inverse kinematics on the GUI and in inverse kinematics calculations using JointPath from programs.

For implementing a custom JointPath class, you can use the helper class CustomJointPathBase. With this class as the base, you can construct a custom JointPath just by registering the calculation function of the inverse kinematics. For an implementation example, the sample HRP4CHandler described below is a good reference.

In addition, if the custom JointPath class also implements the interface called JointSpaceConfigurationHandler, it becomes possible to handle the sets of joint displacement solutions for the same end position and posture (the “configurations” such as the direction of the elbow of an arm or the flip of a wrist), and the configurations can be checked and switched on the GUI (such as the Link Position view).

LinkedJointHandler¶

This is an interface for making other joints move in conjunction with the displacement of a joint. It is used to approximately realize the constraints between joints for models with closed link mechanisms or with multiple joints that mechanically move in conjunction with each other. The linked movements are reflected in the joint operations on the joint displacement view and the scene view, the behavior in the kinematic simulator, and so on.

JointDisplacementPresentationHandler¶

This is an interface for defining the conversion between the internal displacement value of a joint (the joint angle in radians for a revolute joint) and the value displayed and entered on the GUI. For example, the displacement of an axis that is mechanically a revolute joint can be displayed and entered as the distance between two points on the mechanism on the GUI. The conversion is reflected in the GUIs that handle joint displacements, such as the joint displacement view.

Note that BodyHandler is designed to be inherited virtually, so a single handler class can implement multiple interfaces above at the same time.

Implementing a Body Handler ¶

A body handler is implemented as a class inheriting the above interface classes (or BodyHandler directly). In doing so, override the following functions.

initialize(Body* body, std::ostream& os)

This is the initialization process called when the handler is incorporated into a model. It obtains and verifies the target links and so on. If it returns false, the handler is not incorporated. The text output to os is displayed on the message view.

clone()

This returns a copy of the handler object. When a body model is duplicated internally, such as when a simulation is executed, the handler is also duplicated by this function and taken over.

Also, write the following macro in the source file to define the factory function for creating the handler from the shared library.

CNOID_IMPLEMENT_BODY_HANDLER_FACTORY(HandlerClassName)

This factory function also checks the consistency between the version of Choreonoid with which the handler was built and that of the Choreonoid loading it.

The following is the implementation of the body handler sample/general/ClosedLinkSampleHandler.cpp included in the Choreonoid samples. This uses LinkedJointHandler to make three joints of the sample model of a closed link mechanism (share/model/misc/ClosedLinkSample.body) move in conjunction with each other.

#include <cnoid/LinkedJointHandler>
#include <cnoid/Body>
#include <fmt/format.h>

using namespace std;
using namespace cnoid;
using fmt::format;

class ClosedLinkSampleHandler : public LinkedJointHandler
{
public:
    virtual BodyHandler* clone() override;
    virtual bool initialize(Body* body, std::ostream& os) override;
    virtual bool updateLinkedJointDisplacements(Link* masterJoint, double masterJointDisplacement) override;

private:
    Link* joints[3];
};

CNOID_IMPLEMENT_BODY_HANDLER_FACTORY(ClosedLinkSampleHandler)


BodyHandler* ClosedLinkSampleHandler::clone()
{
    return new ClosedLinkSampleHandler(*this);
}


bool ClosedLinkSampleHandler::initialize(Body* body, std::ostream& os)
{
    const char* ids[3] = { "0", "1", "3" };
    for(int i=0; i < 3; ++i){
        string name(format("J{0}", ids[i]));
        joints[i] = body->link(name);
        if(!joints[i]){
            os << name << "is not found." << endl;
            return false;
        }
    }
    return true;
}


bool ClosedLinkSampleHandler::updateLinkedJointDisplacements(Link* masterJoint, double masterJointDisplacement)
{
    if(masterJoint){
        masterJoint->q() = masterJointDisplacement;
    }
    if(!masterJoint || masterJoint == joints[0]){
        joints[1]->q() = -joints[0]->q();
        joints[2]->q() = joints[0]->q();
    } else if(masterJoint == joints[1]){
        joints[0]->q() = -joints[1]->q();
        joints[2]->q() = joints[0]->q();
    } else if(masterJoint == joints[2]){
        joints[0]->q() = joints[2]->q();
        joints[1]->q() = -joints[2]->q();
    }
    return true;
}

In this way, the target joints are obtained in initialize, and the displacements of the other joints are updated according to the displacement of the reference joint (masterJoint) in updateLinkedJointDisplacements. With this process, when one of the joints is operated on the GUI, the other joints also move in conjunction with it.

How to Build ¶

The CMake function choreonoid_add_body_handler provided by Choreonoid can be used to build a body handler.

choreonoid_add_body_handler(MyRobotHandler MyRobotHandler.cpp)

With this function, the handler is built as a shared library and is output to and installed in the “bodyhandler” directory described above. This function can be used in builds within the Choreonoid source tree (such as the ext directory), as well as in external CMake projects that import Choreonoid with find_package(Choreonoid).

Note that because of the version check described above, the handler must be built with the source and environment of the same version as the Choreonoid used to load it.

Samples ¶

The Choreonoid source contains the following samples as implementation examples of body handlers.

HRP4CHandler¶

sample/HRP4C/HRP4CHandler.cpp is a body handler that incorporates analytical inverse kinematics of the legs into the model of the humanoid robot HRP-4C (share/model/HRP4C/HRP4C.body). It is an implementation example using CustomJointPathHandler and CustomJointPathBase, and is a good starting point when incorporating analytical inverse kinematics with your own handler. In HRP4C.body, this handler is specified with the body_handlers key.

This sample corresponds to the CMake option BUILD_HRP4C_HANDLER, and building with this option turned on (it is OFF by default) generates the shared library of the handler in the bodyhandler directory. When the HRP-4C model is loaded in that state, the handler is loaded and the analytical solutions are used in the inverse kinematics calculations for the joint paths of the legs. The sample/HRP4C directory also contains sample projects using this model.

ClosedLinkSampleHandler¶

sample/general/ClosedLinkSampleHandler.cpp, introduced in Implementing a Body Handler, is an example of LinkedJointHandler that makes the joints of the sample model of a closed link mechanism (share/model/misc/ClosedLinkSample.body) move in conjunction with each other.

This one is built as standard when the CMake option ENABLE_SAMPLES is ON (the default). By loading the ClosedLinkSample.body model and operating one of the joints on the GUI, you can see that the joints move in conjunction with each other.

Body Handler¶

What is a Body Handler¶

Specification in Model Files¶

Standard Handler Interfaces¶