Semi working machine.py

Author: ykarmesh

scripts/controller.py

@@ -30,8 +30,9 @@ def __init__(self):
         self.ik_solver = IK(self.BOTTOM_PLATE_LINK, self.GRIPPER_LINK)
 
         # predefined variables
+        self.HOME_POS_MANIPULATOR_00 = [0.0, 0.0, 0.0, 0.0, 0.0]
         self.HOME_POS_MANIPULATOR_01 = [0.004601942375302315, -0.4218447208404541, 1.6260197162628174, -0.1426602154970169, 0.010737866163253784]
-        self.HOME_POS_MANIPULATOR_02 = [0.0, 0.0, 1.22, -0.142, 0.0]
+        self.HOME_POS_MANIPULATOR_02 = [0.0, 0.0, 1.22, -1.57, 1.57]
         self.HOME_POS_CAMERA_01 = [0.0, 0.698]
         self.HOME_POS_CAMERA_02 = [-0.523, -0.523]
         self.IK_POSITION_TOLERANCE = 0.01
@@ -40,20 +41,22 @@ def __init__(self):
 
     def set_camera_angles(self, angles):
         pan_msg = Float64()
-        pan_msg.data = angles[0]
+        pan_msg.data = float(angles[0])
         rospy.loginfo('Going to camera pan: {} rad'.format(angles[0]))
         self.pan_pub.publish(pan_msg)
 
         tilt_msg = Float64()
-        tilt_msg.data = angles[1]
+        tilt_msg.data = float(angles[1])
         rospy.loginfo('Going to camera tilt: {} rad'.format(angles[1]))
         self.tilt_pub.publish(tilt_msg)
         rospy.sleep(4)
 
     def set_arm_joint_angles(self, joint_target):
         joint_state = JointState()
         joint_state.position = tuple(joint_target)
+        rospy.loginfo('Going to arm joint 0: {} rad'.format(joint_target[0]))
         self.arm_pub.publish(joint_state)
+        rospy.sleep(4)
 
     def get_joint_state(self, data):
         self.current_joint_state = data.position[0:5]
@@ -106,7 +109,7 @@ def compute_ik(self, target_pose):
         return result
 
     # Goes to the position given by FRAME and grabs the object from the top
-    def go_to_pose(self, FRAME):
+    def go_to_grasp_pose(self, FRAME):
         try:
             self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time.now(), rospy.Duration(5.0))
             P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time(0))
@@ -141,7 +144,41 @@ def check_grasp(self):
         if self.current_gripper_state:
             if(np.abs(self.current_gripper_state[0]) < self.MIN_CLOSING_GAP \
                 and np.abs(self.current_gripper_state[1] < self.MIN_CLOSING_GAP)):
+                print("Coundn't grasp the object")
                 return False
+            else:
+                return True
         else:
             print("Joint State Subscriber not working")
             return False
+
+    # Goes to the position given by FRAME and grabs the object from the top
+    def go_to_handover_pose(self, pose):
+        # print(pose)
+        try:
+            self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, pose.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
+            P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, pose.header.frame_id, rospy.Time(0))
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
+            rospy.logwarn(e)
+            return False
+
+        O = tf.transformations.euler_from_quaternion(Q)
+        Q = tf.transformations.quaternion_from_euler(0, -np.pi/3, O[2])
+
+        poses = [Pose(Point(P[0], P[1], P[2]-0.45), Quaternion(Q[0], Q[1], Q[2], Q[3]))]
+
+        target_joint = None
+        while target_joint is not None:
+            if self.current_joint_state:
+                target_joint = self.compute_ik(pose)
+            else:
+                print("Joint State Subscriber not working")
+                return False
+
+            if target_joint:
+                self.set_arm_joint_angles(target_joint)
+                rospy.sleep(8)
+            else:
+                self.IK_POSITION_TOLERANCE += 0.1 
+        return True
+

scripts/ik_control.py

@@ -132,7 +132,7 @@ def main():
     # home_joint = [0.0, 0.0, 1.22, -0.142, 0.0]
     set_arm_joint(arm_pub, home_joint)
     close_gripper(gripper_close_pub)
-    set_camera_angles(pan_pub, tilt_pub, rad_from_deg*0., rad_from_deg*40.0) #, ck)
+    set_camera_angles(pan_pub, tilt_pub, rad_from_deg*0., rad_from_deg*20.0) #, ck)
 
     rospy.sleep(10)
 

scripts/machine.py

@@ -8,6 +8,7 @@
 from controller import Controller
 from geometry_msgs.msg import PoseStamped
 from dynamixel_workbench_msgs.srv import SetPID
+from object_detection.srv import ObjectDetection
 
 # Initialize controller
 ctrl = Controller()
@@ -18,13 +19,14 @@ def __init__(self):
 
     def execute(self, userdata):
         userdata.tool_id = -1
+        ctrl.open_gripper()
         ctrl.set_camera_angles(ctrl.HOME_POS_CAMERA_01)
         ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_01)
         rospy.loginfo('Executing state IDLE')
         while(True):
             # TODO: Ask for Input
             userdata.tool_id = input("Enter Tool ID you want:")
