ChppGikWholeBodyTask Class Reference

Implementation of the Humanoid2006 papers by E.Yoshida. More...

#include <hpp/gik/task/whole-body-task.hh>

Inheritance diagram for ChppGikWholeBodyTask:
Collaboration diagram for ChppGikWholeBodyTask:

List of all members.

Public Member Functions

 ChppGikWholeBodyTask (ChppGikStandingRobot *inStandingRobot, double inSamplingPeriod, unsigned int inMaxIterations=1, ChppGikGenericTask *inGenericTask=NULL)
 Constructor.
CjrlHumanoidDynamicRobot & robot () const
 Get associated robot.
void addStateConstraint (CjrlGikStateConstraint *inStateConstraint, unsigned int inPriority)
 Add a prioritized state constraint.
void addMotionConstraint (CjrlGikMotionConstraint *inMotionConstraint, unsigned int inPriority)
 Add a prioritized motion constraint.
bool hadToStep ()
 Tell if last solution computation needed making a step.
void enableStepping (bool inEnabled)
 enable stepping if a solution could not be found with the current support polygon.Default value upon construction is True.
void reset ()
 Reset the planner by clearing entered constraints (state and motion)
bool tasksDuration (double inDuration)
 Change tasks duration (should be greater than 0.0)
void bringBackZMP (bool inChoice, double inStartTime, double inDuration)
 Option to bring back the ZMP before any other motion (Disabled by default).
 ~ChppGikWholeBodyTask ()
 Destructor.

Protected Member Functions

virtual bool algorithmSolve ()
 Compute a motion complying with the constraints contained in whole body motion task.
void clear ()
 Clear data related to previous resolution.
bool executeResolutionPlan ()
 Execute the task resolution plan.
bool basicSolve ()
 Try to compute a RobotMotion complying with the entered constraints without stepping.
void defaultPlannerTaskMaker (double defaultStartTime, double defaultTaskDuration)
 Convert user-entered prioritized constraints to single motion planning tasks with the given start time and duration.
bool onestepSolve ()
 Try to compute a RobotMotion complying with the entered constraints by doing a step at the same time.
void furthestTargetProjection (double centerX, double centerY, double &outX, double &outY, double &outDistance, bool &ats)
 Used by oneStep solve to find the projection of the furthest position or tranformation target from the vertical axis between the footprints.
void createFootprintCandidates (const ChppGikFootprint *startFootprint, double targetX, double targetY, std::vector< ChppGikFootprint * > &inVectorFootprints)
 Used by onestepSolve().
void deleteFootprintCandidates (std::vector< ChppGikFootprint * > &inVectorFootprints)
 Used by onestepSolve().

Protected Attributes

bool attEnableStep
bool attHadToStep
ChppGikGenericTaskattGenericTask
std::vector
< ChppGikPrioritizedStateConstraint * > 
attUserStateTasks
std::vector
< ChppGikReadyElement * > 
attUserMotionTasks
unsigned int attMaxIterations
double attTasksDuration

Detailed Description

Implementation of the Humanoid2006 papers by E.Yoshida.

This class implements a solver for a humanoid robot. Constructor takes as input

Computation of the motion is performed by method ChppGikWholeBodyTask::algorithmSolve in two steps:

  • first the solvers tries to solve the hierarchy of tasks by keeping the feet on the ground (method basicSolve),
  • if failure, the robot tries to solve the hierarchy of tasks by performing a step (method ChppGikWholeBodyTask::onestepSolve)
Note:
before solving, the solver checks the support polygon of the robot (see ChppGikStandingRobot::supportPolygon)

Constructor & Destructor Documentation

ChppGikWholeBodyTask::ChppGikWholeBodyTask ( ChppGikStandingRobot inStandingRobot,
double  inSamplingPeriod,
unsigned int  inMaxIterations = 1,
ChppGikGenericTask inGenericTask = NULL 
)

Constructor.

ChppGikWholeBodyTask::~ChppGikWholeBodyTask ( )

Destructor.


Member Function Documentation

