#include "Behaviors/StateMachine.h"
$nodeclass LinkArm : StateNode {
	$nodeclass LinkArmBehavior : StateNode : doStart{
		fmat::Transform leftArmFinger = kine->linkToBase(LeftFingerOffset);
		fmat::Column<3> leftArmFingerPos = leftArmFinger.translation();
	
		fmat::Transform leftArmFingerFrame = kine->linkToBase(LeftFingerFrameOffset);
		fmat::Column<3> leftArmFingerFramePos = leftArmFingerFrame.translation();

		float dist = (leftArmFingerPos-leftArmFingerFramePos).norm();

		fmat::Transform linktolink = kine->linkToLink(LeftFingerOffset, LeftFingerFrameOffset);

		fmat::Transform leftToRight = kine->linkToLink(LeftFingerOffset, RightFingerOffset);
	
		
		cout << "Position of linkToLink  is " << linktolink << endl;
		cout << "Position of leftToRight  is " << leftToRight << endl;
		cout << "Position of Left is " << leftArmFingerPos << endl;
		cout << "Position of LeftFingerFrame is " << leftArmFingerFramePos << endl;
		cout << "Distance between LeftFingerFrame and LeftFinger is " << dist << endl;
	}
	$setupmachine{
		LinkArmBehavior
	}
}
REGISTER_BEHAVIOR(LinkArm);