Deleted a lot of commented-out dead code. Fixed Connfig so we get a g…

…ood spiral. Tests look good!

config/connfig_peg_10mm.yaml

@@ -9,7 +9,7 @@ task:
     frequency: 0.15 #Hz frequency in spiral_search_motion
     min_amplitude: .002  #meters amplitude in spiral_search_motion
     max_amplitude: .012 #mm
-    max_cycles: 10 #revolutions
+    max_cycles: 62.83 #2*pi*10 for 10 revolutions
 objects:
   stl_file: "file_location.stl"
   local_position: [ 36.5, -202, 7.5 ] #Starting position from kitting_frame

nodes/spiral_search_node

@@ -63,4 +63,5 @@ if __name__ == '__main__':
     # Instantiate the task called for in the task_list:
     assembly_application = conntasks[task_info['task']](conntext, interface, task_info['connfig'])
     assembly_application.main()
+    interface.send_info("Node has control again!")
 

src/conntact/assembly_algorithm_blocks.py

@@ -16,6 +16,7 @@
 from geometry_msgs.msg import (Point, Pose, PoseStamped, Quaternion, Transform,
                                TransformStamped, Vector3, Wrench,
                                WrenchStamped)
+
 from transitions import Machine
 
 from conntact.assembly_tools import AssemblyTools
@@ -65,14 +66,7 @@ def __init__(self, conntext, interface, connfig_name):
         #Configuration variables, to be moved to a yaml file later:
         self.pose_vec = None
         self.wrench_vec = self.conntext.get_command_wrench([0, 0, 0])
-        # self.speed_static = [1/1000,1/1000,1/1000] #Speed at which the system considers itself stopped. Rel. to target hole.
-        # force_dangerous = [55,55,65]                        #Force value which kills the program. Rel. to gripper.
-        # force_transverse_dangerous = np.array([30,30,30])   #Force value transverse to the line from the TCP to the force sensor which kills the program. Rel. to gripper.
-        # force_warning = [40,40,50]                          #Force value which pauses the program. Rel. to gripper.
-        # force_transverse_warning = np.array([20,20,20])     #torque value transverse to the line from the TCP to the force sensor which kills the program. Rel. to gripper.
-        # self.max_force_error = [4, 4, 4]                #Allowable error force with no actual loads on the gripper.
-        # self.cap_check_forces = force_dangerous, force_transverse_dangerous, force_warning, force_transverse_warning
-        # self._bias_wrench = self.conntext.create_wrench([0,0,0], [0,0,0]).wrench #Calculated to remove the steady-state error from wrench readings.
+
         self.next_trigger = ''  # Empty to start. Each callback should decide what next trigger to implement in the main loop
 
         #List the official states here. Takes strings, but the tokens created above so typos are less likely from repeated typing of strings (unchecked by interpreter).
@@ -122,23 +116,14 @@ def post_action(self, trigger_name):
     def is_already_retracting(self):
         return self.is_state_safety_retraction()
     def on_enter_state_finding_surface(self):
-        # self.conntext.select_tool('corner')
-        self.reset_on_state_enter()
-        self.conntext.select_tool(self.tcp_selected)
         self._log_state_transition()
     def on_enter_state_finding_hole(self):
-        # self.conntext.select_tool('corner')
-        self.reset_on_state_enter()
-        self.conntext.select_tool(self.tcp_selected)
         self._log_state_transition()
     def on_enter_state_inserting_along_axis(self):
-        self.reset_on_state_enter()
         self._log_state_transition()
     def on_enter_state_completed_insertion(self):
-        self.reset_on_state_enter()
         self._log_state_transition()
     def on_enter_state_retracting_to_safety(self):
-        self.reset_on_state_enter()
         self._log_state_transition()
 
 
@@ -216,26 +201,6 @@ def algorithm_execute(self):
             # Publish robot motion commands only once per loop, right at the end of the loop:
             self.update_commands()
             self.interface.sleep_until_next_loop()
-
-    # def check_load_cell_feedback(self):
-    #     # self.switch_state = False
-    #     #Take an average of static sensor reading to check that it's stable.
-    #
-    #     if (self.curr_time_numpy > 1.5):
-    #         self._bias_wrench = self.conntext._average_wrench_gripper
-    #         rospy.loginfo("Measured bias wrench. Force: " + str(self.conntext.as_array(self._bias_wrench.force))
-    #             +" and Torque: " + str(self.conntext.as_array(self._bias_wrench.torque)))
-    #
-    #         # acceptable = self.conntext.vectorRegionCompare_symmetrical(self.conntext.as_array(self._bias_wrench.force), np.ndarray(self.max_force_error))
-    #         acceptable = True
-    #
-    #         if(acceptable):
-    #             self.print("Starting linear search.")
-    #             self.next_trigger, self.switch_state = self.post_action(APPROACH_SURFACE_TRIGGER)
-    #
-    #         else:
-    #             rospy.logerr("Starting wrench is dangerously high. Suspending. Try restarting robot if values seem wrong.")
-    #             self.next_trigger, self.switch_state = self.post_action(SAFETY_RETRACTION_TRIGGER)
 
     def inserting_along_axis(self):
         #Continue spiraling downward. Outward normal force is used to verify that the peg can't move
