Drake
GenericPlan< T > Class Template Referenceabstract

This class represents a plan interpretor, which conceptually serves as a bridge between a high level planner (e.g. More...

#include <systems/controllers/plan_eval/generic_plan.h>

Inheritance diagram for GenericPlan< T >:
[legend]

Public Member Functions

std::unique_ptr< GenericPlan< T > > Clone () const
 Returns a unique pointer to a copy of this instance. More...
 
virtual ~GenericPlan ()
 
void Initialize (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_status, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups)
 Initializes this plan. More...
 
void ModifyPlan (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_status, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups)
 Control logic should be implemented in this function. More...
 
void HandlePlan (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_status, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups, const systems::AbstractValue &plan)
 Handles a discrete plan (e.g. More...
 
void UpdateQpInput (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_status, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups, systems::controllers::qp_inverse_dynamics::QpInput *qp_input) const
 Updates qp_input given the current state of the plan and measured robot state in robot_status. More...
 
const ContactStateget_planned_contact_state () const
 Returns the current planned contact state. More...
 
const std::unordered_map< const RigidBody< T > *, manipulation::PiecewiseCartesianTrajectory< T > > & get_body_trajectories () const
 Returns a map of all Cartesian trajectories. More...
 
const manipulation::PiecewiseCartesianTrajectory< T > & get_body_trajectory (const RigidBody< T > *body) const
 Returns the Cartesian trajectory for body. More...
 
const manipulation::PiecewiseCubicTrajectory< T > & get_dof_trajectory () const
 Returns trajectory for all degrees of freedom. More...
 
bool has_body_trajectory (const RigidBody< T > *body) const
 Returns true if there is a Cartesian trajectory for body. More...
 
virtual bool IsRigidBodyTreeCompatible (const RigidBodyTree< T > &robot) const
 Returns true if robot is compatible. More...
 
virtual bool IsRigidBodyTreeAliasGroupsCompatible (const RigidBodyTreeAliasGroups< T > &alias_groups) const
 Returns true if alias_groups is compatible. More...
 
virtual bool IsParamSetCompatible (const systems::controllers::qp_inverse_dynamics::ParamSet &paramset) const
 Returns true if paramset is compatible. More...
 

Protected Member Functions

 GenericPlan ()
 It is convenient to separate allocation with initialization because the later commonly depends on extra information such as measured robot state. More...
 
virtual GenericPlan< T > * CloneGenericPlanDerived () const =0
 Custom fields can be cloned here. More...
 
virtual void InitializeGenericPlanDerived (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_status, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups)=0
 Custom initialization can be implemented here. More...
 
virtual void ModifyPlanGenericPlanDerived (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_status, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups)=0
 Custom state mutation can be implemented here. More...
 
virtual void HandlePlanGenericPlanDerived (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_status, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups, const systems::AbstractValue &plan)=0
 Custom plan handling can be implemented here. More...
 
virtual void UpdateQpInputGenericPlanDerived (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_status, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups, systems::controllers::qp_inverse_dynamics::QpInput *qp_input) const =0
 Custom QpInput updates can be implemented here. More...
 
void UpdateContactState (const ContactState &contact_state)
 Sets the planned contact state to contact_state. More...
 
void set_body_trajectory (const RigidBody< T > *body, const manipulation::PiecewiseCartesianTrajectory< T > &traj)
 Set a Cartesian trajectory for body. More...
 
void remove_body_trajectory (const RigidBody< T > *body)
 Removes a Cartesian trajectory for body. More...
 
void set_dof_trajectory (const manipulation::PiecewiseCubicTrajectory< T > &traj)
 Sets dof trajectory to traj. More...
 
void CheckCompatibilityAndThrow (const RigidBodyTree< T > &robot, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups) const
 Checks robot, paramset and alias_groups's compatibility, throws std::logic_error if any is incompatible. More...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 GenericPlan (const GenericPlan &)=default
 
GenericPlanoperator= (const GenericPlan &)=default
 
 GenericPlan (GenericPlan &&)=default
 
GenericPlanoperator= (GenericPlan &&)=default
 

Detailed Description

template<typename T>
class drake::systems::controllers::plan_eval::GenericPlan< T >

This class represents a plan interpretor, which conceptually serves as a bridge between a high level planner (e.g.

A* planner) and a low level controller (e.g. PID controller). It is responsible for generating high frequency / dense commands that are compatible with the low level controller from the behavioral inputs. One main advantage for this separation is that the planner does not need to worry about any stabilization or realtime requirements associated with the hardware. This class also provides an interface for simple reactive behaviors such as "move arm in a straight line until the hand touches something", which requires little computation but high rate feedback. Here is a concrete example: suppose the robot is a position and velocity controlled manipulator arm, and a motion planner is used to generate a sequence of joint configurations for the robot to follow. A simple implementation can use splines to represent smooth motions through those desired configurations, from which dense position and velocity set points are interpolated and sent to the PID controller at high rate.

For this class, the low level controller of choice is QPController. So the output of this class is an QpInput. The behavioral level inputs are type erased and stored in systems::AbstractValue. Derived classes are responsible for recovering the proper plan type.

Constructor & Destructor Documentation

GenericPlan ( const GenericPlan< T > &  )
protecteddefault
GenericPlan ( GenericPlan< T > &&  )
protecteddefault
virtual ~GenericPlan ( )
inlinevirtual
GenericPlan ( )
inlineprotected

It is convenient to separate allocation with initialization because the later commonly depends on extra information such as measured robot state.

Member Function Documentation

void CheckCompatibilityAndThrow ( const RigidBodyTree< T > &  robot,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups 
) const
inlineprotected

Checks robot, paramset and alias_groups's compatibility, throws std::logic_error if any is incompatible.

Here is the caller graph for this function:

std::unique_ptr< GenericPlan< T > > Clone ( ) const

Returns a unique pointer to a copy of this instance.

Derived classes need to implement CloneGenericPlanDerived() to copy custom internal fields.

virtual GenericPlan<T>* CloneGenericPlanDerived ( ) const
protectedpure virtual

Custom fields can be cloned here.

Here is the caller graph for this function:

const std::unordered_map<const RigidBody<T>*, manipulation::PiecewiseCartesianTrajectory<T> >& get_body_trajectories ( ) const
inline

Returns a map of all Cartesian trajectories.

const manipulation::PiecewiseCartesianTrajectory<T>& get_body_trajectory ( const RigidBody< T > *  body) const
inline

Returns the Cartesian trajectory for body.

const manipulation::PiecewiseCubicTrajectory<T>& get_dof_trajectory ( ) const
inline

Returns trajectory for all degrees of freedom.

const ContactState& get_planned_contact_state ( ) const
inline

Returns the current planned contact state.

void HandlePlan ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_status,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups,
const systems::AbstractValue plan 
)
inline

Handles a discrete plan (e.g.

footstep plan for a walking controller or a sequence of joint angles for a manipulator). Derived classes need to implement custom behaviors in HandlePlanGenericPlanDerived().

Parameters
robot_statusCurrent status of the robot.
paramsetParameters.
alias_groupsTopological information of the robot.
planAbstractValue that contains the plan.
Exceptions
std::logic_errorif the RigidBodyTree reference in robot_status or paramset or alias_groups is incompatible.
virtual void HandlePlanGenericPlanDerived ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_status,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups,
const systems::AbstractValue plan 
)
protectedpure virtual

