#include "Behaviors/StateMachine.h"
 
$nodeclass PostureTest : StateNode : doStart {

    PostureEngine pe;

    pe.takeSnapshot();

    cout << pe.getPosition(GripperFrameOffset) << endl;
    Point target = Point(327.443, -70.35, 70.3435, egocentric);
    pe.solveLinkPosition(target.getCoords(), GripperFrameOffset, fmat::ZERO3);
    cout << pe.getPosition(GripperFrameOffset) << endl;

}

REGISTER_BEHAVIOR(PostureTest);