Ask Your Question
0

Planning with an yet to be attached object

asked 2015-03-13 08:22:18 -0500

Simon Schmeisser gravatar image

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-11-06 10:26:53 -0500

Simon Schmeisser gravatar image

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

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-03-13 08:22:18 -0500

Seen: 830 times

Last updated: Nov 06 '16