Last minute testing

Author: ykarmesh

launch/camera_calibration.launch

@@ -1,6 +1,7 @@
 <launch>
 	<include file="position_control.launch"/>
 	<include file="$(find openface2_ros)/launch/openface2_ros.launch"/>
+	<include file="$(find object_detection)/launch/object_detection.launch"/>
 	<include file="$(find realsense2_camera)/launch/rs_camera.launch">
         <arg name="enable_pointcloud" value="true"/>
     	</include>

scripts/controller.py

@@ -25,6 +25,7 @@ def __init__(self):
         # global variables
         self.current_joint_state = None
         self.current_gripper_state = None
+        self.current_target_state = None
 
         # global frames
         self.GRIPPER_LINK = "gripper_link"
@@ -38,10 +39,11 @@ def __init__(self):
         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, -1.57, 1.57]
         self.HOME_POS_CAMERA_01 = [0.0, 0.698]
-        self.HOME_POS_CAMERA_02 = [-0.523, -0.523]
+        self.HOME_POS_CAMERA_02 = [-0.452, -0.452]
         self.IK_POSITION_TOLERANCE = 0.01
-        self.IK_ORIENTATION_TOLERANCE = np.pi/9
+        self.IK_ORIENTATION_TOLERANCE = np.pi/7
         self.MIN_CLOSING_GAP = 0.002
+        self.MOVEABLE_JOINTS = [0,4]
 
     def set_camera_angles(self, angles):
         pan_msg = Float64()
@@ -53,25 +55,24 @@ def set_camera_angles(self, angles):
         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)
+        rospy.sleep(6)
 
     def get_joint_state(self, data):
         # TODO: Change this when required
-        moveable_joints = [1,2,3]
+        
         # Add timestamp
         self.history['timestamp'].append(time.time())
         if(len(self.history['timestamp']) > 20):
             self.history['timestamp'].popleft()
 
         # Add Joint Feedback
-        joint_angles = np.array(data.position)[moveable_joints]
+        joint_angles = np.array(data.position)[self.MOVEABLE_JOINTS]
         self.history['joint_feedback'].append(joint_angles)
         if(len(self.history['joint_feedback']) > 20):
             self.history['joint_feedback'].popleft()
@@ -137,8 +138,8 @@ def go_to_grasp_pose(self, FRAME):
         O = tf.transformations.euler_from_quaternion(Q)
         Q = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2])
 
-        poses = [Pose(Point(P[0], P[1], P[2]+0.20), Quaternion(Q[0], Q[1], Q[2], Q[3])),
-                 Pose(Point(P[0], P[1], P[2]+0.15), Quaternion(Q[0], Q[1], Q[2], Q[3]))]
+        poses = [Pose(Point(P[0], P[1], P[2]+0.15), Quaternion(Q[0], Q[1], Q[2], Q[3])),
+                 Pose(Point(P[0], P[1], P[2]+0.10), Quaternion(Q[0], Q[1], Q[2], Q[3]))]
 
         target_joint = None
         self.open_gripper()
@@ -151,7 +152,7 @@ def go_to_grasp_pose(self, FRAME):
 
             if target_joint:
                 self.set_arm_joint_angles(target_joint)
-                rospy.sleep(8)
+                self.current_target_state = np.array(target_joint)[self.MOVEABLE_JOINTS]
             else:
                 return False
         return True
@@ -171,7 +172,7 @@ def check_grasp(self):
 
     # Goes to the position given by FRAME and grabs the object from the top
     def go_to_handover_pose(self, pose):
-        # print(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))
@@ -182,20 +183,23 @@ def go_to_handover_pose(self, pose):
         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]))]
+        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:
+        while target_joint is None:
             if self.current_joint_state:
-                target_joint = self.compute_ik(pose)
+                target_joint = self.compute_ik(poses)
             else:
                 print("Joint State Subscriber not working")
                 return False
 
             if target_joint:
+                print(target_joint)
                 self.set_arm_joint_angles(target_joint)
-                rospy.sleep(8)
+                self.current_target_state = np.array(target_joint)[self.MOVEABLE_JOINTS]
             else:
-                self.IK_POSITION_TOLERANCE += 0.1 
+                rospy.logwarn("No Solution was found. Current Tolerance "+ str(self.IK_POSITION_TOLERANCE))
+                self.IK_POSITION_TOLERANCE += 0.05 
+        print(target_joint)
         return True
 

scripts/machine.py

@@ -10,6 +10,7 @@
 from geometry_msgs.msg import PoseStamped
 from dynamixel_workbench_msgs.srv import SetPID
 from object_detection.srv import ObjectDetection