Custom plan handling can be implemented here.

Implemented in HumanoidManipulationPlan< T >, and ManipulatorMoveEndEffectorPlan< T >.

Here is the caller graph for this function:

bool has_body_trajectory ( const RigidBody< T > *  body) const
inline

Returns true if there is a Cartesian trajectory for body.

void Initialize ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_status,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups 
)

Initializes this plan.

This function sets a dummy plan to main the current joint configuration in robot_status. It assumes no contacts and no Cartesian tracking objectives are present. Derived classes can implement custom behaviors or override these in InitializeGenericPlanDerived().

Parameters
robot_statusCurrent status of the robot.
paramsetParameters.
alias_groupsTopological information of the robot.
Exceptions
std::logic_errorif the RigidBodyTree reference in robot_status or paramset or alias_groups is incompatible.

Here is the call graph for this function:

Here is the caller graph for this function:

virtual void InitializeGenericPlanDerived ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_status,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups 
)
protectedpure virtual

Custom initialization can be implemented here.

Implemented in HumanoidManipulationPlan< T >, and ManipulatorMoveEndEffectorPlan< T >.

Here is the caller graph for this function:

virtual bool IsParamSetCompatible ( const systems::controllers::qp_inverse_dynamics::ParamSet paramset) const
inlinevirtual

