From 2735417ab333d32b0e20a697da86144dbf92b7e0 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 17:42:42 -0700 Subject: [PATCH] Move vector normalization code to own func --- src/control/PlanarArmController.h | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 012204c3b..9f4a7dd18 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -50,6 +50,18 @@ template class PlanarArmController { setpoint = newSetPoint; } + /** + * @brief Normalize the input vector to have a set radius, + * while maintaining the same direction it did before. + * + * @param input The input vector to normalize. + * @param radius The radius to normalize the vector to. + */ + void normalizeVectorWithinRadius(Eigen::Vector2d input, double radius) { + input.normalize(); + input *= radius; + } + /** * @brief Gets the current end effector setpoint / target position. * @@ -71,10 +83,10 @@ template class PlanarArmController { // bounds check (new pos + vel vector <= sum of joint lengths) double radius = kin.getSegLens().sum() * safetyFactor; if (pos.norm() > radius) { - // new position is outside of bounds - // shrink velocity vector until it is within radius. - pos.normalize(); - pos *= radius; + // TODO: will need to eventually shrink velocity vector until it is within radius + // instead of just normalizing it. + + normalizeVectorWithinRadius(pos, radius); } return pos; }