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)
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)
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
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)
