public StateHandle (QuadrotorInterface qInterface, Pose pose, Twist twist)
{
poseHandle = new PoseHandle ( qInterface, pose );
twistHandle = new TwistHandle ( qInterface, twist );
}
void registerTwist (Twist twist)
{
twist_ = new TwistHandle ( this, twist );
}