Pose.cpp

#include "StdAfx.h"
#include "Pose.h"


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

#include "Torque.h"

#include "SupportPolygon.h"
#include "ForwardKinematics.h"


using namespace Core;
using namespace Robotics;

using namespace System;
using namespace System::Diagnostics;


/// モデルから属性を取得して生成する [ Constructor ]
Pose::Pose( double time, Model^ model ) :
    m_time( time )
{
    m_standardLink = Link::Standard;      // 基準リンク
    m_positionOfStandardLink = gcnew PositionVector( m_standardLink->AbsolutePosition ); // 基準リンクの位置
    m_postureOfStandardLink = gcnew RotationMatrix( m_standardLink->Posture );           // 基準リンクの姿勢


    // リンクの数だけ 属性を生成する
    CreateAttribute( model->Links->Length );

    // ... モデルの状態を設定する
    SetModelState( model );
}

/// ポーズをコピーして生成する [ Constructor ]
Pose::Pose( double time, Pose^ original ) :
    m_time( time )
{
    m_standardLink = original->m_standardLink;      // 基準リンク
    m_positionOfStandardLink = gcnew PositionVector( original->m_positionOfStandardLink ); // 基準リンクの位置
    m_postureOfStandardLink = gcnew RotationMatrix( original->m_postureOfStandardLink );   // 基準リンクの姿勢

    m_electricPower = original->m_electricPower;                                // 電力
    m_centerOfGravity = gcnew PositionVector( original->m_centerOfGravity );    // 重心
    m_supportPolygon = gcnew Core::SupportPolygon( original->m_supportPolygon );// 支持多角形


    // リンクの数(角度の数で代用している)だけ 属性を生成する
    CreateAttribute( original->m_angleOfJoints->Length );

    // ... 全ての要素をコピーする
    original->m_positionOfLinks->CopyTo( m_positionOfLinks, 0 );// リンクの位置
    original->m_postureOfLinks->CopyTo( m_postureOfLinks, 0 );  // リンクの姿勢
    original->m_angleOfJoints->CopyTo( m_angleOfJoints, 0 );    // 関節の角度
    original->m_torqueOfJoints->CopyTo( m_torqueOfJoints, 0 );  // 関節のトルク
}

/// XMLから生成する [ Constructor ]
Pose::Pose( System::Xml::XmlElement^ element, Model^ model )
{
    // 時間を属性から取得する
    m_time = Double::Parse( element->GetAttribute( "time" ) );


    Xml::XmlElement^ linkElement = element[ "standardLink" ];    // 基準リンクの要素

    // 名称から 基準リンクを設定する
    m_standardLink = model->Link[ linkElement->GetAttribute( "name" ) ];

    m_positionOfStandardLink = gcnew PositionVector( linkElement );    // 基準リンクの位置
    m_postureOfStandardLink = gcnew RotationMatrix( linkElement );     // 基準リンクの姿勢


    // リンクの数だけ 属性を生成する
    CreateAttribute( model->Links->Length );

    // 全ての関節の要素に設定する
    for each( Xml::XmlElement^ node in element[ "joint" ]->ChildNodes )
    {
        Debug::Assert( node != nullptr, "有効な要素である" );

        // 属性からインデックスを取得する
        int index = int::Parse( node->GetAttribute( "index" ) );
        m_angleOfJoints[ index ] = Angle( Double::Parse( node->InnerText ) );   // 角度
    }

    // リンクの位置と姿勢を求める
    ForwardKinematics::CalculatePositionAndPostureOfLinks( model->Links, m_angleOfJoints );

    // ... モデルの状態を設定する
    SetModelState( model );
}

/// 属性を生成する
void Pose::CreateAttribute( int numberOfLinks )
{
    m_positionOfLinks = gcnew Positions( numberOfLinks );
    m_postureOfLinks = gcnew Postures( numberOfLinks );
    m_angleOfJoints = gcnew Angles( numberOfLinks );
    m_torqueOfJoints = gcnew array< double >( numberOfLinks );
}


/// 基準リンクを設定する [ Property ]
void Pose::StandardLink::set( Link^ value )
{
    Debug::Assert( value != nullptr, "基準リンクは定義されている" );
    Debug::Assert( value->IsSupportingPhase(), "基準リンクは支持相である" );

    m_standardLink = value;      // 基準リンク
}

