diff --git a/configs/wx250s_motor_config.yaml b/configs/wx250s_motor_config.yaml deleted file mode 100644 index b4b93b4..0000000 --- a/configs/wx250s_motor_config.yaml +++ /dev/null @@ -1,168 +0,0 @@ -# This file 'organizes' the motors in any Dynamixel-based robot into meaningful units. The deployment will automatically install this file. -# -# Refer to http://emanual.robotis.com/#control-table to look up the control registers for the various motors. -# -# SETUP OF REGISTERS FOR THE INTERBOTIX PLATFORMS -# -# All registers were left to their default values except for the ones listed here: -# 1) ID.........................The ID of the Dynamixel servo. Valids IDs go from 1 - 251. -# 2) Baud_Rate..................The speed at which serial communication occurs. All motors have this set to 1M -# baud (corresponds to a register value of '3') -# 3) Return_Delay_Time..........The amount of time the servo delays in sending a reply packet after receiving a -# command packet. A value of '0' tells the servo to send a reply without any delay. -# 4) Drive_Mode:................Used to define what direction is positive rotation. A value of '0' means CCW is -# positive while a value of '1' means CW is positive. It also defines if profiles are -# built based on velocity or time. 'Velocity' is the default (value of '0') and 'Time' -# is a value of '4'. Adding the 'direction' value to the 'profile' value gives the -# appropiate register value -# 5) Velocity_Limit.............Defines the max speed of the motor. A value of '131' correpsonds to a max speed -# of PI rad/s -# 6) Min_Position_Limit.........Defines the minimum limit of a joint. Values range from 0 to 4095 with 2048 -# being equivalent to '0' rad and 0 being '-PI' rad. -# 7) Max_Position_Limit.........Defines the maximum limit of a joint. Values range from 0 to 4095 with 2048 -# being equivalent to '0' rad and 4095 being 'PI' rad. -# 8) Secondary_ID...............If a joint is controlled by two motors (usually by the shoulder or elbow), one motor -# can be set to follow the commands of another motor by setting this register to the ID -# of the master. A value of '255' disables this. -# -# Each motor's configs are grouped under the name of the joint the motor is controlling. The names are defined at -# http://support.interbotix.com/ under the 'Specifications' section. (Click a robot and scroll down to the 'Default Joint Limits' section.) - -port: /dev/ttyDXL - -joint_order: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] -sleep_positions: [0, -1.80, 1.55, 0, 0.8, 0, 0] - -joint_state_publisher: - update_rate: 1000 - publish_states: true - topic_name: joint_states - -groups: - arm: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] - -grippers: - gripper: - horn_radius: 0.014 - arm_length: 0.024 - left_finger: left_finger - right_finger: right_finger - -shadows: - shoulder: - shadow_list: [shoulder_shadow] - calibrate: true - elbow: - shadow_list: [elbow_shadow] - calibrate: true - -sisters: - -motors: - waist: - ID: 1 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 4 - Velocity_Limit: 131 - Min_Position_Limit: 0 - Max_Position_Limit: 4095 - Secondary_ID: 255 - Position_P_Gain: 400 - Position_D_Gain: 3 - - shoulder: - ID: 2 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 4 - Velocity_Limit: 131 - Min_Position_Limit: 819 - Max_Position_Limit: 3345 - Secondary_ID: 255 - Position_P_Gain: 400 - Position_D_Gain: 3 - - shoulder_shadow: - ID: 3 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 5 - Velocity_Limit: 131 - Min_Position_Limit: 819 - Max_Position_Limit: 3345 - Secondary_ID: 2 - Position_P_Gain: 400 - Position_D_Gain: 3 - - elbow: - ID: 4 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 4 - Velocity_Limit: 131 - Min_Position_Limit: 648 - Max_Position_Limit: 3094 - Secondary_ID: 255 - Position_P_Gain: 400 - Position_D_Gain: 3 - - elbow_shadow: - ID: 5 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 5 - Velocity_Limit: 131 - Min_Position_Limit: 648 - Max_Position_Limit: 3094 - Secondary_ID: 4 - Position_P_Gain: 400 - Position_D_Gain: 3 - - forearm_roll: - ID: 6 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 4 - Velocity_Limit: 131 - Min_Position_Limit: 0 - Max_Position_Limit: 4095 - Secondary_ID: 255 - Position_P_Gain: 400 - Position_D_Gain: 3 - - wrist_angle: - ID: 7 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 5 - Velocity_Limit: 131 - Min_Position_Limit: 910 - Max_Position_Limit: 3447 - Secondary_ID: 255 - Position_P_Gain: 400 - Position_D_Gain: 3 - - wrist_rotate: - ID: 8 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 4 - Velocity_Limit: 131 - Min_Position_Limit: 0 - Max_Position_Limit: 4095 - Secondary_ID: 255 - Position_P_Gain: 500 - Position_D_Gain: 0 - - gripper: - ID: 9 - Baud_Rate: 3 - Return_Delay_Time: 0 - Drive_Mode: 4 - Velocity_Limit: 131 - Min_Position_Limit: 0 - Max_Position_Limit: 4095 - Secondary_ID: 255 - Position_P_Gain: 400 - Position_D_Gain: 3 diff --git a/configs/wx250s_motor_config.yaml b/configs/wx250s_motor_config.yaml new file mode 120000 index 0000000..f7d9727 --- /dev/null +++ b/configs/wx250s_motor_config.yaml @@ -0,0 +1 @@ +../wx_armor/configs/wx250s_motor_config.yaml \ No newline at end of file diff --git a/wx_armor/configs/wx250s_motor_config.yaml b/wx_armor/configs/wx250s_motor_config.yaml index b4b93b4..b4e5a34 100644 --- a/wx_armor/configs/wx250s_motor_config.yaml +++ b/wx_armor/configs/wx250s_motor_config.yaml @@ -5,33 +5,40 @@ # SETUP OF REGISTERS FOR THE INTERBOTIX PLATFORMS # # All registers were left to their default values except for the ones listed here: -# 1) ID.........................The ID of the Dynamixel servo. Valids IDs go from 1 - 251. -# 2) Baud_Rate..................The speed at which serial communication occurs. All motors have this set to 1M -# baud (corresponds to a register value of '3') -# 3) Return_Delay_Time..........The amount of time the servo delays in sending a reply packet after receiving a -# command packet. A value of '0' tells the servo to send a reply without any delay. -# 4) Drive_Mode:................Used to define what direction is positive rotation. A value of '0' means CCW is -# positive while a value of '1' means CW is positive. It also defines if profiles are -# built based on velocity or time. 'Velocity' is the default (value of '0') and 'Time' -# is a value of '4'. Adding the 'direction' value to the 'profile' value gives the -# appropiate register value -# 5) Velocity_Limit.............Defines the max speed of the motor. A value of '131' correpsonds to a max speed -# of PI rad/s -# 6) Min_Position_Limit.........Defines the minimum limit of a joint. Values range from 0 to 4095 with 2048 -# being equivalent to '0' rad and 0 being '-PI' rad. -# 7) Max_Position_Limit.........Defines the maximum limit of a joint. Values range from 0 to 4095 with 2048 -# being equivalent to '0' rad and 4095 being 'PI' rad. -# 8) Secondary_ID...............If a joint is controlled by two motors (usually by the shoulder or elbow), one motor -# can be set to follow the commands of another motor by setting this register to the ID -# of the master. A value of '255' disables this. +# 1) ID............................The ID of the Dynamixel servo. Valid IDs range from 1 to 251. +# 2) Baud_Rate.....................The speed at which serial communication occurs. All motors have this set to 1M +# baud (corresponds to a register value of '3') +# 3) Return_Delay_Time.............The amount of time the servo delays in sending a reply packet after receiving a +# command packet. A value of '0' tells the servo to send a reply without any delay. +# 4) Drive_Mode:...................Used to define what direction is positive rotation. A value of '0' means CCW is +# positive while a value of '1' means CW is positive. It also defines if profiles are +# built based on velocity or time. 'Velocity' is the default (value of '0') and 'Time' +# is a value of '4'. Adding the 'direction' value to the 'profile' value gives the +# appropriate register value +# 5) Velocity_Limit................Defines the max speed of the motor. A value of '131' corresponds to a max speed +# of PI rad/s. NOTE: this is only used in velocity control mode. I.e., has no effect +# in the default position control setting. +# 6) Min_Position_Limit............Defines the minimum limit of a joint. Values range from 0 to 4095 with 2048 +# being equivalent to '0' rad and 0 being '-PI' rad. +# 7) Max_Position_Limit............Defines the maximum limit of a joint. Values range from 0 to 4095 with 2048 +# being equivalent to '0' rad and 4095 being 'PI' rad. +# 8) Secondary_ID..................If a joint is controlled by two motors (usually by the shoulder or elbow), one motor +# can be set to follow the commands of another motor by setting this register to the ID +# of the master. A value of '255' disables this. +# 9) Safety_Velocity_Limit.........Defines the max safety velocity limit [deg/s]. If the joint velocity ever violates +# this limit, we override the current policy and attempt to slowly decelerate to a stop. +# Note that for shadowed motors, though an entry is still expected, this value is unused. +# 10) Safety_Acceleration_Limit....Defines the max safety acceleration limit [deg/s^2]. If the joint acceleration ever violates +# this limit, we override the current policy and attempt to slowly decelerate to a stop. +# Note that for shadowed motors, though an entry is still expected, this value is unused. # # Each motor's configs are grouped under the name of the joint the motor is controlling. The names are defined at # http://support.interbotix.com/ under the 'Specifications' section. (Click a robot and scroll down to the 'Default Joint Limits' section.) port: /dev/ttyDXL -joint_order: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] -sleep_positions: [0, -1.80, 1.55, 0, 0.8, 0, 0] +joint_order: [ waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper ] +sleep_positions: [ 0, -1.80, 1.55, 0, 0.8, 0, 0 ] joint_state_publisher: update_rate: 1000 @@ -39,7 +46,7 @@ joint_state_publisher: topic_name: joint_states groups: - arm: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] + arm: [ waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper ] grippers: gripper: @@ -50,10 +57,10 @@ grippers: shadows: shoulder: - shadow_list: [shoulder_shadow] + shadow_list: [ shoulder_shadow ] calibrate: true elbow: - shadow_list: [elbow_shadow] + shadow_list: [ elbow_shadow ] calibrate: true sisters: @@ -70,6 +77,7 @@ motors: Secondary_ID: 255 Position_P_Gain: 400 Position_D_Gain: 3 + Safety_Velocity_Limit: 180 shoulder: ID: 2 @@ -82,6 +90,7 @@ motors: Secondary_ID: 255 Position_P_Gain: 400 Position_D_Gain: 3 + Safety_Velocity_Limit: 180 shoulder_shadow: ID: 3 @@ -94,6 +103,7 @@ motors: Secondary_ID: 2 Position_P_Gain: 400 Position_D_Gain: 3 + Safety_Velocity_Limit: 180 elbow: ID: 4 @@ -106,6 +116,7 @@ motors: Secondary_ID: 255 Position_P_Gain: 400 Position_D_Gain: 3 + Safety_Velocity_Limit: 180 elbow_shadow: ID: 5 @@ -118,6 +129,7 @@ motors: Secondary_ID: 4 Position_P_Gain: 400 Position_D_Gain: 3 + Safety_Velocity_Limit: 180 forearm_roll: ID: 6 @@ -130,6 +142,7 @@ motors: Secondary_ID: 255 Position_P_Gain: 400 Position_D_Gain: 3 + Safety_Velocity_Limit: 500 wrist_angle: ID: 7 @@ -142,6 +155,7 @@ motors: Secondary_ID: 255 Position_P_Gain: 400 Position_D_Gain: 3 + Safety_Velocity_Limit: 1000 wrist_rotate: ID: 8 @@ -154,6 +168,7 @@ motors: Secondary_ID: 255 Position_P_Gain: 500 Position_D_Gain: 0 + Safety_Velocity_Limit: 1000 gripper: ID: 9 @@ -166,3 +181,4 @@ motors: Secondary_ID: 255 Position_P_Gain: 400 Position_D_Gain: 3 + Safety_Velocity_Limit: 1000 diff --git a/wx_armor/wx_armor/robot_profile.cc b/wx_armor/wx_armor/robot_profile.cc index fe22ce0..7fbf1b3 100644 --- a/wx_armor/wx_armor/robot_profile.cc +++ b/wx_armor/wx_armor/robot_profile.cc @@ -12,12 +12,17 @@ bool convert::decode( for (const auto &child : node["motors"]) { YAML::Node info = child.second; uint8_t motor_id = info["ID"].as(); - profile.motors.emplace_back(MotorInfo{ - .id = motor_id, - .name = child.first.as(), - // By default, set the operation mode to position control. - .op_mode = OpMode::POSITION, - }); + + // Safety velocity limit is given in [deg/s]. Convert it to [rad/s] + float safety_vel_limit = info["Safety_Velocity_Limit"].as(); + safety_vel_limit *= M_PI / 180.; + + profile.motors.emplace_back( + MotorInfo{.id = motor_id, + .name = child.first.as(), + // By default, set the operation mode to position control. + .op_mode = OpMode::POSITION, + .safety_vel_limit = safety_vel_limit}); // Now, populate the register table. for (const auto &kv : info) { @@ -40,6 +45,7 @@ bool convert::decode( name); } profile.joint_ids.emplace_back(motor->id); + profile.joint_motors.emplace_back(motor); profile.joint_names.emplace_back(motor->name); } @@ -95,8 +101,6 @@ std::string_view OpModeName(OpMode mode) { return "CURRENT_BASED_POSITION"; case OpMode::PWM: return "PWM"; - case OpMode::TORQUE: - return "TORQUE"; } return "UNKNOWN"; } diff --git a/wx_armor/wx_armor/robot_profile.h b/wx_armor/wx_armor/robot_profile.h index c776a20..8379717 100644 --- a/wx_armor/wx_armor/robot_profile.h +++ b/wx_armor/wx_armor/robot_profile.h @@ -25,7 +25,9 @@ #pragma once #include +#include #include +#include #include #include #include @@ -43,7 +45,6 @@ enum class OpMode : int { POSITION = 3, // Position controller CURRENT_BASED_POSITION = 5, // Current based position controller PWM = 16, // Pulse-width modulation controller - TORQUE = 100, // Torque controller }; // Convert the OpMode enum to a string e.g. for debugging purpose. @@ -68,9 +69,12 @@ struct MotorInfo { // The following information are currently not used at this moment. Having // them here just to be consistent with dynamixel. OpMode op_mode = OpMode::POSITION; + + // The safety velocity limit in [rad/s] + float safety_vel_limit = M_PI / 2; }; -// The EERPROM area of each motor is logically organized as a a few key/value +// The EERPROM area of each motor is logically organized as a few key/value // pairs, similar to a registry. This class is used to represent such a // key/value pair, together with their associated motor ID. struct RegistryKV { @@ -94,6 +98,9 @@ struct RegistryKV { struct RobotProfile { std::vector motors{}; std::vector joint_ids{}; + // Use this to access the corresponding motor of each joint, in the + // same order as `joint_ids`. + std::vector joint_motors{}; std::vector joint_names{}; std::vector eeprom{}; @@ -111,6 +118,19 @@ struct RobotProfile { } return nullptr; } + + // Returns true if the motor with desired name is found and the + // callback is invoked on it. Returns false otherwise. + inline bool UpdateMotorByName( + const std::string name, std::function callback) { + for (MotorInfo &motor : motors) { + if (motor.name == name) { + callback(&motor); + return true; + } + } + return false; + } }; } // namespace horizon::wx_armor diff --git a/wx_armor/wx_armor/wx_armor_driver.cc b/wx_armor/wx_armor/wx_armor_driver.cc index b0ffbc6..e3bf2e6 100644 --- a/wx_armor/wx_armor/wx_armor_driver.cc +++ b/wx_armor/wx_armor/wx_armor_driver.cc @@ -1,5 +1,6 @@ #include "wx_armor/wx_armor_driver.h" +#include #include #include #include @@ -103,6 +104,12 @@ void FlashEEPROM(DynamixelWorkbench *dxl_wb, const RobotProfile &profile) { size_t num_failed = 0; for (const RegistryKV &kv : profile.eeprom) { int32_t current_value = 0; + + // Hacky fix for now... Probably a better way to handle this + // Safety_Velocity_Limit is our own custom param. Don't write it to EEPROM + if (kv.key == "Safety_Velocity_Limit") { + continue; + } dxl_wb->itemRead(kv.motor_id, kv.key.c_str(), ¤t_value); // The EEPROM on the motors has a limited number of writes during its @@ -286,7 +293,8 @@ void RebootMotorIfInErrorState(DynamixelWorkbench *dxl_wb, WxArmorDriver::WxArmorDriver(const std::string &usb_port, fs::path motor_config_path, bool flash_eeprom, - int32_t current_limit) + int32_t current_limit, + bool gripper_use_pwm_control) : profile_(LoadConfigOrDie(motor_config_path).as()) { WaitUntilPortAvailable(&dxl_wb_, usb_port); @@ -325,6 +333,10 @@ WxArmorDriver::WxArmorDriver(const std::string &usb_port, CalibrateShadowOrDie(&dxl_wb_, profile_); SetCurrentLimit(&dxl_wb_, &profile_, current_limit); + if (gripper_use_pwm_control) { + UsePWMControlOnGripper(); + } + // Now torque back on. TorqueOn(); @@ -417,45 +429,145 @@ std::optional WxArmorDriver::Read() { return std::move(result); } -void WxArmorDriver::SetPosition(const std::vector &position) { - const uint8_t num_joints = static_cast(profile_.joint_ids.size()); - std::vector int_command(num_joints, 0); - for (size_t i = 0; i < profile_.joint_ids.size(); ++i) { - int_command[i] = - dxl_wb_.convertRadian2Value(profile_.joint_ids[i], position.at(i)); +std::vector WxArmorDriver::GetSafetyVelocityLimits() { + std::vector safety_velocity_limits; + + int counter = 0; + for (const auto &motor : profile_.motors) { + // Skip the shadowed motors, which have IDs 3 and 5. + if (counter != 3 && counter != 5) { + safety_velocity_limits.push_back(motor.safety_vel_limit); + } + counter++; } + return safety_velocity_limits; +} - const char *log = nullptr; - std::unique_lock lock{io_mutex_}; +namespace { - // NOTE: The number of data for each motor (= 1) in this call to syncWrite() - // means that each motor will take one int32_t value from int_command.data(). - bool success = dxl_wb_.syncWrite(write_position_handler_index_, - profile_.joint_ids.data(), - num_joints, - int_command.data(), - 1, /* number of data for each motor */ - &log); - lock.unlock(); - if (!success) { - spdlog::error("Cannot write position command: {}", log); +// This class manages a buffer that is used to store the integer commands that +// will be sent to the motor. Following Dynamixel's convention, the buffer is +// organized with DWORD (i.e. 4 bytes) as the unit, where each element is an +// int32. Currently for each motor we are going to write 5 DWORDs, see below. +class CommandBuffer { + public: + // According to Dynamixel: + // + // Goal PWM (2 Bytes) Default = 885 (PWM limit) + // Goal Current (2 Bytes) Default = 1193 (Current Limit) + // Goal Velocity (4 Bytes) Default = 131 (Via experiment) + // Profile Acc (4 bytes) Default = 0 + // Profile Vel (4 Bytes) Default = 0 + // Goal Position (4 Bytes) Default = 0 + static constexpr int NUM_DWORDS = 5; + static constexpr int16_t PWM_LIMIT = 885; + static constexpr int16_t CURRENT_LIMIT = 1193; + // The Goal PWM and Goal Current share the 4 bytes, with Goal PWM on the low + // bits and Goal Current on the high bits. + static constexpr int32_t DEFAULT_PWM_CURRENT = + (CURRENT_LIMIT << 16) | PWM_LIMIT; + static constexpr int32_t DEFAULT_VELOCITY = 131; + // The unit that converts torque in N·m to the integer current representation + // used in Dyanmixel. + static constexpr float NEWTON_METER_TO_INT_CURRENT = CURRENT_LIMIT * 0.666667; + // The unit that converts torque in N·m to the integer pwm representation used + // in Dyanmixel. + static constexpr float NEWTON_METER_TO_INT_PWM = PWM_LIMIT * 0.5; + + CommandBuffer(int num_joints) : buffer_(num_joints * NUM_DWORDS, 0) { + // Intialize the Goal PWM and Current. We need to initialize them because + // even in other Operation Mode such as position control, those values are + // used as limits. + for (size_t i = 0; i < buffer_.size(); i += NUM_DWORDS) { + buffer_[i] = DEFAULT_PWM_CURRENT; + buffer_[i + 1] = DEFAULT_VELOCITY; + } + + buffer_ptr_ = buffer_.data(); + } + + inline void WritePosition(int32_t int_pos, + int32_t moving_time_ms, + int32_t acc_time_ms) { + buffer_ptr_[2] = acc_time_ms; + buffer_ptr_[3] = moving_time_ms; + buffer_ptr_[4] = int_pos; + buffer_ptr_ += NUM_DWORDS; + } + + inline void WriteCurrent(float tau) { + int16_t int_current = + static_cast(std::round(NEWTON_METER_TO_INT_CURRENT * tau)); + buffer_ptr_[0] = (int_current << 16) | PWM_LIMIT; + buffer_ptr_ += NUM_DWORDS; + } + + inline void WritePWM(float tau) { + int16_t int_pwm = + static_cast(std::round(NEWTON_METER_TO_INT_PWM * tau)); + // We need to put the 2 bytes (signed) integer PWM in the position of the + // lower 2 bytes at buffer's [0]. This is how we do it. + uint16_t unsigned_pwm = static_cast(int_pwm); + buffer_ptr_[0] = 0 | unsigned_pwm; + buffer_ptr_ += NUM_DWORDS; + } + + inline void WriteVelocity(int32_t int_velocity) { + buffer_ptr_[2] = int_velocity; + buffer_ptr_ += NUM_DWORDS; } -} -void WxArmorDriver::SetPosition(const std::vector &position, - float moving_time) { + inline int32_t *data() { + // Returns the content of the buffer. Useful when writing to the motors. + return buffer_.data(); + } + + private: + std::vector buffer_{}; + // Each time a Write* method is called, the buffer_ptr_ will increment by the + // number of DWORDs per motor, so that the next Write* method invocation will + // write to the area corresponding to the next motor. + int32_t *buffer_ptr_ = nullptr; +}; + +} // namespace + +void WxArmorDriver::SendCommand(const std::vector &targets, + float moving_time, + float acc_time) { + assert(moving_time / 2 >= acc_time && + "Acceleration time cannot be more than half the moving time."); + int32_t moving_time_ms = static_cast(moving_time * 1000.0); + int32_t acc_time_ms = static_cast(acc_time * 1000.0); const uint8_t num_joints = static_cast(profile_.joint_ids.size()); - std::vector int_command(num_joints * 3, 0); - size_t j = 0; - for (size_t i = 0; i < profile_.joint_ids.size(); ++i) { - // Profile acceleration - int_command[j++] = 0; - // Profile velocity - int_command[j++] = moving_time_ms; - // Goal Position - int_command[j++] = - dxl_wb_.convertRadian2Value(profile_.joint_ids[i], position.at(i)); + CommandBuffer buffer{num_joints}; + const float *raw_target = targets.data(); + + for (const MotorInfo *motor : profile_.joint_motors) { + switch (motor->op_mode) { + case OpMode::POSITION: + case OpMode::CURRENT_BASED_POSITION: + buffer.WritePosition( + dxl_wb_.convertRadian2Value(motor->id, *raw_target), + moving_time_ms, + acc_time_ms); + break; + case OpMode::CURRENT: + buffer.WriteCurrent(*raw_target); + break; + case OpMode::PWM: + buffer.WritePWM(*raw_target); + break; + case OpMode::VELOCITY: + buffer.WriteVelocity( + dxl_wb_.convertVelocity2Value(motor->id, *raw_target)); + break; + default: + spdlog::critical("Operation Mode {} is not supported", + OpModeName(motor->op_mode)); + } + ++raw_target; } const char *log = nullptr; @@ -463,12 +575,14 @@ void WxArmorDriver::SetPosition(const std::vector &position, // NOTE: The number of data for each motor (= 1) in this call to syncWrite() // means that each motor will take one int32_t value from int_command.data(). - bool success = dxl_wb_.syncWrite(write_position_and_profile_handler_index_, - profile_.joint_ids.data(), - num_joints, - int_command.data(), - 3, /* number of data for each motor */ - &log); + bool success = dxl_wb_.syncWrite( + write_handler_index_, + profile_.joint_ids.data(), + num_joints, + buffer.data(), + CommandBuffer::NUM_DWORDS, /* number of words (4 bytes) per motor */ + &log); + lock.unlock(); if (!success) { spdlog::error("Cannot write position command: {}", log); @@ -493,36 +607,67 @@ void WxArmorDriver::SetPID(const std::vector &gain_cfgs) { std::lock_guard lock{io_mutex_}; auto SetPIDSingleMotor = - [this](uint8_t motor_id, int32_t p, int32_t i, int32_t d) -> void { + [this](const MotorInfo &motor, int32_t p, int32_t i, int32_t d) -> void { + if (motor.op_mode != OpMode::POSITION && + motor.op_mode != OpMode::CURRENT_BASED_POSITION) { + spdlog::warn( + "Motor '{}' is currently in {} control mode, and will ignore PID " + "setting.", + motor.name, + OpModeName(motor.op_mode)); + return; + } const char *log; - if (!dxl_wb_.itemWrite(motor_id, "Position_P_Gain", p, &log)) { - spdlog::error("Failed To set P Gain of motor {}: {}", motor_id, log); + if (!dxl_wb_.itemWrite(motor.id, "Position_P_Gain", p, &log)) { + spdlog::error("Failed To set P Gain of motor {}: {}", motor.id, log); } - if (!dxl_wb_.itemWrite(motor_id, "Position_I_Gain", i, &log)) { - spdlog::error("Failed To set I Gain of motor {}: {}", motor_id, log); + if (!dxl_wb_.itemWrite(motor.id, "Position_I_Gain", i, &log)) { + spdlog::error("Failed To set I Gain of motor {}: {}", motor.id, log); } - if (!dxl_wb_.itemWrite(motor_id, "Position_D_Gain", d, &log)) { - spdlog::error("Failed To set D Gain of motor {}: {}", motor_id, log); + if (!dxl_wb_.itemWrite(motor.id, "Position_D_Gain", d, &log)) { + spdlog::error("Failed To set D Gain of motor {}: {}", motor.id, log); } spdlog::info( - "Successfully set PID = ({}, {}, {}) for motor {}", p, i, d, motor_id); + "Successfully set PID = ({}, {}, {}) for motor {}", p, i, d, motor.id); }; for (const PIDGain &gain_cfg : gain_cfgs) { if (gain_cfg.name == "all") { for (const MotorInfo &motor : profile_.motors) { - SetPIDSingleMotor(motor.id, gain_cfg.p, gain_cfg.i, gain_cfg.d); + SetPIDSingleMotor(motor, gain_cfg.p, gain_cfg.i, gain_cfg.d); } } else if (const MotorInfo *motor = profile_.motor(gain_cfg.name)) { - SetPIDSingleMotor(motor->id, gain_cfg.p, gain_cfg.i, gain_cfg.d); + SetPIDSingleMotor(*motor, gain_cfg.p, gain_cfg.i, gain_cfg.d); } else { spdlog::error("Cannot set PID: motor '{}' not found", gain_cfg.name); } } } +void WxArmorDriver::UsePWMControlOnGripper() { + if (!profile_.UpdateMotorByName("gripper", [this](MotorInfo *motor) { + const char *log; + if (!dxl_wb_.setPWMControlMode(motor->id, &log)) { + spdlog::error("Cannot set gripper control to PWM control: {}", log); + } else { + motor->op_mode = OpMode::PWM; + spdlog::info("Gripper is now updated to use PWM control"); + } + })) { + spdlog::error("Cannot find gripper motor."); + } +} + +bool WxArmorDriver::SafetyViolationTriggered() { + return safety_violation_.load(); +} + +void WxArmorDriver::TriggerSafetyViolationMode() { safety_violation_ = true; } + +void WxArmorDriver::ResetSafetyViolationMode() { safety_violation_ = false; } + ControlItem WxArmorDriver::AddItemToRead(const std::string &name) { - // Here we assume that the data allocation on all of the motors are + // Here we assume that the data allocation on all the motors are // identical. Therefore, we can just read the address of the motor ID and // call it a day. const ControlItem *address = @@ -563,88 +708,57 @@ void WxArmorDriver::InitReadHandler() { } void WxArmorDriver::InitWriteHandler() { - static constexpr char GOAL_POSITION[] = "Goal_Position"; - static constexpr char PROFILE_ACC[] = "Profile_Acceleration"; - static constexpr char PROFILE_VEL[] = "Profile_Velocity"; - - OpMode op_mode = profile_.motors.front().op_mode; - if (op_mode != OpMode::POSITION && - op_mode != OpMode::CURRENT_BASED_POSITION) { - spdlog::critical( - "WxArmorDriver only support Operation Mode 'POSITION' and " - "'CURRENT_BASED_POSITION'."); - std::abort(); - } - - // Add the write handler that writes only the goal position - const ControlItem *goal_position_address = - dxl_wb_.getItemInfo(profile_.joint_ids.front(), GOAL_POSITION); - if (goal_position_address == nullptr) { - spdlog::critical("Cannot find onboard item '{}' to write.", GOAL_POSITION); - std::abort(); - } + const char *prev_item_name = nullptr; - write_position_address_ = *goal_position_address; + auto ExtendWriteAddressRange = [this, &prev_item_name](const char *name) { + const ControlItem *item = + dxl_wb_.getItemInfo(profile_.joint_ids.front(), name); - write_position_handler_index_ = dxl_wb_.getTheNumberOfSyncWriteHandler(); - if (!dxl_wb_.addSyncWriteHandler(write_position_address_.address, - write_position_address_.data_length)) { - spdlog::critical("Failed to add sync write handler for {}", GOAL_POSITION); - std::abort(); - } else { - spdlog::info( - "Registered sync write handler for {} (address = {}, length = {})", - GOAL_POSITION, - write_position_address_.address, - write_position_address_.data_length); - } - - // Add the write handler that writes the goal position, as well as - // the velocity and acceleration profile. - const ControlItem *profile_vel_address = - dxl_wb_.getItemInfo(profile_.joint_ids.front(), PROFILE_VEL); - const ControlItem *profile_acc_address = - dxl_wb_.getItemInfo(profile_.joint_ids.front(), PROFILE_ACC); - if (profile_acc_address->address + profile_acc_address->data_length != - profile_vel_address->address) { - spdlog::critical( - "The register address of profile acceleration is not " - "next to that of profile velocity"); - std::abort(); - } - if (profile_vel_address->address + profile_vel_address->data_length != - goal_position_address->address) { - spdlog::critical( - "The register address of profile velocity is not " - "next to that of goal position"); - std::abort(); - } + if (prev_item_name == nullptr) { + write_address_.address = item->address; + write_address_.data_length = item->data_length; + } else { + // Otherwise, extend the current write address (range). However we will + // first verify that the next item is an immediate follower. + if (write_address_.address + write_address_.data_length != + item->address) { + spdlog::critical( + "Expecting the address of '{}' to immediately follow " + "the address of '{}'. However, getting {} instead of {}", + name, + prev_item_name, + item->address, + write_address_.address + write_address_.data_length); + std::abort(); + } + write_address_.data_length += item->data_length; + } + prev_item_name = name; + }; - write_position_and_profile_handler_index_ = - dxl_wb_.getTheNumberOfSyncWriteHandler(); + // First retrieve all the addresses of interest, and ensure that they form a + // contiguous address range and follow the expected order. The resulting write + // address range will be stored in `write_address_`. + ExtendWriteAddressRange("Goal_PWM"); + ExtendWriteAddressRange("Goal_Current"); + ExtendWriteAddressRange("Goal_Velocity"); + ExtendWriteAddressRange("Profile_Acceleration"); + ExtendWriteAddressRange("Profile_Velocity"); + ExtendWriteAddressRange("Goal_Position"); - uint16_t start = profile_acc_address->address; - uint16_t length = profile_acc_address->data_length + - profile_vel_address->data_length + - goal_position_address->data_length; + // Next, creates the write handler. + write_handler_index_ = dxl_wb_.getTheNumberOfSyncWriteHandler(); - if (!dxl_wb_.addSyncWriteHandler(start, length)) { - spdlog::critical("Failed to add sync write handler for {} + {} + {}", - PROFILE_VEL, - PROFILE_ACC, - GOAL_POSITION); + const char *log = nullptr; + if (!dxl_wb_.addSyncWriteHandler( + write_address_.address, write_address_.data_length, &log)) { + spdlog::critical("Failed to create the sync writer: {}", log); std::abort(); - } else { - spdlog::info( - "Registered sync write handler for {} + {} + {} (address = {}, " - "length " - "= {})", - PROFILE_VEL, - PROFILE_ACC, - GOAL_POSITION, - start, - length); } + + spdlog::info("Registered sync writer handler (address = {}, length = {})", + write_address_.address, + write_address_.data_length); } } // namespace horizon::wx_armor diff --git a/wx_armor/wx_armor/wx_armor_driver.h b/wx_armor/wx_armor/wx_armor_driver.h index 248788e..6e0f9fc 100644 --- a/wx_armor/wx_armor/wx_armor_driver.h +++ b/wx_armor/wx_armor/wx_armor_driver.h @@ -117,9 +117,7 @@ struct PIDGain { * @code * WxArmorDriver driver("/dev/ttyUSB0", "path/to/motor_config.yaml"); * driver.TorqueOn(); - * driver.SetPosition({1.0, 1.5, 1.2}); - * driver.FetchSensorData(); - * auto sensorJson = driver.SensorDataToJson(); + * driver.SendCommand({1.0, 1.5, 1.2}); * @endcode * * @note This driver is specific to robots with Dynamixel motors and requires @@ -141,7 +139,8 @@ class WxArmorDriver { WxArmorDriver(const std::string &usb_port, std::filesystem::path motor_config_path, bool flash_eeprom = false, - int32_t current_limit = 250); + int32_t current_limit = 250, + bool gripper_use_pwm_control = false); ~WxArmorDriver(); @@ -156,19 +155,26 @@ class WxArmorDriver { std::optional Read(); /** - * @brief Sets the position of the robot's joints. - * @param position A vector of floats representing the desired joint - * positions. + * @brief Reads the safety velocity limits from RobotProfile and returns it. + * + * @return An array containing the safety velocity limits for each motor + * [rad/s]. */ - void SetPosition(const std::vector &position); + std::vector GetSafetyVelocityLimits(); /** - * @brief Sets the position of the robot's joints, with a desired moving time. - * @param position A vector of floats representing the desired joint - * positions. - * @param moving_time A float in seconds + * @brief Send command to each of the motors. + * @details If acc_time is zero, constant velocity is used. + * @param targets A vector of floats representing the desired joint + * commands, one for each of the joints. Note that the meaning of the + * joint command depends on its operating mode (i.e. position control, + * pwm control, etc). + * @param moving_time A float in seconds (only for position control joints) + * @param acc_time A float in seconds (only for position control joints) */ - void SetPosition(const std::vector &position, float moving_time); + void SendCommand(const std::vector &targets, + float moving_time, + float acc_time = 0.0); /** * @brief Activates the torque in the robot's motors. @@ -216,13 +222,46 @@ class WxArmorDriver { * * Usage Examples: * - To set PID gains for a specific motor: [{"name": "waist", "p": - * 800, "i": 0, "d": 3}] - * - To set PID gains for all motors, with a different configuration for one - * motor: [{"name": "all", "p": 800, "i": 0, "d": 3}, {"name": "wrist", - * "p": 400, "i": 0, "d": 0}] + * 800, "i": 0, "d": 3}] + * - To set PID gains for all motors, with a different configuration for + * one motor: [{"name": "all", "p": 800, "i": 0, "d": 3}, {"name": + * "wrist", "p": 400, "i": 0, "d": 0}] */ void SetPID(const std::vector &gain_cfgs); + /** + * @brief Switch the gripper motor from position control to PWM control. + * + * The gripper is usually powered by the XL430-W250 motor, which does not + * offer true current (torque) control. The PWM control mode is the closest + * mode we can use to mimic the torque control. + */ + void UsePWMControlOnGripper(); + + /** + * @brief Returns the current safety violation mode status. + * + * @return A bool describing whether a safety violation is currently + * triggered and not reset. + */ + bool SafetyViolationTriggered(); + + /** + * @brief Sets off safety violation mode. + * + * @details For functionality to return to normal, ResetSafetyViolation() + * must be called. + */ + void TriggerSafetyViolationMode(); + + /** + * @brief Resets the safety violation status back to false. + * + * @note Currently, this gets called only when a new client connection + * is made after all previous connections are closed. + */ + void ResetSafetyViolationMode(); + private: ControlItem AddItemToRead(const std::string &name); @@ -268,18 +307,17 @@ class WxArmorDriver { // │ Write │ // └──────────────────┘ - // Unlike write, in the future we may have write handler for each of the - // operation mode (e.g. position control, velocity control, pwm control). - // Therefore we will need to store handler index for each of them. - uint8_t write_position_handler_index_; + // Index to the write handler that is used to write the control signals to the + // driver. Since we have only one writer at this moment, so the index is + // exepcted to be 0. + uint8_t write_handler_index_; - // Similar to how we read, we also write to identical addresses on each - // motor. - ControlItem write_position_address_; + // The range of addresses on each motor to write to for each of the command. + ControlItem write_address_; - // Index to the write handler that writes both the position and velocity and - // acceleration profile, and the corresponding register addresses. - uint8_t write_position_and_profile_handler_index_; + // Flag that gets triggered when safety violations such as + // velocity limits are violated. + std::atomic_bool safety_violation_{false}; }; } // namespace horizon::wx_armor diff --git a/wx_armor/wx_armor/wx_armor_ws.cc b/wx_armor/wx_armor/wx_armor_ws.cc index 075a2a6..9747c9a 100644 --- a/wx_armor/wx_armor/wx_armor_ws.cc +++ b/wx_armor/wx_armor/wx_armor_ws.cc @@ -27,9 +27,14 @@ WxArmorDriver *Driver() { .parent_path() / "configs" / "wx250s_motor_config.yaml"); int flash_eeprom = true; - int current_limit = GetEnv("WX_ARMOR_MOTOR_CURRENT_LIMIT", 250); - return std::make_unique( - usb_port, motor_config, static_cast(flash_eeprom), current_limit); + int current_limit = GetEnv("WX_ARMOR_MOTOR_CURRENT_LIMIT", 0); + bool gripper_use_pwm_control = + GetEnv("WX_ARMOR_GRIPPER_PWM_CTRL", false); + return std::make_unique(usb_port, + motor_config, + static_cast(flash_eeprom), + current_limit, + gripper_use_pwm_control); }(); return driver.get(); } @@ -39,6 +44,12 @@ void WxArmorWebController::handleNewMessage(const WebSocketConnectionPtr &conn, const WebSocketMessageType &type) { std::string_view payload{}; + if (Driver()->SafetyViolationTriggered()) { + spdlog::error( + "Call from message handler ignored. Safety violation was encountered."); + return; + } + // Valid message should be in the format of " ". This helper // function try to match the message with predefined command, and if there is // a match, put the payload into `payload`. @@ -53,7 +64,7 @@ void WxArmorWebController::handleNewMessage(const WebSocketConnectionPtr &conn, }; if (type == WebSocketMessageType::Text) { - if (Match("SETPOS")) { + if (Match("SETPOS")) { // SETPOS is soon to be deprecated. // Update the states for bookkeeping purpose. ClientState &state = conn->getContextRef(); state.engaging = true; @@ -61,11 +72,11 @@ void WxArmorWebController::handleNewMessage(const WebSocketConnectionPtr &conn, // Relay the command to the driver. nlohmann::json json = nlohmann::json::parse(payload); - std::vector position(json.size()); + std::vector targets(json.size()); for (size_t i = 0; i < json.size(); ++i) { - position[i] = json.at(i).get(); + targets[i] = json.at(i).get(); } - Driver()->SetPosition(position); + Driver()->SendCommand(targets, 0.0); } else if (Match("MOVETO")) { // Update the states for bookkeeping purpose. ClientState &state = conn->getContextRef(); @@ -75,12 +86,12 @@ void WxArmorWebController::handleNewMessage(const WebSocketConnectionPtr &conn, // Relay the command to the driver. Note that the last numbers // in the list is the moving time, in seconds. nlohmann::json json = nlohmann::json::parse(payload); - std::vector position(json.size() - 1); + std::vector targets(json.size() - 1); for (size_t i = 0; i < json.size() - 1; ++i) { - position[i] = json.at(i).get(); + targets[i] = json.at(i).get(); } float moving_time = json.at(json.size() - 1).get(); - Driver()->SetPosition(position, moving_time); + Driver()->SendCommand(targets, moving_time); } else if (Match("TORQUE ON")) { Driver()->TorqueOn(); } else if (Match("TORQUE OFF")) { @@ -94,24 +105,46 @@ void WxArmorWebController::handleNewMessage(const WebSocketConnectionPtr &conn, void WxArmorWebController::handleConnectionClosed( const WebSocketConnectionPtr &conn) { - publisher_.Unsubscribe(conn); + guardian_thread_.Unsubscribe(conn); spdlog::info("A connection is closed."); } void WxArmorWebController::handleNewConnection( const HttpRequestPtr &req, const WebSocketConnectionPtr &conn) { conn->setContext(std::make_shared()); - publisher_.Subscribe(conn); + guardian_thread_.Subscribe(conn); spdlog::info("A new connection is established."); conn->send("ok"); } static constexpr int MAX_TOLERABLE_CONSECUTIVE_NUM_READ_ERRORS = 6; -WxArmorWebController::Publisher::Publisher() { +void SlowDownToStop(const SensorData &curr_reading, + float dt = 0.5, + float deceleration_time = 1.5) { + std::vector curr_pos = curr_reading.pos; + std::vector curr_vel = curr_reading.vel; + std::vector targets; + + for (size_t i = 0; i < curr_pos.size(); i++) { + // Just some simple kinematic integration to minimize acceleration changes + float curr_target = curr_pos[i] + curr_vel[i] * dt; + targets.push_back(curr_target); + } + + // Overwrite the current trajectory with our decelerating one. + Driver()->SendCommand(targets, deceleration_time, 0.49 * deceleration_time); +} + +WxArmorWebController::GuardianThread::GuardianThread() { thread_ = std::jthread([this]() { + std::vector safety_velocity_limits = + Driver()->GetSafetyVelocityLimits(); while (!shutdown_.load()) { + // Read the sensor data std::optional sensor_data = Driver()->Read(); + + // Publish the sensor data if (sensor_data.has_value()) { num_consecutive_read_errors_ = 0; std::string message = nlohmann::json(sensor_data.value()).dump(); @@ -119,6 +152,36 @@ WxArmorWebController::Publisher::Publisher() { for (const WebSocketConnectionPtr &conn : conns_) { conn->send(message); } + + // Don't check for a new safety violation if state hasn't been reset yet + if (Driver()->SafetyViolationTriggered()) { + spdlog::error( + "Guardian thread safety violation checking ignored. Safety " + "violation is already triggered."); + continue; + } + + // GuardianThread also has the dual role of monitoring for safety + // e.g., checking for velocity limit violations + std::vector curr_velocities = sensor_data.value().vel; + for (int i = 0; i < curr_velocities.size(); i++) { + float cv = fabs(curr_velocities[i]); + float limit = safety_velocity_limits[i]; + if (cv > limit) { + Driver()->TriggerSafetyViolationMode(); + break; + } + } + + // If a safety violation is triggered, first slow down to a stop + // and then kill all client connections. + // This way, user will have to reset on client-side, but the server + // will keep running. + if (Driver()->SafetyViolationTriggered()) { + SlowDownToStop(sensor_data.value()); + KillConnections(); + } + } else { // When fail to read, accumulate the counter, check for threshold and // crash if threshold is exceeded. @@ -137,23 +200,36 @@ WxArmorWebController::Publisher::Publisher() { }); } -WxArmorWebController::Publisher::~Publisher() { +WxArmorWebController::GuardianThread::~GuardianThread() { shutdown_.store(true); if (thread_.joinable()) { thread_.join(); } } -void WxArmorWebController::Publisher::Subscribe( +void WxArmorWebController::GuardianThread::Subscribe( const WebSocketConnectionPtr &conn) { + // Clear all safety violations only if there are no previous connections + if (conns_.empty()) { + Driver()->ResetSafetyViolationMode(); + } std::lock_guard lock{conns_mutex_}; conns_.emplace_back(conn); } -void WxArmorWebController::Publisher::Unsubscribe( +void WxArmorWebController::GuardianThread::Unsubscribe( const WebSocketConnectionPtr &conn) { std::lock_guard lock{conns_mutex_}; conns_.erase(std::remove(conns_.begin(), conns_.end(), conn), conns_.end()); } +void WxArmorWebController::GuardianThread::KillConnections() { + for (auto &conn : conns_) { + // TODO(andrew): log which joint was responsible? + conn->shutdown(drogon::CloseCode::kViolation, + "Shutting down due to velocity limit violation."); + } + conns_.clear(); +} + } // namespace horizon::wx_armor diff --git a/wx_armor/wx_armor/wx_armor_ws.h b/wx_armor/wx_armor/wx_armor_ws.h index e773091..16fb0b4 100644 --- a/wx_armor/wx_armor/wx_armor_ws.h +++ b/wx_armor/wx_armor/wx_armor_ws.h @@ -53,18 +53,19 @@ class WxArmorWebController WS_PATH_LIST_END private: - // The publisher is responsible for continuously (by running a background + // The GuardianThread is responsible for 1) continuously (by running a background // thread) read the sensor data and publish it to the clients that subscribe - // the sensor data. - class Publisher { + // the sensor data and 2) monitoring for safety violations and notifying + // WxArmorDriver accordingly to prevent system damage. + class GuardianThread { public: - Publisher(); - ~Publisher(); + GuardianThread(); + ~GuardianThread(); - // Add the client (identified by the connection) to the subscribption list. + // Add the client (identified by the connection) to the subscription list. void Subscribe(const drogon::WebSocketConnectionPtr &conn); - // Remove the client (identified by the connection) to the subscribption + // Remove the client (identified by the connection) to the subscription // list. void Unsubscribe(const drogon::WebSocketConnectionPtr &conn); @@ -75,13 +76,16 @@ class WxArmorWebController std::atomic_bool shutdown_{false}; std::jthread thread_{}; + // Close and remove all client connections from subscription list + void KillConnections(); + // Book keeping of the number of read errors in a row. Reset to 0 as soon as // a successful read is seen. Server will crash as soon as this number // exceeds the threshold. int num_consecutive_read_errors_ = 0; }; - Publisher publisher_; + GuardianThread guardian_thread_; }; // Helper function to read the environment variable.