Example I

Introduction

In this basic example, a motion for the center of the right hand is planned while projection of the center of mass on the ground and the feet's positions are maintained static.

Code

    Set a sampling period: 5ms second is the value used on HRP2
samplingPeriod = 5e-3;

Assuming that we have already constructed a CjrlHumanoidDynamicRobot object in halfsitting configuration and for which we have stored a pointer robot, create a standing robot:

standingRobot = new ChppGikStandingRobot ( *robot );
Note
The degrees of freedom of the joints of the robot need to be bounded with actual values. If left to 0, the robot will not move.
            Create a Gik solver
ChppGikSolver gikSolver(*robot);
    Select the root of the robot at the right foot (reduces computation complexity)
gikSolver.rootJoint(*robot->rightAnkle());
    Create an empty motion (optional)
ChppRobotMotion solutionMotion(robot, 0.0 , samplingPeriod);
vector3d ZMPworPla;//planned ZMP in absolute frame
vector3d ZMPwstPla;//Planned ZMP in robot waist frame
vector3d ZMPworObs;//Observed ZMP in absolute frame
vector3d ZMPwstObs;//Observed ZMP in robot waist frame
ZMPworPla = com; // the ZMP is planned to remain at its inital position
    Create the constraints defing the tasks.
    <br>
    First priority: Foot on the ground
vector3d localPoint;
localPoint[0] = 0.0;
localPoint[1] = 0.0;
localPoint[2] = 0.0;
CjrlJoint& nsfJoint = *(robot->leftAnkle());
matrix4d nsfTransform = nsfJoint.currentTransformation();
ChppGikTransformationConstraint nsfc(*robot, nsfJoint, localPoint, nsfTransform);
    Second priority: static Center of Mass
vector3d com = robot->positionCenterOfMass();
ChppGikComConstraint comc(*robot, com[0], com[1]);
    Third priority: A position constraint on a point in the right wrist frame
matrix4d curT= robot->rightWrist()->currentTransformation();
CjrlJoint& rwJoint = *(robot->rightWrist());
robot->rightHand()->getCenter(localPoint);
ChppGikPositionConstraint pc(*robot,rwJoint,localPoint, curT*localPoint);
Stack the constraints in a vector
std::vector<CjrlGikStateConstraint*> stack;
stack.push_back(&nsfc);
stack.push_back(&comc);
stack.push_back(&pc);
    Build the weights used in solving the pseudo inverse kinematics
vectorN activated = standingRobot->maskFactory()->wholeBodyMask();// active joints
vectorN weights = standingRobot->maskFactory()->weightsDoubleSupport(); //With these weights, the more mass a joint is lifting the less it's used
vectorN combined = weights;
for (unsigned int i=0;i<combined.size();i++)
combined(i) *= activated(i);
for (unsigned int i=0;i<6;i++)
combined(i) = 0.0; // the absolute trnasformation (freeflyer) of the robot is not controlled
    Set the weights in the solver
gikSolver.weights(combined);

Start the loop where we continuously change the desired target position of the hand, thus generating a motion:

matrix4d waistTransform, inverseWaistTransform;
for (unsigned int j = 0; j< 500;j++)
{

Change the target position for the hand slightly:

p = pc->worldTarget();
p[0] += 0.001;
pc->worldTarget(p);

Attempt solve with a single one step: Prepare the constraints (jacobian and value computation)

gikSolver.prepare(stack);

Solve

gikSolver.solve( stack );

Apply solution to robot

standingRobot->updateRobot(gikSolver.solutionRootPose(), gikSolver.solutionJointConfiguration(), samplingPeriod);

Store the computed sample (optional)

//compute planned and observed ZMP in absolute and waist frames
ZMPworObs = gikSolver.zeroMomentumPoint();
waistTransform = robot->waist()->currentTransformation();
MAL_S4x4_INVERSE(waistTransform,inverseWaistTransform,double);
MAL_S4x4_C_eq_A_by_B(ZMPwstObs,inverseWaistTransform,ZMPworObs);
MAL_S4x4_C_eq_A_by_B(ZMPwstPla,inverseWaistTransform,ZMPworPla);
motionsample.configuration = gikSolver.currentConfiguration();
motionsample.velocity = gikSolver.currentVelocity();
motionsample.acceleration = gikSolver.currentAcceleration();
motionsample.rootpose = gikSolver.solutionRootPose();
motionsample.ZMPwstPla = ZMPwstPla;
motionsample.ZMPwstObs = ZMPwstObs;
motionsample.ZMPworPla = ZMPworPla;
motionsample.ZMPworObs = ZMPworObs;
solutionMotion.appendSample(motionsample);
}