diff --git a/README.md b/README.md
index ce80953..8b95641 100644
--- a/README.md
+++ b/README.md
@@ -86,9 +86,9 @@ up the control loop set:
 
 ```cpp
 kodlab::mjbots::ControlLoopOptions options;
-options.send_pd_commands = true;
+options.use_pd_commands = true;
 ```
-By setting `send_pd_commands` to true the robot will now send pd scales and set points to the
+By setting `use_pd_commands` to true the robot will now send pd scales and set points to the
 moteus. This will slow down the communication with the moteus, so only use this option if you
 are actually going to use the pd commands.
 
diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h
index ce8eea3..fed414c 100644
--- a/include/kodlab_mjbots_sdk/joint_moteus.h
+++ b/include/kodlab_mjbots_sdk/joint_moteus.h
@@ -16,24 +16,12 @@
 
 namespace kodlab{
 namespace mjbots{
+
 /**
- * @brief struct for storing or passing moteus joint configurations
+ * @brief Forward definition of struct for storing moteus joint configurations
  * 
  */
-struct MoteusJointConfig{
-    int can_id;
-    int can_bus;
-    std::string name = "";
-    int direction = 1; 
-    float zero_offset = 0;
-    float gear_ratio = 1.0;
-    float max_torque = std::numeric_limits<float>::infinity();
-    float pos_min = -std::numeric_limits<float>::infinity();
-    float pos_max = std::numeric_limits<float>::infinity();
-    float moteus_kp = 0;
-    float moteus_kd = 0;
-    float soft_start_duration_ms = 1;
-};
+struct MoteusJointConfig;
 
 /**         
  * @brief A JointBase derived class that encapsulates parameters and functions of 
@@ -55,9 +43,10 @@ class JointMoteus: public JointBase
          * @param max_torque    /// Maximum torque limit of the joint [N m] (Default:inf)
          * @param pos_min       /// Minimum joint pose limit before taking protective measures such as torque limiting or shut off (Default:-inf)
          * @param pos_max       /// Maximum joint pose limit before taking protective measures such as torque limiting or shut off (Default:inf)
+         * @param soft_start_duration_ms /// Duration of torque limit ramp (soft start) in ms
+         * @param query_type    /// QueryCommand struct containing register resolutions
          * @param moteus_kp     /// Value of kp set on moteus in units of N m/rev
          * @param moteus_kd     /// Value of kd set on moteus in units of N m s/rev
-         * @param soft_start_duration_ms /// Duration of torque limit ramp (soft start) in ms
          */
         JointMoteus(
             std::string name,
@@ -70,13 +59,10 @@ class JointMoteus: public JointBase
             float pos_min = -std::numeric_limits<float>::infinity(),
             float pos_max = std::numeric_limits<float>::infinity(),
             float soft_start_duration_ms = 1,
+            ::mjbots::moteus::QueryCommand query_type = kDefaultQuery,
             float moteus_kp = 0,
-            float moteus_kd = 0)
-            : JointBase(name, direction, zero_offset, gear_ratio, max_torque, pos_min, pos_max, soft_start_duration_ms),
-              can_bus_(can_bus),
-              can_id_(can_id),
-              moteus_kp_(moteus_kp),
-              moteus_kd_(moteus_kd)  {}
+            float moteus_kd = 0
+        );
 
         /**
          * @brief Construct a new Joint Moteus object without name
@@ -89,9 +75,10 @@ class JointMoteus: public JointBase
          * @param max_torque    /// Maximum torque limit of the joint [N m] (Default:inf)
          * @param pos_min       /// Minimum joint pose limit before taking protective measures such as torque limiting or shut off (Default:-inf)
          * @param pos_max       /// Maximum joint pose limit before taking protective measures such as torque limiting or shut off (Default:inf)
+         * @param soft_start_duration_ms /// Duration of torque limit ramp (soft start) in ms
+         * @param query_type    /// QueryCommand struct containing register resolutions
          * @param moteus_kp     /// Value of kp set on moteus in units of N m/rev
          * @param moteus_kd     /// Value of kd set on moteus in units of N m s/rev
-         * @param soft_start_duration_ms /// Duration of torque limit ramp (soft start) in ms
          */
         JointMoteus(
             int can_id,
@@ -103,34 +90,25 @@ class JointMoteus: public JointBase
             float pos_min = -std::numeric_limits<float>::infinity(),
             float pos_max = std::numeric_limits<float>::infinity(),
             float soft_start_duration_ms = 1,
+            ::mjbots::moteus::QueryCommand query_type = kDefaultQuery,
             float moteus_kp = 0,
-            float moteus_kd = 0)
-            : JointBase("", direction, zero_offset, gear_ratio, max_torque, pos_min, pos_max, soft_start_duration_ms),
-              can_bus_(can_bus),
-              can_id_(can_id),
-              moteus_kp_(moteus_kp),
-              moteus_kd_(moteus_kd) {}
-
+            float moteus_kd = 0
+        );
 
         /**
          * @brief Construct a new Joint Moteus object with a MoteusJointConfig struct
          * 
          * @param config MoteusJointConfig input
          */
