def dd_test():
radius = 0.1
radius_left = radius
radius_right = radius
wheel_distance = 0.5
dddp = DifferentialDriveDynamicsParameters(radius_left=radius_left, radius_right=radius_right,
wheel_distance=wheel_distance)
q0 = geo.SE2_from_translation_angle([0, 0], 0)
v0 = geo.se2.zero()
c0 = q0, v0
s0 = dddp.initialize(c0)
omega = 0.3
commands = WheelVelocityCommands(omega, omega)
dt = 4.0
s1 = s0.integrate(dt, commands)
q1, v1 = s1.TSE2_from_state()
print(geo.se2.friendly(v1))
print(geo.SE2.friendly(q1))
p1, theta = geo.translation_angle_from_SE2(q1)
assert_almost_equal(p1, [dt * radius * omega, 0])
omega_left = 0.3
omega_right = 0
commands = WheelVelocityCommands(omega_left, omega_right)
dt = 4.0
s1 = s0.integrate(dt, commands)
q1, v1 = s1.TSE2_from_state()
p1, theta = geo.translation_angle_from_SE2(q1)
print(geo.se2.friendly(v1))
print(geo.SE2.friendly(q1))
def lane_pose_test1():
outdir = get_comptests_output_dir()
# load one of the maps (see the list using dt-world-draw-maps)
dw = load_map("udem1")
v = 5
# define a SampledSequence with timestamp, command
commands_sequence = SampledSequence.from_iterator(
[
# we set these velocities at 1.0
(1.0, WheelVelocityCommands(0.1 * v, 0.1 * v)),
# at 2.0 we switch and so on
(2.0, WheelVelocityCommands(0.1 * v, 0.4 * v)),
(4.0, WheelVelocityCommands(0.1 * v, 0.4 * v)),
(5.0, WheelVelocityCommands(0.1 * v, 0.2 * v)),
(6.0, WheelVelocityCommands(0.1 * v, 0.1 * v)),
]
)
# we upsample the sequence by 5
commands_sequence = commands_sequence.upsample(5)
## Simulate the dynamics of the vehicle
# start from q0
q0 = geo.SE2_from_translation_angle([1.8, 0.7], 0)
# instantiate the class that represents the dynamics
dynamics = reasonable_duckiebot()
# this function integrates the dynamics
poses_sequence = get_robot_trajectory(dynamics, q0, commands_sequence)
#################
# Visualization and rule evaluation
# Creates an object 'duckiebot'
ego_name = "duckiebot"
db = DB18() # class that gives the appearance
# convert from SE2 to SE2Transform representation
transforms_sequence = poses_sequence.transform_values(SE2Transform.from_SE2)
# puts the object in the world with a certain "ground_truth" constraint
dw.set_object(ego_name, db, ground_truth=transforms_sequence)
# Rule evaluation (do not touch)
interval = SampledSequence.from_iterator(enumerate(commands_sequence.timestamps))
evaluated = evaluate_rules(
poses_sequence=transforms_sequence,
interval=interval,
world=dw,
ego_name=ego_name,
)
timeseries = make_timeseries(evaluated)
# Drawing
area = RectangularArea((0, 0), (3, 3))
draw_static(dw, outdir, area=area, timeseries=timeseries)