ホーム

二次曲面把持の使用方法

火, 04/07/2015 - 18:01 — tsuji

Read me

http://www.hlab.sys.es.osaka-u.ac.jp/grasp/sites/default/files/curvedsurface.doc
http://www.hlab.sys.es.osaka-u.ac.jp/grasp/sites/default/files/curvedsurface.pdf
に画像つきの文書がある．以下はテキストのみ書きだしたもの．

1. Choreonoid及びgraspPlugin
環境構築を，Ubuntu Linuxにて行う．Ubuntu OSが自分のPCに入っていない場合は，先輩にインストールDiskを借りて，自分のPCにインストールしておこう！以降の手順は，WindowsではなくUbuntu上で処理すること．
また，コマンドを実行するために”Terminal”というアプリケーションを用いるが，Ctrl+Alt+Tで立ち上げることが出来る．

iii. インストール
① Zipファイルの展開
ⅰ.の参照サイトURLより，ソースコードからのインストール内の
ソースコードからのビルドとインストール（Ubuntu Linux編）
をクリック．基本的にはここに書いてある処理を行う．
ⅱ.でDLしたchoreonoid-1.3.1.zipの展開をダブルクリックで行う．ダブルクリックすると，以下のウィンドウが出るので，展開．展開場所はhome/(ユーザ名)/に展開することをお勧めする．

② 開発ツールと依存ソフトウェアのインストール
展開した後，Ctrl+Alt+Tで”Terminal”を立ち上げ，以下のコマンドを実行する．
cd choreonoid-1.3.1
※cdは「チェンジディレクトリ」を行うコマンドで，Terminalで現在いるディレクトリの下層ディレクトリに移動するコマンド．Terminalを立ち上げた時は必ずhome/(ユーザ名)/のディレクトリにいる．そのため，choreonoid-1.3.1をhome/(ユーザ名)/のに展開しておくことで，Terminalを立ち上げた後，上のコマンド入力で即座にchoreonoid-1.3.1ディレクトリに移動できる．これが例えば，デスクトップ下に展開してしまった場合は，
cd デスクトップ/choreonoid-1.3.1
とコマンドを叩かなければならないので，正直めんどくさい．
Choreonoid-1.3.1下に移動した後，開発ツールと依存ソフトウェアをインストールするために，以下のコマンドを叩く．
misc/script/install-requisties-ubuntu-11.10.sh
赤字の部分はubuntuのバージョンを入れる．Ubuntuのバージョンの確認法としてホーム画面の右上にある 　ボタンを押し，システム設定をクリック．

こういう画面が出るので，さらに赤丸で囲まれたシステム設定をクリック．

赤のアンダーラインの箇所が，ubuntuのバージョンとなる．この数字を上記したコマンドに打ち込む．すると以下のように，ubuntuのエントリーパスワードが求められるので入力（打ち込んでも表示されない）．
数秒～数分ほどで，以下のように容量の確保が求められるので「y」を押してインストール完了．

③ ビルド設定
choreonoid-1.3.1ディレクトリ下で，以下のコマンドを実行し，Makefileを作成する．
cmake .
ただし，ピリオドの前に半角スペースを空けることに注意．実行の完了が確認できたら，以下のコマンドを入力．
make –j4
これは４つのビルドプロセスを並列処理するコマンドで，コア数が多ければ4の箇所を増やし，更に高速処理が行える．Make完了画面は以下の通り．

ⅰ.の参考URLにはこの後，「Qtスタイルの変更による描画速度の改善」と「Balancerプラグインについて」があるが，これの部分は割愛（多分やらなくても大丈夫なのではなかろうか）．

II. graspPulgin
i. 参照サイトURL
http://www.hlab.sys.es.osaka-u.ac.jp/grasp/ja/node/2

ii. graspPluginのインストールとビルド
ⅰ.の参照サイトと見比べながら，graspPluginのインストールとビルドを行ってほしい．

① graspPluginインストール
いきなりだが，graspPluginのインストールは参照URLのではなく，マーキュリアルというサーバからコピーしてくる．現在choreonoid-1.3.1のディレクトリにいると思うので，以下のコマンドで，その下層ディレクトリextpluginに移動する．
cd extplugin

