def update(self, position, angle):
tform = Transform.pose_transform(position, angle)
points = self._points.transformed(tform).to_euclidean().T
self.circle.center = points[0]
self.arrow.xy = points[1:]
if show_range:
self.range_circle.center = points[0]
def init_positions(cls,
marxbot,
d_object,
marxbot_rel_pose,
min_distance=8.5):
"""
:param marxbot
:param d_object
:param marxbot_rel_pose: initial pose of the marxbot, relative to the goal pose, expressed as r, theta and
angle, that is position in polar coordinates and orientation
:param min_distance: the minimum distance between the marXbot and any object, that correspond to the radius
of the marXbot, is 8.5 cm.
"""
# The goal pose of the marXbot, defined with respect to the docking station reference frame, has the same y-axis
# of the docking station and the x-axis translated of the radius of the d_object plus a small arbitrary distance.
# The goal angle of the marXbot is 180 degree (π).
increment = d_object.radius + min_distance
x_goal = d_object.position[0] + increment
y_goal = d_object.position[1] + 0
marxbot.goal_position = (x_goal, y_goal)
marxbot.goal_angle = np.pi
# Transform the initial pose, relative to the goal, from polar to cartesian coordinates
r, theta, angle = marxbot_rel_pose
point_G = Point.from_polar(r, theta)
# Transform the cartesian coordinates from the goal to the world reference frame
trasform_D_G = Transform.translate(increment, 0)
trasform_W_D = Transform.pose_transform(d_object.position,
d_object.angle)
point_W = trasform_W_D @ trasform_D_G @ point_G
marxbot.initial_position = tuple(Point.to_euclidean(point_W))
marxbot.position = marxbot.initial_position
marxbot.initial_angle = angle
marxbot.angle = marxbot.initial_angle
marxbot.goal_reached = False
