ぱたへね

はてなダイアリーはrustの色分けができないのでこっちに来た

Modern Robotics: Mechanics, Planning, and Control のTwist

教科書から分かったところだけをまとめています。

教科書はこの本です。今回はTwist。 www.amazon.co.jp

Twist

教科書で最初に出てくるのは56ページ目。教科書によるとtwistは角速度と普通の速度の組み合わせを表しています。

Example 3.23からTwistを求めてみます。 車の回転の問題です。車の前輪が傾いているので、ある点を中心にグルグル回ります。回転の中心がr、回転の角速度がωとします。{s}から見たrの座標が(2,-1,0)、前輪がエンドエフェクタに相当してその座標系{b}からみたrの座標は(2, 1.4, 0)です。ωは2 rad/s とします。ここまでが与えられた値です。

f:id:natsutan:20211208205206p:plain
Figure 3.18

まず座標系{s}と{b}の関係を求めます。{s}からみた{b}の座標は(4,0.4,0)、{s}と{b}の座標軸を比べながら、前回の記事を見ながらまとめると、

natsutan.hatenablog.com

T_{sb} =  \begin{bmatrix} -1 & 0 & 0 & 4 \\ 0 & 1 & 0 & 0.4 \\ 0 & 0 & -1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 

Twistは、(ωx, ωy, ωz, vx, vy, vz).Tです。教科書によっては並びが違ったり、ωがベクトル表示になって文字数が減ったりしますが、Twistは必ず1x6の行列になります。ωは2 rad/sと与えられているので、角速度ωを使ってリニアな速度vを求めます。vとωは回転の中心rを使って、この関係があります。

v =  \dot p - \omega \times p = \dot p + \omega \times (-p) = \dot p + p \times \omega  

ここでdotのついたpはpの時間微分なので、pが動かない場合は0でよいはず。つまり、ωとpの外積でvが求められます。

Νs

本当はν(ニュー)だけど、vと見分けがつかないので大文字で。 {s}座標系のr、raは(2,-1,0)です。ωは右手系なので(0, 0, 2)です。

v_s = (2, -1, 0) \times (0, 0, 2) = (-2, -4, 0)
\nu_s = (0, 0, 2, -2 ,-4, 0)^T

Nb

{b}座標系でのr、rbは(2, 1.4, 0)、ωは右手系なので(0, 0, -2)です。

 v_b = (2, 1.4, 0) \times (0, 0, -2) = (2.8, 4, 0)
 \nu_b = (0, 0, -2, 2.8 ,4, 0)^T

Twistの行列表現

Twistは1x6の行列ですが、回転行列っぽい表現が使われることもあります。Twist Nsと区別するために[Ns]と表現されます(本当はν)。 関係式がこの3つ。

 \nu_s =  \begin{bmatrix} \omega_s \\ v_s  \end{bmatrix} \in \mathbb{R}^6  

 [\nu_s]  =  \begin{bmatrix}  [\omega_s] & v_s \\ 0 & 0  \end{bmatrix}  

 [\hat \omega ]  =  \begin{bmatrix}  0 & -\hat \omega_3 &  \hat \omega_2 \\  \hat \omega_3 & 0 & - \hat \omega_1  \\  -\hat \omega_2  & \hat \omega_1 & 0 \end{bmatrix}  

これを使うとNsはこうなります。右下が0な所が回転行列と違います。


 [\nu_s ]  =  \begin{bmatrix}  0 & -2 &  0 & -2 \\  2 & 0 & 0 & 4  \\  0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0  \end{bmatrix}  

Modern Robotics: Mechanics, Planning, and ControlのM

Modern Robotics: Mechanics, Planning, and Control

この本が教科書です。

www.amazon.co.jp

教科書には行列Mが何度も出てきます。

Mは教科書に始めて登場するのは117ページ目で、ロボットがzero positionにいる時の手の先の場所と向きを表す行列です。

簡単な例

f:id:natsutan:20211206214307p:plain
簡単な例

上の図がZero positionだとすると、Mはこうなります。

M =  \begin{bmatrix} 1 & 0 & 0 & L_1+L_2+L_3 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 

