Pick and Place pre_grasp_posture

[Tiago Pimentel]

Good afternoon,

I am trying to pick an object using MoveIt and I've been able to set everything so far, but when I set pre_grasp_posture's and grasp_posture's position it is not sent through the messages to the gripper controller. I have my gripper controller listenning to a message control_msgs/FollowJointTrajectoryActionGoal but the position always comes as 0.0, even though I change its value.
Does anyone know why?

Thank you,
Have a great day,
Tiago Pimentel

Any questions just ask me please.

[Ioan Sucan]

Hello Tiago,

Have you defined the joint names for your gripper controller? Which joints get sent to your controller also depends on the set of joints the controller declares it can operate on.

Ioan

[Steffen P]

Tiago,

I had the same problem and ended up changing approach_and_translate_state.cpp and plan_stage.cpp in moveit_core.

!!!warning ugly hack!!!

See the last commits from there: https://github.com/fivef/moveit_ros/commits/pickup

Just replace the katana_l_finger_joint with your joint names.

Currently working on a more general hack ;-)

Steffen

[Tiago Pimentel]

Good morning,

Thanks for the answers. Ioan, I've set  the controller like this:

  - name: "/sdk/robot/gripper/left"
    action_ns: gripper_action
    default: true
    joints:
      - left_hand
      - left_hand_camera
      - left_hand_camera_axis
      - left_hand_range
      - left_endpoint
      - left_gripper_r_finger
      - left_gripper_l_finger

And I'm changing the position of the left_gripper_r_finger joint when using the pick operation. In the pick operation I'm setting the pre_grasp_posture and grasp_posture using the following commands:

  g.pre_grasp_posture.header.frame_id = "/base";
  g.pre_grasp_posture.header.stamp = ros::Time::now();
  g.pre_grasp_posture.name.resize(1, "open_left_endpoint");
  g.pre_grasp_posture.position.resize(1);
  g.pre_grasp_posture.position[0] = 1.0;

  g.grasp_posture.name.resize(1, "close_left_endpoint");
  g.grasp_posture.position.resize(1);
  g.grasp_posture.position[0] = 0.0;

Being 1.0 the gripper open and 0.0 closed. But what gets published in /sdk/robot/gripper/left/gripper_action is:

header:
  seq: 0
  stamp:
    secs: 1374500036
    nsecs: 638497437
  frame_id: ''
goal_id:
  stamp:
    secs: 1374500036
    nsecs: 638498548
  id: /move_group-5-1374500036.638498548
goal:
  trajectory:
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs: 0
      frame_id: /base
    joint_names: ['left_gripper_r_finger']
    points:
      -
        positions: [0.0]
        velocities: [0.0]
        accelerations: [0.0]
        time_from_start:
          secs: 7
          nsecs: 0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance:
    secs: 0
    nsecs: 0
---
header:
  seq: 1
  stamp:
    secs: 1374500038
    nsecs: 470108194
  frame_id: ''
goal_id:
  stamp:
    secs: 1374500038
    nsecs: 470108908
  id: /move_group-7-1374500038.470108908
goal:
  trajectory:
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs: 0
      frame_id: /base
    joint_names: ['left_gripper_r_finger']
    points:
      -
        positions: [0.0]
        velocities: [0.0]
        accelerations: [0.0]
        time_from_start:
          secs: 7
          nsecs: 0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance:
    secs: 0
    nsecs: 0

The two times with the position as 0.0. Do you know what i'm doing wrong in here? Or do I need to do a hack like Steffen sugested?

And thanks Steffen, I wanted to try to not to have to do any hacks, but I'll have a look at what you've done!

Thank you,
Tiago Pimentel

[Tiago Pimentel]

Hey,

I just found what I was doing wrong! These two lines:
  g.pre_grasp_posture.name.resize(1, "open_left_endpoint");

...
  g.grasp_posture.name.resize(1, "close_left_endpoint");

Should actually be:
  g.pre_grasp_posture.name.resize(1, "left_gripper_r_finger");
...
  g.grasp_posture.name.resize(1, "left_gripper_r_finger");

It was a kind of stupid mistake =P After changing this everything worked fine!

Thank you for all the help,
Tiago Pimentel

On Friday, July 19, 2013 4:22:14 PM UTC-5, Tiago Pimentel wrote:

[Tiago Pimentel]

I'm having another problem now. I want to pick an object that I didn't add to the colision world in MoveIt, but I need to pass an string with the object name to the pick operation. When I gave it an empty string it prints this error:

