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_;
|
double remaining_time_;
|
||||||
|
|
||||||
|
bool tried_current_state_as_seed_;
|
||||||
|
|
||||||
|
std::vector< std::vector<double> > previous_solutions_;
|
||||||
|
|
||||||
ros::Publisher pub;
|
ros::Publisher pub;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -5,6 +5,7 @@
|
|||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
|
#include <moveit/robot_state/robot_state.h>
|
||||||
#include <moveit/move_group_interface/move_group_interface.h>
|
#include <moveit/move_group_interface/move_group_interface.h>
|
||||||
|
|
||||||
#include <eigen_conversions/eigen_msg.h>
|
#include <eigen_conversions/eigen_msg.h>
|
||||||
@ -12,12 +13,14 @@
|
|||||||
#include <Eigen/Geometry>
|
#include <Eigen/Geometry>
|
||||||
|
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
|
#include <functional>
|
||||||
|
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name)
|
moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||||
: moveit::task_constructor::SubTask::SubTask(name),
|
: moveit::task_constructor::SubTask::SubTask(name),
|
||||||
timeout_(0.1),
|
timeout_(0.1),
|
||||||
current_angle_(0.0),
|
current_angle_(0.0),
|
||||||
remaining_time_(timeout_)
|
remaining_time_(timeout_),
|
||||||
|
tried_current_state_as_seed_(false)
|
||||||
{
|
{
|
||||||
ros::NodeHandle nh;
|
ros::NodeHandle nh;
|
||||||
pub= nh.advertise<moveit_msgs::DisplayRobotState>("display_robot_state", 50);
|
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;
|
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
|
bool
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
||||||
planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
|
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::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;
|
geometry_msgs::Pose pose;
|
||||||
const Eigen::Affine3d object_pose= scene_->getFrameTransform(object_);
|
const Eigen::Affine3d object_pose= scene_->getFrameTransform(object_);
|
||||||
if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
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);
|
tf::poseEigenToMsg(object_pose, pose);
|
||||||
|
|
||||||
for(; current_angle_ < 2*M_PI; current_angle_+= 0.2, remaining_time_= timeout_){
|
while(current_angle_ < 2*M_PI){
|
||||||
if( remaining_time_ <= 0.0 )
|
if( remaining_time_ <= 0.0 ){
|
||||||
|
current_angle_+= 0.2;
|
||||||
|
remaining_time_= timeout_;
|
||||||
|
tried_current_state_as_seed_= false;
|
||||||
|
previous_solutions_.clear();
|
||||||
continue;
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_);
|
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();
|
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();
|
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){
|
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);
|
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
|
||||||
sendBothWays(trajectory, grasp_scene);
|
sendBothWays(trajectory, grasp_scene);
|
||||||
return true;
|
return true;
|
||||||
|
|||||||
@ -22,7 +22,7 @@ void spawnObject(){
|
|||||||
o.primitive_poses[0].orientation.w= 1.0;
|
o.primitive_poses[0].orientation.w= 1.0;
|
||||||
o.primitives.resize(1);
|
o.primitives.resize(1);
|
||||||
o.primitives[0].type= shape_msgs::SolidPrimitive::SPHERE;
|
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);
|
psi.applyCollisionObject(o);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user