AutowareをDockerでインストールする

NVIDIA GPUつきのUbuntuにインストールする

rocker --nvidia --x11 --user --volume $HOME/autoware_map -- ghcr.io/autowarefoundation/autoware-universe:humble-latest-prebuilt

autowarefoundation.github.io

上記でエラーが出たので以下を実行する

could not select device driver "" with capabilities: [[gpu]]. - #5 by sk.ahmed401 - Docker and NVIDIA Docker - NVIDIA Developer Forums
“Permission Denied While Trying to Connect to the Docker Daemon Socket” Error | Baeldung on Linux

sudo chmod 666 /var/run/docker.sock

sudo apt install -y nvidia-docker2
sudo systemctl daemon-reload
sudo systemctl restart docker

Rocker Docker Errors with python:3-slim-stretch - unable to detect os for base image · autowarefoundation · Discussion #3470 · GitHub

rocker --nvidia --x11 --user --volume $HOME/autoware --volume $HOME/autoware_map -- ghcr.io/autowarefoundation/autoware-universe:latest-cuda

SLERP（球面線形補間：クォータニオンによる大円補間）による補間

swkagami.hatenablog.com

(23)式を使えば、クォータニオンq1からクォータニオンq2への補間位置が逐次的に求められる。

このテクニックは「実践ロボット制御」でも単位クォータニオンを用いた大円補間として紹介されていた。

clear; close all;

origin = [1,0,0]';
destination = [1,1,1]';
q1 = [0,origin(1),origin(2),origin(3)]';
q2 = [0,destination(1),destination(2),destination(3)]';

phi = acos(dot(q1,q2)/(norm(q1)*norm(q2)));

plot3([0,q1(2)],[0,q1(3)],[0,q1(4)]);
hold on;
plot3([0,q2(2)],[0,q2(3)],[0,q2(4)]);

xlim([-2,2]);
ylim([-2,2]);
zlim([-2,2]);

for t=linspace(0,1,10)
    if(t==0)||(t==1)
        continue
    end
    tmp = sin((1-t)*phi)/sin(phi)*q1+sin(t*phi)/sin(phi)*q2;
    px = tmp(2);
    py = tmp(3);
    pz = tmp(4);
    plot3([0,px],[0,py],[0,pz],'k--');
    big;
    drawnow;
    pause(0.3);
end

クォータニオンによる回転演算子の意味

クォータニオンによる回転ではq⊗r⊗q*で回転を表現する。
qを回転行列Rのようにとらえれば、位置ベクトルの「前に」qを置くのは自然に思える。
ところで、なぜ回転を表すqとその共役q*で挟み込むのか、というのが納得できていない。

クォータニオンの積q⊗r⊗q*では、
ステップ１：tmp = q⊗r
という計算に続いて、
ステップ２：tmp⊗q*
という演算が行われている。
ステップ１の計算でオーバーランした分をステップ２で戻しているのかと思い、以下のコードで調べてみた。

clear; close all;

x=1;
y=1;
z=1;
P = [x;y;z];
O = [0;0;0];
vec = [O,P];
plot3(vec(1,:),vec(2,:),vec(3,:),'magenta','LineWidth',3);
hold on;

Xaxis = [1,0,0]';
Yaxis = [0,1,0]';
Zaxis = [0,0,1]';
OX = [O,Xaxis];
OY = [O,Yaxis];
OZ = [O,Zaxis];
plot3(OX(1,:),OX(2,:),OX(3,:),'r');
plot3(OY(1,:),OY(2,:),OY(3,:),'g');
plot3(OZ(1,:),OZ(2,:),OZ(3,:),'b');
xlim([-2,2]);
ylim([-2,2]);
zlim([-2,2]);

view(180,0);

for theta=linspace(0,2*pi*2,10)
    if (theta==0) || (theta==4*pi)
        continue
    end
    a = [0,1,0]';
    n = a/norm(a);
    x = P(1);
    y = P(2);
    z = P(3);
    nx = n(1);
    ny = n(2);
    nz = n(3);
    q1 = [0,x,y,z]';
    q2 = [cos(theta/2),nx*sin(theta/2),ny*sin(theta/2),nz*sin(theta/2)]';
    q3 = [cos(theta/2),-nx*sin(theta/2),-ny*sin(theta/2),-nz*sin(theta/2)]';

    q_tmp =  MultQuat(q2,q1);
    P2_tmp = q_tmp(2:4);
    vec2_tmp = [O,P2_tmp];
    plot3(vec2_tmp(1,:),vec2_tmp(2,:),vec2_tmp(3,:),'b--');

    big;
    drawnow;
    pause(0.3);

    q =  MultQuat(q_tmp,q3);
    P2 = q(2:4);
    vec2 = [O,P2];
    plot3(vec2(1,:),vec2(2,:),vec2(3,:),'k--');
    
    big;
    drawnow;
    pause(0.3);
end


function q = MultQuat(q1,q2)
pw = q1(1);
px = q1(2);
py = q1(3);
pz = q1(4);
qw = q2(1);
qx = q2(2);
qy = q2(3);
qz = q2(4);
q = [pw*qw-px*qx-py*qy-pz*qz;
    pw*qx+px*qw+py*qz-pz*qy;
    pw*qy-px*qz+py*qw+pz*qx;
    pw*qz+px*qy-py*qx+pz*qw];