[ INFO] [1374511892.459251625]: Planning attempt 1 of at most 1
[ INFO] [1374511892.459634401]: Added plan for pipeline 'pick'. Queue is now of size 1
[ERROR] [1374511892.538832500]: Attempting to attach object '' to link 'left_wrist' but no geometry specified and such an object does not exist in the collision world
[ INFO] [1374511892.571481011]: Starting with 1 states
[ INFO] [1374511892.593849010]: Created 64 (54 start + 10 goal) states in 56 cells (51 start (51 on boundary) + 5 goal (5 on boundary))
[ INFO] [1374511892.593949055]: Solution found in 0.023213 seconds
[ INFO] [1374511892.597074094]: Path simplification took 0.002952 seconds
[ INFO] [1374511892.622171539]: Found successful manipulation plan!
[ INFO] [1374511892.622391968]: Pickup completed after 0.163052 seconds
[ERROR] [1374511896.419397013]: Attempting to attach object '' to link 'left_wrist' but no geometry specified and such an object does not exist in the collision world
[ERROR] [1374511896.419464519]: Execution of path-completion side-effect failed. Preempting.
[ INFO] [1374511896.430038640]: Stopping execution due to preempt request
[ INFO] [1374511896.430115659]: Cancelling execution for /sdk/robot/limb/left
[ INFO] [1374511896.430212276]: Stopped trajectory execution.

And then the operation never returns. Is there anyway to call the operation with no object to attach or should I create a "virtual" object to attach? And, is there any way to unattach an object without using the place operation.

Thank you,

Tiago Pimentel

On Friday, July 19, 2013 4:22:14 PM UTC-5, Tiago Pimentel wrote:

[Ioan Sucan]

Hello Tiago,

Glad to hear your initial problem got resolved!

The pick() operation needs to go somewhere and it assumes it needs to do an attach operation once the object is picked. There is no way to pick an object that does not exist -- you need to first add it to the collision world (e.g., publish a CollisionObject message).

If you know there will be an object at the pose you want to get to, and do not want to attach it, then use the plan() and computeCartesianPath() functions directly, instead of pick.

The API you are using does not have an option to unattach an object that was previously attached. That said, functions for attaching / detaching objects can be added relatively easily. Create a ticket for this perhaps? I think I can get to it soon.

Ioan

[Tiago Pimentel]

Good afternoon,

Thanks for all the help Ioan! I decided to continue using pick and instead of just releasing the object manually I'll use the place operation, but now I got a new problem.
After I call the pick operation, it runs normally and executes corretly, both in the software as in the real robot, the problem is only that it never returns. I'm calling it like this:

  // executes pick operation
  grasps.push_back(g);
  group.setSupportSurfaceName("table");
  ROS_INFO("Pick object");
  group.pick("seenObject", grasps);
  ROS_INFO("End of pick object");

And "Pick object" is printed, but not "End of pick object". I was having the same problem before, but I figured it was because I had deleted the asynchronous spinner and when I put it back it went back to working.

  ros::init (argc, argv, "right_arm_pick_place");
  ros::AsyncSpinner spinner(1);
  spinner.start();

But now it is there and the program is still never returning.
The messages printed by move_group are:

[ INFO] [1374614138.789213043]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1374614138.789577349]: Planning attempt 1 of at most 5
[ INFO] [1374614138.796138756]: No planner specified. Using default.
[ INFO] [1374614138.796523882]: Attempting to use default projection.
[ INFO] [1374614138.797889733]: Starting with 1 states
[ INFO] [1374614138.837896320]: Created 174 (159 start + 15 goal) states in 130 cells (126 start (118 on boundary) + 4 goal (4 on boundary))
[ INFO] [1374614138.837980860]: Solution found in 0.041124 seconds
[ INFO] [1374614138.842515527]: Path simplification took 0.004352 seconds
[ INFO] [1374614150.535449210]: Planning attempt 1 of at most 1
[ INFO] [1374614150.536392867]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1374614150.557447563]: Attaching world object 'seenObject' to link 'left_wrist'
[ INFO] [1374614150.557599549]: Attached object 'seenObject' to link 'left_wrist'
[ INFO] [1374614150.598414695]: No planner specified. Using default.
[ INFO] [1374614150.598528037]: Attempting to use default projection.
[ INFO] [1374614150.599577697]: Starting with 1 states
[ INFO] [1374614150.629173533]: Created 30 (14 start + 16 goal) states in 26 cells (13 start (13 on boundary) + 13 goal (13 on boundary))
[ INFO] [1374614150.629286932]: Solution found in 0.030609 seconds
[ INFO] [1374614150.635742448]: Path simplification took 0.006305 seconds
[ INFO] [1374614150.671570405]: Found successful manipulation plan!
[ INFO] [1374614150.671787928]: Pickup completed after 0.136097 seconds
[ INFO] [1374614160.032403872]: Attaching world object 'seenObject' to link 'left_wrist'
[ INFO] [1374614160.032556590]: Attached object 'seenObject' to link 'left_wrist'

Thank you,
Tiago Pimentel


