fix(ti5 motors):can send & rec bug

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
+              }
+            }
+          }
+        }
+      }
 }

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
     }

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

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"

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<typename SensorType>

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<MySensorData> 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;

cmvr-es/devices/motor/abstract_motor.h

@@ -214,14 +214,14 @@ namespace cmvr::device{
 
 
         // 使用的通讯协议
-        virtual void setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {
+        virtual bool setProtocol(std::shared_ptr<MotorProtocolInterface> 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 {

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;
     }

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);

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 {
+            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];
-            }
 
 
         private:
@@ -113,6 +108,7 @@ namespace cmvr {
             std::map<uint8_t, motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
             std::map<uint8_t, motor::Ti5MotorRPDO2 *> rpdo2_commands_{};
 
+            bool getMotorStatus(uint8_t node_id, msgs::MotorStatus* status) const;
             void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos);
 
 

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<int>(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,
-            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
+                           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,
-            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
+                           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<int>(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;
+    // (void)node_id;
-    CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented";
+    // CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented";
-    return false;
+    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<msgs::RobotDetail>();
+    msgs::MotorStatus motor_status;
-    message_manager_->GetSensorData(data_ptr.get());
+    if (!getMotorStatus(node_id, &motor_status)) {
-    auto cnt = data_ptr->motors().at(node_id).position();
+        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<msgs::RobotDetail>();
+    msgs::MotorStatus motor_status;
-    message_manager_->GetSensorData(data_ptr.get());
+    if (!getMotorStatus(node_id, &motor_status)) {
-    auto cnt = data_ptr->motors().at(node_id).speed();
+        return 0.0;
+    }
+    const auto cnt = motor_status.speed();
     return velocityRawToRadPerSec(cnt, *conversion);
 }

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 (motor_group_cfg.bus_type() == config::MOTOR_BUS_CAN) {
-            if (!addMotor(motor)) {
+            for (auto& motor : motors) {
-                group_ok = false;
+                if (!motor->init()) {
-                break;
+                    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<MujocoMotor>(motors.front())) {
         return MujocoMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
     }
+    if (std::dynamic_pointer_cast<Ti5Motor>(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<Ti5Motor>(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<std::shared_ptr<AbstractMotor>> MotorManager::createCanMotors_(
     motors.reserve(motor_cfgs.size());
     for (const auto& cfg : motor_cfgs) {
         auto motor = std::make_shared<Ti5Motor>(cfg);
-        motor->setProtocol(protocol);
+        if (!motor->setProtocol(protocol)) {
-        if (!motor->init()) {
+            CMVR_LOG(ERROR) << "[MotorManager] failed to register TI5 motor protocols: "
-            CMVR_LOG(ERROR) << "[MotorManager] failed to init TI5 motor: "
                             << cfg.joint_name();
             return {};
         }