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
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:
- 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
Select the root of the robot at the right foot (reduces computation complexity)
gikSolver.rootJoint(*robot->rightAnkle());
Create an empty motion (optional)
Create the constraints defing the tasks.
<br>
First priority: Foot on the ground
localPoint[0] = 0.0;
localPoint[1] = 0.0;
localPoint[2] = 0.0;
CjrlJoint& nsfJoint = *(robot->leftAnkle());
matrix4d nsfTransform = nsfJoint.currentTransformation();
Second priority: static Center of Mass
vector3d com = robot->positionCenterOfMass();
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);
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();
vectorN weights = standingRobot->maskFactory()->weightsDoubleSupport();
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;
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)
ZMPworObs = gikSolver.zeroMomentumPoint();
waistTransform = robot->waist()->currentTransformation();
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);