Skip to content

Commit

Permalink
Arm radius and end effector angle.
Browse files Browse the repository at this point in the history
  • Loading branch information
dziedada committed Dec 10, 2019
1 parent 4261a76 commit 7253a9a
Show file tree
Hide file tree
Showing 12 changed files with 63 additions and 43 deletions.
22 changes: 18 additions & 4 deletions generated/common/messages/arm_path_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand All @@ -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);
}

Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/arm_plan_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/ball_detection_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/ball_detections_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -166,15 +166,15 @@ 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)
if(fp->v == ball_detections_t::getHash)
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);
Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/ball_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/depth_image_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/point_cloud_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -162,15 +162,15 @@ 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)
if(fp->v == point_cloud_t::getHash)
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);
Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/point_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/rgb_image_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

Expand Down
8 changes: 4 additions & 4 deletions generated/common/messages/rgbd_image_t.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -159,15 +159,15 @@ 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)
if(fp->v == rgbd_image_t::getHash)
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);

Expand Down
8 changes: 5 additions & 3 deletions include/planner/arm_planner.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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_ );
Expand Down Expand Up @@ -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));
Expand Down
4 changes: 4 additions & 0 deletions msgtypes/arm_path_t.lcm
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

0 comments on commit 7253a9a

Please sign in to comment.