+import pdb
 
 # Initialize controller
 ctrl = Controller()
@@ -43,17 +44,17 @@ def __init__(self):
 
     def execute(self, userdata):
         rospy.loginfo('Executing state FINDTOOL')
-        rospy.wait_for_service('detect_object')
+        rospy.wait_for_service('object_pose_detection/detect_objects')
         try:
-            get_success = rospy.ServiceProxy('detect_object', ObjectDetection)
-            success = get_success(tool_id)
+            get_success = rospy.ServiceProxy('object_pose_detection/detect_objects', ObjectDetection)
+            success = get_success(userdata.tool_id)
             if success:
-                userdata.frame_name = "/icp_cuboid_frame"
+                userdata.frame_name = "/object_frame"
                 return 'foundTool'
             else:
                 rospy.logwarn("Service returned false")
                 return 'notfoundTool'
-        except: rospy.ServiceException as e:
+        except rospy.ServiceException as e:
             rospy.logwarn("Service called failed:{0}".format(e))
             return 'notfoundTool'
 
@@ -126,15 +127,17 @@ def execute(self, userdata):
 # define state IK2
 class IK2(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['foundIK'], input_keys=['head_pose'])
+        smach.State.__init__(self, outcomes=['foundIK', 'noIK'], input_keys=['head_pose'])
 
     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_handover_pose(userdata.head_pose)
-        # if not success
         rospy.loginfo('Executing state IK2')
+        success = ctrl.go_to_handover_pose(userdata.head_pose)
+        rospy.sleep(8)
+        if not success:
+            return 'noIK'
         return 'foundIK'
 
 # # define state MoveGive
@@ -158,6 +161,7 @@ def execute(self, userdata):
             P,I,D = userdata.PID
             for joint_num in userdata.joint_nums:
                 response = set_PID(joint_num, P, I, D)
+            rospy.sleep(10)
             return 'changed'
         except rospy.ServiceException as e:
             rospy.logwarn("Service call failed:{0}".format(e))
@@ -175,10 +179,12 @@ def execute(self, userdata):
             joint_len = len(ctrl.history['joint_feedback'][0])
             joint_sum = np.zeros(ctrl.history['joint_feedback'][0].shape)
             for joint_feedback in ctrl.history['joint_feedback']:
-                joint_sum += joint_feedback
+                # pdb.set_trace()
+                joint_sum += joint_feedback - ctrl.current_target_state
             avg_joint_sum = joint_sum/joint_len
-            if(np.sum(avg_joint_sum) < MIN_JOINT_MOTION_FOR_HAND_OVER):
+            if(np.sum(avg_joint_sum) > MIN_JOINT_MOTION_FOR_HAND_OVER):
                 ctrl.open_gripper()
+                rospy.sleep(1)
                 return 'opened'
 
 
@@ -190,7 +196,8 @@ def main():
     sm = smach.StateMachine(outcomes=['stop'])
     sm.userdata.tool_id = -1
     sm.userdata.joint_nums = [1,6]
-    sm.userdata.PID = [0,0,0]
+    sm.userdata.PID_Final = [0,0,0]
+    sm.userdata.PID_Initial = [640,5,50]
     sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
     sis.start()
     rospy.sleep(5)
@@ -214,14 +221,18 @@ def main():
         smach.StateMachine.add('ATTENTIONSEEKER', FindAttention(),
                                transitions={'giveTool':'IK2'})
         smach.StateMachine.add('IK2', IK2(),
-                               transitions={'foundIK':'CHANGEPID'})
+                               transitions={'foundIK':'CHANGEPID', 'noIK':'stop'})
         # smach.StateMachine.add('MOVEGIVE', MoveGive(),
         #                        transitions={'reached':'CHANGEPID'})
 
         smach.StateMachine.add('CHANGEPID', ChangePID(),
-                               transitions={'changed':'OPENGRIPPER', 'notchanged': 'CHANGEPID'})
+                               transitions={'changed':'OPENGRIPPER', 'notchanged': 'CHANGEPID'},
+                               remapping={'PID':'PID_Final'})
         smach.StateMachine.add('OPENGRIPPER', OpenGripper(),
-                               transitions={'opened':'IDLE'})
+                               transitions={'opened':'CHANGEBACKPID'})
+        smach.StateMachine.add('CHANGEBACKPID', ChangePID(),
+                               transitions={'changed':'IDLE', 'notchanged': 'CHANGEPID'},
+                               remapping={'PID':'PID_Initial'})