Improve pick and place in Gazebo

Because the robot arm and gripper still sometimes cannot grasp the object, I spent some more time to try to improve it. Some works I have done are listed below. The main challenge is that the fail pick and place doesn’t happen each time, so I need to test multiple times to check does it truly works better after some modification. Sometime, even if it successfully finishs the task(pick and place three objects) for over three times which seems promising, it might fails after a while, so it is kind of hard for me to evaluate the result of modification without a benchmark testing tool.

Change controller parameters

I mainly changed three kinds of parameters in ros controller: goal_time, stopped_velocity_tolerance, trajectory and goal tolerance. The most useful one is to change the trajectory and goal tolerance for each joints.

Because I can always see some warnings as below, it seems that the controller violated the goal tolerence a lot, so I increased the goal tolerance for each joints at first, but the robot worked even worse after this modification and the warnings kept coming out with a larger goal tolerance. However, with a smaller tolerance, though the warning is still spawning, it actually provides a better start pose for next motion.

[ WARN]: Controller  failed with error GOAL_TOLERANCE_VIOLATED: 
[ WARN]: Controller handle  reports status ABORTED

[ WARN]: Fail: ABORTED: Solution found but controller failed during execution

Add multiple grasp messages to pickup() function

For box object, I specified four possible grasp poses with 90 degree step in yaw. For sphere and cylinder objects, I specified 12 possible grasps with 30 degree step in yaw. After this modification, the pickup() function can choose the one with highest success rate to execute.

Provide more time for the gripper to grasp the object

Sometimes the gripper seems to stop after moving to the grasping position. However, when I zoom in to the gripper, I found that it is moving but in a very slow speed, so giving it more time to close the gripper can also contribute to stability.

Change planner in Moveit

The default planner in MoveIt is RRT. There are 23 palnning methods can be choosed in the Open Motion Planning Library(OMPL). I could not see a obvious difference between the trajectory different planner get, so the original one is kept.

rqt_kinematics plugin

The GUI I implemented is modified into a rqt plugin, so it can be loaded and shown together with other rqt plugins. All the button is usable now. With this GUI tool, we can easily debug and play with the robot.

Preparation of the first exercise

World in Gazebo

A table and container are added into the world file. All the objects are placed on the table and will be picked and placed to the container. The table can be changed into the conveyor. ARIAC provides a conveyor model, but it requires some modifications in collision model because when I put some objects on it, they will direcly drop to the ground.

first version API

Currently, I provides following API for the first exercise.

Object Class:

Object information are packaged in Object class. They are stored in object_list and can be accessed by providing the object name.

  • pose
  • height
  • width
  • shape
  • color

Pick_Place Class

With integrated functions, users can quickly implement a solution to pick and place objects by providing their name.

Integrated Solution

  • self.get_object_pose(object_name)
  • self.pickup(object_name, pose)
  • self.place(pose)
  • self.back_to_home()

If the user wants to understand the pipeline better, some basic functions are also provided. With them and move_group API, they can also finish the pick and place task.

Basic functions

  • self.get_object_width(object_name)
  • self.move_pose_arm(pose)
  • self.move_joint_arm(pose)
  • self.move_joint_hand(joint_value)
  • self.generate_grasps(object_name, pose)
  • self.pose2msg(roll, pitch, yaw, x, y, z)
  • self.msg2pose(pose)

move_group API:

  • self.arm.stop()
  • self.arm.set_pose_target(pose)
  • self.arm.go()
  • self.arm.pick(object_name, grasp)
  • self.arm.place(placelocation)
    More API for movegroup can be found here.