src/conntact/assembly_tools.py
def read_board_positions(self):
        """ Calculates pose of target hole relative to robot base frame.
        """
        temp_z_position_offset = 207 #Our robot is reading Z positions wrong on the pendant for some reason.
        taskPos = list(np.array(rospy.get_param('/environment_state/task_frame/position')))
        taskPos[2] = taskPos[2] + temp_z_position_offset
        taskOri = rospy.get_param('/environment_state/task_frame/orientation')
        holePos = list(np.array(rospy.get_param('/objects/'+self.target_hole+'/local_position')))
        holePos[2] = holePos[2] + temp_z_position_offset
        holeOri = rospy.get_param('/objects/'+self.target_hole+'/local_orientation')
        
        #Set up target hole pose
        tf_robot_to_task_board = AssemblyTools.get_tf_from_YAML(taskPos, taskOri, "base_link", "task_board")
        pose_task_board_to_hole = AssemblyTools.get_pose_from_YAML(holePos, holeOri, "base_link")
        self.target_hole_pose = tf2_geometry_msgs.do_transform_pose(pose_task_board_to_hole, tf_robot_to_task_board)
        # self.target_broadcaster = tf2_geometry_msgs.do_transform_pose(self.pose_task_board_to_hole, self.tf_robot_to_task_board)
        targetHoleTF = AssemblyTools.swap_pose_tf(self.target_hole_pose, "target_hole_position")
        self.reference_frames['target_hole_position'] = targetHoleTF
        self.send_reference_TFs()
        self._rate.sleep()
        # self._target_pub.publish(self.target_hole_pose)
        self.x_pos_offset = self.target_hole_pose.pose.position.x
        self.y_pos_offset = self.target_hole_pose.pose.position.y
 Calculates pose of target hole relative to robot base frame.
        
Calculates pose of target hole relative to robot base frame.
Calculates pose of target hole relative to robot base frame .
def read_board_positions(self):
        temp_z_position_offset = 207 
        taskPos = list(np.array(rospy.get_param('/environment_state/task_frame/position')))
        taskPos[2] = taskPos[2] + temp_z_position_offset
        taskOri = rospy.get_param('/environment_state/task_frame/orientation')
        holePos = list(np.array(rospy.get_param('/objects/'+self.target_hole+'/local_position')))
        holePos[2] = holePos[2] + temp_z_position_offset
        holeOri = rospy.get_param('/objects/'+self.target_hole+'/local_orientation')
        tf_robot_to_task_board = AssemblyTools.get_tf_from_YAML(taskPos, taskOri, "base_link", "task_board")
        pose_task_board_to_hole = AssemblyTools.get_pose_from_YAML(holePos, holeOri, "base_link")
        self.target_hole_pose = tf2_geometry_msgs.do_transform_pose(pose_task_board_to_hole, tf_robot_to_task_board)
        targetHoleTF = AssemblyTools.swap_pose_tf(self.target_hole_pose, "target_hole_position")
        self.reference_frames['target_hole_position'] = targetHoleTF
        self.send_reference_TFs()
        self._rate.sleep()
        self.x_pos_offset = self.target_hole_pose.pose.position.x
        self.y_pos_offset = self.target_hole_pose.pose.position.y
def read_board_positions ( self ) : temp_z_position_offset = 207 taskPos = list ( np . array ( rospy . get_param ( '/environment_state/task_frame/position' ) ) ) taskPos [ 2 ] = taskPos [ 2 ] + temp_z_position_offset taskOri = rospy . get_param ( '/environment_state/task_frame/orientation' ) holePos = list ( np . array ( rospy . get_param ( '/objects/' + self . target_hole + '/local_position' ) ) ) holePos [ 2 ] = holePos [ 2 ] + temp_z_position_offset holeOri = rospy . get_param ( '/objects/' + self . target_hole + '/local_orientation' ) tf_robot_to_task_board = AssemblyTools . get_tf_from_YAML ( taskPos , taskOri , "base_link" , "task_board" ) pose_task_board_to_hole = AssemblyTools . get_pose_from_YAML ( holePos , holeOri , "base_link" ) self . target_hole_pose = tf2_geometry_msgs . do_transform_pose ( pose_task_board_to_hole , tf_robot_to_task_board ) targetHoleTF = AssemblyTools . swap_pose_tf ( self . target_hole_pose , "target_hole_position" ) self . reference_frames [ 'target_hole_position' ] = targetHoleTF self . send_reference_TFs ( ) self . _rate . sleep ( ) self . x_pos_offset = self . target_hole_pose . pose . position . x self . y_pos_offset = self . target_hole_pose . pose . position . y
Calculates pose of target hole relative to robot base frame.
Calculates pose of target hole relative to robot base frame .
""" Calculates pose of target hole relative to robot base frame.
        """ #Our robot is reading Z positions wrong on the pendant for some reason. #Set up target hole pose # self.target_broadcaster = tf2_geometry_msgs.do_transform_pose(self.pose_task_board_to_hole, self.tf_robot_to_task_board) # self._target_pub.publish(self.target_hole_pose)
self