Uses an interpolated IK approach from pregrasp to final grasp. More...
#include <GraspSequenceValidator.h>
Classes | |
struct | GraspSequenceDetails |
Public Member Functions | |
void | getGroupJoints (const std::string &group_name, std::vector< std::string > &group_links) |
void | getGroupLinks (const std::string &group_name, std::vector< std::string > &group_links) |
GraspSequenceValidator (planning_environment::CollisionModels *cm=NULL, const std::string &plugin_name="") | |
Also adds a grasp marker at the pre-grasp location. | |
void | setPlanningSceneState (planning_models::KinematicState *state) |
virtual void | testGrasp (const object_manipulation_msgs::PickupGoal &pickup_goal, const object_manipulation_msgs::Grasp &grasp, object_manipulator::GraspExecutionInfo &execution_info) |
virtual void | testGrasps (const object_manipulation_msgs::PickupGoal &pickup_goal, const std::vector< object_manipulation_msgs::Grasp > &grasps, std::vector< object_manipulator::GraspExecutionInfo > &execution_info, bool return_on_first_hit) |
virtual void | testGrasps (const object_manipulation_msgs::PickupGoal &pickup_goal, const std::vector< object_manipulation_msgs::Grasp > &grasps, double twistAngle, std::vector< GraspSequenceDetails > &execution_info, bool return_on_first_hit) |
~GraspSequenceValidator () | |
Public Attributes | |
pluginlib::ClassLoader < kinematics::KinematicsBase > | kinematics_loader_ |
Protected Member Functions | |
planning_environment::CollisionModels * | getCollisionModels () |
bool | getInterpolatedIK (const std::string &arm_name, const tf::Transform &first_pose, const tf::Vector3 &direction, const double &distance, const std::vector< double > &ik_solution, const bool &reverse, const bool &premultiply, trajectory_msgs::JointTrajectory &traj) |
bool | getInterpolatedIK (const std::string &arm_name, const tf::Transform &initialTransform, const tf::Quaternion &rot, const std::vector< double > &ikSolutionAtStart, std::vector< double > &ikSolutionAtEnd, trajectory_msgs::JointTrajectory &traj) |
planning_models::KinematicState * | getPlanningSceneState () |
virtual std::vector < arm_navigation_msgs::LinkPadding > | linkPaddingForGrasp (const object_manipulation_msgs::PickupGoal &pickup_goal) |
Dynamic link padding to be used for grasp operation. | |
Protected Attributes | |
planning_environment::CollisionModels * | cm_ |
double | consistent_angle_ |
std::map< std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware * > | ik_solver_map_ |
unsigned int | num_points_ |
unsigned int | redundancy_ |
planning_models::KinematicState * | state_ |
ros::Publisher | vis_marker_array_publisher_ |
ros::Publisher | vis_marker_publisher_ |
Uses an interpolated IK approach from pregrasp to final grasp.
Initial check consists of IK for the pre-grasp, followed by generation of an interpolated IK path from pre-grasp to grasp. This is then followed by computation of an interpolated IK path from grasp to lift.
Execution consists of using move arm to go to the pre-grasp, then execution of the interpolated IK paths for grasp. Lift is executed separately, called from the higher level executor.
Note that we do not use the pre-grasp from the database directly; rather, the pre-grasp is obtained by backing up the grasp by a pre-defined amount. This allows us more flexibility in choosing the pre-grasp. However, we can no longer always assume the pre-grasp is collision free, which we could if we used the pre-grasp from the database directly.
In the most recent database version, the pre-grasp was obtained by backing up 10 cm, so we know at least that that is not colliding with the object.
Definition at line 36 of file GraspSequenceValidator.h.
GraspSequenceValidator::GraspSequenceValidator | ( | planning_environment::CollisionModels * | cm = NULL , |
const std::string & | plugin_name = "" |
||
) |
Also adds a grasp marker at the pre-grasp location.
Definition at line 19 of file GraspSequenceValidator.cpp.
Definition at line 60 of file GraspSequenceValidator.cpp.
planning_environment::CollisionModels * GraspSequenceValidator::getCollisionModels | ( | void | ) | [protected] |
Definition at line 65 of file GraspSequenceValidator.cpp.
void GraspSequenceValidator::getGroupJoints | ( | const std::string & | group_name, |
std::vector< std::string > & | group_links | ||
) |
Definition at line 114 of file GraspSequenceValidator.cpp.
void GraspSequenceValidator::getGroupLinks | ( | const std::string & | group_name, |
std::vector< std::string > & | group_links | ||
) |
Definition at line 104 of file GraspSequenceValidator.cpp.
bool GraspSequenceValidator::getInterpolatedIK | ( | const std::string & | arm_name, |
const tf::Transform & | first_pose, | ||
const tf::Vector3 & | direction, | ||
const double & | distance, | ||
const std::vector< double > & | ik_solution, | ||
const bool & | reverse, | ||
const bool & | premultiply, | ||
trajectory_msgs::JointTrajectory & | traj | ||
) | [protected] |
Definition at line 124 of file GraspSequenceValidator.cpp.
bool GraspSequenceValidator::getInterpolatedIK | ( | const std::string & | arm_name, |
const tf::Transform & | initialTransform, | ||
const tf::Quaternion & | rot, | ||
const std::vector< double > & | ikSolutionAtStart, | ||
std::vector< double > & | ikSolutionAtEnd, | ||
trajectory_msgs::JointTrajectory & | traj | ||
) | [protected] |
Definition at line 165 of file GraspSequenceValidator.cpp.
planning_models::KinematicState * GraspSequenceValidator::getPlanningSceneState | ( | ) | [protected] |
Definition at line 77 of file GraspSequenceValidator.cpp.
std::vector< arm_navigation_msgs::LinkPadding > GraspSequenceValidator::linkPaddingForGrasp | ( | const object_manipulation_msgs::PickupGoal & | pickup_goal | ) | [protected, virtual] |
Dynamic link padding to be used for grasp operation.
Zero padding on fingertip links
Definition at line 96 of file GraspSequenceValidator.cpp.
void GraspSequenceValidator::setPlanningSceneState | ( | planning_models::KinematicState * | state | ) | [inline] |
Definition at line 92 of file GraspSequenceValidator.h.
virtual void GraspSequenceValidator::testGrasp | ( | const object_manipulation_msgs::PickupGoal & | pickup_goal, |
const object_manipulation_msgs::Grasp & | grasp, | ||
object_manipulator::GraspExecutionInfo & | execution_info | ||
) | [inline, virtual] |
Definition at line 103 of file GraspSequenceValidator.h.
void GraspSequenceValidator::testGrasps | ( | const object_manipulation_msgs::PickupGoal & | pickup_goal, |
const std::vector< object_manipulation_msgs::Grasp > & | grasps, | ||
std::vector< object_manipulator::GraspExecutionInfo > & | execution_info, | ||
bool | return_on_first_hit | ||
) | [virtual] |
Definition at line 226 of file GraspSequenceValidator.cpp.
void GraspSequenceValidator::testGrasps | ( | const object_manipulation_msgs::PickupGoal & | pickup_goal, |
const std::vector< object_manipulation_msgs::Grasp > & | grasps, | ||
double | twistAngle, | ||
std::vector< GraspSequenceDetails > & | execution_info, | ||
bool | return_on_first_hit | ||
) | [virtual] |
Definition at line 855 of file GraspSequenceValidator.cpp.
Definition at line 79 of file GraspSequenceValidator.h.
double GraspSequenceValidator::consistent_angle_ [protected] |
Definition at line 69 of file GraspSequenceValidator.h.
std::map<std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware*> GraspSequenceValidator::ik_solver_map_ [protected] |
Definition at line 68 of file GraspSequenceValidator.h.
Definition at line 84 of file GraspSequenceValidator.h.
unsigned int GraspSequenceValidator::num_points_ [protected] |
Definition at line 70 of file GraspSequenceValidator.h.
unsigned int GraspSequenceValidator::redundancy_ [protected] |
Definition at line 71 of file GraspSequenceValidator.h.
Definition at line 80 of file GraspSequenceValidator.h.
Definition at line 73 of file GraspSequenceValidator.h.
Definition at line 74 of file GraspSequenceValidator.h.