mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add two auxiliary attributes to grasp generator
This commit is contained in:
parent
850fb575d0
commit
751c85cd3c
@ -26,6 +26,8 @@ public:
|
|||||||
|
|
||||||
void setGroup(std::string group_name);
|
void setGroup(std::string group_name);
|
||||||
|
|
||||||
|
void setLink(std::string ik_link);
|
||||||
|
|
||||||
void setGripperGraspPose(std::string pose_name);
|
void setGripperGraspPose(std::string pose_name);
|
||||||
|
|
||||||
void setObject(std::string object);
|
void setObject(std::string object);
|
||||||
@ -36,13 +38,19 @@ public:
|
|||||||
|
|
||||||
void setAngleDelta(double delta);
|
void setAngleDelta(double delta);
|
||||||
|
|
||||||
|
void ignoreCollisions(bool flag);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::string eef_;
|
std::string eef_;
|
||||||
|
|
||||||
std::string group_;
|
std::string group_;
|
||||||
|
|
||||||
|
std::string ik_link_;
|
||||||
|
|
||||||
double grasp_offset_;
|
double grasp_offset_;
|
||||||
|
|
||||||
|
bool ignore_collisions_;
|
||||||
|
|
||||||
std::string gripper_grasp_pose_;
|
std::string gripper_grasp_pose_;
|
||||||
|
|
||||||
std::string object_;
|
std::string object_;
|
||||||
|
|||||||
@ -21,6 +21,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st
|
|||||||
angle_delta_(0.1),
|
angle_delta_(0.1),
|
||||||
current_angle_(0.0),
|
current_angle_(0.0),
|
||||||
grasp_offset_(0.0),
|
grasp_offset_(0.0),
|
||||||
|
ignore_collisions_(false),
|
||||||
remaining_time_(timeout_),
|
remaining_time_(timeout_),
|
||||||
tried_current_state_as_seed_(false)
|
tried_current_state_as_seed_(false)
|
||||||
{
|
{
|
||||||
@ -34,6 +35,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setGroup(std::string grou
|
|||||||
group_= group;
|
group_= group;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
moveit::task_constructor::subtasks::GenerateGraspPose::setLink(std::string ik_link){
|
||||||
|
ik_link_= ik_link;
|
||||||
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setEndEffector(std::string eef){
|
moveit::task_constructor::subtasks::GenerateGraspPose::setEndEffector(std::string eef){
|
||||||
eef_= eef;
|
eef_= eef;
|
||||||
@ -68,21 +74,25 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delt
|
|||||||
|
|
||||||
bool
|
bool
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
|
moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
|
||||||
return current_angle_ < 2*M_PI;
|
return current_angle_ > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
bool isValid(planning_scene::PlanningSceneConstPtr scene,
|
bool isValid(planning_scene::PlanningSceneConstPtr scene,
|
||||||
std::vector< std::vector<double> >* old_solutions,
|
bool ignore_collisions,
|
||||||
robot_state::RobotState* state,
|
std::vector< std::vector<double> >* old_solutions,
|
||||||
const robot_model::JointModelGroup* jmg,
|
robot_state::RobotState* state,
|
||||||
const double* joint_positions){
|
const robot_model::JointModelGroup* jmg,
|
||||||
|
const double* joint_positions){
|
||||||
for( std::vector<double> sol : *old_solutions ){
|
for( std::vector<double> sol : *old_solutions ){
|
||||||
if( jmg->distance(joint_positions, sol.data()) < 0.1 ){
|
if( jmg->distance(joint_positions, sol.data()) < 0.1 ){
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if(ignore_collisions)
|
||||||
|
return true;
|
||||||
|
|
||||||
state->setJointGroupPositions(jmg, joint_positions);
|
state->setJointGroupPositions(jmg, joint_positions);
|
||||||
state->update();
|
state->update();
|
||||||
if( scene->isStateColliding(*state, jmg->getName()) ){
|
if( scene->isStateColliding(*state, jmg->getName()) ){
|
||||||
@ -107,6 +117,8 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
|||||||
? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first)
|
? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first)
|
||||||
: grasp_state.getJointModelGroup(group_);
|
: grasp_state.getJointModelGroup(group_);
|
||||||
|
|
||||||
|
const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_;
|
||||||
|
|
||||||
// empty trajectory -> this subtask only produces states
|
// empty trajectory -> this subtask only produces states
|
||||||
const robot_trajectory::RobotTrajectoryPtr traj;
|
const robot_trajectory::RobotTrajectoryPtr traj;
|
||||||
|
|
||||||
@ -118,6 +130,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
|||||||
std::bind(
|
std::bind(
|
||||||
&isValid,
|
&isValid,
|
||||||
scene_,
|
scene_,
|
||||||
|
ignore_collisions_,
|
||||||
&previous_solutions_,
|
&previous_solutions_,
|
||||||
std::placeholders::_1,
|
std::placeholders::_1,
|
||||||
std::placeholders::_2,
|
std::placeholders::_2,
|
||||||
@ -154,7 +167,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
|||||||
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_active, grasp_pose, jmg_eef->getEndEffectorParentGroup().second, 1, remaining_time_, is_valid);
|
bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, ik_link, 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){
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user