InverseKinematics.h

#pragma once


namespace Core
{
    ref class Link;

    typedef array< Link^ > Links;
    typedef array< Robotics::Angle > Angles;

    typedef System::Collections::Generic::List< Link^ > LinkList;


    /// 逆運動学
    public ref class InverseKinematics
    {
    // Construction --------------------------------------------------------
    public:
        InverseKinematics( Links^ links );


    // Operation -----------------------------------------------------------
    public:
        bool CalculateAngleOfAllJoints( Link^ operationTarget, Robotics::PositionVector^ targetPosition, Robotics::RotationMatrix^ targetPosture );

    private:
        bool NewtonRaphson();

        Robotics::Matrix^ CalculateJacobian();
        Robotics::Vector^ CalculateErrorOfPositionAndPosture();

        void CreateRouteOfLinks();


    // Attribute ===========================================================
    private:
        Link^ m_operationTarget;            ///< 演算対象のリンク

        Robotics::PositionVector^ m_targetPosition;   ///< 目標とする位置[ m ]
        Robotics::RotationMatrix^ m_targetPosture;    ///< 目標とする姿勢


        double m_quantityToRevise;          ///< 修正量係数
        LinkList^ m_routeOfLinks;           ///< リンクの経路


        Links^ m_links;                 ///< リンク
        Angles^ m_anglesBeforeChange;   ///< 変更前の角度


    // static
        literal int RepeatLimit = 50;       ///< 演算をくり返す上限

    };
}