Skip to content

Commit

Permalink
Merge pull request #90 from KodlabPenn/moteus-register-debug-data
Browse files Browse the repository at this point in the history 
Expanding Moteus Communication, Data Collection
  • Loading branch information
@jdcaporale
jdcaporale authored Dec 14, 2023
2 parents 304ce21 + af89c7c commit e91be3a
Show file tree
Hide file tree
Showing 9 changed files with 536 additions and 115 deletions.
4 changes: 2 additions & 2 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand Down
238 changes: 154 additions & 84 deletions include/kodlab_mjbots_sdk/joint_moteus.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,24 +16,12 @@

namespace kodlab{
namespace mjbots{

/**
* @brief struct for storing or passing moteus joint configurations
* @brief Forward definition of struct for storing moteus joint configurations
*
*/
struct MoteusJointConfig{
int can_id;
int can_bus;
std::string name = "";
int direction = 1;
float zero_offset = 0;
float gear_ratio = 1.0;
float max_torque = std::numeric_limits<float>::infinity();
float pos_min = -std::numeric_limits<float>::infinity();
float pos_max = std::numeric_limits<float>::infinity();
float moteus_kp = 0;
float moteus_kd = 0;
float soft_start_duration_ms = 1;
};
struct MoteusJointConfig;

/**
* @brief A JointBase derived class that encapsulates parameters and functions of
Expand All @@ -55,9 +43,10 @@ class JointMoteus: public JointBase
* @param max_torque /// Maximum torque limit of the joint [N m] (Default:inf)
* @param pos_min /// Minimum joint pose limit before taking protective measures such as torque limiting or shut off (Default:-inf)
* @param pos_max /// Maximum joint pose limit before taking protective measures such as torque limiting or shut off (Default:inf)
* @param soft_start_duration_ms /// Duration of torque limit ramp (soft start) in ms
* @param query_type /// QueryCommand struct containing register resolutions
* @param moteus_kp /// Value of kp set on moteus in units of N m/rev
* @param moteus_kd /// Value of kd set on moteus in units of N m s/rev
* @param soft_start_duration_ms /// Duration of torque limit ramp (soft start) in ms
*/
JointMoteus(
std::string name,
Expand All @@ -70,13 +59,10 @@ class JointMoteus: public JointBase
float pos_min = -std::numeric_limits<float>::infinity(),
float pos_max = std::numeric_limits<float>::infinity(),
float soft_start_duration_ms = 1,
::mjbots::moteus::QueryCommand query_type = kDefaultQuery,
float moteus_kp = 0,
float moteus_kd = 0)
: JointBase(name, direction, zero_offset, gear_ratio, max_torque, pos_min, pos_max, soft_start_duration_ms),
can_bus_(can_bus),
can_id_(can_id),
moteus_kp_(moteus_kp),
moteus_kd_(moteus_kd) {}
float moteus_kd = 0
);

/**
* @brief Construct a new Joint Moteus object without name
Expand All @@ -89,9 +75,10 @@ class JointMoteus: public JointBase
* @param max_torque /// Maximum torque limit of the joint [N m] (Default:inf)
* @param pos_min /// Minimum joint pose limit before taking protective measures such as torque limiting or shut off (Default:-inf)
* @param pos_max /// Maximum joint pose limit before taking protective measures such as torque limiting or shut off (Default:inf)
* @param soft_start_duration_ms /// Duration of torque limit ramp (soft start) in ms
* @param query_type /// QueryCommand struct containing register resolutions
* @param moteus_kp /// Value of kp set on moteus in units of N m/rev
* @param moteus_kd /// Value of kd set on moteus in units of N m s/rev
* @param soft_start_duration_ms /// Duration of torque limit ramp (soft start) in ms
*/
JointMoteus(
int can_id,
Expand All @@ -103,129 +90,212 @@ class JointMoteus: public JointBase
float pos_min = -std::numeric_limits<float>::infinity(),
float pos_max = std::numeric_limits<float>::infinity(),
float soft_start_duration_ms = 1,
::mjbots::moteus::QueryCommand query_type = kDefaultQuery,
float moteus_kp = 0,
float moteus_kd = 0)
: JointBase("", direction, zero_offset, gear_ratio, max_torque, pos_min, pos_max, soft_start_duration_ms),
can_bus_(can_bus),
can_id_(can_id),
moteus_kp_(moteus_kp),
moteus_kd_(moteus_kd) {}

float moteus_kd = 0
);

/**
* @brief Construct a new Joint Moteus object with a MoteusJointConfig struct
*
* @param config MoteusJointConfig input
*/
JointMoteus(MoteusJointConfig config)
: JointBase( config.name,
config.direction,
config.zero_offset,
config.gear_ratio,
config.max_torque,
config.pos_min,
config.pos_max,
config.soft_start_duration_ms),
can_id_( config.can_id),
can_bus_( config.can_bus),
moteus_kp_(config.moteus_kp),
moteus_kd_(config.moteus_kd){}
JointMoteus(MoteusJointConfig config);

