まず、初期軌道教示時に、後で述べる障害物回避のルーチンを用いて障害物回避を行うようにした。 また、最適化計算の効率化のために、曲率に基づいた初期軌道のノード数を削減 を滑り状態と非滑り状態に別けて行った。
次に最適化に関しては、軌道の最短化・特異姿勢の回避・障害物の回避・物体の着地の4指標に重みつきの総和を評価とし、準ニュートン法を用いて最適軌道を求めた。 地面や障害物との距離を計算するため、ロボットの各リンクのノード間の軌跡を結んでできる図形と障害物を凸多面体として考え、線形計画問題に帰着した。
これにより、各凸多面体の頂点情報だけで各リンクと障害物または地面との最接近距離と衝突判定が同時に行える。
また滑り状態における各ノードは、滑り面に拘束され着地面の法線ベクトルに直交し、滑り状態を2つの座標軸で表現する。
滑り部分が極端に小さくならないように、教示で与えた着地瞬間のノードと最適化時のノードの距離をコストとすることで、最適軌道を得ることができる。
さらに、最適化を行う際の各ノード間隔のばらつきを、各関節角の差を重み係数にすることで解決し、滑らかな軌道を得ることができた。