add two auxiliary attributes to grasp generator

This commit is contained in:
v4hn 2017-03-07 13:11:47 +01:00
parent 850fb575d0
commit 751c85cd3c
2 changed files with 27 additions and 6 deletions

View File

@ -26,6 +26,8 @@ public:
void setGroup(std::string group_name); void setGroup(std::string group_name);
void setLink(std::string ik_link);
void setGripperGraspPose(std::string pose_name); void setGripperGraspPose(std::string pose_name);
void setObject(std::string object); void setObject(std::string object);
@ -36,13 +38,19 @@ public:
void setAngleDelta(double delta); void setAngleDelta(double delta);
void ignoreCollisions(bool flag);
protected: protected:
std::string eef_; std::string eef_;
std::string group_; std::string group_;
std::string ik_link_;
double grasp_offset_; double grasp_offset_;
bool ignore_collisions_;
std::string gripper_grasp_pose_; std::string gripper_grasp_pose_;
std::string object_; std::string object_;

View File

@ -21,6 +21,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st
angle_delta_(0.1), angle_delta_(0.1),
current_angle_(0.0), current_angle_(0.0),
grasp_offset_(0.0), grasp_offset_(0.0),
ignore_collisions_(false),
remaining_time_(timeout_), remaining_time_(timeout_),
tried_current_state_as_seed_(false) tried_current_state_as_seed_(false)
{ {
@ -34,6 +35,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setGroup(std::string grou
group_= group; group_= group;
} }
void
moveit::task_constructor::subtasks::GenerateGraspPose::setLink(std::string ik_link){
ik_link_= ik_link;
}
void void
moveit::task_constructor::subtasks::GenerateGraspPose::setEndEffector(std::string eef){ moveit::task_constructor::subtasks::GenerateGraspPose::setEndEffector(std::string eef){
eef_= eef; eef_= eef;
@ -68,21 +74,25 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delt
bool bool
moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){ moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
return current_angle_ < 2*M_PI; return current_angle_ > 0;
} }
namespace { namespace {
bool isValid(planning_scene::PlanningSceneConstPtr scene, bool isValid(planning_scene::PlanningSceneConstPtr scene,
std::vector< std::vector<double> >* old_solutions, bool ignore_collisions,
robot_state::RobotState* state, std::vector< std::vector<double> >* old_solutions,
const robot_model::JointModelGroup* jmg, robot_state::RobotState* state,
const double* joint_positions){ const robot_model::JointModelGroup* jmg,
const double* joint_positions){
for( std::vector<double> sol : *old_solutions ){ for( std::vector<double> sol : *old_solutions ){
if( jmg->distance(joint_positions, sol.data()) < 0.1 ){ if( jmg->distance(joint_positions, sol.data()) < 0.1 ){
return false; return false;
} }
} }
if(ignore_collisions)
return true;
state->setJointGroupPositions(jmg, joint_positions); state->setJointGroupPositions(jmg, joint_positions);
state->update(); state->update();
if( scene->isStateColliding(*state, jmg->getName()) ){ if( scene->isStateColliding(*state, jmg->getName()) ){
@ -107,6 +117,8 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first) ? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first)
: grasp_state.getJointModelGroup(group_); : grasp_state.getJointModelGroup(group_);
const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_;
// empty trajectory -> this subtask only produces states // empty trajectory -> this subtask only produces states
const robot_trajectory::RobotTrajectoryPtr traj; const robot_trajectory::RobotTrajectoryPtr traj;
@ -118,6 +130,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
std::bind( std::bind(
&isValid, &isValid,
scene_, scene_,
ignore_collisions_,
&previous_solutions_, &previous_solutions_,
std::placeholders::_1, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_2,
@ -154,7 +167,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
tried_current_state_as_seed_= true; tried_current_state_as_seed_= true;
auto now= std::chrono::steady_clock::now(); auto now= std::chrono::steady_clock::now();
bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, jmg_eef->getEndEffectorParentGroup().second, 1, remaining_time_, is_valid); bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, ik_link, 1, remaining_time_, is_valid);
remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count(); remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
if(succeeded){ if(succeeded){