Torque.cpp

#include "StdAfx.h"
#include "Torque.h"


#include "Model.h"
#include "Link.h"


using namespace Core;
using namespace Robotics;

using namespace System;
using namespace System::Diagnostics;


/// [ Constructor ]
Torque::Torque( Model^ model ) :
    m_model( model )
{
    m_totalCenterOfGravity = model->CenterOfGravity;    // 全体の重心
    m_totalMass = model->Mass;                          // 全体の質量

    // 床反力を求める
    CalculateGroundReactionForce();
}


/// トルクを取得する
double Torque::default::get( Link^ targetLink )
{
    // 胴体ならば、トルクは生じない
    if( targetLink->IsBody() ) return 0.0;


    LinkList^ operationLinks = gcnew LinkList();

    // 対象とするリンクから 末端までのリンクを取得する
    targetLink->AddOwnToRouteToEnd( operationLinks );


    /// @todo 末端以外のリンクが支持相の場合にも対応する。


    // 末端のリンクを取得する
    Link^ endLink = operationLinks[ operationLinks->Count - 1 ];

    // ... それが支持相ならば、演算対象を変更する
    if( endLink->IsSupportingPhase() )
    {
        // 末端までのリンク以外に支持相が存在するならば、その反力も考慮する
        if( m_supportingLink->Count != 1 )
        {
            double torqueByGroundReactionForce = 0.0;

            // 床反力による トルクの和を求める
            for( int i = 0; i < m_supportingLink->Count; i++ )
            {
                torqueByGroundReactionForce += targetLink->CalculateTorque(
                    m_groundReactionForce[ i ],             // 床反力
                    m_supportingLink[ i ]->AbsolutePosition // 支持相のリンクの絶対位置
                    );
            }


            // 質量と重力加速度の積から 重量を求める (重力加速度は、Z軸の負の方向にしか働かない)
            PositionVector^ weight = gcnew PositionVector( 0.0, 0.0, m_totalMass * ( -AccelerationOfGravity ) );

            // ... 重心によるトルクを求める
            double torque = targetLink->CalculateTorque( weight, m_totalCenterOfGravity );

            // ... それに 床反力によるトルクを加算して返す
            return( torque + torqueByGroundReactionForce );
        }


        // 全てのリンクを取得する
        LinkList^ newOperationLinks = gcnew LinkList( m_model->LinkLists );

        // ... そこから 対象とするリンク以外の 末端までのリンクを削除する
        for( int i = 1; i < operationLinks->Count; i++ )
        {
            newOperationLinks->Remove( operationLinks[ i ] );
        }

        // それを演算対象とする
        operationLinks = newOperationLinks;
    }


    // 対象とするリンクの 質量の合計を求める
    double mass = m_model->CalculateMass( operationLinks );

    // 対象とするリンクの 全体の重心を求める
    PositionVector^ centerOfGravity = m_model->CalculateCenterOfGravity( operationLinks );


    // 質量と重力加速度の積から 重量を求める (重力加速度は、Z軸の負の方向にしか働かない)
    PositionVector^ weight = gcnew PositionVector( 0.0, 0.0, mass * ( -AccelerationOfGravity ) );

    // ... 重心によるトルクを求める
    return targetLink->CalculateTorque( weight, centerOfGravity );
}


/// 床反力を求める
void Torque::CalculateGroundReactionForce()
{
    // 質量と重力加速度の積から 重量を求める (反力を考えるので、重力加速度はZ軸の正の方向に働くものとする)
    PositionVector^ weight = gcnew PositionVector( 0.0, 0.0, m_totalMass * AccelerationOfGravity );


    // 支持相のリンクを取得する
    m_supportingLink = GetLinkWithSupportingPhase( m_model->LinkLists );

    int supportingSum = m_supportingLink->Count;    // 支持相のリンクの数
    Debug::Assert( 0 < supportingSum, "支持相のリンクは存在する" );
    Debug::Assert( supportingSum <= 2, "現時点では、支持相が２つまでしか対応できない" );


    // 床反力を格納するフィールドを 支持相の数だけ生成する
    m_groundReactionForce = gcnew PositionVectors( supportingSum );

    if( supportingSum == 1 )
    {
        // 支持相が１つだけならば、そこに全ての重量がかかる
        m_groundReactionForce[ 0 ] = weight;
    }
    else
    {
        array< double >^ momentArm = gcnew array< double >( supportingSum );

        // 反力の位置までの モーメント・アームの大きさを求める
        for( int i = 0; i < supportingSum; i++ )
        {
            // 支持相のリンクから重心までの 変位を求める
            PositionVector^ displacement = m_supportingLink[ i ]->AbsolutePosition - m_totalCenterOfGravity;


            // 反力の軸ベクトルを定義する
            AxisVector^ axisOfReactionForce = gcnew AxisVector( 0.0, 0.0, 1.0 );

            // ... それと変位とのベクトルの外積から モーメント・アームの大きさを求める
            Vector^ momentArmVector = Vector::CrossProduct( axisOfReactionForce, displacement );


            // ベクトルのノルムから それの大きさを求める
            momentArm[ i ] = momentArmVector->Norm();
        }

        if( supportingSum == 2 )
        {
            double length = momentArm[ 0 ] + momentArm[ 1 ];

            // モーメント・アームの大きさに応じて それぞれのリンクに重量がかかる
            m_groundReactionForce[ 0 ] = ( momentArm[ 1 ] / length ) * weight;
            m_groundReactionForce[ 1 ] = ( momentArm[ 0 ] / length ) * weight;
        }
    }
}


/// 支持相のリンクを取得する
LinkList^ Torque::GetLinkWithSupportingPhase( LinkList^ links )
{
    LinkList^ result = gcnew LinkList();

    for each( Link^ link in links )
    {
        if( link->IsSupportingPhase() )
        {
            // 支持相のリンクならば それを追加する
            result->Add( link );
        }
    }
    return result;
}