Model.cpp
#include "StdAfx.h"
#include "Model.h"
#include "Pose.h"
#include "Link.h"
#include "Joint.h"
#include "Shape.h"
#include "SupportPolygon.h"
#include "ForwardKinematics.h"
#include "InverseKinematics.h"
using namespace Core;
using namespace Robotics;
using namespace Microsoft;
using namespace System;
using namespace System::Diagnostics;
/// [ Constructor ]
Model::Model( Core::Links^ links ) :
m_links( links )
{
Core::Link::Body = m_links[ 0 ]; // 胴体
// リンクの 位置と姿勢の変更イベントに登録する
Core::Link::PositionAndPostureChanged
+= gcnew PositionAndPostureChangeEventHandler( this, &Model::LinkPositionAndPostureChanged );
// 関節の構造変化イベントに登録する
Joint::MechanicsChanged += gcnew System::EventHandler( this, &Model::Recompute );
}
/// [ Finalizer ]
Model::!Model()
{
delete m_centerOfGravityFigure;
}
/// 状態を初期化する
void Model::InitializeState()
{
for each( Core::Link^ link in m_links )
{
// 全ての関節角度を初期値にする
link->Joint->InitializeAngle();
}
// 胴体を 原点位置に強制的に設定する
Core::Link::Body->SetPositionAndPostureForcibly(
gcnew PositionVector(),
gcnew RotationMatrix()
);
// ... リンクの位置と姿勢を求める
ForwardKinematics::CalculatePositionAndPostureOfLinks( Core::Link::Body );
// ... 地面との衝突を回避する
EvadeCollisionWithGround();
/// @note
/// イベントを発生させないために、再計算メソッドを呼んで処理させていない。
Debug::WriteLine( "モデルを初期化した" );
}
/// 再計算する
void Model::Recompute( System::Object^ sender, System::EventArgs^ )
{
// 引数から目的とする型を取得する
Joint^ joint = safe_cast< Joint^ >( sender );
// リンクの位置と姿勢を求める
ForwardKinematics::CalculatePositionAndPostureOfLinks( Link[ joint->Name ] );
// ... 状態変化イベントを発生する
StateChanged( this, nullptr );
}
/// 地面との衝突を回避する
bool Model::EvadeCollisionWithGround()
{
const double ThresholdHeight = 0.001; // 衝突しているとみなす高さ[ m ]
/// @note
/// ここではリンクの位置ではなく、形状の頂点位置について考えている。
// 最低高さのリンクを取得する
Core::Link^ link = GetLinkInLowestPosition();
// ... そのリンクの形状の最下部を取得する
PositionVector^ bottom = link->GetBottomOfShape();
// 衝突しているか？
if( bottom->Z < ThresholdHeight )
{
PositionVector^ position = link->AbsolutePosition; // リンクの絶対位置
RotationMatrix^ posture = link->Posture; // リンクの姿勢
// 地面に接地させる
position->Z = 0.0;
// 水平にする
posture->Horizontal();
// 衝突しているリンクを それを回避するように強制的に設定する
link->SetPositionAndPostureForcibly( position, posture );
// ... それを基準に 再計算する
Recompute( link->Joint, nullptr );
/// @todo
/// 姿勢の修正によって 他のリンクが衝突する可能性があるので、
/// 衝突の確認をくり返す。
Debug::WriteLine( String::Format( "地面との衝突を回避した ( 上方に {0:0.000}[ m ] )", -bottom->Z ) );
Debug::Assert( Math::Abs( link->GetBottomOfShape()->Z ) < ThresholdHeight, "衝突を回避したリンクは 地面に接している" );
return true;
}
return false;
}
/// 最低高さのリンクを取得する
Core::Link^ Model::GetLinkInLowestPosition()
{
Core::Link^ result;
double bottom = Double::MaxValue; // 最下部
for each( Core::Link^ target in m_links )
{
// リンクの形状の最下部を取得する
PositionVector^ position = target->GetBottomOfShape();
// ... それの高さと比較する
if( position->Z < bottom )
{
bottom = position->Z;
result = target;
}
}
Debug::Assert( result != nullptr, "最下部のリンクは存在する" );
return result;
}
/// リンクの位置と姿勢が変更された
void Model::LinkPositionAndPostureChanged( System::Object^ sender, PositionAndPostureChangeEventArgs^ e )
{
// 引数から目的とする型を取得する
Core::Link^ targetLink = safe_cast< Core::Link^ >( sender );
/// @todo
/// 直前のポーズに戻してから、逆運動学の演算を実行するようにする。
/// それにより、より適する解を得られると思われる。
// 対象となるリンクと それの位置と姿勢を格納するためのリストを生成する
LinkList^ targetLinks = gcnew LinkList();
Collections::Generic::List< PositionVector^ >^ targetPositions = gcnew Collections::Generic::List< PositionVector^ >();
Collections::Generic::List< RotationMatrix^ >^ targetPostures = gcnew Collections::Generic::List< RotationMatrix^ >();
// ... 指定されたリンクと その位置と姿勢を格納する
targetLinks->Add( targetLink );
targetPositions->Add( e->Position );
targetPostures->Add( e->Posture );
for each( Core::Link^ link in m_links )
{
if( !link->IsStandard() && link->IsSupportingPhase() )
{
// 基準リンクではなく支持相ならば、それを保存する
targetLinks->Add( link );
targetPositions->Add( gcnew PositionVector( link->AbsolutePosition ) );
targetPostures->Add( gcnew RotationMatrix( link->Posture ) );
}
}
// 逆運動学クラスを生成する
InverseKinematics^ inverseKinematics = gcnew InverseKinematics( m_links );
for( int i = 0; i < targetLinks->Count ; i++ )
{
// 全ての関節の回転角を求める
bool success = inverseKinematics->CalculateAngleOfAllJoints(
targetLinks[ i ],
targetPositions[ i ],
targetPostures[ i ]
);
// 求まらなかったならば、処理を中止する
if( !success ) return;
// 対象が胴体でなければ、処理を終える
if( !targetLink->IsBody() ) break;
/// @note
/// 胴体以外ならば、支持相のリンクが移動させられることはないので、
/// 複数のリンクについて、演算を繰り返す必要はない。
}
// 状態変化イベントを発生する
StateChanged( this, nullptr );
}
/// リンクを取得する [ Property ]
Core::Link^ Model::Link::get( System::String^ name )
{
for each( Core::Link^ link in m_links )
{
if( link->Name == name )
{
// 名称の一致するリンクを返す
return link;
}
}
Debug::Fail( "一致するリンクが存在しない" );
return nullptr;
}
/// リンク番号を取得する
int Model::GetLinkIndex( Core::Link^ link )
{
for( int index = 0; index < m_links->Length; index++ )
{
if( m_links[ index ] == link )
{
return index;
}
}
Debug::Fail( "一致するリンクが存在しない" );
return 0;
}
/// 支持多角形を取得する [ Property ]
Core::SupportPolygon^ Model::SupportPolygon::get()
{
// リンクから支持多角形を生成して返す
return Core::SupportPolygon::Create( m_links );
}
/// 重心を取得する [ Property ]
Robotics::PositionVector^ Model::CenterOfGravity::get()
{
// 全てのリンクの 重心を求める
return CalculateCenterOfGravity( LinkLists );
}
/// 質量を取得する [ Property ]
double Model::Mass::get()
{
// リンクの質量を求める
return CalculateMass( LinkLists );
}
/// ポーズをとる
void Model::DoPose( Pose^ pose )
{
// ポーズをとる
pose->PosedModel( this );
// ... 状態変化イベントを発生する
StateChanged( this, nullptr );
}
/// リンクを地面に接地させる
void Model::LinkGrounded( Distinction^ distinction )
{
if( distinction != nullptr )
{
// リンクを取得する
Core::Link^ link = Link[ distinction->Name ];
// リンクが形状であり、基準リンクではないことを確認する
if( ( distinction->GetType() == Shape::typeid ) && ( !link->IsStandard() ) )
{
// 支持相でなければ、地面に接地させる
if( !link->IsSupportingPhase() )
{
PositionVector^ position = link->AbsolutePosition; // リンクの絶対位置
RotationMatrix^ posture = link->Posture; // リンクの姿勢
// 地面に接地させる
position->Z = 0.0;
// 水平にする
posture->Horizontal();
// リンクの位置と姿勢を設定する
link->SetPositionAndPosture( position, posture );
}
if( link->IsSupportingPhase() )
{
// 支持相に設定されたならば、基準リンクにする
Core::Link::Standard = link;
}
}
}
}
/// 重心を求める
Robotics::PositionVector^ Model::CalculateCenterOfGravity( LinkList^ links )
{
PositionVector^ result = gcnew PositionVector( 0.0, 0.0, 0.0 );
// 指定されたリンクの ワールド座標系における 重心ベクトルを合計する
for each( Core::Link^ link in links )
{
result += link->GetCenterOfGravityInWorldPosition();
}
// ... 得られたモーメントを それらの全質量で割って返す
return( result / CalculateMass( links ) );
// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P90
}
/// 質量を求める
double Model::CalculateMass( LinkList^ links )
{
double result = 0.0;
// 指定されたリンクの 質量を合計する
for each( Core::Link^ link in links )
{
result += link->Mass;
}
return result;
}
/// 図形を生成する
void Model::CreateFigure( Microsoft::DirectX::Direct3D::Device^ device )
{
m_centerOfGravityFigure = DirectX::Direct3D::Mesh::Sphere(
device,
0.01f, // 球の半径[ m ]
8, // Z軸に平行な分割数
8 // Z軸に垂直な分割数
);
Debug::WriteLine( "リンクの図形を生成する" );
for each( Core::Link^ link in m_links )
{
// リンクの図形を生成する
link->CreateFigure( device );
}
}
/// 描画する
void Model::Render( Microsoft::DirectX::Direct3D::Device^ device )
{
// ライトを有効にする
device->RenderState->Lighting = true;
// マテリアルを生成する
DirectX::Direct3D::Material material;
material.Ambient = Drawing::Color::Yellow; // 環境光に対する反射係数
material.Diffuse = Drawing::Color::Yellow; // 拡散反射係数
// ... それをマテリアルに設定する
device->Material = material;
PositionVector^ position = CenterOfGravity; // 重心
// ... ワールド座標に それを設定する
device->Transform->World
= DirectX::Matrix::Translation(
safe_cast< float >( position->X ),
safe_cast< float >( position->Y ),
safe_cast< float >( position->Z )
);
// レンダリングする
m_centerOfGravityFigure->DrawSubset( 0 );
}