src/conntact/assembly_tools.py
def readYAML(self):
        """Read data from job config YAML and make certain calculations for later use. Stores peg frames in dictionary tool_data
        """
        
        #job parameters moved in from the peg_in_hole_params.yaml file
        #'peg_4mm' 'peg_8mm' 'peg_10mm' 'peg_16mm'
        #'hole_4mm' 'hole_8mm' 'hole_10mm' 'hole_16mm'
        self.target_peg                 = rospy.get_param('/task/target_peg')
        self.target_hole                = rospy.get_param('/task/target_hole')
        self.activeTCP                  = rospy.get_param('/task/starting_tcp')

        self.read_board_positions()
        
        self.read_peg_hole_dimensions()

        #Spiral parameters
        self._spiral_params             = rospy.get_param('/algorithm/spiral_params')
        
        #Calculate transform from TCP to peg corner        
        self.peg_locations              = rospy.get_param('/objects/'+self.target_peg+'/grasping/pinch_grasping/locations')
        
        # Setup default zero-transform in case it needs to be referenced for consistency.
        self.tool_data['gripper_tip']   = dict()
        a                               = TransformStamped()
        a.header.frame_id               = "tool0"
        a.child_frame_id                = 'gripper_tip'
        a.transform.rotation.w          = 1
        self.tool_data['gripper_tip']['transform']    = a
        self.tool_data['gripper_tip']['matrix']       = AssemblyTools.to_homogeneous(a.transform.rotation, a.transform.translation)
        self.reference_frames['tcp']    = a
        

        for key in list(self.peg_locations):
            #Read in each listed tool position; measure their TF and store in dictionary.
            #Write the position of the peg's corner wrt the gripper tip as a reference-ready TF.
            pegTransform = AssemblyTools.get_tf_from_YAML(self.peg_locations[str(key)]['pose'], self.peg_locations[str(key)]['orientation'],
            "tool0_to_gripper_tip_link", "peg_"+str(key)+"_position")
            self.reference_frames['tcp'] = pegTransform
            self.send_reference_TFs()
            self._rate.sleep()
            a = self.tf_buffer.lookup_transform("tool0", "peg_"+str(key)+"_position", rospy.Time(0), rospy.Duration(1.0))
            self.tool_data[str(key)]=dict()
            self.tool_data[str(key)]['transform']   = a
            self.tool_data[str(key)]['matrix']      = AssemblyTools.to_homogeneous(a.transform.rotation, a.transform.translation)
            rospy.logerr("Added TCP entry for " + str(key))
            
        rospy.logerr("TCP position dictionary now contains: " + str(list(self.tool_data))+ ", selected tool publishing now: ")
        self.select_tool(self.activeTCP)

        self.surface_height = rospy.get_param('/task/assumed_starting_height') #Starting height assumption
        self.restart_height = rospy.get_param('/task/restart_height')
Read data from job config YAML and make certain calculations for later use. Stores peg frames in dictionary tool_data
        
Read data from job config YAML and make certain calculations for later use. Stores peg frames in dictionary tool_data
Read data from job config YAML and make certain calculations for later use . Stores peg frames in dictionary tool_data
def readYAML(self):
        self.target_peg                 = rospy.get_param('/task/target_peg')
        self.target_hole                = rospy.get_param('/task/target_hole')
        self.activeTCP                  = rospy.get_param('/task/starting_tcp')
        self.read_board_positions()
        self.read_peg_hole_dimensions()
        self._spiral_params             = rospy.get_param('/algorithm/spiral_params')
        self.peg_locations              = rospy.get_param('/objects/'+self.target_peg+'/grasping/pinch_grasping/locations')
        self.tool_data['gripper_tip']   = dict()
        a                               = TransformStamped()
        a.header.frame_id               = "tool0"
        a.child_frame_id                = 'gripper_tip'
        a.transform.rotation.w          = 1
        self.tool_data['gripper_tip']['transform']    = a
        self.tool_data['gripper_tip']['matrix']       = AssemblyTools.to_homogeneous(a.transform.rotation, a.transform.translation)
        self.reference_frames['tcp']    = a
        for key in list(self.peg_locations):
            pegTransform = AssemblyTools.get_tf_from_YAML(self.peg_locations[str(key)]['pose'], self.peg_locations[str(key)]['orientation'],
            "tool0_to_gripper_tip_link", "peg_"+str(key)+"_position")
            self.reference_frames['tcp'] = pegTransform
            self.send_reference_TFs()
            self._rate.sleep()
            a = self.tf_buffer.lookup_transform("tool0", "peg_"+str(key)+"_position", rospy.Time(0), rospy.Duration(1.0))
            self.tool_data[str(key)]=dict()
            self.tool_data[str(key)]['transform']   = a
            self.tool_data[str(key)]['matrix']      = AssemblyTools.to_homogeneous(a.transform.rotation, a.transform.translation)
            rospy.logerr("Added TCP entry for " + str(key))
        rospy.logerr("TCP position dictionary now contains: " + str(list(self.tool_data))+ ", selected tool publishing now: ")
        self.select_tool(self.activeTCP)
        self.surface_height = rospy.get_param('/task/assumed_starting_height') 
        self.restart_height = rospy.get_param('/task/restart_height')
