ForwardKinematics.cpp

#include "StdAfx.h"
#include "ForwardKinematics.h"


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

#include "ConnectionRelation.h"


using namespace Core;
using namespace Robotics;

using namespace System;
using namespace System::Diagnostics;


/// リンクの位置と姿勢を求める
void ForwardKinematics::CalculatePositionAndPostureOfLinks( Links^ links, Angles^ angles )
{
    for( int i = 0; i < links->Length; i++ )
    {
        links[ i ]->Joint->Angle = angles[ i ]; // 関節の角度
    }

    // 基準リンクから リンクの位置と姿勢を求める
    CalculatePositionAndPostureOfLinks( Link::Standard );
}

/// リンクの位置と姿勢を求める
void ForwardKinematics::CalculatePositionAndPostureOfLinks( Link^ targetLink )
{
    ForwardKinematics^ forwardKinematics = gcnew ForwardKinematics();


    // 変更されないことを確認するために、基準リンクの位置と姿勢を保存する
    PositionVector^ positionOfStandardLink = gcnew PositionVector( Link::Standard->Joint->AbsolutePosition );
    RotationMatrix^ postureOfStandardLink = gcnew RotationMatrix( Link::Standard->Posture );


    // リンクの経路を格納するリストを生成する
    forwardKinematics->m_routeOfLinks = gcnew LinkList();

    // ... 基準リンクから 胴体へ至る経路を取得する
    Link::Standard->AddOwnToRouteToBody( forwardKinematics->m_routeOfLinks );

    if( forwardKinematics->m_routeOfLinks->Contains( targetLink ) )
    {
        // 指定されたリンクがそこに含まれるならば、それから胴体までを求める
        forwardKinematics->CalculateToBody( targetLink, targetLink->Connection->Lower );

        // ... 胴体から末端までを求める
        forwardKinematics->CalculateToEnd( Link::Body );


        /// @todo 指定されたリンクを含む経路が 再び計算されないようにする。
    }
    else
    {
        // 指定されたリンクから 末端までを求める
        forwardKinematics->CalculateToEnd( targetLink );
    }


    Debug::Assert( positionOfStandardLink == Link::Standard->Joint->AbsolutePosition, "基準リンクの関節の位置は変更されない" );
    Debug::Assert( postureOfStandardLink == Link::Standard->Posture, "基準リンクの姿勢は変更されない" );
}


/// 末端までを求める
void ForwardKinematics::CalculateToEnd( Link^ target )
{
    // 存在しないリンクならば、何もしない
    if( target == nullptr ) return;


    if( target->IsBody() || m_routeOfLinks->Contains( target ) )
    {
        // 胴体または 経路に含まれるリンクならば、演算を行う必要はない

        /// @note
        /// 接続元を基準に演算するため、胴体を対象にはできない。
        /// また、経路に含まれるリンクは、胴体までを求める処理において、
        /// 先に計算済みであるため、くり返し演算しないように除外している。
    }
    else
    {
        Link^ upperLink = target->Connection->Upper;    // 接続元へのハンドル

        Joint^ upperJoint = upperLink->Joint;   // 接続元の関節
        Joint^ targetJoint = target->Joint;     // 対象とする関節


        // 角速度ベクトルによって生じる回転行列を求める
        RotationMatrix^ rotationMatrixByAngularVelocity = RotationMatrix::MatrixExponential(
            targetJoint->AxialDirection,  // 軸方向
            targetJoint->Angle            // 回転角
        );

        // ... 姿勢を設定する (接続元の回転行列と、角速度ベクトルによって生じる回転行列との積)
        target->Posture
            = upperLink->Posture * rotationMatrixByAngularVelocity;

        // ... 関節の絶対位置を設定する (接続元の関節の絶対位置に、接続元の関節との相対位置を加算)
        targetJoint->AbsolutePosition
            = upperJoint->AbsolutePosition + ( upperLink->Posture * targetJoint->RelativePosition );
    }


    // 対等な接続 を呼び出す
    CalculateToEnd( target->Connection->Equal );

    // 接続先 を呼び出す
    CalculateToEnd( target->Connection->Lower );


    // [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P50
}


/// 胴体までを求める
void ForwardKinematics::CalculateToBody( Link^ target, Link^ lowerLink )
{
    // 存在しないリンクならば、何もしない
    if( target == nullptr ) return;


    /// @note
    /// このメソッドでは、接続先から対象とするリンクの演算を行っている。
    /// その為には、そのリンクを決定する接続先が特定されなければならない。
    ///
    /// ところで、対等な接続を持つリンクは 接続先を複数持っていることに等しい。
    /// その場合には、接続元から接続先を一意に特定することができない。
    /// 故に、引数で接続先を受け取るようにしている。


    if( lowerLink == nullptr || target->IsStandard() )
    {
        // 接続先が存在しない末端のリンク または基準リンクならば、演算を行う必要はない
    }
    else
    {
        Debug::Assert( lowerLink->Connection->Upper == target, "接続先の接続元は 対象とするリンクである" );


        Joint^ lowerJoint = lowerLink->Joint;   // 接続先の関節
        Joint^ targetJoint = target->Joint;     // 対象とする関節


        // 接続先の 角速度ベクトルによって生じる回転行列を求める
        RotationMatrix^ rotationMatrixByAngularVelocity = RotationMatrix::MatrixExponential(
            lowerJoint->AxialDirection,  // 軸方向
            lowerJoint->Angle            // 回転角
        );

        // ... 姿勢を設定する (接続先の回転行列と、接続先の 角速度ベクトルによって生じる回転行列の転置行列(逆行列)との積)
        target->Posture
            = lowerLink->Posture * rotationMatrixByAngularVelocity->TransposedMatrix();

        // ... 関節の絶対位置を設定する (接続先の絶対位置から、自分の関節との相対位置を減算)
        targetJoint->AbsolutePosition
            = lowerJoint->AbsolutePosition - ( target->Posture * lowerJoint->RelativePosition );
    }


    // 接続元 を呼び出す
    CalculateToBody( target->Connection->Upper, target );
}

Copyright © 2006-2024 BABA. All Rights Reserved.