コード例 #1
0
 def __update_reference_frame(self):
     """
     Update the reference frame axis vectors
     """
     self.__x_vector = get_pose_x_vec(self.__pose)
     self.__y_vector = get_pose_y_vec(self.__pose)
     self.__z_vector = get_pose_z_vec(self.__pose)
コード例 #2
0
    def update_pose(self, se_object):
        """
        Given an SE object, update the pose of the joint.

        :param se_object: SE3 pose representation of the joint
        :type se_object: class:`spatialmath.pose3d.SE3`
        """
        # if self.stl_offset is not None:
        #     calc_se3 = se_object * self.stl_offset
        # else:
        #     calc_se3 = se_object

        # Get the new pose details
        new_x_axis = get_pose_x_vec(se_object)
        new_y_axis = get_pose_y_vec(se_object)
        # new_z_axis = get_pose_z_vec(se_object)  # not needed
        new_position = get_pose_pos(se_object)

        # Update the graphic object
        self.__graphic_obj.pos = new_position
        self.__graphic_obj.axis = new_x_axis
        self.__graphic_obj.up = new_y_axis

        # Update the reference frame
        self.__pose = se_object
        self.__update_reference_frame()
        self.draw_reference_frame(self.__graphic_ref.visible)
コード例 #3
0
    def draw_reference_frame(self, is_visible):
        """
        Draw a reference frame at the tool point position.

        :param is_visible: Whether the reference frame should be drawn or not
        :type is_visible: `bool`
        """
        self.__graphic_ref.pos = get_pose_pos(self.__pose)
        self.__graphic_ref.axis = get_pose_x_vec(self.__pose)
        self.__graphic_ref.up = get_pose_y_vec(self.__pose)
        self.__graphic_ref.visible = is_visible
コード例 #4
0
    def update_orientation(self, se_object):
        """
        Given an SE object, update just the orientation of the joint.

        :param se_object: SE3 pose representation of the joint
        :type se_object: class:`spatialmath.pose3d.SE3`
        """
        # Get the new pose details
        new_x_axis = get_pose_x_vec(se_object)
        new_y_axis = get_pose_y_vec(se_object)
        # new_z_axis = get_pose_z_vec(se_object)  # not needed

        # Update the graphic object
        self.__graphic_obj.axis = new_x_axis
        self.__graphic_obj.up = new_y_axis

        # Update the reference frame
        self.__update_reference_frame()
        self.draw_reference_frame(self.__graphic_ref.visible)