そこで，マーキュリアルよりgraspPluginをインストールするために以下のコマンドを実行．ただし，これにはマーキュリアルのアカウントを持っておく必要があるので，持ってない場合は辻先生か，他の先生方に相談してほしい．
hg clone http://subaru.ait.kyushu-u.ac.jp/hg/grasp-plugin-hg
もし，どうしてもアカウントが手に入らない場合は，参考URL通りにインストールするとよい．ただしこの場合，通常のGraspPluginは使用できるが，以下の２．開発プログラムに示す関数等は用いることができない．

② PRMインストール
これは参照ＵＲＬと同様の処理を施す．以下のサイトにアクセス．
https://code.google.com/p/grasp-plugin/downloads/list
その後，以下のファイルをダウンロード．
PRM-1.3.zip
このzipファイルをchoreonoidと同様のやり方で，choreonoid-1.3.1/extplugin/graspPlugin以下に展開．

③ 必須パッケージインストール
graspPluginに必要なパッケージをインストールするために，以下のコマンドをchoreonoid-1.3.1/extplugin以下で実行．
./graspPlugin/Grasp/install-requisities-ubuntu.sh
※「ins」くらいの時にTabキーを押すと，ＰＣ側が大体の予測を立て，それ以降を表示してくれるから便利！

④ ビルド
graspPluginをchoreonoidで使用できるようにするために，ccmakeでビルド設定の変更を行う．まず，以下のコマンドで，choreonoid-1.3.1/extpluginからchoreonoid-1.3.1へ移動する．
cd ../
その後，以下のコマンドでビルド設定画面を開く．
ccmake .
ただし，ピリオドの前に半角スペースを空けることに注意．すると以下のようなウィンドウが現れるので，２度「ｃ」キーを押し，configureする．すると，下の方にGRASP_PLUGUINSとGRASP_ROBOT_MODEL_PLUGINSのつが編集できるようになる．

↓キーでGRASP_PLUGINSまで移動し，Enterで編集有効モードにする．中にGrasp，CurvedSurfaceGrasp，GeometryHandleの３つを加え，Enterを押し終了．GRASP_ROBOT_MODEL_PLUGINSにはPA10/Pluginを加え，Enterで終了．
※PCLというライブラリを使うときは，PCLをインストールし，上に示すようにGRASP_PLUGINSにPCLを加える．また，graspPluginをインストールするときに，マーキュリアルからではなく，https://code.google.com/p/grasp-plugin/downloads/listから行っている場合は，CurvedSurfaceGraspは使うことが出来ない．

その後，以下のコマンドにてmakeを実行する．
make –j4
makeがとおれば成功！
 
2. Choreonoid内の追加ボタン解説
choreonoid-1.3.1下に移動し，以下のコマンドでChoreonoidを立ち上げることが出来る．
bin/choreonoid
立ち上げた後，私の研究で用いたPlannerBarに追加したボタンと，キーボード操作による二次曲面の情報ファイル生成について、以下で述べる．

I. キーボード操作
i. ｃキー
二次曲面近似を選択した状態で「ｃ」キーをおすと，近似された二次曲面の式の係数情報と，そのクラスタのidが格納されたファイル「test.yaml」がchoreonoid-1.3.1下に生成される．ちなみに，これにより物体にはカラーリングが施されるが，二次曲面の形状を表示しているのではなく，一つの二次曲面で近似された領域が，一つの色で塗られた物体を表示しているだけである．
また，「ｃ」キーを押すだけだと，設定された近似精度の閾値によって，二次曲面の数が決められているが，Geometryバーにある「Target number clustering」で，近似に用いる二次曲面の数をユーザが指定することが出来る．

ii. ｂキー
「ｃ」キーを押した後に「ｂ」キーを押すと，二次曲面の組み合わせから一葉双曲面と，２つの楕円体により構成されるくびれを把持するための情報が格納されたファイル「boundaryData.yaml」がchoreonoid-1.3.1下に生成されてる．具体的には以下のとおりである．
 boundaryNum：くびれ（共通切断面）の番号
 Common Cutting Plan：共通切断面の式の係数
 shapeNum：一つの共通切断面における境界の番号
 shape：くびれの種類
 id：くびれを生成しているクラスタのid
