This object produces whole body joint motion based on entered "elements".These elements are cartesian motion planners of class ChppGikPrioritizedMotion or class ChppGikLocomotionElement. More...
#include <hpp/gik/task/generic-task.hh>
Public Member Functions | |
ChppGikGenericTask (ChppGikStandingRobot *inStandingRobot, double inSamplingPeriod) | |
constructor | |
bool | addElement (ChppGikPrioritizedMotion *inElement) |
add a prioritized motion (like ChppGikInterpolatedElement or ChppGikReadyElement) | |
bool | addElement (ChppGikLocomotionElement *inElement) |
add a locomotion | |
void | clearElements () |
Clear all the entered elements. | |
void | extraEndTime (double inDuration) |
modify extra end time (use with caution, make sure there is at least one second of planned motion after the endtime of the last planned stepElement) | |
void | dynamicWeights (bool inSwitch) |
Activate/disactivate different inverse kinematics weights according to support polygon type (single or double). | |
void | neutralUpperBody (bool inSwitch) |
True = privelege to lighter joints in upper body. | |
void | automaticJointsMask (bool inSwitch, const vectorN *inMask=0) |
True = use the given joints mask. | |
void | bringBackZMP (bool inChoice, double inStartTime, double inDuration) |
inherited | |
~ChppGikGenericTask () | |
Destructor. | |
bool | filterZmpError () |
Filter zmp error. | |
Protected Member Functions | |
void | computeGikWeights (double inTime, const vectorN &inActiveJoints, vectorN &outGikWeights) |
Protected Attributes | |
CjrlHumanoidDynamicRobot * | attRobot |
ChppGikMotionPlan * | attMotionPlan |
ChppGikLocomotionPlan * | attLocomotionPlan |
ChppGikSolver * | attGikSolver |
This object produces whole body joint motion based on entered "elements".These elements are cartesian motion planners of class ChppGikPrioritizedMotion or class ChppGikLocomotionElement.
Special attention must be paid when adding ChppGikLocomotionElement objects:
- Their time frames should not be overlapping, otherwise the overlapping Element is discarded
- The first ChppGikLocomotionElement will generate 1.6 seconds of motion before its actual start time, the last ChppGikLocomotionElement will generate 1.0 seconds of motion after its end. This is due to the preview controller. For example: the user enters a ChppGikStepElement that starts at time 0.0s and finishes at time 2.0s. Then the motion time scale will begin at time -1.6s to end at time 3.0s.
The first 1.6 second duration cannot be changed, but the latter 1.0s can be modified (with caution) through method ChppGikGenericTask::extraEndTime(). This extra 1.0 second is used to allow the center of mass of the robot to finish its motion.
Example of situation where forgetting these time extensions lead to unsolved problems (due to ZMP out of Support Polygon notifications):
Initial configuration is HalfSitting, the waist of the robot is horizontal, parallel to the flat ground. The user adds a ChppGikPrioritizedMotion to maintain the waist horizontality between time 0.0 and time 2.0. he also adds a ChppGikStepElement to make a step between time 0.0 and time 2.0.
Calling method ChppGikGenericTask::solve() will likely provoke "ZMP out" at time 0.0. This is because the center of mass starts moving due to the Preview Controller between times -1.6s and 0.0 such that at time 0.0, a discontinous constraint on the waist horizontality is suddenly added in the stack of tasks, and provokes large accelerations, and "ZMP out". Moreover, after time 2.0s, the constraint on the waist is no longer included in the stack of tasks, and while the center of mass continues to move during the extra end time of 1.0second, the constraint to be maintained for the waist does not hold anymore at the end the motion (time 3.0s).
The correct way to program such a scenario would be to set a waist ChppGikPrioritizedMotion between time 0.0 and time 4.6s, and to set the ChppGikStepElement to begin at time 1.6s and end at time 1.6+2.0=3.6s.
ChppGikGenericTask::ChppGikGenericTask | ( | ChppGikStandingRobot * | inStandingRobot, |
double | inSamplingPeriod | ||
) |
constructor
ChppGikGenericTask::~ChppGikGenericTask | ( | ) |
Destructor.
bool ChppGikGenericTask::addElement | ( | ChppGikPrioritizedMotion * | inElement | ) |
add a prioritized motion (like ChppGikInterpolatedElement or ChppGikReadyElement)
bool ChppGikGenericTask::addElement | ( | ChppGikLocomotionElement * | inElement | ) |
add a locomotion
void ChppGikGenericTask::automaticJointsMask | ( | bool | inSwitch, |
const vectorN * | inMask = 0 |
||
) |
True = use the given joints mask.
False = automatically computed joints mask.
void ChppGikGenericTask::bringBackZMP | ( | bool | inChoice, |
double | inStartTime, | ||
double | inDuration | ||
) | [virtual] |
inherited
Implements ChppGikRobotTask.
Referenced by ChppGikStepTask::bringBackZMP(), ChppGikStepBackTask::bringBackZMP(), and ChppGikWholeBodyTask::bringBackZMP().
void ChppGikGenericTask::clearElements | ( | ) |
Clear all the entered elements.
void ChppGikGenericTask::computeGikWeights | ( | double | inTime, |
const vectorN & | inActiveJoints, | ||
vectorN & | outGikWeights | ||
) | [protected] |
void ChppGikGenericTask::dynamicWeights | ( | bool | inSwitch | ) |
Activate/disactivate different inverse kinematics weights according to support polygon type (single or double).
Default is Enabled.
void ChppGikGenericTask::extraEndTime | ( | double | inDuration | ) |
modify extra end time (use with caution, make sure there is at least one second of planned motion after the endtime of the last planned stepElement)
bool ChppGikGenericTask::filterZmpError | ( | ) |
Filter zmp error.
void ChppGikGenericTask::neutralUpperBody | ( | bool | inSwitch | ) |
True = privelege to lighter joints in upper body.
False = no privelege.
ChppGikSolver* ChppGikGenericTask::attGikSolver [protected] |
ChppGikMotionPlan* ChppGikGenericTask::attMotionPlan [protected] |
CjrlHumanoidDynamicRobot* ChppGikGenericTask::attRobot [protected] |