src/conntact/assembly_tools.py
def publish_pose(self, pose_stamped_vec):
        """Takes in vector representations of position 
        :param pose_stamped_vec: (list of floats) List of parameters for pose with x,y,z position and orientation quaternion
        """
        # Ensure controller is loaded
        # self.check_controller(self.controller_name)

        # Create poseStamped msg
        goal_pose = PoseStamped()

        # Set the position and orientation
        point = Point()
        quaternion = Quaternion()

        # point.x, point.y, point.z = position
        point.x, point.y, point.z = pose_stamped_vec[0][:]
        goal_pose.pose.position = point

        quaternion.w, quaternion.x, quaternion.y, quaternion.z  = pose_stamped_vec[1][:]
        goal_pose.pose.orientation = quaternion

        # Set header values
        goal_pose.header.stamp = rospy.get_rostime()
        goal_pose.header.frame_id = "base_link"
        


        if(self.activeTCP != "tool0"):
            #Convert pose in TCP coordinates to assign wrist "tool0" position for controller

            b_link = goal_pose.header.frame_id
            goal_matrix = AssemblyTools.to_homogeneous(goal_pose.pose.orientation, goal_pose.pose.position) #tf from base_link to tcp_goal = bTg
            backing_mx = trfm.inverse_matrix(self.tool_data[self.activeTCP]['matrix']) #tf from tcp_goal to wrist = gTw
            goal_matrix = np.dot(goal_matrix, backing_mx) #bTg * gTw = bTw
            goal_pose = AssemblyTools.matrix_to_pose(goal_matrix, b_link)
            
            # self._tool_offset_pub.publish(goal_pose)

            
        self._pose_pub.publish(goal_pose)
Takes in vector representations of position 
        :param pose_stamped_vec: (list of floats) List of parameters for pose with x,y,z position and orientation quaternion
        
Takes in vector representations of position
Takes in vector representations of position
def publish_pose(self, pose_stamped_vec):
        goal_pose = PoseStamped()
        point = Point()
        quaternion = Quaternion()
        point.x, point.y, point.z = pose_stamped_vec[0][:]
        goal_pose.pose.position = point
        quaternion.w, quaternion.x, quaternion.y, quaternion.z  = pose_stamped_vec[1][:]
        goal_pose.pose.orientation = quaternion
        goal_pose.header.stamp = rospy.get_rostime()
        goal_pose.header.frame_id = "base_link"
        if(self.activeTCP != "tool0"):
            b_link = goal_pose.header.frame_id
            goal_matrix = AssemblyTools.to_homogeneous(goal_pose.pose.orientation, goal_pose.pose.position) 
            backing_mx = trfm.inverse_matrix(self.tool_data[self.activeTCP]['matrix']) 
            goal_matrix = np.dot(goal_matrix, backing_mx) 
            goal_pose = AssemblyTools.matrix_to_pose(goal_matrix, b_link)
        self._pose_pub.publish(goal_pose)
def publish_pose ( self , pose_stamped_vec ) : goal_pose = PoseStamped ( ) point = Point ( ) quaternion = Quaternion ( ) point . x , point . y , point . z = pose_stamped_vec [ 0 ] [ : ] goal_pose . pose . position = point quaternion . w , quaternion . x , quaternion . y , quaternion . z = pose_stamped_vec [ 1 ] [ : ] goal_pose . pose . orientation = quaternion goal_pose . header . stamp = rospy . get_rostime ( ) goal_pose . header . frame_id = "base_link" if ( self . activeTCP != "tool0" ) : b_link = goal_pose . header . frame_id goal_matrix = AssemblyTools . to_homogeneous ( goal_pose . pose . orientation , goal_pose . pose . position ) backing_mx = trfm . inverse_matrix ( self . tool_data [ self . activeTCP ] [ 'matrix' ] ) goal_matrix = np . dot ( goal_matrix , backing_mx ) goal_pose = AssemblyTools . matrix_to_pose ( goal_matrix , b_link ) self . _pose_pub . publish ( goal_pose )
Takes in vector representations of position
Takes in vector representations of position
"""Takes in vector representations of position 
        :param pose_stamped_vec: (list of floats) List of parameters for pose with x,y,z position and orientation quaternion
        """ # Ensure controller is loaded # self.check_controller(self.controller_name) # Create poseStamped msg # Set the position and orientation # point.x, point.y, point.z = position # Set header values #Convert pose in TCP coordinates to assign wrist "tool0" position for controller #tf from base_link to tcp_goal = bTg #tf from tcp_goal to wrist = gTw #bTg * gTw = bTw # self._tool_offset_pub.publish(goal_pose)
self
pose_stamped_vec
(list of floats) List of parameters for pose with x,y,z position and orientation quaternion
( list of floats ) List of parameters for pose with x y z position and orientation quaternion