mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fix implicit copy
It seems (not verified) the vector is copied here by bind. Instead, let's pass a pointer.
This commit is contained in:
parent
798f9c0839
commit
c3c6d899cb
@ -62,15 +62,16 @@ moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
|
||||
|
||||
namespace {
|
||||
bool isValid(planning_scene::PlanningSceneConstPtr scene,
|
||||
std::vector< std::vector<double> >& old_solutions,
|
||||
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 ){
|
||||
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());
|
||||
@ -91,12 +92,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
||||
std::bind(
|
||||
&isValid,
|
||||
scene_,
|
||||
previous_solutions_,
|
||||
&previous_solutions_,
|
||||
std::placeholders::_1,
|
||||
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())
|
||||
|
||||
Loading…
Reference in New Issue
Block a user