forked from duckietown/planning-exercises
/
implementation.py
70 lines (53 loc) · 2.07 KB
/
implementation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
from duckietown_world import GenericKinematicsSE2,PlatformDynamicsFactory
from duckietown_serialization_ds1 import Serializable
from contracts import check_isinstance
import geometry as geo
import numpy as np
import math
class CarCommands(Serializable):
'''
This represents the velocity commands for car
kinematics.
'''
def __init__(self, linear_velocity, steering_angle):
self.linear_velocity = linear_velocity
self.steering_angle = steering_angle
class CarParameters(PlatformDynamicsFactory, Serializable):
'''
This class represents the parameters of the ideal differential drive dynamics.
wheel_distance: distance between front and rear wheels
'''
def __init__(self, wheel_distance):
self.wheel_distance = wheel_distance
def initialize(self, c0, t0=0, seed=None):
return CarDynamics(self, c0, t0)
class CarDynamics(GenericKinematicsSE2):
def __init__(self, parameters, c0, t0):
"""
:param parameters: instance of CarParameters
:param c0: initial configuration
:param t0: initial time
"""
check_isinstance(parameters, CarParameters)
self.parameters = parameters
GenericKinematicsSE2.__init__(self, c0, t0)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# Your code comes here!
q,_ = self.TSE2_from_state()
_, direction = geo.translation_angle_from_SE2(q)
linear = [commands.linear_velocity, 0]
angular = (commands.linear_velocity / self.parameters.wheel_distance) * math.tan(commands.steering_angle)
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return CarDynamics(self.parameters, c1, t1)