Pose.h

#pragma once


namespace Core
{
    ref class Model;
    ref class Link;

    ref class SupportPolygon;

    typedef array< Robotics::PositionVector^ > Positions;
    typedef array< Robotics::RotationMatrix^ > Postures;
    typedef array< Robotics::Angle > Angles;


    /// ポーズ
    public ref class Pose
    {
    // Construction --------------------------------------------------------
    public:
        Pose( double time, Model^ model );
        Pose( double time, Pose^ original );
        Pose( System::Xml::XmlElement^ element, Model^ model );


    // Operation -----------------------------------------------------------
    public:
        void SetModelState( Model^ model );

        void PosedModel( Model^ model );
        void ComplementAngleOfJoints( Pose^ before, Pose^ after );

        System::Xml::XmlElement^ SaveSettings( System::Xml::XmlDocument^ document );

    private:
        void CreateAttribute( int numberOfLinks );


    // Attribute ===========================================================
    private:
        double m_time;          ///< 時間[ sec ]

        Link^ m_standardLink;                               ///< 基準リンク
        Robotics::PositionVector^ m_positionOfStandardLink; ///< 基準リンクの位置[ m ] (ワールド座標系)
        Robotics::RotationMatrix^ m_postureOfStandardLink;  ///< 基準リンクの姿勢 (ワールド座標系)

        Positions^ m_positionOfLinks;  ///< リンクの位置[ m ] (ワールド座標系
        Postures^ m_postureOfLinks;    ///< リンクの姿勢 (ワールド座標系)

        Angles^ m_angleOfJoints;            ///< 関節の角度[ rad ]
        array< double >^ m_torqueOfJoints;  ///< 関節のトルク[ Nm ]


        double m_electricPower;                         ///< 電力[ W ]
        Robotics::PositionVector^ m_centerOfGravity;    ///< 重心[ m ] (ワールド座標系)
        SupportPolygon^ m_supportPolygon;               ///< 支持多角形[ m ] (ワールド座標系)


        /// @note
        /// リンクの絶対位置は演算によって求まるため、常に誤差を含んでいる。
        /// よって関節の位置で保存するほうが望ましいと思われる。
        /// しかし、基準リンクは地面に接していることを条件としているため
        /// 関節の位置では その条件を満足しているか判断ができない。
        /// 故に、現時点ではリンクの位置で扱っている。


    // Property ------------------------------------------------------------
    public:
        /// 時間[ sec ]
        property double Time
        {
            double get()
            {
                return m_time;
            }
        }


        /// 基準リンク
        property Link^ StandardLink
        {
            void set( Link^ value );
        }

        /// リンクの位置[ m ]
        property Positions^ PositionOfLinks
        {
            Positions^ get()
            {
                return m_positionOfLinks;
            }
        }

        /// 関節の角度[ rad ]
        property Angles^ AngleOfJoints
        {
            Angles^ get()
            {
                return m_angleOfJoints;
            }
        }

        /// 関節のトルク[ Nm ]
        property array< double >^ TorqueOfJoints
        {
            array< double >^ get()
            {
                return m_torqueOfJoints;
            }
        }

        /// 電力[ W ]
        property double ElectricPower
        {
            double get()
            {
                return m_electricPower;
            }
        }


        /// 重心[ m ]
        property Robotics::PositionVector^ CenterOfGravity
        {
            Robotics::PositionVector^ get()
            {
                return m_centerOfGravity;
            }
        }

        /// 支持多角形[ m ]
        property SupportPolygon^ SupportPolygon
        {
            Core::SupportPolygon^ get()
            {
                return m_supportPolygon;
            }
        }


        /// 安定余裕[ m ]
        property double StabilityMargin
        {
            double get();
        }
    };
}