Model Calculation Manual(Ver.3.1)

Introduction

Documents

Supplements

Support

Related Sites

Archived Docs

Overview

Usually, various simulators such as DynamicsSimulator are used as the CORBA server, but also can be used as a library in OpenHRP3. Here, we will explain how to use it.

Loading model

loadBodyFromModelLoader () is used to load model. This receives the pointer to a Body class, model URL and command line arguments, and loads model using ModelLoader server.

 BodyPtr body = new Body();
    if(!loadBodyFromModelLoader(body, url.c_str(), argc, argv)){
        cerr << "ModelLoader: " << url << " cannot be loaded" << endl;
        return 0;
    }

BodyPtr expresses the robot's structure and has the following tree structures.

Body 
  + Link

In Body, robot information and joint list such as model name are contained. There are three ways to retrieve their joints.

How to retrieve by Joint node name

You can retrieve joints that node indicates by passing the node name to the method link of Body class. Below is an example of retrieving right elbow of "sample.wrl":

Link* link = body->link("RARM_ELBOW");

How to retrieve by jointId

You can retrieve joints with jointId by passing jointId to joint method.

Link* link = body->joint(1);

How to retrieve by linkId

While jointId is an ID in model VRML, linkId is an ID by tree search order of model data. Therefore joints which have no jointId in VRML also have linkId.

Link* link = body->link(1);

You can retrieve a joint class Link using one of the ways above. For example, we show a program to display all joint angles.

for (int i=0; i<body->numLinks(); i++) {
  Link* link = body->link(i);
  cout << link->name << " : " << link->q << endl;
}

Online Viewer

Online Viewer is a function which can display models via CORBA in "3DView" pane of GrxUI. When used by model calculation libraries, use OnlineViewerClient class. Here is a concrete way to use.

OnlineViewer_var olv = getOnlineViewer(argc, argv);
try {
    olv->load(body->modelName().c_str(), url.c_str());
    olv->setLogName("move_ankle");
} catch (CORBA::SystemException& ex) {
    cerr << "Failed to connect GrxUI." << endl;
    return 1;
}

At first, pass command line arguments to getOnlineViewer method for acquisition of OnlineViewer. Next specify model name and URL, and load the model. Here assuming the model has been loaded in body by loadBodyFromModelLoader() on ahead, we get the name by Body::modelName. Finally, World State Item is created.

After initialization, update the contents to display. Do this by copying data retrieved through Body-Link structure to WorldState. The next snippet of code presupposes a sample humanoid has been loaded in a library/OnlineViewer.

// create WorldState
WorldState world;
world.characterPositions.length(1);

// CharacterPosition for SampleRobot
world.collisions.length(0);
CharacterPosition& robot = world.characterPositions[0];
robot.characterName = CORBA::string_dup(body->modelName);
world.time = 0;

// copy Link to WorldState
int n = body->numLinks();
robot.linkPositions.length(n);
for (int i=0; i<n; i++) {
  Link* link = body->link(i);
  setVector3(link->p, robot.linkPositions[i].p);
  setMatrix33ToRowMajorArray(link->R, robot.linkPositions[i].R);
}

// Update
olv->update(world);

Forward Kinematics Calculation

Edit the member q of Link class which represents a joint angle, and invokes Body class::calcForwardKinematics(), then the value is calculated. For example, to calculate a posture rotating 90 degrees at right elbow, write code like this:

Link* elbow = body->link("RARM_ELBOW");
elbow->q += M_PI/2;
body->calcForwardKinematics();

Inverse Kinematics Calculation

Steps like this follow:

Set a path
Set target position vector and rotation matrix
Perform inverse kinematics calculation

Invoking Body::getJointPath method sets a path. Pass links of source and destination. The next code shows how to get a path from its waist to its right ankle.

Link* waist = body->link("WAIST");
Link* ankle = body->link("RLEG_ANKLE_R");
JointPathPtr path = body->getJointPath(waist, ankle);

Set up a position vector and a rotation matrix of the destination link, and perform inverse kinematics calculation.

// move the ankle joint up linearly
vector3 p = ankle->p;
p(2) += 0.01;
matrix33 R = identity<matrix33>();
// inverse kinematics calculation
path->calcInverseKinematics(p, mat)

This sets up a set of joints included in the path to make the ankle joint reside target position/posture.

Samples

The sample program of inverse kinematics is in sample/example/move_ankle of OpenHRP3.

Execution

Start GrxUI and check that OnlineViewer etc. are registered into the name service.

Linux

Please move to (OpenHRP3)/sample/example/mode_ankle and perform as follows.

./move_ankle　-url (OpenHRP3)/sample/model/sample.wrl

Windows

Please move to (OpenHRP3)/sample/example/mode_ankle and create a batch file. Please describe the following contents in move_ankle.bat. Please rewrite the option of a name service if needed.

set PATH=(OpenHRP3)\bin;%PATH%
move_ankle -ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService -url /(OpenHRP3)\sample\model\sample.wrl

Run it.

move_ankle.bat

Using dynamics servers

You can perform forward/inverse kinematics calculation, using model calculation libraries we have described. However, when you want to do advanced calculations such as collision check and dynamics calculations, you need to use a dynamics server directly. Here we will explain how to write a program which moves both hands closely by inverse kinematics and judges whether they touch each other by collision check.

Getting servers

See CORBA initialization and getting various servers in Scheduler creation manual to get various servers: NamingService, ModelLoader, OnlineViewer, DynamicsSimulato, and set them up initially.

