FC2ブログ

爪車

部品が届かないので、基板の実装が出来ない…

3D描写ライブラリを公開しました。

ロボット開発に用いていた3D描写ライブラリをGitHubで公開しました。

いまのところ、Ubuntu 18.04でのみビルド&動作確認をしています。

ScreenshotSimpleDemo.png 

emscriptenでもビルドできるので、Webブラウザで動かせます。
20190406.png

暇なときに、サンプルやドキュメントを追加していきたいと思います。

今月は進捗なし。

磁気シールド

リング型の磁石と磁気式エンコーダを組み合わせて、減速機の減速後の回転角度を直接計測しているのですが、どうもモータの磁石の影響をうけてエンコーダの値が変動してたので、磁気シールドでモータの磁力を遮蔽してみました。
20181102_2.jpg
20181102_1.jpg
出力軸にリング型の磁石を取り付けていて、ケース側のエンコーダで計測するようにしています。

試してみたのは、磁気シールドシート(日立金属のMS-FR 470-100M-T0、エコロガのMS5000M)です。(MS-FR 470-100M-T0を切り売りしているのが、エコロガのMS5000M)
20190210_1.jpg

モータを手で回した時のエンコーダ(14bit)の出力値です。
エンコーダ用磁石は固定しているので、出力値は変動しないのが望ましい状態での計測。
20190210_2.png
ちゃんとした計測ではないのであくまで参考程度ですが、シートを複数枚重ねるとはっきりと磁気遮蔽の効果がみられます。
また、磁気シールドシート自体も磁石にくっつく素材なので、エンコーダにシールドを近づけると、エンコーダの値が変動します。

最終的には、磁気シールドシートを4枚重ねてモータと減速機の間に挟み込むようにしました。 
20199212_1.jpg
20190212_2.jpg

出力軸のエンコーダ出力値。(モータほぼ一定の速度で回転。)
エンコーダがoff-axis配置なので、非線形な値になっています。
20190212_3.png
シート入れる前


20190212_4.png
シート4枚入れた後

エンコーダの値のがたつきが多少改善されました。

本来だと、この出力軸のエンコーダの値をそのまま減速後の回転角度として使用できればよいのですが、off-axis配置で非線形なのと、多少の値のがたつきがあるため、エンコーダの値から一意に出力の回転角度を特定できませんでした。
そのため、出力軸のエンコーダの値とモータ側のエンコーダの値を用いて角度を求めるようにしました。

いろいろ方法はあると思いますが、今のところ以下の方法で回転角度を求めています。
出力軸14bitの上位10bitとモータ側14bitの上位4bitを組み合わせた14bit長のindexを持つテーブルに事前に求めた角度計算用の値を入れておいて、実行時はその値をテーブル引きして角度を求める。
index = (0xFF0 & reducer_14bit) | (motor_14bit >> 10)
angle_14bit = (0x3FFF * rotation_table[index] + motor_14bit) / reduction_ratio




ブートローダ

RS485経由でファームウェアのアップデートができるようにブートローダを作りました。
STM32にはもともと組み込みのブートローダがあるのですが、RS485ドライバ送受信切り替えのためのDriverEnable信号が操作できないので自前で実装しました。

ROMをブートローダとメインのプログラムとブート情報を保存する領域に分けて、ブートローダはブート情報領域を読んで、メインプログラムの情報が書き込まれていれば、メインプログラムを起動。
ブートローダでのファームアップデートは、ビルドしたメインプログラムのバイナリの先頭にブート情報をくっつけて、STM32CubeProgrammer経由でFLASH書き換えするようにしました。
(ブートローダのファーム書き換えプロトコルはSTMのアプリケーションノートAN3155をもとに実装)

ブート情報は、ブート情報の識別子としてのマジックナンバー(8byte)と、メインプログラムのサイズ(4byte)、チェックサム(CRC32)(4byte)、ブート情報のチェックサム(CRC32)(4byte)で構成。

ROMの配置はいまのところこんな感じです。(RAMは共用)(STM32L432KB)
FLASH_BOOT   (rx)  : ORIGIN = 0x8000000,  LENGTH =  18K
FLASH_INFO   (rx)  : ORIGIN = 0x8004800,  LENGTH =   2K
FLASH_MAIN   (rx)  : ORIGIN = 0x8005000,  LENGTH = 100K
FLASH_DATA   (rx)  : ORIGIN = 0x801E000,  LENGTH =   8K

メインのプログラム起動中にファームアップデートする場合は、ブート情報の領域を消去してからリセットし、ブートローダを起動するようにしてそこからアップデートするようにしています。

来年はサーボをたくさん作らないと。

URDFを編集するエディタを作ってみました。
WEBブラウザで動きます。
https://tsumehashi.github.io/urdfblocks/



GoogleのBlocklyとThree.jsを使っています。

GitHubでコード公開しています。(MIT License)
https://github.com/tsumehashi/urdfblocks

Twitterはじめました。
https://twitter.com/tsumehashi

今月は特になし

3Dプリンター買いました。

オリジナルマインドのKitMill AST200を購入しました。

国際ロボット展に行きました。

ヒト型ロボット 192

あまり進捗がないので、載せていなかったロボットの写真を公開します。
一年以上前のものです。既に分解されています。



20170603-2.jpg

AliExpressで中国から卓上リフロー炉T962を買いました。

基板設計していたら、4月分の更新を忘れてしまいました。

HoloLens買いました。

切削後の油べたべたの洗浄のために、超音波洗浄器を買ってみました。
シチズン 超音波洗浄器 SWT710
結構良い感じです。

ROBO-ONE初観戦。
とても面白かったです。

そのうち出場してみたいです。

今月は進捗無。

大量の荷物を送るときは、ヤマト便が便利。

SVG描画

SVGをポリゴンとして描画
nanosvgでパースしてpoly2triで三角ポリゴンに分割
パス単位でマウスピッキング可

Screenshot- 20160630
微妙に細部がおかしい

座標軸の色

座標軸と色の組み合せが、(X:赤, Y:青, Z:緑)という異端な組み合わせだったので修正。


修正前 (X:赤, Y:青, Z:緑)

Screenshot-20160531-2.png
修正後 (X:赤, Y:緑, Z:青)

(X:赤, Y:青, Z:緑)だったのは、(前:赤, 横:青, 上:緑)の組み合わせが個人的にしっくりしていたからという理由です。

来年から本気出す。

3D描画ライブラリ公開

ロボット開発に使用している3D描画ライブラリをGitHubで公開しました。
OpenGLのバックエンドとして、QtOpenGLとGLFWに対応しています。

TinyGraphicsLibrary
https://github.com/tsumeguruma/TinyGraphicsLibrary

Screenshot-20140216.png

まだまだ不完全です。

ミニ卓上CNCフライス盤購入

旋盤市場のPSF240-CNC ミニ卓上CNCフライス盤を購入しました。



結構大きい&重い。
持ち上げられなかったので引きずって移動しました。

一緒にコレットチャックセットとクイックバイスを購入。
オイルトレイは購入だったのに注文し忘れました・・・。

これで、立体的なものを切削する予定です。

来年から本気出す。

順運動学と逆運動学のプログラム

順運動学と逆運動学(6自由度)の簡易的なプログラム。
6自由度のリンクのみに対応しています。
本格的に用いるなら、OpenHRPChoreonoidの導入をお勧めします。

以下のコードはChoreonoidのソースコードを参考にしています。
行列計算にはEigenライブラリを用いています。

#include <iostream>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
class Link;
double rad2deg(double rad); // radianからdegreeへ変換
double deg2rad(double deg); // degreeからradianへ変換
Eigen::Matrix3d rodrigues(const Eigen::Vector3d& axis, double q); // ロドリゲスの式
Eigen::Vector3d omegaFromRot(const Eigen::Matrix3d& R); // 行列対数関数
void calcForwardKinematics(Link* link); // 順運動学
Eigen::MatrixXd calcJacobian(Link* links, int numLinks, Link* targetLink); // ヤコビアンの計算
void calcInverseKinematics(Link* links, int numLinks, Link* targetLink, const Eigen::Vector3d& target_p, const Eigen::Matrix3d& target_R); // 逆運動学
Eigen::Matrix3d rotFromRpy(double r, double p, double y); // roll, pitch, yawから回転行列を求める
class Link
{
public:
Link() {
parent = NULL;
child = NULL;
sibling = NULL;
p.setZero();
R.setIdentity();
q = 0.0;
b.setZero();
a.setZero();
}
virtual ~Link() {}
std::string name; // リンクの名前
Link* parent; // 親リンク
Link* child; // 子リンク
Link* sibling; // 兄弟リンク
Eigen::Vector3d p; // 位置
Eigen::Matrix3d R; // 姿勢
double q; // 回転角度
Eigen::Vector3d b; // 親リンクからの相対位置
Eigen::Vector3d a; // 回転軸
};
// radianからdegree
double rad2deg(double rad)
{
return (180/M_PI) * rad;
}
// degreeからradian
double deg2rad(double deg)
{
return (M_PI/180) * deg;
}
// ロドリゲスの式
Eigen::Matrix3d rodrigues(const Eigen::Vector3d& axis, double q) {
return Eigen::AngleAxisd(q, axis).toRotationMatrix();
}
// 行列対数関数
// Choreonoid 1.4.0 から借用
Eigen::Vector3d omegaFromRot(const Eigen::Matrix3d& R)
{
    double alpha = (R(0,0) + R(1,1) + R(2,2) - 1.0) / 2.0;
    if(fabs(alpha - 1.0) < 1.0e-6) {   //th=0,2PI;
        return Eigen::Vector3d::Zero();
    } else {
        double th = acos(alpha);
        double s = sin(th);
        if (s < std::numeric_limits<double>::epsilon()) {   //th=PI
            return Eigen::Vector3d( sqrt((R(0,0)+1)*0.5)*th, sqrt((R(1,1)+1)*0.5)*th, sqrt((R(2,2)+1)*0.5)*th );
        }
        double k = -0.5 * th / s;
        return Eigen::Vector3d((R(1,2) - R(2,1)) * k,
                       (R(2,0) - R(0,2)) * k,
                       (R(0,1) - R(1,0)) * k);
    }
}
// 順運動学
void calcForwardKinematics(Link* link)
{
if (!link) return;
Link* parent = link->parent;
if (parent) {
link->p = parent->p + parent->R * link->b;
link->R = parent->R * rodrigues(link->a, link->q);
}
calcForwardKinematics(link->sibling);
calcForwardKinematics(link->child);
}
// ヤコビアンの計算
Eigen::MatrixXd calcJacobian(Link* links, int numLinks, Link* targetLink)
{
Eigen::MatrixXd J(6, numLinks); // (位置+姿勢の6自由度 x Joint数)
for (int i = 0; i < numLinks; ++i) {
Link* link = &links[i];
Eigen::Vector3d omega = link->R * link->a;
Eigen::Vector3d arm = targetLink->p - link->p;
Eigen::Vector3d dp = omega.cross(arm);
J(0, i) = dp(0);
J(1, i) = dp(1);
J(2, i) = dp(2);
J(3, i) = omega(0);
J(4, i) = omega(1);
J(5, i) = omega(2);
}
return J;
}
// 逆運動学 (6自由度)
void calcInverseKinematics(Link* links, int numLinks, Link* targetLink, const Eigen::Vector3d& target_p, const Eigen::Matrix3d& target_R)
{
    const int MAX_IK_ITERATION = 50;
    const double lambda = 0.5;
Link* rootLink = &links[0];
    Eigen::VectorXd dq(numLinks);
    Eigen::VectorXd v(6);
    for (int i = 0; i < MAX_IK_ITERATION ; ++i) {
    Eigen::MatrixXd J = calcJacobian(links, numLinks, targetLink);
    Eigen::Vector3d dp = target_p - targetLink->p;
    Eigen::Vector3d omega = targetLink->R * omegaFromRot(targetLink->R.transpose() * target_R);
    // 誤差の計算
    double err = sqrt(dp.dot(dp) + omega.dot(omega));
    if (err < 1e-6) {
//     std::cout << "err = " << err << std::endl;
//     std::cout << "iteration = " << i << std::endl;
    break;
    }
    v << dp, omega;
    dq = J.inverse() * v;
    for (int j = 0; j < numLinks; ++j) {
    links[j].q += lambda * dq(j);
    }
    calcForwardKinematics(rootLink);
    }
}
// roll, pitch, yawから回転行列を求める
// Choreonoid 1.4.0 から借用
Eigen::Matrix3d rotFromRpy(double r, double p, double y)
{
    const double cr = cos(r);
    const double sr = sin(r);
    const double cp = cos(p);
    const double sp = sin(p);
    const double cy = cos(y);
    const double sy = sin(y);
    Eigen::Matrix3d R;
    R << cp*cy, sr*sp*cy - cr*sy, cr*sp*cy + sr*sy,
         cp*sy, sr*sp*sy + cr*cy, cr*sp*sy - sr*cy,
         -sp  , sr*cp           , cr*cp;
    return R;
}
int main(int argc, char* argv[])
{
enum {
HIP_Y = 0,
HIP_R,
HIP_P,
KNEE_P,
ANKLE_P,
ANKLE_R,
NUM_LINKS
};
// リンクの設定
Link links[NUM_LINKS];
links[HIP_Y].name = "HIP_Y";
links[HIP_Y].parent = NULL;
links[HIP_Y].child = &links[HIP_R];
links[HIP_Y].b << 0.0, 0.0, 0.0;
links[HIP_Y].a << 0.0, 0.0, 0.1; // yaw
links[HIP_R].name = "HIP_R";
links[HIP_R].parent = &links[HIP_Y];
links[HIP_R].child = &links[HIP_P];
links[HIP_R].b << 0.0, 0.0, 0.0;
links[HIP_R].a << 1.0, 0.0, 0.0; // row
links[HIP_P].name = "HIP_P";
links[HIP_P].parent = &links[HIP_R];
links[HIP_P].child = &links[KNEE_P];
links[HIP_P].b << 0.0, 0.0, -0.02;
links[HIP_P].a << 0.0, 1.0, 0.0; // pitch
links[KNEE_P].name = "KNEE_P";
links[KNEE_P].parent = &links[HIP_P];
links[KNEE_P].child = &links[ANKLE_P];
links[KNEE_P].b << 0.0, 0.0, -0.1;
links[KNEE_P].a << 0.0, 1.0, 0.0; // pitch
links[ANKLE_P].name = "ANKLE_P";
links[ANKLE_P].parent = &links[KNEE_P];
links[ANKLE_P].child = &links[ANKLE_R];
links[ANKLE_P].b << 0.0, 0.0, -0.1;
links[ANKLE_P].a << 0.0, 1.0, 0.0; // pitch
links[ANKLE_R].name = "ANKLE_R";
links[ANKLE_R].parent = &links[ANKLE_P];
links[ANKLE_R].child = NULL;
links[ANKLE_R].b << 0.0, 0.0, 0.0;
links[ANKLE_R].a << 1.0, 0.0, 0.0; // row
// 順運動学
links[HIP_Y].q   = 0.0;
links[HIP_R].q   = 0.0;
links[HIP_P].q   = deg2rad(-45);
links[KNEE_P].q  = deg2rad(90);
links[ANKLE_P].q = deg2rad(-45);
links[ANKLE_R].q = 0.0;
Link* rootLink = &links[HIP_Y];
std::cout << "ForwardKinematics" << std::endl;
calcForwardKinematics(rootLink);
std::cout << "ANKLE_R p = " << links[ANKLE_R].p.transpose() << std::endl;
std::cout << "ANKLE_R R = \n" << links[ANKLE_R].R << std::endl;
// 逆運動学
Eigen::Vector3d target_p;
Eigen::Matrix3d target_R;
target_p << 0.0, 0.01, -0.12;
// target_R = rotFromRpy(0.0, deg2rad(10), 0.0);
target_R.setIdentity();
std::cout << "InverseKinematics" << std::endl;
calcInverseKinematics(links, NUM_LINKS, &links[ANKLE_R], target_p, target_R);
std::cout << "ANKLE_R p = " << links[ANKLE_R].p.transpose() << std::endl;
std::cout << "ANKLE_R R = \n" << links[ANKLE_R].R << std::endl;
return 0;
}

PC購入

新しいPCを買いました。

CPU : Intel Core i7 4770
GPU : NVIDIA GTX 560

CUDAで遊んでみたい。

あけましておめでとうございます。

ccacheでビルド時間の短縮

最近ccacheというものを知りました。
コンパイル結果をキャッシュすることで再コンパイル時間を短縮してくれます。

使用方法は簡単に

ccache g++ main.cpp

てな感じです。

qmakeで利用する場合は
QMAKE_CXX = ccache g++

cmakeの場合は
set(CXX="ccache g++")

参考ページ
ccacheで再コンパイル処理を高速化

Top|Next »

HOME

tsumehashi

Author:tsumehashi
FC2ブログへようこそ!

07 | 2019/08 | 09
- - - - 1 2 3
4 5 6 7 8 9 10
11 12 13 14 15 16 17
18 19 20 21 22 23 24
25 26 27 28 29 30 31