y2015/util/kinematics.h - RealtimeRoboticsGroup/test - Gitiles

 #ifndef Y2015_UTIL_KINEMATICS_H_
 #define Y2015_UTIL_KINEMATICS_H_

 #include <cmath>
 #include "Eigen/Dense"
 #include "y2015/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  // Y2015_UTIL_KINEMATICS_H_
	#ifndef Y2015_UTIL_KINEMATICS_H_
	#define Y2015_UTIL_KINEMATICS_H_

	#include <cmath>
	#include "Eigen/Dense"
	#include "y2015/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 // Y2015_UTIL_KINEMATICS_H_