まず左上の3x3は座標軸の回転を表します。この図では回転していません。 画像処理で言うところの座標同士が歪まないので、一番下の段は(0, 0, 0, 1)で固定。numpy等でMを定義するとき右下の1を忘れがちなので注意。 一番右列が基準となる点からの(x,y,z)の座標。この組み合わせで上記のMになります。

難しい例

f:id:natsutan:20211206215531p:plain
Fig 4.3
教科書p121の図4.3から。

M =  \begin{bmatrix} 0 & 0 & 1 & L_1 \\ 0 & 1 & 0 & 0 \\ -1 & 0 & 0 & -L_2 \\ 0 & 0 & 0 & 1  \end{bmatrix} 

一番右の列は、基準となる座標系(\hat x_0,\hat  y_0, \hat  z_0)から見たエンドエフェクタの位置(座標)

(L_1, 0, -L_2)^T

を入れます。

ややこしいのは左上の3x3の部分です。 ここは基準となる座標系と、手の先の座標系のこの関係から持ってきます。

\begin{bmatrix}\hat  x_3  \\ \hat y_3  \\ \hat z_3   \end{bmatrix} =  \begin{bmatrix} 0 & 0 & 1  \\ 0 & 1 & 0  \\ -1 & 0 & 0   \end{bmatrix}\begin{bmatrix} \hat x_0 \\ \hat y_0  \\ \hat z_0   \end{bmatrix} 

下の段は(0,0,0,1)なので、合わせると上の行列Mになります。

Modern Robotics: Mechanics, Planning, and Control

CourseraのModern Robotics: Mechanics, Planning, and Controlを受講しています。

www.coursera.org

全部やると4週×6つの結構なボリュームがある講義です。 動画は5分くらいの短いのがいくつかあるだけで、基本的には指定された教科書を読んでQuizに挑戦していきます。ロボットシミュレータをイントールする課題がでましたが、今の所起動して視点を変えるくらいしか使っていないです。

教科書はこれです。

www.amazon.co.jp

最初のModern Robotics, Course 1: Foundations of Robot Motionは無事終了し、Modern Robotics, Course 2: Robot Kinematicsに入りました。

Quizを解くときは分かった気になるのですが、次の講義にいくとすっかり忘れてしまっていて、速度を落として勉強することにしました。今、詰まっている所は運動学と逆運動学で使うヤコビアンの所。

  • Mを確実に導出できるようにする。
  • Space Jacobianの導出
  • Slist とSpace Jacobianの違い
  • BlistとBody Jacobianの違い
  • Body Jacobianを確実に導出できるようにする。
  • Body Jacobianの意味を理解する
  • Space JacobianからBody Jacobianへ変換出来るようにする。
  • Body JacobianからSpace Jacobianに変換出来るようにする。
  • Space Jacobianを使ってFKを解く
  • Body Jacobianを使ってFKを解く
  • Space Jacobianを使ってIKを解く
  • Body Jacobianを使ってIKを解く

これを目標にしたい。

onnxのnamedtupledict

Qumicoでいろいろやろうとしていたらエラーがでたので調べたメモ

namedtupledictはonnxの中で定義されている関数。

github.com

namedtupleに辞書形式でのアクセスを追加しています。

簡単な使い方

import collections

def namedtupledict(*a, **kw):
   namedtuple = collections.namedtuple(*a, **kw)
   def getitem(self, key):
      if type(key) == str:
          return getattr(self, key)
      return tuple.__getitem__(self, key)
   namedtuple.__getitem__ = getitem
   return namedtuple


x = namedtupledict('x', ('name', 'age', 'address'))

x1 = x('natu', 17, 'Japan')

print(x1.name)
print(x1[1])
print(x1['address'])
      

実行結果

natu
17
Japan

見開きのPDFが1ページずれているときの対処方法

購入した書籍がpdfで、見開きのページがずれていて上手く読めなかったときの対処方法です。

職場の同僚に勧められたので、翔泳社の紙1枚に書くだけでうまくいく プロジェクト進行の技術が身につく本を買いました。

www.shoeisha.co.jp

電子版がpdfで入手できて早速開いてみたら、見開きのページが全ページずれていてちょっと読めないレベル。ファイルに保護がかかっていて、1ページ削除したり、2ページ目からpdfに印刷などもできず、ちょっと困ってしまいました。

f:id:natsutan:20210702204631p:plain

