Drake
HumanoidManipulationPlan< T > Class Template Reference

A baseline manipulation plan interpretor for a humanoid robot. More...

#include <drake/examples/QPInverseDynamicsForHumanoids/plan_eval/dev/humanoid_manipulation_plan.h>

Inheritance diagram for HumanoidManipulationPlan< T >:
[legend]
Collaboration diagram for HumanoidManipulationPlan< T >:
[legend]

Public Member Functions

 HumanoidManipulationPlan ()
 
double get_zmp_height () const
 Returns the center of mass height used in the ZMP planner. More...
 
void set_zmp_height (double z)
 Sets the center of mass height used in the ZMP planner to z. More...
 
bool IsRigidBodyTreeCompatible (const RigidBodyTree< T > &robot) const override
 Returns true if robot has at least 2 rigid bodies; the first rigid body has a RPY parametrized floating joint, and it has the same number of generalized positions and velocities. More...
 
bool IsRigidBodyTreeAliasGroupsCompatible (const RigidBodyTreeAliasGroups< T > &alias_groups) const override
 Returns true if alias_groups has singleton body groups: "pelvis", "torso", "left_foot" and "right_foot". More...
 
- Public Member Functions inherited from GenericPlan< T >
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 IsParamSetCompatible (const systems::controllers::qp_inverse_dynamics::ParamSet &paramset) const
 Returns true if paramset is compatible. More...
 

Protected Member Functions

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) override
 Assuming both feet are in contact, initializes a plan that holds the current configuration in robot_status. More...
 
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) override
 This function generates various trajectories from the given plan in plan. More...
 
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 override
 Updates the X and Y dimension of the desired linear momentum change in qp_input based on the center of mass acceleration computed using the ZMP planner's policy. More...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 HumanoidManipulationPlan (const HumanoidManipulationPlan &)=default
 
HumanoidManipulationPlanoperator= (const HumanoidManipulationPlan &)=default
 
 HumanoidManipulationPlan (HumanoidManipulationPlan &&)=default
 
HumanoidManipulationPlanoperator= (HumanoidManipulationPlan &&)=default
 
- Protected Member Functions inherited from GenericPlan< T >
 GenericPlan ()
 It is convenient to separate allocation with initialization because the later commonly depends on extra information such as measured robot state. 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...
 
 GenericPlan (const GenericPlan &)=default
 
GenericPlanoperator= (const GenericPlan &)=default
 
 GenericPlan (GenericPlan &&)=default
 
GenericPlanoperator= (GenericPlan &&)=default
 

Detailed Description

template<typename T>
class drake::examples::qp_inverse_dynamics::HumanoidManipulationPlan< T >

A baseline manipulation plan interpretor for a humanoid robot.

The plan essentially consists of a sequence of time and generalized positions, which are used to generate splines, which are then used as desired trajectories to populate QpInput for the QPController.

See also
HandlePlanGenericPlanDerived for more details about the behavior.

Constructor & Destructor Documentation

HumanoidManipulationPlan ( const HumanoidManipulationPlan< T > &  )
protecteddefault
HumanoidManipulationPlan ( HumanoidManipulationPlan< T > &&  )
protecteddefault

Member Function Documentation

double get_zmp_height ( ) const
inline

Returns the center of mass height used in the ZMP planner.

See also
ZMPPlanner::Plan for more details.
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 
)
overrideprotectedvirtual

This function generates various trajectories from the given plan in plan.

Let T_plan be the timing information and Q_plan be the generalized positions contained in plan, and t and q be the current time and estimated generalized position represented by robot_status, we define Ts = {t, T_plan + t} and Qs = {q, Q_plan}. Given timing Ts and knot points for the generalized positions Qs, the following trajectories are generated:

1. Cubic splines for all degrees of freedom. The splines are generated from
   `Ts` and `Qs` assuming zero end point velocities and continuous
position,
   velocity and acceleration at all intermediate knot points.
2. Cartesian trajectories are made for the pelvis and torso link. The knot
   poses are computed by forward kinematics at each `Qs`. The linear part
of
   the Cartesian trajectory is constructed similarly as above, and the
   rotation part is based on Slerp (linear interpolation between
quaternion).
   Timing is given by `Ts`.
3. A desired ZMP trajectory is generated using Pchip from the center of
mass
   XY positions computed by forward kinematics at each `Qs` and timing
given
   by `Ts`. A nominal center of mass trajectory and its associated optimal
   linear policy is then planned using systems::ZMPPlanner.

Note that these trajectories are arbitrarily designed, without considering position, velocity or acceleration consistency. The QpController will trade off tracking errors based on weights specified in the parameters.

This function also assumes that both feet are in contact with the ground, and every configuration in Q_plan is statically stable given the support region defined by the current foot poses. T_plan has to be strictly increasing, and T_plan[0] is bigger than 0. The current implementation assumes that the message is encoded with a RPY parametrized floating base model as opposed to a quaternion floating joint.

This function returns without changing the current trajectories if the time stamp in plan has not changed from when the current trajectories were last generated or the number of knots in plan is smaller than 1.

Aborts if assumptions about T_plan is not valid.

Exceptions
ifplan is not of type robotlocomotion::robot_plan_t

Implements GenericPlan< T >.

Here is the call graph for this function:

Here is the caller graph for this function:

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 
)
overrideprotectedvirtual

Assuming both feet are in contact, initializes a plan that holds the current configuration in robot_status.

Implements GenericPlan< T >.

Here is the call graph for this function:

Here is the caller graph for this function:

bool IsRigidBodyTreeAliasGroupsCompatible ( const RigidBodyTreeAliasGroups< T > &  alias_groups) const
overridevirtual

Returns true if alias_groups has singleton body groups: "pelvis", "torso", "left_foot" and "right_foot".

Reimplemented from GenericPlan< T >.

Here is the caller graph for this function:

bool IsRigidBodyTreeCompatible ( const RigidBodyTree< T > &  robot) const
overridevirtual

Returns true if robot has at least 2 rigid bodies; the first rigid body has a RPY parametrized floating joint, and it has the same number of generalized positions and velocities.

Reimplemented from GenericPlan< T >.

Here is the call graph for this function:

Here is the caller graph for this function:

HumanoidManipulationPlan& operator= ( HumanoidManipulationPlan< T > &&  )
protecteddefault
HumanoidManipulationPlan& operator= ( const HumanoidManipulationPlan< T > &  )
protecteddefault
void set_zmp_height ( double  z)
inline

Sets the center of mass height used in the ZMP planner to z.

See also
ZMPPlanner::Plan for more details.

Here is the call graph for this function:

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
overrideprotectedvirtual

Updates the X and Y dimension of the desired linear momentum change in qp_input based on the center of mass acceleration computed using the ZMP planner's policy.

Sets the other dimensions of desired centroidal momentum change to zero.

Implements GenericPlan< T >.

Here is the call graph for this function:

Here is the caller graph for this function:


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