　　一葉双曲面なら１つのidと0，楕円体によるくびれなら２つのidが表示
 targetPoint：ハンドをアプローチさせる時のターゲットとなる点の座標
 graspVec：グリッパの開閉軸とハンドのアプローチ軸
boundaryNumとshapeNumの具体図

iii. ｖキー
「ｃ」キーを押した後に「ｖ」キーを押すと，二次曲面の組み合わせから，複数の二次曲面を把持するための情報が格納されたファイル「DepartData.yaml」がchoreonoid-1.3.1下に生成されてる．具体的には以下のとおりである．
 二次曲面の組み合わせ
 Middle point：ハンドをアプローチさせる時のターゲットとなる点の座標
 approachVec：ハンドのアプローチ軸
 fingerVec：グリッパの開閉軸

iv. xキー
void ObjectAnalysis::LmitedInitialClustersFromOneFace()にて定義されるある限定的な範囲に対して二次曲面近似を行う．

II. Plannerボタン

i. QuadricSurfaceGrasp
楕円体，楕円柱を把持するためのボタン． choreonoid-1.3.1ディレクトリ内に「test.yaml」ファイルがある状態でQuadricSurfaceGraspボタンを押すと，以下の図のように，どの二次曲面を把持するかの選択画面が出る．

楕円体と楕円柱を対象としたボタンなので，「ellipsoid」「cylindrical surface_x(,y,z)」のみが有効．上の図ならば，2,4,6,7番のみが有効．その番号を入力してエンターを押せば把持が始まる．
また楕円体を把持する場合は，どの二次曲面との境界を把持するかの選択を行う．

ii. ConstrictionGrasp
一葉双曲面と，２つの楕円体から生成されるくびれを把持するためのボタン．choreonoid-1.3.1ディレクトリ内に「boundaryData.yaml」ファイルがある状態でConstrictionGraspボタンを押すと，以下の図のように，どの共通切断面に対して把持を行うのかを選択する（共通切断面については後述）．

その後，一葉双曲面か，くびれかを選択する．

ただし，選択を失敗した場合，表示されている番号ではなく「5963」を入力すると，把持計画が強制終了できる．

iii. DepartSurfaceGrasp
楕円体-平面，楕円柱-平面，平面-平面を把持するためのボタン．choreonoid-1.3.1ディレクトリ内に「DepartData.yaml」ファイルがある状態でDepartSurfaceGraspボタンを押すと，以下の図のように，１つめの二次曲面を選択できる．

選択すると，もう一つの二次曲面を選択することが出来る．選択を失敗した場合，表示されている番号ではなく「5963」を入力すると，把持計画が強制終了できる．

iv. check
現在は何にも使われていない（はずの）ボタン．関数の挙動をチェックする用に設置している．このボタンの中身は，後述するが，graspPlugin/CurvedSurfaceGrasp/EllipsoidGraspController.cpp内のcheckMotion()という関数が反映されているため，この関数をいじるとよい．

v. check_shape
test.yamlに格納されている二次曲面の形状を表示するボタン．押すと以下のように，二次曲面のidと形状を表記してくれる．

 
3. 開発プログラム
私が開発したプログラムが格納されているファイルは，大きく以下の３つである．ただし，すべてのcppファイル，hファイルはchoreonoid-1.3.1/extplugin/graspPlugin/以下にある．
 CurvedSurfaceGrasp/EllipsoidGraspController.h/cpp
 CurvedSurfaceGrasp/SurfaceForceClosure.h/cpp
 GeometryHandler/GeometryAnalysis.h/cpp
これらに作った関数を以下で説明していく．なお，省略している関数はあまり重要ではない(もしくは作っただけで使ってない)ので，気にしなくてもいい・・・と思う．

I. EllipsoidGraspController.h/cpp
 void calcConffience