-        JointMoteus(MoteusJointConfig config)
-            : JointBase( config.name,
-                         config.direction,
-                         config.zero_offset,
-                         config.gear_ratio,
-                         config.max_torque,
-                         config.pos_min,
-                         config.pos_max,
-                         config.soft_start_duration_ms),
-              can_id_( config.can_id),
-              can_bus_( config.can_bus),
-              moteus_kp_(config.moteus_kp),
-              moteus_kd_(config.moteus_kd){}
+        JointMoteus(MoteusJointConfig config);
         
+        /**
+         * @brief Update the joint of the moteus. Converts rot/s to rad/s and saves other state values.
+         * 
+         * @param reply_message reply message sent by moteus to pi3hat
+         */
+        void UpdateMoteus(::mjbots::moteus::QueryResult reply_message);
+
         /**
          * @brief Destroy the JointMoteus object, virtual to ensure proper 
          * destruction of derived classes
@@ -138,94 +116,186 @@ class JointMoteus: public JointBase
         virtual ~JointMoteus(){}
 
         /**
-         * @brief Update the joint of the moteus. Converts rot/s to rad/s and saves mode
+         * @brief Determines whether the QueryCommand struct ignores position
+         * and velocity when requesting data from the moteus, potentially
+         * characteristic of an open-loop controller
          * 
-         * @param reply_pos position reported by moteus [rot]
-         * @param reply_vel velocity reported by moteus [rot/s]
-         * @param reply_torque torque measured by the moteus [Nm]
-         * @param mode      moteus mode
+         * @return bool 
          */
-        void UpdateMoteus(float reply_pos, float reply_vel, float reply_torque, ::mjbots::moteus::Mode mode ){
-            UpdateState( 2 * M_PI * reply_pos, 2 * M_PI * reply_vel, reply_torque);
-            mode_ = mode;
-        }
+        bool is_open_loop_query();
 
         /**
          * @brief Get the can id 
          * 
          * @return int 
          */
-        int get_can_id() const {return can_id_;}
+        int get_can_id() const;
 
         /**
          * @brief Get the can bus object
          * 
          * @return int 
          */
-        int get_can_bus() const {return can_bus_;}
+        int get_can_bus() const;
+
+        /**
+         * @brief Get the QueryCommand object, which contains resolutions
+         * 
+         * @return ::mjbots::moteus::QueryCommand
+         */
+        [[nodiscard]] ::mjbots::moteus::QueryCommand get_query_command() const;
 
         /**
-         * @brief Get the mode reference 
+         * @brief Get the mode reference
          * 
          * @return const ::mjbots::moteus::Mode& 
          */
-        const ::mjbots::moteus::Mode & get_mode_reference()   const {return mode_; }
+        const ::mjbots::moteus::Mode & get_mode_reference() const;
+
+        /**
+         * @brief Get the current operational mode of the servo
+         * 
+         * @return ::mjbots::moteus::Mode
+         */
+        [[nodiscard]] ::mjbots::moteus::Mode get_mode() const;
+
+        /**
+         * @brief Get the current position of the servo, measured in rotations
+         * of the output shaft. Overrides function in joint_base.h
+         * 
+         * @return float 
+         */
+        float get_position() const override;
+
+        /**
+         * @brief Get the current velocity of the servo, measured in Hz at the 
+         * output shaft. Overrides function in joint_base.h
+         * 
+         * @return float
+         */
+        float get_velocity() const override;
+
+        /**
+         * @brief Get the measured joint torque. Overrides function in
+         * joint_base.h
+         * 
+         * @return float
+         */
+        float get_measured_torque() const override;
+
+        /**
+         * @brief Get the board temperature, measured in degrees Celsius
+         * 
+         * @return float
+         */
+        [[nodiscard]] float get_temperature() const;
+
+        /**
+         * @brief Get the current in the Q phase, measured in Amperes
+         * 
+         * @return float
+         */
+        [[nodiscard]] float get_q_current() const;
+
+        /**
+         * @brief Get the current in the D phase, measured in Amperes
+         * 
+         * @return float
+         */
+        [[nodiscard]] float get_d_current() const;
+
+        /**
+         * @brief Get the input voltage
+         * 
+         * @return float
+         */
+        [[nodiscard]] float get_voltage() const;
+
+        /**
+         * @brief Get the fault code, which will be set if the current
+         * operational mode of the servo is set to 1 (Fault)
+         * 
+         * @return const ::mjbots::moteus::Fault&
+         */
+        const ::mjbots::moteus::Fault & get_fault() const;
 
         /*!
          * @brief accessor for kp_scale
          * @return kp_scale
          */