翔泳社さんに問い合わせたところ、Acrobatであれば[見開きページ表示で表紙を表示]にチェックを入れると良いとのこと。

f:id:natsutan:20210702204759p:plain

やってみたら上手くいきました。なんでこんな状態で配布するんだろ・・・。

ゼロからのOS自作入門(その5)RustでCの構造体を受け取る

ゼロからのOS自作入門(その4)osbook_day03c の続き。

natsutan.hatenablog.com

4日目はUEFIアプリからグラフィックスの情報をカーネルに渡します。今、UEFIアプリがC、カーネルをRustで書いているので、CとRustの間で構造体のやりとりが必要です。

std::os::raw を使う

gihyo.jp

実践Rust入門を見ると、#[repr(C)]とstd::os::rawのやり方が紹介されています。

本を見ながら、こんな感じで書いてみました。

use std::os::raw::c_int;
use std::os::raw::c_uint;
use std::os::raw::c_uchar;

#[repr(C)]
struct FrameBufferConfig {
    frame_buffer : *mut c_uchar,
    pixels_per_scan_line : c_int,
    horizontal_reslution: c_uint,
    vertical_resolution: c_uint,
    pixel_format : PixelFormat
}

実際にやってみるとエラーがでます。

error[E0433]: failed to resolve: use of undeclared crate or module `std`
 --> src/main.rs:7:5
  |
7 | use std::os::raw::c_int;
  |     ^^^ use of undeclared crate or module `std`

#![no_std] を設定しているので、stdから始まるクレートが使えないようです。

困っていたらTwitterで的確なアドバイスをいただきました。いつもありがとうございます。

ctyを使う

使い方はとても簡単で、cty::から使いたい型をuseするだけ。

use cty::{uint32_t, c_uchar};

#[derive(Debug, Copy, Clone)]
#[repr(C)]
pub struct FrameBufferConfig {
    frame_buffer : *mut c_uchar,
    pixels_per_scan_line : uint32_t,
    horizontal_reslution: uint32_t,
    vertical_resolution: uint32_t,
    pixel_format : PixelFormat
}


