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:

                vector3d p;
        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);

}