-        [[nodiscard]] float get_kp_scale() const {
-          if(moteus_kp_ == 0){
-            LOG_WARN("Moteus kp is set to 0, while attempting to send pd commands");
-            return 0;
-          }
-          // Multiply by 2pi to convert kp from radians to revolutions
-          // Divide by gear ratio to get servo kp rather than joint kp
-          const float kp_scale = kp_/moteus_kp_ * 2 * M_PI/gear_ratio_;
-          if(kp_scale > 1){
-            LOG_WARN("kp_scale is greater than 1, will be limited to 1. Either use a lower kp or increase kp in the moteus");
-            LOG_WARN("With the current moteus kp of %.3f, the max value of the joint kp is %.3f", moteus_kp_, kp_/kp_scale);
-          }
-          return kp_scale;
-        }
+        [[nodiscard]] float get_kp_scale() const;
 
         /*!
          * @brief accessor for kd_scale
          * @return kd_scale
          */
-        [[nodiscard]] float get_kd_scale() const {
-          if(moteus_kd_ == 0){
-            LOG_WARN("Moteus kd is set to 0, while attempting to send pd commands");
-            return 0;
-          }
-          const float kd_scale = kd_/moteus_kd_ * 2 * M_PI/gear_ratio_;
-          if(kd_scale > 1){
-            LOG_WARN("kd_scale is greater than 1, will be limited to 1. Either use a lower kd or increase kd in the moteus");
-            LOG_WARN("With the current moteus kd of %.3f, the max value of the joint kd is %.3f", moteus_kp_, kd_/kd_scale);
-          }
-          return kd_scale;
-        }
+        [[nodiscard]] float get_kd_scale() const;
 
         /*!
          * @brief accessor for the moteus position target
          * @return the moteus position target in units of revolutions
          */
-        [[nodiscard]] float get_moteus_position_target()const{return (position_target_ + zero_offset_) * gear_ratio_/direction_/2/M_PI;}
+        [[nodiscard]] float get_moteus_position_target()const;
 
         /*!
          * @brief accessor for the moteus velocity target
          * @return the moteus velocity target in units of revolutions/s
          */
-        [[nodiscard]] float get_moteus_velocity_target()const{return (velocity_target_) * gear_ratio_/direction_/2/M_PI;}
+        [[nodiscard]] float get_moteus_velocity_target()const;
+
+        /**
+         * @brief Define a default resolution struct
+         */
+        static const ::mjbots::moteus::QueryCommand kDefaultQuery;
 
+        /**
+         * @brief Define a resolution struct that includes torques
+         */
+        static const ::mjbots::moteus::QueryCommand kTorqueQuery;
+
+        /**
+         * @brief Define a resolution struct that adds fields commonly used in
+         * debugging (board temperature and fault code) to kTorqueQuery
+         */
+        static const ::mjbots::moteus::QueryCommand kDebugQuery;
+
+        /**
+         * @brief Define a resolution struct that includes all useful registers
+         */
+        static const ::mjbots::moteus::QueryCommand kComprehensiveQuery;
 
     private:
         int can_id_;   /// the can id of this joint's moteus
         int can_bus_;  /// the can bus the moteus communicates on
+        ::mjbots::moteus::QueryCommand query_type_;
         ::mjbots::moteus::Mode mode_ = ::mjbots::moteus::Mode::kStopped; /// joint's moteus mode
         float moteus_kp_ = 0;
         float moteus_kd_ = 0;
+        float temperature_ = 0;
+        float q_current_ = 0;
+        float d_current_ = 0;
+        float voltage_ = 0;
+        ::mjbots::moteus::Fault fault_ = ::mjbots::moteus::Fault::kSuccess; // fault mode
+};
+
+/**
+ * @brief struct for storing or passing moteus joint configurations
+ * 
+ */
+struct MoteusJointConfig{
+    int can_id;
+    int can_bus;
+    ::mjbots::moteus::QueryCommand query_type = JointMoteus::kDefaultQuery;
+    std::string name = "";
+    int direction = 1; 
+    float zero_offset = 0;
+    float gear_ratio = 1.0;
+    float max_torque = std::numeric_limits<float>::infinity();
+    float pos_min = -std::numeric_limits<float>::infinity();
+    float pos_max = std::numeric_limits<float>::infinity();
+    float moteus_kp = 0;
+    float moteus_kd = 0;
+    float soft_start_duration_ms = 1;
 };
 }//namespace mjbots
 }//namespace kodlab
