// ######################################################################
double MotionPlanner2dModule::RotateToFaceTarget(Eigen::Translation3d const & translation)
{
double angle = atan2(translation.y(), translation.x());
itsAngularPidComponent->setObservedValue(-angle);
itsAngularPidComponent->setDesiredValue(0.0);
return itsAngularPidComponent->update();
}
YAML::Node SetPositionYaml( const Eigen::Translation3d& trans )
{
YAML::Node node;
node["x"] = trans.x();
node["y"] = trans.y();
node["z"] = trans.z();
return node;
}
double MotionPlanner2dModule::GetAngle(Eigen::Translation3d const & translation)
{
return atan2(translation.y(), translation.x());
}