From 29a899b305491652903b550e1a8573eae0f269bd Mon Sep 17 00:00:00 2001 From: lgv Date: Fri, 11 Sep 2026 20:03:46 +0800 Subject: [PATCH] fix(ti5 motors):can send & rec bug --- cmvr-es/config/devices/arm/arm.pb.txt | 151 ++++++++++++++++++ cmvr-es/config/devices/dexhand/dexhand.pb.txt | 2 +- cmvr-es/config/manager/device_manager.pb.txt | 34 ++-- cmvr-es/config/manager/task_manager.pb.txt | 2 +- cmvr-es/devices/canbus/can_comm/can_sender.h | 3 + .../canbus/can_comm/can_sender_test.cc | 13 ++ cmvr-es/devices/motor/abstract_motor.h | 6 +- .../can/src/can_motor_bus_runtime.cpp | 8 +- .../drivers/ti5_canopen/include/ti5_motor.h | 7 +- .../include/ti5_motor_canopen_protocol.h | 8 +- .../src/ti5_motor_canopen_protocol.cpp | 114 ++++++++++--- .../motor/manager/src/motor_manager.cpp | 58 +++++-- 12 files changed, 347 insertions(+), 59 deletions(-) diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt index 6dd5fca9..2bc59236 100644 --- a/cmvr-es/config/devices/arm/arm.pb.txt +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -149,4 +149,155 @@ arm { } } } + + robot_arms { + id: "left_arm" + + motor { + motor_system_id: "left_arm_can_motors" + motor_group_ids: "left_arm_can_motors" + dof: 7 + joint_names: "L_SHOULDER_P" + joint_names: "L_SHOULDER_R" + joint_names: "L_SHOULDER_Y" + joint_names: "L_ELBOW_R" + joint_names: "L_WRIST_P" + joint_names: "L_WRIST_Y" + joint_names: "L_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "L_WRIST_R_S" + tcp_frame_name: "L_FINGER_TIP_FIXED" + max_iters: 100 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-6 + joint_limit_policy { + limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + } + soft_limit { + enable: true + margin_ratio: 0.01 + min_margin_rad: 0.01 + } + avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + weight: 0.05 + } + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_continuity_check { + enable: true + max_joint_delta_rad: 0.05 + max_joint_velocity_rad_s: 10.0 + max_joint_acceleration_rad_s2: 5000.0 + } + cartesian_step_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 45.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 45.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + } + + speed_l { + pinocchio_cartesian_motion_planner { + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + enforce_joint_acceleration_limits: true + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_velocity_check { + enable: true + max_joint_velocity_rad_s: 30.0 + max_joint_acceleration_rad_s2: 10000.0 + } + cartesian_velocity_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 5.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 5.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + stop_acceleration: 10 + } + } + } + } + } } diff --git a/cmvr-es/config/devices/dexhand/dexhand.pb.txt b/cmvr-es/config/devices/dexhand/dexhand.pb.txt index 75a8c811..aa6a7497 100644 --- a/cmvr-es/config/devices/dexhand/dexhand.pb.txt +++ b/cmvr-es/config/devices/dexhand/dexhand.pb.txt @@ -2,7 +2,7 @@ dexhand { dexhands { id: "hand1" rh56dftp { - ip: "192.168.1.213" + ip: "192.168.1.223" port: 6000 poll_interval_ms: 10 } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 8aeb0d25..6ff5bff6 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -6,35 +6,35 @@ device_manager { id: "mujoco_world" type: DEVICE_TYPE_MUJOCO_WORLD config_file: "devices/mujoco/mujoco_world.pb.txt" - enable: true + enable: false } devices { id: "right_arm_mujoco_motors" type: DEVICE_TYPE_MOTOR_SYSTEM config_file: "devices/motor/mujoco_motors.pb.txt" - enable: true + enable: false } devices { id: "mujoco_right_arm" type: DEVICE_TYPE_ROBOT_ARM config_file: "devices/arm/arm_mujoco_qp.pb.txt" - enable: true + enable: false } devices { id: "mujoco_viewer" type: DEVICE_TYPE_MUJOCO_VIEWER config_file: "devices/mujoco/mujoco_viewer.pb.txt" - enable: true + enable: false } devices { id: "mujoco_hand_cam" type: DEVICE_TYPE_CAMERA config_file: "devices/camera/camera.pb.txt" - enable: true + enable: false } devices { @@ -53,12 +53,19 @@ device_manager { devices { - id: "hand2" + id: "hand1" type: DEVICE_TYPE_DEXHAND config_file: "devices/dexhand/dexhand.pb.txt" - enable: false + enable: true } + devices { + id: "hand2" + type: DEVICE_TYPE_DEXHAND + config_file: "devices/dexhand/dexhand.pb.txt" + enable: true + } + devices { id: "paxini_tip_1" type: DEVICE_TYPE_DEXHAND @@ -70,7 +77,7 @@ device_manager { id: "mujoco_zero_touch_dexhand" type: DEVICE_TYPE_DEXHAND config_file: "devices/dexhand/dexhand.pb.txt" - enable: true + enable: false } devices { @@ -84,7 +91,7 @@ device_manager { id: "right_arm_can_motors" type: DEVICE_TYPE_MOTOR_SYSTEM config_file: "devices/motor/ti5_motors.pb.txt" - enable: false + enable: true } devices { @@ -112,9 +119,16 @@ device_manager { id: "right_arm" type: DEVICE_TYPE_ROBOT_ARM config_file: "devices/arm/arm.pb.txt" - enable: false + enable: true } + devices { + id: "left_arm" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/arm.pb.txt" + enable: false + } + devices { id: "aubo_arm" type: DEVICE_TYPE_ROBOT_ARM diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index 1b4c4fed..2785290f 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -5,7 +5,7 @@ task_manager { run_mode: TASK_RUN_MODE_PERIODIC_STEP control_period_s: 0.001 config_file: "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt" - enable: true + enable: false } tasks { id: "grpc_server" diff --git a/cmvr-es/devices/canbus/can_comm/can_sender.h b/cmvr-es/devices/canbus/can_comm/can_sender.h index 63e4b42c..34272241 100644 --- a/cmvr-es/devices/canbus/can_comm/can_sender.h +++ b/cmvr-es/devices/canbus/can_comm/can_sender.h @@ -247,6 +247,9 @@ namespace cmvr { curr_period_ = period_; Update(); + if (send_with_once_) { + has_sent_ = true; + } } template diff --git a/cmvr-es/devices/canbus/can_comm/can_sender_test.cc b/cmvr-es/devices/canbus/can_comm/can_sender_test.cc index 39e941ad..5bad78fe 100644 --- a/cmvr-es/devices/canbus/can_comm/can_sender_test.cc +++ b/cmvr-es/devices/canbus/can_comm/can_sender_test.cc @@ -77,6 +77,19 @@ namespace cmvr { sensor_data->enable = (bytes[2] != 0); } + TEST(CanSenderTest, OneShotMessageWaitsForExplicitUpdate) { + MyProtocol protocol; + SenderMessage message(MyProtocol::ID, &protocol, true); + + EXPECT_TRUE(message.has_sent()); + + protocol.SetSpeed(12.3); + protocol.SetEnable(true); + message.Update(); + + EXPECT_FALSE(message.has_sent()); + } + TEST(CanSenderTest, OneRunCase) { cmvr::config::SocketCanConfig cfg; diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 28a7d288..e3ab5788 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -214,14 +214,14 @@ namespace cmvr::device{ // 使用的通讯协议 - virtual void setProtocol(std::shared_ptr protocol) { + virtual bool setProtocol(std::shared_ptr protocol) { std::scoped_lock lock(mtx_); protocol_ = std::move(protocol); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->initNode(node_id_); + return protocol_->initNode(node_id_); } uint8_t id() const { diff --git a/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp index 09c1284c..637978e6 100644 --- a/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp @@ -68,16 +68,16 @@ bool CanMotorBusRuntime::start() return false; } - auto ret = sender_->Start(); + auto ret = receiver_->Start(); if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; stop(); return false; } - ret = receiver_->Start(); + ret = sender_->Start(); if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; stop(); return false; } diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h index d1fc84ae..d10a4db2 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h @@ -59,7 +59,12 @@ namespace cmvr { // canopen_protocol->configProfile(node_id_,4000,8000,8000); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE); - canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + if (!canopen_protocol->setMode( + node_id_, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + CMVR_LOG(ERROR) << "[Ti5Motor] failed to initialize operation mode: " + << info_.joint_name; + return false; + } // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb); diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h index 3b64a284..cb3951df 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h @@ -74,12 +74,7 @@ namespace cmvr { return data_ptr; } - msgs::RunMode getMode(uint8_t node_id) override { - return GetRobotDetail()->motors().at(node_id).run_mode(); - // cur_mode_[node_id] = feed_mode; - // return feed_mode; - // return cur_mode_[node_id]; - } + msgs::RunMode getMode(uint8_t node_id) override; private: @@ -113,6 +108,7 @@ namespace cmvr { std::map rpdo1_commands_{}; std::map rpdo2_commands_{}; + bool getMotorStatus(uint8_t node_id, msgs::MotorStatus* status) const; void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos); diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp index 14937d69..47fd04c4 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp @@ -67,7 +67,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { if (sdo_commands_[node_id] == nullptr) { CMVR_LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; + return false; } can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true); @@ -78,7 +78,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { if (rpdo1_commands_[node_id] == nullptr) { CMVR_LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; + return false; } can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true); @@ -88,11 +88,38 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { if (rpdo2_commands_[node_id] == nullptr) { CMVR_LOG(ERROR) << "Ti5 Motor RPDO2 Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; + return false; } can_sender_->AddMessage(rpdo2_commands_[node_id]->ID(), rpdo2_commands_[node_id], true); - return ErrorCode::OK; + return true; +} + +bool Ti5MotorCanopenProtocol::getMotorStatus( + const uint8_t node_id, + msgs::MotorStatus* const status) const { + if (!message_manager_ || !status) { + return false; + } + + msgs::RobotDetail robot_detail; + if (message_manager_->GetSensorData(&robot_detail) != ErrorCode::OK) { + return false; + } + const auto it = robot_detail.motors().find(node_id); + if (it == robot_detail.motors().end()) { + return false; + } + status->CopyFrom(it->second); + return true; +} + +cmvr::msgs::RunMode Ti5MotorCanopenProtocol::getMode(const uint8_t node_id) { + msgs::MotorStatus status; + if (!getMotorStatus(node_id, &status)) { + return msgs::RUN_MODE_UNSPECIFIED; + } + return status.run_mode(); } void Ti5MotorCanopenProtocol::setMotorConversion( @@ -286,12 +313,28 @@ bool Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { cw.enable_operation = 1; seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + int32_t current_position = 0; + if (mode == RUN_MODE_PROFILE_POSITION || mode == RUN_MODE_CYCLIC_SYNC_POSITION) { + seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); + msgs::MotorStatus status; + if (!waitUntil([&]() { + return getMotorStatus(node_id, &status) && + status.has_sdo_response() && + status.sdo_response().index() == CIA402_ACTUAL_POSITION_6064; + }, 500)) { + CMVR_LOG(ERROR) << "motor " << static_cast(node_id) + << ": actual-position feedback timed out"; + return false; + } + current_position = status.position(); + } + switch (mode) { case RUN_MODE_PROFILE_POSITION: { // 4 : 设置目标位置（为当前位置） - auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, + SUB_INDEX_0, current_position); // 5 : 触发位置运动（new_set_point 翻转） @@ -308,8 +351,8 @@ bool Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { case RUN_MODE_CYCLIC_SYNC_POSITION: { // configRPDO1(node_id, true); // 设置目标位置为当前位置 - auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, + SUB_INDEX_0, current_position); //3 : 使能 15 cw.enable_operation = 1; @@ -541,7 +584,11 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { // 2: 等待确认清除成功 seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0); if (!waitUntil([&]() { - return GetRobotDetail()->motors().at(node_id).position_offset() == 0; + msgs::MotorStatus status; + return getMotorStatus(node_id, &status) && + status.has_sdo_response() && + status.sdo_response().index() == CANOPEN_POSITION_OFFSET_2008 && + status.position_offset() == 0; }, 1000)) { CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed"; return false; @@ -549,7 +596,17 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { // 3: 读取当前位置 0x6064 seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); - auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); + msgs::MotorStatus status; + if (!waitUntil([&]() { + return getMotorStatus(node_id, &status) && + status.has_sdo_response() && + status.sdo_response().index() == CIA402_ACTUAL_POSITION_6064; + }, 500)) { + CMVR_LOG(ERROR) << "motor " << static_cast(node_id) + << ": actual-position feedback timed out during calibration"; + return false; + } + const auto cur_pos = status.position(); // 4: 将当前位置写入偏置寄存器 seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos); @@ -561,10 +618,14 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { // 6: 确认写入成功 seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); if (!waitUntil([&]() { - return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos; + msgs::MotorStatus latest_status; + return getMotorStatus(node_id, &latest_status) && + latest_status.has_sdo_response() && + latest_status.sdo_response().index() == CANOPEN_POSITION_OFFSET_2008 && + latest_status.position_offset() == cur_pos; }, 500)) { - return false; CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed"; + return false; } return true; @@ -575,9 +636,10 @@ bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) { } bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) { - (void)node_id; - CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented"; - return false; + // (void)node_id; + // CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented"; + torqueOff(node_id); + return true; } bool Ti5MotorCanopenProtocol::quickStop(uint8_t node_id) { @@ -594,8 +656,12 @@ bool Ti5MotorCanopenProtocol::quickStop(uint8_t node_id) { } bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { + msgs::MotorStatus motor_status; + if (!getMotorStatus(node_id, &motor_status)) { + return false; + } statusword_t st{}; - st.value = GetRobotDetail()->motors().at(node_id).status_word(); + st.value = motor_status.status_word(); return st.target_reached == 1; } @@ -617,9 +683,11 @@ double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) { if (!conversion) { return 0.0; } - auto data_ptr = std::make_unique(); - message_manager_->GetSensorData(data_ptr.get()); - auto cnt = data_ptr->motors().at(node_id).position(); + msgs::MotorStatus motor_status; + if (!getMotorStatus(node_id, &motor_status)) { + return 0.0; + } + const auto cnt = motor_status.position(); return countsToRad(cnt, *conversion); } @@ -628,8 +696,10 @@ double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) { if (!conversion) { return 0.0; } - auto data_ptr = std::make_unique(); - message_manager_->GetSensorData(data_ptr.get()); - auto cnt = data_ptr->motors().at(node_id).speed(); + msgs::MotorStatus motor_status; + if (!getMotorStatus(node_id, &motor_status)) { + return 0.0; + } + const auto cnt = motor_status.speed(); return velocityRawToRadPerSec(cnt, *conversion); } diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index cf4f5e0a..1d1b262e 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -105,14 +105,14 @@ bool MotorManager::init() continue; } - const bool start_before_motor_init = + const bool start_before_motor_creation = motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT; - if (start_before_motor_init && !bus_runtime->start()) { + if (start_before_motor_creation && !bus_runtime->start()) { bus_runtime->stop(); all_ok = false; continue; } - if (start_before_motor_init) { + if (start_before_motor_creation) { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } @@ -123,11 +123,21 @@ bool MotorManager::init() all_ok = false; continue; } + if (!start_before_motor_creation && !bus_runtime->start()) { + bus_runtime->stop(); + all_ok = false; + continue; + } + bool group_ok = true; - for (auto& motor : motors) { - if (!addMotor(motor)) { - group_ok = false; - break; + if (motor_group_cfg.bus_type() == config::MOTOR_BUS_CAN) { + for (auto& motor : motors) { + if (!motor->init()) { + CMVR_LOG(ERROR) << "[MotorManager] failed to initialize CAN motor: " + << motor->jointName(); + group_ok = false; + break; + } } } if (!group_ok) { @@ -135,7 +145,14 @@ bool MotorManager::init() all_ok = false; continue; } - if (!start_before_motor_init && !bus_runtime->start()) { + + for (auto& motor : motors) { + if (!addMotor(motor)) { + group_ok = false; + break; + } + } + if (!group_ok) { bus_runtime->stop(); all_ok = false; continue; @@ -268,6 +285,26 @@ bool MotorManager::commandCyclicPositionsAtomic( if (std::dynamic_pointer_cast(motors.front())) { return MujocoMotor::commandCyclicPositionsAtomic(motors, positions, velocities); } + if (std::dynamic_pointer_cast(motors.front())) { + // TI5 currently exposes only a per-motor RPDO command. Keep the + // fallback here so callers retain one manager-level entry point. + static std::once_flag ti5_non_atomic_warning; + std::call_once(ti5_non_atomic_warning, []() { + CMVR_LOG(WARNING) + << "[MotorManager] TI5 motors use non-atomic cyclic position commands"; + }); + for (std::size_t i = 0; i < motors.size(); ++i) { + if (!std::dynamic_pointer_cast(motors[i])) { + CMVR_LOG(ERROR) << "[MotorManager] mixed motor types in cyclic position command"; + return false; + } + if (!motors[i]->commandCyclicPosition(positions[i], velocities[i])) { + CMVR_LOG(ERROR) << "[MotorManager] failed to command TI5 motor at index " << i; + return false; + } + } + return true; + } CMVR_LOG(ERROR) << "[MotorManager] atomic cyclic position is unsupported for motor type: " << motors.front()->typeName(); @@ -483,9 +520,8 @@ std::vector> MotorManager::createCanMotors_( motors.reserve(motor_cfgs.size()); for (const auto& cfg : motor_cfgs) { auto motor = std::make_shared(cfg); - motor->setProtocol(protocol); - if (!motor->init()) { - CMVR_LOG(ERROR) << "[MotorManager] failed to init TI5 motor: " + if (!motor->setProtocol(protocol)) { + CMVR_LOG(ERROR) << "[MotorManager] failed to register TI5 motor protocols: " << cfg.joint_name(); return {}; }