generate grasp poses: retrieve multiple IK solutions & check collisions

This commit is contained in:
v4hn 2017-03-02 17:07:42 +01:00
parent 34bd5f8614
commit 19c351b27d
3 changed files with 55 additions and 10 deletions

View File

@ -45,6 +45,10 @@ protected:
double remaining_time_;
bool tried_current_state_as_seed_;
std::vector< std::vector<double> > previous_solutions_;
ros::Publisher pub;
};

View File

@ -5,6 +5,7 @@
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <eigen_conversions/eigen_msg.h>
@ -12,12 +13,14 @@
#include <Eigen/Geometry>
#include <chrono>
#include <functional>
moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name)
: moveit::task_constructor::SubTask::SubTask(name),
timeout_(0.1),
current_angle_(0.0),
remaining_time_(timeout_)
remaining_time_(timeout_),
tried_current_state_as_seed_(false)
{
ros::NodeHandle nh;
pub= nh.advertise<moveit_msgs::DisplayRobotState>("display_robot_state", 50);
@ -51,6 +54,23 @@ moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
return current_angle_ < 2*M_PI;
}
namespace {
bool isValid(planning_scene::PlanningSceneConstPtr scene,
std::vector< std::vector<double> >& old_solutions,
robot_state::RobotState* state,
const robot_model::JointModelGroup* jmg,
const double* joint_positions){
for( std::vector<double> 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());
}
}
bool
moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
@ -61,6 +81,16 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
const moveit::core::JointModelGroup* jmg= grasp_state.getJointModelGroup(group_);
const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind(
&isValid,
scene_,
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())
@ -68,22 +98,33 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
tf::poseEigenToMsg(object_pose, pose);
for(; current_angle_ < 2*M_PI; current_angle_+= 0.2, remaining_time_= timeout_){
if( remaining_time_ <= 0.0 )
while(current_angle_ < 2*M_PI){
if( remaining_time_ <= 0.0 ){
current_angle_+= 0.2;
remaining_time_= timeout_;
tried_current_state_as_seed_= false;
previous_solutions_.clear();
continue;
}
pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_);
std::cout << "trying " << current_angle_ << std::endl;
std::cout << "trying " << current_angle_ << " with remaining time " << remaining_time_ << std::endl;
if(tried_current_state_as_seed_)
grasp_state.setToRandomPositions(jmg);
tried_current_state_as_seed_= true;
auto now= std::chrono::steady_clock::now();
bool succeeded= grasp_state.setFromIK(jmg, pose, eef_, remaining_time_/*,validState*/);
bool succeeded= grasp_state.setFromIK(jmg, pose, eef_, 1, remaining_time_, is_valid);
remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
moveit_msgs::DisplayRobotState drs;
robot_state::robotStateToRobotStateMsg(grasp_state, drs.state);
pub.publish(drs);
if(succeeded){
moveit_msgs::DisplayRobotState drs;
robot_state::robotStateToRobotStateMsg(grasp_state, drs.state);
pub.publish(drs);
previous_solutions_.emplace_back();
grasp_state.copyJointGroupPositions(jmg, previous_solutions_.back());
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
sendBothWays(trajectory, grasp_scene);
return true;

View File

@ -22,7 +22,7 @@ void spawnObject(){
o.primitive_poses[0].orientation.w= 1.0;
o.primitives.resize(1);
o.primitives[0].type= shape_msgs::SolidPrimitive::SPHERE;
o.primitives[0].dimensions.resize(1, 0.07);
o.primitives[0].dimensions.resize(1, 0.03);
psi.applyCollisionObject(o);
}