We will use the contracts
and six
libraries to create the classes,
as well as all previously introduced libraries.
from abc import ABCMeta
from contracts import contract, check_isinstance
from six import with_metaclass
from duckietown_serialization_ds1 import Serializable
import geometry as geo
import numpy as np
In duckietown-world
there is an interface for the dynamics of a platform. The interface is:
class PlatformDynamics(with_metaclass(ABCMeta)):
def integrate(self, dt, commands):
"""
Returns the result of applying commands for dt.
:param dt: time interval
:param commands: class-specific commands
:return: the next state
"""
def TSE2_from_state(self):
""" Returns pose, velocity for the state. """
class PlatformDynamicsFactory(with_metaclass(ABCMeta)):
def initialize(cls, c0, t0=0, seed=None):
"""
Returns the dynamics initalized at a certain configuration.
:param c0: configuration in TSE2
:param t0: time in which to sstart
:param seed: seed for a possible random number generator
"""
In order to explain it let's create a simple system with integrator dynamics in 2D.
The state of the system at time step $t$ is $x(t) = [p(t), v(t)]^T$, where $p\in \mathbb{R}^2$ is position, and $v \in \mathbb{R}^2$ is linear velocity.
The input commands to the system are sequences of linear velocities in 2D (sequences of 2 numbers) $u = [u_x, u_y]^T$.
Let's create our class Integrator2D
and constructor with initial parameters for position, velocity and time.
class Integrator2D(PlatformDynamicsFactory, PlatformDynamics, Serializable):
"""
This represents the dynamics of a 2D integrator.
Commands are sequences of 2 numbers / array of two numbers.
"""
def __init__(self, p0, v0, t0):
"""
:param p0: initial point
:param v0: initial velocity
:param t0: initial time
"""
self.t0 = t0
self.v0 = v0
self.p0 = p0
Now, we need to implement initialize(cls, c0, t0=0, seed=None)
class method from PlatformDynamicsFactory
abstract class.
The input variable is c0
which is a configuration in TSE2
, and the method must return the dynamics, basically instance of Integrator2D
class.
Note that $TSE(2)$ is a tangent space which will be represented as a tuple $(q,v)$ where $q\in SE(2)$ and $v \in se(2)$
Recall that the set of velocities on $SE(2)$ is called the lie algebra $se(2)$.
To do so, we must get the position from pose in $SE(2)$ and linear velocity from $se(2)$, and the library geometry
has the appropriate functions.
def initialize(cls, c0, t0=0, seed=None):
"""
This class initializes the dynamics at a given configuration
"""
# pose, velocity in SE(2), se(2)
q, v = c0
# get position p from pose
p, theta = geo.translation_angle_from_SE2(q)
# get linear velocity from se(2)
v2d, _ = geo.linear_angular_from_se2(v)
# create the integrator2d initial state
return Integrator2D(p, v2d, t0)
The method integrate
for 2D case is simple. It takes dt
as time $\Delta t$ for how long to apply the commands
, commands are $u = [u_x, u_y]^T$.
$ x(t+\Delta t) = [p(t+\Delta t), v(t+\Delta t)]^T$, where:
$ p(t+\Delta t) = p(t) + \Delta t \cdot u$
$ v(t+\Delta t) = u $
def integrate(self, dt, commands):
"""
Returns the next state after applying commands for time dt.
:param dt: for how long to apply the commands
:param commands: sequences of two numbers
:return: another Integrator2D
"""
# convert things to float, array
dt = float(dt)
commands = np.array(commands, np.float64)
# time incremensts by dt
t1 = self.t0 + dt
# we integrate commands
p1 = self.p0 + dt * commands
# the velocity is the commands
v1 = commands
return Integrator2D(p1, v1, t1)
Finally, the last method to implement is TSE2_from_state()
which returns a configuration in TSE2
, a tuple (q,v)
where $q\in SE(2)$ and $v \in se(2)$
def TSE2_from_state(self):
"""
For visualization purposes, this function gets a configuration in SE2
from the internal state.
"""
# pose
q = geo.SE2_from_R2(self.p0)
# velocity
linear = self.v0
angular = 0.0
v = geo.se2_from_linear_angular(linear, angular)
return q, v
Let's test the created class with a simple example
class Integrator2D(PlatformDynamicsFactory, PlatformDynamics, Serializable):
"""
This represents the dynamics of a 2D integrator.
Commands are sequences of 2 numbers / array of two numbers.
"""
def __init__(self, p0, v0, t0):
"""
:param p0: initial point
:param v0: initial velocity
:param t0: initial time
"""
self.t0 = t0
self.v0 = v0
self.p0 = p0
def initialize(cls, c0, t0=0, seed=None):
"""
This class initializes the dynamics at a given configuration
"""
# pose, velocity in SE(2), se(2)
q, v = c0
# get position p from pose
p, theta = geo.translation_angle_from_SE2(q)
# get linear velocity from se(2)
v2d, _ = geo.linear_angular_from_se2(v)
# create the integrator2d initial state
return Integrator2D(p, v2d, t0)
def integrate(self, dt, commands):
"""
Returns the next state after applying commands for time dt.
:param dt: for how long to apply the commands
:param commands: sequences of two numbers
:return: another Integrator2D
"""
# convert things to float, array
dt = float(dt)
commands = np.array(commands, np.float64)
# time incremensts by dt
t1 = self.t0 + dt
# we integrate commands
p1 = self.p0 + dt * commands
# the velocity is the commands
v1 = commands
return Integrator2D(p1, v1, t1)
def TSE2_from_state(self):
"""
For visualization purposes, this function gets a configuration in SE2
from the internal state.
"""
# pose
q = geo.SE2_from_R2(self.p0)
# velocity
linear = self.v0
angular = 0.0
v = geo.se2_from_linear_angular(linear, angular)
return q, v
To test the class, let's initialize the system at zero initial conditions and apply the same commands every second.
init_pose = [0,0]
init_vel = [0,0]
init_time = 0
system = Integrator2D(init_pose,init_vel,init_time)
commands = [1,0.5]
delta_t = 1
for i in range(5):
system = system.integrate(delta_t,commands)
print(system)
The system can also be represented as configuration in $TSE(2)$, and as pose and velocity in $SE(2)$ and $se(2)$, respectively.
config = system.TSE2_from_state()
pose,vel = config
print(config)
print(pose)
print(vel)
Also, the system can be initialized if we have the configuration in TSE2
same_system = Integrator2D.initialize(system,config)
print(same_system)
The next class we are going to explain as an example is GenericKinematicsSE2
. We give the developed class below and explain the parts as it is similar to the previous example except that we don't have the conversions anymore as the pose is directly in $SE(2)$, and all velocities and commands must belong to $se(2)$.
class GenericKinematicsSE2(PlatformDynamicsFactory, PlatformDynamics, Serializable):
"""
Any dynamics on SE(2)
Commands = velocities in se(2)
"""
@classmethod
@contract(c0='TSE2')
def initialize(cls, c0, t0=0, seed=None):
return GenericKinematicsSE2(c0, t0)
@contract(c0='TSE2')
def __init__(self, c0, t0):
# start at q0, v0
q0, v0 = c0
geo.SE2.belongs(q0)
geo.se2.belongs(v0)
self.t0 = t0
self.v0 = v0
self.q0 = q0
def integrate(self, dt, commands):
""" commands = velocity in body frame """
# convert to float
dt = float(dt)
# the commands must belong to se(2)
geo.se2.belongs(commands)
v = commands
# suppose we hold v for dt, which pose are we going to?
diff = geo.SE2.group_from_algebra(dt * v) # exponential map
# compute the absolute new pose; applying diff from q0
q1 = geo.SE2.multiply(self.q0, diff)
# the new configuration
c1 = q1, v
# the new time
t1 = self.t0 + dt
# return the new state
return GenericKinematicsSE2(c1, t1)
@contract(returns='TSE2')
def TSE2_from_state(self):
return self.q0, self.v0
Now let's create a platform with differential drive dynamics DifferentialDriveDynamics
based on GenericKinematicsSE2
. In order to do that, we will create two additional classes for parameters DifferentialDriveDynamicsParameters
and commands WheelVelocityCommands
.
WheelVelocityCommands
is a simple class with left and right wheels velocitiy commands for differential drive kinematics expressed in $[rad/s]$. If both of the commands are positive the car moves forward.
class WheelVelocityCommands(Serializable):
def __init__(self, left_wheel_angular_velocity, right_wheel_angular_velocity):
self.left_wheel_angular_velocity = left_wheel_angular_velocity
self.right_wheel_angular_velocity = right_wheel_angular_velocity
DifferentialDriveDynamicsParameters
represents the parameters of the ideal differential drive dynamics. The parameters are left and right wheel radii and the weel distance.
class DifferentialDriveDynamicsParameters(PlatformDynamicsFactory, Serializable):
'''
This class represents the parameters of the ideal differential drive dynamics.
radius_left, radius_right: wheels radii
wheel_distance: distance between two wheels
'''
def __init__(self, radius_left, radius_right, wheel_distance):
self.radius_left = radius_left
self.radius_right = radius_right
self.wheel_distance = wheel_distance
def initialize(self, c0, t0=0, seed=None):
return DifferentialDriveDynamics(self, c0, t0)
Finally, DifferentialDriveDynamics
is derived from GenericKinematicsSE2
and initialization parameters are the instances of DifferentialDriveDynamicsParameters
.
The method integrate
takes instances of WheelVelocityCommands
and computes linear velocity based on differential drive equations (see LaValle's book p. 726 ch.13.1.2.2).
The linear velocity is then represented as $se(2)$ velocity and integrate
method from GenericKinematicsSE2
is used.
class DifferentialDriveDynamics(GenericKinematicsSE2):
"""
This represents the state of differential drive.
This is a particular case of GenericKinematicsSE2.
"""
def __init__(self, parameters, c0, t0):
"""
:param parameters: instance of DifferentialDriveDynamicsParameters
:param c0: initial configuration
:param t0: initial time
"""
check_isinstance(parameters, DifferentialDriveDynamicsParameters)
self.parameters = parameters
GenericKinematicsSE2.__init__(self, c0, t0)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of WheelVelocityCommands
:return:
"""
check_isinstance(commands, WheelVelocityCommands)
# Compute the linear velocity for the wheels
# by multiplying radius times angular velocity
v_r = self.parameters.radius_right * commands.right_wheel_angular_velocity
v_l = self.parameters.radius_left * commands.left_wheel_angular_velocity
# compute the linear, angular velocities for the platform
# using the differential drive equations
longitudinal = (v_r + v_l) * 0.5
angular = (v_r - v_l) / self.parameters.wheel_distance
lateral = 0.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 DifferentialDriveDynamics(self.parameters, c1, t1)
Let us test our DifferentialDriveDynamics
class. To do this, create myDiff
and pass the following parameters and initial configuration:
left_radius = 2.5 # cm
right_radius = 2.5
wheel_dist = 7.5
init_pose = np.array([0,0])
init_vel = np.array([0,0])
init_time = 0
q0 = geo.SE2_from_R2(init_pose)
v0 = geo.se2_from_linear_angular(init_vel, 0)
c0= q0, v0
myCarParams = DifferentialDriveDynamicsParameters(left_radius, right_radius, wheel_dist)
myDiff = DifferentialDriveDynamics(myCarParams, c0, init_time)
Now, use this newly created myDiff
and update it according to a fixed set of commands (one command for simplicity):
del_t = 1
for i in range(5):
myCarCommands = WheelVelocityCommands(10, 11) # go straight ahead or turn in place
myDiff = myDiff.integrate(del_t, myCarCommands)
current_p, theta = geo.translation_angle_from_SE2(myDiff.q0)
print('pose: {}'.format(current_p))
print('theta: {}'.format(np.rad2deg(theta)))
Fork and clone the following repository: https://github.com/duckietown/planning-exercises
In the implementation.py
implement the equivalent CarDynamics according to the description of Lavalle’s book (see 13.1.2.1 A simple car).
Create a class CarDynamics
that integrates the dynamics of a car in the pattern of DifferentialDriveDynamics
, as well as classes CarCommmands
and CarParameters
.
CarCommands
has two variables (please make sure the order is respected):
linear_velocity
steering_angle
CarParameters
has one variable:
wheel_distance
To test your solution run test.py
in the repository.
Copy the implementation.py
file to ethz-fall2018-subs/Homework Assignment 4/AMOD18-ETH-last_name-duckiebot_name/
. Push your solution and make a pull request.
Deadline is 23:59:59 on Tuesday 13.11.2018.