diff --git a/include/kodlab_mjbots_sdk/mjbots_control_loop.h b/include/kodlab_mjbots_sdk/mjbots_control_loop.h
index c28897c..18fa8fc 100644
--- a/include/kodlab_mjbots_sdk/mjbots_control_loop.h
+++ b/include/kodlab_mjbots_sdk/mjbots_control_loop.h
@@ -40,7 +40,8 @@ struct ControlLoopOptions {
                                             /// 400, 200, 100.
   bool dry_run = false;  ///< If true, torques sent to moteus boards will always be zero
   bool print_torques = false;  ///< If true, torque commands will be printed to console
-  bool send_pd_commands = false; ///< If true, the control loop will send pd setpoints & gains in addition to ffwd torque commands
+  bool use_pd_commands = false; ///< If true, the control loop will send pd setpoints & gains in addition to ffwd torque commands
+  bool open_loop = false; ///< If true, open loop control of the robot is permitted
 };
 
 /*!
@@ -167,7 +168,7 @@ class MjbotsControlLoop : public AbstractRealtimeObject {
 
 template<class log_type, class input_type, class robot_type>
 MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::vector<kodlab::mjbots::JointMoteus> joints, const ControlLoopOptions &options)
-  : MjbotsControlLoop<log_type, input_type,robot_type>( make_share_vector(joints), options){}
+  : MjbotsControlLoop<log_type, input_type,robot_type>(make_share_vector(joints), options){}
 
 template<class log_type, class input_type, class robot_type>
 MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(
@@ -190,6 +191,13 @@ MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::shar
     joints_moteus.push_back(
         std::dynamic_pointer_cast<kodlab::mjbots::JointMoteus>(j));
   }
+  // Open loop check
+  for(const auto & j : joints_moteus){
+      if(!options.open_loop && j->is_open_loop_query()){
+        LOG_FATAL("ControlLoopOptions has open_loop = false, but joint moteus QueryCommand struct does not query position and/or velocity from moteus");
+        kodlab::ActivateCtrlC();
+      }
+  }
   // Initialize mjbots_interface
   mjbots_interface_ = std::make_shared<kodlab::mjbots::MjbotsHardwareInterface>(
       std::move(joints_moteus), options.realtime_params,
@@ -197,7 +205,8 @@ MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::shar
       robot_->GetIMUDataSharedPtr(), options.imu_world_offset_deg,
       options.dry_run,
       options.print_torques,
-      options.send_pd_commands);
+      options.use_pd_commands
+  );
   num_joints_ = robot_->joints.size();
   SetupOptions(options);
 }
diff --git a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h
index 27946fd..843c9ac 100644
--- a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h
+++ b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h
@@ -49,7 +49,7 @@ class MjbotsHardwareInterface  {
    * @param imu_world_offset_deg [Optional] IMU orientation offset. Useful for re-orienting gravity, etc.
    * @param dry_run if true, sends zero-torques to Moteus controllers
    * @param print_torques if true, prints torque commands
-   * @param send_pd_commands if true, packets to the moteus include pd gains and setpoints
+   * @param use_pd_commands if true, packets to the moteus include pd gains and setpoints
    */
   MjbotsHardwareInterface(std::vector<std::shared_ptr<JointMoteus>> joint_list,
                        const RealtimeParams &realtime_params,
@@ -59,7 +59,7 @@ class MjbotsHardwareInterface  {
                        std::optional<::mjbots::pi3hat::Euler > imu_world_offset_deg = std::nullopt,
                        bool dry_run = false,
                        bool print_torques = false,
-                       bool send_pd_commands = false
+                       bool use_pd_commands = false
                        );
 
   /**
@@ -97,6 +97,12 @@ class MjbotsHardwareInterface  {
    */
   void Shutdown();
 
+  /*!
+    * @brief Get a vector of shared_ptrs to joints 
+    * @return a vector of shared pointers to the desired joints
+    */
+  std::vector<std::shared_ptr<JointMoteus>> GetJoints();
+
   /*!
    * @brief accessor for the joint modes
    * @return the joint modes
@@ -128,7 +134,7 @@ class MjbotsHardwareInterface  {
   u_int64_t cycle_count_ = 0;                        /// Number of cycles/commands sent
   bool dry_run_;                                     ///< dry run active flag
   bool print_torques_;                               ///< print torques active flag
-  bool send_pd_commands_;                            ///< Include pd gains and setpoints in the moteus packet
+  bool use_pd_commands_;                            ///< Include pd gains and setpoints in the moteus packet
 
   std::map<int, int> servo_bus_map_;       /// map from servo id to servo bus
 
diff --git a/include/kodlab_mjbots_sdk/moteus_protocol.h b/include/kodlab_mjbots_sdk/moteus_protocol.h
index 9df9fdb..3fc9ec7 100644
--- a/include/kodlab_mjbots_sdk/moteus_protocol.h
+++ b/include/kodlab_mjbots_sdk/moteus_protocol.h
@@ -117,6 +117,7 @@ enum Register : uint32_t {
 };
 
 enum class Mode {
+  kUnset = -1,
   kStopped = 0,
   kFault = 1,
   kEnabling = 2,
@@ -133,6 +134,33 @@ enum class Mode {
   kNumModes,
 };
 
+enum class Fault {
+  kUnset = -1,
+  kSuccess = 0,
+
+  kDmaStreamTransferError = 1,
+  kDmaStreamFifoError = 2,
+  kUartOverrunError = 3,
+  kUartFramingError = 4,
+  kUartNoiseError = 5,
+  kUartBufferOverrunError = 6,
+  kUartParityError = 7,
+
+  kCalibrationFault = 32,
+  kMotorDriverFault = 33,
+  kOverVoltage = 34,
+  kEncoderFault = 35,
+  kMotorNotConfigured = 36,
+  kPwmCycleOverrun = 37,
+  kOverTemperature = 38,
+  kStartOutsideLimit = 39,
+  kUnderVoltage = 40,
+  kConfigChanged = 41,
+  kThetaInvalid = 42,
+  kPositionInvalid = 43,
+  kDriverEnableFault = 44,
+};
+
 enum class Resolution {
   kInt8,
   kInt16,
@@ -652,7 +680,7 @@ inline void EmitQueryCommand(
 }
 
 struct QueryResult {
-  Mode mode = Mode::kStopped;
+  Mode mode = Mode::kUnset;
   double position = std::numeric_limits<double>::quiet_NaN();
   double velocity = std::numeric_limits<double>::quiet_NaN();
   double torque = std::numeric_limits<double>::quiet_NaN();
@@ -661,7 +689,20 @@ struct QueryResult {
   bool rezero_state = false;
   double voltage = std::numeric_limits<double>::quiet_NaN();
   double temperature = std::numeric_limits<double>::quiet_NaN();
-  int fault = 0;
+  Fault fault = Fault::kUnset;
+
+  bool all_unset() const {
+    return mode == Mode::kUnset &&
+        position == std::numeric_limits<double>::quiet_NaN() &&
+        velocity == std::numeric_limits<double>::quiet_NaN() &&
+        torque == std::numeric_limits<double>::quiet_NaN() &&
+        q_current == std::numeric_limits<double>::quiet_NaN() &&
+        d_current == std::numeric_limits<double>::quiet_NaN() &&
+        rezero_state == false &&
+        voltage == std::numeric_limits<double>::quiet_NaN() &&
+        temperature == std::numeric_limits<double>::quiet_NaN() &&
+        fault == Fault::kUnset;
+  }
 };
 
 inline QueryResult ParseQueryResult(const uint8_t* data, size_t size) {
@@ -710,7 +751,7 @@ inline QueryResult ParseQueryResult(const uint8_t* data, size_t size) {
         break;
       }
       case Register::kFault: {
-        result.fault = parser.ReadInt(res);
+        result.fault = static_cast<Fault>(parser.ReadInt(res));
         break;
       }
       default: {
diff --git a/include/kodlab_mjbots_sdk/robot_base.h b/include/kodlab_mjbots_sdk/robot_base.h
index a48d630..a1eb583 100644
--- a/include/kodlab_mjbots_sdk/robot_base.h
+++ b/include/kodlab_mjbots_sdk/robot_base.h
@@ -167,7 +167,6 @@ namespace kodlab
         /*!
          * @brief Get the vector of shared_ptrs to joints 
          * @note Added getter to public member for interface consistency with subvector getters
-         * @param joint_indices set of desired joint indices std::initializer_list of ints
          * @return a vector shared pointers to the desired joints
          */
         std::vector<std::shared_ptr<JointBase>> GetJoints(){return joints;}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4286649..607bdf5 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -43,6 +43,7 @@ set(SOURCE_LIST "pi3hat.cpp"
                 "robot_base.cpp"
                 "joint_base.cpp"
                 "cartesian_leg.cpp"
+                "joint_moteus.cpp"
         )
 
 # Make an automatic library - will be static or dynamic based on user setting
diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp
new file mode 100644
index 0000000..7f4c4e5
--- /dev/null
+++ b/src/joint_moteus.cpp
@@ -0,0 +1,296 @@
+/**
+ * @file joint_moteus.cpp
+ * @author J. Diego Caporale (jdcap@seas.upenn.edu), Lucien Peach 
+ * (peach@seas.upenn.edu), Shane Rozen-Levy (srozen01@seas.upenn.edu)
+ * @brief Moteus powered joint class implementation
+ * @date 2023-07-13
+ * 
+ * @copyright BSD 3-Clause License, Copyright (c) 2021 The Trustees of the University of Pennsylvania. All Rights Reserved
+ * 
+ */
+
+
+#include "kodlab_mjbots_sdk/joint_moteus.h"
+
+namespace kodlab{
+namespace mjbots{
+
+JointMoteus::JointMoteus(std::string name,
+                         int can_id,
+                         int can_bus,
+                         int direction,
+                         float zero_offset,
+                         float gear_ratio,
+                         float max_torque,
+                         float pos_min,
+                         float pos_max,
+                         float soft_start_duration_ms,
+                         ::mjbots::moteus::QueryCommand kDefaultQuery,
+                         float moteus_kp,
+                         float moteus_kd)
+    : JointBase(name, direction, zero_offset, gear_ratio, max_torque, pos_min, pos_max, soft_start_duration_ms),
+      can_bus_(can_bus),
+      can_id_(can_id),
+      query_type_(kDefaultQuery),
+      moteus_kp_(moteus_kp),
+      moteus_kd_(moteus_kd) {}
+
+JointMoteus::JointMoteus(int can_id,
+                         int can_bus,
+                         int direction,
+                         float zero_offset,
+                         float gear_ratio,
+                         float max_torque,
+                         float pos_min,
+                         float pos_max,
+                         float soft_start_duration_ms,
+                         ::mjbots::moteus::QueryCommand kDefaultQuery,
+                         float moteus_kp,
+                         float moteus_kd)
+    : JointBase("", direction, zero_offset, gear_ratio, max_torque, pos_min, pos_max, soft_start_duration_ms),
+      can_bus_(can_bus),
+      can_id_(can_id),
+      query_type_(kDefaultQuery),
+      moteus_kp_(moteus_kp),
+      moteus_kd_(moteus_kd) {}
+
+JointMoteus::JointMoteus(MoteusJointConfig config)
+    : JointBase(config.name, 
+                config.direction,
+                config.zero_offset,
+                config.gear_ratio,
+                config.max_torque,
+                config.pos_min,
+                config.pos_max,
+                config.soft_start_duration_ms),
+        can_id_(config.can_id),
+        can_bus_(config.can_bus),
+        query_type_(config.query_type),
+        moteus_kp_(config.moteus_kp),
+        moteus_kd_(config.moteus_kd) {}
+        
+void JointMoteus::UpdateMoteus(::mjbots::moteus::QueryResult reply_message){
+                    UpdateState(2 * M_PI * reply_message.position, 2 * M_PI * reply_message.velocity, reply_message.torque);
+                    mode_ = reply_message.mode;
+                    q_current_ = reply_message.q_current;
+                    d_current_ = reply_message.d_current;
+                    voltage_ = reply_message.voltage;
+                    temperature_ = reply_message.temperature;
+                    fault_ = reply_message.fault;
+}
+
+bool JointMoteus::is_open_loop_query() {
+  return (query_type_.position == ::mjbots::moteus::Resolution::kIgnore ||
+    query_type_.velocity == ::mjbots::moteus::Resolution::kIgnore);
+}
+
+int JointMoteus::get_can_id() const {
+  return can_id_;
+}
+
+int JointMoteus::get_can_bus() const {
+  return can_bus_;
+}
+
+::mjbots::moteus::QueryCommand JointMoteus::get_query_command() const {
+  return query_type_;
+}
+
+const ::mjbots::moteus::Mode & JointMoteus::get_mode_reference() const {
+  if(query_type_.mode == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Mode resolution in QueryCommand is set to kIgnore, while attempting to get value");
+  }
+  return mode_;
+}
+
+::mjbots::moteus::Mode JointMoteus::get_mode() const {
+  if(query_type_.mode == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Mode resolution in QueryCommand is set to kIgnore, while attempting to get value");
+  }
+  return mode_;
+}
+
+float JointMoteus::get_position() const {
+  if(query_type_.position == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Position resolution in QueryCommand is set to kIgnore, while attempting to get value");
+    return 0;
+  } else {
+    return position_;
+  }
+}
+
+float JointMoteus::get_velocity() const {
+  if(query_type_.velocity == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Velocity resolution in QueryCommand is set to kIgnore, while attempting to get value");
+    return 0;
+  } else {
+    return velocity_;
+  }
+}
+
+float JointMoteus::get_measured_torque() const {
+  if(query_type_.torque == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Torque resolution in QueryCommand is set to kIgnore, while attempting to get value");
+    return 0;
+  } else {
+    return measured_torque_;
+  }
+}
+
+float JointMoteus::get_temperature() const {
+  if(query_type_.temperature == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Temperature resolution in QueryCommand is set to kIgnore, while attempting to get value");
+    return 0;
+  } else {
+    return temperature_;
+  }
+}
+
+float JointMoteus::get_q_current() const {
+  if(query_type_.q_current == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Q Current resolution in QueryCommand is set to kIgnore, while attempting to get value");
+    return 0;
+  } else {
+    return q_current_;
+  }
+}
+
+float JointMoteus::get_d_current() const {
+  if(query_type_.d_current == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("D Current resolution in QueryCommand is set to kIgnore, while attempting to get value");
+    return 0;
+  } else {
+    return d_current_;
+  }
+}
+
+float JointMoteus::get_voltage() const {
+  if(query_type_.voltage == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Voltage resolution in QueryCommand is set to kIgnore, while attemping to get value");
+    return 0;
+  } else {
+    return voltage_;
+  }
+}
+
+const ::mjbots::moteus::Fault & JointMoteus::get_fault() const {
+  if(query_type_.fault == ::mjbots::moteus::Resolution::kIgnore){
+    LOG_WARN("Fault code resolution in QueryCommand is set to kIgnore, while attempting to get value");
+    return ::mjbots::moteus::Fault::kSuccess;
+  } else {
+    return fault_;
+  }
+}
+
+[[nodiscard]] float JointMoteus::get_kp_scale() const {
+  if(moteus_kp_ == 0){
+    LOG_WARN("Moteus kp is set to 0, while attempting to send pd commands");
+    return 0;
+  }
+  // Multiply by 2pi to convert kp from radians to revolutions
+  // Divide by gear ratio to get servo kp rather than joint kp
+  const float kp_scale = kp_/moteus_kp_ * 2 * M_PI/gear_ratio_;
+  if(kp_scale > 1){
+    LOG_WARN("kp_scale is greater than 1, will be limited to 1. Either use a lower kp or increase kp in the moteus");
+    LOG_WARN("With the current moteus kp of %.3f, the max value of the joint kp is %.3f", moteus_kp_, kp_/kp_scale);
+  }
+  return kp_scale;
+}
+
+[[nodiscard]] float JointMoteus::get_kd_scale() const {
+  if(moteus_kd_ == 0){
+    LOG_WARN("Moteus kd is set to 0, while attempting to send pd commands");
+    return 0;
+  }
+  const float kd_scale = kd_/moteus_kd_ * 2 * M_PI/gear_ratio_;
+  if(kd_scale > 1){
+    LOG_WARN("kd_scale is greater than 1, will be limited to 1. Either use a lower kd or increase kd in the moteus");
+    LOG_WARN("With the current moteus kd of %.3f, the max value of the joint kd is %.3f", moteus_kp_, kd_/kd_scale);
+  }
+  return kd_scale;
+}
+
+[[nodiscard]] float JointMoteus::get_moteus_position_target() const {
+  return (position_target_ + zero_offset_) * gear_ratio_/direction_/2/M_PI;
+}
+
+[[nodiscard]] float JointMoteus::get_moteus_velocity_target() const {
+  return (velocity_target_) * gear_ratio_/direction_/2/M_PI;
+}
+
+/**
+ * @brief Default query defintion which includes mode, position, and velocity
+ * readings. Same resolutions as specified in the QueryCommand struct within
+ * moteus_protocol.h.
+ */
+const ::mjbots::moteus::QueryCommand JointMoteus::kDefaultQuery{
+  ::mjbots::moteus::Resolution::kInt8,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore
+};
+
+/**
+ * @brief Query definition which includes mode, position, velocity, and torque 
+ * readings.
+ */
+const ::mjbots::moteus::QueryCommand JointMoteus::kTorqueQuery{
+  ::mjbots::moteus::Resolution::kInt8,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt32,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore
+};
+
+/**
+ * @brief Query defintion which includes common values used in the debugging
+ * process (board temperature and fault code) along with mode, position,
+ * velocity, and torque.
+ */
+const ::mjbots::moteus::QueryCommand JointMoteus::kDebugQuery{
+  ::mjbots::moteus::Resolution::kInt8,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt32,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt8
+};
+
+/**
+ * @brief Comprehensive query defintion which includes all QueryCommand values
+ * at non-kIgnore resolution, with the exception of rezero_state (as this value
+ * is undocumented and unused). These are mode, position, velocity, torque,
+ * q_current, d_current, rezero_state (kIgnore), voltage, temperature, and 
+ * fault code.
+ */
+const ::mjbots::moteus::QueryCommand JointMoteus::kComprehensiveQuery{
+  ::mjbots::moteus::Resolution::kInt8,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt32,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kIgnore,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt16,
+  ::mjbots::moteus::Resolution::kInt8
+};
+
+} //namespace mjbots
+} //namespace kodlab
+
diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp
index 523ed74..68b112e 100644
--- a/src/mjbots_hardware_interface.cpp
+++ b/src/mjbots_hardware_interface.cpp
@@ -14,19 +14,14 @@
 
 namespace kodlab::mjbots {
 void MjbotsHardwareInterface::InitializeCommand() {
-  for (const auto &joint : joints) {
-    commands_.push_back({});
-    commands_.back().id = joint->get_can_id(); //id
-  }
-
   ::mjbots::moteus::PositionResolution res; // This is just for the command
-  if(send_pd_commands_){
+  if(use_pd_commands_){
     res.position = ::mjbots::moteus::Resolution::kInt16;
     res.velocity = ::mjbots::moteus::Resolution::kInt16;
     res.feedforward_torque = ::mjbots::moteus::Resolution::kInt16;
     res.kp_scale = ::mjbots::moteus::Resolution::kInt16;
     res.kd_scale = ::mjbots::moteus::Resolution::kInt16;
-    res.maximum_torque = ::mjbots::moteus::Resolution::kInt8;
+    res.maximum_torque = ::mjbots::moteus::Resolution::kInt16;
   }else{
     res.position = ::mjbots::moteus::Resolution::kIgnore;
     res.velocity = ::mjbots::moteus::Resolution::kIgnore;
@@ -37,12 +32,13 @@ void MjbotsHardwareInterface::InitializeCommand() {
   }
   res.stop_position = ::mjbots::moteus::Resolution::kIgnore;
   res.watchdog_timeout = ::mjbots::moteus::Resolution::kIgnore;
-  for (auto &cmd : commands_) {
-    cmd.resolution = res;
-    cmd.mode = ::mjbots::moteus::Mode::kStopped;
-    if(send_pd_commands_){
-      cmd.query.torque = ::mjbots::moteus::Resolution::kInt16;
-    }
+
+  for (const auto &joint : joints) {
+    commands_.push_back({});
+    commands_.back().id = joint->get_can_id(); //id
+    commands_.back().query = joint->get_query_command(); //query
+    commands_.back().resolution = res;
+    commands_.back().mode = ::mjbots::moteus::Mode::kStopped;
   }
 }
 
@@ -70,11 +66,11 @@ MjbotsHardwareInterface::MjbotsHardwareInterface(std::vector<std::shared_ptr<Joi
                                                  std::optional<::mjbots::pi3hat::Euler> imu_world_offset_deg,
                                                  bool dry_run,
                                                  bool print_torques,
-                                                 bool send_pd_commands)
+                                                 bool use_pd_commands)
     : imu_data_(imu_data_ptr ? imu_data_ptr : std::make_shared<::kodlab::IMUData<float>>()),
       dry_run_(dry_run),
       print_torques_(print_torques),
-      send_pd_commands_(send_pd_commands)
+      use_pd_commands_(use_pd_commands)
 { 
   LOG_IF_WARN(dry_run_, "\nDRY RUN: NO TORQUES COMMANDED");
   joints = joint_ptrs;
@@ -129,11 +125,10 @@ void MjbotsHardwareInterface::ProcessReply() {
   // Copy results to object so controller can use
   for (auto & joint : joints) {
     const auto servo_reply = Get(replies_, joint->get_can_id());
-    if(std::isnan(servo_reply.position)){
+    if(joint->get_query_command().any_set() && servo_reply.all_unset()){
       std::cout<<"Missing can frame for servo: " << joint->get_can_id()<< std::endl;
     } else{
-      joint->UpdateMoteus(servo_reply.position, servo_reply.velocity,
-                          send_pd_commands_ ? servo_reply.torque : 0,servo_reply.mode);
+      joint->UpdateMoteus(servo_reply);
     }
   }
   imu_data_->Update(*(moteus_data_.attitude));
@@ -144,7 +139,7 @@ void MjbotsHardwareInterface::SendCommand() {
 
   for (int servo = 0; servo < num_joints_; servo++) {// TODO Move to a seperate update method (allow non-ff torque commands)?
     commands_[servo].position.feedforward_torque = (dry_run_ ? 0 : joints[servo]->get_servo_torque());
-    if(send_pd_commands_){
+    if(use_pd_commands_){
       commands_[servo].position.position = joints[servo]->get_moteus_position_target();
       commands_[servo].position.velocity = joints[servo]->get_moteus_velocity_target();
       commands_[servo].position.kp_scale = dry_run_ ? 0 : joints[servo]->get_kp_scale();
@@ -163,6 +158,10 @@ void MjbotsHardwareInterface::SendCommand() {
   moteus_interface_->Cycle(moteus_data_);
 }
 
+std::vector<std::shared_ptr<JointMoteus>> MjbotsHardwareInterface::GetJoints() {
+  return joints;
+}
+
 std::vector<::mjbots::moteus::Mode> MjbotsHardwareInterface::GetJointModes() {
   std::vector<::mjbots::moteus::Mode>modes(modes_.begin(), modes_.end());
   return modes;