Planning with an yet to be attached object

Question

I'm trying to implement a classical bin picking using Moveit.

So the problem I'm trying to solve at the moment is to find out which ones of several available boxes or objects can be grasped and transported away without collisions. I have a list of possible grasp positions and objects and search for which one there is

(a) a path for the empty graber towards the object and

(b) a path for the graber and the attached object towards a save position above the box

So now I am wondering how to tell the MoveGroup interface that I want to temporarily attach the object to the arm for planning. As far as I understand there is MoveGroup::setStartState(RobotState) and then I should be able to somehow attach the object and use MoveGroup::plan() for further planning.

So how do I

1. Construct the correct RobotState given an MoveGroup::Plan/RobotTrajectory from which I want to use the end state
2. How do I attach the box?

My current Code so far:

std::vector possibleGrasps = provider.getPossibleGrasps();

Eigen::Affine3d preApproach;
preApproach = Eigen::Translation3d(Eigen::Vector3d(1.1, 0, 1.0)) * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
group.setPoseTarget(preApproach);
group.move();

bool anyPathFound = false;

moveit_msgs::CollisionObject collision_object;
moveit::planning_interface::MoveGroup::Plan plan;

visual_tools_->setLifetime(20);


BOOST_FOREACH(Grasp grasp, possibleGrasps)
{
      group.setPoseTarget(grasp.graspPosition);

      visual_tools_->publishAxis(grasp.graspPosition);

      moveit::planning_interface::MoveItErrorCode error = group.plan(plan);
            
      if (error != moveit_msgs::MoveItErrorCodes::SUCCESS)
      {
             continue;
      }
            
      group.setStartState(plan.);

      // Object andocken (an RobotState) und weiteren Weg planen
      
      if (error == moveit_msgs::MoveItErrorCodes::SUCCESS)
       {
             anyPathFound = true;
             collision_object = grasp.object;
             break;
        }
 }

if (!anyPathFound) {
      ROS_ERROR("Could not find any valid pick");
      break;
 }

group.execute(plan);

provider.claimObject(collision_object.id);

ROS_INFO("Attach the object to the robot");
group.attachObject(collision_object.id);
/* Sleep to give Rviz time to show the object attached (different color). */
sleep(0.5);
    
        
Eigen::Affine3d approach_place = tower[box];
approach_place.translate(-0.33 * Eigen::Vector3d::UnitZ());

group.setPoseTarget(approach_place);
group.move();
group.setPoseTarget(tower[box]);
bool placed = group.move();
ros::Duration(0.5).sleep(); // sleep for half a second
    
ROS_INFO("Detach the object from the robot");
group.detachObject(collision_object.id);
/* Sleep to give Rviz time to show the object detached. */
sleep(0.5);

I'm using ros Indigo on Kubuntu 14.04 and the included moveit

---

Originally posted by Simon Schmeisser on ROS Answers with karma: 229 on 2015-03-13

Post score: 0

Answer 1

Here is a short sketch how to do it:

Get a PlanningScene (by using LockedPlanningScene from PlanningSceneUpdater)

diff and detachFromParent (necessary?) to not spoil your "real" PlanningScene

Now you can use all of the api of PlanningScene and RobotState to check for collisions etc. Most of the trajectory planning code also allows passing in a PlanningScene to operate on

---

Originally posted by Simon Schmeisser with karma: 229 on 2016-11-06

This answer was ACCEPTED on the original site

Post score: 0