Final code for presentation

external/dynamixel-workbench-msgs/dynamixel_workbench_msgs/srv/SetPID.srv

@@ -1,8 +1,8 @@
 # This message is used to set PID for different joints
 
 uint8 joint_num #Possible values: joint_1_id : 1, joint_2_1_id: 2, joint_2_2_id: 3, joint_3_id: 4 joint_4_id: 5, joint_5_id: 6 , gripper: 7, pan: 8, tilt: 9
-uint8 P
-uint8 I
-uint8 D
+uint16 P
+uint16 I
+uint16 D
 ---
 bool result

scripts/controller.py

@@ -61,7 +61,7 @@ def set_arm_joint_angles(self, joint_target):
         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(6)
+        rospy.sleep(8)
 
     def get_joint_state(self, data):
         # TODO: Change this when required
@@ -74,7 +74,7 @@ def get_joint_state(self, data):
         # Add Joint Feedback
         joint_angles = np.array(data.position)[self.MOVEABLE_JOINTS]
         self.history['joint_feedback'].append(joint_angles)
-        if(len(self.history['joint_feedback']) > 20):
+        if(len(self.history['joint_feedback']) > 10):
             self.history['joint_feedback'].popleft()
 
         self.current_joint_state = data.position[0:5]
@@ -140,6 +140,7 @@ def go_to_grasp_pose(self, FRAME):
 
         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]))]
+        print(poses)
 
         target_joint = None
         self.open_gripper()

scripts/machine.py

@@ -14,7 +14,7 @@
 
 # Initialize controller
 ctrl = Controller()
-MIN_JOINT_MOTION_FOR_HAND_OVER = 0.1
+MIN_JOINT_MOTION_FOR_HAND_OVER = 0.05
 # define state Idle
 class Idle(smach.State):
     def __init__(self):
@@ -99,7 +99,7 @@ def __init__(self):
 
     def execute(self, userdata):
         ctrl.set_camera_angles(ctrl.HOME_POS_CAMERA_02)
-        ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_02)
+        ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_00)
         rospy.loginfo('Executing state MoveHome2')
         return 'reached'
 
@@ -135,7 +135,7 @@ def execute(self, userdata):
         ## Make a different function with gripper pointing upwards and call it from here
         rospy.loginfo('Executing state IK2')
         success = ctrl.go_to_handover_pose(userdata.head_pose)
-        rospy.sleep(8)
+        rospy.sleep(3)
         if not success:
             return 'noIK'
         return 'foundIK'
@@ -161,7 +161,6 @@ 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))
@@ -178,11 +177,12 @@ def execute(self, userdata):
         while(True):
             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']:
-                # pdb.set_trace()
+            for joint_feedback in list(ctrl.history['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):
+            # print(joint_sum)
+            if(np.abs(np.sum(avg_joint_sum)) > MIN_JOINT_MOTION_FOR_HAND_OVER):
                 ctrl.open_gripper()
                 rospy.sleep(1)
                 return 'opened'
@@ -195,7 +195,7 @@ def main():
     # Create a SMACH state machine
     sm = smach.StateMachine(outcomes=['stop'])
     sm.userdata.tool_id = -1
-    sm.userdata.joint_nums = [1,6]
+    sm.userdata.joint_nums = [0,5]
     sm.userdata.PID_Final = [0,0,0]
     sm.userdata.PID_Initial = [640,5,50]
     sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')