FridgeKinematics:
- Added class to perform forward and inverse kinematics.
- Added basic test to hand verify results.
- Added claw fridge collision check by Mr. Schuh.
- Revered code review changes that broke the math.
- Added tests for collision from Mr. Schuh.
- Constants are working, thanks Brian.
- Added velocity calculation based on jacobian.
Change-Id: Iba752fbe6ebecca6dfd3102b23c8aebc3cc652a2
diff --git a/aos/common/util/kinematics_test.cc b/aos/common/util/kinematics_test.cc
new file mode 100644
index 0000000..268f78d
--- /dev/null
+++ b/aos/common/util/kinematics_test.cc
@@ -0,0 +1,242 @@
+#include <cmath>
+
+#include "gtest/gtest.h"
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/util/kinematics.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+// For verifcation against Mr. Schuh's internal tests.
+// Please do not comment on this in a code review. We can remove this after the
+// season to satisfy any OCD.
+bool k_internal_debug = false;
+
+class KinematicsTest : public ::testing::Test {
+ public:
+ KinematicsTest()
+ : lower_height_limit_(0.01),
+ upper_height_limit_(0.65),
+ lower_angle_limit_(-M_PI / 2.0),
+ upper_angle_limit_(M_PI / 2.0) {}
+
+ void SetUp() {
+ ::aos::common::testing::EnableTestLogging();
+ kinematics_ = ElevatorArmKinematics(arm_length_, upper_height_limit_, lower_height_limit_,
+ upper_angle_limit_, lower_angle_limit_);
+ }
+
+ protected:
+ double lower_height_limit_;
+ double upper_height_limit_;
+ double lower_angle_limit_;
+ double upper_angle_limit_;
+ double arm_length_ = 0.7366;
+
+ ElevatorArmKinematics kinematics_;
+};
+
+// Used for internal debugging and verification only not acctual testing.
+// Please do not comment on this in a code review. We can remove this after
+// the season to satisfy any OCD.
+TEST_F(KinematicsTest,
+ PrintPointsSoWeCanHandVerifyThemUntilABetterTestIsMadeAndBrianIsHappy) {
+ if (k_internal_debug) {
+ for (double y = -1.0; y <= 1.0; y += 0.2) {
+ for (double x = -1.0; x <= 1.0; x += 0.2) {
+ ElevatorArmKinematics::KinematicResult res;
+ int region = kinematics_.InverseKinematic(x, y, 0.0, 0.0, &res);
+ printf(
+ " %12.3f %12.3f %8.3f %9.3f %8.2f %12d %12.4f %10.4f "
+ "%15.4f %16.4f\n",
+ x, y, res.elevator_height, res.arm_angle,
+ res.arm_angle * 180.0 / M_PI, region, res.fridge_x, res.fridge_h,
+ res.fridge_x - x, res.fridge_h - y);
+ }
+ }
+
+ // Make a set of calls to test the grabber arm intersection test code.
+ printf(
+ "# ArmAngle (degrees) ElevatorHeight ClawAngle (degrees) GrabberX "
+ "GrabberH intersectReturn SafeClawAngle (degrees)\n");
+ for (double elevator_height = kinematics_.get_elevator_min();
+ elevator_height <= kinematics_.get_elevator_max();
+ elevator_height += 0.10) {
+ for (double arm_angle = kinematics_.get_lower_angle_limit();
+ arm_angle <= kinematics_.get_upper_angle_limit() + 0.01;
+ arm_angle += M_PI * 0.5 / 9.0) {
+ double claw_angle = M_PI * 0.25;
+ double safe_claw_angle;
+ double intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
+ elevator_height, arm_angle, claw_angle, &safe_claw_angle);
+ Eigen::Vector2d grabber_location =
+ kinematics_.ForwardKinematicNoChecking(elevator_height, arm_angle);
+
+ printf(
+ " %8.4f %8.2f %8.2f %14.4f %9.2f %9.2f %9.2f %10.3f %13.4f "
+ "%12.3f\n",
+ arm_angle, arm_angle * 180.0 / M_PI, elevator_height, claw_angle,
+ claw_angle * 180.0 / M_PI, grabber_location.x(),
+ grabber_location.y(), intersectReturnValue, safe_claw_angle,
+ safe_claw_angle * 180.0 / M_PI);
+ }
+ printf("\n");
+ }
+ }
+}
+
+TEST_F(KinematicsTest, ValidIntersectCheckPointAtBottomOfElevatorRange) {
+ double safe_claw_angle;
+ double elevator_height = 0.01;
+ double arm_angle = 30.0 * M_PI / 180.0;
+ double claw_angle = M_PI * 0.25;
+ bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
+ elevator_height, arm_angle, claw_angle, &safe_claw_angle);
+ EXPECT_TRUE(intersectReturnValue);
+ EXPECT_EQ(safe_claw_angle, claw_angle);
+}
+
+TEST_F(KinematicsTest, ValidIntersectCheckPointAtMiddleOfElevatorRange) {
+ double safe_claw_angle;
+ double elevator_height = 0.4;
+ double arm_angle = 30.0 * M_PI / 180.0;
+ double claw_angle = M_PI * 0.25;
+ bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
+ elevator_height, arm_angle, claw_angle, &safe_claw_angle);
+ EXPECT_TRUE(intersectReturnValue);
+ EXPECT_EQ(safe_claw_angle, claw_angle);
+}
+
+TEST_F(KinematicsTest,
+ invalidIntersectCheckPointAtBottomOfElevatorRangeWithSafeClawAngle) {
+ double safe_claw_angle;
+ double elevator_height = 0.01;
+ double arm_angle = -20.0 * M_PI / 180.0;
+ double claw_angle = M_PI * 0.25;
+ bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
+ elevator_height, arm_angle, claw_angle, &safe_claw_angle);
+ EXPECT_FALSE(intersectReturnValue);
+ EXPECT_NEAR(safe_claw_angle, 0.0435733, 0.000001);
+}
+
+TEST_F(KinematicsTest,
+ invalidIntersectCheckPointAtMiddleOfElevatorRangeWithSafeClawAngle) {
+ double safe_claw_angle;
+ double elevator_height = 0.41;
+ double arm_angle = -60.0 * M_PI / 180.0;
+ double claw_angle = M_PI * 0.25;
+ bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
+ elevator_height, arm_angle, claw_angle, &safe_claw_angle);
+ EXPECT_FALSE(intersectReturnValue);
+ EXPECT_NEAR(safe_claw_angle, 0.12655341, 0.000001);
+}
+
+TEST_F(KinematicsTest,
+ invalidIntersectCheckPointAtBottomOfElevatorRangeNoSafeClawAngle) {
+ double safe_claw_angle;
+ double elevator_height = 0.01;
+ double arm_angle = -30.0 * M_PI / 180.0;
+ double claw_angle = M_PI * 0.25;
+ bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
+ elevator_height, arm_angle, claw_angle, &safe_claw_angle);
+ EXPECT_FALSE(intersectReturnValue);
+ EXPECT_EQ(safe_claw_angle, -1.0);
+}
+
+TEST_F(KinematicsTest,
+ invalidIntersectCheckPointAtMiddleOfElevatorRangeNoSafeClawAngle) {
+ double safe_claw_angle;
+ double elevator_height = 0.41;
+ double arm_angle = -70.0 * M_PI / 180.0;
+ double claw_angle = M_PI * 0.25;
+ bool intersectReturnValue = kinematics_.GrabberArmIntersectionCheck(
+ elevator_height, arm_angle, claw_angle, &safe_claw_angle);
+ EXPECT_FALSE(intersectReturnValue);
+ EXPECT_EQ(safe_claw_angle, -1.0);
+}
+
+// Tests that velocity calulations are correct
+TEST_F(KinematicsTest, InverseKinematicVelocity) {
+ ElevatorArmKinematics::KinematicResult result;
+ // move striaght up and verify that only hieght changes
+ EXPECT_EQ(0, kinematics_.InverseKinematic(0.0, 0.2, 0.0, 0.7, &result));
+ EXPECT_NEAR(0.0, result.arm_velocity, 0.00001);
+ EXPECT_NEAR(0.7, result.elevator_velocity, 0.00001);
+ // check the negative
+ EXPECT_EQ(0, kinematics_.InverseKinematic(0.0, 0.2, 0.0, -0.7, &result));
+ EXPECT_NEAR(0.0, result.arm_velocity, 0.00001);
+ EXPECT_NEAR(-0.7, result.elevator_velocity, 0.00001);
+ // even with the arm out we should still just move up
+ EXPECT_EQ(0, kinematics_.InverseKinematic(M_PI / 6, 0.2, 0.0, 0.7, &result));
+ EXPECT_NEAR(0.0, result.arm_velocity, 0.00001);
+ EXPECT_NEAR(0.7, result.elevator_velocity, 0.00001);
+ // even with the arm back we should still just move up
+ EXPECT_EQ(0, kinematics_.InverseKinematic(-M_PI / 6, 0.2, 0.0, 0.7, &result));
+ EXPECT_NEAR(0.0, result.arm_velocity, 0.00001);
+ EXPECT_NEAR(0.7, result.elevator_velocity, 0.00001);
+
+ // should move only angle forward
+ EXPECT_EQ(0, kinematics_.InverseKinematic(0.0, 0.2, 1.0, 0.0, &result));
+ EXPECT_NEAR(-1.35759, result.arm_velocity, 0.00001);
+ EXPECT_NEAR(0.0, result.elevator_velocity, 0.00001);
+ // check the negative
+ EXPECT_EQ(0, kinematics_.InverseKinematic(0.0, 0.2, -1.0, 0.0, &result));
+ EXPECT_NEAR(1.35759, result.arm_velocity, 0.00001);
+ EXPECT_NEAR(0.0, result.elevator_velocity, 0.00001);
+ // with the arm out a change in x should make arm angle greater and
+ // bring the evevator down.
+ EXPECT_EQ(0, kinematics_.InverseKinematic(0.2, 0.2, 1.0, 0.0, &result));
+ EXPECT_GT(0.0, result.arm_velocity);
+ EXPECT_LT(0.0, result.elevator_velocity);
+ // with the arm out a change in x should make arm angle greater and
+ // bring the evevator down.
+ EXPECT_EQ(0, kinematics_.InverseKinematic(-0.2, 0.2, 1.0, 0.0, &result));
+ EXPECT_GT(0.0, result.arm_velocity);
+ EXPECT_GT(0.0, result.elevator_velocity);
+}
+
+// Tests that velocity calulations are correct
+TEST_F(KinematicsTest, ForwardKinematicVelocity) {
+ ElevatorArmKinematics::KinematicResult result;
+
+ // moving the arm forward at zero should result in x velocity
+ EXPECT_EQ(0, kinematics_.ForwardKinematic(0.2, 0.0, 0.0, 1.35759, &result));
+ EXPECT_NEAR(-1.0, result.fridge_x_velocity, 0.00001);
+ EXPECT_NEAR(0.0, result.fridge_h_velocity, 0.00001);
+ // check the negative
+ EXPECT_EQ(0, kinematics_.ForwardKinematic(0.2, 0.0, 0.0, -1.35759, &result));
+ EXPECT_NEAR(1.0, result.fridge_x_velocity, 0.00001);
+ EXPECT_NEAR(0.0, result.fridge_h_velocity, 0.00001);
+ // moving the arm up at zero should result in h velocity
+ EXPECT_EQ(0, kinematics_.ForwardKinematic(0.2, 0.0, 1.0, 0.0, &result));
+ EXPECT_NEAR(0.0, result.fridge_x_velocity, 0.00001);
+ EXPECT_NEAR(1.0, result.fridge_h_velocity, 0.00001);
+ // check the negative
+ EXPECT_EQ(0, kinematics_.ForwardKinematic(0.2, 0.0, -1.0, 0.0, &result));
+ EXPECT_NEAR(0.0, result.fridge_x_velocity, 0.00001);
+ EXPECT_NEAR(-1.0, result.fridge_h_velocity, 0.00001);
+ // arm is forward a negative angle should make x head forward and y head down.
+ EXPECT_EQ(0, kinematics_.ForwardKinematic(0.5, -0.2, 0.0, -1.0, &result));
+ EXPECT_GT(result.fridge_x_velocity, 0.0);
+ EXPECT_LT(result.fridge_h_velocity, 0.0);
+ // arm is forward a positive angle should make x head backwardward and y head up.
+ EXPECT_EQ(0, kinematics_.ForwardKinematic(0.5, -0.2, 0.0, 1.0, &result));
+ EXPECT_LT(result.fridge_x_velocity, 0.0);
+ EXPECT_GT(result.fridge_h_velocity, 0.0);
+ // arm is backward a negative angle should make x head forward and y head down.
+ EXPECT_EQ(0, kinematics_.ForwardKinematic(0.5, 0.2, 0.0, -1.0, &result));
+ EXPECT_GT(result.fridge_x_velocity, 0.0);
+ EXPECT_GT(result.fridge_h_velocity, 0.0);
+ // arm is backward a negative angle should make x head forward and y head down.
+ EXPECT_EQ(0, kinematics_.ForwardKinematic(0.5, 0.2, 0.0, 1.0, &result));
+ EXPECT_LT(result.fridge_x_velocity, 0.0);
+ EXPECT_LT(result.fridge_h_velocity, 0.0);
+}
+
+} // namespace testing
+} // namespace util
+} // namespace aos