diff --git a/src/Globals.cpp b/src/Globals.cpp index 6018367f..dace4c86 100644 --- a/src/Globals.cpp +++ b/src/Globals.cpp @@ -41,7 +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, +control::PlanarArmController<2> planarArmController(planarArmKinematics, Constants::arm::SAFETY_FACTOR); std::atomic armIKEnabled = false; } // namespace Globals diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 21c230bd..d91900d0 100644 --- a/src/control/PlanarArmController.h +++ b/src/control/PlanarArmController.h @@ -30,19 +30,74 @@ class PlanarArmController { /** * @brief Construct a new controller object. * - * @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). + * arm joints). * @param safetyFactor the percentage factor to scale maximum arm extension radius by to - * prevent singularity lock. + * prevent singularity lock. Must be in range (0, 1). */ - PlanarArmController(const navtypes::Vectord& currJointPos, - kinematics::PlanarArmKinematics kin_obj, const double 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); + PlanarArmController(kinematics::PlanarArmKinematics kin_obj, const double safetyFactor) + : kin(kin_obj), safetyFactor(safetyFactor) { CHECK_F(safetyFactor > 0.0 && safetyFactor < 1.0); + + for (unsigned int i = 0; i < kin_obj.getNumSegments(); i++) { + CHECK_F(kin_obj.getSegLens()[i] > 0.0); + } + } + + /** + * @brief Constructs a copy of an existing PlanarArmController object. + * + * @param other The existing PlanarArmController to copy. + */ + PlanarArmController(PlanarArmController&& other) + : kin(std::move(other.kin)), safetyFactor(other.safetyFactor) { + std::lock_guard lock(other.mutex); + mutableFields = std::move(other.mutableFields); + } + + /** + * @brief Instantiates the PlanarArmController with the current joint positions, + * returning true if the joint positions are valid. If PlanarArmController is + * already initialized, the PlanarArmController is reinitialized with the supplied + * positions and this function returns true. Otherwise, controller gets uninitialized and + * function returns false. + * + * @param currJointPos The current joint positions of the arm. + * @return true iff the joint positions are within the robot's maximum arm extension + * radius and the controller was successfully initialized in the function call. + */ + bool tryInitController(const navtypes::Vectord& currJointPos) { + std::lock_guard lock(mutex); + + if (is_setpoint_valid(currJointPos, kin, safetyFactor)) { + if (!mutableFields.has_value()) { + mutableFields.emplace(); + } + + Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(currJointPos); + mutableFields->setpoint = normalizeEEWithinRadius(newSetPoint); + return true; + } + + mutableFields.reset(); + return false; + } + + /** + * @brief Returns whether the target joint positions are within the arm controller's radius + * limit. + * + * @param targetJointPos The target joint positions. + * @return whether the target joint positions are within the arm controller's radius limit. + */ + static bool is_setpoint_valid(const navtypes::Vectord& targetJointPos, + kinematics::PlanarArmKinematics kin_obj, + double safetyFactor) { + // Compute the new EE position to determine if it is within + // safety factor * length of fully extended arm. + double eeRadius = kin_obj.jointPosToEEPos(targetJointPos).norm(); + double maxRadius = kin_obj.getSegLens().sum() * safetyFactor; + return eeRadius <= maxRadius; } /** @@ -53,7 +108,7 @@ class PlanarArmController { void set_setpoint(const navtypes::Vectord& targetJointPos) { Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(targetJointPos); std::lock_guard lock(mutex); - setpoint = normalizeEEWithinRadius(newSetPoint); + mutableFields->setpoint = normalizeEEWithinRadius(newSetPoint); } /** @@ -67,10 +122,11 @@ class PlanarArmController { { std::lock_guard lock(mutex); // calculate current EE setpoint - pos = setpoint; - if (velTimestamp.has_value()) { - double dt = util::durationToSec(currTime - velTimestamp.value()); - pos += velocity * dt; + pos = mutableFields->setpoint; + if (mutableFields->velTimestamp.has_value()) { + double dt = + util::durationToSec(currTime - mutableFields->velTimestamp.value()); + pos += mutableFields->velocity * dt; } } @@ -87,14 +143,15 @@ class PlanarArmController { */ void set_x_vel(robot::types::datatime_t currTime, double targetVel) { std::lock_guard lock(mutex); - if (velTimestamp.has_value()) { - double dt = util::durationToSec(currTime - velTimestamp.value()); + if (mutableFields->velTimestamp.has_value()) { + double dt = util::durationToSec(currTime - mutableFields->velTimestamp.value()); // bounds check (new pos + vel vector <= sum of joint lengths) - setpoint = normalizeEEWithinRadius(setpoint + velocity * dt); + mutableFields->setpoint = normalizeEEWithinRadius(mutableFields->setpoint + + mutableFields->velocity * dt); } - velocity(0) = targetVel; - velTimestamp = currTime; + mutableFields->velocity(0) = targetVel; + mutableFields->velTimestamp = currTime; } /** @@ -106,14 +163,15 @@ class PlanarArmController { */ void set_y_vel(robot::types::datatime_t currTime, double targetVel) { std::lock_guard lock(mutex); - if (velTimestamp.has_value()) { - double dt = util::durationToSec(currTime - velTimestamp.value()); + if (mutableFields->velTimestamp.has_value()) { + double dt = util::durationToSec(currTime - mutableFields->velTimestamp.value()); // bounds check (new pos + vel vector <= sum of joint lengths) - setpoint = normalizeEEWithinRadius(setpoint + velocity * dt); + mutableFields->setpoint = normalizeEEWithinRadius(mutableFields->setpoint + + mutableFields->velocity * dt); } - velocity(1) = targetVel; - velTimestamp = currTime; + mutableFields->velocity(1) = targetVel; + mutableFields->velTimestamp = currTime; } /** @@ -133,22 +191,26 @@ class PlanarArmController { // get new joint positions for target EE bool success = false; navtypes::Vectord jp = kin.eePosToJointPos(newPos, currJointPos, success); - velTimestamp = currTime; + mutableFields->velTimestamp = currTime; if (!success) { LOG_F(WARNING, "IK Failure!"); - velocity.setZero(); + mutableFields->velocity.setZero(); } else { - setpoint = newPos; + mutableFields->setpoint = newPos; } return jp; } private: + struct MutableFields { + Eigen::Vector2d setpoint; + Eigen::Vector2d velocity; + std::optional velTimestamp; + }; + + std::optional mutableFields; std::mutex mutex; const kinematics::PlanarArmKinematics kin; - Eigen::Vector2d setpoint; - Eigen::Vector2d velocity; - std::optional velTimestamp; const double safetyFactor; /** @@ -165,14 +227,15 @@ class PlanarArmController { // setpoint = setpoint inside circle // eePos = new setpoint outside circle // radius = ||setpoint + a*(eePos - setpoint)|| solve for a - double diffDotProd = (eePos - setpoint).dot(setpoint); - double differenceNorm = (eePos - setpoint).squaredNorm(); + double diffDotProd = + (eePos - mutableFields->setpoint).dot(mutableFields->setpoint); + double differenceNorm = (eePos - mutableFields->setpoint).squaredNorm(); double discriminant = std::pow(diffDotProd, 2) - - differenceNorm * (setpoint.squaredNorm() - std::pow(radius, 2)); + differenceNorm * (mutableFields->setpoint.squaredNorm() - std::pow(radius, 2)); double a = (-diffDotProd + std::sqrt(discriminant)) / differenceNorm; // new constrained eePos = (1 - a) * (ee inside circle) + a * (ee outside circle) - eePos = (1 - a) * setpoint + a * eePos; + eePos = (1 - a) * mutableFields->setpoint + a * eePos; } return eePos; } diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index 94b0df2b..fb058687 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -106,9 +106,11 @@ void MissionControlProtocol::handleRequestArmIKEnabled(const json& j) { if (!Globals::armIKEnabled) { DataPoint> armJointPositions = robot::getMotorPositionsRad(Constants::arm::IK_MOTORS); - // Rover responds with armIKEnabledReport after requestArmIKEnable is processed - if (armJointPositions.isValid()) { - Globals::planarArmController.set_setpoint(armJointPositions.getData()); + + bool success = + Globals::planarArmController.tryInitController(armJointPositions.getData()); + + if (success) { this->setArmIKEnabled(true); } else { // unable to enable IK diff --git a/tests/control/PlanarArmControllerTest.cpp b/tests/control/PlanarArmControllerTest.cpp index ea952885..f8d8caea 100644 --- a/tests/control/PlanarArmControllerTest.cpp +++ b/tests/control/PlanarArmControllerTest.cpp @@ -16,7 +16,7 @@ std::string toString(const Eigen::Vector2d& pose) { return ss.str(); } -void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double dist = 1e-5, +void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double dist = 1e-4, double angle = 1e-5) { std::stringstream ss; ss << "Expected: " << toString(p1) << ", Actual: " << toString(p2); @@ -26,10 +26,30 @@ void assertApprox(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, double d REQUIRE(diff.norm() <= dist); } -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, Constants::arm::SAFETY_FACTOR); +TEST_CASE("Test Planar Arm Controller Double Init", "[control][planararmcontroller]") { + 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); + PlanarArmController<2> foo(kin_obj, Constants::arm::SAFETY_FACTOR); + REQUIRE(foo.tryInitController({0, M_PI_2})); + REQUIRE(foo.tryInitController({0, M_PI_2})); + REQUIRE(foo.tryInitController({M_PI_4, M_PI_2})); + + // 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())); +} + +TEST_CASE("Test Planar Arm Safety Factor Violation", "[control][planararmcontroller]") { + 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); + PlanarArmController<2> foo(kin_obj, Constants::arm::SAFETY_FACTOR); + REQUIRE_FALSE(foo.tryInitController({0, 0})); } TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { @@ -40,7 +60,8 @@ TEST_CASE("Test Planar Arm Safety Factor", "[control][planararmcontroller]") { kinematics::PlanarArmKinematics<2> kin_obj(segLens, minAngles, maxAngles, 0.0, 0); // Instantiate PlanarArmController. - PlanarArmController<2> foo({0, M_PI_2}, kin_obj, Constants::arm::SAFETY_FACTOR); + PlanarArmController<2> foo(kin_obj, Constants::arm::SAFETY_FACTOR); + REQUIRE(foo.tryInitController({0, M_PI_2})); // 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.