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:

Public Member Functions

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

Protected Member Functions

virtual bool algorithmSolve ()
 Compute a motion complying with the constraints contained in whole body motion task. More...
 
void clear ()
 Clear data related to previous resolution. More...
 
bool executeResolutionPlan ()
 Execute the task resolution plan. More...
 
bool basicSolve ()
 Try to compute a RobotMotion complying with the entered constraints without stepping. More...
 
void defaultPlannerTaskMaker (double defaultStartTime, double defaultTaskDuration)
 Convert user-entered prioritized constraints to single motion planning tasks with the given start time and duration. More...
 
bool onestepSolve ()
 Try to compute a RobotMotion complying with the entered constraints by doing a step at the same time. More...
 
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. More...
 
void createFootprintCandidates (const ChppGikFootprint *startFootprint, double targetX, double targetY, std::vector< ChppGikFootprint *> &inVectorFootprints)
 Used by onestepSolve(). More...
 
void deleteFootprintCandidates (std::vector< ChppGikFootprint *> &inVectorFootprints)
 Used by onestepSolve(). More...
 
- Protected Member Functions inherited from ChppGikRobotTask
void cleanUp ()
 Delete objects created in this instance. More...
 
void backupRobot ()
 Backup current robot configuration (q, dot q, ddot q) More...
 
void restoreRobot ()
 Restore saved robot configuration (q, dot q, ddot q) and compute forward kinematics. More...
 
void cropMotion (ChppGikRobotTask *inRobotTask)
 concatanate a task solution motion to the current attSolutionMotion and apply the last solution configuration to the robot. More...
 

Protected Attributes

bool attEnableStep
 
bool attHadToStep
 
ChppGikGenericTaskattGenericTask
 
std::vector< ChppGikPrioritizedStateConstraint * > attUserStateTasks
 
std::vector< ChppGikReadyElement * > attUserMotionTasks
 
unsigned int attMaxIterations
 
double attTasksDuration
 
- Protected Attributes inherited from ChppGikRobotTask
ChppGikStandingRobotattStandingRobot
 Associated robot. More...
 
double attSamplingPeriod
 Motion sampling period and its half. More...
 
double attEps
 
double attStartTime
 Start time. More...
 
bool attShowTime
 option to show resolution time More...
 
ChppRobotMotionattSolutionMotion
 The solution joint motion. More...
 
std::string attTaskName
 This task's name. More...
 
CjrlRobotConfiguration attInitialConfiguration
 Backup of robot configuration before task solving {. More...
 
struct timeval * Tps
 timimg {@ More...
 
struct timeval * Tpf
 
struct timezone * Tzp
 

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::ChppGikWholeBodyTask ( ChppGikStandingRobot inStandingRobot,
double  inSamplingPeriod,
unsigned int  inMaxIterations = 1,
ChppGikGenericTask inGenericTask = NULL 
)

Constructor.

◆ ~ChppGikWholeBodyTask()

ChppGikWholeBodyTask::~ChppGikWholeBodyTask ( )

Destructor.

Referenced by bringBackZMP().

Member Function Documentation

◆ addMotionConstraint()

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.

◆ addStateConstraint()

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.

◆ algorithmSolve()

virtual bool ChppGikWholeBodyTask::algorithmSolve ( )
protectedvirtual

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

Implements ChppGikRobotTask.

Referenced by bringBackZMP().

◆ basicSolve()

bool ChppGikWholeBodyTask::basicSolve ( )
protected

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

Referenced by bringBackZMP().

◆ bringBackZMP()

void ChppGikWholeBodyTask::bringBackZMP ( bool  inChoice,
double  inStartTime,
double  inDuration 
)
inlinevirtual

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 algorithmSolve(), attGenericTask, basicSolve(), ChppGikGenericTask::bringBackZMP(), clear(), createFootprintCandidates(), defaultPlannerTaskMaker(), deleteFootprintCandidates(), executeResolutionPlan(), furthestTargetProjection(), onestepSolve(), and ~ChppGikWholeBodyTask().

Referenced by ChppGikReachTask::bringBackZMP().

◆ clear()

void ChppGikWholeBodyTask::clear ( )
protected

Clear data related to previous resolution.

Referenced by bringBackZMP().

◆ createFootprintCandidates()

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.

Referenced by bringBackZMP().

◆ defaultPlannerTaskMaker()

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.

Referenced by bringBackZMP().

◆ deleteFootprintCandidates()

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

Used by onestepSolve().

Delete footprints created by createFootprintCandidates()

Referenced by bringBackZMP().

◆ enableStepping()

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.

◆ executeResolutionPlan()

bool ChppGikWholeBodyTask::executeResolutionPlan ( )
protected

Execute the task resolution plan.

Referenced by bringBackZMP().

◆ furthestTargetProjection()

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.

Referenced by bringBackZMP().

◆ hadToStep()

bool ChppGikWholeBodyTask::hadToStep ( )

Tell if last solution computation needed making a step.

◆ onestepSolve()

bool ChppGikWholeBodyTask::onestepSolve ( )
protected

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

Referenced by bringBackZMP().

◆ reset()

void ChppGikWholeBodyTask::reset ( )

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

◆ robot()

CjrlHumanoidDynamicRobot& ChppGikWholeBodyTask::robot ( ) const

Get associated robot.

◆ tasksDuration()

bool ChppGikWholeBodyTask::tasksDuration ( double  inDuration)
inline

Change tasks duration (should be greater than 0.0)

References attTasksDuration.

Referenced by ChppGikReachTask::tasksDuration().

Member Data Documentation

◆ attEnableStep

bool ChppGikWholeBodyTask::attEnableStep
protected

◆ attGenericTask

ChppGikGenericTask* ChppGikWholeBodyTask::attGenericTask
protected

Referenced by bringBackZMP().

◆ attHadToStep

bool ChppGikWholeBodyTask::attHadToStep
protected

◆ attMaxIterations

unsigned int ChppGikWholeBodyTask::attMaxIterations
protected

◆ attTasksDuration

double ChppGikWholeBodyTask::attTasksDuration
protected

Referenced by tasksDuration().

◆ attUserMotionTasks

std::vector<ChppGikReadyElement*> ChppGikWholeBodyTask::attUserMotionTasks
protected

◆ attUserStateTasks

std::vector<ChppGikPrioritizedStateConstraint*> ChppGikWholeBodyTask::attUserStateTasks
protected