diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt new file mode 100644 index 00000000..15c2b0de --- /dev/null +++ b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt @@ -0,0 +1,53 @@ +motor { + id: "ethercat_motors" + + motor_groups { + id: "right_arm_ethercat" + bus_type: MOTOR_BUS_ETHERCAT + vendor: MOTOR_VENDOR_EYOU + protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402 + + ethercat { + master_index: 0 + cycle_us: 1000 + + cia402 { + profile_position_trigger_delay_ms: 2 + state_transition_timeout_ms: 1200 + velocity_stop_timeout_ms: 2000 + status_poll_period_ms: 10 + stopped_velocity_tolerance_rad_s: 0.001 + } + + slaves { motor_id: 1 alias: 0 position: 0 } + slaves { motor_id: 2 alias: 0 position: 1 } + slaves { motor_id: 3 alias: 0 position: 2 } + slaves { motor_id: 4 alias: 0 position: 3 } + slaves { motor_id: 5 alias: 0 position: 4 } + slaves { motor_id: 6 alias: 0 position: 5 } + slaves { motor_id: 7 alias: 0 position: 6 } + } + + joint_limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + } + + motors { + motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + } + } +} diff --git a/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt new file mode 100644 index 00000000..53c04267 --- /dev/null +++ b/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt @@ -0,0 +1,44 @@ +motor { + id: "ethercat_motors" + + motor_groups { + id: "right_arm_ethercat" + bus_type: MOTOR_BUS_ETHERCAT + vendor: MOTOR_VENDOR_EYOU + protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402 + + ethercat { + master_index: 0 + cycle_us: 1000 + + cia402 { + profile_position_trigger_delay_ms: 2 + state_transition_timeout_ms: 1200 + velocity_stop_timeout_ms: 2000 + status_poll_period_ms: 10 + stopped_velocity_tolerance_rad_s: 0.001 + } + + slaves { motor_id: 1 alias: 0 position: 0 } + slaves { motor_id: 2 alias: 0 position: 1 } + slaves { motor_id: 3 alias: 0 position: 2 } + slaves { motor_id: 4 alias: 0 position: 3 } + } + + joint_limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } + } + + motors { + motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + } + } +} diff --git a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt index 4ebaac09..7271eba6 100644 --- a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt +++ b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt @@ -6,7 +6,6 @@ motor { bus_type: MOTOR_BUS_MUJOCO vendor: MOTOR_VENDOR_MUJOCO protocol: MOTOR_PROTOCOL_MUJOCO - tool_frame: "R_FINGER_TIP" mujoco { world_id: "mujoco_world" } diff --git a/cmvr-es/config/devices/motor/ti5_motors.pb.txt b/cmvr-es/config/devices/motor/ti5_motors.pb.txt index ca50e0ec..20a28aff 100644 --- a/cmvr-es/config/devices/motor/ti5_motors.pb.txt +++ b/cmvr-es/config/devices/motor/ti5_motors.pb.txt @@ -6,7 +6,6 @@ motor { bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN - tool_frame: "L_FINGER_TIP" can { channel_id: 0 } @@ -16,13 +15,13 @@ motor { urdf_path: "model/xiaoyan_description/dual_arm.urdf" } motors { - motors { id: 23 joint_name: "L_SHOULDER_P" } - motors { id: 24 joint_name: "L_SHOULDER_R" } - motors { id: 25 joint_name: "L_SHOULDER_Y" } - motors { id: 26 joint_name: "L_ELBOW_R" } - motors { id: 27 joint_name: "L_WRIST_P" } - motors { id: 28 joint_name: "L_WRIST_Y" } - motors { id: 29 joint_name: "L_WRIST_R" } + motors { id: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 24 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 25 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 26 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 27 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 28 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 29 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -31,7 +30,6 @@ motor { bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN - tool_frame: "R_FINGER_TIP" can { channel_id: 1 } @@ -47,13 +45,13 @@ motor { joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } } motors { - motors { id: 16 joint_name: "R_SHOULDER_P" } - motors { id: 17 joint_name: "R_SHOULDER_R" } - motors { id: 18 joint_name: "R_SHOULDER_Y" } - motors { id: 19 joint_name: "R_ELBOW_R" } - motors { id: 20 joint_name: "R_WRIST_P" } - motors { id: 21 joint_name: "R_WRIST_Y" } - motors { id: 22 joint_name: "R_WRIST_R" } + motors { id: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 17 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 18 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 19 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 20 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 21 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 22 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -73,9 +71,9 @@ motor { joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } } motors { - motors { id: 32 joint_name: "HEAD_Y" } - motors { id: 30 joint_name: "HEAD_P" } - motors { id: 31 joint_name: "HEAD_R" } + motors { id: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 31 joint_name: "HEAD_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -94,8 +92,8 @@ motor { joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } } motors { - motors { id: 4 joint_name: "WAIST_Y" } - motors { id: 15 joint_name: "WAIST_P" } + motors { id: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 15 joint_name: "WAIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 5a3ff6be..d7980070 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -2,6 +2,7 @@ device_manager { name: "cmvr_es" version: "0.1" description: "cmvr edge system version 0.1" + init_all_motors_when_no_active_joints: true devices { id: "mujoco_world" @@ -74,6 +75,13 @@ device_manager { enable: false } + devices { + id: "ethercat_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ethercat_motors.pb.txt" + enable: false + } + devices { id: "right_arm" type: DEVICE_TYPE_ROBOT_ARM diff --git a/cmvr-es/devices/motor/manager/CMakeLists.txt b/cmvr-es/devices/motor/manager/CMakeLists.txt index 8eea8209..8ea5f495 100644 --- a/cmvr-es/devices/motor/manager/CMakeLists.txt +++ b/cmvr-es/devices/motor/manager/CMakeLists.txt @@ -12,6 +12,7 @@ target_link_libraries(motor_manager PRIVATE cmvr_es::device::ti5_canopen_motor_driver cmvr_es::device::mujoco_motor_driver + cmvr_es::device::ethercat_motor_driver cmvr_es::ik_solver glog ) diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index d1e6affe..5e83b043 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -1,21 +1,27 @@ -#include "motor/manager/include/motor_manager.h" +#include "devices/motor/manager/include/motor_manager.h" +#include #include #include #include +#include #include #include #include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" #include "common/base/logging/logger.h" #include "common/config/config_files.h" -#include "../../bus_runtime/abstract_motor_bus_runtime.h" -#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h" -#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" -#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" -#include "motor/drivers/mujoco/include/mujoco_motor.h" -#include "motor/drivers/ti5_canopen/include/ti5_motor.h" -#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" +#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" +#include "devices/motor/drivers/mujoco/include/mujoco_motor.h" +#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h" +#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" namespace cmvr::device { @@ -91,11 +97,17 @@ bool MotorManager::init() all_ok = false; continue; } - if (!bus_runtime->start()) { + + const bool start_before_motor_init = + motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT; + if (start_before_motor_init && !bus_runtime->start()) { bus_runtime->stop(); all_ok = false; continue; } + if (start_before_motor_init) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime); if (motors.empty()) { @@ -116,6 +128,11 @@ bool MotorManager::init() all_ok = false; continue; } + if (!start_before_motor_init && !bus_runtime->start()) { + bus_runtime->stop(); + all_ok = false; + continue; + } bus_runtimes_.push_back(std::move(bus_runtime)); } @@ -398,8 +415,19 @@ std::shared_ptr MotorManager::createBusRuntime_( return std::make_shared(); case config::MOTOR_BUS_MUJOCO: return std::make_shared(); - case config::MOTOR_BUS_ETHERCAT: - return std::make_shared(); + case config::MOTOR_BUS_ETHERCAT: { + auto runtime = std::make_shared(); + if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU && + group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { + runtime->setPdoMapping(createEyouCia402PdoMapping()); + return runtime; + } + CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return nullptr; + } default: CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: " << config::MotorBusType_Name(group_cfg.bus_type()) @@ -534,6 +562,14 @@ std::vector> MotorManager::createEthercatMotors_( CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << group_cfg.id(); return {}; } + if (group_cfg.vendor() != config::MOTOR_VENDOR_EYOU || + group_cfg.protocol() != config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { + CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return {}; + } for (const auto& motor_cfg : motor_cfgs) { if (!ethercat_bus_runtime->slaveForMotor(motor_cfg.id())) { CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT slave config for motor id " @@ -542,11 +578,22 @@ std::vector> MotorManager::createEthercatMotors_( } } - CMVR_LOG(ERROR) << "[MotorManager] EtherCAT motor creation is not implemented: vendor=" - << config::MotorVendor_Name(group_cfg.vendor()) - << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) - << ", group=" << group_cfg.id(); - return {}; + auto protocol = std::make_shared( + ethercat_bus_runtime, group_cfg.ethercat().cia402()); + + std::vector> motors; + motors.reserve(motor_cfgs.size()); + for (const auto& cfg : motor_cfgs) { + auto motor = std::make_shared( + cfg, protocol, std::make_unique(ethercat_bus_runtime)); + if (!motor->init()) { + CMVR_LOG(ERROR) << "[MotorManager] failed to init EYOU EtherCAT motor: " + << cfg.joint_name(); + return {}; + } + motors.push_back(std::move(motor)); + } + return motors; } } // namespace cmvr::device diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index d957afb4..88e7a501 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -26,6 +26,9 @@ using namespace cmvr::device; namespace { +using GroupJointSelection = std::unordered_map>; +using MotorJointSelections = std::unordered_map; + void logSection(const char* title) { CMVR_LOG(INFO) << "---------------- " << title << " ----------------"; @@ -71,6 +74,32 @@ bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group, return false; } +void addAllMotorJoints(const std::string& motor_system_id, + const cmvr::config::MotorRootConfig& root_cfg, + MotorJointSelections& selections) +{ + auto& group_selection = selections[motor_system_id]; + for (const auto& motor_group : root_cfg.motor().motor_groups()) { + if (motor_group.id().empty()) { + continue; + } + + auto& selected_joints = group_selection[motor_group.id()]; + for (const auto& motor : motor_group.motors().motors()) { + if (!motor.joint_name().empty()) { + selected_joints.insert(motor.joint_name()); + } + } + if (selected_joints.empty()) { + group_selection.erase(motor_group.id()); + } + } + + if (group_selection.empty()) { + selections.erase(motor_system_id); + } +} + } // namespace template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); @@ -256,8 +285,7 @@ void DeviceManager::log_device_plan_() const void DeviceManager::pre_scan_robot_arm_dependencies_() const { - using GroupJointSelection = std::unordered_map>; - std::unordered_map selections; + MotorJointSelections selections; std::unordered_map motor_roots; for (const auto& entry : cfg_.devices()) { @@ -387,6 +415,15 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } } + if (selections.empty() && cfg_.init_all_motors_when_no_active_joints()) { + CMVR_LOG(INFO) << "[DeviceManager]: No active motor joints from RobotArm; " + << "initialize all configured motors because " + << "init_all_motors_when_no_active_joints=true"; + for (const auto& [motor_system_id, root_cfg] : motor_roots) { + addAllMotorJoints(motor_system_id, root_cfg, selections); + } + } + MotorManager::clearActiveJoints(); for (auto& [motor_system_id, group_selection] : selections) { MotorManager::setActiveJoints(motor_system_id, std::move(group_selection)); diff --git a/protos/cmvr/config/device_manager_config/device_manager_config.proto b/protos/cmvr/config/device_manager_config/device_manager_config.proto index c238d2bf..d36d328b 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -34,6 +34,7 @@ message DeviceManagerConfig { string version = 2; string description = 3; repeated DeviceConfigEntry devices = 4; + bool init_all_motors_when_no_active_joints = 20; } message DeviceManagerRootConfig { DeviceManagerConfig device_manager = 1;