From 68897aee37174019243542ea6cc940581c2f66ef Mon Sep 17 00:00:00 2001 From: Geeoon Chung Date: Sun, 4 Jun 2023 15:08:31 -0700 Subject: [PATCH 1/7] Added handler for inverse kinematics special case joint request (#240) * Added special case for inverse kinematics * Reformat * Added special case for inverse kinematics * Reformat * Added ik global * Renamed arm IK global. made type atomic as well * Added armIKEnabled handler and validater * Added case for when armIK global is enabled/disabled * Fixed build error * Fixed formatting error * Removed redundant code * Set armIKEnabled to false on e stop and connect * Fixed build error * Proper namespace for validateKey * Added special case for inverse kinematics * Reformat * Added ik global * Renamed arm IK global. made type atomic as well * Added armIKEnabled handler and validater * Added case for when armIK global is enabled/disabled * Fixed build error * Fixed formatting error * Removed redundant code * Set armIKEnabled to false on e stop and connect * Fixed build error * Proper namespace for validateKey * Added special case for inverse kinematics * Reformat * Added ik global * Renamed arm IK global. made type atomic as well * Added armIKEnabled handler and validater * Added case for when armIK global is enabled/disabled * Fixed build error * Fixed formatting error * Removed redundant code * Set armIKEnabled to false on e stop and connect * Fixed build error * Proper namespace for validateKey * Added armIKEnabled handler and validater * Added case for when armIK global is enabled/disabled * Fixed build error * Fixed formatting error * Removed redundant code * Proper namespace for validateKey * removed old bool for arm ik enabled * Set setpoint to current ee position on ik enable * Reformat and crash on no valid data * Reformat * Added updateArmThread * Reformat * fix more things * Added scalar to velocity set * Addressed some PR comments * Reformat * Addressed PR comments * Reformat * Addressed PR comments * fix formatting * Addressed PR comments * Reformat --------- Co-authored-by: Abhay D --- src/Constants.h | 42 +++++++++-- src/Globals.cpp | 1 + src/Globals.h | 1 + src/network/MissionControlProtocol.cpp | 73 ++++++++++++++++++- src/network/MissionControlProtocol.h | 4 +- src/world_interface/data.cpp | 4 + src/world_interface/data.h | 11 ++- .../kinematic_common_interface.cpp | 40 +++++----- 8 files changed, 148 insertions(+), 28 deletions(-) diff --git a/src/Constants.h b/src/Constants.h index 64c47114a..621d71a4c 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -103,6 +103,7 @@ constexpr const char* DGPS_PROTOCOL_NAME = "/dgps"; constexpr const char* ARDUPILOT_PROTOCOL_NAME = "/ardupilot"; constexpr std::chrono::milliseconds JOINT_POWER_REPEAT_PERIOD(333); +constexpr std::chrono::milliseconds ARM_IK_UPDATE_PERIOD(50); namespace Nav { // Distance (m) we could have traveled forward in the time it takes to turn 1 radian @@ -130,29 +131,58 @@ namespace video { constexpr int H264_RF_CONSTANT = 40; } // namespace video +/** + * A map that pairs each of the joints to its corresponding motor. + */ +constexpr frozen::unordered_map + JOINT_MOTOR_MAP{{robot::types::jointid_t::armBase, robot::types::motorid_t::armBase}, + {robot::types::jointid_t::shoulder, robot::types::motorid_t::shoulder}, + {robot::types::jointid_t::elbow, robot::types::motorid_t::elbow}, + {robot::types::jointid_t::forearm, robot::types::motorid_t::forearm}, + {robot::types::jointid_t::wrist, robot::types::motorid_t::wrist}, + {robot::types::jointid_t::hand, robot::types::motorid_t::hand}, + {robot::types::jointid_t::activeSuspension, + robot::types::motorid_t::activeSuspension}}; + // Arm inverse kinematics namespace arm { +/** + * Maximum commanded end-effector velocity, in m/s + */ +constexpr double MAX_EE_VEL = 0.3; constexpr double IK_SOLVER_THRESH = 0.001; constexpr int IK_SOLVER_MAX_ITER = 50; /** - * The motors used in IK. The ordering in this array is the canonical ordering of these motors - * for IK purposes. + * The joints corresponding to the motors used for IK in the arm. The ordering in this array is + * the canonical ordering of these joints for IK purposes. + */ +constexpr std::array IK_MOTOR_JOINTS = { + robot::types::jointid_t::shoulder, robot::types::jointid_t::elbow}; + +/** + * The motors used in IK. The i-th element in this array corresponds to the joint in the i-th + * element of `IK_MOTOR_JOINTS` */ -constexpr std::array IK_MOTORS = { - robot::types::motorid_t::shoulder, robot::types::motorid_t::elbow}; +constexpr std::array IK_MOTORS = ([]() constexpr { + std::array ret{}; + for (size_t i = 0; i < IK_MOTOR_JOINTS.size(); i++) { + ret[i] = JOINT_MOTOR_MAP.at(IK_MOTOR_JOINTS[i]); + } + return ret; +})(); /** - * Map from motor ids to min and max joint limits + * Map from motor ids to min and max joint limits in millidegrees */ constexpr frozen::unordered_map, IK_MOTORS.size()> JOINT_LIMITS{{robot::types::motorid_t::shoulder, {18200, 152500}}, {robot::types::motorid_t::elbow, {-169100, 0}}}; /** - * Map from motor ids to segment length + * Map from motor ids to segment length in meters */ constexpr frozen::unordered_map SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.3848608}, diff --git a/src/Globals.cpp b/src/Globals.cpp index 39bdd1905..81e86aa62 100644 --- a/src/Globals.cpp +++ b/src/Globals.cpp @@ -42,4 +42,5 @@ const kinematics::PlanarArmKinematics planarArmKinematics(getSegLens(), getJointLimits(true), getJointLimits(false), IK_SOLVER_THRESH, IK_SOLVER_MAX_ITER); control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics); +std::atomic armIKEnabled = false; } // namespace Globals diff --git a/src/Globals.h b/src/Globals.h index ad2708bb9..69fe74b60 100644 --- a/src/Globals.h +++ b/src/Globals.h @@ -29,4 +29,5 @@ extern std::atomic AUTONOMOUS; extern robot::types::mountedperipheral_t mountedPeripheral; extern const kinematics::PlanarArmKinematics<2> planarArmKinematics; extern control::PlanarArmController<2> planarArmController; +extern std::atomic armIKEnabled; } // namespace Globals diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index 58460ad2e..b90d1207b 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -36,6 +36,7 @@ const std::chrono::milliseconds TELEM_REPORT_PERIOD = 100ms; constexpr const char* EMERGENCY_STOP_REQ_TYPE = "emergencyStopRequest"; constexpr const char* OPERATION_MODE_REQ_TYPE = "operationModeRequest"; constexpr const char* DRIVE_REQ_TYPE = "driveRequest"; +constexpr const char* ARM_IK_ENABLED_TYPE = "setArmIKEnabled"; constexpr const char* MOTOR_POWER_REQ_TYPE = "motorPowerRequest"; constexpr const char* JOINT_POWER_REQ_TYPE = "jointPowerRequest"; constexpr const char* MOTOR_POSITION_REQ_TYPE = "motorPositionRequest"; @@ -82,6 +83,7 @@ void MissionControlProtocol::handleEmergencyStopRequest(const json& j) { } // TODO: reinit motors Globals::E_STOP = stop; + Globals::armIKEnabled = false; } static bool validateOperationModeRequest(const json& j) { @@ -107,6 +109,10 @@ static bool validateDriveRequest(const json& j) { util::hasKey(j, "steer") && util::validateRange(j, "steer", -1, 1); } +static bool validateArmIKEnable(const json& j) { + return util::validateKey(j, "enabled", val_t::boolean); +} + void MissionControlProtocol::handleDriveRequest(const json& j) { // TODO: ignore this message if we are in autonomous mode. // fit straight and steer to unit circle; i.e. if || > 1, scale each @@ -121,6 +127,33 @@ void MissionControlProtocol::handleDriveRequest(const json& j) { this->setRequestedCmdVel(dtheta, dx); } +void MissionControlProtocol::handleSetArmIKEnabled(const json& j) { + bool enabled = j["enabled"]; + if (enabled) { + Globals::armIKEnabled = false; + if (_arm_ik_repeat_thread.joinable()) { + _arm_ik_repeat_thread.join(); + } + navtypes::Vectord armJointPositions; + for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) { + auto positionDataPoint = robot::getMotorPos(Constants::arm::IK_MOTORS[i]); + assert(positionDataPoint.isValid()); // crash if data is not valid + double position = static_cast(positionDataPoint.getData()); + position *= M_PI / 180.0 / 1000.0; + armJointPositions(i) = position; + } + Globals::planarArmController.set_setpoint(armJointPositions); + Globals::armIKEnabled = true; + _arm_ik_repeat_thread = + std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this); + } else { + Globals::armIKEnabled = false; + if (_arm_ik_repeat_thread.joinable()) { + _arm_ik_repeat_thread.join(); + } + } +} + void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) { { std::lock_guard power_lock(this->_joint_power_mutex); @@ -132,7 +165,8 @@ void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) { static bool validateJoint(const json& j) { return util::validateKey(j, "joint", val_t::string) && util::validateOneOf(j, "joint", - {"armBase", "shoulder", "elbow", "forearm", "wrist", "hand"}); + {"armBase", "shoulder", "elbow", "forearm", "wrist", "hand", + "ikUp", "ikForward"}); } static bool validateJointPowerRequest(const json& j) { @@ -233,6 +267,9 @@ void MissionControlProtocol::sendCameraStreamReport( } void MissionControlProtocol::handleConnection() { + // Turn off inverse kinematics on connection + Globals::armIKEnabled = false; + // TODO: send the actual mounted peripheral, as specified by the command-line parameter json j = {{"type", MOUNTED_PERIPHERAL_REP_TYPE}}; @@ -289,6 +326,8 @@ void MissionControlProtocol::stopAndShutdownPowerRepeat() { this->_joint_repeat_thread.join(); } } + // Turn off inverse kinematics so that IK state will be in sync with mission control + Globals::armIKEnabled = false; } MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server) @@ -313,6 +352,14 @@ MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server) this->addMessageHandler(DRIVE_REQ_TYPE, std::bind(&MissionControlProtocol::handleDriveRequest, this, _1), validateDriveRequest); + this->addMessageHandler( + ARM_IK_ENABLED_TYPE, + std::bind(&MissionControlProtocol::handleSetArmIKEnabled, this, _1), + validateArmIKEnable); + this->addMessageHandler( + ARM_IK_ENABLED_TYPE, + std::bind(&MissionControlProtocol::handleSetArmIKEnabled, this, _1), + validateArmIKEnable); this->addMessageHandler( JOINT_POWER_REQ_TYPE, std::bind(&MissionControlProtocol::handleJointPowerRequest, this, _1), @@ -380,6 +427,30 @@ void MissionControlProtocol::jointPowerRepeatTask() { } } +void MissionControlProtocol::updateArmIKRepeatTask() { + dataclock::time_point next_update_time = dataclock::now(); + while (Globals::armIKEnabled) { + navtypes::Vectord armJointPositions; + for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) { + DataPoint positionDataPoint = + robot::getMotorPos(Constants::arm::IK_MOTORS[i]); + assert(positionDataPoint.isValid()); // crash if data is not valid + double position = static_cast(positionDataPoint.getData()); + position *= M_PI / 180.0 / 1000.0; + armJointPositions(i) = position; + } + navtypes::Vectord targetJointPositions = + Globals::planarArmController.getCommand(dataclock::now(), armJointPositions); + targetJointPositions /= M_PI / 180.0 / 1000.0; // convert from radians to millidegrees + for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) { + robot::setMotorPos(Constants::arm::IK_MOTORS[i], + static_cast(targetJointPositions(i))); + } + next_update_time += Constants::ARM_IK_UPDATE_PERIOD; + std::this_thread::sleep_until(next_update_time); + } +} + void MissionControlProtocol::telemReportTask() { dataclock::time_point pt = dataclock::now(); diff --git a/src/network/MissionControlProtocol.h b/src/network/MissionControlProtocol.h index bec9e2ba9..d9748a6e1 100644 --- a/src/network/MissionControlProtocol.h +++ b/src/network/MissionControlProtocol.h @@ -34,6 +34,7 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta private: void videoStreamTask(); void jointPowerRepeatTask(); + void updateArmIKRepeatTask(); void telemReportTask(); SingleClientWSServer& _server; std::shared_mutex _stream_mutex; @@ -55,16 +56,17 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta std::mutex _joint_repeat_thread_mutex; std::thread _joint_repeat_thread; std::condition_variable _power_repeat_cv; + std::thread _arm_ik_repeat_thread; void handleEmergencyStopRequest(const json& j); void handleOperationModeRequest(const json& j); void handleCameraStreamOpenRequest(const json& j); void handleCameraStreamCloseRequest(const json& j); void handleJointPowerRequest(const json& j); void handleDriveRequest(const json& j); + void handleSetArmIKEnabled(const json& j); void sendCameraStreamReport(const CameraID& cam, const std::vector>& videoData); void sendJointPositionReport(const std::string& jointName, int32_t position); - void sendCameraStreamReport(const CameraID& cam, const std::string& b64_data); void sendRoverPos(); void handleConnection(); void startPowerRepeat(); diff --git a/src/world_interface/data.cpp b/src/world_interface/data.cpp index a0a276d32..aaabce871 100644 --- a/src/world_interface/data.cpp +++ b/src/world_interface/data.cpp @@ -46,6 +46,10 @@ std::string to_string(robot::types::jointid_t joint) { return "drillArm"; case jointid_t::activeSuspension: return "activeSuspension"; + case jointid_t::ikUp: + return "ikUp"; + case jointid_t::ikForward: + return "ikForward"; default: // should never happen return ""; diff --git a/src/world_interface/data.h b/src/world_interface/data.h index 2c2fb9f1d..ffb33ef26 100644 --- a/src/world_interface/data.h +++ b/src/world_interface/data.h @@ -83,12 +83,15 @@ enum class jointid_t { wrist, hand, drill_arm, - activeSuspension + activeSuspension, + ikForward, + ikUp }; constexpr auto all_jointid_t = frozen::make_unordered_set( {jointid_t::armBase, jointid_t::shoulder, jointid_t::elbow, jointid_t::forearm, - jointid_t::wrist, jointid_t::hand, jointid_t::drill_arm, jointid_t::activeSuspension}); + jointid_t::wrist, jointid_t::hand, jointid_t::drill_arm, jointid_t::activeSuspension, + jointid_t::ikForward, jointid_t::ikUp}); constexpr auto name_to_jointid = frozen::make_unordered_map( {{"armBase", jointid_t::armBase}, @@ -98,7 +101,9 @@ constexpr auto name_to_jointid = frozen::make_unordered_map jointMotorMap{ - {robot::types::jointid_t::armBase, robot::types::motorid_t::armBase}, - {robot::types::jointid_t::shoulder, robot::types::motorid_t::shoulder}, - {robot::types::jointid_t::elbow, robot::types::motorid_t::elbow}, - {robot::types::jointid_t::forearm, robot::types::motorid_t::forearm}, - {robot::types::jointid_t::wrist, robot::types::motorid_t::wrist}, - {robot::types::jointid_t::hand, robot::types::motorid_t::hand}, - {robot::types::jointid_t::activeSuspension, robot::types::motorid_t::activeSuspension}}; std::unordered_map jointPowerValues{}; std::mutex jointPowerValuesMutex; @@ -106,8 +98,8 @@ void setJointPower(robot::types::jointid_t joint, double power) { void setJointPos(robot::types::jointid_t joint, int32_t targetPos) { using robot::types::jointid_t; - if (jointMotorMap.find(joint) != jointMotorMap.end()) { - setMotorPos(jointMotorMap.at(joint), targetPos); + if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) { + setMotorPos(Constants::JOINT_MOTOR_MAP.at(joint), targetPos); } // FIXME: need to do some extra control (probably implementing our own PID control) for the // differential position, since the potentiometers are giving us joint position instead of @@ -129,13 +121,13 @@ void setJointPos(robot::types::jointid_t joint, int32_t targetPos) { // FIXME: this should ideally never happen, but we don't have support for all joints // yet because we don't know anything about the drill arm (and need to do extra work // for the differential) - log(LOG_WARN, "setJointPower called for currently unsupported joint %s\n", + log(LOG_WARN, "setJointPos called for currently unsupported joint %s\n", util::to_string(joint).c_str()); } } types::DataPoint getJointPos(robot::types::jointid_t joint) { - if (jointMotorMap.find(joint) != jointMotorMap.end()) { - return getMotorPos(jointMotorMap.at(joint)); + if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) { + return getMotorPos(Constants::JOINT_MOTOR_MAP.at(joint)); } // FIXME: need to do some extra work for differential - we will have to figure out which // motor boards the potentiometers are plugged into and query those for "motor position" @@ -143,7 +135,7 @@ types::DataPoint getJointPos(robot::types::jointid_t joint) { // FIXME: this should ideally never happen, but we don't have support for all joints // yet because we don't know anything about the drill arm (and need to do extra work // for the differential) - log(LOG_WARN, "getJointPower called for currently unsupported joint %s\n", + log(LOG_WARN, "getJointPos called for currently unsupported joint %s\n", util::to_string(joint).c_str()); return {}; } @@ -170,9 +162,23 @@ double getJointPowerValue(types::jointid_t joint) { void setJointMotorPower(robot::types::jointid_t joint, double power) { using robot::types::jointid_t; - - if (jointMotorMap.find(joint) != jointMotorMap.end()) { - setMotorPower(jointMotorMap.at(joint), power); + if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) { + bool isIKMotor = std::find(Constants::arm::IK_MOTOR_JOINTS.begin(), + Constants::arm::IK_MOTOR_JOINTS.end(), + joint) != Constants::arm::IK_MOTOR_JOINTS.end(); + if (!Globals::armIKEnabled || !isIKMotor) { + setMotorPower(Constants::JOINT_MOTOR_MAP.at(joint), power); + } + } else if (joint == jointid_t::ikForward || joint == jointid_t::ikUp) { + if (Globals::armIKEnabled) { + if (joint == jointid_t::ikForward) { + Globals::planarArmController.set_x_vel(dataclock::now(), + power * Constants::arm::MAX_EE_VEL); + } else { + Globals::planarArmController.set_y_vel(dataclock::now(), + power * Constants::arm::MAX_EE_VEL); + } + } } else { log(LOG_WARN, "setJointPower called for currently unsupported joint %s\n", util::to_string(joint).c_str()); From 988c1276700b70f8d3e8aadb637f70d351e61828 Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Thu, 8 Jun 2023 13:02:12 -0700 Subject: [PATCH 2/7] Set ik speed (#256) --- src/Constants.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Constants.h b/src/Constants.h index 621d71a4c..27aae6327 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -150,7 +150,7 @@ namespace arm { /** * Maximum commanded end-effector velocity, in m/s */ -constexpr double MAX_EE_VEL = 0.3; +constexpr double MAX_EE_VEL = 0.1; constexpr double IK_SOLVER_THRESH = 0.001; constexpr int IK_SOLVER_MAX_ITER = 50; From 86cbf8a1f368357593569894ebfac7a29e078369 Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Fri, 9 Jun 2023 12:57:01 -0700 Subject: [PATCH 3/7] Cleanup for joint code (#257) * Cleanup for joint code * Address review comment --- src/network/MissionControlProtocol.cpp | 42 ++++++++----------- src/world_interface/data.cpp | 2 - src/world_interface/data.h | 6 +-- .../kinematic_common_interface.cpp | 23 ++++++---- src/world_interface/world_interface.h | 34 +++++++++++++++ 5 files changed, 70 insertions(+), 37 deletions(-) diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index b90d1207b..b40884d60 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -134,15 +134,13 @@ void MissionControlProtocol::handleSetArmIKEnabled(const json& j) { if (_arm_ik_repeat_thread.joinable()) { _arm_ik_repeat_thread.join(); } - navtypes::Vectord armJointPositions; - for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) { - auto positionDataPoint = robot::getMotorPos(Constants::arm::IK_MOTORS[i]); - assert(positionDataPoint.isValid()); // crash if data is not valid - double position = static_cast(positionDataPoint.getData()); - position *= M_PI / 180.0 / 1000.0; - armJointPositions(i) = position; - } - Globals::planarArmController.set_setpoint(armJointPositions); + DataPoint> armJointPositions = + robot::getMotorPositionsRad(Constants::arm::IK_MOTORS); + // TODO: there should be a better way of handling invalid data than crashing. + // It should somehow just not enable IK, but then it needs to communicate back to MC + // that IK wasn't enabled? + assert(armJointPositions.isValid()); + Globals::planarArmController.set_setpoint(armJointPositions.getData()); Globals::armIKEnabled = true; _arm_ik_repeat_thread = std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this); @@ -430,21 +428,17 @@ void MissionControlProtocol::jointPowerRepeatTask() { void MissionControlProtocol::updateArmIKRepeatTask() { dataclock::time_point next_update_time = dataclock::now(); while (Globals::armIKEnabled) { - navtypes::Vectord armJointPositions; - for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) { - DataPoint positionDataPoint = - robot::getMotorPos(Constants::arm::IK_MOTORS[i]); - assert(positionDataPoint.isValid()); // crash if data is not valid - double position = static_cast(positionDataPoint.getData()); - position *= M_PI / 180.0 / 1000.0; - armJointPositions(i) = position; - } - navtypes::Vectord targetJointPositions = - Globals::planarArmController.getCommand(dataclock::now(), armJointPositions); - targetJointPositions /= M_PI / 180.0 / 1000.0; // convert from radians to millidegrees - for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) { - robot::setMotorPos(Constants::arm::IK_MOTORS[i], - static_cast(targetJointPositions(i))); + DataPoint> armJointPositions = + robot::getMotorPositionsRad(Constants::arm::IK_MOTORS); + if (armJointPositions.isValid()) { + navtypes::Vectord targetJointPositions = + Globals::planarArmController.getCommand(dataclock::now(), armJointPositions); + targetJointPositions /= + M_PI / 180.0 / 1000.0; // convert from radians to millidegrees + for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) { + robot::setMotorPos(Constants::arm::IK_MOTORS[i], + static_cast(targetJointPositions(i))); + } } next_update_time += Constants::ARM_IK_UPDATE_PERIOD; std::this_thread::sleep_until(next_update_time); diff --git a/src/world_interface/data.cpp b/src/world_interface/data.cpp index aaabce871..efc47856f 100644 --- a/src/world_interface/data.cpp +++ b/src/world_interface/data.cpp @@ -42,8 +42,6 @@ std::string to_string(robot::types::jointid_t joint) { return "hand"; case jointid_t::wrist: return "wrist"; - case jointid_t::drill_arm: - return "drillArm"; case jointid_t::activeSuspension: return "activeSuspension"; case jointid_t::ikUp: diff --git a/src/world_interface/data.h b/src/world_interface/data.h index ffb33ef26..2f4784d24 100644 --- a/src/world_interface/data.h +++ b/src/world_interface/data.h @@ -82,7 +82,6 @@ enum class jointid_t { forearm, wrist, hand, - drill_arm, activeSuspension, ikForward, ikUp @@ -90,8 +89,8 @@ enum class jointid_t { constexpr auto all_jointid_t = frozen::make_unordered_set( {jointid_t::armBase, jointid_t::shoulder, jointid_t::elbow, jointid_t::forearm, - jointid_t::wrist, jointid_t::hand, jointid_t::drill_arm, jointid_t::activeSuspension, - jointid_t::ikForward, jointid_t::ikUp}); + jointid_t::wrist, jointid_t::hand, jointid_t::activeSuspension, jointid_t::ikForward, + jointid_t::ikUp}); constexpr auto name_to_jointid = frozen::make_unordered_map( {{"armBase", jointid_t::armBase}, @@ -100,7 +99,6 @@ constexpr auto name_to_jointid = frozen::make_unordered_map getJointPos(robot::types::jointid_t joint) { if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) { return getMotorPos(Constants::JOINT_MOTOR_MAP.at(joint)); - } - // FIXME: need to do some extra work for differential - we will have to figure out which - // motor boards the potentiometers are plugged into and query those for "motor position" - else { - // FIXME: this should ideally never happen, but we don't have support for all joints - // yet because we don't know anything about the drill arm (and need to do extra work - // for the differential) + } else if (joint == jointid_t::ikForward || joint == jointid_t::ikUp) { + DataPoint> armJointPositions = + robot::getMotorPositionsRad(Constants::arm::IK_MOTORS); + if (armJointPositions.isValid()) { + Eigen::Vector2d eePos = + Globals::planarArmKinematics.jointPosToEEPos(armJointPositions.getData()); + Eigen::Vector2i eePosInt = (1000 * eePos).array().round().cast(); + return DataPoint(armJointPositions.getTime(), + joint == jointid_t::ikForward ? eePosInt.x() + : eePosInt.y()); + } else { + return {}; + } + } else { + // This should ideally never happen, but may if we haven't implemented a joint yet. log(LOG_WARN, "getJointPos called for currently unsupported joint %s\n", util::to_string(joint).c_str()); return {}; diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h index 02fa0d3fd..83dcded77 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -7,6 +7,7 @@ #include "data.h" #include "motor/base_motor.h" +#include #include #include @@ -276,6 +277,39 @@ void setJointPos(robot::types::jointid_t joint, int32_t targetPos); * in millidegrees. */ types::DataPoint getJointPos(robot::types::jointid_t joint); + +/** + * @brief Get the positions in radians of multiple motors at the same time. + * + * This is useful for kinematics classes which expect motor positions as a vector. + * + * @tparam N Number of motors. + * @param motors The motors to get the positions of. + * @return types::DataPoint> The joint positions. This will be an empty + * data point if any of the requested motors do not have any associated data. The timestamp of + * this data point is the oldest timestamp of the requested motors. + */ +template +types::DataPoint> +getMotorPositionsRad(const std::array& motors) { + navtypes::Vectord motorPositions; + std::optional timestamp; + for (size_t i = 0; i < motors.size(); i++) { + types::DataPoint positionDataPoint = robot::getMotorPos(motors[i]); + if (positionDataPoint.isValid()) { + if (!timestamp.has_value() || timestamp > positionDataPoint.getTime()) { + timestamp = positionDataPoint.getTime(); + } + double position = static_cast(positionDataPoint.getData()); + position *= M_PI / 180.0 / 1000.0; + motorPositions(i) = position; + } else { + return {}; + } + } + return types::DataPoint>(timestamp.value(), motorPositions); +} + // TODO: document void zeroJoint(robot::types::jointid_t joint); } // namespace robot From 5ed1e052a500e7587f82e24c884a004982cc88ba Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Wed, 14 Jun 2023 11:46:54 -0700 Subject: [PATCH 4/7] Tune active suspension, fix validator (#259) --- src/network/MissionControlProtocol.cpp | 6 +++--- src/world_interface/real_world_constants.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index b40884d60..53a992761 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -162,9 +162,9 @@ void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) { static bool validateJoint(const json& j) { return util::validateKey(j, "joint", val_t::string) && - util::validateOneOf(j, "joint", - {"armBase", "shoulder", "elbow", "forearm", "wrist", "hand", - "ikUp", "ikForward"}); + std::any_of(all_jointid_t.begin(), all_jointid_t.end(), [&](const auto& joint) { + return j["joint"].get() == util::to_string(joint); + }); } static bool validateJointPowerRequest(const json& j) { diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h index 0d4937e4d..f15373d9c 100644 --- a/src/world_interface/real_world_constants.h +++ b/src/world_interface/real_world_constants.h @@ -121,7 +121,7 @@ constexpr auto positive_pwm_scales = {motorid_t::rearLeftWheel, 0.7}, {motorid_t::rearRightWheel, 0.7}, {motorid_t::hand, -0.75}, - {motorid_t::activeSuspension, 1.0}}); + {motorid_t::activeSuspension, -0.5}}); /** * @brief A mapping of motorids to power scale factors when commanded with negative power. * Negative values mean that the motor is inverted. @@ -137,6 +137,6 @@ constexpr auto negative_pwm_scales = {motorid_t::rearLeftWheel, 0.7}, {motorid_t::rearRightWheel, 0.7}, {motorid_t::hand, -0.75}, - {motorid_t::activeSuspension, 1.0}}); + {motorid_t::activeSuspension, -0.5}}); } // namespace robot From 4cab5f2bfd984e0913d921207fdebb5885d28873 Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Fri, 16 Jun 2023 17:54:34 -0700 Subject: [PATCH 5/7] Tune pot bounds (#260) --- src/world_interface/real_world_constants.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h index f15373d9c..7c3d3a27b 100644 --- a/src/world_interface/real_world_constants.h +++ b/src/world_interface/real_world_constants.h @@ -82,11 +82,11 @@ constexpr auto potMotors = frozen::make_unordered_map( {{motorid_t::armBase, {.adc_lo = 123, .mdeg_lo = -200 * 1000, .adc_hi = 456, .mdeg_hi = 200 * 1000}}, {motorid_t::forearm, - {.adc_lo = 123, .mdeg_lo = -360 * 1000, .adc_hi = 456, .mdeg_hi = 360 * 1000}}, + {.adc_lo = 1208, .mdeg_lo = -180 * 1000, .adc_hi = 841, .mdeg_hi = 180 * 1000}}, {motorid_t::wrist, {.adc_lo = 123, .mdeg_lo = -100 * 1000, .adc_hi = 456, .mdeg_hi = 100 * 1000}}, {motorid_t::activeSuspension, - {.adc_lo = 123, .mdeg_lo = -100 * 1000, .adc_hi = 456, .mdeg_hi = 100 * 1000}}}); + {.adc_lo = 251, .mdeg_lo = -19 * 1000, .adc_hi = 1645, .mdeg_hi = 31 * 1000}}}); /** @brief A mapping of motorids to their corresponding serial number. */ constexpr auto motorSerialIDMap = frozen::make_unordered_map( From 03e0786f6d0c7706afb197bacc88744ade9284a4 Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Fri, 16 Jun 2023 19:06:35 -0700 Subject: [PATCH 6/7] Add general telemetry prompting to FakeCANBoard (#258) * Add general telemetry prompting to fakecanboard * Update deps --- src/CAN/CAN.cpp | 3 +- src/CAN/CANUtils.h | 5 +-- src/CAN/FakeCANBoard.cpp | 76 +++++++++++++++++++++++----------------- src/CMakeLists.txt | 2 +- update_deps.sh | 2 +- 5 files changed, 50 insertions(+), 38 deletions(-) diff --git a/src/CAN/CAN.cpp b/src/CAN/CAN.cpp index 2761293ff..6447b962f 100644 --- a/src/CAN/CAN.cpp +++ b/src/CAN/CAN.cpp @@ -70,7 +70,8 @@ const std::unordered_map telemCodeToTypeMap = { {PACKET_TELEMETRY_GYRO_X, telemtype_t::gyro_x}, {PACKET_TELEMETRY_GYRO_Y, telemtype_t::gyro_y}, {PACKET_TELEMETRY_GYRO_Z, telemtype_t::gyro_z}, - {PACKET_TELEMETRY_LIM_SW_STATE, telemtype_t::limit_switch}}; + {PACKET_TELEMETRY_LIM_SW_STATE, telemtype_t::limit_switch}, + {PACKET_TELEMETRY_ADC_RAW, telemtype_t::adc_raw}}; // the telemetry map will store telemetry code instead of telem enum // this means unrecognized telemetry types won't cause UB diff --git a/src/CAN/CANUtils.h b/src/CAN/CANUtils.h index 045163b93..4be2c9037 100644 --- a/src/CAN/CANUtils.h +++ b/src/CAN/CANUtils.h @@ -48,8 +48,9 @@ enum class telemtype_t { gyro_x = PACKET_TELEMETRY_GYRO_X, gyro_y = PACKET_TELEMETRY_GYRO_Y, gyro_z = PACKET_TELEMETRY_GYRO_Z, - limit_switch = PACKET_TELEMETRY_LIM_SW_STATE - // TODO: add further telemetry types if required + limit_switch = PACKET_TELEMETRY_LIM_SW_STATE, + adc_raw = PACKET_TELEMETRY_ADC_RAW + // add further telemetry types if required }; /** diff --git a/src/CAN/FakeCANBoard.cpp b/src/CAN/FakeCANBoard.cpp index b8a8569fb..23ec9df9e 100644 --- a/src/CAN/FakeCANBoard.cpp +++ b/src/CAN/FakeCANBoard.cpp @@ -27,20 +27,17 @@ enum class TestMode { PIDVel, Encoder, LimitSwitch, - ScienceTelemetry, + Telemetry, ScienceMotors, ScienceServos }; -std::unordered_set modes = {static_cast(TestMode::ModeSet), - static_cast(TestMode::PWM), - static_cast(TestMode::PID), - static_cast(TestMode::Encoder), - static_cast(TestMode::PIDVel), - static_cast(TestMode::LimitSwitch), - static_cast(TestMode::ScienceTelemetry), - static_cast(TestMode::ScienceMotors), - static_cast(TestMode::ScienceServos)}; +std::unordered_set modes = { + static_cast(TestMode::ModeSet), static_cast(TestMode::PWM), + static_cast(TestMode::PID), static_cast(TestMode::Encoder), + static_cast(TestMode::PIDVel), static_cast(TestMode::LimitSwitch), + static_cast(TestMode::Telemetry), static_cast(TestMode::ScienceMotors), + static_cast(TestMode::ScienceServos)}; int prompt(std::string_view message) { std::string str; @@ -72,37 +69,32 @@ int main() { ss << static_cast(TestMode::PID) << " for PID\n"; ss << static_cast(TestMode::Encoder) << " for ENCODER\n"; ss << static_cast(TestMode::LimitSwitch) << " for LIMIT SWITCH\n"; - ss << static_cast(TestMode::ScienceTelemetry) << " for SCIENCE TELEMETRY\n"; + ss << static_cast(TestMode::Telemetry) << " for TELEMETRY\n"; ss << static_cast(TestMode::ScienceMotors) << " for SCIENCE MOTORS\n"; - ss << static_cast(TestMode::ScienceTelemetry) << " for SCIENCE SERVOS\n"; + ss << static_cast(TestMode::ScienceServos) << " for SCIENCE SERVOS\n"; int test_type = prompt(ss.str().c_str()); if (modes.find(test_type) == modes.end()) { std::cout << "Unrecognized response: " << test_type << std::endl; std::exit(1); } TestMode testMode = static_cast(test_type); - int serial = 0; - if (testMode == TestMode::PWM || testMode == TestMode::PID || - testMode == TestMode::Encoder || testMode == TestMode::LimitSwitch) { - serial = prompt("Enter motor serial"); - } bool mode_has_been_set = false; while (true) { if (testMode == TestMode::ModeSet) { - serial = prompt("Enter motor serial"); + int serial = prompt("Enter motor serial"); int mode = prompt("Enter mode (0 for PWM, 1 for PID)"); std::cout << "got " << serial << " and " << mode << std::endl; can::motor::setMotorMode(serial, mode == 0 ? motormode_t::pwm : motormode_t::pid); } else if (testMode == TestMode::PWM) { + int serial = prompt("Enter motor serial"); int pwm = prompt("Enter PWM"); can::motor::setMotorMode(serial, motormode_t::pwm); can::motor::setMotorPower(serial, static_cast(pwm)); } else if (testMode == TestMode::PID) { - // Don't send all five packets at once. On some motor boards, the CAN buffer - // only fits four packets. - + static int serial; if (!mode_has_been_set) { + serial = prompt("Enter motor serial"); // AVR board firmware resets the angle target every time it receives a // mode set packet, so we only want to send this once. // TODO: do we need to set the PPJR? @@ -126,6 +118,8 @@ int main() { static double vel_timeout; if (!mode_has_been_set) { + int serial = prompt("Enter motor serial"); + // set pid mode can::motor::setMotorMode(serial, motormode_t::pid); mode_has_been_set = true; @@ -179,7 +173,10 @@ int main() { motor->setMotorPower(0.0); } } else if (testMode == TestMode::Encoder) { + static int serial; if (!mode_has_been_set) { + serial = prompt("Enter motor serial"); + int sensorType; do { sensorType = @@ -212,7 +209,9 @@ int main() { std::this_thread::sleep_for(20ms); } else if (testMode == TestMode::LimitSwitch) { static bool testLimits = false; + static int serial; if (!mode_has_been_set) { + serial = prompt("Enter motor serial"); testLimits = static_cast(prompt("Set limits? 1=yes,0=no")); if (testLimits) { int lo = prompt("Low position"); @@ -242,28 +241,39 @@ int main() { std::cout << "\33[2K\rEncoder value: " << encoderStr << std::flush; std::this_thread::sleep_for(20ms); } - } else if (testMode == TestMode::ScienceTelemetry) { + } else if (testMode == TestMode::Telemetry) { if (!mode_has_been_set) { - serial = 0x01; - can::deviceid_t id = std::make_pair(can::devicegroup_t::science, serial); + auto group = static_cast(prompt("Enter group code")); + int serial = prompt("Enter serial"); + can::deviceid_t deviceID = std::make_pair(group, serial); + auto telemType = static_cast(prompt("Enter telemetry type")); can::addDeviceTelemetryCallback( - id, can::telemtype_t::limit_switch, + deviceID, telemType, [](can::deviceid_t id, can::telemtype_t telemType, DataPoint data) { - std::cout << "Science: serial=" << std::hex + std::cout << "Telemetry: group=" << std::hex + << static_cast(id.first) << ", serial=" << std::hex << static_cast(id.second) << ", type=" << std::hex << static_cast(telemType) << ", data=" << std::dec << data.getDataOrElse(0) << std::endl; }); + int telemPeriod = prompt("Telemetry timing (ms)"); + bool useTimingPacket = + static_cast(prompt("What telemetry method?\n0 for pull packets\n1 " + "for telemetry timing packet")); + if (useTimingPacket) { + CANPacket packet; + AssembleTelemetryTimingPacket( + &packet, static_cast(deviceID.first), deviceID.second, + static_cast(telemType), telemPeriod); + can::sendCANPacket(packet); + } else { + can::scheduleTelemetryPull(deviceID, telemType, + std::chrono::milliseconds(telemPeriod)); + } mode_has_been_set = true; } - auto reqType = prompt("Enter telemetry type"); - int telemPeriod = prompt("Telemetry timing (ms)"); - CANPacket packet; - AssembleTelemetryTimingPacket(&packet, - static_cast(can::devicegroup_t::science), - serial, reqType, telemPeriod); - can::sendCANPacket(packet); + std::this_thread::sleep_for(1s); } else if (testMode == TestMode::ScienceMotors) { // CAREFUL, there are no safety checks / limit switches // 0: drill up/down (down is positive, ~500 PWM) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 293ac622c..c968f6a60 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -109,7 +109,7 @@ find_package(nlohmann_json 3.2.0 REQUIRED) # Find the CAN library; contains packet and protocol definitions and does not # actually require physical CAN to be present. -find_package(HindsightCAN 1.3.2 REQUIRED) +find_package(HindsightCAN 1.4.0 REQUIRED) find_package(Threads REQUIRED) find_package(OpenCV REQUIRED) diff --git a/update_deps.sh b/update_deps.sh index 41a632aab..2a3fdd0b8 100755 --- a/update_deps.sh +++ b/update_deps.sh @@ -4,5 +4,5 @@ sudo apt update sudo apt install -y build-essential unzip gnupg2 lsb-release git \ cmake libeigen3-dev libopencv-dev libopencv-contrib-dev \ libwebsocketpp-dev libboost-system-dev gpsd gpsd-clients libgps-dev nlohmann-json3-dev \ - catch2 urg-lidar rplidar ublox-linux hindsight-can=1.3.2 frozen libargparse-dev libavutil-dev \ + catch2 urg-lidar rplidar ublox-linux hindsight-can=1.4.0 frozen libargparse-dev libavutil-dev \ libx264-dev From c39dbd41268f7f9eae67d7fa4ef4e392530adc8a Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Wed, 6 Sep 2023 22:59:26 -0700 Subject: [PATCH 7/7] Update camera configs (#263) * Update camera configs * Add comment --- rover-config/50-rover-cameras.rules | 6 +++--- rover-config/rover-config.sh | 2 -- src/world_interface/real_world_interface.cpp | 2 +- 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/rover-config/50-rover-cameras.rules b/rover-config/50-rover-cameras.rules index 1f88fae13..6911955cb 100644 --- a/rover-config/50-rover-cameras.rules +++ b/rover-config/50-rover-cameras.rules @@ -7,10 +7,10 @@ # Install this in /etc/udev/rules.d # Mast camera -SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", KERNELS=="1-2.3", SYMLINK+="video20" +SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", KERNELS=="1-2.2", SYMLINK+="video20" # Forearm camera -SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", KERNELS=="1-2.1", SYMLINK+="video30" +# SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", KERNELS=="1-2.1", SYMLINK+="video30" # Hand camera -SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", SYMLINK+="video40" +SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", KERNELS=="1-2.1", SYMLINK+="video40" diff --git a/rover-config/rover-config.sh b/rover-config/rover-config.sh index dbcc23c11..db43287f9 100755 --- a/rover-config/rover-config.sh +++ b/rover-config/rover-config.sh @@ -1,7 +1,5 @@ #!/usr/bin/env bash -cp -r ./.motion/ /home/$USER sudo cp ./50-rover-cameras.rules /etc/udev/rules.d/ -sudo cp ./50-usb-hokuyo-lidar.rules /etc/udev/rules.d/ # Reloads udev rules instead of having to reboot sudo udevadm control --reload-rules diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index ade8610a7..ed181762a 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -129,7 +129,7 @@ void openCamera(CameraID camID, const char* cameraPath) { } void setupCameras() { - // openCamera(Constants::AR_CAMERA_ID, Constants::AR_CAMERA_CONFIG_PATH); + openCamera(Constants::AR_CAMERA_ID, Constants::AR_CAMERA_CONFIG_PATH); openCamera(Constants::HAND_CAMERA_ID, Constants::HAND_CAMERA_CONFIG_PATH); } } // namespace