void ChppGikWholeBodyTask::addMotionConstraint ( CjrlGikMotionConstraint inMotionConstraint,
unsigned int  inPriority 
)

Add a prioritized motion constraint.

The motion constraint is inserted as-is to the whole body motion task. Simultaneous motion subtasks might be generated by the implemented algorithms.

void ChppGikWholeBodyTask::addStateConstraint ( CjrlGikStateConstraint inStateConstraint,
unsigned int  inPriority 
)

Add a prioritized state constraint.

The state constraint is transformed by the implemented algorithms into motion subtasks which are added to the whole body motion task.

virtual bool ChppGikWholeBodyTask::algorithmSolve ( ) [protected, virtual]

Compute a motion complying with the constraints contained in whole body motion task.

Implements ChppGikRobotTask.

bool ChppGikWholeBodyTask::basicSolve ( ) [protected]

Try to compute a RobotMotion complying with the entered constraints without stepping.

void ChppGikWholeBodyTask::bringBackZMP ( bool  inChoice,
double  inStartTime,
double  inDuration 
) [inline, virtual]

Option to bring back the ZMP before any other motion (Disabled by default).

A motion is planned to bring back the ZMP to the closest point on the line segment defining the center of the safe zone. The start time and the duration of this ZMP motion is left up to the user. The start time is defined relatively to the start of the locomotion plan. Example: bringBackZmp(-0.5, 2.0 ) will make the zmp planning start 0.5 seconds before the first locomotion element (if none, absolute time 0.0s), and take 2.0 seconds to finish. USE AT YOUR OWN RISK

Implements ChppGikRobotTask.

References attGenericTask, and ChppGikGenericTask::bringBackZMP().

Referenced by ChppGikReachTask::bringBackZMP().

void ChppGikWholeBodyTask::clear ( ) [protected]

Clear data related to previous resolution.

void ChppGikWholeBodyTask::createFootprintCandidates ( const ChppGikFootprint startFootprint,
double  targetX,
double  targetY,
std::vector< ChppGikFootprint * > &  inVectorFootprints 
) [protected]

Used by onestepSolve().

Compute footprint candidates for a stepping motion that can help realize the constraints entered by the user. attMaxIterations footprints are computed.

void ChppGikWholeBodyTask::defaultPlannerTaskMaker ( double  defaultStartTime,
double  defaultTaskDuration 
) [protected]

Convert user-entered prioritized constraints to single motion planning tasks with the given start time and duration.

void ChppGikWholeBodyTask::deleteFootprintCandidates ( std::vector< ChppGikFootprint * > &  inVectorFootprints) [protected]

Used by onestepSolve().

Delete footprints created by createFootprintCandidates()

void ChppGikWholeBodyTask::enableStepping ( bool  inEnabled)

enable stepping if a solution could not be found with the current support polygon.Default value upon construction is True.

bool ChppGikWholeBodyTask::executeResolutionPlan ( ) [protected]

Execute the task resolution plan.

void ChppGikWholeBodyTask::furthestTargetProjection ( double  centerX,
double  centerY,
double &  outX,
double &  outY,
double &  outDistance,
bool &  ats 
) [protected]

Used by oneStep solve to find the projection of the furthest position or tranformation target from the vertical axis between the footprints.

bool ChppGikWholeBodyTask::hadToStep ( )

Tell if last solution computation needed making a step.

bool ChppGikWholeBodyTask::onestepSolve ( ) [protected]

Try to compute a RobotMotion complying with the entered constraints by doing a step at the same time.

void ChppGikWholeBodyTask::reset ( )

Reset the planner by clearing entered constraints (state and motion)

CjrlHumanoidDynamicRobot& ChppGikWholeBodyTask::robot ( ) const

Get associated robot.

bool ChppGikWholeBodyTask::tasksDuration ( double  inDuration) [inline]

Change tasks duration (should be greater than 0.0)

References attTasksDuration.

Referenced by ChppGikReachTask::tasksDuration().


Member Data Documentation

unsigned int ChppGikWholeBodyTask::attMaxIterations [protected]

Referenced by tasksDuration().