This class provides some common functionality for generating IK plans for robot arms, including things like creating a MultibodyPlant, setting joint velocity limits, implementing a robot status update handler suitable for invoking from an LCM callback, and generating plans to move a specified link to a goal configuration.
This can be useful when building simple demonstration programs to move a robot arm, for example when testing new arms which haven't been previously used with Drake, or testing modifications to existing robot configurations. See the kuka_iiwa_arm and kinova_jaco_arm examples for existing uses.
#include <drake/manipulation/util/move_ik_demo_base.h>
Public Member Functions | |
MoveIkDemoBase (std::string robot_description, std::string base_link, std::string ik_link, int print_interval) | |
~MoveIkDemoBase () | |
const multibody::MultibodyPlant< double > & | plant () const |
void | set_joint_velocity_limits (const Eigen::Ref< const Eigen::VectorXd > &) |
Set the joint velocity limits when building the plan. More... | |
void | HandleStatus (const Eigen::Ref< const Eigen::VectorXd > &q) |
Update the current robot status. More... | |
std::optional< lcmt_robot_plan > | Plan (const math::RigidTransformd &goal_pose) |
Attempt to generate a plan moving ik_link (specified at construction time) from the joint configuration specified in the last call to HandleStatus to a configuration with ik_link at goal_pose . More... | |
int | status_count () const |
Returns a count of how many times HandleStatus has been called. More... | |
Does not allow copy, move, or assignment | |
MoveIkDemoBase (const MoveIkDemoBase &)=delete | |
MoveIkDemoBase & | operator= (const MoveIkDemoBase &)=delete |
MoveIkDemoBase (MoveIkDemoBase &&)=delete | |
MoveIkDemoBase & | operator= (MoveIkDemoBase &&)=delete |
|
delete |
|
delete |
MoveIkDemoBase | ( | std::string | robot_description, |
std::string | base_link, | ||
std::string | ik_link, | ||
int | print_interval | ||
) |
robot_description | A description file to load of the robot to plan. |
base_link | Name of the base link of the robot, will be welded to the world in the planning model. |
ik_link | Name of the link to plan a pose for. |
print_interval | Print an updated end effector position every N calls to HandleStatus. |
~MoveIkDemoBase | ( | ) |
void HandleStatus | ( | const Eigen::Ref< const Eigen::VectorXd > & | q | ) |
Update the current robot status.
q | must be equal to the number of positions in the MultibodyPlant (see plant()). |
|
delete |
|
delete |
std::optional<lcmt_robot_plan> Plan | ( | const math::RigidTransformd & | goal_pose | ) |
Attempt to generate a plan moving ik_link (specified at construction time) from the joint configuration specified in the last call to HandleStatus
to a configuration with ik_link at goal_pose
.
Returns nullopt if planning failed.
If | HandleStatus has not been invoked. |
const multibody::MultibodyPlant<double>& plant | ( | ) | const |
void set_joint_velocity_limits | ( | const Eigen::Ref< const Eigen::VectorXd > & | ) |
Set the joint velocity limits when building the plan.
The default velocity limits from the robot description will be used if this isn't set.
int status_count | ( | ) | const |
Returns a count of how many times HandleStatus
has been called.