Skip to content

Commit

Permalink
Improved formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
DrDab committed Sep 30, 2023
1 parent 93d0aab commit 8e6bc25
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 7 deletions.
2 changes: 1 addition & 1 deletion src/Constants.h
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,7 @@ constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t

// Arm inverse kinematics
namespace arm {
/**
/**
* Percentage of fully extended overall arm length to limit arm extension within.
*/
constexpr double SAFETY_FACTOR = 0.95;
Expand Down
11 changes: 6 additions & 5 deletions src/control/PlanarArmController.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,14 @@ template <unsigned int N> 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<N>& currJointPos,
kinematics::PlanarArmKinematics<N> 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<N> 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);
}

/**
Expand Down
4 changes: 3 additions & 1 deletion tests/control/PlanarArmControllerTest.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include "../../src/control/PlanarArmController.h"

#include "../../src/Constants.h"

#include <catch2/catch.hpp>
Expand All @@ -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);
}

0 comments on commit 8e6bc25

Please sign in to comment.