Vehicle Researcher
8 years ago
parent
07238569c7
commit
ee8459a55f
29 changed files with 564 additions and 185 deletions
@ -1 +1 @@ |
||||
Subproject commit b77861eb00d45e25af501f78bbed155d2df06159 |
||||
Subproject commit 008089045e371a9eebbe79d978ff22ae3e70bdba |
||||
@ -1 +1 @@ |
||||
Subproject commit 5409c51041cfe8f139650a4e0decf4f6d863eb07 |
||||
Subproject commit ff8a4bd4e844c2b0a0dd7efb321b31781d8034c8 |
||||
@ -1 +1 @@ |
||||
#define OPENPILOT_VERSION "0.3.5" |
||||
#define OPENPILOT_VERSION "0.3.6" |
||||
|
||||
@ -0,0 +1,87 @@ |
||||
#!/usr/bin/env python |
||||
import numpy as np |
||||
from numpy.linalg import inv |
||||
from selfdrive.car.honda.interface import CarInterface |
||||
|
||||
## dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## |
||||
|
||||
# Xdot = A*X + B*U |
||||
# where X = [v, r], with v and r lateral speed and rotational speed, respectively |
||||
# and U is the steering angle (controller input) |
||||
# |
||||
# A depends on longitudinal speed, u, and vehicle parameters CP |
||||
|
||||
def create_dyn_state_matrices(u, CP): |
||||
A = np.zeros((2,2)) |
||||
B = np.zeros((2,1)) |
||||
A[0,0] = - (CP.cF + CP.cR)/(CP.m*u) |
||||
A[0,1] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.m*u) - u |
||||
A[1,0] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.j*u) |
||||
A[1,1] = - (CP.cF*CP.aF**2 + CP.cR*CP.aR**2) / (CP.j*u) |
||||
B[0,0] = (CP.cF + CP.chi*CP.cR) / CP.m / CP.sR |
||||
B[1,0] = (CP.cF*CP.aF - CP.chi*CP.cR*CP.aR) / CP.j / CP.sR |
||||
return A, B |
||||
|
||||
|
||||
def kin_ss_sol(sa, u, CP): |
||||
# kinematic solution, useful when speed ~ 0 |
||||
K = np.zeros((2,1)) |
||||
K[0,0] = CP.aR / CP.sR / CP.l * u |
||||
K[1,0] = 1. / CP.sR / CP.l * u |
||||
return K * sa |
||||
|
||||
|
||||
def dyn_ss_sol(sa, u, CP): |
||||
# Dynamic solution, useful when speed > 0 |
||||
A, B = create_dyn_state_matrices(u, CP) |
||||
return - np.matmul(inv(A), B) * sa |
||||
|
||||
def calc_slip_factor(CP): |
||||
# the slip factor is a measure of how the curvature changes with speed |
||||
# it's positive for Oversteering vehicle, negative (usual case) otherwise |
||||
return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR) |
||||
|
||||
class VehicleModel(object): |
||||
def __init__(self, CP, init_state=np.asarray([[0.],[0.]])): |
||||
self.dt = 0.1 |
||||
lookahead = 2. # s |
||||
self.steps = int(lookahead / self.dt) |
||||
self.update_state(init_state) |
||||
self.state_pred = np.zeros((self.steps, self.state.shape[0])) |
||||
self.CP = CP |
||||
|
||||
def update_state(self, state): |
||||
self.state = state |
||||
|
||||
def steady_state_sol(self, sa, u): |
||||
# if the speed is too small we can't use the dynamic model |
||||
# (tire slip is undefined), we then use the kinematic model |
||||
if u > 0.1: |
||||
return dyn_ss_sol(sa, u, self.CP) |
||||
else: |
||||
return kin_ss_sol(sa, u, self.CP) |
||||
|
||||
def calc_curvature(self, sa, u): |
||||
# this formula can be derived from state equations in steady state conditions |
||||
sf = calc_slip_factor(self.CP) |
||||
return (1. - self.CP.chi)/(1. - sf * u**2) * sa / self.CP.sR / self.CP.l |
||||
|
||||
def get_steer_from_curvature(self, curv, u): |
||||
# this function is the exact inverse of calc_curvature, returning steer angle given curvature |
||||
sf = calc_slip_factor(self.CP) |
||||
return self.CP.l * self.CP.sR * (1. - sf * u**2) / (1. - self.CP.chi) * curv |
||||
|
||||
def state_prediction(self, sa, u): |
||||
# U is the matrix of the controls |
||||
# u is the long speed |
||||
A, B = create_dyn_state_matrices(u, self.CP) |
||||
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt |
||||
|
||||
|
||||
if __name__ == '__main__': |
||||
# load car params |
||||
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {}) |
||||
print CP |
||||
VM = VehicleModel(CP) |
||||
print VM.steady_state_sol(.1, 0.15) |
||||
|
||||
@ -0,0 +1,26 @@ |
||||
CC = clang
|
||||
CXX = clang++
|
||||
|
||||
WARN_FLAGS = -Werror=implicit-function-declaration \
|
||||
-Werror=incompatible-pointer-types \
|
||||
-Werror=int-conversion \
|
||||
-Werror=return-type \
|
||||
-Werror=format-extra-args
|
||||
|
||||
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
|
||||
|
||||
.PHONY: all |
||||
all: libvisionipc.so |
||||
|
||||
visionipc.o: ../../common/visionipc.c ../../common/visionipc.h |
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -MMD \
|
||||
-I../.. -I../../.. \
|
||||
-c -o '$@' ../../common/visionipc.c
|
||||
|
||||
libvisionipc.so: visionipc.o |
||||
$(CC) -shared -fPIC -o '$@' visionipc.o
|
||||
|
||||
.PHONY: clean |
||||
clean: |
||||
rm visionipc.o libvisionipc.so
|
||||
@ -0,0 +1,93 @@ |
||||
#!/usr/bin/env python |
||||
import os |
||||
import time |
||||
import subprocess |
||||
from cffi import FFI |
||||
import ctypes |
||||
|
||||
import numpy as np |
||||
|
||||
gf_dir = os.path.dirname(os.path.abspath(__file__)) |
||||
|
||||
subprocess.check_call(["make"], cwd=gf_dir) |
||||
|
||||
|
||||
ffi = FFI() |
||||
ffi.cdef(""" |
||||
|
||||
typedef enum VisionStreamType { |
||||
VISION_STREAM_UI_BACK, |
||||
VISION_STREAM_UI_FRONT, |
||||
VISION_STREAM_YUV, |
||||
VISION_STREAM_MAX, |
||||
} VisionStreamType; |
||||
|
||||
typedef struct VisionUIInfo { |
||||
int big_box_x, big_box_y; |
||||
int big_box_width, big_box_height; |
||||
int transformed_width, transformed_height; |
||||
|
||||
int front_box_x, front_box_y; |
||||
int front_box_width, front_box_height; |
||||
} VisionUIInfo; |
||||
|
||||
typedef struct VisionStreamBufs { |
||||
VisionStreamType type; |
||||
|
||||
int width, height, stride; |
||||
size_t buf_len; |
||||
|
||||
union { |
||||
VisionUIInfo ui_info; |
||||
} buf_info; |
||||
} VisionStreamBufs; |
||||
|
||||
typedef struct VisionBuf { |
||||
int fd; |
||||
size_t len; |
||||
void* addr; |
||||
} VisionBuf; |
||||
|
||||
typedef struct VisionBufExtra { |
||||
uint32_t frame_id; // only for yuv |
||||
} VisionBufExtra; |
||||
|
||||
typedef struct VisionStream { |
||||
int ipc_fd; |
||||
int last_idx; |
||||
int num_bufs; |
||||
VisionStreamBufs bufs_info; |
||||
VisionBuf *bufs; |
||||
} VisionStream; |
||||
|
||||
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info); |
||||
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra); |
||||
void visionstream_destroy(VisionStream *s); |
||||
|
||||
""" |
||||
) |
||||
|
||||
clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so")) |
||||
|
||||
|
||||
def getframes(): |
||||
s = ffi.new("VisionStream*") |
||||
buf_info = ffi.new("VisionStreamBufs*") |
||||
err = clib.visionstream_init(s, clib.VISION_STREAM_UI_BACK, True, buf_info) |
||||
assert err == 0 |
||||
|
||||
w = buf_info.width |
||||
h = buf_info.height |
||||
assert buf_info.stride == w*3 |
||||
assert buf_info.buf_len == w*h*3 |
||||
|
||||
while True: |
||||
buf = clib.visionstream_get(s, ffi.NULL) |
||||
|
||||
pbuf = ffi.buffer(buf.addr, buf.len) |
||||
yield np.frombuffer(pbuf, dtype=np.uint8).reshape((h, w, 3)) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
for buf in getframes(): |
||||
print buf.shape, buf[101, 101] |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:71026624dc5524417a9f569d35655deb790289631c641453c835046a50ba2a2a |
||||
size 1365152 |
||||
oid sha256:51d01404e86f6e1f1d2ecea67bfea72c8e568f03b4f839b4480b40c27993e685 |
||||
size 1367088 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:58e476760c0f87f13b68b75d64860fb5c57b5509b4174d31f466074e0423e7eb |
||||
size 918928 |
||||
oid sha256:fc4b862553d2ec9d63740b554997e5069808c531d9d722b4ce2cf83a2f4429ed |
||||
size 935360 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:2441337ea200d9750160c1e83a57da0431af2972801e4f2acb26dc110f46be77 |
||||
size 16399784 |
||||
oid sha256:e2dc36a454c0447387826dd0de64ee47e170012df0ec041189ae77d64aa6de3f |
||||
size 16407976 |
||||
|
||||
Loading…
Reference in new issue