From ebd952da4a1cdb72fd85db051fda2a60619b6965 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Fri, 16 Jun 2023 16:18:13 -0400 Subject: [PATCH 01/19] Moteus Registers Debug Options Initial commit for moteus-register-debug-data branch. Introduce additional options to send moteus packages for logging and debugging. --- .../kodlab_mjbots_sdk/mjbots_hardware_interface.h | 15 ++++++++++++++- src/mjbots_hardware_interface.cpp | 13 +++++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h index 27946fd..74bcc62 100644 --- a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h +++ b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h @@ -50,6 +50,11 @@ class MjbotsHardwareInterface { * @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 send_current if true, packets to the moteus include q_current and d_current registers + * @param send_voltage if true, packets to the moteus include voltage register + * @param send_temperature if true, packets to the moteus include temperature register + * @param send_fault if true, packets to the moteus include fault code register + * */ MjbotsHardwareInterface(std::vector> joint_list, const RealtimeParams &realtime_params, @@ -59,7 +64,11 @@ 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 send_pd_commands = false, + bool send_current = false, + bool send_voltage = false, + bool send_temperature = false, + bool send_fault = false ); /** @@ -129,6 +138,10 @@ class MjbotsHardwareInterface { 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 send_current_; ///< Include q_current and d_current registers in the moteus packet + bool send_voltage_; ///< Include voltage register in the moteus packet + bool send_temperature_; ///< Include temperature register in the moteus packet + bool send_fault_; ///< Include fault code register in the moteus packet std::map servo_bus_map_; /// map from servo id to servo bus diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index 523ed74..ab6ab94 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -43,6 +43,19 @@ void MjbotsHardwareInterface::InitializeCommand() { if(send_pd_commands_){ cmd.query.torque = ::mjbots::moteus::Resolution::kInt16; } + if(send_current_){ + cmd.query.q_current = ::mjbots::moteus::Resolution::kInt16; + cmd.query.d_current = ::mjbots::moteus::Resolution::kInt16; + } + if(send_voltage_){ + cmd.query.voltage = ::mjbots::moteus::Resolution::kInt16; + } + if(send_temperature_){ + cmd.query.temperature = ::mjbots::moteus::Resolution::kInt16; + } + if(send_fault_){ + cmd.query.fault = ::mjbots::moteus::Resolution::kInt8; + } } } From 15a876716c336a6a1b0ce316df30c660101d40e9 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Thu, 29 Jun 2023 14:46:35 -0400 Subject: [PATCH 02/19] Moteus Registers Debug Additions Additional ternary options for UpdateMoteus(). Debug register additions in joint_moteus.h. Fault codes added to moteus_protocol.h. --- include/kodlab_mjbots_sdk/joint_moteus.h | 111 ++++++++++++++++++-- include/kodlab_mjbots_sdk/moteus_protocol.h | 30 +++++- src/mjbots_hardware_interface.cpp | 7 +- 3 files changed, 135 insertions(+), 13 deletions(-) diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index 49ca6ef..0e2c5cc 100644 --- a/include/kodlab_mjbots_sdk/joint_moteus.h +++ b/include/kodlab_mjbots_sdk/joint_moteus.h @@ -33,6 +33,10 @@ struct MoteusJointConfig{ float moteus_kp = 0; float moteus_kd = 0; float soft_start_duration_ms = 1; + float temperature = 0; + float q_current = 0; + float d_current = 0; + float voltage = 0; }; /** @@ -58,6 +62,10 @@ class JointMoteus: public JointBase * @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 + * @param temperature /// Value of the board temperature in units of Celsius + * @param q_current /// Value of current in the Q phase in units of Amperes + * @param d_current /// Value of current in the D phase in units of Amperes + * @param voltage /// Value of the current input voltage */ JointMoteus( std::string name, @@ -71,12 +79,20 @@ class JointMoteus: public JointBase float pos_max = std::numeric_limits::infinity(), float soft_start_duration_ms = 1, float moteus_kp = 0, - float moteus_kd = 0) + float moteus_kd = 0, + float temperature = 0, + float q_current = 0, + float d_current = 0, + float voltage = 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) {} + moteus_kd_(moteus_kd), + temperature_(temperature), + q_current_(q_current), + d_current_(d_current), + voltage_(voltage) {} /** * @brief Construct a new Joint Moteus object without name @@ -92,6 +108,10 @@ class JointMoteus: public JointBase * @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 + * @param temperature /// Value of the board temperature in units of Celsius + * @param q_current /// Value of current in the Q phase in units of Amperes + * @param d_current /// Value of current in the D phase in units of Amperes + * @param voltage /// Value of the current input voltage */ JointMoteus( int can_id, @@ -104,12 +124,20 @@ class JointMoteus: public JointBase float pos_max = std::numeric_limits::infinity(), float soft_start_duration_ms = 1, float moteus_kp = 0, - float moteus_kd = 0) + float moteus_kd = 0, + float temperature = 0, + float q_current = 0, + float d_current = 0, + float voltage = 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) {} + moteus_kd_(moteus_kd), + temperature_(temperature), + q_current_(q_current), + d_current_(d_current), + voltage_(voltage) {} /** @@ -129,20 +157,43 @@ class JointMoteus: public JointBase can_id_( config.can_id), can_bus_( config.can_bus), moteus_kp_(config.moteus_kp), - moteus_kd_(config.moteus_kd){} + moteus_kd_(config.moteus_kd), + temperature_(config.temperature), + q_current_(config.q_current), + d_current_(config.d_current), + voltage_(config.voltage) {} /** - * @brief Update the joint of the moteus. Converts rot/s to rad/s and saves mode + * @brief Update the joint of the moteus. Converts rot/s to rad/s and saves registers * * @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 + * @param reply_q_current current in the Q phase reported by moteus [A] + * @param reply_d_current current in the D phase reported by moteus [A] + * @param reply_voltage voltage reported by the moteus [V] + * @param reply_temperature temperature reported by the moteus [C] + * @param fault fault code */ - 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; - } + void UpdateMoteus( + float reply_pos, + float reply_vel, + float reply_torque, + ::mjbots::moteus::Mode mode, + float reply_q_current, + float reply_d_current, + float reply_voltage, + float reply_temperature, + ::mjbots::moteus::Fault fault){ + UpdateState( 2 * M_PI * reply_pos, 2 * M_PI * reply_vel, reply_torque); + mode_ = mode; + q_current_ = reply_q_current; + d_current_ = reply_d_current; + voltage_ = reply_voltage; + temperature_ = reply_temperature; + fault_ = fault; + } /** * @brief Get the can id @@ -165,6 +216,41 @@ class JointMoteus: public JointBase */ const ::mjbots::moteus::Mode & get_mode_reference() const {return mode_; } + /** + * @brief Get the temperature register + * + * @return float + */ + virtual float get_temperature() const {return temperature_;} + + /** + * @brief Get the q_current register + * + * @return float + */ + virtual float get_q_current() const {return q_current_;} + + /** + * @brief Get the d_current register + * + * @return float + */ + virtual float get_d_current() const {return d_current_;} + + /** + * @brief Get the voltage register + * + * @return float + */ + virtual float get_voltage() const {return voltage_;} + + /** + * @brief Get the fault register + * + * @return const ::mjbots::moteus::Fault& + */ + const ::mjbots::moteus::Fault & get_fault() const {return fault_;} + /*! * @brief accessor for kp_scale * @return kp_scale @@ -220,6 +306,11 @@ class JointMoteus: public JointBase ::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 }; }//namespace mjbots }//namespace kodlab diff --git a/include/kodlab_mjbots_sdk/moteus_protocol.h b/include/kodlab_mjbots_sdk/moteus_protocol.h index 9df9fdb..7549cbc 100644 --- a/include/kodlab_mjbots_sdk/moteus_protocol.h +++ b/include/kodlab_mjbots_sdk/moteus_protocol.h @@ -133,6 +133,32 @@ enum class Mode { kNumModes, }; +enum class Fault { + 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, @@ -661,7 +687,7 @@ struct QueryResult { bool rezero_state = false; double voltage = std::numeric_limits::quiet_NaN(); double temperature = std::numeric_limits::quiet_NaN(); - int fault = 0; + Fault fault = Fault::kSuccess; }; inline QueryResult ParseQueryResult(const uint8_t* data, size_t size) { @@ -710,7 +736,7 @@ inline QueryResult ParseQueryResult(const uint8_t* data, size_t size) { break; } case Register::kFault: { - result.fault = parser.ReadInt(res); + result.fault = static_cast(parser.ReadInt(res)); break; } default: { diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index ab6ab94..1de51f1 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -146,7 +146,12 @@ void MjbotsHardwareInterface::ProcessReply() { 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); + send_pd_commands_ ? servo_reply.torque : 0, servo_reply.mode, + send_current_ ? servo_reply.q_current : 0, + send_current_ ? servo_reply.d_current : 0, + send_voltage_ ? servo_reply.voltage : 0, + send_temperature_ ? servo_reply.temperature : 0, + servo_reply.fault); } } imu_data_->Update(*(moteus_data_.attitude)); From 2b974a621548c7519ce9d17b458ceffad1e509b4 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Thu, 29 Jun 2023 19:45:17 -0400 Subject: [PATCH 03/19] Remove erroneous comment Copy and paste error comment from the other joint getters --- include/kodlab_mjbots_sdk/robot_base.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/kodlab_mjbots_sdk/robot_base.h b/include/kodlab_mjbots_sdk/robot_base.h index 1417eb0..49e27af 100644 --- a/include/kodlab_mjbots_sdk/robot_base.h +++ b/include/kodlab_mjbots_sdk/robot_base.h @@ -165,7 +165,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> GetJoints(){return joints;} From 0cb43aa978eb65816607d046bcec34679d54e63f Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Thu, 29 Jun 2023 19:48:45 -0400 Subject: [PATCH 04/19] Clarify comment, value is board temperature --- include/kodlab_mjbots_sdk/mjbots_hardware_interface.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h index 74bcc62..5bdf72e 100644 --- a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h +++ b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h @@ -52,7 +52,7 @@ class MjbotsHardwareInterface { * @param send_pd_commands if true, packets to the moteus include pd gains and setpoints * @param send_current if true, packets to the moteus include q_current and d_current registers * @param send_voltage if true, packets to the moteus include voltage register - * @param send_temperature if true, packets to the moteus include temperature register + * @param send_temperature if true, packets to the moteus include board temperature register * @param send_fault if true, packets to the moteus include fault code register * */ @@ -140,7 +140,7 @@ class MjbotsHardwareInterface { bool send_pd_commands_; ///< Include pd gains and setpoints in the moteus packet bool send_current_; ///< Include q_current and d_current registers in the moteus packet bool send_voltage_; ///< Include voltage register in the moteus packet - bool send_temperature_; ///< Include temperature register in the moteus packet + bool send_temperature_; ///< Include board temperature register in the moteus packet bool send_fault_; ///< Include fault code register in the moteus packet std::map servo_bus_map_; /// map from servo id to servo bus From 57289631d898ce8423f3c3403329358de0742ce6 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Thu, 29 Jun 2023 19:51:33 -0400 Subject: [PATCH 05/19] Add bool arguments for debug options Constructor was missing necessary boolean options, causing compilation issues. --- src/mjbots_hardware_interface.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index 1de51f1..e620cf5 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -83,7 +83,11 @@ MjbotsHardwareInterface::MjbotsHardwareInterface(std::vector imu_world_offset_deg, bool dry_run, bool print_torques, - bool send_pd_commands) + bool send_pd_commands, + bool send_current, + bool send_voltage, + bool send_temperature, + bool send_fault) : imu_data_(imu_data_ptr ? imu_data_ptr : std::make_shared<::kodlab::IMUData>()), dry_run_(dry_run), print_torques_(print_torques), From cf80bb1f76ddf8b792f90294d50a1978c9105956 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Thu, 29 Jun 2023 19:52:58 -0400 Subject: [PATCH 06/19] Add joint vector getter to hardware interface --- include/kodlab_mjbots_sdk/mjbots_hardware_interface.h | 6 ++++++ src/mjbots_hardware_interface.cpp | 4 ++++ 2 files changed, 10 insertions(+) diff --git a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h index 5bdf72e..b3388e8 100644 --- a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h +++ b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h @@ -106,6 +106,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> GetJoints(); + /*! * @brief accessor for the joint modes * @return the joint modes diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index e620cf5..fe65f7e 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -185,6 +185,10 @@ void MjbotsHardwareInterface::SendCommand() { moteus_interface_->Cycle(moteus_data_); } +std::vector> MjbotsHardwareInterface::GetJoints() { + return joints; +} + std::vector<::mjbots::moteus::Mode> MjbotsHardwareInterface::GetJointModes() { std::vector<::mjbots::moteus::Mode>modes(modes_.begin(), modes_.end()); return modes; From eec4c234bcfb9c4135d004952e469440b9ce3425 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Fri, 30 Jun 2023 17:52:24 -0400 Subject: [PATCH 07/19] Added Boolean Options to mjbots_control_loop.h --- include/kodlab_mjbots_sdk/mjbots_control_loop.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/include/kodlab_mjbots_sdk/mjbots_control_loop.h b/include/kodlab_mjbots_sdk/mjbots_control_loop.h index 8826564..942ccd0 100644 --- a/include/kodlab_mjbots_sdk/mjbots_control_loop.h +++ b/include/kodlab_mjbots_sdk/mjbots_control_loop.h @@ -41,6 +41,10 @@ struct ControlLoopOptions { 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 send_current = false; ///< If true, the control loop will send d_current and q_current registers + bool send_voltage = false; ///< If true, the control loop will send voltage registers + bool send_temperature = false; ///< If true, the control loop will send board temperature registers + bool send_fault = false; ///< If true, the control loop will send fault code registers }; /*! @@ -192,7 +196,11 @@ MjbotsControlLoop::MjbotsControlLoop(std::shar robot_->GetIMUDataSharedPtr(), options.imu_world_offset_deg, options.dry_run, options.print_torques, - options.send_pd_commands); + options.send_pd_commands, + options.send_current, + options.send_voltage, + options.send_temperature, + options.send_fault); num_joints_ = robot_->joints.size(); SetupOptions(options); } From fbe6033c22403ee1d40874597f9a4108f5440004 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Thu, 13 Jul 2023 16:15:27 -0400 Subject: [PATCH 08/19] joint_moteus.cpp Source File Addition Edited joint_moteus.h and created new corresponding source file joint_moteus.cpp. Added preliminary comments for user QueryCommand struct entry. --- include/kodlab_mjbots_sdk/joint_moteus.h | 172 ++++++++--------------- src/joint_moteus.cpp | 157 +++++++++++++++++++++ 2 files changed, 212 insertions(+), 117 deletions(-) create mode 100644 src/joint_moteus.cpp diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index 0e2c5cc..5b573ad 100644 --- a/include/kodlab_mjbots_sdk/joint_moteus.h +++ b/include/kodlab_mjbots_sdk/joint_moteus.h @@ -1,7 +1,7 @@ /** * @file joint_moteus.h * @author J. Diego Caporale (jdcap@seas.upenn.edu) - * @brief Moteus powered joint class + * @brief Moteus powered joint class header * @date 2022-02-22 * * @copyright BSD 3-Clause License, Copyright (c) 2021 The Trustees of the University of Pennsylvania. All Rights Reserved @@ -33,10 +33,6 @@ struct MoteusJointConfig{ float moteus_kp = 0; float moteus_kd = 0; float soft_start_duration_ms = 1; - float temperature = 0; - float q_current = 0; - float d_current = 0; - float voltage = 0; }; /** @@ -59,13 +55,9 @@ 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 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 - * @param temperature /// Value of the board temperature in units of Celsius - * @param q_current /// Value of current in the Q phase in units of Amperes - * @param d_current /// Value of current in the D phase in units of Amperes - * @param voltage /// Value of the current input voltage */ JointMoteus( std::string name, @@ -78,21 +70,10 @@ class JointMoteus: public JointBase float pos_min = -std::numeric_limits::infinity(), float pos_max = std::numeric_limits::infinity(), float soft_start_duration_ms = 1, + // ::mjbots::moteus::QueryCommand query_type=kDefaultQuery, float moteus_kp = 0, - float moteus_kd = 0, - float temperature = 0, - float q_current = 0, - float d_current = 0, - float voltage = 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), - temperature_(temperature), - q_current_(q_current), - d_current_(d_current), - voltage_(voltage) {} + float moteus_kd = 0 + ); /** * @brief Construct a new Joint Moteus object without name @@ -108,10 +89,6 @@ class JointMoteus: public JointBase * @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 - * @param temperature /// Value of the board temperature in units of Celsius - * @param q_current /// Value of current in the Q phase in units of Amperes - * @param d_current /// Value of current in the D phase in units of Amperes - * @param voltage /// Value of the current input voltage */ JointMoteus( int can_id, @@ -124,44 +101,15 @@ class JointMoteus: public JointBase float pos_max = std::numeric_limits::infinity(), float soft_start_duration_ms = 1, float moteus_kp = 0, - float moteus_kd = 0, - float temperature = 0, - float q_current = 0, - float d_current = 0, - float voltage = 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), - temperature_(temperature), - q_current_(q_current), - d_current_(d_current), - voltage_(voltage) {} - + 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), - temperature_(config.temperature), - q_current_(config.q_current), - d_current_(config.d_current), - voltage_(config.voltage) {} + JointMoteus(MoteusJointConfig config); /** * @brief Update the joint of the moteus. Converts rot/s to rad/s and saves registers @@ -169,138 +117,112 @@ class JointMoteus: public JointBase * @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 + * @param mode moteus mode * @param reply_q_current current in the Q phase reported by moteus [A] * @param reply_d_current current in the D phase reported by moteus [A] * @param reply_voltage voltage reported by the moteus [V] * @param reply_temperature temperature reported by the moteus [C] - * @param fault fault code - */ - void UpdateMoteus( - float reply_pos, - float reply_vel, - float reply_torque, - ::mjbots::moteus::Mode mode, - float reply_q_current, - float reply_d_current, - float reply_voltage, - float reply_temperature, - ::mjbots::moteus::Fault fault){ - UpdateState( 2 * M_PI * reply_pos, 2 * M_PI * reply_vel, reply_torque); - mode_ = mode; - q_current_ = reply_q_current; - d_current_ = reply_d_current; - voltage_ = reply_voltage; - temperature_ = reply_temperature; - fault_ = fault; - } + * @param fault fault code + */ + void UpdateMoteus(float reply_pos, + float reply_vel, + float reply_torque, + ::mjbots::moteus::Mode mode, + float reply_q_current, + float reply_d_current, + float reply_voltage, + float reply_temperature, + ::mjbots::moteus::Fault fault); /** * @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 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 temperature register * * @return float */ - virtual float get_temperature() const {return temperature_;} + float get_temperature() const; /** * @brief Get the q_current register * * @return float */ - virtual float get_q_current() const {return q_current_;} + float get_q_current() const; /** * @brief Get the d_current register * * @return float */ - virtual float get_d_current() const {return d_current_;} + float get_d_current() const; /** * @brief Get the voltage register * * @return float */ - virtual float get_voltage() const {return voltage_;} + float get_voltage() const; /** * @brief Get the fault register * * @return const ::mjbots::moteus::Fault& */ - const ::mjbots::moteus::Fault & get_fault() const {return 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; private: + // std::map qc_map = + // { + // {"DEFAULT", kDefaultQuery}, + // {"ALL", kAllQuery}, + // {"CURRENT", kCurrentQuery}, + // {"DEBUG", kDebugQuery}, + // } int can_id_; /// the can id of this joint's moteus int can_bus_; /// the can bus the moteus communicates on ::mjbots::moteus::Mode mode_ = ::mjbots::moteus::Mode::kStopped; /// joint's moteus mode @@ -311,6 +233,22 @@ class JointMoteus: public JointBase float d_current_ = 0; float voltage_ = 0; ::mjbots::moteus::Fault fault_ = ::mjbots::moteus::Fault::kSuccess; // fault mode + ::mjbots::moteus::QueryCommand query_resolutions_; + // static const ::mjbots::moteus::QueryCommand kDefaultQuery = + // { + // Resolution::kInt8; + // Resolution::kInt16; + // Resolution::kInt16; + // Resolution::kIgnore; + // Resolution::kIgnore; + // = Resolution::kIgnore; + // state = Resolution::kIgnore; + // = Resolution::kIgnore; + // ture = Resolution::kIgnore; + // Resolution::kIgnore; + + // } + }; }//namespace mjbots }//namespace kodlab diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp new file mode 100644 index 0000000..40e46e8 --- /dev/null +++ b/src/joint_moteus.cpp @@ -0,0 +1,157 @@ +/** + * @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 query_type=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), + 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 query_type=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), + 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), + moteus_kp_(config.moteus_kp), + moteus_kd_(config.moteus_kd) {} + +void JointMoteus::UpdateMoteus(float reply_pos, + float reply_vel, + float reply_torque, + ::mjbots::moteus::Mode mode, + float reply_q_current, + float reply_d_current, + float reply_voltage, + float reply_temperature, + ::mjbots::moteus::Fault fault){ + UpdateState(2 * M_PI * reply_pos, 2 * M_PI * reply_vel, reply_torque); + mode_ = mode; + q_current_ = reply_q_current; + d_current_ = reply_d_current; + voltage_ = reply_voltage; + temperature_ = reply_temperature; + fault_ = fault; +} + +int JointMoteus::get_can_id() const { + return can_id_; +} + +int JointMoteus::get_can_bus() const { + return can_bus_; +} + +const ::mjbots::moteus::Mode & JointMoteus::get_mode_reference() const { + return mode_; +} + +float JointMoteus::get_temperature() const { + return temperature_; +} + +float JointMoteus::get_q_current() const { + return q_current_; +} + +float JointMoteus::get_d_current() const { + return d_current_; +} + +float JointMoteus::get_voltage() const { + return voltage_; +} + +const ::mjbots::moteus::Fault & JointMoteus::get_fault() const { + 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; +} + +} //namespace mjbots +} //namespace kodlab + From 4a5692876a91fcec16a0fc426da4d3254f563d68 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Fri, 14 Jul 2023 16:28:17 -0400 Subject: [PATCH 09/19] QueryCommand user library additions Added library of preset QueryCommand structs for common register requests. --- include/kodlab_mjbots_sdk/joint_moteus.h | 95 ++++++++++++++---------- src/CMakeLists.txt | 1 + src/joint_moteus.cpp | 63 +++++++++++++++- 3 files changed, 116 insertions(+), 43 deletions(-) diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index 5b573ad..0fef8e6 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::infinity(); - float pos_min = -std::numeric_limits::infinity(); - float pos_max = std::numeric_limits::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 @@ -43,6 +31,30 @@ struct MoteusJointConfig{ class JointMoteus: public JointBase { public: + /** + * @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 debug resolution struct + * + */ + static const ::mjbots::moteus::QueryCommand kDebugQuery; + + /** + * @brief Define a resolution struct that includes all registers + * + */ + static const ::mjbots::moteus::QueryCommand kComprehensiveQuery; + /** * @brief Construct a new Joint Moteus object * @@ -56,6 +68,7 @@ class JointMoteus: public JointBase * @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 */ @@ -70,7 +83,7 @@ class JointMoteus: public JointBase float pos_min = -std::numeric_limits::infinity(), float pos_max = std::numeric_limits::infinity(), float soft_start_duration_ms = 1, - // ::mjbots::moteus::QueryCommand query_type=kDefaultQuery, + ::mjbots::moteus::QueryCommand query_type = kDefaultQuery, float moteus_kp = 0, float moteus_kd = 0 ); @@ -86,9 +99,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, @@ -100,6 +114,7 @@ class JointMoteus: public JointBase float pos_min = -std::numeric_limits::infinity(), float pos_max = std::numeric_limits::infinity(), float soft_start_duration_ms = 1, + ::mjbots::moteus::QueryCommand query_type = kDefaultQuery, float moteus_kp = 0, float moteus_kd = 0 ); @@ -112,7 +127,7 @@ class JointMoteus: public JointBase JointMoteus(MoteusJointConfig config); /** - * @brief Update the joint of the moteus. Converts rot/s to rad/s and saves registers + * @brief Update the joint of the moteus. Converts rot/s to rad/s and saves other state values. * * @param reply_pos position reported by moteus [rot] * @param reply_vel velocity reported by moteus [rot/s] @@ -214,17 +229,10 @@ class JointMoteus: public JointBase */ [[nodiscard]] float get_moteus_velocity_target()const; - private: - // std::map qc_map = - // { - // {"DEFAULT", kDefaultQuery}, - // {"ALL", kAllQuery}, - // {"CURRENT", kCurrentQuery}, - // {"DEBUG", kDebugQuery}, - // } 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; @@ -233,22 +241,27 @@ class JointMoteus: public JointBase float d_current_ = 0; float voltage_ = 0; ::mjbots::moteus::Fault fault_ = ::mjbots::moteus::Fault::kSuccess; // fault mode - ::mjbots::moteus::QueryCommand query_resolutions_; - // static const ::mjbots::moteus::QueryCommand kDefaultQuery = - // { - // Resolution::kInt8; - // Resolution::kInt16; - // Resolution::kInt16; - // Resolution::kIgnore; - // Resolution::kIgnore; - // = Resolution::kIgnore; - // state = Resolution::kIgnore; - // = Resolution::kIgnore; - // ture = Resolution::kIgnore; - // Resolution::kIgnore; - - // } +}; +/** + * @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::infinity(); + float pos_min = -std::numeric_limits::infinity(); + float pos_max = std::numeric_limits::infinity(); + float moteus_kp = 0; + float moteus_kd = 0; + float soft_start_duration_ms = 1; }; + }//namespace mjbots }//namespace kodlab 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 index 40e46e8..61ca6ce 100644 --- a/src/joint_moteus.cpp +++ b/src/joint_moteus.cpp @@ -15,6 +15,62 @@ namespace kodlab{ namespace mjbots{ +// kDefaultQuery defintion +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 +}; + +// kTorqueQuery definition +const ::mjbots::moteus::QueryCommand JointMoteus::kTorqueQuery = { + ::mjbots::moteus::Resolution::kInt8, + ::mjbots::moteus::Resolution::kInt16, + ::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 +}; + +// kDebugQuery definition +const ::mjbots::moteus::QueryCommand JointMoteus::kDebugQuery = { + ::mjbots::moteus::Resolution::kInt8, + ::mjbots::moteus::Resolution::kInt16, + ::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::kInt16, + ::mjbots::moteus::Resolution::kInt8 +}; + +// kComprehensiveQuery definition +const ::mjbots::moteus::QueryCommand JointMoteus::kComprehensiveQuery = { + ::mjbots::moteus::Resolution::kInt8, + ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kIgnore, + ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt8 +}; + JointMoteus::JointMoteus(std::string name, int can_id, int can_bus, @@ -25,12 +81,13 @@ JointMoteus::JointMoteus(std::string name, float pos_min, float pos_max, float soft_start_duration_ms, - // ::mjbots::moteus::QueryCommand query_type=kDefaultQuery, + ::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) {} @@ -43,12 +100,13 @@ JointMoteus::JointMoteus(int can_id, float pos_min, float pos_max, float soft_start_duration_ms, - // ::mjbots::moteus::QueryCommand query_type=kDefaultQuery, + ::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) {} @@ -63,6 +121,7 @@ JointMoteus::JointMoteus(MoteusJointConfig config) 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) {} From df77cfe73bd8a19225dc92ada7803d67d7e51a83 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Mon, 17 Jul 2023 15:21:49 -0400 Subject: [PATCH 10/19] UpdateMoteus() and boolean options streamlining Implemented supporting architecture for QueryCommand struct input. Modified UpdateMoteus() to accept entire reply_message. --- include/kodlab_mjbots_sdk/joint_moteus.h | 27 ++++------- .../kodlab_mjbots_sdk/mjbots_control_loop.h | 11 +---- .../mjbots_hardware_interface.h | 14 +----- src/joint_moteus.cpp | 28 +++++------- src/mjbots_hardware_interface.cpp | 45 ++++--------------- 5 files changed, 33 insertions(+), 92 deletions(-) diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index 0fef8e6..71dbda2 100644 --- a/include/kodlab_mjbots_sdk/joint_moteus.h +++ b/include/kodlab_mjbots_sdk/joint_moteus.h @@ -129,25 +129,9 @@ class JointMoteus: public JointBase /** * @brief Update the joint of the moteus. Converts rot/s to rad/s and saves other state values. * - * @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 - * @param reply_q_current current in the Q phase reported by moteus [A] - * @param reply_d_current current in the D phase reported by moteus [A] - * @param reply_voltage voltage reported by the moteus [V] - * @param reply_temperature temperature reported by the moteus [C] - * @param fault fault code + * @param reply_message reply message sent by moteus to pi3hat */ - void UpdateMoteus(float reply_pos, - float reply_vel, - float reply_torque, - ::mjbots::moteus::Mode mode, - float reply_q_current, - float reply_d_current, - float reply_voltage, - float reply_temperature, - ::mjbots::moteus::Fault fault); + void UpdateMoteus(::mjbots::moteus::QueryResult reply_message); /** * @brief Get the can id @@ -170,6 +154,13 @@ class JointMoteus: public JointBase */ const ::mjbots::moteus::Mode & get_mode_reference() const; + /** + * @brief Get the QueryCommand object + * + * @return const ::mjbots::moteus::QueryCommand + */ + const ::mjbots::moteus::QueryCommand get_query_command() const; + /** * @brief Get the temperature register * diff --git a/include/kodlab_mjbots_sdk/mjbots_control_loop.h b/include/kodlab_mjbots_sdk/mjbots_control_loop.h index 942ccd0..e53f7bd 100644 --- a/include/kodlab_mjbots_sdk/mjbots_control_loop.h +++ b/include/kodlab_mjbots_sdk/mjbots_control_loop.h @@ -41,10 +41,6 @@ struct ControlLoopOptions { 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 send_current = false; ///< If true, the control loop will send d_current and q_current registers - bool send_voltage = false; ///< If true, the control loop will send voltage registers - bool send_temperature = false; ///< If true, the control loop will send board temperature registers - bool send_fault = false; ///< If true, the control loop will send fault code registers }; /*! @@ -196,11 +192,8 @@ MjbotsControlLoop::MjbotsControlLoop(std::shar robot_->GetIMUDataSharedPtr(), options.imu_world_offset_deg, options.dry_run, options.print_torques, - options.send_pd_commands, - options.send_current, - options.send_voltage, - options.send_temperature, - options.send_fault); + options.send_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 b3388e8..1fbaecf 100644 --- a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h +++ b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h @@ -50,10 +50,6 @@ class MjbotsHardwareInterface { * @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 send_current if true, packets to the moteus include q_current and d_current registers - * @param send_voltage if true, packets to the moteus include voltage register - * @param send_temperature if true, packets to the moteus include board temperature register - * @param send_fault if true, packets to the moteus include fault code register * */ MjbotsHardwareInterface(std::vector> joint_list, @@ -64,11 +60,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 send_current = false, - bool send_voltage = false, - bool send_temperature = false, - bool send_fault = false + bool send_pd_commands = false ); /** @@ -144,10 +136,6 @@ class MjbotsHardwareInterface { 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 send_current_; ///< Include q_current and d_current registers in the moteus packet - bool send_voltage_; ///< Include voltage register in the moteus packet - bool send_temperature_; ///< Include board temperature register in the moteus packet - bool send_fault_; ///< Include fault code register in the moteus packet std::map servo_bus_map_; /// map from servo id to servo bus diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp index 61ca6ce..fe4abdb 100644 --- a/src/joint_moteus.cpp +++ b/src/joint_moteus.cpp @@ -125,22 +125,14 @@ JointMoteus::JointMoteus(MoteusJointConfig config) moteus_kp_(config.moteus_kp), moteus_kd_(config.moteus_kd) {} -void JointMoteus::UpdateMoteus(float reply_pos, - float reply_vel, - float reply_torque, - ::mjbots::moteus::Mode mode, - float reply_q_current, - float reply_d_current, - float reply_voltage, - float reply_temperature, - ::mjbots::moteus::Fault fault){ - UpdateState(2 * M_PI * reply_pos, 2 * M_PI * reply_vel, reply_torque); - mode_ = mode; - q_current_ = reply_q_current; - d_current_ = reply_d_current; - voltage_ = reply_voltage; - temperature_ = reply_temperature; - fault_ = fault; +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; } int JointMoteus::get_can_id() const { @@ -155,6 +147,10 @@ const ::mjbots::moteus::Mode & JointMoteus::get_mode_reference() const { return mode_; } +const ::mjbots::moteus::QueryCommand JointMoteus::get_query_command() const { + return query_type_; +} + float JointMoteus::get_temperature() const { return temperature_; } diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index fe65f7e..f8fab43 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -14,11 +14,6 @@ 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_){ res.position = ::mjbots::moteus::Resolution::kInt16; @@ -37,25 +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; - } - if(send_current_){ - cmd.query.q_current = ::mjbots::moteus::Resolution::kInt16; - cmd.query.d_current = ::mjbots::moteus::Resolution::kInt16; - } - if(send_voltage_){ - cmd.query.voltage = ::mjbots::moteus::Resolution::kInt16; - } - if(send_temperature_){ - cmd.query.temperature = ::mjbots::moteus::Resolution::kInt16; - } - if(send_fault_){ - cmd.query.fault = ::mjbots::moteus::Resolution::kInt8; - } + + 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; } } @@ -83,11 +66,7 @@ MjbotsHardwareInterface::MjbotsHardwareInterface(std::vector imu_world_offset_deg, bool dry_run, bool print_torques, - bool send_pd_commands, - bool send_current, - bool send_voltage, - bool send_temperature, - bool send_fault) + bool send_pd_commands) : imu_data_(imu_data_ptr ? imu_data_ptr : std::make_shared<::kodlab::IMUData>()), dry_run_(dry_run), print_torques_(print_torques), @@ -149,13 +128,7 @@ void MjbotsHardwareInterface::ProcessReply() { if(std::isnan(servo_reply.position)){ 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, - send_current_ ? servo_reply.q_current : 0, - send_current_ ? servo_reply.d_current : 0, - send_voltage_ ? servo_reply.voltage : 0, - send_temperature_ ? servo_reply.temperature : 0, - servo_reply.fault); + joint->UpdateMoteus(servo_reply); } } imu_data_->Update(*(moteus_data_.attitude)); From cb67f46e9c9972254ea0d7695b8ff37edfb3d247 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Mon, 17 Jul 2023 15:27:56 -0400 Subject: [PATCH 11/19] Change to --- README.md | 4 ++-- include/kodlab_mjbots_sdk/mjbots_control_loop.h | 4 ++-- include/kodlab_mjbots_sdk/mjbots_hardware_interface.h | 6 +++--- src/mjbots_hardware_interface.cpp | 8 ++++---- 4 files changed, 11 insertions(+), 11 deletions(-) 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/mjbots_control_loop.h b/include/kodlab_mjbots_sdk/mjbots_control_loop.h index e53f7bd..04585eb 100644 --- a/include/kodlab_mjbots_sdk/mjbots_control_loop.h +++ b/include/kodlab_mjbots_sdk/mjbots_control_loop.h @@ -40,7 +40,7 @@ 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 }; /*! @@ -192,7 +192,7 @@ MjbotsControlLoop::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 1fbaecf..a925f98 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> joint_list, @@ -60,7 +60,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 ); /** @@ -135,7 +135,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 servo_bus_map_; /// map from servo id to servo bus diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index f8fab43..7696c10 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -15,7 +15,7 @@ namespace kodlab::mjbots { void MjbotsHardwareInterface::InitializeCommand() { ::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; @@ -66,11 +66,11 @@ MjbotsHardwareInterface::MjbotsHardwareInterface(std::vector 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>()), 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; @@ -139,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(); From 6693b6eb9cfc0af7b1dbc6bea448fe56ac73acfc Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Fri, 21 Jul 2023 17:53:37 -0400 Subject: [PATCH 12/19] Changed torque resolutions to kInt32 within QueryCommand struct library --- src/joint_moteus.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp index fe4abdb..2ca1438 100644 --- a/src/joint_moteus.cpp +++ b/src/joint_moteus.cpp @@ -34,7 +34,7 @@ const ::mjbots::moteus::QueryCommand JointMoteus::kTorqueQuery = { ::mjbots::moteus::Resolution::kInt8, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kInt16, - ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt32, ::mjbots::moteus::Resolution::kIgnore, ::mjbots::moteus::Resolution::kIgnore, ::mjbots::moteus::Resolution::kIgnore, @@ -48,7 +48,7 @@ const ::mjbots::moteus::QueryCommand JointMoteus::kDebugQuery = { ::mjbots::moteus::Resolution::kInt8, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kInt16, - ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt32, ::mjbots::moteus::Resolution::kIgnore, ::mjbots::moteus::Resolution::kIgnore, ::mjbots::moteus::Resolution::kIgnore, @@ -62,7 +62,7 @@ const ::mjbots::moteus::QueryCommand JointMoteus::kComprehensiveQuery = { ::mjbots::moteus::Resolution::kInt8, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kInt16, - ::mjbots::moteus::Resolution::kInt16, + ::mjbots::moteus::Resolution::kInt32, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kIgnore, From a1d036cce9f5b71346378c906db8a66298751855 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Fri, 21 Jul 2023 18:01:25 -0400 Subject: [PATCH 13/19] Changed feedforward_torque resolution in InitializeCommand() to kInt32 --- src/mjbots_hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index 7696c10..b0af5d7 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -25,7 +25,7 @@ void MjbotsHardwareInterface::InitializeCommand() { }else{ res.position = ::mjbots::moteus::Resolution::kIgnore; res.velocity = ::mjbots::moteus::Resolution::kIgnore; - res.feedforward_torque = ::mjbots::moteus::Resolution::kInt16; + res.feedforward_torque = ::mjbots::moteus::Resolution::kInt32; res.kp_scale = ::mjbots::moteus::Resolution::kIgnore; res.kd_scale = ::mjbots::moteus::Resolution::kIgnore; res.maximum_torque = ::mjbots::moteus::Resolution::kIgnore; From af524382274e1a523cdb6fe4c58a2cc259e86fda Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Fri, 21 Jul 2023 18:24:34 -0400 Subject: [PATCH 14/19] Undo change on commanded feedforward_torque res, adjust commanded max torque res for use_pd_commands --- src/mjbots_hardware_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index b0af5d7..16baf18 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -21,11 +21,11 @@ void MjbotsHardwareInterface::InitializeCommand() { 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; - res.feedforward_torque = ::mjbots::moteus::Resolution::kInt32; + res.feedforward_torque = ::mjbots::moteus::Resolution::kInt16; res.kp_scale = ::mjbots::moteus::Resolution::kIgnore; res.kd_scale = ::mjbots::moteus::Resolution::kIgnore; res.maximum_torque = ::mjbots::moteus::Resolution::kIgnore; From f2c290eb5a403b172ec286ac97a2577160f80a5f Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Wed, 26 Jul 2023 13:54:33 -0400 Subject: [PATCH 15/19] LOG_WARN implementation and override functions at joint_moteus level --- include/kodlab_mjbots_sdk/joint_moteus.h | 34 ++++++++- src/joint_moteus.cpp | 92 ++++++++++++++++++++---- 2 files changed, 110 insertions(+), 16 deletions(-) diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index 71dbda2..43a6ccb 100644 --- a/include/kodlab_mjbots_sdk/joint_moteus.h +++ b/include/kodlab_mjbots_sdk/joint_moteus.h @@ -147,6 +147,13 @@ class JointMoteus: public JointBase */ int get_can_bus() const; + /** + * @brief Get the QueryCommand object + * + * @return const ::mjbots::moteus::QueryCommand + */ + const ::mjbots::moteus::QueryCommand get_query_command() const; + /** * @brief Get the mode reference * @@ -155,11 +162,32 @@ class JointMoteus: public JointBase const ::mjbots::moteus::Mode & get_mode_reference() const; /** - * @brief Get the QueryCommand object + * @brief Get the position, overrides joint_base.h * - * @return const ::mjbots::moteus::QueryCommand + * @return float */ - const ::mjbots::moteus::QueryCommand get_query_command() const; + float get_position() const override; + + /** + * @brief Get the velocity, overrides joint_base.h + * + * @return float + */ + float get_velocity() const override; + + /** + * @brief Get the torque command, overrides joint_base.h + * + * @return float + */ + float get_servo_torque() const override; + + /** + * @brief Get the measured joint torque, overrides joint_base.h + * + * @return float + */ + float get_measured_torque() const override; /** * @brief Get the temperature register diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp index 2ca1438..e5bd161 100644 --- a/src/joint_moteus.cpp +++ b/src/joint_moteus.cpp @@ -16,7 +16,7 @@ namespace kodlab{ namespace mjbots{ // kDefaultQuery defintion -const ::mjbots::moteus::QueryCommand JointMoteus::kDefaultQuery = { +const ::mjbots::moteus::QueryCommand JointMoteus::kDefaultQuery{ ::mjbots::moteus::Resolution::kInt8, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kInt16, @@ -26,11 +26,11 @@ const ::mjbots::moteus::QueryCommand JointMoteus::kDefaultQuery = { ::mjbots::moteus::Resolution::kIgnore, ::mjbots::moteus::Resolution::kIgnore, ::mjbots::moteus::Resolution::kIgnore, - ::mjbots::moteus::Resolution::kIgnore + ::mjbots::moteus::Resolution::kIgnore }; // kTorqueQuery definition -const ::mjbots::moteus::QueryCommand JointMoteus::kTorqueQuery = { +const ::mjbots::moteus::QueryCommand JointMoteus::kTorqueQuery{ ::mjbots::moteus::Resolution::kInt8, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kInt16, @@ -44,7 +44,7 @@ const ::mjbots::moteus::QueryCommand JointMoteus::kTorqueQuery = { }; // kDebugQuery definition -const ::mjbots::moteus::QueryCommand JointMoteus::kDebugQuery = { +const ::mjbots::moteus::QueryCommand JointMoteus::kDebugQuery{ ::mjbots::moteus::Resolution::kInt8, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kInt16, @@ -58,7 +58,7 @@ const ::mjbots::moteus::QueryCommand JointMoteus::kDebugQuery = { }; // kComprehensiveQuery definition -const ::mjbots::moteus::QueryCommand JointMoteus::kComprehensiveQuery = { +const ::mjbots::moteus::QueryCommand JointMoteus::kComprehensiveQuery{ ::mjbots::moteus::Resolution::kInt8, ::mjbots::moteus::Resolution::kInt16, ::mjbots::moteus::Resolution::kInt16, @@ -143,32 +143,98 @@ int JointMoteus::get_can_bus() const { return can_bus_; } +const ::mjbots::moteus::QueryCommand JointMoteus::get_query_command() const { + return query_type_; +} + const ::mjbots::moteus::Mode & JointMoteus::get_mode_reference() const { - return mode_; + if(query_type_.mode == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Mode resolution is set to kIgnore, while attempting to log register"); + return ::mjbots::moteus::Mode::kStopped; + } else { + return mode_; + } } -const ::mjbots::moteus::QueryCommand JointMoteus::get_query_command() const { - return query_type_; +float JointMoteus::get_position() const { + if(query_type_.position == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Position resolution is set to kIgnore, while attempting to log register"); + return 0; + } else { + return position_; + } +} + +float JointMoteus::get_velocity() const { + if(query_type_.velocity == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Velocity resolution is set to kIgnore, while attempting to log register"); + return 0; + } else { + return velocity_; + } +} + +float JointMoteus::get_servo_torque() const { + if(query_type_.torque == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Torque resolution is set to kIgnore, while attempting to log register"); + return 0; + } else { + return servo_torque_; + } +} + +float JointMoteus::get_measured_torque() const { + if(query_type_.torque == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Torque resolution is set to kIgnore, while attempting to log register"); + return 0; + } else { + return measured_torque_; + } } float JointMoteus::get_temperature() const { - return temperature_; + if(query_type_.temperature == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Temperature resolution is set to kIgnore, while attempting to log register"); + return 0; + } else { + return temperature_; + } } float JointMoteus::get_q_current() const { - return q_current_; + if(query_type_.q_current == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Q Current resolution is set to kIgnore, while attempting to log register"); + return 0; + } else { + return q_current_; + } } float JointMoteus::get_d_current() const { - return d_current_; + if(query_type_.d_current == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("D Current resolution is set to kIgnore, while attempting to log register"); + return 0; + } else { + return d_current_; + } } float JointMoteus::get_voltage() const { - return voltage_; + if(query_type_.voltage == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Voltage resolution is set to kIgnore, while attemping to log register"); + return 0; + } else { + return voltage_; + } } const ::mjbots::moteus::Fault & JointMoteus::get_fault() const { - return fault_; + if(query_type_.fault == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Fault code resolution is set to kIgnore, while attempting to log register"); + return ::mjbots::moteus::Fault::kSuccess; + } else { + return fault_; + } } [[nodiscard]] float JointMoteus::get_kp_scale() const { From c5c18507cad3e3710de2a50731699ad0ab9ebb78 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Thu, 27 Jul 2023 15:32:06 -0400 Subject: [PATCH 16/19] Open loop control permissions and control safety features Adds a boolean open_loop flag for user specification of open loop control and safety checks for improper closed loop operation --- include/kodlab_mjbots_sdk/joint_moteus.h | 8 ++++++++ include/kodlab_mjbots_sdk/mjbots_control_loop.h | 16 +++++++++++++--- .../mjbots_hardware_interface.h | 6 ++++-- src/joint_moteus.cpp | 8 ++++++++ src/mjbots_hardware_interface.cpp | 6 ++++-- 5 files changed, 37 insertions(+), 7 deletions(-) diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index 43a6ccb..aceffd2 100644 --- a/include/kodlab_mjbots_sdk/joint_moteus.h +++ b/include/kodlab_mjbots_sdk/joint_moteus.h @@ -133,6 +133,13 @@ class JointMoteus: public JointBase */ void UpdateMoteus(::mjbots::moteus::QueryResult reply_message); + /** + * @brief Determines whether open loop options should be enabled + * + * @return bool + */ + bool is_open_loop(); + /** * @brief Get the can id * @@ -249,6 +256,7 @@ class JointMoteus: public JointBase [[nodiscard]] float get_moteus_velocity_target()const; private: + bool open_loop_flag_ = false; // if true, open loop controller expected 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_; diff --git a/include/kodlab_mjbots_sdk/mjbots_control_loop.h b/include/kodlab_mjbots_sdk/mjbots_control_loop.h index 04585eb..6c48e7a 100644 --- a/include/kodlab_mjbots_sdk/mjbots_control_loop.h +++ b/include/kodlab_mjbots_sdk/mjbots_control_loop.h @@ -41,6 +41,7 @@ struct ControlLoopOptions { 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 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 }; /*! @@ -162,7 +163,7 @@ class MjbotsControlLoop : public AbstractRealtimeObject { template MjbotsControlLoop::MjbotsControlLoop(std::vector joints, const ControlLoopOptions &options) - : MjbotsControlLoop( make_share_vector(joints), options){} + : MjbotsControlLoop(make_share_vector(joints), options){} template MjbotsControlLoop::MjbotsControlLoop( @@ -192,10 +193,19 @@ MjbotsControlLoop::MjbotsControlLoop(std::shar robot_->GetIMUDataSharedPtr(), options.imu_world_offset_deg, options.dry_run, options.print_torques, - options.use_pd_commands -); + options.use_pd_commands, + options.open_loop + ); num_joints_ = robot_->joints.size(); SetupOptions(options); + + // Open loop check + for(size_t i = 0; i < num_joints_; ++i){ + if(!options.open_loop && joints_moteus[i]->is_open_loop()){ + LOG_FATAL("Open loop option not set, not receiving position and/or velocity"); + kodlab::ActivateCtrlC(); + } + } } template diff --git a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h index a925f98..cdf5719 100644 --- a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h +++ b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h @@ -50,7 +50,7 @@ class MjbotsHardwareInterface { * @param dry_run if true, sends zero-torques to Moteus controllers * @param print_torques if true, prints torque commands * @param use_pd_commands if true, packets to the moteus include pd gains and setpoints - * + * @param open_loop if true, open loop control of the robot is permitted */ MjbotsHardwareInterface(std::vector> joint_list, const RealtimeParams &realtime_params, @@ -60,7 +60,8 @@ class MjbotsHardwareInterface { std::optional<::mjbots::pi3hat::Euler > imu_world_offset_deg = std::nullopt, bool dry_run = false, bool print_torques = false, - bool use_pd_commands = false + bool use_pd_commands = false, + bool open_loop = false ); /** @@ -136,6 +137,7 @@ class MjbotsHardwareInterface { bool dry_run_; ///< dry run active flag bool print_torques_; ///< print torques active flag bool use_pd_commands_; ///< Include pd gains and setpoints in the moteus packet + bool open_loop_; ///< Open loop controller flag std::map servo_bus_map_; /// map from servo id to servo bus diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp index e5bd161..32f0dd6 100644 --- a/src/joint_moteus.cpp +++ b/src/joint_moteus.cpp @@ -135,6 +135,14 @@ void JointMoteus::UpdateMoteus(::mjbots::moteus::QueryResult reply_message){ fault_ = reply_message.fault; } +bool JointMoteus::is_open_loop() { + if(query_type_.position == ::mjbots::moteus::Resolution::kIgnore || + query_type_.velocity == ::mjbots::moteus::Resolution::kIgnore){ + open_loop_flag_ = true; + } + return open_loop_flag_; +} + int JointMoteus::get_can_id() const { return can_id_; } diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index 16baf18..6b0c4f9 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -66,11 +66,13 @@ MjbotsHardwareInterface::MjbotsHardwareInterface(std::vector imu_world_offset_deg, bool dry_run, bool print_torques, - bool use_pd_commands) + bool use_pd_commands, + bool open_loop) : imu_data_(imu_data_ptr ? imu_data_ptr : std::make_shared<::kodlab::IMUData>()), dry_run_(dry_run), print_torques_(print_torques), - use_pd_commands_(use_pd_commands) + use_pd_commands_(use_pd_commands), + open_loop_(open_loop) { LOG_IF_WARN(dry_run_, "\nDRY RUN: NO TORQUES COMMANDED"); joints = joint_ptrs; From 557d08da299087bcea579b96bf81aaf8db62b4f4 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Fri, 28 Jul 2023 16:15:41 -0400 Subject: [PATCH 17/19] Bolster open loop functionality Prevents unnecessary errors during open loop, adds safety features for closed loop, adds get_mode() to joint_moteus --- include/kodlab_mjbots_sdk/joint_moteus.h | 8 +++++++- .../kodlab_mjbots_sdk/mjbots_control_loop.h | 15 +++++++-------- include/kodlab_mjbots_sdk/moteus_protocol.h | 19 +++++++++++++++++-- src/joint_moteus.cpp | 18 +++++++++++++----- src/mjbots_hardware_interface.cpp | 2 +- 5 files changed, 45 insertions(+), 17 deletions(-) diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index aceffd2..26f3500 100644 --- a/include/kodlab_mjbots_sdk/joint_moteus.h +++ b/include/kodlab_mjbots_sdk/joint_moteus.h @@ -168,6 +168,13 @@ class JointMoteus: public JointBase */ const ::mjbots::moteus::Mode & get_mode_reference() const; + /** + * @brief Get the mode + * + * @return const ::mjbots::moteus::Mode + */ + const ::mjbots::moteus::Mode get_mode() const; + /** * @brief Get the position, overrides joint_base.h * @@ -256,7 +263,6 @@ class JointMoteus: public JointBase [[nodiscard]] float get_moteus_velocity_target()const; private: - bool open_loop_flag_ = false; // if true, open loop controller expected 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_; diff --git a/include/kodlab_mjbots_sdk/mjbots_control_loop.h b/include/kodlab_mjbots_sdk/mjbots_control_loop.h index 6c48e7a..9a30d5e 100644 --- a/include/kodlab_mjbots_sdk/mjbots_control_loop.h +++ b/include/kodlab_mjbots_sdk/mjbots_control_loop.h @@ -186,6 +186,13 @@ MjbotsControlLoop::MjbotsControlLoop(std::shar joints_moteus.push_back( std::dynamic_pointer_cast(j)); } + // Open loop check + for(const auto & j : joints_moteus){ + if(!options.open_loop && j->is_open_loop()){ + LOG_FATAL("Open loop option not set, not receiving position and/or velocity"); + kodlab::ActivateCtrlC(); + } + } // Initialize mjbots_interface mjbots_interface_ = std::make_shared( std::move(joints_moteus), options.realtime_params, @@ -198,14 +205,6 @@ MjbotsControlLoop::MjbotsControlLoop(std::shar ); num_joints_ = robot_->joints.size(); SetupOptions(options); - - // Open loop check - for(size_t i = 0; i < num_joints_; ++i){ - if(!options.open_loop && joints_moteus[i]->is_open_loop()){ - LOG_FATAL("Open loop option not set, not receiving position and/or velocity"); - kodlab::ActivateCtrlC(); - } - } } template diff --git a/include/kodlab_mjbots_sdk/moteus_protocol.h b/include/kodlab_mjbots_sdk/moteus_protocol.h index 7549cbc..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, @@ -134,6 +135,7 @@ enum class Mode { }; enum class Fault { + kUnset = -1, kSuccess = 0, kDmaStreamTransferError = 1, @@ -678,7 +680,7 @@ inline void EmitQueryCommand( } struct QueryResult { - Mode mode = Mode::kStopped; + Mode mode = Mode::kUnset; double position = std::numeric_limits::quiet_NaN(); double velocity = std::numeric_limits::quiet_NaN(); double torque = std::numeric_limits::quiet_NaN(); @@ -687,7 +689,20 @@ struct QueryResult { bool rezero_state = false; double voltage = std::numeric_limits::quiet_NaN(); double temperature = std::numeric_limits::quiet_NaN(); - Fault fault = Fault::kSuccess; + Fault fault = Fault::kUnset; + + bool all_unset() const { + return mode == Mode::kUnset && + position == std::numeric_limits::quiet_NaN() && + velocity == std::numeric_limits::quiet_NaN() && + torque == std::numeric_limits::quiet_NaN() && + q_current == std::numeric_limits::quiet_NaN() && + d_current == std::numeric_limits::quiet_NaN() && + rezero_state == false && + voltage == std::numeric_limits::quiet_NaN() && + temperature == std::numeric_limits::quiet_NaN() && + fault == Fault::kUnset; + } }; inline QueryResult ParseQueryResult(const uint8_t* data, size_t size) { diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp index 32f0dd6..2ca58e2 100644 --- a/src/joint_moteus.cpp +++ b/src/joint_moteus.cpp @@ -136,11 +136,14 @@ void JointMoteus::UpdateMoteus(::mjbots::moteus::QueryResult reply_message){ } bool JointMoteus::is_open_loop() { + bool rv; if(query_type_.position == ::mjbots::moteus::Resolution::kIgnore || query_type_.velocity == ::mjbots::moteus::Resolution::kIgnore){ - open_loop_flag_ = true; + rv = true; + } else { + rv = false; } - return open_loop_flag_; + return rv; } int JointMoteus::get_can_id() const { @@ -158,10 +161,15 @@ const ::mjbots::moteus::QueryCommand JointMoteus::get_query_command() const { const ::mjbots::moteus::Mode & JointMoteus::get_mode_reference() const { if(query_type_.mode == ::mjbots::moteus::Resolution::kIgnore){ LOG_WARN("Mode resolution is set to kIgnore, while attempting to log register"); - return ::mjbots::moteus::Mode::kStopped; - } else { - return mode_; } + return mode_; +} + +const ::mjbots::moteus::Mode JointMoteus::get_mode() const { + if(query_type_.mode == ::mjbots::moteus::Resolution::kIgnore){ + LOG_WARN("Mode resolution is set to kIgnore, while attempting to log register"); + } + return mode_; } float JointMoteus::get_position() const { diff --git a/src/mjbots_hardware_interface.cpp b/src/mjbots_hardware_interface.cpp index 6b0c4f9..c8f08e7 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -127,7 +127,7 @@ 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); From acbb387b53721933d5371c564b296eae2ed81967 Mon Sep 17 00:00:00 2001 From: Lucien Peach Date: Thu, 10 Aug 2023 19:02:44 -0400 Subject: [PATCH 18/19] Comment resolution and related improvements --- include/kodlab_mjbots_sdk/joint_moteus.h | 98 +++++----- .../kodlab_mjbots_sdk/mjbots_control_loop.h | 7 +- .../mjbots_hardware_interface.h | 5 +- src/joint_moteus.cpp | 167 ++++++++++-------- src/mjbots_hardware_interface.cpp | 6 +- 5 files changed, 145 insertions(+), 138 deletions(-) diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index 2036adc..56fcf58 100644 --- a/include/kodlab_mjbots_sdk/joint_moteus.h +++ b/include/kodlab_mjbots_sdk/joint_moteus.h @@ -1,7 +1,7 @@ /** * @file joint_moteus.h * @author J. Diego Caporale (jdcap@seas.upenn.edu) - * @brief Moteus powered joint class header + * @brief Moteus powered joint class * @date 2022-02-22 * * @copyright BSD 3-Clause License, Copyright (c) 2021 The Trustees of the University of Pennsylvania. All Rights Reserved @@ -31,30 +31,6 @@ struct MoteusJointConfig; class JointMoteus: public JointBase { public: - /** - * @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 debug resolution struct - * - */ - static const ::mjbots::moteus::QueryCommand kDebugQuery; - - /** - * @brief Define a resolution struct that includes all registers - * - */ - static const ::mjbots::moteus::QueryCommand kComprehensiveQuery; - /** * @brief Construct a new Joint Moteus object * @@ -140,11 +116,13 @@ class JointMoteus: public JointBase virtual ~JointMoteus(){} /** - * @brief Determines whether open loop options should be enabled + * @brief Determines whether the QueryCommand struct ignores position + * and velocity when requesting data from the moteus, potentially + * characteristic of an open-loop controller * * @return bool */ - bool is_open_loop(); + bool is_open_loop_query(); /** * @brief Get the can id @@ -161,84 +139,88 @@ class JointMoteus: public JointBase int get_can_bus() const; /** - * @brief Get the QueryCommand object + * @brief Get the QueryCommand object, which contains resolutions * - * @return const ::mjbots::moteus::QueryCommand + * @return ::mjbots::moteus::QueryCommand */ - const ::mjbots::moteus::QueryCommand get_query_command() const; + [[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; /** - * @brief Get the mode + * @brief Get the current operational mode of the servo * - * @return const ::mjbots::moteus::Mode + * @return ::mjbots::moteus::Mode */ - const ::mjbots::moteus::Mode get_mode() const; + [[nodiscard]] ::mjbots::moteus::Mode get_mode() const; /** - * @brief Get the position, overrides joint_base.h + * @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 velocity, overrides joint_base.h + * @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 torque command, overrides joint_base.h + * @brief Get the torque command. Overrides function in joint_base.h * * @return float */ float get_servo_torque() const override; /** - * @brief Get the measured joint torque, overrides joint_base.h + * @brief Get the measured joint torque. Overrides function in + * joint_base.h * * @return float */ float get_measured_torque() const override; /** - * @brief Get the temperature register + * @brief Get the board temperature, measured in degrees Celsius * * @return float */ - float get_temperature() const; + [[nodiscard]] float get_temperature() const; /** - * @brief Get the q_current register + * @brief Get the current in the Q phase, measured in Amperes * * @return float */ - float get_q_current() const; + [[nodiscard]] float get_q_current() const; /** - * @brief Get the d_current register + * @brief Get the current in the D phase, measured in Amperes * * @return float */ - float get_d_current() const; + [[nodiscard]] float get_d_current() const; /** - * @brief Get the voltage register + * @brief Get the input voltage * * @return float */ - float get_voltage() const; + [[nodiscard]] float get_voltage() const; /** - * @brief Get the fault register + * @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& */ @@ -268,6 +250,27 @@ class JointMoteus: public JointBase */ [[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 @@ -301,6 +304,5 @@ struct MoteusJointConfig{ 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 4d064c8..18fa8fc 100644 --- a/include/kodlab_mjbots_sdk/mjbots_control_loop.h +++ b/include/kodlab_mjbots_sdk/mjbots_control_loop.h @@ -193,8 +193,8 @@ MjbotsControlLoop::MjbotsControlLoop(std::shar } // Open loop check for(const auto & j : joints_moteus){ - if(!options.open_loop && j->is_open_loop()){ - LOG_FATAL("Open loop option not set, not receiving position and/or velocity"); + 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(); } } @@ -205,8 +205,7 @@ MjbotsControlLoop::MjbotsControlLoop(std::shar robot_->GetIMUDataSharedPtr(), options.imu_world_offset_deg, options.dry_run, options.print_torques, - options.use_pd_commands, - options.open_loop + 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 cdf5719..843c9ac 100644 --- a/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h +++ b/include/kodlab_mjbots_sdk/mjbots_hardware_interface.h @@ -50,7 +50,6 @@ class MjbotsHardwareInterface { * @param dry_run if true, sends zero-torques to Moteus controllers * @param print_torques if true, prints torque commands * @param use_pd_commands if true, packets to the moteus include pd gains and setpoints - * @param open_loop if true, open loop control of the robot is permitted */ MjbotsHardwareInterface(std::vector> joint_list, const RealtimeParams &realtime_params, @@ -60,8 +59,7 @@ class MjbotsHardwareInterface { std::optional<::mjbots::pi3hat::Euler > imu_world_offset_deg = std::nullopt, bool dry_run = false, bool print_torques = false, - bool use_pd_commands = false, - bool open_loop = false + bool use_pd_commands = false ); /** @@ -137,7 +135,6 @@ class MjbotsHardwareInterface { bool dry_run_; ///< dry run active flag bool print_torques_; ///< print torques active flag bool use_pd_commands_; ///< Include pd gains and setpoints in the moteus packet - bool open_loop_; ///< Open loop controller flag std::map servo_bus_map_; /// map from servo id to servo bus diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp index 2ca58e2..7dbc8c8 100644 --- a/src/joint_moteus.cpp +++ b/src/joint_moteus.cpp @@ -15,62 +15,6 @@ namespace kodlab{ namespace mjbots{ -// kDefaultQuery defintion -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 -}; - -// kTorqueQuery definition -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 -}; - -// kDebugQuery definition -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 -}; - -// kComprehensiveQuery definition -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 -}; - JointMoteus::JointMoteus(std::string name, int can_id, int can_bus, @@ -135,15 +79,9 @@ void JointMoteus::UpdateMoteus(::mjbots::moteus::QueryResult reply_message){ fault_ = reply_message.fault; } -bool JointMoteus::is_open_loop() { - bool rv; - if(query_type_.position == ::mjbots::moteus::Resolution::kIgnore || - query_type_.velocity == ::mjbots::moteus::Resolution::kIgnore){ - rv = true; - } else { - rv = false; - } - return rv; +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 { @@ -154,27 +92,27 @@ int JointMoteus::get_can_bus() const { return can_bus_; } -const ::mjbots::moteus::QueryCommand JointMoteus::get_query_command() const { +::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 is set to kIgnore, while attempting to log register"); + LOG_WARN("Mode resolution in QueryCommand is set to kIgnore, while attempting to get value"); } return mode_; } -const ::mjbots::moteus::Mode JointMoteus::get_mode() const { +::mjbots::moteus::Mode JointMoteus::get_mode() const { if(query_type_.mode == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("Mode resolution is set to kIgnore, while attempting to log register"); + 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 is set to kIgnore, while attempting to log register"); + LOG_WARN("Position resolution in QueryCommand is set to kIgnore, while attempting to get value"); return 0; } else { return position_; @@ -183,7 +121,7 @@ float JointMoteus::get_position() const { float JointMoteus::get_velocity() const { if(query_type_.velocity == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("Velocity resolution is set to kIgnore, while attempting to log register"); + LOG_WARN("Velocity resolution in QueryCommand is set to kIgnore, while attempting to get value"); return 0; } else { return velocity_; @@ -192,7 +130,7 @@ float JointMoteus::get_velocity() const { float JointMoteus::get_servo_torque() const { if(query_type_.torque == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("Torque resolution is set to kIgnore, while attempting to log register"); + LOG_WARN("Torque resolution in QueryCommand is set to kIgnore, while attempting to get value"); return 0; } else { return servo_torque_; @@ -201,7 +139,7 @@ float JointMoteus::get_servo_torque() const { float JointMoteus::get_measured_torque() const { if(query_type_.torque == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("Torque resolution is set to kIgnore, while attempting to log register"); + LOG_WARN("Torque resolution in QueryCommand is set to kIgnore, while attempting to get value"); return 0; } else { return measured_torque_; @@ -210,7 +148,7 @@ float JointMoteus::get_measured_torque() const { float JointMoteus::get_temperature() const { if(query_type_.temperature == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("Temperature resolution is set to kIgnore, while attempting to log register"); + LOG_WARN("Temperature resolution in QueryCommand is set to kIgnore, while attempting to get value"); return 0; } else { return temperature_; @@ -219,7 +157,7 @@ float JointMoteus::get_temperature() const { float JointMoteus::get_q_current() const { if(query_type_.q_current == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("Q Current resolution is set to kIgnore, while attempting to log register"); + LOG_WARN("Q Current resolution in QueryCommand is set to kIgnore, while attempting to get value"); return 0; } else { return q_current_; @@ -228,7 +166,7 @@ float JointMoteus::get_q_current() const { float JointMoteus::get_d_current() const { if(query_type_.d_current == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("D Current resolution is set to kIgnore, while attempting to log register"); + LOG_WARN("D Current resolution in QueryCommand is set to kIgnore, while attempting to get value"); return 0; } else { return d_current_; @@ -237,7 +175,7 @@ float JointMoteus::get_d_current() const { float JointMoteus::get_voltage() const { if(query_type_.voltage == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("Voltage resolution is set to kIgnore, while attemping to log register"); + LOG_WARN("Voltage resolution in QueryCommand is set to kIgnore, while attemping to get value"); return 0; } else { return voltage_; @@ -246,7 +184,7 @@ float JointMoteus::get_voltage() const { const ::mjbots::moteus::Fault & JointMoteus::get_fault() const { if(query_type_.fault == ::mjbots::moteus::Resolution::kIgnore){ - LOG_WARN("Fault code resolution is set to kIgnore, while attempting to log register"); + LOG_WARN("Fault code resolution in QueryCommand is set to kIgnore, while attempting to get value"); return ::mjbots::moteus::Fault::kSuccess; } else { return fault_; @@ -289,6 +227,79 @@ const ::mjbots::moteus::Fault & JointMoteus::get_fault() 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 c8f08e7..68b112e 100644 --- a/src/mjbots_hardware_interface.cpp +++ b/src/mjbots_hardware_interface.cpp @@ -66,13 +66,11 @@ MjbotsHardwareInterface::MjbotsHardwareInterface(std::vector imu_world_offset_deg, bool dry_run, bool print_torques, - bool use_pd_commands, - bool open_loop) + bool use_pd_commands) : imu_data_(imu_data_ptr ? imu_data_ptr : std::make_shared<::kodlab::IMUData>()), dry_run_(dry_run), print_torques_(print_torques), - use_pd_commands_(use_pd_commands), - open_loop_(open_loop) + use_pd_commands_(use_pd_commands) { LOG_IF_WARN(dry_run_, "\nDRY RUN: NO TORQUES COMMANDED"); joints = joint_ptrs; From af89c7c3147000c7af1fdfca302a9988e1bc3b99 Mon Sep 17 00:00:00 2001 From: jdcaporale Date: Thu, 14 Dec 2023 02:43:01 -0500 Subject: [PATCH 19/19] Apply suggestions from code review Remove accidentally added function Co-authored-by: Griffin Addison <82300925+griffinaddison@users.noreply.github.com> --- include/kodlab_mjbots_sdk/joint_moteus.h | 7 ------- src/joint_moteus.cpp | 9 --------- 2 files changed, 16 deletions(-) diff --git a/include/kodlab_mjbots_sdk/joint_moteus.h b/include/kodlab_mjbots_sdk/joint_moteus.h index 56fcf58..fed414c 100644 --- a/include/kodlab_mjbots_sdk/joint_moteus.h +++ b/include/kodlab_mjbots_sdk/joint_moteus.h @@ -175,13 +175,6 @@ class JointMoteus: public JointBase */ float get_velocity() const override; - /** - * @brief Get the torque command. Overrides function in joint_base.h - * - * @return float - */ - float get_servo_torque() const override; - /** * @brief Get the measured joint torque. Overrides function in * joint_base.h diff --git a/src/joint_moteus.cpp b/src/joint_moteus.cpp index 7dbc8c8..7f4c4e5 100644 --- a/src/joint_moteus.cpp +++ b/src/joint_moteus.cpp @@ -128,15 +128,6 @@ float JointMoteus::get_velocity() const { } } -float JointMoteus::get_servo_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 servo_torque_; - } -} - 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");