Dynamics tutorial

Packages

We will use the contracts and six libraries to create the classes, as well as all previously introduced libraries.

In [1]:
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

Abstract classes

In duckietown-world there is an interface for the dynamics of a platform. The interface is:

In [2]:
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.

Integrator2D

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.

In [3]:
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.

In [4]:
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 $

In [5]:
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)$

In [6]:
    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

In [7]:
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.

In [8]:
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)
Integrator2D(v0=[ 1.   0.5],p0=[ 1.   0.5],t0=1.0)
Integrator2D(v0=[ 1.   0.5],p0=[ 2.  1.],t0=2.0)
Integrator2D(v0=[ 1.   0.5],p0=[ 3.   1.5],t0=3.0)
Integrator2D(v0=[ 1.   0.5],p0=[ 4.  2.],t0=4.0)
Integrator2D(v0=[ 1.   0.5],p0=[ 5.   2.5],t0=5.0)

The system can also be represented as configuration in $TSE(2)$, and as pose and velocity in $SE(2)$ and $se(2)$, respectively.

In [9]:
config = system.TSE2_from_state()
pose,vel = config
print(config)
(array([[ 1. ,  0. ,  5. ],
       [ 0. ,  1. ,  2.5],
       [ 0. ,  0. ,  1. ]]), array([[ 0. , -0. ,  1. ],
       [ 0. ,  0. ,  0.5],
       [ 0. ,  0. ,  0. ]]))
In [10]:
print(pose)
[[ 1.   0.   5. ]
 [ 0.   1.   2.5]
 [ 0.   0.   1. ]]
In [11]:
print(vel)
[[ 0.  -0.   1. ]
 [ 0.   0.   0.5]
 [ 0.   0.   0. ]]

Also, the system can be initialized if we have the configuration in TSE2

In [12]:
same_system = Integrator2D.initialize(system,config)
print(same_system)
Integrator2D(v0=[ 1.   0.5],p0=[ 5.   2.5],t0=0)

GenericKinematicsSE2

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)$.

In [13]:
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

Differential Drive Dynamics

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.

In [14]:
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.

In [15]:
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.

In [16]:
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:

In [17]:
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):

In [18]:
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)))
    
pose: [ 25.76658237   4.33464048]
theta: 19.098593171
pose: [ 48.69662199  16.86137821]
theta: 38.1971863421
pose: [ 66.26584005  36.20119341]
theta: 57.2957795131
pose: [ 76.54010973  60.2250411 ]
theta: 76.3943726841
pose: [ 78.38837667  86.28822941]
theta: 95.4929658551

Exercise

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.

In [19]: