#include "Behaviors/StateMachine.h"

$nodeclass TwoReach : StateNode {

	$nodeclass StraightReach : DynamicMotionSequenceNode : doStart {
		//Instantiate posture engine
		PostureEngine* myEngine = new PostureEngine();

		//Get the start position
		fmat::Transform reach1 = kine->linkToBase(GripperFrameOffset);
		fmat::Column<3> reach1Trans = reach1.translation();

		//Solve link position in sequences 
		for (int i = 1; i <= 10 ; i++) {
			fmat::Column<3> offset = fmat::pack((30*i),0,0);
			myEngine->solveLinkPosition(reach1Trans, GripperFrameOffset, offset);
			getMC()->setPose(*myEngine);
		}
	}

	$setupmachine {
		startnode: PostureNode("reach1.pos") =T(2000)=> StraightReach
	}

}
REGISTER_BEHAVIOR(TwoReach);