Several things happened in this experiment, so let's discuss it in parts!
First of all, we saw a couple of cubic and rectangular blocks appearing in the RViz. Well, those were the table and the object to be grasped, of course! Inside the pick_and_place.py script, we created these two objects and added them into the MoveIt planning scene. Whenever we add an object to the planning scene, it will appear in this way.
Second, the pick action starts. After getting the grasp object position, our node sends this position to the grasp server, which will generate IK and check if there's any valid IK in order to pick the object up. If it finds any feasible IK solution, the arm will begin to execute the specified motions in order to pick the object up.
After the object is picked by the gripper, the place action starts. Again, our node will send a pose to the grasp server where the arm should place the object. Then, the server will check for a valid IK solution for that specified pose. If it finds any valid solution, the gripper will move to that position and release the object.
We can also have a look at the /grasp and /place topics in order to better see what's going on.
Ok, so now we have a better understanding of what happened in the previous exercise. Let's take a deeper look into the code in order to see how all of this works.
First of all, let's check how we created and added both the table and the grasping object:
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
#Table position
p.pose.position.x = 0.45
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
In this section of the code, we are creating and adding a table to the planning scene.
Here, we first create a PoseStamped message, and we fill it in with the necessary information—the frame_id, the time stamp, and the position and orientation where the table will be placed. Then, we add the table to the scene using the add_box function.
The following code is the creation of the grasp object:
def _add_grasp_block_(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0.05
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Grasp Object can size
self._scene.add_box(name, p, (0.03, 0.03, 0.09))
return p.pose
In the preceding section of the code, we are doing the exact same thing as we did with the table, but for the grasp object.
After creating the grasp object and the grasp table, we will see how to set the pick position and the place position from the following code snippet. Here, the pose of the grasp object created in the planning scene is retrieved and fed into the place pose in which the Y axis of the place pose is decreased by 0.06. So, when the pick and place happens, the grasp object will be placed 0.06 meters (6 cm) away from the initial pose of the object in the direction of Y:
# Add table and grap object to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_grasp_obj = self._add_grasp_block_(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_grasp_obj.position.x self._pose_place.position.y = self._pose_grasp_obj.position.y - 0.06 self._pose_place.position.z = self._pose_grasp_obj.position.z self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
The next step is to generate the grasp pose array data for visualization, and then send the grasp goal to the grasp server. If there is a grasp sequence, it will be published; otherwise, it will respond with an error:
def _generate_grasps(self, pose, width):
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
......................
.......................
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
This function will create a pose array data for the pose of the place:
def _generate_places(self, target):
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
............................................
.......................................
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
The next function is _create_pickup_goal(), which will create a goal for picking up the grasping object. This goal has to be sent into MoveIt!:
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
Also, there is the _create_place_goal() function, which creates a place goal for MoveIt:
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
The important functions that are performing picking and placing are as follows. These functions will generate a pick and place sequence, which will be sent to MoveIt, and print the results of the motion planning, regardless of whether it is successful or not:
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True