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.
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 );
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.
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:
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);
}