二次曲面の係数VectorXd coを引数として，二次曲面の半径(co_rad)，中心座標(co_gap)，回転行列(co_evec)，形状(co_shape)を導出する．

 virtual void graspConvexShape
PlannerバーのQuadricSurfaceGraspボタンが押されると実行する関数．test.yamlに格納されている二次曲面の式から，二次曲面の半径(rad)，中心座標(gap)，回転行列(evec)，形状(shape)等を計算(この部分はcalcCofficienceで計算される)．その後，形状がellipsoid，cylindrical_surface_x(_y, _z)の二次曲面に対して把持姿勢を生成(createGraspPosForEllipsoid or createGraspPosForEllipse)，評価(calcEllValue or calcCylValue )までを行う．ちなみにQuadricSurfaceGraspボタンを押したときに選ぶ数字が格納されている変数はcnt2

 virtual void graspConstriction
PlannerバーのConstrictionGraspボタンが押されると実行する関数．boundaryData.yamlに格納されている凹部分に対する把持姿勢生成情報(targetPoint, graspVec)から，hyperboloid，elliposoidsに対する把持姿勢を生成(createGraspPosForEllipse)し，安定性の評価(calcConstValue)を行う．
この過程で，グリッパが凹部分を挟み込むために，グリッパを微調整するConstrictionApproachFingerや，接触点の数を判別するcheckContactNumなどを行う．

 virtual void graspDepartSurface
PlannerバーのDepartSurfaceGraspボタンが押されると実行する関数．DerpartData.yamlに格納されている曲面と平面の組み合わせに対する把持姿勢生成情報(Middle point, approachVec, fingVec)をもとに，把持姿勢を生成(createGraspPosForDepartSurface)し，安定性の評価(calcDepartValue)を行う．

 void closeFingers_move
指を閉じる関数．引数としてmode，fingVecがある．modeは，４種類あり，それぞれ以下のとおりである．
 mode=0：一度グリッパを最大まで開き，物体と指が干渉したら終了
 mode=1：一度グリッパを最大まで開き，物体と指が干渉し，さらにpress(グローバル変数)分だけ，物体にめり込む把持を行う．
 mode=2：現在のグリッパの位置からグリッパを閉じ始め，物体と指が干渉したら終了
 mode=3：現在のグリッパの位置からグリッパを閉じ始め，さらにpress(グローバル変数)分だけ，物体にめり込む把持を行う．

グリッパと指の干渉判定はcalcContactPoint関数にて行っている．この関数の引数にあたる以下の３つのベクトル
 物体からみた時のグリッパの相対位置（めり込む前）
 謎
 物体から指への法線ベクトル
が，各指毎にfingVecに格納されている．

 void createGraspPosForEllipsoid
３軸が分かっている場合の把持姿勢生成関数．具体的には，引数であるApproachSelectよりハンドのアプローチ姿勢を，CloseFingSelectよりグリッパの開閉姿勢を生成する．他の引数radは，物体からハンドをどの程度離すか（0.17-radという箇所）で用いる．Ell_p，Ell_Rは把持対象の二次曲面の位置，姿勢．

 void createGraspPosForEllipse
２軸が分かっている場合の把持姿勢生成関数．引数はcreateGraspPosForEllipsoidと同じ．

 void calcObjFingPosAndNor
安定性評価の関数に用いるObjFing_pとObjFing_nを導出する関数．引数のp_subはグリッパが物体にめり込む前の，物体から見た時のグリッパの相対位置が格納されている．

 void calcCylArea
楕円柱とグリッパの接触面積を導出する関数．対象となる楕円柱の式の係数qcと，楕円柱の半径radを用いる．グリッパと楕円柱の式から解析的に面積を導出する場合は，cpp上部にある#define calcContactAreaAnalyticalを，干渉メッシュ情報から面積を導出する場合は# calcContactAreaUsingMeshesをコメントインする．こうして接触面を構成する２つのパラメータb,Lを導出し，calcEnにてenを導出し，格納する．

 double calcEllValue
