From 7253a9a8e799e2d738bedc5511f9f400de827e1b Mon Sep 17 00:00:00 2001
From: dziedada <dziedada@umich.edu>
Date: Mon, 9 Dec 2019 23:27:26 -0500
Subject: [PATCH] Arm radius and end effector angle.

---
 generated/common/messages/arm_path_t.hpp      | 22 +++++++++++++++----
 generated/common/messages/arm_plan_t.hpp      |  8 +++----
 .../common/messages/ball_detection_t.hpp      |  8 +++----
 .../common/messages/ball_detections_t.hpp     |  8 +++----
 generated/common/messages/ball_t.hpp          |  8 +++----
 generated/common/messages/depth_image_t.hpp   |  8 +++----
 generated/common/messages/point_cloud_t.hpp   |  8 +++----
 generated/common/messages/point_t.hpp         |  8 +++----
 generated/common/messages/rgb_image_t.hpp     |  8 +++----
 generated/common/messages/rgbd_image_t.hpp    |  8 +++----
 include/planner/arm_planner.hpp               |  8 ++++---
 msgtypes/arm_path_t.lcm                       |  4 ++++
 12 files changed, 63 insertions(+), 43 deletions(-)

diff --git a/generated/common/messages/arm_path_t.hpp b/generated/common/messages/arm_path_t.hpp
index 43a96c1..1e9595f 100644
--- a/generated/common/messages/arm_path_t.hpp
+++ b/generated/common/messages/arm_path_t.hpp
@@ -23,6 +23,8 @@ class arm_path_t
 
         std::vector< std::vector< double > > waypoints;
 
+        std::vector< double > wrist_angles;
+
         int32_t    angles_num;
 
         std::vector< std::vector< double > > angles;
@@ -73,13 +75,13 @@ class arm_path_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int arm_path_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -133,6 +135,11 @@ int arm_path_t::_encodeNoHash(void *buf, int offset, int maxlen) const
         if(tlen < 0) return tlen; else pos += tlen;
     }
 
+    if(this->waypoints_num > 0) {
+        tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->wrist_angles[0], this->waypoints_num);
+        if(tlen < 0) return tlen; else pos += tlen;
+    }
+
     tlen = __int32_t_encode_array(buf, offset + pos, maxlen - pos, &this->angles_num, 1);
     if(tlen < 0) return tlen; else pos += tlen;
 
@@ -163,6 +170,12 @@ int arm_path_t::_decodeNoHash(const void *buf, int offset, int maxlen)
         }
     }
 
+    if(this->waypoints_num) {
+        this->wrist_angles.resize(this->waypoints_num);
+        tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->wrist_angles[0], this->waypoints_num);
+        if(tlen < 0) return tlen; else pos += tlen;
+    }
+
     tlen = __int32_t_decode_array(buf, offset + pos, maxlen - pos, &this->angles_num, 1);
     if(tlen < 0) return tlen; else pos += tlen;
 
@@ -186,15 +199,16 @@ int arm_path_t::_getEncodedSizeNoHash() const
     int enc_size = 0;
     enc_size += __int32_t_encoded_array_size(NULL, 1);
     enc_size += this->waypoints_num * __double_encoded_array_size(NULL, 3);
+    enc_size += __double_encoded_array_size(NULL, this->waypoints_num);
     enc_size += __int32_t_encoded_array_size(NULL, 1);
     enc_size += this->angles_num * __double_encoded_array_size(NULL, 4);
     enc_size += __double_encoded_array_size(NULL, 1);
     return enc_size;
 }
 
-uint64_t arm_path_t::_computeHash(const __lcm_hash_ptr *)
+int64_t arm_path_t::_computeHash(const __lcm_hash_ptr *)
 {
-    uint64_t hash = 0x79da7c220ec69bcfLL;
+    int64_t hash = 0x1d6aa5865aeef863LL;
     return (hash<<1) + ((hash>>63)&1);
 }
 
diff --git a/generated/common/messages/arm_plan_t.hpp b/generated/common/messages/arm_plan_t.hpp
index b0d7d6d..5ffb186 100644
--- a/generated/common/messages/arm_plan_t.hpp
+++ b/generated/common/messages/arm_plan_t.hpp
@@ -80,13 +80,13 @@ class arm_plan_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int arm_plan_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -211,9 +211,9 @@ int arm_plan_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t arm_plan_t::_computeHash(const __lcm_hash_ptr *)
+int64_t arm_plan_t::_computeHash(const __lcm_hash_ptr *)
 {
-    uint64_t hash = 0xad0fbb7edfdde504LL;
+    int64_t hash = 0xad0fbb7edfdde504LL;
     return (hash<<1) + ((hash>>63)&1);
 }
 
diff --git a/generated/common/messages/ball_detection_t.hpp b/generated/common/messages/ball_detection_t.hpp
index c9a6f30..67c63a2 100644
--- a/generated/common/messages/ball_detection_t.hpp
+++ b/generated/common/messages/ball_detection_t.hpp
@@ -68,13 +68,13 @@ class ball_detection_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int ball_detection_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -157,9 +157,9 @@ int ball_detection_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t ball_detection_t::_computeHash(const __lcm_hash_ptr *)
+int64_t ball_detection_t::_computeHash(const __lcm_hash_ptr *)
 {
-    uint64_t hash = 0x8c197938dee35d1fLL;
+    int64_t hash = 0x8c197938dee35d1fLL;
     return (hash<<1) + ((hash>>63)&1);
 }
 