end

青がステップ１の計算結果、黒がステップ２の計算結果である。
回転軸に垂直な平面でみると黒が青をカバーする事がわかる。

実際には黒と青は一致しているわけではなく、青線は３次元的に配置されている。
カンでは
青線（ステップ１の計算結果）がどういう物理的な意味を持っているのか、調べる必要がある。

ロドリゲスの公式による回転とクォータニオンによる回転

ロドリゲスの公式とクォータニオンによる回転の両方を実装する
クォータニオンには主にハミルトン方式とJPL方式の２つの定義があるが、ハミルトンを採用する。

最初に間違えてクォータニオンによる回転をq⊗rとしていた。qは回転を表すクォータニオン、rは位置ベクトルを表すクォータニオン。正しくはq⊗r⊗q*とすべき（q*は共役クォータニオン）。

ロドリゲスの公式による回転: 回転軸nは単位ベクトル


クォータニオンによる回転: 回転軸nは単位ベクトル


クォータニオンの積

clear; close all;

x=1;
y=1;
z=1;
P = [x;y;z];
O = [0;0;0];
vec = [O,P];
plot3(vec(1,:),vec(2,:),vec(3,:),'magenta','LineWidth',3);
title('ロドリゲスの公式による回転');
hold on;

Xaxis = [1,0,0]';
Yaxis = [0,1,0]';
Zaxis = [0,0,1]';
OX = [O,Xaxis];
OY = [O,Yaxis];
OZ = [O,Zaxis];
plot3(OX(1,:),OX(2,:),OX(3,:),'r');
plot3(OY(1,:),OY(2,:),OY(3,:),'g');
plot3(OZ(1,:),OZ(2,:),OZ(3,:),'b');
xlim([-2,2]);
ylim([-2,2]);
zlim([-2,2]);

view(180,0);

for theta=linspace(0,2*pi,10)
    if (theta==0) || (theta==2*pi)
        continue
    end
    a = [0,1,0]';
    n = a/norm(a);
    nx = n(1);
    ny = n(2);
    nz = n(3);
    R = rodriguez(nx,ny,nz,theta);
    P2 = R*P;
    vec2 = [O,P2];
    plot3(vec2(1,:),vec2(2,:),vec2(3,:),'k--');
    big;
    drawnow;

    pause(0.3);
end


figure;
plot3(vec(1,:),vec(2,:),vec(3,:),'magenta','LineWidth',3);
title('クォータニオンによる回転');
hold on;

Xaxis = [1,0,0]';
Yaxis = [0,1,0]';
Zaxis = [0,0,1]';
OX = [O,Xaxis];
OY = [O,Yaxis];
OZ = [O,Zaxis];
plot3(OX(1,:),OX(2,:),OX(3,:),'r');
plot3(OY(1,:),OY(2,:),OY(3,:),'g');
plot3(OZ(1,:),OZ(2,:),OZ(3,:),'b');
xlim([-2,2]);
ylim([-2,2]);
zlim([-2,2]);

view(180,0);
for theta=linspace(0,2*pi,10)
    if (theta==0) || (theta==2*pi)
        continue
    end
    a = [0,1,0]';
    n = a/norm(a);
    q = rotate(P,n(1),n(2),n(3),theta);
    P2 = q(2:4);
    vec2 = [O,P2];
    plot3(vec2(1,:),vec2(2,:),vec2(3,:),'k--');
    big;
    drawnow;

    pause(0.3);
end


function R = rodriguez(nx,ny,nz,theta)
C = cos(theta);
S = sin(theta);
R = [nx^2*(1-C)+C     nx*ny*(1-C)-nz*S nz*nx*(1-C)+ny*S;
    nx*ny*(1-C)-nz*S ny^2*(1-C)+C     ny*nz*(1-C)-nx*S;
    nz*nx*(1-C)-ny*S ny*nz*(1-C)+nx*S nz^2*(1-C)+C     ];
end


function q = rotate(P,nx,ny,nz,theta)  
  x = P(1);
  y = P(2);
  z = P(3);
  q1 = [0,x,y,z]';
  q2 = [cos(theta/2),nx*sin(theta/2),ny*sin(theta/2),nz*sin(theta/2)]';
  q3 = [cos(theta/2),-nx*sin(theta/2),-ny*sin(theta/2),-nz*sin(theta/2)]';
  qdash =  MultQuat(q2,q1);  
  q = MultQuat(qdash,q3);
end

function q = MultQuat(q1,q2)
  pw = q1(1);
  px = q1(2);
  py = q1(3);
  pz = q1(4);
  qw = q2(1);
  qx = q2(2);
  qy = q2(3);
  qz = q2(4);  
  q = [pw*qw-px*qx-py*qy-pz*qz;
       pw*qx+px*qw+py*qz-pz*qy;
       pw*qy-px*qz+py*qw+pz*qx;
       pw*qz+px*qy-py*qx+pz*qw]; 
end