mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
generate grasp poses: retrieve multiple IK solutions & check collisions
This commit is contained in:
parent
34bd5f8614
commit
19c351b27d
@ -45,6 +45,10 @@ protected:
|
||||
|
||||
double remaining_time_;
|
||||
|
||||
bool tried_current_state_as_seed_;
|
||||
|
||||
std::vector< std::vector<double> > previous_solutions_;
|
||||
|
||||
ros::Publisher pub;
|
||||
};
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user