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());