On Friday, July 19, 2013 4:22:14 PM UTC-5, Tiago Pimentel wrote:

[Ioan Sucan]

This sounds like a bug. Is there a way for me to run this?

[Tiago Pimentel]

I tried setting a move command where the pick command was and it also doesn't return, but if I put the move command in the main function it returns normally. My code is like this:


// ros
#include <ros/ros.h>

// MoveIt!
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/move_group_interface/move_group.h>
#include <shape_tools/solid_primitive_dims.h>

// transform library
#include <tf/tf.h>

#include <baxter_controller_communication/ObjectPosition.h>

static const std::string ROBOT_DESCRIPTION="robot_description";
bool IN_POSITION = false;

// callback of object recognized messages
void objectMoveCallback(const baxter_controller_communication::ObjectPosition::ConstPtr& msg, const ros::Publisher& collisionObject_pub, moveit::planning_interface::MoveGroup *group)
{
  // checks if robot is in home position and, if not, does nothing
  if (!IN_POSITION)
    return;

  // sets in home position as false
  IN_POSITION = false;
  ROS_INFO("Position:\tx: %lf\n\t\ty: %lf\n\t\tz: %lf", msg->position.x, msg->position.y, msg->position.z);
  ROS_INFO("Size:\tw: %lf\n\t\tl: %lf", msg->size.width, msg->size.length);

  // counts number of objects picked
  static int count = 0;
  count++;

  // sends robot arm to drop position
  // this is where the pick function would be ---------------------------------------
  ROS_DEBUG("Going to drop position!");
  group->setNamedTarget("drop_left");
  group->move();
  ROS_DEBUG("/Going to drop position!");
}

int main(int argc, char **argv)
{
  // starts ros with a asyncronous spinner (needed for pick operations to return with success or failure)
  ros::init (argc, argv, "left_arm_pick_place");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // set verbosity/logging level to debug
  log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
  my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);

  // starts node handling and messages subscribing and publishing
  ros::NodeHandle nh;

  // subscribes to object recognition messages
  // starts left arm as planning group
  moveit::planning_interface::MoveGroup *group = new moveit::planning_interface::MoveGroup("left_arm");
  group->allowReplanning(true);
  group->setPlanningTime(195.0);

  ros::WallDuration(1.0).sleep();

  ros::Subscriber sub = nh.subscribe<baxter_controller_communication::ObjectPosition>("/roscv/left_hand_camera/objectPose", 1000, boost::bind(&objectMoveCallback, _1, collisionObject_pub, group));

  // wait a moment
  ros::WallDuration(1.0).sleep();

  // sends robot arm to home position for object identification
  ROS_DEBUG("Going to home position!");
  group->setNamedTarget("home_left");
  group->move();
  ROS_DEBUG("/Going to home position!");

  // waits for a period of time to be sure everything has started, sets robot as in home position
  ros::WallDuration(2.0).sleep();
  IN_POSITION = true;

  // announces initialization is complete, wait wuns program while waits for shutdown
  ROS_INFO("Initialization complete!");
  ros::waitForShutdown();

  // returns success
  return 0;
}

The messages that get printed are:

[ INFO] [1374675664.614413482]: Ready to take MoveGroup commands for group left_arm.
[ INFO] [1374675664.614536410]: Replanning: yes
[DEBUG] [1374675670.619164547]: Going to home position!
[DEBUG] [1374675675.264773581]: /Going to home position!
[ INFO] [1374675677.264948128]: Initialization complete!
[ INFO] [1374675677.462830472]: Position:    x: 0.791429
        y: 0.409189
        z: -0.180000
[ INFO] [1374675677.462903609]: Size:    w: 0.106153
        l: 0.132385
[DEBUG] [1374675679.463419504]: Going to drop position!

So it goes to home position and returns from the movement call, but when sent to drop the program stops forever. I think the problem is that, when the command is inside a callback function, it is not returning, but I don't know why.

Thank you for all the help,

Tiago Pimentel

On Friday, July 19, 2013 4:22:14 PM UTC-5, Tiago Pimentel wrote:

[Tiago Pimentel]

Good afternoon,

I think that's probably the problem, the functions can't be called from inside a callback function, so I took them out of there and now the program works fine! The robot is able to pick up objects and place them in a container correctly.
When I set constraints to the place action it takes too long to calculate, but if I give it no constraints it's pretty fast.

Thank you for all the help,
Tiago Pimentel

Ps: For anyone that is interested, async movements work fine if called from callback functions, only the ones that block while executing have a problem.

On Friday, July 19, 2013 4:22:14 PM UTC-5, Tiago Pimentel wrote:

[Longfei Zhao]

Hello Tiago,

Do you mean that "group.pick()" should not be called in the callback function ( here for example: objectMoveCallback) ? Would you mind to post the final code which works fine please? Thanks very much!