generate_grasp_pose: add grasp offset

This (as probably known from many other implementations...) allows
to grasp the object with the end effector link being further away from it.
This commit is contained in:
v4hn 2017-03-02 19:23:56 +01:00
parent 42fb4c147b
commit 9c5c87fc58
3 changed files with 21 additions and 6 deletions

View File

@ -28,6 +28,8 @@ public:
void setObject(std::string object); void setObject(std::string object);
void setGraspOffset(double grasp_offset);
void setTimeout(double timeout); void setTimeout(double timeout);
void setAngleDelta(double delta); void setAngleDelta(double delta);
@ -37,6 +39,8 @@ protected:
std::string eef_; std::string eef_;
double grasp_offset_;
std::string object_; std::string object_;
double timeout_; double timeout_;

View File

@ -20,6 +20,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st
timeout_(0.1), timeout_(0.1),
angle_delta_(0.1), angle_delta_(0.1),
current_angle_(0.0), current_angle_(0.0),
grasp_offset_(0.0),
remaining_time_(timeout_), remaining_time_(timeout_),
tried_current_state_as_seed_(false) tried_current_state_as_seed_(false)
{ {
@ -43,6 +44,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setObject(std::string obj
object_= object; object_= object;
} }
void
moveit::task_constructor::subtasks::GenerateGraspPose::setGraspOffset(double offset){
grasp_offset_= offset;
}
void void
moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout){ moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout){
timeout_= timeout; timeout_= timeout;
@ -102,12 +108,12 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
std::placeholders::_2, std::placeholders::_2,
std::placeholders::_3); std::placeholders::_3);
geometry_msgs::Pose pose; geometry_msgs::Pose object_pose, grasp_pose;
const Eigen::Affine3d object_pose= scene_->getFrameTransform(object_); const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_);
if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
throw std::runtime_error("requested object does not exist or could not be retrieved"); throw std::runtime_error("requested object does not exist or could not be retrieved");
tf::poseEigenToMsg(object_pose, pose); tf::poseEigenToMsg(object_pose_eigen, object_pose);
while(current_angle_ < 2*M_PI){ while(current_angle_ < 2*M_PI){
if( remaining_time_ <= 0.0 ){ if( remaining_time_ <= 0.0 ){
@ -119,14 +125,18 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
continue; continue;
} }
pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_); grasp_pose= object_pose;
grasp_pose.position.x-= grasp_offset_*cos(current_angle_);
grasp_pose.position.y-= grasp_offset_*sin(current_angle_);
grasp_pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_);
if(tried_current_state_as_seed_) if(tried_current_state_as_seed_)
grasp_state.setToRandomPositions(jmg); grasp_state.setToRandomPositions(jmg);
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, pose, eef_, 1, remaining_time_, is_valid); bool succeeded= grasp_state.setFromIK(jmg, grasp_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();
if(succeeded){ if(succeeded){

View File

@ -43,6 +43,7 @@ int main(int argc, char** argv){
st->setObject("object"); st->setObject("object");
st->setTimeout(0.5); st->setTimeout(0.5);
st->setAngleDelta(0.1); st->setAngleDelta(0.1);
st->setGraspOffset(0.03);
t.addStart(st); t.addStart(st);