def evolve_world_pose_tractor(
self,
world_pose_tractor: SE3,
v_left: float,
v_right: float,
delta_t: float,
) -> SE3:
v, w = self.wheel_velocity_to_unicycle(v_left, v_right)
tractor_pose_t = SE3.exp([v * delta_t, 0, 0, 0, 0, w * delta_t])
return world_pose_tractor.dot(tractor_pose_t)
def evolve_world_pose_tractor(
self,
world_pose_tractor: SE3,
v_left: float,
v_right: float,
delta_t: float,
) -> SE3:
tractor_pose_delta = self.compute_tractor_pose_delta(
v_left, v_right, delta_t)
return world_pose_tractor.dot(tractor_pose_delta)