-            if(isinstance(userdata.tool_id, int) and userdata.tool_id>0 and userdata.tool_id<4):
+            if(isinstance(userdata.tool_id, int) and userdata.tool_id>0 and userdata.tool_id<5):
                 break
 
         # Return success
@@ -35,24 +37,35 @@ def execute(self, userdata):
 # define state FindTool
 class FindTool(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['foundTool'], input_keys=['tool_id'], output_keys=['frame_name'])
+        smach.State.__init__(self, outcomes=['foundTool', 'notfoundTool'], input_keys=['tool_id'], output_keys=['frame_name'])
 
     def execute(self, userdata):
         rospy.loginfo('Executing state FINDTOOL')
-        print(userdata.tool_id)
-        ## TODO: change the name of the variable frame_name
-        userdata.frame_name = "random"
-        return 'foundTool'
+        rospy.wait_for_service('detect_object')
+        try:
+            get_success = rospy.ServiceProxy('detect_object', ObjectDetection)
+            success = get_success(tool_id)
+            if success:
+                userdata.frame_name = "/icp_cuboid_frame"
+                return 'foundTool'
+            else:
+                rospy.logwarn("Service returned false")
+                return 'notfoundTool'
+        except: rospy.ServiceException as e:
+            rospy.logwarn("Service called failed:{0}".format(e))
+            return 'notfoundTool'
 
 # define state IK1
 class IK1(smach.State):
     def __init__(self):
         smach.State.__init__(self, outcomes=['noIK','foundIK'], input_keys=['frame_name'])
 
     def execute(self, userdata):
-        success = ctrl.go_to_pose(userdata.frame_name)
+        ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_00)
         rospy.loginfo('Executing state IK1')
+        success = ctrl.go_to_grasp_pose(userdata.frame_name)
         return 'foundIK' if success else 'noIK'
+        # return 'foundIK'
 
 # define state Move
 # class Move(smach.State):
@@ -66,7 +79,7 @@ def execute(self, userdata):
 # define state Grab
 class Grab(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['grabSuccess','grabFailure'])
+        smach.State.__init__(self, outcomes=['grabSuccess', 'grabFailure'])
 
     def execute(self, userdata):
         ctrl.close_gripper()
@@ -104,7 +117,7 @@ def __init__(self):
 
     def execute(self, userdata):
         ## Call service to get the frame id of the hand centroid
-        posestamped = rospy.wait_for_message("/openface2/head_pose", PoseStamped)
+        userdata.head_pose = rospy.wait_for_message("/openface2/head_pose", PoseStamped)
         rospy.loginfo('Executing state FindAttention')
         return 'giveTool'
 
@@ -117,7 +130,7 @@ def execute(self, userdata):
         ## Wait till IK not found. Change tolerance and call compute IK again
         ## Break go to pose into find pose and compute IK functions
         ## Make a different function with gripper pointing upwards and call it from here
-        success = ctrl.go_to_pose(userdata.head_pose)
+        success = ctrl.go_to_handover_pose(userdata.head_pose)
         # if not success
         rospy.loginfo('Executing state IK2')
         return 'foundIK'
@@ -134,7 +147,7 @@ def execute(self, userdata):
 # define state ChangePID
 class ChangePID(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['changed', 'not_changed'], input_keys=['joint_nums','PID'])
+        smach.State.__init__(self, outcomes=['changed', 'notchanged'], input_keys=['joint_nums','PID'])
 
     def execute(self, userdata):
         rospy.wait_for_service('SetPID')
@@ -146,7 +159,7 @@ def execute(self, userdata):
             return 'changed'
         except rospy.ServiceException as e:
             rospy.logwarn("Service call failed:{0}".format(e))
-            return 'not_changed'
+            return 'notchanged'
 
 # define state OpenGripper
 class OpenGripper(smach.State):
@@ -155,7 +168,7 @@ def __init__(self):
 
     def execute(self, userdata):
         ## Detect when to Open gripper
-        self.open_gripper()
+        ctrl.open_gripper()
         rospy.loginfo('Executing state OpenGripper')
         return 'opened'
 
@@ -166,17 +179,18 @@ def main():
     # Create a SMACH state machine
     sm = smach.StateMachine(outcomes=['stop'])
     sm.userdata.tool_id = -1
-    sm.userdata.joint_nums = [1,2,3]
+    sm.userdata.joint_nums = [1,6]
     sm.userdata.PID = [0,0,0]
     sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
     sis.start()
+    rospy.sleep(5)
     # Open the container
     with sm:
         # Add states to the container
         smach.StateMachine.add('IDLE', Idle(),
                                transitions={'gotToolInput':'FINDTOOL'})
         smach.StateMachine.add('FINDTOOL', FindTool(),
-                               transitions={'foundTool':'IK1'})
+                               transitions={'foundTool':'IK1', 'notfoundTool':'IDLE'})
         smach.StateMachine.add('IK1', IK1(),
                                transitions={'noIK':'stop','foundIK':'GRAB'})
         # smach.StateMachine.add('MOVE', Move(),