@@ -316,19 +281,12 @@ def all_states_calc(self):
         self.conntext.current_pose = self.conntext.get_current_pos()
         self.curr_time = rospy.get_rostime() - self.start_time
         self.curr_time_numpy = np.double(self.curr_time.to_sec())
-        marked_state = 1; #returns to this state after a soft restart in state 99
-        # self.wrench_vec  = self.conntext.get_command_wrench([0,0,-2])
-        # self.pose_vec = self.full_compliance_position()
         self.conntext.update_avg_speed()
         self.conntext.update_average_wrench()
-        # self._update_plots()
-        # rospy.loginfo_throttle(1, Fore.BLUE + "Average wrench in newtons  is force \n" + str(self.conntext._average_wrench_world.force)+ 
-        #     " and torque \n" + str(self.conntext._average_wrench_world.torque) + Style.RESET_ALL)
-        # rospy.loginfo_throttle(1, Fore.CYAN + "\nAverage speed in mm/second is \n" + str(1000*self.conntext.average_speed) +Style.RESET_ALL)
 
     def checkForceCap(self):
         if(not self.conntext.force_cap_check()):
-            self.next_trigger, self.switch_state = self.post_action(SAFETY_RETRACTION_TRIGGER) 
+            self.next_trigger, self.switch_state = self.post_action(SAFETY_RETRACTION_TRIGGER)
             rospy.logerr("Force/torque unsafe; pausing application.")
 
 

src/conntact/assembly_tools.py

@@ -518,7 +518,7 @@ def update_avg_speed(self) -> None:
         if (time is not None and time > 0.0):  # Update only if we're using a new pose; also, avoid divide by zero
             speedDiff = distance / time
             self.average_speed = self.filters.average_speed(speedDiff)
-            self.interface.send_info("Speed is {}".format(self.average_speed), 2)
+            # self.interface.send_info("Speed is {}".format(self.average_speed), 2)
             if (np.linalg.norm(self.average_speed) > self.params['robot']['hard_speed_limit']):
                 self.interface.send_error("Speed too high, quitting. Speed: {} \nTotal:{}".format(
                     self.average_speed, np.linalg.norm(self.average_speed)))

src/conntact/spiral_search.py

@@ -68,7 +68,7 @@ def read_peg_hole_dimensions(self):
     def readYAML(self):
         """Read data from job config YAML and make certain calculations for later use. Stores peg frames in dictionary tool_data
         """
-        
+
         activeTCP = self.connfig['task']['starting_tcp']
 
         self.read_board_positions()
@@ -99,34 +99,6 @@ def read_board_positions(self):
         self.x_pos_offset = self.target_hole_pose.transform.translation.x
         self.y_pos_offset = self.target_hole_pose.transform.translation.y
 
-    # def finding_hole(self):
-    #     #Spiral until we descend 1/3 the specified hole depth (provisional fraction)
-    #     #This triggers the hole position estimate to be updated to limit crazy
-    #     #forces and oscillations. Also reduces spiral size.
-    #
-    #     seeking_force = -7.0
-    #     self.wrench_vec  = self.conntext.get_command_wrench([0,0,seeking_force])
-    #     self.pose_vec = self.spiral_search_motion(self._spiral_params["frequency"],
-    #         self._spiral_params["min_amplitude"], self._spiral_params["max_cycles"])
-    #
-    #     if(not self.conntext.force_cap_check(*self.cap_check_forces)):
-    #         self.next_trigger, self.switch_state = self.post_action(SAFETY_RETRACTION_TRIGGER)
-    #         self.interface.send_error("Force/torque unsafe; pausing application.")
-    #     elif( self.conntext.current_pose.transform.translation.z <= self.conntext.surface_height - .0004):
-    #         #If we've descended at least 5mm below the flat surface detected, consider it a hole.
-    #         self.completion_confidence = self.completion_confidence + 1/self.rate_selected
-    #         self.interface.send_error(1, "Monitoring for hole location, confidence = " + str(self.completion_confidence))
-    #         if(self.completion_confidence > .90):
-    #                 #Descended from surface detection point. Updating hole location estimate.
-    #                 self.conntext.x_pos_offset = self.conntext.current_pose.transform.translation.x
-    #                 self.conntext.y_pos_offset = self.conntext.current_pose.transform.translation.y
-    #                 self.conntext._amp_limit_cp = 2 * np.pi * 4 #limits to 3 spirals outward before returning to center.
-    #                 #TODO - Make these runtime changes pass as parameters to the "spiral_search_basic_compliance_control" function
-    #                 self.print("Hole found, peg inserting...")
-    #                 self.next_trigger, self.switch_state = self.post_action(INSERT_PEG_TRIGGER)
-    #     else:
-    #         self.completion_confidence = np.max( np.array([self.completion_confidence * 95/self.rate_selected, .01]))
-
     def all_states_calc(self):
         self.publish_plotted_values(('state', self.state))
         super().all_states_calc()
@@ -237,7 +209,7 @@ class ExitStep(AssemblyStep):
     def __init__(self, algorithmBlocks: (AlgorithmBlocks)) -> None:
         AssemblyStep.__init__(self, algorithmBlocks)
         self.comply_axes = [1, 1, 1]
-        self.seeking_force = [0, 0, 7]
+        self.seeking_force = [0, 0, 15]
 
     def exitConditions(self) -> bool:
         return self.noForce() and self.above_restart_height()