Returns true if paramset is compatible.

The default implementation always returns true. Derived class must override to implement meaningful checks.

Here is the caller graph for this function:

virtual bool IsRigidBodyTreeAliasGroupsCompatible ( const RigidBodyTreeAliasGroups< T > &  alias_groups) const
inlinevirtual

Returns true if alias_groups is compatible.

The default implementation always returns true. Derived class must override to implement meaningful checks.

Reimplemented in HumanoidManipulationPlan< T >.

Here is the caller graph for this function:

virtual bool IsRigidBodyTreeCompatible ( const RigidBodyTree< T > &  robot) const
inlinevirtual

Returns true if robot is compatible.

The default implementation always returns true. Derived class must override to implement meaningful checks.

Reimplemented in HumanoidManipulationPlan< T >.

Here is the caller graph for this function:

void ModifyPlan ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_status,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups 
)
inline

Control logic should be implemented in this function.

This function is intended to be called in a main loop. All internal state mutations (e.g. state machine transition, reference trajectory generation, etc) should be done here. Derived classes need to implement custom behaviors in ModifyPlanGenericPlanDerived().

Parameters
robot_statusCurrent status of the robot.
paramsetParameters.
alias_groupsTopological information of the robot.
Exceptions
std::logic_errorif the RigidBodyTree reference in robot_status or paramset or alias_groups is incompatible.
virtual void ModifyPlanGenericPlanDerived ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_status,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups 
)
protectedpure virtual

Custom state mutation can be implemented here.

Here is the caller graph for this function:

GenericPlan& operator= ( GenericPlan< T > &&  )
protecteddefault
GenericPlan& operator= ( const GenericPlan< T > &  )
protecteddefault
void remove_body_trajectory ( const RigidBody< T > *  body)
inlineprotected

Removes a Cartesian trajectory for body.

void set_body_trajectory ( const RigidBody< T > *  body,
const manipulation::PiecewiseCartesianTrajectory< T > &  traj 
)
inlineprotected

Set a Cartesian trajectory for body.

Replaces the existing one if it exists.

void set_dof_trajectory ( const manipulation::PiecewiseCubicTrajectory< T > &  traj)
inlineprotected

Sets dof trajectory to traj.

void UpdateContactState ( const ContactState contact_state)
inlineprotected

Sets the planned contact state to contact_state.

void UpdateQpInput ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_status,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups,
systems::controllers::qp_inverse_dynamics::QpInput qp_input 
) const

Updates qp_input given the current state of the plan and measured robot state in robot_status.

Specifically, this function performs the following:

1. Generates all ContactInformation for each body that are planned to be in
contact.
2. For all degrees of freedom, desired accelerations are computed by:
`vd_d = kp * (q* - q) + kd * (v* - v) + vd*`, where `q*, v*` and `vd*` are
interpolated from the planned trajectory, `q, v` are from robot_status,
and kp and kd are looked up from paramset.
3. For all bodies that have a Cartesian tracking objective, desired
Cartesian accelerations are computed by:
`xd_d = kp * (x* - x) + kd * (xd* - xd) + xdd*`, where `x*, xd*, xdd*` are
interpolated from the planned trajectories, `x, xd` are from
robot_status, and kp and kd are looked up from paramset.

Derived classes can implement custom behaviors or override these in UpdateQpInputGenericPlanDerived().

Parameters
robot_statusCurrent status of the robot.
paramsetParameters.
alias_groupsTopological information of the robot.
[out]qp_inputOutput for QpInput.
Exceptions
std::logic_errorif the RigidBodyTree reference in robot_status or paramset or alias_groups is incompatible.

Here is the call graph for this function:

Here is the caller graph for this function:

virtual void UpdateQpInputGenericPlanDerived ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_status,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups,
systems::controllers::qp_inverse_dynamics::QpInput qp_input 
) const
protectedpure virtual

Custom QpInput updates can be implemented here.

Implemented in HumanoidManipulationPlan< T >.

Here is the caller graph for this function:


The documentation for this class was generated from the following files: