011 - vnoidベースの開発例その１
今回の内容
2023年11月11日に本大会初の試みとなるプレチャレンジが開催されました。
私たちteamMA1もそのイベントに参加しました。
今回はそこで発表した内容についてより詳しく説明しようと思います。
vnoidベースの開発例として、あくまでご参考までに見ていただければと思います。
teamMA1の戦略
私たちは、本年度より改装されたアスレチックコースのうち、
まずは低難度コースの踏破を目指します。
低難度コースは不整地、S字路、階段の３つのエリアに分かれており、
私たちはジョイスティックで人型ロボットを遠隔操作することでこれらの踏破を目指します。
不整地やS字路に関しては配布されているvnoidを用いれば、
苦戦しつつもなんとか踏破できます。
しかし階段を歩かせるには独自に改良が必要です。
階段は不整地のようなちょっとした凹凸ではなく、明確な床面の高低差があります。
よって、人型ロボットはその高低差を意識的に昇り降りしなければいけません。
そこで本チームでは、まず床面の高低差を検出する機能をvnoidに追加します。
また、床面の高低差を人型ロボットに伝えるだけでは階段の踏破は難しいと思われます。
階段には一段ごとに着地可能な領域が明確に定められており、
そこを踏み外すと歩行を継続できません。
そこで、高低差を含む床面の着地可能領域を検出する機能を追加します。
その機能を実現するために、人型ロボットに深度カメラを搭載します。
私たちの基本戦略は、床面上の着地可能領域内に着地足が収まるように、
ジョイスティックからの入力を制限するというものです。
この方法で低難度コースの完走を目指します。
なお、開発はUbuntu環境で行います。
深度カメラを搭載する
まずは、床面の情報をロボットに伝えるために深度カメラを搭載します。
深度カメラとは、通常のカメラで撮影される二次元画像に加えて、
その画像に対応する深度マップ(画像内の物体表面までの視点からの距離を格納したもの)
を取得するカメラです。
なお、距離センサでも代用可能だと思います。
Choreonoidでは深度カメラのデバイス型がRangeCameraとして定義されております。
このデバイス型から生成される深度カメラのオブジェクトには、
深度マップ付きの二次元画像を取得する機能が備わっています。
そのため、ロボットモデルに深度カメラを取りつけさえすれば、
すぐにシミュレーションで深度カメラを試すことができます。
ロボットへの深度カメラの搭載例を紹介します。
vnoidのロボットモデルは、vnoid/model/sample_robot/sample_robot_ver2.body
に記述されています。
私たちは、ChoreonoidのBodyファイルチュートリアルを参考に、
以下のようにロボットの頭リンク(HEAD_P)に深度カメラ(CameraBody)を搭載しました。
-
name: HEAD_P
parent: HEAD_Y
...
-
name: CameraBody
parent: HEAD_P
jointType: fixed
mass: 0
centerOfMass: [0, 0, 0]
inertia: [1, 0, 0,
0, 1, 0,
0, 0, 1]
elements:
Transform:
translation: [0.10, 0, 0.0]
rotation: [[0.0, 0.0, 1.0, -90.0], [1.0, 0.0, 0.0, -30.0]]
elements:
-
type: Shape
geometry: {type: Box, size: [0.25, 0.1, 0.1]}
apperarance: &Rail_apperance
material:
diffuseColor: [0.1, 0.1, 0.8]
specularColor: [0.5, 0.5, 0.5]
shininess: 0.6
-
translation: [0.0, 0.05, 0.0]
type: Shape
geometry:
type: Cylinder
radius: 0.03
height: 0.02
appearance:
material:
diffuseColor: [0.8, 0.8, 0.8]
specularColor: [0.5, 0.5, 0.5]
shininess: 0.6
-
type: Camera
name: RangeCamera
id: 0
translation: [0.0, 0.0, 0.0]
rotation: [1.0, 0.0, 0.0, 60.0]
format: COLOR_DEPTH
lens_type: "NORMAL"
on: true
width: 640
height: 480
field_of_view: 60
near_clip_distance: 0.4
far_clip_distance: 4.5
frame_rate: 30
このようにすると、次の画像のように深度カメラが取り付けられます。
PCL(Point Clound Library)の導入
Choreonoidでは、撮影した床面の深度マップ付きの二次元画像から、
視点座標系における床面の三次元点群を抽出することができます。
この三次元点群を処理するためのライブラリとして、
PCL(Point Clound Library)を導入します。
Ubuntu環境であれば、
以下のコマンドをターミナル上で入力することでインストールできます。
sudo apt install libpcl-dev
sudo apt install pcl-tools
コントローラの枠組み
私たちは、下図のように人型ロボットのコントローラを拡張しました。
VnoidSampleControllerはChoreonoidのサンプル用に設計された
SimpleControllerを継承したクラスです。
VnoidSampleControllerからMyCameraにスキャン指令(GoundScan)を送ると、
MyCameraから床面の着地可能領域が返ってくるという設計にしました。
また、VnoidSampleControllerからMyRobotに制御指令(control)を送ると、
床面上の着地可能領域内に着地足が収まるように着地位置が計画され、
それを追従できるような歩行安定化制御をするように設計します。
vnoid/controller/sample_controller/main.cppの中身は以下のように変更しました。
1#include <cnoid/SimpleController>
2#include <cnoid/Body>
3#include <cnoid/Camera>
4#include <cnoid/RangeCamera>
5#include <cnoid/Joystick>
6
7#include <vector>
8#include <math.h>
9#include <stdio.h>
10#include <iostream>
11#include <fstream>
12
13#include "myrobot.h"
14#include "mycamera.h"
15
16using namespace cnoid;
17using namespace cnoid::vnoid;
18
19class VnoidSampleController : public SimpleController{
20public:
21 MyRobot* robot;
22 MyCamera* camera;
23 Joystick joystick;
24 bool PreButtonState;
25 int count;
26
27public:
28 virtual bool configure(SimpleControllerConfig* config){
29 return true;
30 }
31
32 virtual bool initialize(SimpleControllerIO* io){
33 camera = new MyCamera();
34 camera->Init(io);
35 count = 0;
36
37 robot = new MyRobot();
38 robot->Init(io);
39
40 return true;
41 }
42
43 virtual bool control() {
44 joystick.readCurrentState();
45 bool ButtonState = joystick.getButtonState(Joystick::A_BUTTON);
46 if (ButtonState && !PreButtonState) {
47 printf("push A_BUTTON\n");
48 camera->GroundScan();
49 }
50 PreButtonState = ButtonState;
51
52 robot->Control();
53 count++;
54 return true;
55 }
56};
57
58CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(VnoidSampleController)
現状、ジョイスティックのAボタンを押すと、
MyCameraのGroundScan()が呼び出される仕様となっています。
深度カメラ視野内の平面検出
PCLを使って、取得した三次元点群のうち、平面を構成している点群を抽出します。
そのためにvnoid/src/mycamera.h, mycamera.cppを次のように作成しました。
1#pragma once
2
3#include <cnoid/SimpleController>
4#include <cnoid/EigenTypes>
5#include <cnoid/RangeCamera>
6
7#include <iostream>
8#include <pcl/ModelCoefficients.h>
9#include <pcl/io/pcd_io.h>
10#include <pcl/point_types.h>
11#include <pcl/sample_consensus/method_types.h>
12#include <pcl/sample_consensus/model_types.h>
13#include <pcl/segmentation/sac_segmentation.h>
14#include <pcl/visualization/cloud_viewer.h>
15
16using namespace std;
17
18namespace cnoid {
19namespace vnoid {
20
21class MyCamera : RangeCamera
22{
23public:
24 DeviceList<RangeCamera> cameras;
25 double timeCounter;
26 double timeStep;
27
28public:
29 virtual void Init(SimpleControllerIO* io);
30 virtual void GroundScan();
31
32 MyCamera();
33
34};
35
36} // namespace vnoid
37} // namespace cnoid
1#include "mycamera.h"
2
3void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
4{
5 viewer.setBackgroundColor(0.2, 0.2, 0.2);
6 printf("viewerOneOff\n");
7}
8
9void viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
10{
11 printf("viewerPsyco\n");
12}
13
14namespace cnoid {
15namespace vnoid {
16
17MyCamera::MyCamera() {
18 timeStep = 1.0;
19}
20
21void MyCamera::Init(SimpleControllerIO* io) {
22 // enable camera
23 cameras << io->body()->devices();
24 for (size_t i = 0; i < cameras.size(); ++i) {
25 Device* camera = cameras[i];
26 io->enableInput(camera);
27
28 printf("Device type: %s, ", camera->typeName());
29 printf("id: %d, ", camera->id());
30 printf("name: %s.\n", camera->name());
31 }
32
33 timeCounter = 0.0;
34 timeStep = io->timeStep();
35}
36
37void MyCamera::GroundScan() {
38 // get cameras
39 // when there are several cameras
40 for (size_t i = 0; i < cameras.size(); i++) {
41 RangeCamera* camera = cameras[i];
42 // describe here
43 }
44 // only one camera
45 RangeCamera* camera = cameras[0];
46
47 // Get an image of the current scene
48 const Image& RangeImage = camera->constImage();
49 // Save an image of current scene
50 RangeImage.save("pointcloud.png");
51 printf("save image.\n");
52
53 // width and height of this image
54 const int width = RangeImage.width();
55 const int height = RangeImage.height();
56 // get color data of this image
57 const unsigned char* pixels = RangeImage.pixels();
58
59 // point cloud variable declaration
60 // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
61 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
62 // initialize point cloud
63 cloud->width = width;
64 cloud->height = height;
65 cloud->is_dense = false;
66 cloud->points.resize(cloud->width * cloud->height);
67
68 // Stores values (coordinates, color) for each point in a point cloud
69 std::size_t i = 0;
70 for (const auto& e : camera->constPoints()) {
71 //pcl::PointXYZ& point = cloud->points[i];
72 pcl::PointXYZRGB& point = cloud->points[i];
73
74 // X, Y, Z
75 point.x = e(0);
76 point.y = e(1);
77 point.z = e(2);
78
79 point.r = 255;
80 point.g = 255;
81 point.b = 255;
82
83 ++i;
84 }
85
86 // create the model coeeficients object
87 pcl::ModelCoefficients::Ptr coeffs (new pcl::ModelCoefficients);
88
89 // create the inliers object
90 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
91
92 // create the SACSegmentation object
93 pcl::SACSegmentation<pcl::PointXYZRGB> seg;
94
95 // optional setting
96 seg.setOptimizeCoefficients(true);
97
98 // madatory settings
99 seg.setModelType(pcl::SACMODEL_PLANE); // detect plane
100 seg.setMethodType(pcl::SAC_RANSAC); // use RANSAC algorithm
101 seg.setDistanceThreshold(0.01); // how close a point must be to the ,model in order to be consider an inlier
102
103 // input cloud to segmentation
104 seg.setInputCloud(cloud);
105
106 // plane segmentation
107 seg.segment(*inliers, *coeffs);
108
109 // coloring plane segments
110 for (size_t i = 0; i < inliers->indices.size(); i++){
111 cloud->points[inliers->indices[i]].r = 255;
112 cloud->points[inliers->indices[i]].g = 0;
113 cloud->points[inliers->indices[i]].b = 0;
114 }
115
116 // make a viewer of point cloud
117 pcl::visualization::CloudViewer viewer("PointCloudViewer");
118 viewer.showCloud(cloud);
119
120
121 // set the thread that called at once in visualization
122 viewer.runOnVisualizationThreadOnce(viewerOneOff);
123
124 // set the thread running while visualization
125 viewer.runOnVisualizationThread(viewerPsycho);
126
127}
128
129} // namespace vnoid
130} // namespace cnoid
mycamera.cppのプログラム概要を説明します。
まず、Init関数で、bodyファイルにおいて定義した台数分だけ
カメラデバイス(cameras)を認識させます。
今回一つの深度カメラのみを用いるので、
45行目でRangeCameraのオブジェクト(camera)を、
cameras[0]に設定します。
48行目でカメラのシャッターを切ります。
54~57行目で、画像の幅や高さ、ピクセル数などを取得します。
これには、bodyファイルにて設定したパラメータが反映されます。
61行目で、pcl::PointCloud< pcl::PointXYZRGB >::Ptr型として、
cloudオブジェクトを生成します。
これは、色つき三次元点群を格納するためのオブジェクトです。
63~66行目で、cloudにも撮影画像のサイズ情報を与えます。
68行目~84行目で、cloudに色つき三次元点群を格納します。
camera->constPoints()は、深度マップを三次元点群に変換して返す関数で、
その点を格子点ごとにcloud->points[i]に格納します。
後の平面検出のために、RGB値は白色となるように設定します。
87行目以降は、RANSACアルゴリズムを用いて、
三次元点群から平面を検出するためのプログラムです。
87行目で、平面モデル(方程式)の係数を定義します。
90行目で、インライアを定義します。
インライアとは、平面モデルから外れていないと推定されるデータのことです。
93行目で、三次元点群を分割するためのオブジェクトを定義します。
96行目で、外れ値の存在を前提として最適化するように設定します。
99行目で、RANSACのモードを平面検出に設定します。
101行目で、インライアとして判定する際の閾値を設定します。
104行目で、入力三次元点群をセットします。
107行目で平面検出をします。
110~114行目で、インライア判定された三次元点群のみを赤色に塗ります。
こうすることで、平面として検出された三次元点群のみ赤色で表示されます。
117~125行目は、PointCloudViewerを呼び出して、
三次元点群を表示するためのプログラムです。
実際にプログラムを実行すると、
ビューワーが起動して次のような画像が表示されます。
期待通り、平面として検出された三次元点群のみ赤色で表示されています。
平面上の三次元点群を支持足基準座標系に変換
PCLにより視点座標系における検出平面上の三次元点群$\boldsymbol{{}^Cp_G}$が得られました。
これらをすべて視点座標系から支持足座標系へ変換します。
これには順運動学計算を用いるので、
リンク先の解説と合わせて読んでいただければと思います。
まず、視点座標系における検出平面の三次元点群を、ベースリンク座標系に変換します。
$$ \boldsymbol{{}^Bp_G} = \boldsymbol{{}^Bp_H} + \boldsymbol{{}^BR_H} (\boldsymbol{{}^Hp_C} + \boldsymbol{{}^HR_C} \boldsymbol{{}^Cp_G}) $$
ここで、$B$はベースリンク座標、$H$は頭リンク座標、
$C$はカメラ視点座標、$G$は床の平面上の各点座標を意味します。
頭リンクから見たカメラ視点の位置$\boldsymbol{{}^Hp_C}$や姿勢$\boldsymbol{{}^HR_C}$は搭載時に自ら設定するので既知です。
また、ベースリンクから頭リンクまでの関節は歩行中にほとんど回転しないので、
$\boldsymbol{{}^Bp_H} = \boldsymbol{I}$として、上式は次のように簡単に書けます。
$$ \boldsymbol{{}^Bp_G} = \boldsymbol{{}^Bp_H} + \boldsymbol{{}^Hp_C} + \boldsymbol{{}^HR_C} \boldsymbol{{}^Cp_G} $$
求めたい足座標系における検出平面上の三次元点群$\boldsymbol{{}^Fp_G}$を用いると、
ベースリンク座標系における検出平面上の三次元点群$\boldsymbol{{}^Bp_G}$は次のようにも書けます。
$$ \boldsymbol{{}^Bp_G} = \boldsymbol{{}^Bp_A} + \boldsymbol{{}^BR_A} (\boldsymbol{{}^Ap_F} + \boldsymbol{{}^AR_F} \boldsymbol{{}^Fp_G}) $$
ここで、$A$は足首リンク座標、$F$は足裏中心座標を意味します。
vnoidには、ロボットの関節角から、
ベースリンク座標系における足首の位置$\boldsymbol{{}^Bp_A}$・姿勢$\boldsymbol{{}^BR_A}$を計算する
順運動学計算器(CompLegFK)がvnoid/src/fksolver.cppに用意されています。
よって、上式において、$\boldsymbol{{}^Bp_A}$や$\boldsymbol{{}^BR_A}$は既知です。
また、$\boldsymbol{{}^Ap_F}$は足首から足裏中心までの相対位置ですが、
単にz軸方向に足の厚み分だけオフセットすることを表現するので既知です。
さらに、足首リンク座標と足裏中心座標の姿勢は一致するので、
$\boldsymbol{{}^AR_F} = \boldsymbol{I}$とできます。
よって、上式は次のように簡単に書き直せます。
$$ \boldsymbol{{}^Bp_G} = \boldsymbol{{}^Bp_A} + \boldsymbol{{}^BR_A} (\boldsymbol{{}^Ap_F} + \boldsymbol{{}^Fp_G}) $$
以上の簡単化した二式を用いて、
視点座標における検出平面上の三次元点群を足座標系に変換します。
$$ \boldsymbol{{}^Fp_G} = \boldsymbol{{}^BR_A}{}^T (\boldsymbol{{}^Bp_H} + \boldsymbol{{}^Hp_C} + \boldsymbol{{}^HR_C \boldsymbol{{}^Cp_G}} - \boldsymbol{{}^Bp_A}) - \boldsymbol{{}^Ap_F} $$
支持側の足で以上の計算をすることで、
支持足を基準とした検出平面上の三次元点群$\boldsymbol{{}^Fp_G}$が得られます。
着地可能領域の検出
支持足座標系における検出平面上の三次元点群が得られました。
着地可能領域を、これら点群の凹包として定義します。
現在は、PCLのConcaveHullクラスを用いて、
凹包の頂点の集合を抽出できないかを模索している段階です。
着地可能領域内で着地位置を計画
着地可能領域に含まれる支持足基準の高低差情報を用いて、
自動的に階段の昇降が可能となるように着地高さの計画を行います。
また、着地可能領域外を踏めば、壁にぶつかったり、
床を踏み外したりといったことが起こり得ます。
そこで、操縦者の入力により領域外に出てしまうなら、
その入力は受け付けないという危険防止装置を組み込みます。
こうすることで、着地可能領域内であれば
操縦者が自由に着地位置を計画できるようなシステムを構築できます。
領域の内外判定にはCrossing Number Algorithmを用いる予定です。
まとめ・次回予告
今回は、私たちのチームの開発内容を紹介しました。
私たちは、深度カメラを用いて着地できる床上の領域を検出し、
その中での歩行を遠隔操作で実現できるようなシステムを開発しております。
vnoidベースの開発の一例として見ていただけたなら幸いです。
次回： 012 - 歩行制御器