def readYAML ( self ) : self . target_peg = rospy . get_param ( '/task/target_peg' ) self . target_hole = rospy . get_param ( '/task/target_hole' ) self . activeTCP = rospy . get_param ( '/task/starting_tcp' ) self . read_board_positions ( ) self . read_peg_hole_dimensions ( ) self . _spiral_params = rospy . get_param ( '/algorithm/spiral_params' ) self . peg_locations = rospy . get_param ( '/objects/' + self . target_peg + '/grasping/pinch_grasping/locations' ) self . tool_data [ 'gripper_tip' ] = dict ( ) a = TransformStamped ( ) a . header . frame_id = "tool0" a . child_frame_id = 'gripper_tip' a . transform . rotation . w = 1 self . tool_data [ 'gripper_tip' ] [ 'transform' ] = a self . tool_data [ 'gripper_tip' ] [ 'matrix' ] = AssemblyTools . to_homogeneous ( a . transform . rotation , a . transform . translation ) self . reference_frames [ 'tcp' ] = a for key in list ( self . peg_locations ) : pegTransform = AssemblyTools . get_tf_from_YAML ( self . peg_locations [ str ( key ) ] [ 'pose' ] , self . peg_locations [ str ( key ) ] [ 'orientation' ] , "tool0_to_gripper_tip_link" , "peg_" + str ( key ) + "_position" ) self . reference_frames [ 'tcp' ] = pegTransform self . send_reference_TFs ( ) self . _rate . sleep ( ) a = self . tf_buffer . lookup_transform ( "tool0" , "peg_" + str ( key ) + "_position" , rospy . Time ( 0 ) , rospy . Duration ( 1.0 ) ) self . tool_data [ str ( key ) ] = dict ( ) self . tool_data [ str ( key ) ] [ 'transform' ] = a self . tool_data [ str ( key ) ] [ 'matrix' ] = AssemblyTools . to_homogeneous ( a . transform . rotation , a . transform . translation ) rospy . logerr ( "Added TCP entry for " + str ( key ) ) rospy . logerr ( "TCP position dictionary now contains: " + str ( list ( self . tool_data ) ) + ", selected tool publishing now: " ) self . select_tool ( self . activeTCP ) self . surface_height = rospy . get_param ( '/task/assumed_starting_height' ) self . restart_height = rospy . get_param ( '/task/restart_height' )
Read data from job config YAML and make certain calculations for later use.
Read data from job config YAML and make certain calculations for later use .
"""Read data from job config YAML and make certain calculations for later use. Stores peg frames in dictionary tool_data
        """ #job parameters moved in from the peg_in_hole_params.yaml file #'peg_4mm' 'peg_8mm' 'peg_10mm' 'peg_16mm' #'hole_4mm' 'hole_8mm' 'hole_10mm' 'hole_16mm' #Spiral parameters #Calculate transform from TCP to peg corner         # Setup default zero-transform in case it needs to be referenced for consistency. #Read in each listed tool position; measure their TF and store in dictionary. #Write the position of the peg's corner wrt the gripper tip as a reference-ready TF. #Starting height assumption
self