InverseKinematics.cpp
#include "StdAfx.h"
#include "InverseKinematics.h"
#include "ForwardKinematics.h"
#include "Link.h"
#include "Joint.h"
#include "ConnectionRelation.h"
using namespace Core;
using namespace Robotics;
using namespace System;
using namespace System::Diagnostics;
/// [ Constructor ]
InverseKinematics::InverseKinematics( Links^ links ) :
m_links( links )
{
m_anglesBeforeChange = gcnew Angles( m_links->Length );
for( int i = 0; i < m_links->Length; i++ )
{
// 全てのリンクの角度を保存する
m_anglesBeforeChange[ i ] = m_links[ i ]->Joint->Angle;
}
}
/// 全ての関節の回転角を求める (逆運動学)
bool InverseKinematics::CalculateAngleOfAllJoints( Link^ operationTarget, Robotics::PositionVector^ targetPosition, Robotics::RotationMatrix^ targetPosture )
{
Debug::Assert( !operationTarget->IsStandard(), "演算対象は基準リンクではない" );
m_quantityToRevise = 0.5; // 修正量係数
m_operationTarget = operationTarget; // 演算対象のリンク
m_targetPosition = gcnew PositionVector( targetPosition ); // 目標とする位置
m_targetPosture = gcnew RotationMatrix( targetPosture ); // 目標とする姿勢
// 演算対象となるリンクを並べた経路を生成する
CreateRouteOfLinks();
const int DegreeOfFreedomCanManage = 6; // 対応できる自由度数
if( m_routeOfLinks->Count == DegreeOfFreedomCanManage )
{
// ニュートン・ラプソン法により求める
if( !NewtonRaphson() )
{
// 演算に失敗したならば、リンクを元の状態に戻す
ForwardKinematics::CalculatePositionAndPostureOfLinks( m_links, m_anglesBeforeChange );
return false;
}
}
else
{
Debug::WriteLine( "対応できない自由度数である" );
return false;
}
return true;
}
/// ニュートン・ラプソン法により求める
bool InverseKinematics::NewtonRaphson()
{
double error = Double::MaxValue; // 誤差
// 上限回数まで 修正をくり返す
for( int count = 0; count < RepeatLimit; count++ )
{
// 位置と姿勢の微小変化量(誤差)を求める
Vector^ errorOfPositionAndPosture = CalculateErrorOfPositionAndPosture();
// ... それのノルムを求める
double norm = errorOfPositionAndPosture->Norm();
if( error < norm )
{
Debug::WriteLine( "誤差が発散している Counter : " + count );
return false;
}
if( Math::Abs( error - norm ) < Double::Epsilon )
{
Debug::WriteLine( "誤差を抑え込めない Counter : " + count );
return false;
}
error = norm; // ノルムを誤差として設定する
const double ErrorEnoughSmall = 1.0E-6; // 十分に小さいと見なす 誤差の大きさ
if( error < ErrorEnoughSmall )
{
// 誤差が十分に小さくなったならば、演算を終了する
return true;
}
// ヤコビアンを求める
Matrix^ jacobian = CalculateJacobian();
Matrix^ inverseJacobian;
// ... それの逆行列を求める
if( !jacobian->InverseMatrix( inverseJacobian ) )
{
Debug::WriteLine( "ヤコビアンの逆行列が存在しない" );
return false;
}
// 関節角度 修正量を求める (ヤコビアンの逆行列と 位置と姿勢の微小変化量との積)
Vector^ angleCorrection = ( inverseJacobian * errorOfPositionAndPosture );
// ... それに修正量係数を掛ける (変化量を小さくすることで 数値計算を安定化する)
angleCorrection *= m_quantityToRevise;
for( int i = 0; i < m_routeOfLinks->Count; i++)
{
// 経路にあるリンクの関節に 角度範囲内で 関節角度 修正量を加算する
m_routeOfLinks[ i ]->Joint->AddAngleWithinRange( angleCorrection[ i + 1 ] );
}
// 経路の末尾のリンクを取得する
Link^ lastLink = m_routeOfLinks[ m_routeOfLinks->Count - 1 ];
// ... リンクの位置と姿勢を求める
ForwardKinematics::CalculatePositionAndPostureOfLinks( lastLink );
/// @note
/// 末尾のリンクを指定することで、経路にある全てのリンクが演算対象となる。
}
Debug::WriteLine( "指定回数内で、計算を完了することが出来なかった" );
return false;
// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P58
}
/// ヤコビアンを求める
Robotics::Matrix^ InverseKinematics::CalculateJacobian()
{
Matrix^ result = gcnew Matrix( 6, m_routeOfLinks->Count );
int column = 1; // 列
for each( Link^ link in m_routeOfLinks )
{
// ワールド座標系での軸方向を取得する
AxisVector^ worldAxialDirection = link->AxialDirection;
// ... 微小回転を設定する
result[ 4 ][ column ] = worldAxialDirection->X;
result[ 5 ][ column ] = worldAxialDirection->Y;
result[ 6 ][ column ] = worldAxialDirection->Z;
if( link == m_routeOfLinks[ 0 ] )
{
// 目標とするリンクならば、それとの微小変位はゼロとする
result[ 1 ][ column ] = 0.0;
result[ 2 ][ column ] = 0.0;
result[ 3 ][ column ] = 0.0;
}
else
{
// 目標とするリンクまでの変位量を求める
PositionVector^ displacement = ( m_routeOfLinks[ 0 ]->AbsolutePosition - link->AbsolutePosition );
// ... ワールド座標系での軸方向とその変位量との ベクトルの外積から 微小変位を求める
Vector^ smallDisplacement = Vector::CrossProduct( worldAxialDirection, displacement );
// ... 微小変位を設定する
result[ 1 ][ column ] = smallDisplacement[ 1 ];
result[ 2 ][ column ] = smallDisplacement[ 2 ];
result[ 3 ][ column ] = smallDisplacement[ 3 ];
}
column++;
}
return result;
// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P60
}
/// 位置と姿勢の微小変化量(誤差)を求める
Robotics::Vector^ InverseKinematics::CalculateErrorOfPositionAndPosture()
{
// 目標位置と演算対象の位置との差から 位置の誤差を求める
Vector^ errorOfPosition = m_targetPosition - m_operationTarget->AbsolutePosition;
// 演算対象の回転行列の逆行列と 目標姿勢の回転行列との積を求める
RotationMatrix^ productOfPosture = m_operationTarget->Posture->TransposedMatrix() * m_targetPosture;
// ... それの角速度ベクトルを求める
Vector^ angularVelocity = productOfPosture->AngularVelocityVector();
// ... 演算対象の姿勢とそれとの積から 姿勢の誤差を求める
Vector^ errorOfPosture = m_operationTarget->Posture * angularVelocity;
// 位置と姿勢の誤差を並べたベクトルを作成して返す
return Vector::Combine( errorOfPosition, errorOfPosture );
// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P64
}
/// リンクの経路を生成する
void InverseKinematics::CreateRouteOfLinks()
{
Debug::Assert( m_quantityToRevise != 0.0, "修正量係数は定義されている" );
// リンクの経路を格納するリストを生成する
m_routeOfLinks = gcnew LinkList();
if( m_operationTarget->IsBody() )
{
// 対象とするのが胴体ならば、基準リンクから 胴体までを探索する
Link::Standard->AddOwnToRouteToBody( m_routeOfLinks );
// ... 胴体から基準リンクへの順となるように、並びを反転させる
m_routeOfLinks->Reverse();
// 修正量係数の符号を反転させる
m_quantityToRevise = -m_quantityToRevise;
/// @note
/// メソッドが 胴体までの経路を探索するもののため、
/// このように反対からの経路を作成して、その後に反転している。
}
else
{
// 対象とするリンクから 胴体までを探索する
m_operationTarget->AddOwnToRouteToBody( m_routeOfLinks );
}
Debug::Assert( m_routeOfLinks->Count != 0, "経路は存在する" );
Debug::Assert( !m_routeOfLinks->Contains( Link::Body ), "胴体は経路に含まれない" );
}