#include "OMS_SPFA_Robot.h"


// calculate inverse kinematics from pose R
// and move robot to that new pose
// In the future, we should also check for singularities
void OMS_SPFA_Robot::
moveTo(	const TransformationMatrix& R) 
		throw(RobotControllerException){

	c.moveTo( m.inv(pose = R) );					

}

// ??
void OMS_SPFA_Robot::
goHome()
		throw(RobotControllerException)
{
	c.goHome();	
}