diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index eebdc185..d1c74db8 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -62,15 +62,16 @@ moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){ namespace { bool isValid(planning_scene::PlanningSceneConstPtr scene, - std::vector< std::vector >& old_solutions, + std::vector< std::vector >* old_solutions, robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const double* joint_positions){ - for( std::vector sol : old_solutions ){ + for( std::vector sol : *old_solutions ){ if( jmg->distance(joint_positions, sol.data()) < 0.1 ){ return false; } } + state->setJointGroupPositions(jmg, joint_positions); state->update(); return scene->isStateColliding(*state, jmg->getName()); @@ -91,12 +92,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ std::bind( &isValid, scene_, - previous_solutions_, + &previous_solutions_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - geometry_msgs::Pose pose; const Eigen::Affine3d object_pose= scene_->getFrameTransform(object_); if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())