#include "Behaviors/StateMachine.h"

$nodeclass Test1 : VisualRoutinesStateNode : doStart {
	cout << "NumArmJoints = " << NumArmJoints << endl;
	KinematicJoint *gripperFrameKJ = kine->getKinematicJoint(GripperFrameOffset)->cloneBranch();
	KinematicJoint* KJjoints[NumArmJoints];
	for (unsigned int i=0; i<NumArmJoints; i++)
		KJjoints[i] = NULL;
	gripperFrameKJ->getRoot()->buildChildMap(KJjoints, ArmOffset, NumArmJoints);
	for (unsigned int i=0; i<NumArmJoints; i++)
		cout << i << ": " << (void*)KJjoints[i] << endl;
}

REGISTER_BEHAVIOR(Test1);