// ######################################################################
double MotionPlanner2dModule::RotateToFaceTarget(Eigen::Translation3d const & translation)
{
  double angle = atan2(translation.y(), translation.x());

  itsAngularPidComponent->setObservedValue(-angle);
  itsAngularPidComponent->setDesiredValue(0.0);
  return itsAngularPidComponent->update();
}
Ejemplo n.º 2
0
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());
}