楕円体把持における安定性評価関数．calcObjFingPosAndNorより，ObjFing_pとObjFing_nを導出する．その後解析的，または干渉メッシュより面積を導出し(使い分けはcalcCylAreaと同様)，calcEnよりenを導出する．その後，SurfaceForceClosure::forceClosureTestEllipsoidSoftFinger_uto2より，安定性の評価を行う．
また引数のtimeは，cpp上部の#define DEBUG_TIMEをコメントインすることで，評価にかかった時間が格納される変数である．

 double calcCylValue
楕円柱把持における安定性評価関数．calcObjFingPosAndNorより，ObjFing_pとObjFing_nを導出，calcCylAreaよりenを導出し，SurfaceForceClosure::forceClosureTestEllipsoidSoftFinger_uto2より，安定性の評価を行う．

 void ConstrictionApproachFinger
凹部分を把持する場合の，グリッパの微調整関数．BoundaryCata.yamlの情報だけでは，凹部分を綺麗に挟み込むことは困難であるため，この関数を作成．Cpp上部の#define controlFingerUpDownをコメントインすると，ハンドの上下のみで微調整を行う．一方#define controlFingerUpDownandRollをコメントインすると，ハンドの上下に加え，ハンドの回転も行い，微調整を行う．
ちなみにcontrolFingerUpDownのみで結構精度よく微調整できた．

 void checkContactNum
凹部分を把持する際の，グリッパと把持対象との接触点数を判別する関数．判別手法は修論に記載しているので，そちらを参照すること

 void calcEllipsoidsEn
複数の楕円体により生成されるくびれを把持する場合のenを導出する関数．接触点数contactNum毎に接触面を構成するパラメータb,Lの導出法が異なる．また，これはグリッパと物体の干渉メッシュ情報を用いたb,Lの導出を行っておらず，解析的にb,Lを導出している．
calcEnを用い，b,Lからenを導出する．

 void calcHypeboloidEn
一葉双曲面を把持する場合のenを導出する関数．説明としてはcalcEllipsoidsEnとほぼ同様．

 double calcConstValue
凹部分を把持する場合の安定性評価を行う関数．ObjFing_pとObjFing_nの導出法だが，めんどくさいので解説は省略する．頑張って読み解いてほしい．その後，calcEllipsoidsEnやcalcHypeboloidEnでenを導出し，SurfaceForceClosure::forceClosureTestEllipsoidSoftFinger_uto2より，安定性の評価を行う．

 double calcEn
接触面を構成するパラメータb,Lよりenを導出する関数．Modeが８つ準備されており，それぞれが以下の応力分布を示している．
 mode=0：回転放物面（楕円体とか）
 mode=1：放物線柱面（楕円柱とか）
 mode=2：双極放物面―状態１（くびれとか）
 mode=5：双極放物面―状態２（くびれとか）
 mode=6：直方体（平面とか）

 void createGraspPosForDepartSurface
曲面と平面の組み合わせに対して把持姿勢を生成する関数．Tmp_pointに向かってtmp_approacVec方向からハンドをアプローチさせ，tmp_fingerVec方向に指を閉じる．

 double calcDepartValue
曲面と平面の組み合わせに対して安定性の評価を行う．calcContactPointよりObjFing_pとObjFing_nを導出し，calcEllPlaEn，calcCylPlaEn，calPlaPlaEnより，把持対象に対応したenを導出する．その後SurfaceForceClosure::forceClosureTestEllipsoidSoftFinger_uto2より，安定性の評価を行う．
ただし引数であるexchange_checkは，把持対象となる2つの二次曲面のidが昇順であれば0，降順であれば１となる．

 void calcEllPlaEn
楕円体と平面の組み合わせを把持する際のenを導出する関数．２本の指のどちらがどちらの曲面と干渉しているかの判別を行い，楕円体側は解析的に，平面側はグリッパとの干渉メッシュ情報を用いて，接触面を構成するパラメータb,Lを導出している．それらのb,LからcalcEnをもちいてenを導出する．

 void calcCylPlaEn
