#include "Shared/RobotInfo.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"

$nodeclass Trajectory {
    $nodeclass MoveGripper1 : PostureNode {
	virtual void doStart() {
	    unsigned int link = LeftFingerFrameOffset;
	    fmat::Column<3> target = fmat::pack(200,0,200);
	    getMC()->solveLinkPosition(target, link, fmat::pack(0,0,0));
	}
    }

    $nodeclass MoveGripper2 : PostureNode {
	virtual void doStart() {
	    unsigned int link = LeftFingerFrameOffset;
	    fmat::Column<3> target = fmat::pack(400,0,200);
	    getMC()->solveLinkPosition(target, link, fmat::pack(0,0,0));
	}
    }

    $setupmachine{
	startnode: MoveGripper1 =C=> StateNode =T(2000)=> MoveGripper2
    }
}

REGISTER_BEHAVIOR(Trajectory);