From 85ea0df0892f274b94134dd8c2cb8eca1bbb786f Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 18:17:02 -0700 Subject: [PATCH 01/33] Added safetyFactor check --- src/Globals.cpp | 2 +- src/control/PlanarArmController.h | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/Globals.cpp b/src/Globals.cpp index 81e86aa62..de639348f 100644 --- a/src/Globals.cpp +++ b/src/Globals.cpp @@ -41,6 +41,6 @@ robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperip const kinematics::PlanarArmKinematics planarArmKinematics(getSegLens(), getJointLimits(true), getJointLimits(false), IK_SOLVER_THRESH, IK_SOLVER_MAX_ITER); -control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics); +control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics, 0.95); std::atomic armIKEnabled = false; } // namespace Globals diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index fba5fe7a6..6cac34944 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -29,10 +29,12 @@ template class PlanarArmController { * @param currJointPos The current joint positions of the arm. * @param kin_obj PlanarArmKinematics object for the arm (should have the same number of * arm joints). + * @param safetyFactor the percentage factor to scale maximum arm extension radius by to prevent singularity lock. */ PlanarArmController(const navtypes::Vectord& currJointPos, - kinematics::PlanarArmKinematics kin_obj) - : kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}) {} + kinematics::PlanarArmKinematics kin_obj, + const double safetyFactor) + : kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}), safetyFactor(safetyFactor) {} /** * @brief Sets the end effector setpoint / target position. @@ -65,7 +67,7 @@ template class PlanarArmController { // bounds check (new pos + vel vector <= sum of joint lengths) double radius = kin.getSegLens().sum(); - if (pos.norm() > radius) { + if (pos.norm() > safetyFactor * radius) { // new position is outside of bounds // TODO: will need to eventually shrink velocity vector until it is within radius // instead of just normalizing it @@ -135,5 +137,6 @@ template class PlanarArmController { Eigen::Vector2d setpoint; Eigen::Vector2d velocity; std::optional velTimestamp; + const double safetyFactor; }; } // namespace control \ No newline at end of file From 7630036179cdf956105d37834c16ba37ec121085 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 18:22:42 -0700 Subject: [PATCH 02/33] Move safety factor constant into Constants --- src/Constants.h | 15 ++++----------- src/Globals.cpp | 2 +- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/Constants.h b/src/Constants.h index 27aae6327..d7f2c4e00 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -56,17 +56,6 @@ constexpr double MAX_WHEEL_VEL = WHEEL_RADIUS * MAX_DRIVE_PWM / PWM_PER_RAD_PER_ const double MAX_DTHETA = kinematics::DiffDriveKinematics(EFF_WHEEL_BASE) .wheelVelToRobotVel(-MAX_WHEEL_VEL, MAX_WHEEL_VEL)(2); -// Joint limits -// TODO: make sure these are still accurate with the new arm. -constexpr double ARM_BASE_MIN = -M_PI / 2; -constexpr double ARM_BASE_MAX = M_PI / 2; -// constexpr double SHOULDER_MIN = M_PI / 2; // TODO mechanical problem with the moon gear. -// Use this value during actual rover operation. -constexpr double SHOULDER_MIN = 0.0; -constexpr double SHOULDER_MAX = M_PI * 5. / 6.; // Can't quite lie flat -constexpr double ELBOW_MIN = 0.0; -constexpr double ELBOW_MAX = M_PI * 29. / 30.; // I think this should prevent self-collisions - // TODO: We need to recalibrate the camera, since we replaced it with a different one. // TODO: rename cameras (in MC as well) as appropriate constexpr const char* AR_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml"; @@ -146,6 +135,10 @@ constexpr frozen::unordered_map planarArmKinematics(getSegLens(), getJointLimits(true), getJointLimits(false), IK_SOLVER_THRESH, IK_SOLVER_MAX_ITER); -control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics, 0.95); +control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics, Constants::arm::SAFETY_FACTOR); std::atomic armIKEnabled = false; } // namespace Globals From b5ff95b654b4c0e5ba442ee24cdfd3c3b67bf4cf Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 18:28:39 -0700 Subject: [PATCH 03/33] Add safetyFactor range limit assertion --- src/control/PlanarArmController.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 6cac34944..f0a0cd372 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -34,7 +34,9 @@ template class PlanarArmController { PlanarArmController(const navtypes::Vectord& currJointPos, kinematics::PlanarArmKinematics kin_obj, const double safetyFactor) - : kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}), safetyFactor(safetyFactor) {} + : kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}), safetyFactor(safetyFactor) { + assert(safetyFactor > 0.0 && safetyFactor < 1.0); + } /** * @brief Sets the end effector setpoint / target position. From f03380457fb1dc204ec71a7cbad17801b8dd1d0a Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 18:28:58 -0700 Subject: [PATCH 04/33] Update planar arm tests to use safety factor --- tests/control/PlanarArmControllerTest.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 29a905c65..05ad7f823 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -1,4 +1,5 @@ #include "../../src/control/PlanarArmController.h" +#include "../../src/Constants.h" #include @@ -9,5 +10,10 @@ using namespace std::chrono_literals; TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") { navtypes::Vectord<2> vec({0, 0}); kinematics::PlanarArmKinematics<2> kin_obj(vec, vec, vec, 0.0, 0); - PlanarArmController<2> foo({0, 0}, kin_obj); + PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); +} + +TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { + PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); + } \ No newline at end of file From d7b14ce2290748d86ff653d4ee887beb9f7f65d7 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 18:43:06 -0700 Subject: [PATCH 05/33] Set setpoint in safety factor test --- tests/control/PlanarArmControllerTest.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 05ad7f823..b2ae6defa 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -14,6 +14,11 @@ TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") { } TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { + navtypes::Vectord<2> segLens({2.0, 3.0}); + navtypes::Vectord<2> minAngles({-M_PI, -M_PI}); + navtypes::Vectord<2> maxAngles({M_PI, M_PI}); + kinematics::PlanarArmKinematics<2> kin_obj(segLens, minAngles, maxAngles, 0.0, 0); + PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); - + foo.set_setpoint({5.0, 0.0}); } \ No newline at end of file From 5b6bf0b98a17033031bbed9dfc303b0c5baf8b67 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 19:04:57 -0700 Subject: [PATCH 06/33] Test overreaching arm is normalized --- tests/control/PlanarArmControllerTest.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index b2ae6defa..dd123eda3 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -14,11 +14,12 @@ TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") { } TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { - navtypes::Vectord<2> segLens({2.0, 3.0}); + navtypes::Vectord<2> segLens({4.0, 6.0}); navtypes::Vectord<2> minAngles({-M_PI, -M_PI}); navtypes::Vectord<2> maxAngles({M_PI, M_PI}); kinematics::PlanarArmKinematics<2> kin_obj(segLens, minAngles, maxAngles, 0.0, 0); PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); - foo.set_setpoint({5.0, 0.0}); + foo.set_setpoint({10.0, 0.0}); + REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 1.0); } \ No newline at end of file From 26468626132434fbd3b4f842c7d731669d6b92bd Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 19:13:52 -0700 Subject: [PATCH 07/33] Bound resolved joint pos within safety factor --- src/control/PlanarArmController.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index f0a0cd372..0e296b76f 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -68,12 +68,13 @@ template class PlanarArmController { } // bounds check (new pos + vel vector <= sum of joint lengths) - double radius = kin.getSegLens().sum(); - if (pos.norm() > safetyFactor * radius) { + double radius = kin.getSegLens().sum() * safetyFactor; + if (pos.norm() > radius) { // new position is outside of bounds // TODO: will need to eventually shrink velocity vector until it is within radius // instead of just normalizing it pos.normalize(); + pos *= radius; } return pos; } From fec0cb793e2b48abbc8cac2087098b7ce55dee3a Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 19:14:03 -0700 Subject: [PATCH 08/33] Added within factor test --- tests/control/PlanarArmControllerTest.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index dd123eda3..9a96a988c 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -21,5 +21,8 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); foo.set_setpoint({10.0, 0.0}); - REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 1.0); + REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5); + + foo.set_setpoint({9.0, 0.0}); + REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5); } \ No newline at end of file From 93d0aabb2e5c6e26b46bee8cb708e2613239c303 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 28 Sep 2023 19:23:58 -0700 Subject: [PATCH 09/33] Try shorter extension --- tests/control/PlanarArmControllerTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 9a96a988c..356ad4a62 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -23,6 +23,6 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { foo.set_setpoint({10.0, 0.0}); REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5); - foo.set_setpoint({9.0, 0.0}); - REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5); + foo.set_setpoint({2.0, 3.0}); + REQUIRE(fabs(foo.get_setpoint(robot::types::dataclock::now()).norm() - 2.1166861277) < 1e-6); } \ No newline at end of file From 8e6bc2523735a43822b30bcf372a1eedeb2b03aa Mon Sep 17 00:00:00 2001 From: DrDab Date: Sat, 30 Sep 2023 15:42:17 -0700 Subject: [PATCH 10/33] Improved formatting --- src/Constants.h | 2 +- src/control/PlanarArmController.h | 11 ++++++----- tests/control/PlanarArmControllerTest.cpp | 4 +++- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/Constants.h b/src/Constants.h index d7f2c4e00..17e82fb4d 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -135,7 +135,7 @@ constexpr frozen::unordered_map class PlanarArmController { * @param currJointPos The current joint positions of the arm. * @param kin_obj PlanarArmKinematics object for the arm (should have the same number of * arm joints). - * @param safetyFactor the percentage factor to scale maximum arm extension radius by to prevent singularity lock. + * @param safetyFactor the percentage factor to scale maximum arm extension radius by to + * prevent singularity lock. */ PlanarArmController(const navtypes::Vectord& currJointPos, - kinematics::PlanarArmKinematics kin_obj, - const double safetyFactor) - : kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}), safetyFactor(safetyFactor) { - assert(safetyFactor > 0.0 && safetyFactor < 1.0); + kinematics::PlanarArmKinematics kin_obj, const double safetyFactor) + : kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}), + safetyFactor(safetyFactor) { + assert(safetyFactor > 0.0 && safetyFactor < 1.0); } /** diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 356ad4a62..33c9951ed 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -1,4 +1,5 @@ #include "../../src/control/PlanarArmController.h" + #include "../../src/Constants.h" #include @@ -24,5 +25,6 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5); foo.set_setpoint({2.0, 3.0}); - REQUIRE(fabs(foo.get_setpoint(robot::types::dataclock::now()).norm() - 2.1166861277) < 1e-6); + REQUIRE(fabs(foo.get_setpoint(robot::types::dataclock::now()).norm() - 2.1166861277) < + 1e-6); } \ No newline at end of file From dffe2dc144501f1ef2edebc5be603aced3f411fb Mon Sep 17 00:00:00 2001 From: DrDab Date: Sat, 30 Sep 2023 15:44:05 -0700 Subject: [PATCH 11/33] Remove now-obsolete todo --- src/control/PlanarArmController.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 966d6a500..012204c3b 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -72,8 +72,7 @@ template class PlanarArmController { double radius = kin.getSegLens().sum() * safetyFactor; if (pos.norm() > radius) { // new position is outside of bounds - // TODO: will need to eventually shrink velocity vector until it is within radius - // instead of just normalizing it + // shrink velocity vector until it is within radius. pos.normalize(); pos *= radius; } From 10044f6adadda1d5c3c1f82b1ac0a93a57513ba6 Mon Sep 17 00:00:00 2001 From: DrDab Date: Sat, 30 Sep 2023 15:59:03 -0700 Subject: [PATCH 12/33] More formatting fixes --- src/Globals.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Globals.cpp b/src/Globals.cpp index a1e20fe22..6018367f0 100644 --- a/src/Globals.cpp +++ b/src/Globals.cpp @@ -41,6 +41,7 @@ robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperip const kinematics::PlanarArmKinematics planarArmKinematics(getSegLens(), getJointLimits(true), getJointLimits(false), IK_SOLVER_THRESH, IK_SOLVER_MAX_ITER); -control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics, Constants::arm::SAFETY_FACTOR); +control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics, + Constants::arm::SAFETY_FACTOR); std::atomic armIKEnabled = false; } // namespace Globals From 4174d9b14b74f81a453284d597f79bc49531bad8 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 17:37:02 -0700 Subject: [PATCH 13/33] Use std abs over fabs in test --- tests/control/PlanarArmControllerTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 33c9951ed..b71b940ba 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -25,6 +25,6 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5); foo.set_setpoint({2.0, 3.0}); - REQUIRE(fabs(foo.get_setpoint(robot::types::dataclock::now()).norm() - 2.1166861277) < + REQUIRE(std::abs(foo.get_setpoint(robot::types::dataclock::now()).norm() - 2.1166861277) < 1e-6); } \ No newline at end of file From 2735417ab333d32b0e20a697da86144dbf92b7e0 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 17:42:42 -0700 Subject: [PATCH 14/33] 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; } From edff9dded785ae9a51c1f473412ce59608fd9ac1 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 17:45:15 -0700 Subject: [PATCH 15/33] Add more logic to normalizeVectorWithinRadius --- src/control/PlanarArmController.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 9f4a7dd18..69703b842 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -52,14 +52,19 @@ template class PlanarArmController { /** * @brief Normalize the input vector to have a set radius, - * while maintaining the same direction it did before. - * + * while maintaining the same direction it did before if it exceeds that set radius. + * * @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; + if (input.norm() > radius) { + // TODO: will need to eventually shrink velocity vector until it is within radius + // instead of just normalizing it. + + input.normalize(); + input *= radius; + } } /** @@ -82,12 +87,7 @@ template class PlanarArmController { // bounds check (new pos + vel vector <= sum of joint lengths) double radius = kin.getSegLens().sum() * safetyFactor; - if (pos.norm() > radius) { - // TODO: will need to eventually shrink velocity vector until it is within radius - // instead of just normalizing it. - - normalizeVectorWithinRadius(pos, radius); - } + normalizeVectorWithinRadius(pos, radius); return pos; } From affcec073e2bb8b1b9b28d8c52373c1b5e861947 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 17:46:45 -0700 Subject: [PATCH 16/33] Fix return type --- src/control/PlanarArmController.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 69703b842..13fc7d1d7 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -57,7 +57,7 @@ template class PlanarArmController { * @param input The input vector to normalize. * @param radius The radius to normalize the vector to. */ - void normalizeVectorWithinRadius(Eigen::Vector2d input, double radius) { + Eigen::Vector2d normalizeVectorWithinRadius(Eigen::Vector2d input, double radius) { if (input.norm() > radius) { // TODO: will need to eventually shrink velocity vector until it is within radius // instead of just normalizing it. @@ -65,6 +65,8 @@ template class PlanarArmController { input.normalize(); input *= radius; } + + return input; } /** @@ -87,8 +89,7 @@ template class PlanarArmController { // bounds check (new pos + vel vector <= sum of joint lengths) double radius = kin.getSegLens().sum() * safetyFactor; - normalizeVectorWithinRadius(pos, radius); - return pos; + return normalizeVectorWithinRadius(pos, radius); } /** From 9d0cdfe058db4ee7e4b97b5a13865bdb9904cb3f Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 17:49:04 -0700 Subject: [PATCH 17/33] Add normalize functionality to set_setpoint --- src/control/PlanarArmController.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 13fc7d1d7..9ec6e6211 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -47,7 +47,8 @@ template class PlanarArmController { void set_setpoint(const navtypes::Vectord& targetJointPos) { Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(targetJointPos); std::lock_guard lock(mutex); - setpoint = newSetPoint; + double radius = kin.getSegLens().sum() * safetyFactor; + setpoint = normalizeVectorWithinRadius(newSetPoint, radius); } /** From 62a352ae45ce473bba8aa9cf8da0af520e8b5c76 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 17:53:43 -0700 Subject: [PATCH 18/33] Actually compute unnormalized setpoint instead of magic num --- tests/control/PlanarArmControllerTest.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index b71b940ba..ef2474eb4 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -25,6 +25,9 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5); foo.set_setpoint({2.0, 3.0}); - REQUIRE(std::abs(foo.get_setpoint(robot::types::dataclock::now()).norm() - 2.1166861277) < + Eigen::Vector2d origSetPoint = kin_obj.jointPosToEEPos({2.0, 3.0}); + double expectedNorm = origSetPoint.norm(); + + REQUIRE(std::abs(foo.get_setpoint(robot::types::dataclock::now()).norm() - expectedNorm) < 1e-6); } \ No newline at end of file From 73e01bea22463818adc338182962777ca714eb6d Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 18:23:45 -0700 Subject: [PATCH 19/33] Make normalizeVector private --- src/control/PlanarArmController.h | 38 +++++++++++++++---------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 9ec6e6211..660144fdb 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -51,25 +51,6 @@ template class PlanarArmController { setpoint = normalizeVectorWithinRadius(newSetPoint, radius); } - /** - * @brief Normalize the input vector to have a set radius, - * while maintaining the same direction it did before if it exceeds that set radius. - * - * @param input The input vector to normalize. - * @param radius The radius to normalize the vector to. - */ - Eigen::Vector2d normalizeVectorWithinRadius(Eigen::Vector2d input, double radius) { - if (input.norm() > radius) { - // TODO: will need to eventually shrink velocity vector until it is within radius - // instead of just normalizing it. - - input.normalize(); - input *= radius; - } - - return input; - } - /** * @brief Gets the current end effector setpoint / target position. * @@ -155,5 +136,24 @@ template class PlanarArmController { Eigen::Vector2d velocity; std::optional velTimestamp; const double safetyFactor; + + /** + * @brief Normalize the input vector to have a set radius, + * while maintaining the same direction it did before if it exceeds that set radius. + * + * @param input The input vector to normalize. + * @param radius The radius to normalize the vector to. + */ + Eigen::Vector2d normalizeVectorWithinRadius(Eigen::Vector2d input, double radius) { + if (input.norm() > radius) { + // TODO: will need to eventually shrink velocity vector until it is within radius + // instead of just normalizing it. + + input.normalize(); + input *= radius; + } + + return input; + } }; } // namespace control \ No newline at end of file From cc7c16bfac3160aa15ccf273d48da8868bb2811e Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 18:30:11 -0700 Subject: [PATCH 20/33] Rename input vector to better reflect end effector --- src/control/PlanarArmController.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 660144fdb..2daa65b04 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -138,22 +138,22 @@ template class PlanarArmController { const double safetyFactor; /** - * @brief Normalize the input vector to have a set radius, + * @brief Normalize the input vector (end-effector position) to have a set radius, * while maintaining the same direction it did before if it exceeds that set radius. * - * @param input The input vector to normalize. - * @param radius The radius to normalize the vector to. + * @param eePos The end-effector position to normalize. + * @param radius The radius to normalize the end-effector to. */ - Eigen::Vector2d normalizeVectorWithinRadius(Eigen::Vector2d input, double radius) { - if (input.norm() > radius) { + Eigen::Vector2d normalizeVectorWithinRadius(Eigen::Vector2d eePos, double radius) { + if (eePos.norm() > radius) { // TODO: will need to eventually shrink velocity vector until it is within radius // instead of just normalizing it. - input.normalize(); - input *= radius; + eePos.normalize(); + eePos *= radius; } - return input; + return eePos; } }; } // namespace control \ No newline at end of file From 0e03abe64720945d416c581ae3755aef714e59b3 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 18:31:44 -0700 Subject: [PATCH 21/33] Make normalize func name more descriptive --- src/control/PlanarArmController.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 2daa65b04..e17202ee0 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -48,7 +48,7 @@ template class PlanarArmController { Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(targetJointPos); std::lock_guard lock(mutex); double radius = kin.getSegLens().sum() * safetyFactor; - setpoint = normalizeVectorWithinRadius(newSetPoint, radius); + setpoint = normalizeEEWithinRadius(newSetPoint, radius); } /** @@ -71,7 +71,7 @@ template class PlanarArmController { // bounds check (new pos + vel vector <= sum of joint lengths) double radius = kin.getSegLens().sum() * safetyFactor; - return normalizeVectorWithinRadius(pos, radius); + return normalizeEEWithinRadius(pos, radius); } /** @@ -144,7 +144,7 @@ template class PlanarArmController { * @param eePos The end-effector position to normalize. * @param radius The radius to normalize the end-effector to. */ - Eigen::Vector2d normalizeVectorWithinRadius(Eigen::Vector2d eePos, double radius) { + Eigen::Vector2d normalizeEEWithinRadius(Eigen::Vector2d eePos, double radius) { if (eePos.norm() > radius) { // TODO: will need to eventually shrink velocity vector until it is within radius // instead of just normalizing it. From 0cfc214a1874499ad4c6044f3774323cb595ea80 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 2 Oct 2023 18:40:52 -0700 Subject: [PATCH 22/33] Add actual vector comparison --- tests/control/PlanarArmControllerTest.cpp | 31 +++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index ef2474eb4..a0c3e1591 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -2,12 +2,40 @@ #include "../../src/Constants.h" +#include + #include using namespace Catch::literals; using namespace control; using namespace std::chrono_literals; +constexpr double PI = M_PI; + +std::string toString(const Eigen::Vector2d& pose) { + std::stringstream ss; + ss << "(" << pose(0) << ", " << pose(1) << ")"; + return ss.str(); +} + +void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double dist = 1e-5, + double angle = 1e-5) { + Eigen::Vector2d diff = p1 - p2; + bool distEqual = diff.norm() <= dist; + double thetaDiff = std::fmod(abs(diff(1)), 2 * PI); + // change domain from [0, 2pi) to (-pi, pi] + if (thetaDiff > PI) { + thetaDiff -= 2 * PI; + } + bool angleEqual = abs(thetaDiff) <= angle; + if (distEqual && angleEqual) { + SUCCEED(); + } else { + std::cout << "Expected: " << toString(p1) << ", Actual: " << toString(p2) << std::endl; + FAIL(); + } +} + TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") { navtypes::Vectord<2> vec({0, 0}); kinematics::PlanarArmKinematics<2> kin_obj(vec, vec, vec, 0.0, 0); @@ -28,6 +56,5 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { Eigen::Vector2d origSetPoint = kin_obj.jointPosToEEPos({2.0, 3.0}); double expectedNorm = origSetPoint.norm(); - REQUIRE(std::abs(foo.get_setpoint(robot::types::dataclock::now()).norm() - expectedNorm) < - 1e-6); + assertApprox(origSetPoint, foo.get_setpoint(robot::types::dataclock::now())); } \ No newline at end of file From 7edb33bba71b536fec79a49a4a069fce4d5e3aa1 Mon Sep 17 00:00:00 2001 From: DrDab Date: Sat, 7 Oct 2023 09:32:34 -0700 Subject: [PATCH 23/33] Refactored redundant code --- src/control/PlanarArmController.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index e17202ee0..b11b1c57a 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -47,8 +47,7 @@ template class PlanarArmController { void set_setpoint(const navtypes::Vectord& targetJointPos) { Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(targetJointPos); std::lock_guard lock(mutex); - double radius = kin.getSegLens().sum() * safetyFactor; - setpoint = normalizeEEWithinRadius(newSetPoint, radius); + setpoint = normalizeEEWithinRadius(newSetPoint); } /** @@ -70,8 +69,7 @@ template class PlanarArmController { } // bounds check (new pos + vel vector <= sum of joint lengths) - double radius = kin.getSegLens().sum() * safetyFactor; - return normalizeEEWithinRadius(pos, radius); + return normalizeEEWithinRadius(pos); } /** @@ -142,9 +140,10 @@ template class PlanarArmController { * while maintaining the same direction it did before if it exceeds that set radius. * * @param eePos The end-effector position to normalize. - * @param radius The radius to normalize the end-effector to. */ - Eigen::Vector2d normalizeEEWithinRadius(Eigen::Vector2d eePos, double radius) { + Eigen::Vector2d normalizeEEWithinRadius(Eigen::Vector2d eePos) { + double radius = kin.getSegLens().sum() * safetyFactor; + if (eePos.norm() > radius) { // TODO: will need to eventually shrink velocity vector until it is within radius // instead of just normalizing it. From fbca1801a669808bd4a5a34bc750dd9f6d4579f2 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 12 Oct 2023 18:35:33 -0700 Subject: [PATCH 24/33] Remove unnecessary checks --- tests/control/PlanarArmControllerTest.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index a0c3e1591..419532b44 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -22,18 +22,14 @@ void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double d double angle = 1e-5) { Eigen::Vector2d diff = p1 - p2; bool distEqual = diff.norm() <= dist; - double thetaDiff = std::fmod(abs(diff(1)), 2 * PI); - // change domain from [0, 2pi) to (-pi, pi] - if (thetaDiff > PI) { - thetaDiff -= 2 * PI; - } - bool angleEqual = abs(thetaDiff) <= angle; - if (distEqual && angleEqual) { - SUCCEED(); - } else { - std::cout << "Expected: " << toString(p1) << ", Actual: " << toString(p2) << std::endl; - FAIL(); + + if (!distEqual) { + std::stringstream ss; + ss << "Expected: " << toString(p1) << ", Actual: " << toString(p2); + INFO(ss.str()); } + + REQUIRE(distEqual); } TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") { From 2007f996846979b531394acf56404596432db146 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 12 Oct 2023 18:44:05 -0700 Subject: [PATCH 25/33] Minor cleanup --- tests/control/PlanarArmControllerTest.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 419532b44..c4769a09e 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -50,7 +50,6 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { foo.set_setpoint({2.0, 3.0}); Eigen::Vector2d origSetPoint = kin_obj.jointPosToEEPos({2.0, 3.0}); - double expectedNorm = origSetPoint.norm(); assertApprox(origSetPoint, foo.get_setpoint(robot::types::dataclock::now())); } \ No newline at end of file From dfa279fae0de675d991cb8d4cf2b7370c814c87b Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 16 Oct 2023 18:36:31 -0700 Subject: [PATCH 26/33] Fixed tests --- tests/control/PlanarArmControllerTest.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index c4769a09e..8214a39e8 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -39,17 +39,23 @@ TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") { } TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { - navtypes::Vectord<2> segLens({4.0, 6.0}); + // Set lengths and relative orientation bounds of robot joint segments. + navtypes::Vectord<2> segLens({6.0, 4.0}); navtypes::Vectord<2> minAngles({-M_PI, -M_PI}); navtypes::Vectord<2> maxAngles({M_PI, M_PI}); kinematics::PlanarArmKinematics<2> kin_obj(segLens, minAngles, maxAngles, 0.0, 0); + // Instantiate PlanarArmController. PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); - foo.set_setpoint({10.0, 0.0}); - REQUIRE(foo.get_setpoint(robot::types::dataclock::now()).norm() == 9.5); - foo.set_setpoint({2.0, 3.0}); - Eigen::Vector2d origSetPoint = kin_obj.jointPosToEEPos({2.0, 3.0}); + // Attempt to straighten out the EE all the way, violating max length safety factor. + foo.set_setpoint({0.0, 0.0}); + navtypes::Vectord<2> limitedSetpoint = foo.get_setpoint(robot::types::dataclock::now()); + assertApprox({9.5, 0.0}, limitedSetpoint); - assertApprox(origSetPoint, foo.get_setpoint(robot::types::dataclock::now())); + // Try setting the joints to be orthogonal but within max length, such that: + // - End effector position is (6,4), which implies that: + // - Max length = sqrt(6^2 + 4^2) = sqrt(36 + 16) < 7.22 < 9.5 (max length) + foo.set_setpoint({0.0, M_PI_2}); + assertApprox({6.0, 4.0}, foo.get_setpoint(robot::types::dataclock::now())); } \ No newline at end of file From 03cc6053758ea90793a1c93daaf7c8c5c77baf2b Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 16 Oct 2023 18:42:19 -0700 Subject: [PATCH 27/33] More cleanup --- tests/control/PlanarArmControllerTest.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 8214a39e8..3af7c5fd8 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -48,10 +48,10 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { // Instantiate PlanarArmController. PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); - // Attempt to straighten out the EE all the way, violating max length safety factor. + // Attempt to straighten out the end effector all the way, violating max length safety factor. + // This should cause the EE to be repositioned to fit the length constraint. foo.set_setpoint({0.0, 0.0}); - navtypes::Vectord<2> limitedSetpoint = foo.get_setpoint(robot::types::dataclock::now()); - assertApprox({9.5, 0.0}, limitedSetpoint); + assertApprox({9.5, 0.0}, foo.get_setpoint(robot::types::dataclock::now())); // Try setting the joints to be orthogonal but within max length, such that: // - End effector position is (6,4), which implies that: From a458dbbd5ee25f914fd642a85b955ee5deea5ef4 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 16 Oct 2023 18:57:38 -0700 Subject: [PATCH 28/33] Improve formatting --- tests/control/PlanarArmControllerTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 3af7c5fd8..1836f32dc 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -48,8 +48,8 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { // Instantiate PlanarArmController. PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); - // Attempt to straighten out the end effector all the way, violating max length safety factor. - // This should cause the EE to be repositioned to fit the length constraint. + // Attempt to straighten out the end effector all the way, violating max length safety + // factor. This should cause the EE to be repositioned to fit the length constraint. foo.set_setpoint({0.0, 0.0}); assertApprox({9.5, 0.0}, foo.get_setpoint(robot::types::dataclock::now())); From 699c6814fda013f3e3e74c3fbfd994e898112a53 Mon Sep 17 00:00:00 2001 From: DrDab Date: Mon, 16 Oct 2023 19:07:57 -0700 Subject: [PATCH 29/33] More formatting improvements --- tests/control/PlanarArmControllerTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 1836f32dc..f78c5ca90 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -48,8 +48,8 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { // Instantiate PlanarArmController. PlanarArmController<2> foo({0, 0}, kin_obj, Constants::arm::SAFETY_FACTOR); - // Attempt to straighten out the end effector all the way, violating max length safety - // factor. This should cause the EE to be repositioned to fit the length constraint. + // Attempt to straighten out end-effector all the way, exceeding max length. + // This should cause the EE to be repositioned to fit the length constraint. foo.set_setpoint({0.0, 0.0}); assertApprox({9.5, 0.0}, foo.get_setpoint(robot::types::dataclock::now())); From 95306c6406a5586665e3de5611463650694ae6e1 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 19 Oct 2023 18:22:39 -0700 Subject: [PATCH 30/33] Rm unnecessary INFO wrapping --- tests/control/PlanarArmControllerTest.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index f78c5ca90..8f9b50168 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -23,11 +23,9 @@ void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double d Eigen::Vector2d diff = p1 - p2; bool distEqual = diff.norm() <= dist; - if (!distEqual) { - std::stringstream ss; - ss << "Expected: " << toString(p1) << ", Actual: " << toString(p2); - INFO(ss.str()); - } + std::stringstream ss; + ss << "Expected: " << toString(p1) << ", Actual: " << toString(p2); + INFO(ss.str()); REQUIRE(distEqual); } From c557aa15088ca7fca7f8f697230ede33f969d2f3 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 19 Oct 2023 18:23:24 -0700 Subject: [PATCH 31/33] Remove unused constant --- tests/control/PlanarArmControllerTest.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index 8f9b50168..fee55761c 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -10,8 +10,6 @@ using namespace Catch::literals; using namespace control; using namespace std::chrono_literals; -constexpr double PI = M_PI; - std::string toString(const Eigen::Vector2d& pose) { std::stringstream ss; ss << "(" << pose(0) << ", " << pose(1) << ")"; From 81086c2124bfc978b9330b858f12ef98a9220d88 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 19 Oct 2023 18:25:37 -0700 Subject: [PATCH 32/33] Improve REQUIRE logging --- tests/control/PlanarArmControllerTest.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index fee55761c..d1e27c701 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -18,14 +18,12 @@ std::string toString(const Eigen::Vector2d& pose) { void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double dist = 1e-5, double angle = 1e-5) { - Eigen::Vector2d diff = p1 - p2; - bool distEqual = diff.norm() <= dist; - std::stringstream ss; ss << "Expected: " << toString(p1) << ", Actual: " << toString(p2); INFO(ss.str()); - REQUIRE(distEqual); + Eigen::Vector2d diff = p1 - p2; + REQUIRE(diff.norm() <= dist); } TEST_CASE("Test Planar Arm Controller", "[control][planararmcontroller]") { From 109ecb34668dfeeb4badf5824a5e9be1e03fe17f Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 19 Oct 2023 18:29:17 -0700 Subject: [PATCH 33/33] Add safety factor logic in PlanarArmController constructor --- src/control/PlanarArmController.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index b11b1c57a..01c4719f7 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -34,8 +34,10 @@ template class PlanarArmController { */ PlanarArmController(const navtypes::Vectord& currJointPos, kinematics::PlanarArmKinematics kin_obj, const double safetyFactor) - : kin(kin_obj), setpoint(kin.jointPosToEEPos(currJointPos)), velocity({0.0, 0.0}), - safetyFactor(safetyFactor) { + : kin(kin_obj), velocity({0.0, 0.0}), safetyFactor(safetyFactor) { + // NOTE: currJointPos could extend beyond the safetyFactor, so safety factor + // normalization logic is performed. + set_setpoint(currJointPos); assert(safetyFactor > 0.0 && safetyFactor < 1.0); }