diff --git a/generated/common/messages/ball_detections_t.hpp b/generated/common/messages/ball_detections_t.hpp
index 37bcb0a..1d92a18 100644
--- a/generated/common/messages/ball_detections_t.hpp
+++ b/generated/common/messages/ball_detections_t.hpp
@@ -70,13 +70,13 @@ class ball_detections_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int ball_detections_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -166,7 +166,7 @@ int ball_detections_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t ball_detections_t::_computeHash(const __lcm_hash_ptr *p)
+int64_t ball_detections_t::_computeHash(const __lcm_hash_ptr *p)
 {
     const __lcm_hash_ptr *fp;
     for(fp = p; fp != NULL; fp = fp->parent)
@@ -174,7 +174,7 @@ uint64_t ball_detections_t::_computeHash(const __lcm_hash_ptr *p)
             return 0;
     const __lcm_hash_ptr cp = { p, (void*)ball_detections_t::getHash };
 
-    uint64_t hash = 0x6006d07682a20528LL +
+    int64_t hash = 0x6006d07682a20528LL +
          ball_detection_t::_computeHash(&cp);
 
     return (hash<<1) + ((hash>>63)&1);
diff --git a/generated/common/messages/ball_t.hpp b/generated/common/messages/ball_t.hpp
index 91135dd..7beef7f 100644
--- a/generated/common/messages/ball_t.hpp
+++ b/generated/common/messages/ball_t.hpp
@@ -72,13 +72,13 @@ class ball_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int ball_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -175,9 +175,9 @@ int ball_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t ball_t::_computeHash(const __lcm_hash_ptr *)
+int64_t ball_t::_computeHash(const __lcm_hash_ptr *)
 {
-    uint64_t hash = 0x28f1bc4c1af0a391LL;
+    int64_t hash = 0x28f1bc4c1af0a391LL;
     return (hash<<1) + ((hash>>63)&1);
 }
 
diff --git a/generated/common/messages/depth_image_t.hpp b/generated/common/messages/depth_image_t.hpp
index 2b178d9..eafba66 100644
--- a/generated/common/messages/depth_image_t.hpp
+++ b/generated/common/messages/depth_image_t.hpp
@@ -71,13 +71,13 @@ class depth_image_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int depth_image_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -172,9 +172,9 @@ int depth_image_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t depth_image_t::_computeHash(const __lcm_hash_ptr *)
+int64_t depth_image_t::_computeHash(const __lcm_hash_ptr *)
 {
-    uint64_t hash = 0x77d33e30a3ee87ddLL;
+    int64_t hash = 0x77d33e30a3ee87ddLL;
     return (hash<<1) + ((hash>>63)&1);
 }
 
diff --git a/generated/common/messages/point_cloud_t.hpp b/generated/common/messages/point_cloud_t.hpp
index c6be43d..96ed224 100644
--- a/generated/common/messages/point_cloud_t.hpp
+++ b/generated/common/messages/point_cloud_t.hpp
@@ -66,13 +66,13 @@ class point_cloud_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int point_cloud_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -162,7 +162,7 @@ int point_cloud_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t point_cloud_t::_computeHash(const __lcm_hash_ptr *p)
+int64_t point_cloud_t::_computeHash(const __lcm_hash_ptr *p)
 {
     const __lcm_hash_ptr *fp;
     for(fp = p; fp != NULL; fp = fp->parent)
@@ -170,7 +170,7 @@ uint64_t point_cloud_t::_computeHash(const __lcm_hash_ptr *p)
             return 0;
     const __lcm_hash_ptr cp = { p, (void*)point_cloud_t::getHash };
 
-    uint64_t hash = 0x36d6241a447d6df8LL +
+    int64_t hash = 0x36d6241a447d6df8LL +
          point_t::_computeHash(&cp);
 
     return (hash<<1) + ((hash>>63)&1);
diff --git a/generated/common/messages/point_t.hpp b/generated/common/messages/point_t.hpp
index be87a8d..319e4bb 100644
--- a/generated/common/messages/point_t.hpp
+++ b/generated/common/messages/point_t.hpp
@@ -64,13 +64,13 @@ class point_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int point_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -153,9 +153,9 @@ int point_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t point_t::_computeHash(const __lcm_hash_ptr *)
+int64_t point_t::_computeHash(const __lcm_hash_ptr *)
 {
-    uint64_t hash = 0x2a14f112c253ac0cLL;
+    int64_t hash = 0x2a14f112c253ac0cLL;
     return (hash<<1) + ((hash>>63)&1);
 }
 
diff --git a/generated/common/messages/rgb_image_t.hpp b/generated/common/messages/rgb_image_t.hpp
index b37cd8c..c6385b9 100644
--- a/generated/common/messages/rgb_image_t.hpp
+++ b/generated/common/messages/rgb_image_t.hpp
@@ -71,13 +71,13 @@ class rgb_image_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int rgb_image_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -172,9 +172,9 @@ int rgb_image_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t rgb_image_t::_computeHash(const __lcm_hash_ptr *)
+int64_t rgb_image_t::_computeHash(const __lcm_hash_ptr *)
 {
-    uint64_t hash = 0x27ead5419f99ebc4LL;
+    int64_t hash = 0x27ead5419f99ebc4LL;
     return (hash<<1) + ((hash>>63)&1);
 }
 
diff --git a/generated/common/messages/rgbd_image_t.hpp b/generated/common/messages/rgbd_image_t.hpp
index ee79d99..565d43d 100644
--- a/generated/common/messages/rgbd_image_t.hpp
+++ b/generated/common/messages/rgbd_image_t.hpp
@@ -70,13 +70,13 @@ class rgbd_image_t
         inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
         inline int _getEncodedSizeNoHash() const;
         inline int _decodeNoHash(const void *buf, int offset, int maxlen);
-        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+        inline static int64_t _computeHash(const __lcm_hash_ptr *p);
 };
 
 int rgbd_image_t::encode(void *buf, int offset, int maxlen) const
 {
     int pos = 0, tlen;
-    int64_t hash = (int64_t)getHash();
+    int64_t hash = getHash();
 
     tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
     if(tlen < 0) return tlen; else pos += tlen;
@@ -159,7 +159,7 @@ int rgbd_image_t::_getEncodedSizeNoHash() const
     return enc_size;
 }
 
-uint64_t rgbd_image_t::_computeHash(const __lcm_hash_ptr *p)
+int64_t rgbd_image_t::_computeHash(const __lcm_hash_ptr *p)
 {
     const __lcm_hash_ptr *fp;
     for(fp = p; fp != NULL; fp = fp->parent)
@@ -167,7 +167,7 @@ uint64_t rgbd_image_t::_computeHash(const __lcm_hash_ptr *p)
             return 0;
     const __lcm_hash_ptr cp = { p, (void*)rgbd_image_t::getHash };
 
-    uint64_t hash = 0xf4ea25c8c999ba68LL +
+    int64_t hash = 0xf4ea25c8c999ba68LL +
          rgb_image_t::_computeHash(&cp) +
          depth_image_t::_computeHash(&cp);
 
diff --git a/include/planner/arm_planner.hpp b/include/planner/arm_planner.hpp
index c8a3d42..573141c 100644
--- a/include/planner/arm_planner.hpp
+++ b/include/planner/arm_planner.hpp
@@ -37,7 +37,8 @@ using Eigen::Vector4d;
 
 constexpr int MAX_LOG_ODDS = 5;
 constexpr int MIN_LOG_ODDS = -5;
-constexpr double REACHABLE_RADIUS = 0.21;
+constexpr double REACHABLE_RADIUS = 0.19;
+constexpr double INNER_RADIUS = 0.08;
 
 class ArmPlanner
 	{
@@ -156,8 +157,7 @@ class ArmPlanner
 				{
 				double x = bestBall->reachPrediction.ball_inrange_position_[0];
 				double y = bestBall->reachPrediction.ball_inrange_position_[1];
-				double innerRadius = 0.13; // TODO i thought we might need this to restrict noisy velocities
-				if ( x > 0 && y != 0 && bestBall->reachPrediction.ball_inrange_position_.norm() > innerRadius )
+				if ( x > 0 && y != 0.0 && bestBall->reachPrediction.ball_inrange_position_.norm() > INNER_RADIUS )
 					{
 					std::cout << "x: " << y << " y: " << x << std::endl;
 					publishPlan( bestBall->reachPrediction.ball_inrange_position_ );
@@ -275,12 +275,14 @@ class ArmPlanner
             path.waypoints_num = 1;
             
             std::vector<std::vector< double > > waypoints( 1, std::vector<double>(3, 0));
+            std::vector< double > wrist_angles( 1, 0 );
 
             // Flip X and Y for Arm Coordinate system
             waypoints[0][0] = endpoint[1];
             waypoints[0][1] = endpoint[0];
 			waypoints[0][2] = 1;
             path.waypoints = waypoints;
+            path.wrist_angles = wrist_angles;
             path.speed = 1.0;
             path.angles_num = 1;
             std::vector<std::vector< double > > angles( 1, std::vector<double>(4, 0));
diff --git a/msgtypes/arm_path_t.lcm b/msgtypes/arm_path_t.lcm
index e3c490b..00748f6 100644
--- a/msgtypes/arm_path_t.lcm
+++ b/msgtypes/arm_path_t.lcm
@@ -3,11 +3,15 @@
  */
 struct arm_path_t
 {
+	// waypoints method
 	int32_t waypoints_num;
 	// the third value is mode: 1 or 2
 	double waypoints[waypoints_num][3];
 	double wrist_angles[waypoints_num];
+
+	// angles method
 	int32_t angles_num;
 	double angles[angles_num][4]; 
+	
 	double speed;
 }
\ No newline at end of file