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/build/aos_all.gyp b/aos/build/aos_all.gyp
index 638185d..bd333a3 100644
--- a/aos/build/aos_all.gyp
+++ b/aos/build/aos_all.gyp
@@ -45,6 +45,7 @@
'<(AOS)/common/libc/libc.gyp:aos_strerror_test',
'<(AOS)/common/libc/libc.gyp:aos_strsignal_test',
'<(AOS)/common/util/util.gyp:run_command_test',
+ '<(AOS)/common/util/util.gyp:kinematics_test',
],
},
],
diff --git a/aos/common/util/kinematics.h b/aos/common/util/kinematics.h
new file mode 100644
index 0000000..5718390
--- /dev/null
+++ b/aos/common/util/kinematics.h
@@ -0,0 +1,388 @@
+#ifndef AOS_COMMON_UTIL_KINEMATICS_H_
+#define AOS_COMMON_UTIL_KINEMATICS_H_
+
+#include <cmath>
+#include "Eigen/Dense"
+#include "frc971/constants.h"
+
+namespace aos {
+namespace util {
+
+// A class for performing forward and inverse kinematics on the elevator-arm
+// system. It can calculate where the fridge grabbers will be if the arm and
+// elevator are at a given position, as well as where the arm and elevator
+// should go in order to get the grabbers to a specific location.
+class ElevatorArmKinematics {
+ public:
+ typedef enum {
+ // These specify the particular region that an invalid request was in. Right
+ // is toward the front of the robot, left is toward the back.
+
+ // Request is valid.
+ REGION_VALID = 0,
+ // Request is farther right than the arm can extend.
+ REGION_RIGHT = 1 << 0,
+ // Request is towards the front of the robot but higher than we can extend
+ // with the elevator and the arm.
+ REGION_UPPER_RIGHT = 1 << 1,
+ // We can get the x part of the request, which is towards the front of the
+ // robot, but not the h part, which is too high.
+ REGION_INSIDE_UPPER_RIGHT = 1 << 2,
+ // We can get the x part of the request, which is towards the front of the
+ // robot, but not the h part, which is too low.
+ REGION_INSIDE_LOWER_RIGHT = 1 << 3,
+ // Request is towards the front of the robot but lower than we can extend
+ // with the elevator and the arm.
+ REGION_LOWER_RIGHT = 1 << 4,
+ // Request is farther left than the arm can extend.
+ REGION_LEFT = 1 << 5,
+ // Request is towards the back of the robot but higher than we can extend
+ // with the elevator and the arm.
+ REGION_UPPER_LEFT = 1 << 6,
+ // We can get the x part of the request, which is towards the front of the
+ // robot, but not the h part, which is too high.
+ REGION_INSIDE_UPPER_LEFT = 1 << 7,
+ // We can get the x part of the request, which is towards the back of the
+ // robot, but not the h part, which is too low.
+ REGION_INSIDE_LOWER_LEFT = 1 << 8,
+ // Request is towards the back of the robot but lower than we can extend
+ // with the elevator and the arm.
+ REGION_LOWER_LEFT = 1 << 9,
+ // Request is invalid, but don't know where it is out of range.
+ REGION_UNKNOWN = 1 << 10,
+ } Region;
+
+ class KinematicResult {
+ public:
+ // The elevator height result from an inverse kinematic.
+ double elevator_height;
+ // The arm angle result from an inverse kinematic.
+ double arm_angle;
+ // Resulting velocity of the elevator given x,y velocities.
+ double elevator_velocity;
+ // Resulting velocity of the arm given x,y velocities.
+ double arm_velocity;
+ // The fridge height value from a forward kinematic.
+ double fridge_h;
+ // The fridge x value from a forward kinematic.
+ double fridge_x;
+ // Resulting velocity of the fridge height given arm and elevator
+ // velocities.
+ double fridge_h_velocity;
+ // Resulting velocity of the fridge x given arm and elevator velocities.
+ double fridge_x_velocity;
+ };
+
+ // If we use the default constructor we wil just always not be able to move.
+ ElevatorArmKinematics()
+ : length_arm_(1.0),
+ elevator_max_(0.0),
+ elevator_min_(0.0),
+ upper_angle_limit_(0.0),
+ lower_angle_limit_(0.0) {}
+
+ ElevatorArmKinematics(double length_arm, double height_max, double height_min,
+ double angle_max, double angle_min)
+ : length_arm_(length_arm),
+ elevator_max_(height_max),
+ elevator_min_(height_min),
+ upper_angle_limit_(angle_max),
+ lower_angle_limit_(angle_min),
+ geometry_(frc971::constants::GetValues().clawGeometry) {}
+
+ ~ElevatorArmKinematics() {}
+
+ // Limit a number to the speed of light. The loops should handle this a lot
+ // better than overflow.
+ void LimitLightSpeed(double* num) {
+ if (*num > 299792458.0) {
+ *num = 299792458.0;
+ }
+ if (*num < -299792458.0) {
+ *num = -299792458.0;
+ }
+ if (!::std::isfinite(*num)) {
+ *num = 0.0;
+ }
+ }
+
+ // Calculates the arm angle in radians and the elevator height in meters for
+ // a desired Fridge grabber height and x location. x is positive going
+ // toward the front of the robot.
+ // h is positive going up. x=0 and h=0 is the location of the top fridge
+ // grabber when the elevator is at 0 height and the arm angle is 0 (vertical).
+ // Both the x and h values are given in meters.
+ // Returns the region of the request.
+ // Result is:
+ // the angle of the arm in radians
+ // the height of the elevator in meters
+ // the resulting x
+ // and the resulting h
+ // If an impossible location is requested, the arm angle and elevator height
+ // returned are the closest possible for the requested fridge grabber height.
+ // If the requested height is above the max possible height, the angle
+ // will be 0 and the height will be the max possible height of the elevator.
+ int InverseKinematic(double request_x, double request_h,
+ double request_x_velocity, double request_y_velocity,
+ KinematicResult* result) {
+ int valid_or_invalid = REGION_VALID;
+
+ double square_arm = length_arm_ * length_arm_;
+ double term = ::std::sqrt(square_arm - request_x * request_x);
+
+ // Check to see if the x location can be satisfied. If the requested x
+ // location
+ // is further out than the arm can go, it is not possible for any elevator
+ // location.
+ if (request_x > length_arm_) {
+ result->arm_angle = -M_PI * 0.5;
+ valid_or_invalid |= REGION_RIGHT;
+ } else if (request_x < -length_arm_) {
+ result->arm_angle = M_PI * 0.5;
+ valid_or_invalid |= REGION_LEFT;
+ } else {
+ result->arm_angle = ::std::asin(-request_x / length_arm_);
+ result->arm_velocity = (-1.0 / term) * request_x_velocity;
+ LimitLightSpeed(&result->arm_velocity);
+ }
+
+ result->elevator_height =
+ request_h + length_arm_ * (1.0 - ::std::cos(result->arm_angle));
+ result->elevator_velocity =
+ (request_x / (square_arm * term)) * request_x_velocity +
+ request_y_velocity;
+ LimitLightSpeed(&result->elevator_velocity);
+
+ // Check to see if the requested elevator height is possible
+ if (request_h > elevator_max_) {
+ // The elevator cannot go high enough with any arm angle to satisfy this
+ // request. So position the elevator at the top and the arm angle set to
+ // vertical.
+ result->elevator_height = elevator_max_;
+ result->arm_angle = 0.0;
+ if (request_x >= 0) {
+ valid_or_invalid |= REGION_UPPER_RIGHT;
+ } else {
+ valid_or_invalid |= REGION_UPPER_LEFT;
+ }
+ } else if (request_h < -length_arm_ + elevator_min_) {
+ // The elevator cannot go low enough with any arm angle to satisfy this
+ // request. So position the elevator at the bottom and the arm angle to
+ // satisfy the x request The elevator will move up as the grabber moves to
+ // the center of the robot when in this part of the motion space.
+ result->elevator_height = elevator_min_;
+ if (request_x >= 0) {
+ valid_or_invalid |= REGION_LOWER_RIGHT;
+ } else {
+ valid_or_invalid |= REGION_LOWER_LEFT;
+ }
+ } else if (result->elevator_height > elevator_max_) {
+ // Impossibly high request. So get as close to the x request with the
+ // elevator at the top of its range.
+ result->elevator_height = elevator_max_;
+ if (request_x >= 0) {
+ result->arm_angle =
+ -::std::acos((length_arm_ + request_h - elevator_max_) /
+ length_arm_);
+ valid_or_invalid |= REGION_INSIDE_UPPER_RIGHT;
+ } else {
+ result->arm_angle = ::std::acos(
+ (length_arm_ + request_h - elevator_max_) / length_arm_);
+ valid_or_invalid |= REGION_INSIDE_UPPER_LEFT;
+ }
+ } else if (result->elevator_height < elevator_min_) {
+ // Impossibly low request. So satisfy the x request with the elevator
+ // at the bottom of its range.
+ // The elevator will move up as the grabber moves to the center of the
+ // robot
+ // when in this part of the motion space.
+ result->elevator_height = elevator_min_;
+ if (request_x >= 0) {
+ valid_or_invalid |= REGION_INSIDE_LOWER_RIGHT;
+ } else {
+ valid_or_invalid |= REGION_INSIDE_LOWER_LEFT;
+ }
+ }
+
+ // if we are not in a valid region we will zero the velocity for now
+ if (valid_or_invalid != REGION_VALID) {
+ result->arm_velocity = 0.0;
+ result->elevator_velocity = 0.0;
+ }
+
+ if (ForwardKinematic(result->elevator_height, result->arm_angle,
+ result->elevator_velocity, result->arm_velocity,
+ result) == REGION_UNKNOWN) {
+ return REGION_UNKNOWN;
+ }
+ return valid_or_invalid;
+ }
+
+ // Takes an elevator height and arm angle and projects these to the resulting
+ // fridge height and x offset. Returns REGION_UNKNOWN if the values are
+ // outside
+ // limits. This will result in the height/angle being bounded and the
+ // resulting position will be returned.
+ Region ForwardKinematic(double elevator_height, double arm_angle,
+ double elevator_velocity, double arm_velocity,
+ KinematicResult* result) {
+ result->elevator_height = elevator_height;
+ result->arm_angle = arm_angle;
+
+ Region valid = REGION_VALID;
+ if (elevator_height < elevator_min_) {
+ LOG(WARNING, "elevator %.2f limited at %.2f\n", result->elevator_height,
+ elevator_min_);
+ result->elevator_height = elevator_min_;
+ valid = REGION_UNKNOWN;
+ }
+ if (elevator_height > elevator_max_) {
+ LOG(WARNING, "elevator %.2f limited at %.2f\n", result->elevator_height,
+ elevator_max_);
+ result->elevator_height = elevator_max_;
+ valid = REGION_UNKNOWN;
+ }
+ if (arm_angle < lower_angle_limit_) {
+ LOG(WARNING, "arm %.2f limited at %.2f\n", result->arm_angle,
+ lower_angle_limit_);
+ result->arm_angle = lower_angle_limit_;
+ valid = REGION_UNKNOWN;
+ }
+ if (arm_angle > upper_angle_limit_) {
+ result->arm_angle = upper_angle_limit_;
+ LOG(WARNING, "arm %.2f limited at %.2f\n", result->arm_angle,
+ upper_angle_limit_);
+ valid = REGION_UNKNOWN;
+ }
+ // Compute the fridge grabber height and x location using the computed
+ // elevator height and arm angle.
+ result->fridge_h = result->elevator_height +
+ (::std::cos(result->arm_angle) - 1.0) * length_arm_;
+ result->fridge_x = -::std::sin(result->arm_angle) * length_arm_;
+ // velocity based on joacobian
+ result->fridge_x_velocity =
+ -length_arm_ * ::std::cos(result->arm_angle) * arm_velocity;
+ LimitLightSpeed(&result->fridge_x_velocity);
+ result->fridge_h_velocity =
+ -length_arm_ * ::std::sin(result->arm_angle) * arm_velocity +
+ elevator_velocity;
+ LimitLightSpeed(&result->fridge_h_velocity);
+ return valid;
+ }
+
+ // Same as ForwardKinematic but without any checking.
+ Eigen::Vector2d ForwardKinematicNoChecking(double elevator_height,
+ double arm_angle) {
+ // Compute the fridge grabber height and x location using the computed
+ // elevator height and arm angle.
+ Eigen::Vector2d grabber_location;
+ grabber_location.y() =
+ elevator_height + (::std::cos(arm_angle) - 1.0) * length_arm_;
+ grabber_location.x() = -::std::sin(arm_angle) * length_arm_;
+ return grabber_location;
+ }
+
+ // 2 dimensional version of cross product
+ double Cross(Eigen::Vector2d a, Eigen::Vector2d b) {
+ double crossProduct = a.x() * b.y() - a.y() * b.x();
+ return crossProduct;
+ }
+
+ // Tell whether or not it is safe to move the grabber to a position.
+ // Returns true if the current move is safe.
+ // If it returns false then a safe_claw_angle that is greater than zero is
+ // acceptable otherwise if safe_claw_angle is less than zero there will be no
+ // valid solution.
+ bool GrabberArmIntersectionCheck(double elevator_height, double arm_angle,
+ double claw_angle, double* safe_claw_angle) {
+ Eigen::Vector2d grabber_location =
+ ForwardKinematicNoChecking(elevator_height, arm_angle);
+ if (grabber_location.x() < geometry_.grabber_always_safe_x_max ||
+ grabber_location.y() > geometry_.grabber_always_safe_h_min) {
+ *safe_claw_angle = claw_angle;
+ return true;
+ }
+ Eigen::Vector2d grabber_bottom_end;
+ Eigen::Vector2d claw_i_unit_direction(::std::cos(claw_angle),
+ sin(claw_angle));
+ Eigen::Vector2d claw_j_unit_direction(-::std::sin(claw_angle),
+ cos(claw_angle));
+
+ // Vector from the center of the arm rotation axis to front bottom
+ // corner of the grabber.
+ Eigen::Vector2d grabber_end_location_from_arm_axis(
+ geometry_.grabber_half_length, -geometry_.grabber_delta_y);
+
+ // Bottom front corner of the grabber. This is what will usually hit the
+ // claw first.
+ grabber_bottom_end = grabber_location + grabber_end_location_from_arm_axis;
+
+ // Location of the claw horizontal axis of rotation relative to the
+ // arm axis of rotation with the elevator at 0 and the arm angle of 0
+ // The horizontal axis is the up and down motion axis.
+ Eigen::Vector2d claw_updown_axis(geometry_.grabber_arm_horz_separation,
+ -geometry_.grabber_arm_vert_separation);
+
+ // This point is used to make a cross product with the bottom end of the
+ // grabber
+ // The result of the cross product tells if the parts intersect or not.
+ Eigen::Vector2d claw_top_ref_point =
+ claw_updown_axis + geometry_.claw_top_thickness * claw_j_unit_direction;
+
+ Eigen::Vector2d claw_top_ref_point_to_grabber_bottom_end =
+ grabber_bottom_end - claw_top_ref_point;
+ double claw_grabber_check =
+ Cross(claw_i_unit_direction, claw_top_ref_point_to_grabber_bottom_end);
+
+ // Now set the safe claw angle.
+ if (claw_grabber_check > 0.0) {
+ *safe_claw_angle = claw_angle;
+ return true;
+ } else if (grabber_bottom_end.y() <
+ claw_updown_axis.y() +
+ geometry_.claw_top_thickness) { // grabber is too close
+ *safe_claw_angle = -1.0;
+ return false;
+ } else {
+ // To find the safe angle for the claw, draw a line between the claw
+ // rotation axis and the lower front corner of the grabber. The angle of
+ // this line is used with the angle between the edge of the claw and the
+ // center line of the claw to determine the angle of the claw.
+ Eigen::Vector2d claw_axis_to_grabber_bottom_end =
+ grabber_bottom_end - claw_updown_axis;
+ double hypot = claw_axis_to_grabber_bottom_end.norm();
+ double angleDiff = ::std::asin(geometry_.claw_top_thickness / hypot);
+ *safe_claw_angle = ::std::atan2(claw_axis_to_grabber_bottom_end.y(),
+ claw_axis_to_grabber_bottom_end.x()) -
+ angleDiff;
+ return false;
+ }
+ }
+
+ double get_elevator_min() { return elevator_min_; }
+
+ double get_elevator_max() { return elevator_max_; }
+
+ double get_upper_angle_limit() { return upper_angle_limit_; }
+
+ double get_lower_angle_limit() { return lower_angle_limit_; }
+
+ private:
+ // length of the arm
+ double length_arm_;
+ // max height the elevator can go.
+ double elevator_max_;
+ // min height the elevator can go.
+ double elevator_min_;
+ // arm angle upper limit
+ double upper_angle_limit_;
+ // arm angle lower limit
+ double lower_angle_limit_;
+ // Geometry of the arm + fridge
+ frc971::constants::Values::ClawGeometry geometry_;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_COMMON_UTIL_KINEMATICS_H_
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
diff --git a/aos/common/util/util.gyp b/aos/common/util/util.gyp
index e9a37c9..f60622e 100644
--- a/aos/common/util/util.gyp
+++ b/aos/common/util/util.gyp
@@ -149,5 +149,34 @@
'<(EXTERNALS):gtest',
],
},
+ {
+ 'target_name': 'kinematics',
+ 'type': 'static_library',
+ 'sources': [
+ #'kinematics.h',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ },
+ {
+ 'target_name': 'kinematics_test',
+ 'type': 'executable',
+ 'sources': [
+ 'kinematics_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
+ 'kinematics'
+ ],
+ },
],
}
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 58066c6..04b9576 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -63,6 +63,43 @@
const double kMaxAllowedLeftRightArmDifference = 0.04; // radians
const double kMaxAllowedLeftRightElevatorDifference = 0.01; // meters
+const Values::ClawGeometry kClawGeometry{
+ // Horizontal distance from the center of the grabber to the end.
+ 0.5 * 18.0 * 0.0254,
+ // Vertical distance from the arm rotation center to the bottom of
+ // the
+ // grabber. Distance measured with arm vertical (theta = 0).
+ 5.1 * 0.0254,
+ // Vertical separation of the claw and arm rotation centers with the
+ // elevator at 0.0 and the arm angle set to zero.
+ 10.5 * 0.0254,
+ // Horizontal separation of the claw and arm rotation centers with
+ // the
+ // elevator at 0.0 and the arm angle set to zero.
+ 6.5 * 0.0254,
+ // Distance between the center of the claw to the top of the claw.
+ // 2.75 inches would work most of the time. Using 3.5 inches because
+ // of the
+ // pnumatics fitting on the piston.
+ 3.5 * 0.0254,
+ // The grabber is safe at any height if it is behind this location.
+ // The location of the grabber is used here and not the location of
+ // the end
+ // of the grabber. The grabber location is (0, 0) when the elevator
+ // is at 0
+ // and the arm angle is 0.
+ (18.0 - 0.3) * 0.0254,
+ // The grabber is safe at any x if it is above this location.
+ // The location of the grabber is used here and not the location of
+ // the end
+ // of the grabber. The grabber location is (0, 0) when the elevator
+ // is at 0
+ // and the arm angle is 0.
+ // The "-5.4" is the location of the bottom of the grabber when
+ // the elevator is at the bottom (-0.3 inches) and arm angle is 0.
+ -8.0 * 0.0254,
+};
+
// Gearing ratios of the pots and encoders for the elevator and arm.
// Ratio is output shaft rotations per encoder/pot rotation
// Checked by Daniel on 2/13/15.
@@ -154,6 +191,7 @@
kMaxAllowedLeftRightArmDifference,
kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
};
break;
case kPracticeTeamNumber:
@@ -222,6 +260,7 @@
kMaxAllowedLeftRightArmDifference,
kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
};
break;
case kCompTeamNumber:
@@ -292,6 +331,7 @@
kMaxAllowedLeftRightArmDifference,
kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
index d66853b..fe47418 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -116,6 +116,29 @@
double max_allowed_left_right_arm_difference;
double max_allowed_left_right_elevator_difference;
+
+ struct ClawGeometry {
+ // Horizontal distance from the center of the grabber to the end.
+ double grabber_half_length;
+ // Vertical distance from the arm rotation center to the bottom of the
+ // grabber. Distance measured with arm vertical (theta = 0).
+ double grabber_delta_y;
+ // Vertical separation of the claw and arm rotation centers with the
+ // elevator at 0.0 and the arm angle set to zero.
+ double grabber_arm_vert_separation;
+ // Horizontal separation of the claw and arm rotation centers with the
+ // elevator at 0.0 and the arm angle set to zero.
+ double grabber_arm_horz_separation;
+ // Distance between the center of the claw to the top of the claw.
+ // The line drawn at this distance parallel to the claw centerline is used
+ // to determine if claw interfears with the grabber.
+ double claw_top_thickness;
+ // The grabber is safe at any height if it is behind this location.
+ double grabber_always_safe_h_min;
+ // The grabber is safe at any x if it is above this location.
+ double grabber_always_safe_x_max;
+ };
+ ClawGeometry clawGeometry;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and