mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
42fb4c147b
commit
9c5c87fc58
@ -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_;
|
||||||
|
|||||||
@ -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){
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user