Planning with an yet to be attached object

edit

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<grasp> 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