楕円柱と平面の組み合わせを把持する際のenを導出する関数．２本の指のどちらがどちらの曲面と干渉しているかの判別を行い，楕円柱，平面共に干渉メッシュ情報を用いて，接触面を構成するパラメータb,Lを導出している．それらのb,LからcalcEnをもちいてenを導出する．

 void calcPlaPlaEn
平面と平面の組み合わせを把持する際のenを導出する関数．２本の指のどちらがどちらの平面と干渉しているかの判別を行い，干渉メッシュ情報を用いて，接触面を構成するパラメータb,Lを導出している．それらのb,LからcalcEnをもちいてenを導出する．

 Bool checkCollisionUsingMeshes
物体とグリッパが干渉しているかのチェックを行う関数．具体的にはグリッパ表面のメッシュと，物体のメッシュが交差していればtrueを返し，交差していなければfalseを返す．

 void calcContactRegionParameters
グリッパと物体の接触領域を構成するパラメータb,Lを導出する関数．calcEllPlaEn, calcCylPlaEn, calcPlaPlaEnに対してこの関数使えば，もっとスマートになるかも？

 void calcContactRegionPoints
グリッパと物体の接触領域の境界点collision_pointを導出する関数．Modeが２つ用意されており，それぞれが以下のとおりである．
 mode=0：実際のメッシュ情報のみをもちいた境界点群
 mode=1：mode=0より導出された点群間を補完し，新たにcollision_point[2]と[3]に格納する．

 void double_vec3_QSort
double型の配列と，それに対応したVector3型の配列のクイックソートを行う関数．ネットで拾ってきたやつなので，信頼度は高いと思う．

 void double_Swap
double型の要素を交換する関数．

 void vec3_Swap
Vector3型の要素を交換する関数．

 void PCAforPoints
引数である点群posに対してPCAをかけ，その時の主軸３軸mainAxisを導出する関数．mainAxisの昇順に第一，第二，第三主軸

 double calcAngle
引数として渡される２つのベクトルがなす角度を0～πの範囲で導出する関数．

 double calcAngleFull
引数として渡される２つのベクトルがなす角度を-π～πの範囲で導出する関数．

 virtual void checkShape
test.yamlに格納されている二次曲面の形状を判別する関数．Plannerバーのcheck_shapeボタンが押されると実行する関数．

 static double calcContactPoint
グリッパと物体との最小距離を求める関数．得られる引数として，以下の３つがある．
 Po：物体からみた時のグリッパの相対位置
 Pf：謎
 ObjN：物体から指への法線ベクトル

 define DEBUG_TIME
コメントインすると，把持計画全体の時間を図ることが出来る．

 define DEBUG_CloseFinger
コメントインすると，グリッパを閉じる動作を確認することが出来る．

 “define controlFingerUpDown” and “define controlFingerUpDownandRoll”
凹部分を挟む場合，どちらの方法で微調整を行うか選べる．前者はハンドを上下させるのみで調整，後者はハンドの回転も行い調整を行う．

 “define calcContactAreaAnalytical” and “calcContactAreaUsingMeshes”
グリッパと物体の接触領域を構成するパラメータを導出する方法を選べる．前者は，グリッパ表面の式と二次曲面の式を用いて解析に解く．後者は干渉メッシュ情報を用いて解く．

II. SurfaceForceClosure.h/cpp
 double forceClosureTestEllipsoidSoftFinger_uto2
面接触かつ，摩擦円錐を楕円体近似したときのForceClosureに基づいた安定性評価を行う関数．引数は以下の通り．
 wrench：レンチ
 Pc：物体からみた時のグリッパの相対位置
 Nc：物体からグリッパへの法線ベクトル
 Point：指の数（凹部分を把持するとき，接触点が増えても，２本指であればここは２）
 Mu：静止摩擦係数
 F_max：最大荷重
 En：最大摩擦モーメントと最大摩擦力の比
 double NormalForceClosureTest_uto
面接触におけるForceClosureにもとづいた安定性評価を行う関数．forceClosureTestEllipsoidSoftFinger_uto2とは，行列Ｇの中身が異なる．

III. GeometryAnalysis.h/cpp
 void calcApproximatedPlane