However, for initial configuration of DynamicsSimulator, please specify the next initial posture and collision check pair.

Initial posture

Specify the next array as its initial posture.

double init_pos[] = {0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00,
		     1.75E-01, -3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00,
		     0.00E+00, 0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02,
		     0.00E+00, 1.75E-01, 3.49E-03, 0, -1.57E+00, 0.00E+00,
		     0.00E+00, 0.00E+00, 0, 0, 0};

Set this array init_pos to its joint angle.

DblSequence q;
q.length(DOF);
for (int i=0; i<DOF; i++) {
  q[i] = init_pos[i];
}
dynamicsSimulator->setCharacterAllLinkData("robot", DynamicsSimulator::JOINT_VALUE, q);

Setting of collision check pair

DblSequence6 dc, sc;
dc.length(0);
sc.length(0);

dynamicsSimulator->registerCollisionCheckPair
   ("robot",
    "RARM_WRIST_R",
    "robot",
    "LARM_WRIST_R",
     0.5,
     0.5,
     dc,
     sc,
     0.0);

This indicates to perform collision check between the joints of both hands.

Preparation of inverse kinematics calculation

Record its initial posture and move a little at once. First, prepare initial postures of both hands. Also, prepare the variable dp which represents how much to move.

double RARM_p[] = {0.197403, -0.210919, 0.93732};
double RARM_R[] = {0.174891, -0.000607636, -0.984588,
		   0.00348999, 0.999994, 2.77917e-06,
		   0.984582, -0.00343669, 0.174892};

double LARM_p[] = {0.197403, 0.210919, 0.93732};
double LARM_R[] = {0.174891, 0.000607636, -0.984588,
		   -0.00348999, 0.999994, -2.77917e-06,
		   0.984582, 0.00343669, 0.174892};
double dp;

Simulation loop

Simulation loop executes the following three processings.

Inverse kinematics calculation

Move one step to close both hands.

LinkPosition link;
link.p[0] = RARM_p[0];
link.p[1] = RARM_p[1] + dp;
link.p[2] = RARM_p[2];
for (int i=0; i<9; i++) 
  link.R[i] = RARM_R[i];
dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
						  CORBA::string_dup("CHEST"),
						  CORBA::string_dup("RARM_WRIST_R"),
						  link);



link.p[0] = LARM_p[0];
link.p[1] = LARM_p[1] - dp;
link.p[2] = LARM_p[2];
for (int i=0; i<9; i++) 
  link.R[i] = LARM_R[i];
dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
						  CORBA::string_dup("CHEST"),
						  CORBA::string_dup("LARM_WRIST_R"),
						  link);							

dynamicsSimulator->calcWorldForwardKinematics();
dp += 0.001;

Updating OnlineViewer

Update display contents in OnlineViewer. It is different from a scheduler that updates time manually because time is not changed.

dynamicsSimulator->getWorldState(state);
state->time = dp*10;
onlineViewer->update(state);

Collision Check

When there are one or more collision points for the pair of joints which we have registered before, we quit the simulation loop.

dynamicsSimulator->checkCollision(true);

if (state->collisions.length() > 0) {
  if (state->collisions[0].points.length() > 0) {
    break;
  }
}

Samples

The sample program of inverse kinematics and collision check is in sample/example/clap of OpenHRP3.

Execution

Start GrxUI and check that OnlineViewer etc. are registered into the name service.

Linux

Please move to (OpenHRP3)/sample/example/mode_ankle and perform as follows.

./clap　-url (OpenHRP3)/sample/model/ -ddp 0.01 -timeK 10

Windows

Please move to (OpenHRP3)/sample/example/mode_ankle and create a batch file. Please describe the following contents in clap.bat. Please rewrite the option of a name service if needed.

set PATH=(OpenHRP3)\bin;%PATH%
clap -ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService -url /(OpenHRP3)\sample\model\ -ddp 0.01 -timeK 10

Run it.

clap.bat

Notice

Alias

Alias is when the assigned variable is also used in the right expression, so the value of the valuable causes interference during calculation. Since interference make the result inappropriate. See these links about tvmet, ublas, and their implementation.

Addressing alias in tvmet

Addressing alias in ublas

By default, tvmet does not assume interference, and it performs well. You have to care interference enough. When interference occurs, introduce a temporary variable, or wrap alias() on the assigned variable.

On the other hand, ublas assumes interference by default, there is not such a problem in general. However its performance gets lost in a case where there is no interference. In such a case, wrap noalis() on the assigned variable not to copy uselessly.

Expression Template

Expression Template is to record a expression itself when the expression is given, not to calculate then, deferring to calculate until the calculation is needed. For example, an expression, a*b+c is recorded as a template like this:

Add< Multiple < a , b > , c >

You must care an order to calculate. Supposing n dimension square matrixes A, B, C, D, consider to assign multiply A, B, C to D.

matrix D(prod(A, prod(B, C)));
matrix D(prod(A, matrix(prod(B, C))));

You can consider these two differences. First the second statement does not store an intermediate value in a temporary area, but the first statement stores B*C and they do multiply. The first statement calculates a intermediate value and store the result in a temporary area, and again calculate a final result, so there are 2 matrix calculation and 2n^3 multiplies. The second does not store a intermediate value and actual calculation performs when the final result is stored in D, so its calculation goes like this:

Quantity of this multiply is calculated for one element of D in (n+1)n, but D has n dimension and the number of its elements is n^2, so it needs (n+1)n^3. Therefore these statements have different orders, and the second is slower than the first. When using Expression Template, please notice such a difference of orders.