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)
# compute the linear, angular velocities for the platform
# using the simple car equations
longitudinal = commands.linear_velocity
angular = commands.linear_velocity / self.parameters.wheel_distance * math.tan(
commands.steering_angle)
lateral = 0
linear = [longitudinal, lateral]
# 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)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# Your code comes here!
# linear_velocity = [x_dot, y_dot]
linear = [commands.linear_velocity, 0]
# angular = [theta_dot]
angular = commands.linear_velocity/self.parameters.wheel_distance * np.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)
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)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# calculate linear velocities
x_vel = commands.linear_velocity
y_vel = 0
linear = [x_vel, y_vel]
# calculate angular velocities
angular = commands.linear_velocity * math.tan(
commands.steering_angle) / self.parameters.wheel_distance
# 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)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# Your code comes here!
linear = [0, 0]
angular = 0
# 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)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
us = commands.linear_velocity
ut = commands.steering_angle
thetha = self.theta
L = self.parameters.wheel_distance
# Simple car equations.
linear = us * np.cos(thetha), us * np.sin(thetha)
angular = us / L * np.tan(ut)
# 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
# Update state.
self.theta = s1.q0
return CarDynamics(self.parameters, c1, t1)