/// モデルの状態を設定する
void Pose::SetModelState( Model^ model )
{
    m_centerOfGravity = gcnew PositionVector( model->CenterOfGravity );     // 重心
    m_supportPolygon = gcnew Core::SupportPolygon( model->SupportPolygon ); // 支持多角形


    Torque^ torque = gcnew Torque( model ); // トルク
    double totalCurrent = 0.0;              // 電流の合計

    int index = 0;
    for each( Link^ link in model->Links )
    {
        m_positionOfLinks[ index ] = gcnew PositionVector( link->AbsolutePosition ); // リンクの位置
        m_postureOfLinks[ index ] = gcnew RotationMatrix( link->Posture );           // リンクの姿勢


        double torqueValue = torque[ link ];  // トルク

        m_angleOfJoints[ index ] = link->Joint->Angle;    // 関節の角度
        m_torqueOfJoints[ index ] = torqueValue;          // 関節のトルク

        // トルクにトルク定数を積算して 電流の合計に加算する
        totalCurrent += Math::Abs( torqueValue * link->Joint->TorqueConstant );
        index++;
    }

    // 電流の合計にモデルの電圧を積算して 電力を求める
    m_electricPower = totalCurrent * model->Voltage;
}


/// モデルにポーズをとらせる
void Pose::PosedModel( Model^ model )
{
    if( !m_standardLink->IsStandard() )
    {
        // 現在の基準リンクでなければ、それの位置と姿勢を強制的に設定する
        m_standardLink->SetPositionAndPostureForcibly(
            gcnew PositionVector( m_positionOfStandardLink ),
            gcnew RotationMatrix( m_postureOfStandardLink )
            );

        /// @note
        /// ハンドルを渡すと変更される恐れがあるため、
        /// インスタンスを生成して渡している。

        Debug::Assert( m_standardLink->IsStandard(), "基準リンクに設定される" );
    }

    // リンクの位置と姿勢を求める
    ForwardKinematics::CalculatePositionAndPostureOfLinks( model->Links, m_angleOfJoints );
}


/// 関節の角度を補完する
void Pose::ComplementAngleOfJoints( Pose^ before, Pose^ after )
{
    Debug::Assert(
        ( this->Time > before->Time ) &&
        ( this->Time < after->Time ),
        "前後のポーズの時間関係は正しい"
    );

    double interval = ( after->Time - before->Time );                       // 時間の差[ sec ]
    double supplementaryValue = ( this->Time - before->Time ) / interval;   // 補正値


    for( int i = 0; i < m_angleOfJoints->Length; i++ )
    {
        // 前後のポーズ間の角度差に補正値をかけて、角速度を求める
        double angularVelocity
            = ( after->m_angleOfJoints[ i ] - before->m_angleOfJoints[ i ] ) * supplementaryValue;

        // ... 前のポーズの角度に 角速度を加算した値を設定する
        m_angleOfJoints[ i ] = before->m_angleOfJoints[ i ] + angularVelocity;
    }
}


/// 設定を書き込む
System::Xml::XmlElement^ Pose::SaveSettings( System::Xml::XmlDocument^ document )
{
    // ルートを生成する
    Xml::XmlElement^ root = document->CreateElement( "pose" );

    // ... 時間を属性として設定する
    root->SetAttribute( "time", m_time.ToString() );


    // 基準リンクの要素を生成する
    Xml::XmlElement^ standardLink = document->CreateElement( "standardLink" );

    // ... 基準リンクの名称を 属性として設定する
    standardLink->SetAttribute( "name", m_standardLink->Name );

    standardLink->AppendChild( m_positionOfStandardLink->ToXml( document ) );   // 基準リンクの位置
    standardLink->AppendChild( m_postureOfStandardLink->ToXml( document ) );    // 基準リンクの姿勢

    // ... 基準リンクの要素をルートに追加する
    root->AppendChild( standardLink );


    // 関節の要素を生成する
    Xml::XmlElement^ jointElement = document->CreateElement( "joint" );

    for( int index = 0; index < m_angleOfJoints->Length; index++ )
    {
        // 角度の要素を生成する
        Xml::XmlElement^ angleElement = document->CreateElement( "angle" );

        // ... インデックスを属性として設定する
        angleElement->SetAttribute( "index", index.ToString() );

        // ... テキストを追加する
        angleElement->InnerText = safe_cast< double >( m_angleOfJoints[ index ] ).ToString();

        /// @note
        /// 角度クラスの ToString()を使用すると精度が失われるため、
        /// doubleの ToString()を用いている。


        // ... 関節の要素に追加する
        jointElement->AppendChild( angleElement );
    }

    // ... 関節の要素をルートに追加する
    root->AppendChild( jointElement );

    return root;
}


/// 安定余裕を取得する
double Pose::StabilityMargin::get()
{
    // 安定余裕を求めて返す
    return m_supportPolygon->CalculateStabilityMargin( m_centerOfGravity );
}

Copyright © 2006-2024 BABA. All Rights Reserved.