updating example code documentation

Exercises/catkin_ws/src/pick_and_place/scripts/pick_place_example.py

@@ -78,20 +78,20 @@ def add_items(scene, frame):
         Returns the items that can be picked as an array.
     '''
     rospy.loginfo("-- Adding Objects --")
-    height_from_ground = 0.0 
+    height_from_ground = 0.0 #change this to move the table up or down
 
     #Define the objects
     table1 = create_object(obj_id="table1", ref_frame=frame, 
-                            dimensions=[0.2, 0.5, 0.01], 
-                            pose=[0.3, 0.0, height_from_ground, 0.0, 0.0, 0.0],)
+                            dimensions=[0.1, 0.5, 0.01], 
+                            pose=[0.2, 0.0, height_from_ground, 0.0, 0.0, 0.0],)
 
     table2 = create_object(obj_id="table2", ref_frame=frame, 
-                            dimensions=[0.2, 0.5, 0.01], 
-                            pose=[-0.3, 0.0, height_from_ground, 0.0, 0.0, 0.0])
+                            dimensions=[0.1, 0.5, 0.01], 
+                            pose=[-0.2, 0.0, height_from_ground, 0.0, 0.0, 0.0])
 
     box1 = create_object(obj_id="box1", ref_frame=frame, 
-                         dimensions=[0.1, 0.025, 0.025], 
-                         pose=[0.3, 0, height_from_ground+(0.05/2.0), 0.0, 0.0, 0.0])
+                         dimensions=[0.05, 0.025, 0.025], 
+                         pose=[0.2, 0, height_from_ground+(0.05/2.0), 0.0, 0.0, 0.0])
 
     #Add Objects to the scene
     scene.add_object(table1)
@@ -139,7 +139,6 @@ def gripper_action(gripper_end, action="open"):
         set_gripper_position(gripper_end,"gripper_open")
     elif(action == "close"):
         set_gripper_position(gripper_end,"gripper_close")
-
     else:
         print("Action undefined") 
 
@@ -156,17 +155,13 @@ def move_to_pose(arm, goal_pose, goal_angle):
         None
     '''
     #NOTE:
-    #The planner will create a plan for the panda_link8 to reach
-    #the goal pose. But since we have an end-endeffector attached to it
-    #it will cause collision.
-
-    #distance between wrist(panda_link8) and gripper_end(panda_hand_tcp)
-    wrist_to_tcp = 0.103
+    #The planner will create a plan for the gripper_center to reach
+    #the goal pose.
 
     # rospy.logwarn("planning and moving to location")
     gripper_angle = geometry_msgs.msg.Quaternion()
 
-    #Orientation of the tcp(world) w.r.t base frame(gripper_center) 
+    #Orientation of the tcp(gripper_center) w.r.t base frame(world) 
     rpy = [math.radians(goal_angle[0]), math.radians(goal_angle[1]), math.radians(goal_angle[2])]
     quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
 
@@ -186,7 +181,7 @@ def move_to_pose(arm, goal_pose, goal_angle):
             .format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z, 
                     goal_angle[0], goal_angle[1], goal_angle[2]))
     
-    arm.set_goal_position_tolerance(0.1) #setting tolerence for pose 
+    arm.set_goal_position_tolerance(0.01) #setting tolerence for pose 
     arm.set_pose_target(grasp_pose) #setting the pose of the end-effector
 
     plan = arm.go(wait=True) #move the arm to the grasp_pose.
@@ -262,27 +257,27 @@ def main():
     #Adding objects to the scene
     add_items(scene, planning_frame)
     #Simulating Gripper actions
-    # rospy.loginfo("------------------")    
+    rospy.loginfo("------------------")    
     
-    # rospy.loginfo("Closing Gripper")
-    # gripper_action(gripper, action="close")
+    rospy.loginfo("Closing Gripper")
+    gripper_action(gripper, action="close")
     
-    # rospy.loginfo("Openning Gripper")
-    # gripper_action(gripper, action="open")
+    rospy.loginfo("Openning Gripper")
+    gripper_action(gripper, action="open")
 
-    # rospy.loginfo("------------------")
+    rospy.loginfo("------------------")
     
     rospy.loginfo("Moving to pose1")
     
-    position1 = [0.321, -0.034, 0.118]
-    orientation1 = [-90.038, 27.682, -5.997]
+    position1 = [0.231, -0.022, 0.103]
+    orientation1 = [-90.174, 41.199, -5.554]
     move_to_pose(arm, position1, orientation1) #sending goal pose
 
     rospy.loginfo("Moving to pose2")
     position2 = position1 #copying the position from pose1
-    orientation2 = [-89.934, 27.686, 174.061] #copying the orientation from pose1
+    orientation2 = [-89.870, 41.196, 173.669] #copying the orientation from pose1
     position2[0] = -1*position2[0] #creating a different x value
-    position2[1] = -1*position2[1] #creating a different x value
+    position2[1] = -1*position2[1] #creating a different y value
     move_to_pose(arm, position2, orientation2) #sending the goal pose
     
 if __name__ == '__main__':