/**
* @brief Update the joint of the moteus. Converts rot/s to rad/s and saves other state values.
*
* @param reply_message reply message sent by moteus to pi3hat
*/
void UpdateMoteus(::mjbots::moteus::QueryResult reply_message);

/**
* @brief Destroy the JointMoteus object, virtual to ensure proper
* destruction of derived classes
*/
virtual ~JointMoteus(){}

/**
* @brief Update the joint of the moteus. Converts rot/s to rad/s and saves mode
* @brief Determines whether the QueryCommand struct ignores position
* and velocity when requesting data from the moteus, potentially
* characteristic of an open-loop controller
*
* @param reply_pos position reported by moteus [rot]
* @param reply_vel velocity reported by moteus [rot/s]
* @param reply_torque torque measured by the moteus [Nm]
* @param mode moteus mode
* @return bool
*/
void UpdateMoteus(float reply_pos, float reply_vel, float reply_torque, ::mjbots::moteus::Mode mode ){
UpdateState( 2 * M_PI * reply_pos, 2 * M_PI * reply_vel, reply_torque);
mode_ = mode;
}
bool is_open_loop_query();

/**
* @brief Get the can id
*
* @return int
*/
int get_can_id() const {return can_id_;}
int get_can_id() const;

/**
* @brief Get the can bus object
*
* @return int
*/
int get_can_bus() const {return can_bus_;}
int get_can_bus() const;

/**
* @brief Get the QueryCommand object, which contains resolutions
*
* @return ::mjbots::moteus::QueryCommand
*/
[[nodiscard]] ::mjbots::moteus::QueryCommand get_query_command() const;

/**
* @brief Get the mode reference
* @brief Get the mode reference
*
* @return const ::mjbots::moteus::Mode&
*/
const ::mjbots::moteus::Mode & get_mode_reference() const {return mode_; }
const ::mjbots::moteus::Mode & get_mode_reference() const;

/**
* @brief Get the current operational mode of the servo
*
* @return ::mjbots::moteus::Mode
*/
[[nodiscard]] ::mjbots::moteus::Mode get_mode() const;

/**
* @brief Get the current position of the servo, measured in rotations
* of the output shaft. Overrides function in joint_base.h
*
* @return float
*/
float get_position() const override;

/**
* @brief Get the current velocity of the servo, measured in Hz at the
* output shaft. Overrides function in joint_base.h
*
* @return float
*/
float get_velocity() const override;

/**
* @brief Get the measured joint torque. Overrides function in
* joint_base.h
*
* @return float
*/
float get_measured_torque() const override;

/**
* @brief Get the board temperature, measured in degrees Celsius
*
* @return float
*/
[[nodiscard]] float get_temperature() const;

/**
* @brief Get the current in the Q phase, measured in Amperes
*
* @return float
*/
[[nodiscard]] float get_q_current() const;

/**
* @brief Get the current in the D phase, measured in Amperes
*
* @return float
*/
[[nodiscard]] float get_d_current() const;

/**
* @brief Get the input voltage
*
* @return float
*/
[[nodiscard]] float get_voltage() const;

/**
* @brief Get the fault code, which will be set if the current
* operational mode of the servo is set to 1 (Fault)
*
* @return const ::mjbots::moteus::Fault&
*/
const ::mjbots::moteus::Fault & get_fault() const;

/*!
* @brief accessor for kp_scale
* @return kp_scale
*/
[[nodiscard]] float get_kp_scale() const {
if(moteus_kp_ == 0){
LOG_WARN("Moteus kp is set to 0, while attempting to send pd commands");
return 0;
}
// Multiply by 2pi to convert kp from radians to revolutions
// Divide by gear ratio to get servo kp rather than joint kp
const float kp_scale = kp_/moteus_kp_ * 2 * M_PI/gear_ratio_;
if(kp_scale > 1){
LOG_WARN("kp_scale is greater than 1, will be limited to 1. Either use a lower kp or increase kp in the moteus");
LOG_WARN("With the current moteus kp of %.3f, the max value of the joint kp is %.3f", moteus_kp_, kp_/kp_scale);
}
return kp_scale;
}
[[nodiscard]] float get_kp_scale() const;

/*!
* @brief accessor for kd_scale
* @return kd_scale
*/
[[nodiscard]] float get_kd_scale() const {
if(moteus_kd_ == 0){
LOG_WARN("Moteus kd is set to 0, while attempting to send pd commands");
return 0;
}
const float kd_scale = kd_/moteus_kd_ * 2 * M_PI/gear_ratio_;
if(kd_scale > 1){
LOG_WARN("kd_scale is greater than 1, will be limited to 1. Either use a lower kd or increase kd in the moteus");
LOG_WARN("With the current moteus kd of %.3f, the max value of the joint kd is %.3f", moteus_kp_, kd_/kd_scale);
}
return kd_scale;
}
[[nodiscard]] float get_kd_scale() const;

