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 setGraspOffset(double grasp_offset);
|
||||
|
||||
void setTimeout(double timeout);
|
||||
|
||||
void setAngleDelta(double delta);
|
||||
@ -37,6 +39,8 @@ protected:
|
||||
|
||||
std::string eef_;
|
||||
|
||||
double grasp_offset_;
|
||||
|
||||
std::string object_;
|
||||
|
||||
double timeout_;
|
||||
|
||||
@ -20,6 +20,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st
|
||||
timeout_(0.1),
|
||||
angle_delta_(0.1),
|
||||
current_angle_(0.0),
|
||||
grasp_offset_(0.0),
|
||||
remaining_time_(timeout_),
|
||||
tried_current_state_as_seed_(false)
|
||||
{
|
||||
@ -43,6 +44,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setObject(std::string obj
|
||||
object_= object;
|
||||
}
|
||||
|
||||
void
|
||||
moveit::task_constructor::subtasks::GenerateGraspPose::setGraspOffset(double offset){
|
||||
grasp_offset_= offset;
|
||||
}
|
||||
|
||||
void
|
||||
moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout){
|
||||
timeout_= timeout;
|
||||
@ -102,12 +108,12 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
||||
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())
|
||||
geometry_msgs::Pose object_pose, grasp_pose;
|
||||
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_);
|
||||
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");
|
||||
|
||||
tf::poseEigenToMsg(object_pose, pose);
|
||||
tf::poseEigenToMsg(object_pose_eigen, object_pose);
|
||||
|
||||
while(current_angle_ < 2*M_PI){
|
||||
if( remaining_time_ <= 0.0 ){
|
||||
@ -119,14 +125,18 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
||||
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_)
|
||||
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_, 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();
|
||||
|
||||
if(succeeded){
|
||||
|
||||
@ -43,6 +43,7 @@ int main(int argc, char** argv){
|
||||
st->setObject("object");
|
||||
st->setTimeout(0.5);
|
||||
st->setAngleDelta(0.1);
|
||||
st->setGraspOffset(0.03);
|
||||
|
||||
t.addStart(st);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user