4#include "world_interface/data.h"
11#include <frozen/unordered_map.h>
14using robot::types::jointid_t;
19extern const double SHOULDER_LENGTH;
20extern const double ELBOW_LENGTH;
25extern const float MDEG_PER_DEG;
28extern const double ROBOT_LENGTH;
29extern const double ROBOT_WIDTH;
33extern const double WHEEL_BASE;
38extern const double EFF_WHEEL_BASE;
39extern const double WHEEL_RADIUS;
40extern const double PWM_PER_RAD_PER_SEC;
41extern const double MAX_DRIVE_PWM;
49extern const double MAX_WHEEL_VEL;
56extern const double MAX_DTHETA;
61extern const CameraID WRIST_CAMERA_ID;
67extern const CameraID DRILL_CAMERA_ID;
70extern const std::unordered_map<robot::types::CameraID, std::string> CAMERA_CONFIG_PATHS;
77extern const char* MC_PROTOCOL_NAME;
81extern const char* SIM_PROTOCOL_NAME;
85extern const char* DGPS_PROTOCOL_NAME;
89extern const char* ARDUPILOT_PROTOCOL_NAME;
98extern const double STEER_EPSILON;
103extern const double RADIAN_COST;
105extern const double SAFE_RADIUS;
106extern const int MAX_ITERS;
107extern const double PLAN_RESOLUTION;
108extern const double SEARCH_RADIUS_INCREMENT;
109extern const double GPS_WAYPOINT_RADIUS;
110extern const double LANDMARK_WAYPOINT_RADIUS;
111extern const double EPS;
123extern const int H264_RF_CONSTANT;
128extern const std::unordered_map<CameraID, int> STREAM_RFS;
135constexpr auto JOINT_MOTOR_MAP = frozen::make_unordered_map<jointid_t, motorid_t>(
136 {{jointid_t::armBase, motorid_t::armBase},
137 {jointid_t::shoulder, motorid_t::shoulder},
138 {jointid_t::elbow, motorid_t::elbow},
139 {jointid_t::forearm, motorid_t::forearm},
140 {jointid_t::hand, motorid_t::hand},
141 {jointid_t::drillActuator, motorid_t::drillActuator},
142 {jointid_t::drillMotor, motorid_t::drillMotor}});
149extern const double SAFETY_FACTOR;
154extern const double MAX_EE_VEL;
155extern const double IK_SOLVER_THRESH;
157extern const int IK_SOLVER_MAX_ITER;
163extern const std::array<jointid_t, 2> IK_MOTOR_JOINTS;
169extern const std::array<motorid_t, 2> IK_MOTORS;
174constexpr frozen::unordered_map<motorid_t, std::pair<int, int>, IK_MOTORS.size()> JOINT_LIMITS{
175 {motorid_t::shoulder, {18200, 152500}}, {motorid_t::elbow, {-169100, 0}}};
180constexpr frozen::unordered_map<
motorid_t, double, IK_MOTORS.size()> SEGMENT_LENGTHS{
181 {motorid_t::shoulder, 0.3848608}, {motorid_t::elbow, 0.461264}};
184namespace autonomous {
185extern const double THETA_KP;
186extern const double DRIVE_VEL;
187extern const double SLOW_DRIVE_THRESHOLD;
188extern const double DONE_THRESHOLD;
190extern const util::dseconds CLOSE_TO_TARGET_DUR_VAL;
193extern const double CONTROL_HZ;
duration< int64_t, milli > milliseconds
motorid_t
The motors on the robot.
Definition data.h:57
uint32_t CameraID
The type of a camera id.
Definition data.h:46