#[no_mangle]
pub extern "C" fn KernelMain(frame_buffer_config: *mut FrameBufferConfig) -> ! {

    let fb_buffer_config = unsafe {*frame_buffer_config};

こんな記述でUEFI側を変更すること無く構造体の受け渡しが出来ました。

C言語側の構造体定義はこうなっています。

struct FrameBufferConfig {
    uint8_t *frame_buffer;
    uint32_t pixels_per_scan_line;
    uint32_t horizontal_reslution;
    uint32_t vertical_resolution;
    enum PixelFormat pixel_format;
};

C言語側もint等を使わずにビット幅を指定しているので、上手く渡せているようです。

これでUEFIからもらったグラフィックスの情報を使って、座標を指定して色を塗ることが出来るようになりました。

f:id:natsutan:20210509103009j:plain

全ソース

カーネル側の全ソースです。四日目分はまだ作業中。

#![no_std]
#![no_main]

#![feature(asm)]
#![feature(abi_efiapi)]

use cty::{uint32_t, c_uchar};

extern crate rlibc;
extern crate panic_halt;


#[derive(Debug, Copy, Clone)]
#[repr(C)]
enum PixelFormat {
    KPixelRGBResv8bitPerColor,
    KPixelBGRResv8BitPerColor
}

#[derive(Debug, Copy, Clone)]
#[repr(C)]
pub struct FrameBufferConfig {
    frame_buffer : *mut c_uchar,
    pixels_per_scan_line : uint32_t,
    horizontal_reslution: uint32_t,
    vertical_resolution: uint32_t,
    pixel_format : PixelFormat
}


#[derive(Debug, Copy, Clone)]
struct PixelColor {
    r : u8,
    g : u8,
    b : u8
}


fn pixel_offset(x:u32, y:u32, config: &FrameBufferConfig) -> u32 {
    4 * (config.pixels_per_scan_line * y + x)
}

fn write(x:u32, y:u32, pixel:PixelColor, config: &FrameBufferConfig) {
    let offset = pixel_offset(x, y, config);
    unsafe {
        *(config.frame_buffer).offset(offset as isize) = pixel.r;
        *(config.frame_buffer).offset(offset as isize + 1) = pixel.g;
        *(config.frame_buffer).offset(offset as isize + 2) = pixel.b;
    }
}


#[no_mangle]
pub extern "C" fn KernelMain(frame_buffer_config: *mut FrameBufferConfig) -> ! {

    let fb_buffer_config = unsafe {*frame_buffer_config};

    let white = PixelColor{r:255, b:255, g:255};
    for y in 0..fb_buffer_config.vertical_resolution {
        for x in 0..fb_buffer_config.horizontal_reslution {
            write(x, y, white, &fb_buffer_config);
        } 
    }
    
    let green = PixelColor{r:0, g:255, b:0};
    for y in 0..200 {
        for x in 0..200 {
            write(x, y, green, &fb_buffer_config);
        } 
    }


    loop {
        unsafe {
            asm!("hlt")
        }
    }
}

基礎から学ぶ組込Rust

基礎から学ぶ組込Rustの本を読みました。久しぶりに組込プログラマに戻れた気がしてとても楽しめました。

www.c-r.com

どんな本

Wio TerminalをRustで制御していく本です。

  • 組込がある程度分かっていて、組込Rustに興味がある人
  • Rustはある程度分かっていて、組込に興味ある人

この辺の人達がターゲットで、Rustに関しては別にもう一冊本があった方が良いです。 どちらも初めてだとだいぶきついと思います。

読む前は、組込向けの最適化の話、Cのライブラリとの連携とか込み入った話の本かなと思っていたのですが、いざ読んでみるとどちらかというと入門書と呼べる内容でした。

Rustの説明があっさりあったあと、6章のペリフェラル制御からが本番です。Rustの文法そのものよりはこういう形でRustの機能を上手く使ってますという事例が丁寧に説明されていました。

各ペリフェラル(周辺回路)の簡単な紹介、回路図を使った接続情報の確認、動かすために必要な設定、それをRustでどう書くのかが続いた後、練習問題的なやってみようがあります。順番にやるだけで、Wio Terminalのいろんな機能を実際に動かして試すことが出来ます。基本的に全てお膳立てされていて、本の通りにloopの中を記入すると動きます。順番に動かしていくだけでもとても楽しかったです。

Rustの記述について

基本的には実績のあるクレートを使っているため、あんまりハードウェアに密接なコードは出てこないですし、はまりがちな所有権の問題などもクリアーされているコードから始めています。

take()を使ったシングルトン(ペリフェラルの排他制御)の実装や、panic handlerでエラー情報を出す所は面白かったです。未だに仕組みがわからないのが、GPIOの所で出てきた型状態プログラミング(Typestate Programming)です。 これを使うと、GPIOの設定を出力にしていない状態で信号を出力すると「コンパイル時」にエラーになります。

割り込みの所は、メインルーチンと割り込みハンドラでシェアするオブジェクトの扱いが動くソースコードになっていて、実務で使うときは参考にしたいです。

unsafeにする所も、こういう単位でunsafeを使うんだと勉強になりました。

組込プログラミングに関する記述について

実は、こっちがメインじゃないかというくらい良かったです。

回路図の説明、データシートの記述から、デバイスを制御するために必要な情報が整理されています。 実際、初めてやる人がこの記述だけで他のデバイスを制御できるかというとだいぶ厳しいですが、それでもこういう所を確認しないといけないんだなというのはなんとなく伝わると思います。非組込プログラマ向けに、こういう所をやさしく書いている本は少ないのでとても貴重だと思います。

新人の頃、タイマーで時間を計ろうとしたらカウンターの説明にしか読めなくて途方にくれたことを思い出しました。(いや実際にカウンターなんですが・・・)

加速度センサーや光センサーなど、今まで業務で使ったことがないセンサーを動かせて楽しかったです。

出来た物

画像表示

グラフィックスのライブラリを使って、こんな表示が出来ます。 先にシミュレータで表示できることを確認し、クレイト化してから、実機に表示しています。

f:id:natsutan:20210426004301j:plain

スペアナ

謎のスペアナが出来ました。マイクから音を拾ってリアルタイムにFFTしています。

f:id:natsutan:20210426004317j:plain

まとめ

とても面白い本だったので、興味ある人はぜひ本を買って動かしてみてください。 実際にデバイスやセンサーが動く楽しさは読んでいるだけでは伝わらないです。

↓念のため正誤表も確認してください