Updating pick_place example code

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

@@ -12,6 +12,7 @@ from  geometry_msgs.msg import Pose
 from shape_msgs.msg import SolidPrimitive
 from moveit_msgs.msg import *
 import tf 
+import actionlib
 
 def create_object(obj_id, ref_frame, dimensions, pose):
     '''
@@ -76,20 +77,20 @@ def add_items(scene, frame):
         Returns the items that can be picked as an array.
     '''
     rospy.loginfo("-- Adding Objects --")
-    height_from_ground = 0.3 
+    height_from_ground = 0.0 
 
     #Define the objects
     table1 = create_object(obj_id="table1", ref_frame=frame, 
                             dimensions=[0.2, 0.5, 0.01], 
-                            pose=[0.5, 0, height_from_ground, 0.0, 0.0, 0.0])
+                            pose=[0.3, 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.5, 0, height_from_ground, 0.0, 0.0, 0.0])
+                            pose=[-0.3, 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.5, 0, height_from_ground+(0.05/2.0), 0.0, 0.0, 0.0])
+                         pose=[0.3, 0, height_from_ground+(0.05/2.0), 0.0, 0.0, 0.0])
 
     #Add Objects to the scene
     scene.add_object(table1)
@@ -97,6 +98,28 @@ def add_items(scene, frame):
     scene.add_object(box1)
     rospy.sleep(2) #Waiting to spawn the objects in the world
 
+def set_gripper_position(gripper, gripper_action):
+    '''
+        Function to set named position to open and close gripper
+    Args:
+        gripper:        group_name of gripper in the manipulator
+
+        gripper_action: action to perform. Allowed values ["gripper_open", "gripper_close"]
+
+    Returns:
+        None.
+    '''
+    execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)
+    execute_trajectory_client.wait_for_server()
+    
+    gripper.set_named_target(gripper_action)
+    plan_success, plan, planning_time, error_code = gripper.plan()
+    goal=ExecuteTrajectoryGoal()
+    goal.trajectory = plan
+    execute_trajectory_client.send_goal(goal)
+    execute_trajectory_client.wait_for_result()
+    rospy.loginfo('\033[32m' + "Now at: {}".format(gripper_action) + '\033[0m')
+
 def gripper_action(gripper_end, action="open"):
     '''
     Function to execute the required action of the gripper.
@@ -112,13 +135,14 @@ def gripper_action(gripper_end, action="open"):
     print("Executing gripper action = ", action)
     
     if(action == "open"):
-        gripper_end.move(gripper_end.max_bound(), True)
+        set_gripper_position(gripper_end,"gripper_open")
     elif(action == "close"):
-        gripper_end.move(gripper_end.max_bound()*0.5, True)
+        set_gripper_position(gripper_end,"gripper_close")
+
     else:
         print("Action undefined") 
 
-def move_to_pose(arm, goal_pose):
+def move_to_pose(arm, goal_pose, goal_yaw):
     '''
     Function to move the robot arm to the requested pose
 
@@ -141,8 +165,16 @@ def move_to_pose(arm, goal_pose):
     # rospy.logwarn("planning and moving to location")
     gripper_angle = geometry_msgs.msg.Quaternion()
 
-    #Orientation of the tcp(panda_hand_tcp) w.r.t base frame(panda_link0) 
-    quaternion = tf.transformations.quaternion_from_euler(math.radians(180), 0, math.radians(-45))
+    #Orientation of the tcp(world) w.r.t base frame(gripper_center) 
+    # rpy = [math.radians(149.22), math.radians(-89.87), math.radians(goal_yaw)]
+    rpy_deg = [-90.019, 0.025, 173.970]
+    rpy = [math.radians(rpy_deg[0]), math.radians(rpy_deg[1]), math.radians(rpy_deg[2])]
+    quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
+    # quaternion = tf.transformations.quaternion_from_euler(math.radians(125.472), 
+    #                 math.radians(14.218), math.radians(138.266))
+    # quaternion = tf.transformations.quaternion_from_euler(math.radians(125.472), 
+    #                 math.radians(14.218), math.radians(15.245))
+
 
     gripper_angle.x = quaternion[0]
     gripper_angle.y = quaternion[1]
@@ -154,7 +186,7 @@ def move_to_pose(arm, goal_pose):
     grasp_pose.position.x = goal_pose.position.x
     grasp_pose.position.y = goal_pose.position.y
     #adding tcp distance because the planning frame is panda_link8
-    grasp_pose.position.z = goal_pose.position.z + wrist_to_tcp  
+    grasp_pose.position.z = goal_pose.position.z 
     grasp_pose.orientation = gripper_angle
     
     print("Exectuting move_to_pose ({} , {},  {})"
@@ -188,11 +220,11 @@ def initialize_robot_params():
     rospy.loginfo("--- MoveIt APIs initialising ---")
     scene = moveit_commander.PlanningSceneInterface() 
     robot_commander = moveit_commander.RobotCommander()
-    gripper = robot_commander.get_joint('panda_finger_joint1')
+    gripper = moveit_commander.MoveGroupCommander("gripper_group")
 
     rospy.sleep(3) #essential to intialize the API's
 
-    group_name = "panda_arm" #group name of the robot arm we are using
+    group_name = "arm_group" #group name of the robot arm we are using
    
     move_group = moveit_commander.MoveGroupCommander(group_name)
     move_group.set_goal_position_tolerance(1E-2)
@@ -236,27 +268,32 @@ 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")
     pose1 = Pose()
-    pose1.position.x = 0.5 #x positin in base frame
-    pose1.position.y = 0.0
-    pose1.position.z = 0.5
-    move_to_pose(arm, pose1) #sending goal pose
+    # [-0.152, 0.135, 0.233]
+    # pose1.position.x = -0.152 #x positin in base frame
+    # pose1.position.y = 0.135
+    # pose1.position.z = 0.233
+    t = [0.024, 0.227, 0.233]
+    pose1.position.x = t[0] #x positin in base frame
+    pose1.position.y = t[1]
+    pose1.position.z = t[2]
+    move_to_pose(arm, pose1, goal_yaw=2) #sending goal pose
 
     rospy.loginfo("Moving to pose2")
     pose2 = copy.deepcopy(pose1) #making a copy of the pose
     pose2.position.x += -1 #creating a different x value
-    move_to_pose(arm, pose2) #sending the goal pose
+    move_to_pose(arm, pose2, 178) #sending the goal pose
     
 if __name__ == '__main__':
     #starting point of the code