diff --git a/native/pi_control/include/pi_device_config.hpp b/native/pi_control/include/pi_device_config.hpp index ab8223e..f31e311 100644 --- a/native/pi_control/include/pi_device_config.hpp +++ b/native/pi_control/include/pi_device_config.hpp @@ -137,6 +137,7 @@ class DeviceConfig { const std::string fn_servo_home_pos = "home_pos"; ///< Field name for home position (relative radian). const std::string fn_servo_response_delay = "response_delay"; ///< Field name for response_delay (sec). const std::string fn_servo_response_can_id = "response_can_id"; ///< Field name for the CAN id a passive encoder answers on (optional; default encoder id + 1, the i2rt ``plus_one`` firmware receive mode). + const std::string fn_passive_encoder_firmware_compat = "passive_encoder_firmware_compat"; ///< Field name (bool, passive encoders only; optional, default false). When true the driver tolerates multiple teaching-handle firmware revisions during EEPROM/frequency setup (both the <=2.2.x READINGS and >=2.3.x GET_EEPROM reply formats, plus the legacy <=2.2.12 8-bit frequency layout). Isolated to the YAM handle: ARX effectors have no passive encoder and never read this. const std::string fn_servo_reverse = "reverse_flag"; ///< Field name for direction reverse flag (bool). const std::string fn_joystick_deadband = "joystick_deadband"; ///< Field name for joystick deadband (raw byte half-width applied symmetrically around the X/Y channel center). diff --git a/native/pi_control/include/pi_driver_arx.hpp b/native/pi_control/include/pi_driver_arx.hpp index 194e33d..fcd7e7c 100644 --- a/native/pi_control/include/pi_driver_arx.hpp +++ b/native/pi_control/include/pi_driver_arx.hpp @@ -42,6 +42,7 @@ class PassiveEncoderRoute { public: int encoder_id_; ///< Encoder request CAN id (the report arrives on the map key). int data_index_; ///< Cache slot in received_servo_data_ for this encoder. + bool firmware_compat_ = false; ///< Tolerate multiple teaching-handle firmware revisions during EEPROM/frequency setup (config ``passive_encoder_firmware_compat``). Default false = single-format setup. }; /*! @@ -146,9 +147,12 @@ class DriverArx : public DriverCan { * @param response_can_id CAN id the encoder answers on. * @param encoder_id Encoder request CAN id. * @param data_index Cache slot in received_servo_data_ for this encoder. + * @param firmware_compat Tolerate multiple teaching-handle firmware revisions during + * EEPROM/frequency setup (config ``passive_encoder_firmware_compat``; default false). * @return ReturnCode::SUCCESS, or INVALID_PARAM for an out-of-range data index. */ - ReturnCode register_passive_encoder(int response_can_id, int encoder_id, int data_index); + ReturnCode register_passive_encoder(int response_can_id, int encoder_id, int data_index, + bool firmware_compat = false); /*! * @brief Snapshots the cached state of a polled passive encoder. @@ -185,9 +189,10 @@ class DriverArx : public DriverCan { /*! * @brief Validates firmware and EEPROM frequencies for one passive encoder. * @param request_can_id CAN id used for encoder configuration requests. + * @param firmware_compat Tolerate multiple teaching-handle firmware revisions (default false). * @return ReturnCode::SUCCESS when the encoder is ready for passive polling. */ - ReturnCode configure_passive_encoder(int request_can_id); + ReturnCode configure_passive_encoder(int request_can_id, bool firmware_compat); /*! * @brief Sends a configuration request to a passive encoder. @@ -202,14 +207,21 @@ class DriverArx : public DriverCan { /*! * @brief Reads one passive-encoder EEPROM byte. + * @param firmware_compat When true, accept both the <=2.2.x READINGS (0x86, 5-byte) and + * >=2.3.x GET_EEPROM (0x87, 3-byte) reply formats; when false, require the single + * READINGS format (original behavior). */ - ReturnCode read_passive_encoder_eeprom(int request_can_id, uint8_t device, uint8_t offset, uint8_t* p_value); + ReturnCode read_passive_encoder_eeprom(int request_can_id, uint8_t device, uint8_t offset, uint8_t* p_value, + bool firmware_compat); /*! * @brief Reads a low/high EEPROM frequency pair. + * @param firmware_compat When true, fall back to the 8-bit low byte if the 16-bit high-byte + * offset is unimplemented (legacy <=2.2.12 EEPROM layout); when false, a missing high + * byte is an error (original behavior). */ ReturnCode read_passive_encoder_frequency(int request_can_id, uint8_t device, uint8_t high_offset, - uint8_t low_offset, int* p_frequency); + uint8_t low_offset, int* p_frequency, bool firmware_compat); /*! * @brief Drains frames queued by startup validation before reception starts. diff --git a/native/pi_control/src/pi_driver_arx.cpp b/native/pi_control/src/pi_driver_arx.cpp index 98baf82..f64ddc4 100644 --- a/native/pi_control/src/pi_driver_arx.cpp +++ b/native/pi_control/src/pi_driver_arx.cpp @@ -92,29 +92,32 @@ ReturnCode DriverArx::open(int baud_rate) { } ReturnCode DriverArx::configure_passive_encoders() { - std::set request_can_ids; + // request_can_id -> firmware_compat. A single encoder may have several + // routes; OR their flags so any route opting into compat mode enables it. + std::map request_firmware_compat; { std::lock_guard lock(received_servo_data_mutex_); for (const auto& [response_can_id, route] : passive_encoder_routes_) { (void)response_can_id; - request_can_ids.insert(route.encoder_id_); + request_firmware_compat[route.encoder_id_] = + request_firmware_compat[route.encoder_id_] || route.firmware_compat_; } } - for (int request_can_id : request_can_ids) { - ReturnCode return_code = configure_passive_encoder(request_can_id); + for (const auto& [request_can_id, firmware_compat] : request_firmware_compat) { + ReturnCode return_code = configure_passive_encoder(request_can_id, firmware_compat); if (return_code != ReturnCode::SUCCESS) { return return_code; } } - if (!request_can_ids.empty()) { + if (!request_firmware_compat.empty()) { drain_startup_frames(); } return ReturnCode::SUCCESS; } -ReturnCode DriverArx::configure_passive_encoder(int request_can_id) { +ReturnCode DriverArx::configure_passive_encoder(int request_can_id, bool firmware_compat) { const uint8_t version_request[] = {kPassiveEncoderAllDevices, kPassiveEncoderReqVersion}; ReturnCode return_code = send_passive_encoder_request(request_can_id, version_request, sizeof(version_request)); @@ -144,13 +147,15 @@ ReturnCode DriverArx::configure_passive_encoder(int request_can_id) { int adc_frequency = -1; int report_frequency = -1; return_code = read_passive_encoder_frequency(request_can_id, device, kPassiveEncoderAdcFrequencyHighOffset, - kPassiveEncoderAdcFrequencyLowOffset, &adc_frequency); + kPassiveEncoderAdcFrequencyLowOffset, &adc_frequency, + firmware_compat); if (return_code != ReturnCode::SUCCESS) { PI_ERROR("Passive encoder CAN id 0x%03X device %u: failed to read ADC frequency", request_can_id, device); return return_code; } return_code = read_passive_encoder_frequency(request_can_id, device, kPassiveEncoderReportFrequencyHighOffset, - kPassiveEncoderReportFrequencyLowOffset, &report_frequency); + kPassiveEncoderReportFrequencyLowOffset, &report_frequency, + firmware_compat); if (return_code != ReturnCode::SUCCESS) { PI_ERROR("Passive encoder CAN id 0x%03X device %u: failed to read report frequency", request_can_id, device); @@ -168,7 +173,8 @@ ReturnCode DriverArx::configure_passive_encoder(int request_can_id) { } std::this_thread::sleep_for(std::chrono::milliseconds(100)); return_code = read_passive_encoder_frequency(request_can_id, device, kPassiveEncoderAdcFrequencyHighOffset, - kPassiveEncoderAdcFrequencyLowOffset, &adc_frequency); + kPassiveEncoderAdcFrequencyLowOffset, &adc_frequency, + firmware_compat); if (return_code != ReturnCode::SUCCESS || adc_frequency != kPassiveEncoderRequiredAdcFrequency) { PI_ERROR("Passive encoder CAN id 0x%03X device %u: ADC frequency verification failed (read %d, " "expected %d)", @@ -190,7 +196,8 @@ ReturnCode DriverArx::configure_passive_encoder(int request_can_id) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); return_code = read_passive_encoder_frequency(request_can_id, device, kPassiveEncoderReportFrequencyHighOffset, - kPassiveEncoderReportFrequencyLowOffset, &report_frequency); + kPassiveEncoderReportFrequencyLowOffset, &report_frequency, + firmware_compat); if (return_code != ReturnCode::SUCCESS || report_frequency != kPassiveEncoderRequiredReportFrequency) { PI_ERROR("Passive encoder CAN id 0x%03X device %u: report frequency verification failed (read %d, " "expected %d)", @@ -248,7 +255,7 @@ ReturnCode DriverArx::wait_for_passive_encoder_reply(int request_can_id, int exp } ReturnCode DriverArx::read_passive_encoder_eeprom(int request_can_id, uint8_t device, uint8_t offset, - uint8_t* p_value) { + uint8_t* p_value, bool firmware_compat) { if (p_value == nullptr) { return ReturnCode::INVALID_PARAM; } @@ -259,32 +266,76 @@ ReturnCode DriverArx::read_passive_encoder_eeprom(int request_can_id, uint8_t de return return_code; } - can_frame_t reply{}; - return_code = wait_for_passive_encoder_reply( - request_can_id, device, kPassiveEncoderReqReadings | kPassiveEncoderResponseFlag, 5, - kPassiveEncoderEepromTimeoutMs, &reply); - if (return_code != ReturnCode::SUCCESS) { - return return_code; + if (!firmware_compat) { + // Original behavior: the encoder replies to GET_EEPROM in the READINGS + // response format (command 0x86, 5 bytes, big-endian int16 value). + can_frame_t reply{}; + return_code = wait_for_passive_encoder_reply( + request_can_id, device, kPassiveEncoderReqReadings | kPassiveEncoderResponseFlag, 5, + kPassiveEncoderEepromTimeoutMs, &reply); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + const int16_t value = static_cast((reply.data[2] << 8) | reply.data[3]); + *p_value = static_cast(value & 0xFF); + return ReturnCode::SUCCESS; } - const int16_t value = static_cast((reply.data[2] << 8) | reply.data[3]); - *p_value = static_cast(value & 0xFF); - return ReturnCode::SUCCESS; + // Compat mode: firmware <=2.2.x replies to GET_EEPROM in the READINGS + // response format (command 0x86, 5 bytes, big-endian int16 value); firmware + // >=2.3.x uses a dedicated GET_EEPROM response (command 0x87, 3 bytes, + // single value byte). Accept both. + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(kPassiveEncoderEepromTimeoutMs); + while (std::chrono::steady_clock::now() < deadline) { + can_frame_t frame{}; + return_code = read_frame(&frame, sizeof(frame)); + if (return_code == ReturnCode::NO_RESPONSE) { + continue; + } + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + if (static_cast(frame.can_id & 0x7FF) != request_can_id || frame.data[0] != device) { + continue; + } + if (frame.can_dlc == 5 && + frame.data[1] == (kPassiveEncoderReqReadings | kPassiveEncoderResponseFlag)) { + const int16_t value = static_cast((frame.data[2] << 8) | frame.data[3]); + *p_value = static_cast(value & 0xFF); + return ReturnCode::SUCCESS; + } + if (frame.can_dlc == 3 && + frame.data[1] == (kPassiveEncoderReqGetEeprom | kPassiveEncoderResponseFlag)) { + *p_value = frame.data[2]; + return ReturnCode::SUCCESS; + } + } + return ReturnCode::NO_RESPONSE; } ReturnCode DriverArx::read_passive_encoder_frequency(int request_can_id, uint8_t device, uint8_t high_offset, - uint8_t low_offset, int* p_frequency) { + uint8_t low_offset, int* p_frequency, bool firmware_compat) { if (p_frequency == nullptr) { return ReturnCode::INVALID_PARAM; } uint8_t high = 0; uint8_t low = 0; - ReturnCode return_code = read_passive_encoder_eeprom(request_can_id, device, high_offset, &high); - if (return_code != ReturnCode::SUCCESS) { + ReturnCode return_code = read_passive_encoder_eeprom(request_can_id, device, high_offset, &high, firmware_compat); + if (firmware_compat && return_code == ReturnCode::NO_RESPONSE) { + // Firmware <=2.2.12 exposes a 27-byte EEPROM (offsets 0-26): the 16-bit + // frequency high bytes at offsets 27/28 do not exist and reads for them + // get no reply. Treat a silent high byte like the 0xFF "uninitialized" + // marker and fall back to the 8-bit value in the low byte. + PI_WARN("Passive encoder CAN id 0x%03X device %u: EEPROM offset %u not implemented " + "(legacy 8-bit layout); using 8-bit frequency from offset %u", + request_can_id, device, high_offset, low_offset); + high = 0xFF; + } else if (return_code != ReturnCode::SUCCESS) { return return_code; } - return_code = read_passive_encoder_eeprom(request_can_id, device, low_offset, &low); + return_code = read_passive_encoder_eeprom(request_can_id, device, low_offset, &low, firmware_compat); if (return_code != ReturnCode::SUCCESS) { return return_code; } @@ -626,7 +677,8 @@ ReturnCode DriverArx::send_disable_once(int id, int type) { return return_code; } -ReturnCode DriverArx::register_passive_encoder(int response_can_id, int encoder_id, int data_index) { +ReturnCode DriverArx::register_passive_encoder(int response_can_id, int encoder_id, int data_index, + bool firmware_compat) { if (data_index < 0 || data_index >= MAX_SERVO_INFO_BUF_SIZE) { PI_ERROR("Passive encoder id=%d: data_index %d out of range [0, %d)", encoder_id, data_index, MAX_SERVO_INFO_BUF_SIZE); @@ -646,10 +698,10 @@ ReturnCode DriverArx::register_passive_encoder(int response_can_id, int encoder_ } std::lock_guard lock(received_servo_data_mutex_); - passive_encoder_routes_[response_can_id] = PassiveEncoderRoute{encoder_id, data_index}; + passive_encoder_routes_[response_can_id] = PassiveEncoderRoute{encoder_id, data_index, firmware_compat}; PI_INFO("Driver", InfoLevel::HELPFUL_1, - "Registered passive encoder route: encoder_id=%d response_can_id=0x%02X data_index=%d", - encoder_id, response_can_id, data_index); + "Registered passive encoder route: encoder_id=%d response_can_id=0x%02X data_index=%d firmware_compat=%d", + encoder_id, response_can_id, data_index, firmware_compat ? 1 : 0); return ReturnCode::SUCCESS; } diff --git a/native/pi_control/src/pi_servo_can_encoder.cpp b/native/pi_control/src/pi_servo_can_encoder.cpp index 784ba0b..e8e99f0 100644 --- a/native/pi_control/src/pi_servo_can_encoder.cpp +++ b/native/pi_control/src/pi_servo_can_encoder.cpp @@ -95,13 +95,26 @@ ReturnCode ServoCanPassiveEncoder::init_config_model(const json& servo_config, c } } - return_code = p_driver_can_->register_passive_encoder(response_can_id_, id_, data_index_); + // Optional (default false): tolerate multiple teaching-handle firmware + // revisions during EEPROM/frequency setup. Only the YAM handle carries this + // passive encoder, so the compat path stays isolated from ARX arms. + bool firmware_compat = false; + if (servo_config.contains(p_config->fn_passive_encoder_firmware_compat)) { + return_code = + p_config->get_field_value(servo_config, p_config->fn_passive_encoder_firmware_compat, firmware_compat); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + } + + return_code = p_driver_can_->register_passive_encoder(response_can_id_, id_, data_index_, firmware_compat); if (return_code != ReturnCode::SUCCESS) { return return_code; } PI_INFO("Servo", InfoLevel::HELPFUL_1, - "Passive encoder ID %d: response_can_id=0x%02X button_num=%d", id_, response_can_id_, button_num_); + "Passive encoder ID %d: response_can_id=0x%02X button_num=%d firmware_compat=%d", id_, response_can_id_, + button_num_, firmware_compat ? 1 : 0); return ReturnCode::SUCCESS; } diff --git a/src/openpi_control/config.py b/src/openpi_control/config.py index c24d812..84dbcde 100644 --- a/src/openpi_control/config.py +++ b/src/openpi_control/config.py @@ -16,7 +16,7 @@ "ARX_X5", "Yam", ) -SUPPORTED_EFFECTORS = ("E_ARX", "E_Yam", "E_Yam_Handle") +SUPPORTED_EFFECTORS = ("E_ARX", "E_Yam", "E_Yam_Handle", "E_Yam_Handle_compat") @dataclass(frozen=True, slots=True) diff --git a/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat.json b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat.json new file mode 100644 index 0000000..9d2d5f8 --- /dev/null +++ b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat.json @@ -0,0 +1,59 @@ +{ + "config_version": "1.1.0", + "device_model": "E_Yam_Handle_compat", + "device_type": "effector", + "effector_type": "arx", + "topic_type": "ROS", + "driver_type": "CAN", + "algo_type": "Algo", + "planning_type": "None", + "catalog": { + "display_label": "E_Yam_Handle_compat: YAM Teaching Handle (multi-firmware compat)", + "order": 11, + "baudrate": 1000000, + "need_repeated_command": false + }, + "read_only": true, + "publishes_joystick": true, + "open_at_min": true, + "joints": [ + { + "joint_id": 7, + "vel_max": 3.2, + "torq_min": 0, + "torq_max": 0, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.2, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": 0, + "threshold_angle_change": 0.00872664625997165, + "threshold_time_sec": 0.1, + "max_length": 0.07, + "servos": [ + { + "servo_model": "CAN Passive Encoder", + "servo_id": 1294, + "data_index": 6, + "dir_invert": 1, + "response_delay": 0.025, + "kT": 0, + "kA": 0, + "kV": 0, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "joystick_channel_num": 0, + "joystick_button_num": 2, + "passive_encoder_firmware_compat": true + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_01.json b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_01.json new file mode 100644 index 0000000..8c5fd68 --- /dev/null +++ b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_01.json @@ -0,0 +1,34 @@ +{ + "config_version": "1.1.0", + "device_model": "E_Yam_Handle_compat", + "device_type": "effector", + "effector_type": "arx", + "spring_effect": false, + + "control_mode": "position", + "dist_to_torque_const": 0.0, + + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 7, + "reference_servo_index": 0, + "spring_invert": false, + "servos": [ + { + "servo_id": 1294, + + "pos_min": 0, + "pos_max": 0.63, + "zero_pos": 0.0, + "home_pos": 0, + "spring_home_pos": 0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_mass.json b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_mass.json new file mode 100644 index 0000000..3d686c8 --- /dev/null +++ b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_mass.json @@ -0,0 +1,17 @@ +{ + "_comment": "Identical inertia to E_Yam_Handle_mass.json; this is the firmware-compat config variant of the same physical YAM teaching handle. YAM teaching handle (i2rt yam_teaching_handle.xml inertial: mass 0.258, pos (-5.10177e-05, -0.08, 0.05), quat (0.98383, 0.174699, 0, 0), diaginertia (0.000956992, 0.000799286, 0.000469547)), expressed in the Yam.urdf end_link frame. COM transformed through the handle-vs-crank last_joint_mount delta (handle mounts at (0.00358881, -0.03891574, 0.04880005) vs crank (0, -0.0398, 0.0412), shared 180-degree mount rotation about (0,-1,1)/sqrt(2)) plus the -0.01 m z offset between the i2rt last-link frame and end_link (measured from the crank inertial's baked-in counterpart). Inertia matrix about the COM is identical to the crank gripper's (same quat and diaginertia in the source models).", + "mass": 0.258, + "center_of_mass": [ + -0.00363983, + -0.08760005, + 0.03911574 + ], + "inertia": { + "ixx": 0.000955866, + "iyy": 0.000761363, + "izz": 0.000508596, + "ixy": 1.47274e-05, + "ixz": -5.44088e-06, + "iyz": 0.000106418 + } +}