#include "Behaviors/StateMachine.h"

$nodeclass LinkOrient {

	$nodeclass MoveArm : PostureNode : doStart {
		fmat::Column<3> target = fmat::pack(250,0,16);
		fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(-M_PI/2);
		fmat::Column<3> posOffset = fmat::pack(0,0,0);
		fmat::Quaternion oriOffset = fmat::Quaternion::identity();
		cout << "ArmElbow max speed=" << getMC()->getMaxSpeed(ArmElbowOffset) << endl;
		bool success = getMC()->solveLink(target, oriTgt, GripperFrameOffset, posOffset, oriOffset);
		cout << "success=" << success << endl;
	}

  $nodeclass ReportPosition : StateNode : doStart {
		fmat::Column<3> pos = kine->linkToBase(GripperFrameOffset).translation();
		cout << "Position = " << pos << endl;
  }

  $setupmachine{
    MoveArm =C=> StateNode =T(200)=> // add a little time for the arm to settle
     ReportPosition
  }

}

REGISTER_BEHAVIOR(LinkOrient);