Drake
ManipulatorMoveEndEffectorPlan< T > Class Template Reference

A concrete plan that sets up a Cartesian tracking objective for the end effector assuming no contacts. More...

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

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

Public Member Functions

 ManipulatorMoveEndEffectorPlan ()
 
- 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 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...
 

Static Public Attributes

static constexpr const char * kEndEffectorAliasGroupName = "flange"
 

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
 A desired stationary Cartesian spline is set up for the body whose alias in alias_groups is kEndEffectorAliasGroupName. More...
 
void HandlePlanGenericPlanDerived (const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &robot_stauts, const systems::controllers::qp_inverse_dynamics::ParamSet &paramset, const RigidBodyTreeAliasGroups< T > &alias_groups, const systems::AbstractValue &plan) override
 This function assumes that the bytes in plan encodes a valid lcmt_manipulator_plan_move_end_effector plan, from which the desired poses (x_i) for the end effector and timing (t_i) are extracted and used to construct a Cartesian spline. More...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 ManipulatorMoveEndEffectorPlan (const ManipulatorMoveEndEffectorPlan &)=default
 
ManipulatorMoveEndEffectorPlanoperator= (const ManipulatorMoveEndEffectorPlan &)=default
 
 ManipulatorMoveEndEffectorPlan (ManipulatorMoveEndEffectorPlan &&)=default
 
ManipulatorMoveEndEffectorPlanoperator= (ManipulatorMoveEndEffectorPlan &&)=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::systems::controllers::plan_eval::ManipulatorMoveEndEffectorPlan< T >

A concrete plan that sets up a Cartesian tracking objective for the end effector assuming no contacts.

The joint space tracking objective is set to track the measured joint position when Initialize() is called. Note that joint position tracking can be turned off by setting the position gains to zero in the parameters passed to UpdateQpInput.

Constructor & Destructor Documentation

Member Function Documentation

void HandlePlanGenericPlanDerived ( const systems::controllers::qp_inverse_dynamics::RobotKinematicState< T > &  robot_stauts,
const systems::controllers::qp_inverse_dynamics::ParamSet paramset,
const RigidBodyTreeAliasGroups< T > &  alias_groups,
const systems::AbstractValue plan 
)
overrideprotectedvirtual

This function assumes that the bytes in plan encodes a valid lcmt_manipulator_plan_move_end_effector plan, from which the desired poses (x_i) for the end effector and timing (t_i) are extracted and used to construct a Cartesian spline.

Let t_now be the time in robot_stauts. If t_0 = 0 (first time stamp in plan is zero), the spline is constructed with MakeCubicLinearWithEndLinearVelocity(t_i + t_now, x_i, 0, 0), which starts immediately from x_0 regardless of what's the current planned desired pose and velocity. This introduces a discontinuity in the interpolated desired position and velocity when interpolating, which can cause a larger discontinuity (due to gains) in output and instability on robot. If t_0 > 0, the spline is constructed with MakeCubicLinearWithEndLinearVelocity({0, t_i} + t_now, {x_d_now, x_i}, xd_d_now, 0), where x_d_now and xd_d_now are the current desired pose and velocity of the end effector. This results in a smoother transition to the new plan.

Exceptions
std::bad_castif plan is not of type lcmt_manipulator_plan_move_end_effector;

Implements GenericPlan< T >.

Here is the call 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

A desired stationary Cartesian spline is set up for the body whose alias in alias_groups is kEndEffectorAliasGroupName.

Implements GenericPlan< T >.

Here is the call graph for this function:

ManipulatorMoveEndEffectorPlan& operator= ( const ManipulatorMoveEndEffectorPlan< T > &  )
protecteddefault
ManipulatorMoveEndEffectorPlan& operator= ( ManipulatorMoveEndEffectorPlan< T > &&  )
protecteddefault

Member Data Documentation

constexpr const char* kEndEffectorAliasGroupName = "flange"
static

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