Source code for bdsim.blocks.robots

"""
Robot blocks:
- have inputs and outputs
- are a subclass of ``FunctionBlock`` |rarr| ``Block`` for kinematics and have no states
- are a subclass of ``TransferBlock`` |rarr| ``Block`` for dynamics and have states

"""
# The constructor of each class ``MyClass`` with a ``@block`` decorator becomes a method ``MYCLASS()`` of the BlockDiagram instance.


# TODO: quadrotor dyanmics and display

import numpy as np
from math import sin, cos, atan2, tan, sqrt, pi

import matplotlib.pyplot as plt
import time

from bdsim.components import TransferBlock, block

# ------------------------------------------------------------------------ #
[docs]@block class Bicycle(TransferBlock): """ :blockname:`BICYCLE` .. table:: :align: left +------------+---------+---------+ | inputs | outputs | states | +------------+---------+---------+ | 2 | 3 | 3 | +------------+---------+---------+ | float | float | | +------------+---------+---------+ """
[docs] def __init__(self, *inputs, x0=None, L=1, vlim=1, slim=1, **kwargs): """ :param ``*inputs``: Optional incoming connections :type ``*inputs``: Block or Plug :param x0: Inital state, defaults to 0 :type x0: array_like, optional :param L: Wheelbase, defaults to 1 :type L: float, optional :param vlim: Velocity limit, defaults to 1 :type vlim: float, optional :param slim: Steering limit, defaults to 1 :type slim: float, optional :param ``**kwargs``: common Block options :return: a BICYCLE block :rtype: Bicycle instance Create a vehicle model with Bicycle kinematics. Bicycle kinematic model with state :math:`[x, y, \theta]`. The block has two input ports: 1. Vehicle speed (metres/sec). The velocity limit ``vlim`` is applied to the magnitude of this input. 2. Steering wheel angle (radians). The steering limit ``slim`` is applied to the magnitude of this input. and three output ports: 1. x position in the world frame (metres) 2. y positon in the world frame (metres) 3. heading angle with respect to the world frame (radians) """ super().__init__(nin=2, nout=3, inputs=inputs, **kwargs) self.nstates = 3 self.vlim = vlim self.slim = slim self.type = 'bicycle' self.L = L if x0 is None: self._x0 = np.zeros((self.nstates,)) else: assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates) self._x0 = x0 self.inport_names(('v', '$\gamma$')) self.outport_names(('x', 'y', r'$\theta$')) self.state_names(('x', 'y', r'$\theta$'))
def output(self, t): return list(self._x) def deriv(self): theta = self._x[2] # get inputs and clip them v = self.inputs[0] v = min(self.vlim, max(v, -self.vlim)) gamma = self.inputs[1] gamma = min(self.slim, max(gamma, -self.slim)) xd = np.r_[v * cos(theta), v * sin(theta), v * tan(gamma)/self.L ] return xd
# ------------------------------------------------------------------------ #
[docs]@block class Unicycle(TransferBlock): """ :blockname:`UNICYCLE` .. table:: :align: left +------------+---------+---------+ | inputs | outputs | states | +------------+---------+---------+ | 2 | 3 | 3 | +------------+---------+---------+ | float | float | | +------------+---------+---------+ """
[docs] def __init__(self, *inputs, x0=None, **kwargs): r""" :param ``*inputs``: Optional incoming connections :type ``*inputs``: Block or Plug :param x0: Inital state, defaults to 0 :type x0: array_like, optional :param ``*inputs``: Optional incoming connections :type ``*inputs``: Block or Plug :param ``**kwargs``: common Block options :return: a UNICYCLE block :rtype: Unicycle instance Create a vehicle model with Unicycle kinematics. Unicycle kinematic model with state :math:`[x, y, \theta]`. The block has two input ports: 1. Vehicle speed (metres/sec). 2. Angular velocity (radians/sec). and three output ports: 1. x position in the world frame (metres) 2. y positon in the world frame (metres) 3. heading angle with respect to the world frame (radians) """ super().__init__(nin=2, nout=3, inputs=inputs, **kwargs) self.nstates = 3 self.type = 'unicycle' if x0 is None: self._x0 = np.zeros((slef.nstates,)) else: assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates) self._x0 = x0
def output(self, t): return list(self._x) def deriv(self): theta = self._x[2] v = self.inputs[0] omega = self.inputs[1] xd = np.r_[v * cos(theta), v * sin(theta), omega] return xd
# ------------------------------------------------------------------------ #
[docs]@block class DiffSteer(TransferBlock): """ :blockname:`DIFFSTEER` .. table:: :align: left +------------+---------+---------+ | inputs | outputs | states | +------------+---------+---------+ | 2 | 3 | 3 | +------------+---------+---------+ | float | float | | +------------+---------+---------+ """
[docs] def __init__(self, *inputs, R=1, W=1, x0=None, **kwargs): r""" :param ``*inputs``: Optional incoming connections :type ``*inputs``: Block or Plug :param x0: Inital state, defaults to 0 :type x0: array_like, optional :param R: Wheel radius, defaults to 1 :type R: float, optional :param W: Wheel separation in lateral direction, defaults to 1 :type R: float, optional :param ``**kwargs``: common Block options :return: a DIFFSTEER block :rtype: DifSteer instance Create a differential steer vehicle model with Unicycle kinematics, with inputs given as wheel angular velocity. Unicycle kinematic model with state :math:`[x, y, \theta]`. The block has two input ports: 1. Left-wheel angular velocity (radians/sec). 2. Right-wheel angular velocity (radians/sec). and three output ports: 1. x position in the world frame (metres) 2. y positon in the world frame (metres) 3. heading angle with respect to the world frame (radians) Note: - wheel velocity is defined such that if both are positive the vehicle moves forward. """ super().__init__(nin=2, nout=3, inputs=inputs, **kwargs) self.nstates = 3 self.type = 'diffsteer' self.R = R self.W = W if x0 is None: self._x0 = np.zeros((slef.nstates,)) else: assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates) self._x0 = x0
def output(self, t): return list(self._x) def deriv(self): theta = self._x[2] v = self.R * (self.inputs[0] + self.inputs[1]) / 2 omega = (self.inputs[1] + self.inputs[0]) / self.W xd = np.r_[v * cos(theta), v * sin(theta), omega] return xd
# seriallink # RNE # fkine # robot plot # ------------------------------------------------------------------------ #
[docs]@block class MultiRotor(TransferBlock): """ :blockname:`MULTIROTOR` .. table:: :align: left +------------+---------+---------+ | inputs | outputs | states | +------------+---------+---------+ | 1 | 1 | 16 | +------------+---------+---------+ | A(4,) | dict | | +------------+---------+---------+ """ # Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04 # A simulation of idealised X-4 Flyer II flight dynamics. # version 2.0 2005 modified to be compatible with latest version of Matlab # version 3.0 2006 fixed rotation matrix problem # version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring # version 5.0 8/8/11, simplified and restructured # version 6.0 25/10/13, fixed rotation matrix/inverse wronskian definitions, flapping cross-product bug # # New in version 2: # - Generalised rotor thrust model # - Rotor flapping model # - Frame aerodynamic drag model # - Frame aerodynamic surfaces model # - Internal motor model # - Much coolage # # Version 1.3 # - Rigid body dynamic model # - Rotor gyroscopic model # - External motor model # # ARGUMENTS # u Reference inputs 1x4 # tele Enable telemetry (1 or 0) 1x1 # crash Enable crash detection (1 or 0) 1x1 # init Initial conditions 1x12 # # INPUTS # u = [N S E W] # NSEW motor commands 1x4 # # CONTINUOUS STATES # z Position 3x1 (x,y,z) # v Velocity 3x1 (xd,yd,zd) # n Attitude 3x1 (Y,P,R) # o Angular velocity 3x1 (wx,wy,wz) # w Rotor angular velocity 4x1 # # Notes: z-axis downward so altitude is -z(3) # # CONTINUOUS STATE MATRIX MAPPING # x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4] # # # CONTINUOUS STATE EQUATIONS # z` = v # v` = g*e3 - (1/m)*T*R*e3 # I*o` = -o X I*o + G + torq # R = f(n) # n` = inv(W)*o #
[docs] def __init__(self, model, *inputs, groundcheck=True, speedcheck=True, x0=None, **kwargs): r""" :param model: Vehicle geometric and inertial parameters :type model: dict :param ``*inputs``: Optional incoming connections :type ``*inputs``: Block or Plug :param groundcheck: Prevent vehicle moving below ground , defaults to True :type groundcheck: bool :param speedcheck: Check for zero rotor speed, defaults to True :type speedcheck: bool :param x0: Initial state, defaults to all zeros :type x0: TYPE, optional :param ``**kwargs``: common Block options :return: a MULTIROTOR block :rtype: MultiRotor instance Create a a multi-rotor dynamic model block. The block has one input port which is a vector of input rotor speeds in (radians/sec). These are, looking down, clockwise from the front rotor which lies on the x-axis. The block has one output port which is a dictionary signal with the following items: - ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]` - ``vb`` translational velocity in the world frame (metres/sec) - ``w`` angular rates in the world frame as yaw-pitch-roll rates (radians/second) - ``a1s`` longitudinal flapping angles (radians) - ``b1s`` lateral flapping angles (radians) Based on MATLAB code developed by Pauline Pounds 2004. """ super().__init__(nin=1, nout=1, inputs=inputs, **kwargs) self.type = 'quadrotor' try: nrotors = model['nrotors'] except KeyError: raise RuntimeError('vehicle model does not contain nrotors') assert nrotors % 2 == 0, 'Must have an even number of rotors' self.nstates = 12 if x0 is not None: assert len(x0) == self.nstates, "x0 is the wrong length" else: x0 = np.zeros((self.nstates,)) self._x0 = x0 self.nrotors = nrotors self.model = model self.groundcheck = groundcheck self.speedcheck = speedcheck self.D = np.zeros((3,self.nrotors)) for i in range(0, self.nrotors): theta = i / self.nrotors * 2 * pi # Di Rotor hub displacements (1x3) # first rotor is on the x-axis, clockwise order looking down from above self.D[:,i] = np.r_[ model['d'] * cos(theta), model['d'] * sin(theta), model['h']] self.a1s = np.zeros((self.nrotors,)) self.b1s = np.zeros((self.nrotors,))
def output(self, t=None): model = self.model # compute output vector as a function of state vector # z Position 3x1 (x,y,z) # v Velocity 3x1 (xd,yd,zd) # n Attitude 3x1 (Y,P,R) # o Angular velocity 3x1 (Yd,Pd,Rd) n = self._x[3:6] # RPY angles phi = n[0] # yaw the = n[1] # pitch psi = n[2] # roll # rotz(phi)*roty(the)*rotx(psi) # BBF > Inertial rotation matrix R = np.array([ [cos(the) * cos(phi), sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)], [cos(the) * sin(phi), sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi), cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi)], [-sin(the), sin(psi) * cos(the), cos(psi) * cos(the)] ]) #inverted Wronskian iW = np.array([ [0, sin(psi), cos(psi)], [0, cos(psi) * cos(the), -sin(psi) * cos(the)], [cos(the), sin(psi) * sin(the), cos(psi) * sin(the)] ]) / cos(the) # return velocity in the body frame out = {} out['x'] = self._x[0:6] out['vb'] = np.linalg.inv(R) @ self._x[6:9] # translational velocity mapped to body frame out['w'] = iW @ self._x[9:12] # RPY rates mapped to body frame out['a1s'] = self.a1s out['b1s'] = self.b1s return [out] def deriv(self): model = self.model # Body-fixed frame references # ei Body fixed frame references 3x1 e3 = np.r_[0, 0, 1] # process inputs w = self.inputs[0] if len(w) != self.nrotors: raise RuntimeError('input vector wrong size') if self.speedcheck and np.any(w == 0): # might need to fix this, preculudes aerobatics :( # mu becomes NaN due to 0/0 raise RuntimeError('quadrotor_dynamics: not defined for zero rotor speed'); # EXTRACT STATES FROM X z = self._x[0:3] # position in {W} n = self._x[3:6] # RPY angles {W} v = self._x[6:9] # velocity in {W} o = self._x[9:12] # angular velocity in {W} # PREPROCESS ROTATION AND WRONSKIAN MATRICIES phi = n[0] # yaw the = n[1] # pitch psi = n[2] # roll # rotz(phi)*roty(the)*rotx(psi) # BBF > Inertial rotation matrix R = np.array([ [cos(the)*cos(phi), sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi), cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi)], [cos(the)*sin(phi), sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi), cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi)], [-sin(the), sin(psi)*cos(the), cos(psi)*cos(the)] ]) # Manual Construction # Q3 = [cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1]; % RZ %Rotation mappings # Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; % RY # Q1 = [1 0 0;0 cos(psi) -sin(psi);0 sin(psi) cos(psi)]; % RX # R = Q3*Q2*Q1 %Rotation matrix # # RZ * RY * RX # inverted Wronskian iW = np.array([ [0, sin(psi), cos(psi)], [0, cos(psi)*cos(the), -sin(psi)*cos(the)], [cos(the), sin(psi)*sin(the), cos(psi)*sin(the)] ]) / cos(the) # ROTOR MODEL T = np.zeros((3,4)) Q = np.zeros((3,4)) tau = np.zeros((3,4)) a1s = self.a1s b1s = self.b1s for i in range(0, self.nrotors): # for each rotor # Relative motion Vr = np.cross(o, self.D[:,i]) + v mu = sqrt(np.sum(Vr[0:2]**2)) / (abs(w[i]) * model['r']) # Magnitude of mu, planar components lc = Vr[2] / (abs(w[i]) * model['r']) # Non-dimensionalised normal inflow li = mu # Non-dimensionalised induced velocity approximation alphas = atan2(lc, mu) j = atan2(Vr[1], Vr[0]) # Sideslip azimuth relative to e1 (zero over nose) J = np.array([ [cos(j), -sin(j)], [sin(j), cos(j)] ]) # BBF > mu sideslip rotation matrix # Flapping beta = np.array([ [((8/3*model['theta0'] + 2 * model['theta1']) * mu - 2 * lc * mu) / (1 - mu**2 / 2)], # Longitudinal flapping [0] # Lattitudinal flapping (note sign) ]) # sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)]; beta = J.T @ beta; # Rotate the beta flapping angles to longitudinal and lateral coordinates. a1s[i] = beta[0] - 16 / model['gamma'] / abs(w[i]) * o[1] b1s[i] = beta[1] - 16 / model['gamma'] / abs(w[i]) * o[0] # Forces and torques # Rotor thrust, linearised angle approximations T[:,i] = model['Ct'] * model['rho'] * model['A'] * model['r']**2 * w[i]**2 * \ np.r_[-cos(b1s[i]) * sin(a1s[i]), sin(b1s[i]), -cos(a1s[i])*cos(b1s[i])] # Rotor drag torque - note that this preserves w[i] direction sign Q[:,i] = -model['Cq'] * model['rho'] * model['A'] * model['r']**3 * w[i] * abs(w[i])* e3 tau[:,i] = np.cross(T[:,i], self.D[:,i]) # Torque due to rotor thrust # RIGID BODY DYNAMIC MODEL dz = v dn = iW @ o dv = model['g'] * e3 + R @ np.sum(T, axis=1) / model['M'] # vehicle can't fall below ground, remember z is down if self.groundcheck and z[2] > 0: z[0] = 0 dz[0] = 0 do = np.linalg.inv(model['J']) @ (np.cross(-o, model['J'] @ o) + np.sum(tau, axis=1) + np.sum(Q, axis=1)) # row sum of torques # stash the flapping information for plotting self.a1s = a1s self.b1s = b1s return np.r_[dz, dn, dv, do] # This is the state derivative vector
if __name__ == "__main__": import pathlib import os.path exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_robots.py")).read())