SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(ti5 motors):can send & rec bug

Browse Source
This commit is contained in:
lgv 2026-09-11 20:03:46 +08:00
parent f768960ff2
commit 29a899b305
12 changed files with 347 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
cmvr-es/config/devices/arm/arm.pb.txt
View File

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

2
cmvr-es/config/devices/dexhand/dexhand.pb.txt
View File

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

30
cmvr-es/config/manager/device_manager.pb.txt
View File

@ -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 {
@ -52,11 +52,18 @@ device_manager {
}
devices {
id: "hand1"
type: DEVICE_TYPE_DEXHAND
config_file: "devices/dexhand/dexhand.pb.txt"
enable: true
}
devices {
id: "hand2"
type: DEVICE_TYPE_DEXHAND
config_file: "devices/dexhand/dexhand.pb.txt"
enable: false
enable: true
}
devices {
@ -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,6 +119,13 @@ device_manager {
id: "right_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm.pb.txt"
enable: true
}
devices {
id: "left_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm.pb.txt"
enable: false
}

2
cmvr-es/config/manager/task_manager.pb.txt
View File

@ -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"

3
cmvr-es/devices/canbus/can_comm/can_sender.h
View File

@ -247,6 +247,9 @@ namespace cmvr {
curr_period_ = period_;
Update();
if (send_with_once_) {
has_sent_ = true;
}
}
template<typename SensorType>

13
cmvr-es/devices/canbus/can_comm/can_sender_test.cc
View File

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

6
cmvr-es/devices/motor/abstract_motor.h
View File

@ -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 {

8
cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp
View File

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

7
cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h
View File

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

8
cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h
View File

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

114
cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp
View File

@ -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, 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<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;
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<msgs::RobotDetail>();
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<msgs::RobotDetail>();
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);
}

52
cmvr-es/devices/motor/manager/src/motor_manager.cpp
View File

@ -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,19 +123,36 @@ 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;
if (motor_group_cfg.bus_type() == config::MOTOR_BUS_CAN) {
for (auto& motor : motors) {
if (!addMotor(motor)) {
if (!motor->init()) {
CMVR_LOG(ERROR) << "[MotorManager] failed to initialize CAN motor: "
<< motor->jointName();
group_ok = false;
break;
}
}
}
if (!group_ok) {
bus_runtime->stop();
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->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 {};
}