/*!
* @brief accessor for the moteus position target
* @return the moteus position target in units of revolutions
*/
[[nodiscard]] float get_moteus_position_target()const{return (position_target_ + zero_offset_) * gear_ratio_/direction_/2/M_PI;}
[[nodiscard]] float get_moteus_position_target()const;

/*!
* @brief accessor for the moteus velocity target
* @return the moteus velocity target in units of revolutions/s
*/
[[nodiscard]] float get_moteus_velocity_target()const{return (velocity_target_) * gear_ratio_/direction_/2/M_PI;}
[[nodiscard]] float get_moteus_velocity_target()const;

/**
* @brief Define a default resolution struct
*/
static const ::mjbots::moteus::QueryCommand kDefaultQuery;

/**
* @brief Define a resolution struct that includes torques
*/
static const ::mjbots::moteus::QueryCommand kTorqueQuery;

/**
* @brief Define a resolution struct that adds fields commonly used in
* debugging (board temperature and fault code) to kTorqueQuery
*/
static const ::mjbots::moteus::QueryCommand kDebugQuery;

/**
* @brief Define a resolution struct that includes all useful registers
*/
static const ::mjbots::moteus::QueryCommand kComprehensiveQuery;

private:
int can_id_; /// the can id of this joint's moteus
int can_bus_; /// the can bus the moteus communicates on
::mjbots::moteus::QueryCommand query_type_;
::mjbots::moteus::Mode mode_ = ::mjbots::moteus::Mode::kStopped; /// joint's moteus mode
float moteus_kp_ = 0;
float moteus_kd_ = 0;
float temperature_ = 0;
float q_current_ = 0;
float d_current_ = 0;
float voltage_ = 0;
::mjbots::moteus::Fault fault_ = ::mjbots::moteus::Fault::kSuccess; // fault mode
};

/**
* @brief struct for storing or passing moteus joint configurations
*
*/
struct MoteusJointConfig{
int can_id;
int can_bus;
::mjbots::moteus::QueryCommand query_type = JointMoteus::kDefaultQuery;
std::string name = "";
int direction = 1;
float zero_offset = 0;
float gear_ratio = 1.0;
float max_torque = std::numeric_limits<float>::infinity();
float pos_min = -std::numeric_limits<float>::infinity();
float pos_max = std::numeric_limits<float>::infinity();
float moteus_kp = 0;
float moteus_kd = 0;
float soft_start_duration_ms = 1;
};
}//namespace mjbots
}//namespace kodlab
15 changes: 12 additions & 3 deletions include/kodlab_mjbots_sdk/mjbots_control_loop.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,8 @@ struct ControlLoopOptions {
/// 400, 200, 100.
bool dry_run = false; ///< If true, torques sent to moteus boards will always be zero
bool print_torques = false; ///< If true, torque commands will be printed to console
bool send_pd_commands = false; ///< If true, the control loop will send pd setpoints & gains in addition to ffwd torque commands
bool use_pd_commands = false; ///< If true, the control loop will send pd setpoints & gains in addition to ffwd torque commands
bool open_loop = false; ///< If true, open loop control of the robot is permitted
};

/*!
Expand Down Expand Up @@ -167,7 +168,7 @@ class MjbotsControlLoop : public AbstractRealtimeObject {

template<class log_type, class input_type, class robot_type>
MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::vector<kodlab::mjbots::JointMoteus> joints, const ControlLoopOptions &options)
: MjbotsControlLoop<log_type, input_type,robot_type>( make_share_vector(joints), options){}
: MjbotsControlLoop<log_type, input_type,robot_type>(make_share_vector(joints), options){}

template<class log_type, class input_type, class robot_type>
MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(
Expand All @@ -190,14 +191,22 @@ MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::shar
joints_moteus.push_back(
std::dynamic_pointer_cast<kodlab::mjbots::JointMoteus>(j));
}
// Open loop check
for(const auto & j : joints_moteus){
if(!options.open_loop && j->is_open_loop_query()){
LOG_FATAL("ControlLoopOptions has open_loop = false, but joint moteus QueryCommand struct does not query position and/or velocity from moteus");
kodlab::ActivateCtrlC();
}
}
// Initialize mjbots_interface
mjbots_interface_ = std::make_shared<kodlab::mjbots::MjbotsHardwareInterface>(
std::move(joints_moteus), options.realtime_params,
options.imu_mounting_deg, options.attitude_rate_hz,
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);
}
Expand Down
Loading

0 comments on commit e91be3a

Please sign in to comment.