２つの曲面の境界の点群を格納しているboundaryListより，その境界点群を平面で近似したときの平面の式を導出する関数．点群数が10以下の場合は，共通切断面の傾きの信頼度が低くなるため，共通切断面を導出しない．10以上の場合は，その点群にPCAをかけ，法線ベクトルを導出し，共通切断面の式を導出する．
引数のlistNumはなぜか関数の中で用いられていないので、無視しましょう・・・

 void createLineEllPla
楕円体-平面に対する把持姿勢を生成するためのapproachVec，fingVecを導出する関数．導出法は修論を参考にすること．

 void createLineCylPla
楕円柱-平面に対する把持姿勢を生成するためのapproachVec，fingVecを導出する関数．導出法は修論を参考にすること．

 void createLinePlaPla
平面-平面に対する把持姿勢を生成するためのapproachVec，fingVecを導出する関数．導出法は修論を参考にすること．

 void createBoundaryData
ｂキーを押したときに実行されboundaryData.yamlを生成する関数．曲面同士の境界情報boundaryListを作成し，それらの境界に対して共通切断面を生成しておく．calcConstrictionより，凹部分にたいする把持姿勢を生成するtargetPoint，graspVecを導出し，くびれを生成する二次曲面情報と共に，boundaryData.yamlに書き込む．

 void sortBoundary
boundarydataの中身を整理する関数．境界を構成する二次曲面の組み合わせでかぶっているものがあれば，それをboudarydataから削除したり，境界を構成する二次曲面のidを昇順にするなどの整理を行う．

 void calcConstriction
一葉双曲面や，２つの楕円体より構成されるくびれの情報を計算し，targetPointやgraspVec等を導出する関数．一葉双曲面が二次曲面の組み合わせ内にある場合，calcHyperboloidを用いて，targetPointとgraspVecを導出する．
一方２つの楕円体が境界を生成している場合，calcBoundaryAreaで共通切断面との各々の接触面の面積と，targetPoint，graspVecを導出する．面積とcheckConstrictionによりくびれ判定を行い，くびれていると判定された場合は，targetPointとgraspVecをboudarydataに格納する．

 int checkQuadricSurface
test.yamlにおける二次曲面の形状を判別する関数．

 void calcBoundaryArea
2つの楕円体が境界を構成する場合，それらと共通切断面(係数は引数のco)の接触領域の面積Sと，把持姿勢生成に用いるtargetPoint，graspVecを導出する．

 void calcHyperboloid
二次曲面の組み合わせの中に一葉双曲面があれば，それを把持するためのtargetPointとgraspVecを導出する関数．

 bool checkConstriction
2つの楕円体と，それらの共通切断面との接触領域の面積より，それら２つの楕円体がくびれを生成しているかの判別を行う関数．判別の方法は修論を参考にすること．

 void createDepartData
ｖキーを押したときに実行されDepartData.yamlを生成する関数．二次曲面のidと形状を全て把握し，それらを２つづつの組み合わせにしたリストdepartDataを生成する．その後，それぞれの組み合わせ毎にgrasp_point，approachVec，fingerVecを導出し，DepartData.yamlに書き込む．

 void calcBoundaryMainAxis
boundaryListに格納されている境界点群に対してPCAをかけ，第一主軸と第二主軸を導出し，v1，v2に格納する関数．

 cnoid::Vector3 calcMesheCenter
clusterNodeに格納されている１つの二次曲面で近似されたメッシュ群の重心を計算する関数．引数のidはcluster番号を指す．

 cnoid::Vector3 calcBoundaryCenter
１つの二次曲面に近似されたクラスタと，他のクラスタとの境界点群の重心を導出する関数．

 void LimitedInitialClustersFromOneFace()
ある限定的な範囲でinitalClusterFromFaceを行う関数．このinitialClusterFromFaceは，初期状態で，メッシュにクラスタを振り分ける関数．LimitedInitialClusterFromOneFaceで，クラスタを振り分けるメッシュの範囲を指定し，二次曲面近似を行うと、その範囲のみの二次曲面近似が可能となる．
この限定する範囲は、lim_x, lim_y, lim_zにより現在は手動で与えている．