diff --git a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt index 2a885ac4..4ebaac09 100644 --- a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt +++ b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt @@ -2,16 +2,35 @@ motor { id: "mujoco_motors" motor_groups { - id: "right_arm_mujoco" + id: "mujoco_right_arm" bus_type: MOTOR_BUS_MUJOCO + vendor: MOTOR_VENDOR_MUJOCO + protocol: MOTOR_PROTOCOL_MUJOCO tool_frame: "R_FINGER_TIP" + mujoco { + world_id: "mujoco_world" + } - motors { id: 1 joint_name: "R_SHOULDER_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } - motors { id: 2 joint_name: "R_SHOULDER_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } - motors { id: 3 joint_name: "R_SHOULDER_Y" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } - motors { id: 4 joint_name: "R_ELBOW_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } - motors { id: 5 joint_name: "R_WRIST_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 } - motors { id: 6 joint_name: "R_WRIST_Y" limit_q_lb: -1.102 limit_q_ub: 1.02 limit_qd: 3.0 } - motors { id: 7 joint_name: "R_WRIST_R" limit_q_lb: -0.293 limit_q_ub: 1.57079 limit_qd: 3.0 } + 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" } + motors { id: 2 joint_name: "R_SHOULDER_R" } + motors { id: 3 joint_name: "R_SHOULDER_Y" } + motors { id: 4 joint_name: "R_ELBOW_R" } + motors { id: 5 joint_name: "R_WRIST_P" } + motors { id: 6 joint_name: "R_WRIST_Y" } + motors { id: 7 joint_name: "R_WRIST_R" } + } } } diff --git a/cmvr-es/config/devices/motor/ti5_motors.pb.txt b/cmvr-es/config/devices/motor/ti5_motors.pb.txt index f596e644..ca50e0ec 100644 --- a/cmvr-es/config/devices/motor/ti5_motors.pb.txt +++ b/cmvr-es/config/devices/motor/ti5_motors.pb.txt @@ -4,31 +4,39 @@ motor { motor_groups { id: "left_arm_can" bus_type: MOTOR_BUS_CAN + vendor: MOTOR_VENDOR_TI5 + protocol: MOTOR_PROTOCOL_CANOPEN tool_frame: "L_FINGER_TIP" can { channel_id: 0 } joint_limits { + enable: true source: JOINT_LIMIT_SOURCE_URDF urdf_path: "model/xiaoyan_description/dual_arm.urdf" } - 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 { + 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" } + } } motor_groups { id: "right_arm_can" bus_type: MOTOR_BUS_CAN + vendor: MOTOR_VENDOR_TI5 + protocol: MOTOR_PROTOCOL_CANOPEN tool_frame: "R_FINGER_TIP" can { channel_id: 1 } 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 } @@ -38,44 +46,56 @@ motor { 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 { 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 { + 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" } + } } motor_groups { id: "head_can" bus_type: MOTOR_BUS_CAN + vendor: MOTOR_VENDOR_TI5 + protocol: MOTOR_PROTOCOL_CANOPEN can { channel_id: 2 } joint_limits { + enable: true source: JOINT_LIMIT_SOURCE_CUSTOM joints { joint_name: "HEAD_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } joints { joint_name: "HEAD_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } } - motors { id: 32 joint_name: "HEAD_Y" } - motors { id: 30 joint_name: "HEAD_P" } - motors { id: 31 joint_name: "HEAD_R" } + motors { + motors { id: 32 joint_name: "HEAD_Y" } + motors { id: 30 joint_name: "HEAD_P" } + motors { id: 31 joint_name: "HEAD_R" } + } } motor_groups { id: "waist_can" bus_type: MOTOR_BUS_CAN + vendor: MOTOR_VENDOR_TI5 + protocol: MOTOR_PROTOCOL_CANOPEN can { channel_id: 3 } joint_limits { + enable: true source: JOINT_LIMIT_SOURCE_CUSTOM joints { joint_name: "WAIST_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } } - motors { id: 4 joint_name: "WAIST_Y" } - motors { id: 15 joint_name: "WAIST_P" } + motors { + motors { id: 4 joint_name: "WAIST_Y" } + motors { id: 15 joint_name: "WAIST_P" } + } } } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 6894bdf1..2672abfe 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -3,6 +3,41 @@ device_manager { version: "0.1" description: "cmvr edge system version 0.1" + devices { + id: "mujoco_world" + type: DEVICE_TYPE_MUJOCO_WORLD + config_file: "devices/mujoco/mujoco_world.pb.txt" + enable: true + } + + devices { + id: "mujoco_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/mujoco_motors.pb.txt" + enable: true + } + + devices { + id: "mujoco_right_arm" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/arm_mujoco.pb.txt" + enable: true + } + + devices { + id: "mujoco_viewer" + type: DEVICE_TYPE_MUJOCO_VIEWER + config_file: "devices/mujoco/mujoco_viewer.pb.txt" + enable: true + } + + devices { + id: "mujoco_hand_cam" + type: DEVICE_TYPE_CAMERA + config_file: "devices/camera/camera.pb.txt" + enable: false + } + devices { id: "right_hand_cam" type: DEVICE_TYPE_CAMERA diff --git a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt index 91088f69..c0b7d99b 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt @@ -11,7 +11,8 @@ target_link_libraries(motor_robot_arm cmvr_es::algorithms::arm_motion cmvr_es::ik_solver cmvr_es::algorithms::arm_control - cmvr_es::device::motor_system + cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver glog ) @@ -25,8 +26,8 @@ add_executable(motor_robot_arm_mujoco_test target_link_libraries(motor_robot_arm_mujoco_test PRIVATE cmvr_es::device::motor_robot_arm - cmvr_es::device::motor_system - cmvr_es::device::mujoco_motor + cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver cmvr_es::mujoco_viewer cmvr_es::proto gtest diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index 39c3b309..7a0af759 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -15,7 +15,7 @@ #include "cmvr/config/arm_config/arm_config.pb.h" #include "devices/arm/robot_arm.h" #include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" -#include "motor/motor_manager.h" +#include "motor/manager/include/motor_manager.h" namespace cmvr::device { @@ -105,8 +105,7 @@ private: static CartesianVelocityController::Config toCartesianVelocityControllerConfig_( const config::CartesianVelocityControllerConfig& config); - static std::vector withDefaultQdMax_(const std::vector& qd_max, - std::size_t dof); + std::vector moveLJointVelocityLimits_(const std::vector& qd_max) const; static Result unsupported_(const std::string& name); private: diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index a23481f3..9e1fdae2 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -2,7 +2,6 @@ #include #include -#include #include #include #include @@ -11,13 +10,17 @@ #include "algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h" #include "algorithms/kinematics/ik_solver/ik_solver_factory.h" #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "common/base/protobuf_utils.h" +#include "common/base/string_utils.h" #include "common/base/logging/logger.h" #include "common/math/transform_math.h" #include "motor/abstract_motor.h" -#include "motor/motor_system/include/motor_system.h" +#include "motor/manager/include/motor_manager.h" namespace cmvr::device { +using cmvr::common::joinStrings; +using cmvr::common::repeatedToVector; namespace { struct BusyGuard { @@ -25,24 +28,6 @@ struct BusyGuard { ~BusyGuard() { busy.store(false); } }; -std::string joinStrings(const std::vector& values) -{ - std::ostringstream oss; - for (std::size_t i = 0; i < values.size(); ++i) { - if (i > 0) { - oss << ", "; - } - oss << values[i]; - } - return oss.str(); -} - -std::vector repeatedToVector( - const google::protobuf::RepeatedPtrField& values) -{ - return {values.begin(), values.end()}; -} - } // namespace MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg) @@ -99,10 +84,10 @@ bool MotorRobotArm::init() << "], joints=[" << joinStrings(joint_names_) << "]"; if (!motor_manager_) { - motor_manager_ = MotorSystem::managerFor(motor_system_id_); + motor_manager_ = MotorManager::managerFor(motor_system_id_); } if (!motor_manager_) { - CMVR_LOG(ERROR) << "[MotorRobotArm] MotorSystem is not initialized: " << motor_system_id_; + CMVR_LOG(ERROR) << "[MotorRobotArm] MotorManager is not initialized: " << motor_system_id_; CMVR_LOG(ERROR) << "[MotorRobotArm] (init): Arm '" << id_ << "' initialized motors=[], missing motors=[" << joinStrings(joint_names_) << "]"; @@ -394,7 +379,7 @@ Result MotorRobotArm::moveL(const CartesianPose& target, CartesianJointTrajectory trajectory; if (!cartesian_planner_->planMoveL(target, q_start, - withDefaultQdMax_(options.joint_velocity_limits, getDof()), + moveLJointVelocityLimits_(options.joint_velocity_limits), options.velocity, options.acceleration, options.jerk, @@ -402,6 +387,12 @@ Result MotorRobotArm::moveL(const CartesianPose& target, trajectory)) { return Result::failure(ArmErrorCode::CommandFailed, "moveL planner failed"); } + if (trajectory.truncated) { + CMVR_LOG(WARNING) << "[MotorRobotArm][moveL] planned_path_m=" + << trajectory.planned_path_length + << ", truncated_reason=" << trajectory.truncation_reason + << ", executable_path_m=" << trajectory.executable_path_length; + } return executeMoveLTrajectory_(trajectory) ? Result::success() : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed"); @@ -690,8 +681,10 @@ bool MotorRobotArm::configureAlgorithms_() } const auto* speed_l_config = CartesianMotionPlannerFactory::speedLConfig(speed_l); const auto* move_l_config = CartesianMotionPlannerFactory::moveLConfig(move_l); - if (!speed_l_config || !move_l_config || - !cartesian_motion->configureMoveL(*move_l_config) || + if (!speed_l_config || !move_l_config) { + return false; + } + if (!cartesian_motion->configureMoveL(*move_l_config) || !cartesian_motion->configureSpeedL(*speed_l_config, getDof())) { return false; } @@ -749,16 +742,26 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController result.stop_measured_velocity_norm = config.stop_measured_velocity_norm() > 0.0 ? config.stop_measured_velocity_norm() : result.stop_measured_velocity_norm; + result.stop_acceleration = + config.stop_acceleration() > 0.0 ? config.stop_acceleration() + : result.stop_acceleration; return result; } -std::vector MotorRobotArm::withDefaultQdMax_(const std::vector& qd_max, - const std::size_t dof) +std::vector MotorRobotArm::moveLJointVelocityLimits_( + const std::vector& qd_max) const { if (!qd_max.empty()) { return qd_max; } - return std::vector(dof, 2.5); + const auto pinocchio_solver = std::dynamic_pointer_cast(ik_solver_); + if (pinocchio_solver) { + const auto& limits = pinocchio_solver->jointLimitPolicy().limits(); + if (!limits.enable()) { + return {}; + } + } + return {}; } Result MotorRobotArm::unsupported_(const std::string& name) diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp index 79b468d8..5ec14ee0 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp @@ -17,9 +17,9 @@ #include #include "common/io/proto_file_io.h" -#include "devices/motor/mujoco_motor/include/mujoco_joint_bridge.h" -#include "devices/motor/motor_system/include/motor_system.h" +#include "devices/motor/manager/include/motor_manager.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" namespace cmvr::device { namespace { @@ -54,84 +54,6 @@ std::filesystem::path findProjectRoot() return search(std::filesystem::path(__FILE__).parent_path()); } -class MotorRobotArmViewer final : public MuJocoViewer { -public: - MotorRobotArmViewer(const std::string& model_path, - std::shared_ptr bridge) - : MuJocoViewer(model_path.c_str()), bridge_(std::move(bridge)) - { - position_actuator_ids_.fill(-1); - qpos_ids_.fill(-1); - qvel_ids_.fill(-1); - } - -protected: - void initOnce(mjModel* model, mjData* data) override - { - setupCamera(2.5, -160.0, -25.0); - bool valid = true; - for (std::size_t i = 0; i < kDof; ++i) { - const std::string actuator_name = std::string(kJointNames[i]) + "_pos"; - position_actuator_ids_[i] = mj_name2id(model, mjOBJ_ACTUATOR, actuator_name.c_str()); - const int joint_id = mj_name2id(model, mjOBJ_JOINT, kJointNames[i]); - if (position_actuator_ids_[i] < 0 || joint_id < 0) { - valid = false; - continue; - } - qpos_ids_[i] = model->jnt_qposadr[joint_id]; - qvel_ids_[i] = model->jnt_dofadr[joint_id]; - position_reference_[i] = data->qpos[qpos_ids_[i]]; - } - bridge_->markReady(valid); - } - - void controlCallback(mjModel* model, mjData* data) override - { - std::vector measured_position(kDof, 0.0); - std::vector measured_velocity(kDof, 0.0); - for (std::size_t i = 0; i < kDof; ++i) { - if (qpos_ids_[i] >= 0) { - measured_position[i] = data->qpos[qpos_ids_[i]]; - } - if (qvel_ids_[i] >= 0) { - measured_velocity[i] = data->qvel[qvel_ids_[i]]; - } - } - bridge_->publishMeasured(measured_position, measured_velocity); - - const auto commands = bridge_->commands(); - for (std::size_t i = 0; i < kDof; ++i) { - const int actuator_id = position_actuator_ids_[i]; - if (actuator_id < 0) { - continue; - } - - if (commands.mode[i] == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { - if (last_mode_[i] != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY && qpos_ids_[i] >= 0) { - position_reference_[i] = data->qpos[qpos_ids_[i]]; - } - position_reference_[i] += commands.velocity[i] * model->opt.timestep; - } else { - position_reference_[i] = commands.position[i]; - } - - const double lower = model->actuator_ctrlrange[2 * actuator_id]; - const double upper = model->actuator_ctrlrange[2 * actuator_id + 1]; - position_reference_[i] = std::clamp(position_reference_[i], lower, upper); - data->ctrl[actuator_id] = position_reference_[i]; - last_mode_[i] = commands.mode[i]; - } - } - -private: - std::shared_ptr bridge_; - std::array position_actuator_ids_{}; - std::array qpos_ids_{}; - std::array qvel_ids_{}; - std::array position_reference_{}; - std::array last_mode_{}; -}; - double maxPositionError(const std::vector& actual, const std::vector& expected) { @@ -208,6 +130,18 @@ protected: project_root_ = findProjectRoot(); ASSERT_FALSE(project_root_.empty()); + config::MujocoWorldRootConfig world_root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(), + &world_root_config)); + ASSERT_GT(world_root_config.worlds_size(), 0); + auto world_config = world_root_config.worlds(0); + world_config.set_model_path( + (project_root_ / "model/xiaoyan_description/dual_arm.xml").string()); + world_device_ = std::make_shared(world_config); + ASSERT_TRUE(world_device_->init()); + ASSERT_TRUE(world_device_->start()); + config::MotorRootConfig motor_root_config; ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(), @@ -217,14 +151,15 @@ protected: for (const auto* joint_name : kJointNames) { right_arm_joints.insert(joint_name); } - MotorSystem::clearActiveJoints(); - MotorSystem::setActiveJoints( - "mujoco_motors", {{"right_arm_mujoco", std::move(right_arm_joints)}}); + MotorManager::clearActiveJoints(); + MotorManager::setActiveJoints( + "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}}); - motor_system_ = std::make_shared("mujoco_motors", motor_root_config.motor()); + motor_system_ = std::make_shared("mujoco_motors", motor_root_config.motor()); ASSERT_NO_THROW(motor_system_->init()); - bridge_ = MotorSystem::mujocoBridgeFor("mujoco_motors"); - ASSERT_TRUE(bridge_); + world_ = MotorManager::mujocoWorldFor("mujoco_motors"); + ASSERT_TRUE(world_); + ASSERT_TRUE(world_->isLoaded()); config::ArmRootConfig root_config; ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( @@ -249,12 +184,16 @@ protected: if (motor_system_) { motor_system_->stop(); } - MotorSystem::clearActiveJoints(); + if (world_device_) { + world_device_->stop(); + } + MotorManager::clearActiveJoints(); } std::filesystem::path project_root_; - std::shared_ptr motor_system_; - std::shared_ptr bridge_; + std::shared_ptr world_device_; + std::shared_ptr motor_system_; + std::shared_ptr world_; std::unique_ptr arm_; }; @@ -262,14 +201,14 @@ TEST_P(MotorRobotArmMujocoTest, MoveJ) { MotorRobotArm& arm = *arm_; - MotorRobotArmViewer viewer( - (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_); + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -25.0); ScenarioOutcome outcome; std::thread scenario([&] { try { - if (!bridge_->waitUntilReady(std::chrono::seconds(10))) { - throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready"); + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); } std::this_thread::sleep_for(std::chrono::milliseconds(300)); @@ -310,14 +249,14 @@ TEST_P(MotorRobotArmMujocoTest, MoveL) { MotorRobotArm& arm = *arm_; - MotorRobotArmViewer viewer( - (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_); + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -25.0); ScenarioOutcome outcome; std::thread scenario([&] { try { - if (!bridge_->waitUntilReady(std::chrono::seconds(10))) { - throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready"); + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); } std::this_thread::sleep_for(std::chrono::milliseconds(300)); @@ -379,14 +318,14 @@ TEST_P(MotorRobotArmMujocoTest, SpeedL) { MotorRobotArm& arm = *arm_; - MotorRobotArmViewer viewer( - (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_); + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -25.0); ScenarioOutcome outcome; std::thread scenario([&] { try { - if (!bridge_->waitUntilReady(std::chrono::seconds(10))) { - throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready"); + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); } std::this_thread::sleep_for(std::chrono::milliseconds(300)); @@ -446,9 +385,8 @@ INSTANTIATE_TEST_SUITE_P( ArmPlannerCombinations, MotorRobotArmMujocoTest, ::testing::Values( - ArmMujocoConfigCase{"DlsIkDlsMotion", "arm_mujoco_dls_ik_dls_motion.pb.txt"}, - ArmMujocoConfigCase{"DlsIkQpMotion", "arm_mujoco_dls_ik_qp_motion.pb.txt"}, - ArmMujocoConfigCase{"QpIkQpMotion", "arm_mujoco_qp_ik_qp_motion.pb.txt"}), + ArmMujocoConfigCase{"DlsIk", "arm_mujoco.pb.txt"}, + ArmMujocoConfigCase{"QpIk", "arm_mujoco_qp.pb.txt"}), [](const ::testing::TestParamInfo& info) { return std::string(info.param.name); }); diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 9c79fddf..76689b2f 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -1,31 +1,15 @@ -add_subdirectory(ti5_motor) -add_subdirectory(mujoco_motor) -add_subdirectory(motor_system) +add_library(motor_core INTERFACE) -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- -include_directories( - ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include -) +target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices) -link_directories( - ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib -) - - -add_executable(motor_manager_test - ${CMAKE_CURRENT_SOURCE_DIR}/motor_manager_test.cpp -) - - -target_link_libraries(motor_manager_test - PRIVATE - cmvr_es::device::canbus - cmvr_es::device::ti5motor - gtest - gtest_main - pthread - glog +target_link_libraries(motor_core + INTERFACE cmvr_es::proto ) + +add_library(cmvr_es::device::motor_core ALIAS motor_core) + +add_subdirectory(drivers/ti5_canopen) +add_subdirectory(drivers/mujoco) +add_subdirectory(bus_runtime) +add_subdirectory(manager) diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 0cf6f7aa..983ccec7 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -7,7 +7,7 @@ #pragma once -#include "../abstract_device.h" +#include "devices/abstract_device.h" #include "common/base/logging/logger.h" #include "motor/motor_protocol_interface.h" #include @@ -66,7 +66,7 @@ namespace cmvr::device{ return protocol_->getMode(node_id_); } - void torqueOff() { + virtual void torqueOff() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; diff --git a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt new file mode 100644 index 00000000..238d1841 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt @@ -0,0 +1,20 @@ +add_library(motor_bus_runtime SHARED + can/src/can_motor_bus_runtime.cpp + mujoco/src/mujoco_motor_bus_runtime.cpp + ethercat/src/ethercat_motor_bus_runtime.cpp +) + +target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(motor_bus_runtime + PUBLIC + cmvr_es::proto + cmvr_es::device::motor_core + cmvr_es::mujoco_world + PRIVATE + cmvr_es::device::canbus + glog +) + +add_library(cmvr_es::device::motor_bus_runtime ALIAS motor_bus_runtime) +install(TARGETS motor_bus_runtime LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/bus_runtime/abstract_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/abstract_motor_bus_runtime.h new file mode 100644 index 00000000..28b9d9b5 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/abstract_motor_bus_runtime.h @@ -0,0 +1,30 @@ +#ifndef CMVR_ES_ABSTRACT_MOTOR_BUS_RUNTIME_H +#define CMVR_ES_ABSTRACT_MOTOR_BUS_RUNTIME_H + +#include + +#include "cmvr/config/motor_config/motor_config.pb.h" + +namespace cmvr::simulate { + +class MujocoWorld; + +} // namespace cmvr::simulate + +namespace cmvr::device { + +class AbstractMotorBusRuntime { +public: + virtual ~AbstractMotorBusRuntime() = default; + + virtual bool init(const config::MotorGroupConfig& group_cfg) = 0; + virtual bool start() = 0; + virtual void stop() = 0; + virtual config::MotorBusType busType() const = 0; + + virtual std::shared_ptr mujocoWorld() const { return nullptr; } +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ABSTRACT_MOTOR_BUS_RUNTIME_H diff --git a/cmvr-es/devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h new file mode 100644 index 00000000..9158a018 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h @@ -0,0 +1,45 @@ +#ifndef CMVR_ES_CAN_MOTOR_BUS_RUNTIME_H +#define CMVR_ES_CAN_MOTOR_BUS_RUNTIME_H + +#include +#include + +#include "cmvr/msgs/robot_detail.pb.h" +#include "../../abstract_motor_bus_runtime.h" + +namespace cmvr::device { + +class AbstractCanbus; + +template +class CanReceiver; + +template +class CanSender; + +template +class MessageManager; + +class CanMotorBusRuntime final : public AbstractMotorBusRuntime { +public: + bool init(const config::MotorGroupConfig& group_cfg) override; + bool start() override; + void stop() override; + config::MotorBusType busType() const override { return config::MOTOR_BUS_CAN; } + + const std::string& id() const { return id_; } + std::shared_ptr> sender() const { return sender_; } + std::shared_ptr> messageManager() const { return message_manager_; } + +private: + std::string id_; + std::shared_ptr client_; + std::shared_ptr> sender_; + std::shared_ptr> receiver_; + std::shared_ptr> message_manager_; + bool started_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CAN_MOTOR_BUS_RUNTIME_H 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 new file mode 100644 index 00000000..09c1284c --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp @@ -0,0 +1,102 @@ +#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h" + +#include "canbus/can_client/socket/socket_can_client_raw.h" +#include "canbus/can_comm/can_receiver.h" +#include "canbus/can_comm/can_sender.h" +#include "canbus/can_comm/message_manager.h" +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +bool CanMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) +{ + id_ = group_cfg.id(); + if (id_.empty()) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] group id is empty"; + return false; + } + if (group_cfg.bus_type() != config::MOTOR_BUS_CAN) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] invalid bus type for group: " << id_; + return false; + } + if (!group_cfg.has_can()) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] missing can config: " << id_; + return false; + } + + client_ = std::make_shared(group_cfg.can()); + sender_ = std::make_shared>(); + receiver_ = std::make_shared>(); + message_manager_ = std::make_shared>(); + if (!client_ || !sender_ || !receiver_ || !message_manager_) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] invalid runtime: " << id_; + return false; + } + + if (!client_->init()) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to init CAN client: " << id_; + return false; + } + + auto ret = sender_->Init(client_.get(), false); + if (ret != ErrorCode::OK) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to init CAN sender: " << id_; + return false; + } + + ret = receiver_->Init(client_.get(), message_manager_.get(), false); + if (ret != ErrorCode::OK) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to init CAN receiver: " << id_; + return false; + } + + return true; +} + +bool CanMotorBusRuntime::start() +{ + if (started_) { + return true; + } + if (!client_ || !sender_ || !receiver_) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] runtime is not initialized: " << id_; + return false; + } + + if (!client_->start()) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN client: " << id_; + return false; + } + + auto ret = sender_->Start(); + if (ret != ErrorCode::OK) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; + stop(); + return false; + } + + ret = receiver_->Start(); + if (ret != ErrorCode::OK) { + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; + stop(); + return false; + } + started_ = true; + return true; +} + +void CanMotorBusRuntime::stop() +{ + if (receiver_) { + receiver_->Stop(); + } + if (sender_) { + sender_->Stop(); + } + if (client_) { + client_->stop(); + } + started_ = false; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h new file mode 100644 index 00000000..5d7f8edf --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h @@ -0,0 +1,31 @@ +#ifndef CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H +#define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H + +#include +#include + +#include "../../abstract_motor_bus_runtime.h" + +namespace cmvr::device { + +class EthercatMotorBusRuntime final : public AbstractMotorBusRuntime { +public: + bool init(const config::MotorGroupConfig& group_cfg) override; + bool start() override; + void stop() override; + config::MotorBusType busType() const override { return config::MOTOR_BUS_ETHERCAT; } + + const std::string& id() const { return id_; } + const config::EtherCATConfig& config() const { return config_; } + const config::EthercatSlaveConfig* slaveForMotor(int motor_id) const; + +private: + std::string id_; + config::EtherCATConfig config_; + std::unordered_map slaves_by_motor_id_; + bool started_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp new file mode 100644 index 00000000..45c44243 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp @@ -0,0 +1,78 @@ +#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) +{ + id_ = group_cfg.id(); + if (id_.empty()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] group id is empty"; + return false; + } + if (group_cfg.bus_type() != config::MOTOR_BUS_ETHERCAT) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid bus type for group: " << id_; + return false; + } + if (!group_cfg.has_ethercat()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing ethercat config: " << id_; + return false; + } + + config_ = group_cfg.ethercat(); + if (config_.master_id().empty()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] master_id is empty: " << id_; + return false; + } + if (config_.cycle_us() <= 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] cycle_us must be positive: " << id_; + return false; + } + + slaves_by_motor_id_.clear(); + for (const auto& slave : config_.slaves()) { + if (slave.motor_id() <= 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid motor_id in slave config: " << id_; + return false; + } + if (slave.slave_index() < 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid slave_index for motor " + << slave.motor_id() << " in group: " << id_; + return false; + } + if (slaves_by_motor_id_.count(slave.motor_id()) > 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] duplicate slave motor_id: " + << slave.motor_id() << " in group: " << id_; + return false; + } + slaves_by_motor_id_[slave.motor_id()] = &slave; + } + + return true; +} + +bool EthercatMotorBusRuntime::start() +{ + if (started_) { + return true; + } + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] EtherCAT master is not implemented yet: " << id_; + return false; +} + +void EthercatMotorBusRuntime::stop() +{ + started_ = false; +} + +const config::EthercatSlaveConfig* EthercatMotorBusRuntime::slaveForMotor(const int motor_id) const +{ + const auto it = slaves_by_motor_id_.find(motor_id); + if (it == slaves_by_motor_id_.end()) { + return nullptr; + } + return it->second; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h new file mode 100644 index 00000000..b0ee93f1 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h @@ -0,0 +1,36 @@ +#ifndef CMVR_ES_MUJOCO_MOTOR_BUS_RUNTIME_H +#define CMVR_ES_MUJOCO_MOTOR_BUS_RUNTIME_H + +#include +#include + +#include "../../abstract_motor_bus_runtime.h" + +namespace cmvr::simulate { + +class MujocoWorld; + +} // namespace cmvr::simulate + +namespace cmvr::device { + +class MujocoMotorBusRuntime final : public AbstractMotorBusRuntime { +public: + bool init(const config::MotorGroupConfig& group_cfg) override; + bool start() override; + void stop() override; + config::MotorBusType busType() const override { return config::MOTOR_BUS_MUJOCO; } + std::shared_ptr mujocoWorld() const override { return world_; } + + const std::string& id() const { return id_; } + +private: + std::string id_; + std::string world_id_; + std::shared_ptr world_; + bool started_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MUJOCO_MOTOR_BUS_RUNTIME_H diff --git a/cmvr-es/devices/motor/bus_runtime/mujoco/src/mujoco_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/mujoco/src/mujoco_motor_bus_runtime.cpp new file mode 100644 index 00000000..34c7a4a8 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/mujoco/src/mujoco_motor_bus_runtime.cpp @@ -0,0 +1,71 @@ +#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" + +#include "common/base/logging/logger.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" + +namespace cmvr::device { + +bool MujocoMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) +{ + id_ = group_cfg.id(); + if (id_.empty()) { + CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] group id is empty"; + return false; + } + if (group_cfg.bus_type() != config::MOTOR_BUS_MUJOCO) { + CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] invalid bus type for group: " << id_; + return false; + } + if (!group_cfg.has_mujoco()) { + CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] missing mujoco config: " << id_; + return false; + } + + const auto& mujoco_cfg = group_cfg.mujoco(); + if (mujoco_cfg.world_id().empty()) { + CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] missing world_id: " << id_; + return false; + } + world_id_ = mujoco_cfg.world_id(); + + world_ = simulate::MujocoWorldDevice::worldFor(world_id_); + if (!world_) { + CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] world not found: " + << world_id_ << " for group: " << id_; + return false; + } + if (!world_->isLoaded()) { + CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] world is not loaded: " + << world_id_; + return false; + } + return true; +} + +bool MujocoMotorBusRuntime::start() +{ + if (started_) { + return true; + } + if (!world_) { + CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] runtime is not initialized: " << id_; + return false; + } + if (!world_->isRunning() && !world_->start()) { + CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] failed to start world: " + << world_id_ << ", error=" << world_->lastError(); + return false; + } + started_ = true; + return true; +} + +void MujocoMotorBusRuntime::stop() +{ + if (world_) { + world_->stop(); + } + started_ = false; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/mujoco/CMakeLists.txt b/cmvr-es/devices/motor/drivers/mujoco/CMakeLists.txt new file mode 100644 index 00000000..ae60e207 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/mujoco/CMakeLists.txt @@ -0,0 +1,15 @@ +add_library(mujoco_motor_driver SHARED + src/mujoco_motor.cpp +) + +target_include_directories(mujoco_motor_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(mujoco_motor_driver + PUBLIC + cmvr_es::proto + cmvr_es::device::motor_core + cmvr_es::mujoco_world +) + +add_library(cmvr_es::device::mujoco_motor_driver ALIAS mujoco_motor_driver) +install(TARGETS mujoco_motor_driver LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/mujoco_motor/include/mujoco_motor.h b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h similarity index 56% rename from cmvr-es/devices/motor/mujoco_motor/include/mujoco_motor.h rename to cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h index bbe6baaa..b0293806 100644 --- a/cmvr-es/devices/motor/mujoco_motor/include/mujoco_motor.h +++ b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h @@ -5,24 +5,25 @@ #include #include #include +#include -#include "devices/motor/abstract_motor.h" -#include "devices/motor/mujoco_motor/include/mujoco_joint_bridge.h" +#include "motor/abstract_motor.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" namespace cmvr::device { class MujocoMotor final : public AbstractMotor { public: - MujocoMotor(std::size_t joint_index, - std::string joint_name, - std::shared_ptr bridge, + MujocoMotor(std::string joint_name, + std::shared_ptr world, std::uint8_t node_id = 0); std::string typeName() const override { return "MujocoMotor"; } - bool init() override { return true; } + bool init() override; void setMode(msgs::RunMode mode) override; msgs::RunMode getMode() override; + void torqueOff() override; void setLimitQ(double ub, double lb) override; void setLimitQd(double qd) override; @@ -38,11 +39,21 @@ public: double getQ() override; double getQd() override; + static bool setTargetsAtomic(const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities); + private: - std::size_t joint_index_{0}; - std::shared_ptr bridge_; + double clampQ_(double q) const; + double clampQd_(double qd) const; + std::shared_ptr worldLocked_() const; + + std::weak_ptr world_; + msgs::RunMode mode_{msgs::RUN_MODE_UNSPECIFIED}; + double target_q_{0.0}; double limit_qdd_upper_{0.0}; double limit_qdd_lower_{0.0}; + bool initialized_{false}; }; } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp new file mode 100644 index 00000000..160c1005 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp @@ -0,0 +1,249 @@ +#include "motor/drivers/mujoco/include/mujoco_motor.h" + +#include +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +MujocoMotor::MujocoMotor(std::string joint_name, + std::shared_ptr world, + const std::uint8_t node_id) + : AbstractMotor(node_id), + world_(std::move(world)) +{ + info_.id = id(); + info_.joint_name = std::move(joint_name); + id_ = info_.joint_name; +} + +bool MujocoMotor::init() +{ + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + CMVR_LOG(ERROR) << "[MujocoMotor] world is null for joint: " << info_.joint_name; + return false; + } + if (!world->isLoaded()) { + CMVR_LOG(ERROR) << "[MujocoMotor] world is not loaded for joint: " << info_.joint_name; + return false; + } + if (!world->hasJoint(info_.joint_name)) { + CMVR_LOG(ERROR) << "[MujocoMotor] joint not found in world: " << info_.joint_name; + return false; + } + world->getJointPosition(info_.joint_name, target_q_); + mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + initialized_ = true; + return true; +} + +void MujocoMotor::setMode(const msgs::RunMode mode) +{ + std::scoped_lock lock(mtx_); + mode_ = mode; +} + +msgs::RunMode MujocoMotor::getMode() +{ + std::scoped_lock lock(mtx_); + return mode_; +} + +void MujocoMotor::torqueOff() +{ + brake(); + std::scoped_lock lock(mtx_); + mode_ = msgs::RUN_MODE_UNSPECIFIED; +} + +void MujocoMotor::setLimitQ(const double ub, const double lb) +{ + std::scoped_lock lock(mtx_); + info_.limit_q_ub = ub; + info_.limit_q_lb = lb; +} + +void MujocoMotor::setLimitQd(const double qd) +{ + std::scoped_lock lock(mtx_); + info_.limit_qd = std::abs(qd); +} + +void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd) +{ + std::scoped_lock lock(mtx_); + limit_qdd_upper_ = std::abs(u_qdd); + limit_qdd_lower_ = -std::abs(l_qdd); + info_.limit_qdd = std::max(limit_qdd_upper_, std::abs(limit_qdd_lower_)); +} + +void MujocoMotor::brake() +{ + const auto world = worldLocked_(); + double q = 0.0; + if (!world || !world->getJointPosition(info_.joint_name, q)) { + return; + } + std::scoped_lock lock(mtx_); + target_q_ = q; + mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + world->setJointTargetState(info_.joint_name, q, 0.0); +} + +void MujocoMotor::setQ(const double q) +{ + setTarget(q, 0.0); +} + +void MujocoMotor::setTarget(const double q, const double qd) +{ + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return; + } + target_q_ = clampQ_(q); + world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd)); +} + +void MujocoMotor::setTarget(const double qd) +{ + setQd(qd); +} + +bool MujocoMotor::calibrateZeroQ() +{ + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + target_q_ = clampQ_(0.0); + return world->setJointPosition(info_.joint_name, target_q_); +} + +bool MujocoMotor::reachedTargetQ() +{ + double q = 0.0; + { + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world || !world->getJointPosition(info_.joint_name, q)) { + return false; + } + return std::abs(q - target_q_) < 1e-3; + } +} + +void MujocoMotor::setQd(const double qd) +{ + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return; + } + world->setJointTargetVelocity(info_.joint_name, clampQd_(qd)); +} + +double MujocoMotor::getQ() +{ + const auto world = worldLocked_(); + double q = 0.0; + if (!world || !world->getJointPosition(info_.joint_name, q)) { + return 0.0; + } + return q; +} + +double MujocoMotor::getQd() +{ + const auto world = worldLocked_(); + double qd = 0.0; + if (!world || !world->getJointVelocity(info_.joint_name, qd)) { + return 0.0; + } + return qd; +} + +bool MujocoMotor::setTargetsAtomic(const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) +{ + if (motors.size() != positions.size() || motors.size() != velocities.size() || motors.empty()) { + return false; + } + + std::shared_ptr world; + std::vector joint_names; + std::vector clamped_positions; + std::vector clamped_velocities; + joint_names.reserve(motors.size()); + clamped_positions.reserve(motors.size()); + clamped_velocities.reserve(motors.size()); + + for (std::size_t i = 0; i < motors.size(); ++i) { + const auto& motor = motors[i]; + if (!motor) { + return false; + } + + std::scoped_lock lock(motor->mtx_); + auto motor_world = motor->worldLocked_(); + if (!motor_world) { + return false; + } + if (!world) { + world = motor_world; + } else if (world.get() != motor_world.get()) { + CMVR_LOG(ERROR) << "[MujocoMotor] batch target motors belong to different worlds"; + return false; + } + + joint_names.push_back(motor->info_.joint_name); + clamped_positions.push_back(motor->clampQ_(positions[i])); + clamped_velocities.push_back(motor->clampQd_(velocities[i])); + } + + if (!world || !world->setJointTargetStates(joint_names, clamped_positions, clamped_velocities)) { + if (world) { + CMVR_LOG(ERROR) << "[MujocoMotor] failed to set batch joint targets: " << world->lastError(); + } + return false; + } + + for (std::size_t i = 0; i < motors.size(); ++i) { + std::scoped_lock lock(motors[i]->mtx_); + motors[i]->target_q_ = clamped_positions[i]; + motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + } + return true; +} + +double MujocoMotor::clampQ_(const double q) const +{ + if (std::isfinite(info_.limit_q_lb) && std::isfinite(info_.limit_q_ub) && + info_.limit_q_ub > info_.limit_q_lb) { + return std::clamp(q, info_.limit_q_lb, info_.limit_q_ub); + } + return q; +} + +double MujocoMotor::clampQd_(const double qd) const +{ + if (std::isfinite(info_.limit_qd) && info_.limit_qd > 0.0) { + return std::clamp(qd, -info_.limit_qd, info_.limit_qd); + } + return qd; +} + +std::shared_ptr MujocoMotor::worldLocked_() const +{ + return world_.lock(); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/CMakeLists.txt b/cmvr-es/devices/motor/drivers/ti5_canopen/CMakeLists.txt new file mode 100644 index 00000000..a170f78b --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/CMakeLists.txt @@ -0,0 +1,24 @@ +add_library(ti5_canopen_motor_driver SHARED + src/protocol/ti5_motor_sdo_response.cpp + src/protocol/ti5_motor_tpdo1.cpp + src/protocol/ti5_motor_tpdo2.cpp + src/protocol/ti5_motor_rpdo1.cpp + src/protocol/ti5_motor_rpdo2.cpp + src/ti5_motor_canopen_protocol.cpp + src/ti5_motor.cpp +) + +target_include_directories(ti5_canopen_motor_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + +add_library(cmvr_es::device::ti5_canopen_motor_driver ALIAS ti5_canopen_motor_driver) + +target_link_libraries(ti5_canopen_motor_driver + PUBLIC + cmvr_es::device::motor_core + PRIVATE + cmvr_es::device::canbus + cmvr_es::proto + glog +) + +install(TARGETS ti5_canopen_motor_driver LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h similarity index 100% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h rename to cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h similarity index 100% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h rename to cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h similarity index 100% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h rename to cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h similarity index 100% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h rename to cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo2.h similarity index 100% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h rename to cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo2.h diff --git a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h similarity index 93% rename from cmvr-es/devices/motor/ti5_motor/ti5_motor.h rename to cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h index 6fd1aa37..64db8ea2 100644 --- a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h @@ -2,10 +2,10 @@ // Created by lgv on 2025/8/1. // -#include "../abstract_motor.h" +#include "motor/abstract_motor.h" #include "common/base/logging/logger.h" #include "motor/motor_protocol_interface.h" -#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" +#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" #include "cmvr/config/motor_config/motor_config.pb.h" /** @@ -15,7 +15,7 @@ namespace cmvr { namespace device { class Ti5Motor : public AbstractMotor { public: - explicit Ti5Motor(const config::Ti5MotorConfig &config) { + explicit Ti5Motor(const config::MotorConfigItem &config) { info_.id = config.id(); info_.joint_name = config.joint_name(); info_.limit_q_lb = config.limit_q_lb(); diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h similarity index 96% rename from cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h rename to cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h index 2abec2cc..e837cd89 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h @@ -5,7 +5,7 @@ #pragma once #include "cmvr/msgs/motor.pb.h" #include "motor/motor_protocol_interface.h" -#include "../../../canbus/abstract_canbus.h" +#include "canbus/abstract_canbus.h" #include "canbus/can_comm/can_receiver.h" #include "canbus/can_comm/can_sender.h" #include "canbus/can_comm/message_manager.h" @@ -15,8 +15,8 @@ #include "canbus/canopen/sdo_request_protocol.h" #include "canbus/canopen/sync_protocol.h" #include "canbus/canopen/nmt_request_protocol.h" -#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h" -#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h" #include namespace cmvr { diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo1.cpp similarity index 89% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp rename to cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo1.cpp index e4e9c2c6..b1194f6a 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo1.cpp @@ -2,7 +2,7 @@ // Created by lgv on 2025/7/28. // -#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h" #include "common/base/logging/logger.h" using namespace cmvr::msgs; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo2.cpp similarity index 84% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp rename to cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo2.cpp index 24edb9c3..8e8999f5 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo2.cpp @@ -2,7 +2,7 @@ // Created by lgv on 2026/3/3. // -#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h" #include "common/base/logging/logger.h" using namespace cmvr::msgs; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp similarity index 95% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp rename to cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp index 48eaadf3..b5486dec 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp @@ -3,7 +3,7 @@ // Created by lgv on 2025/7/24. // -#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h" using namespace cmvr::device::motor; using namespace cmvr::msgs; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo1.cpp similarity index 93% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp rename to cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo1.cpp index 99c88c38..4cb811b0 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo1.cpp @@ -2,7 +2,7 @@ // Created by lgv on 2025/7/25. // -#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h" #include "common/base/logging/logger.h" #include "canbus/canopen/register.h" using namespace cmvr::msgs; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp similarity index 93% rename from cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp rename to cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp index 7907fdc6..e4c98b36 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp @@ -2,7 +2,7 @@ // Created by lgv on 2025/7/25. // -#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo2.h" #include "common/base/logging/logger.h" using namespace cmvr::device::motor; diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor.cpp new file mode 100644 index 00000000..ed7d945f --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor.cpp @@ -0,0 +1,5 @@ +// +// Created by lgv on 2025/8/1. +// + +#include "motor/drivers/ti5_canopen/include/ti5_motor.h" diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp similarity index 98% rename from cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp rename to cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp index 0b9a5eda..c5546b2a 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp @@ -3,11 +3,11 @@ // Created by lgv on 2025/8/1. // -#include "ti5_motor_canopen_protocol.h" +#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" #include "canbus/canopen/register.h" -#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h" -#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h" -#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo2.h" +#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h" #include "canbus/canopen/nmt_response_protocol.h" using namespace cmvr::device; using namespace cmvr::msgs; diff --git a/cmvr-es/devices/motor/manager/CMakeLists.txt b/cmvr-es/devices/motor/manager/CMakeLists.txt new file mode 100644 index 00000000..8eea8209 --- /dev/null +++ b/cmvr-es/devices/motor/manager/CMakeLists.txt @@ -0,0 +1,20 @@ +add_library(motor_manager SHARED + src/motor_manager.cpp +) + +target_include_directories(motor_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(motor_manager + PUBLIC + cmvr_es::proto + cmvr_es::device::motor_core + cmvr_es::device::motor_bus_runtime + PRIVATE + cmvr_es::device::ti5_canopen_motor_driver + cmvr_es::device::mujoco_motor_driver + cmvr_es::ik_solver + glog +) + +add_library(cmvr_es::device::motor_manager ALIAS motor_manager) +install(TARGETS motor_manager LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h new file mode 100644 index 00000000..3bbd2cab --- /dev/null +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -0,0 +1,98 @@ +#ifndef CMVR_ES_MOTOR_MANAGER_H +#define CMVR_ES_MOTOR_MANAGER_H + +#include +#include +#include +#include +#include +#include +#include + +#include "devices/abstract_device.h" +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "motor/abstract_motor.h" + +namespace cmvr::msgs { + +class RobotDetail; + +} // namespace cmvr::msgs + +namespace cmvr::simulate { + +class MujocoWorld; + +} // namespace cmvr::simulate + +namespace cmvr::device { + +class AbstractMotorBusRuntime; + +class MotorManager final : public AbstractDevice, + public std::enable_shared_from_this { +public: + MotorManager(std::string id, const config::MotorConfig& cfg); + ~MotorManager() override; + + DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; } + std::string typeName() const override { return "MotorManager"; } + + bool init() override; + bool stop() override; + + bool addMotor(const std::shared_ptr& motor); + std::shared_ptr getMotor(std::uint8_t node_id) const; + std::shared_ptr getMotor(const std::string& joint_name) const; + const std::unordered_map>& motorsMap() const; + + static std::shared_ptr managerFor(const std::string& id); + static std::shared_ptr mujocoWorldFor(const std::string& id); + static void setActiveJoints(const std::string& motor_manager_id, + std::unordered_map> group_joints); + static void clearActiveJoints(); + +private: + using ActiveJointSelection = std::unordered_map>; + + bool selectActiveMotors_(const std::string& group_name, + const google::protobuf::RepeatedPtrField& source, + std::vector& selected) const; + bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, + std::vector& selected) const; + std::shared_ptr createBusRuntime_( + const config::MotorGroupConfig& group_cfg) const; + std::vector> createMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const; + std::vector> createCanMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const; + std::vector> createMujocoMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const; + std::vector> createEthercatMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const; + +private: + config::MotorConfig cfg_; + std::vector> bus_runtimes_; + mutable std::mutex motors_mutex_; + std::unordered_map> motors_by_id_; + std::unordered_map> motors_by_joint_; + bool initialized_{false}; + + static std::mutex registry_mutex_; + static std::unordered_map> managers_; + static std::unordered_map> mujoco_world_registry_; + static std::unordered_map active_joints_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MOTOR_MANAGER_H diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp new file mode 100644 index 00000000..d1e6affe --- /dev/null +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -0,0 +1,552 @@ +#include "motor/manager/include/motor_manager.h" + +#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" + +namespace cmvr::device { + +std::mutex MotorManager::registry_mutex_; +std::unordered_map> MotorManager::managers_; +std::unordered_map> MotorManager::mujoco_world_registry_; +std::unordered_map MotorManager::active_joints_; + +MotorManager::MotorManager(std::string id, const config::MotorConfig& cfg) + : cfg_(cfg) +{ + id_ = std::move(id); + if (!cfg_.id().empty() && cfg_.id() != id_) { + CMVR_LOG(ERROR) << "[MotorManager] config id '" << cfg_.id() + << "' does not match device id '" << id_ << "'"; + id_.clear(); + } +} + +MotorManager::~MotorManager() = default; + +bool MotorManager::init() +{ + if (initialized_) { + return true; + } + if (id_.empty()) { + CMVR_LOG(ERROR) << "[MotorManager] id is empty"; + return false; + } + + bus_runtimes_.clear(); + bus_runtimes_.reserve(static_cast(cfg_.motor_groups_size())); + { + std::lock_guard lock(motors_mutex_); + motors_by_id_.clear(); + motors_by_joint_.clear(); + } + + bool all_ok = true; + for (const auto& motor_group_cfg : cfg_.motor_groups()) { + const auto& group_name = motor_group_cfg.id(); + if (group_name.empty()) { + CMVR_LOG(ERROR) << "[MotorManager] motor group id is empty in manager: " << id_; + all_ok = false; + continue; + } + if (!motor_group_cfg.has_motors()) { + CMVR_LOG(ERROR) << "[MotorManager] motor group missing motors: " << group_name; + all_ok = false; + continue; + } + + std::vector selected_motor_cfgs; + const bool selected_active_group = selectActiveMotors_( + group_name, motor_group_cfg.motors().motors(), selected_motor_cfgs); + if (!selected_active_group) { + continue; + } + + if (!applyConfiguredJointLimits_(motor_group_cfg, selected_motor_cfgs)) { + all_ok = false; + continue; + } + + auto bus_runtime = createBusRuntime_(motor_group_cfg); + if (!bus_runtime) { + all_ok = false; + continue; + } + if (!bus_runtime->init(motor_group_cfg)) { + bus_runtime->stop(); + all_ok = false; + continue; + } + if (!bus_runtime->start()) { + bus_runtime->stop(); + all_ok = false; + continue; + } + + auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime); + if (motors.empty()) { + CMVR_LOG(ERROR) << "[MotorManager] motor group created no motors: " << group_name; + bus_runtime->stop(); + all_ok = false; + continue; + } + bool group_ok = true; + for (auto& motor : motors) { + if (!addMotor(motor)) { + group_ok = false; + break; + } + } + if (!group_ok) { + bus_runtime->stop(); + all_ok = false; + continue; + } + + bus_runtimes_.push_back(std::move(bus_runtime)); + } + + if (!all_ok) { + for (auto& bus_runtime : bus_runtimes_) { + if (bus_runtime) { + bus_runtime->stop(); + } + } + CMVR_LOG(ERROR) << "[MotorManager] motor initialization failed: " << id_; + return false; + } + + initialized_ = true; + { + std::lock_guard lock(registry_mutex_); + managers_[id_] = shared_from_this(); + mujoco_world_registry_.erase(id_); + for (const auto& bus_runtime : bus_runtimes_) { + if (!bus_runtime) { + continue; + } + auto world = bus_runtime->mujocoWorld(); + if (world) { + mujoco_world_registry_[id_] = std::move(world); + break; + } + } + } + CMVR_LOG(INFO) << "[MotorManager] Initialized motor manager: " << id_; + return true; +} + +bool MotorManager::stop() +{ + { + std::lock_guard lock(motors_mutex_); + for (const auto& [joint_name, motor] : motors_by_joint_) { + (void)joint_name; + if (motor) { + motor->torqueOff(); + } + } + } + for (auto& bus_runtime : bus_runtimes_) { + if (bus_runtime) { + bus_runtime->stop(); + } + } + return true; +} + +bool MotorManager::addMotor(const std::shared_ptr& motor) +{ + if (!motor) { + CMVR_LOG(ERROR) << "[MotorManager] Cannot register null motor"; + return false; + } + if (motor->jointName().empty()) { + CMVR_LOG(ERROR) << "[MotorManager] Cannot register motor with empty joint name"; + return false; + } + + std::lock_guard lock(motors_mutex_); + const auto node_id = motor->id(); + const auto& joint_name = motor->jointName(); + if (motors_by_id_.count(node_id) > 0) { + CMVR_LOG(ERROR) << "[MotorManager] Duplicate motor id: " + << static_cast(node_id); + return false; + } + if (motors_by_joint_.count(joint_name) > 0) { + CMVR_LOG(ERROR) << "[MotorManager] Duplicate motor joint: " << joint_name; + return false; + } + motors_by_id_[node_id] = motor; + motors_by_joint_[joint_name] = motor; + return true; +} + +std::shared_ptr MotorManager::getMotor(const std::uint8_t node_id) const +{ + std::lock_guard lock(motors_mutex_); + const auto it = motors_by_id_.find(node_id); + if (it == motors_by_id_.end()) { + return nullptr; + } + return it->second; +} + +std::shared_ptr MotorManager::getMotor(const std::string& joint_name) const +{ + std::lock_guard lock(motors_mutex_); + const auto it = motors_by_joint_.find(joint_name); + if (it == motors_by_joint_.end()) { + return nullptr; + } + return it->second; +} + +const std::unordered_map>& MotorManager::motorsMap() const +{ + return motors_by_joint_; +} + +std::shared_ptr MotorManager::managerFor(const std::string& id) +{ + std::lock_guard lock(registry_mutex_); + const auto it = managers_.find(id); + if (it == managers_.end()) { + return nullptr; + } + return it->second.lock(); +} + +std::shared_ptr MotorManager::mujocoWorldFor(const std::string& id) +{ + std::lock_guard lock(registry_mutex_); + const auto it = mujoco_world_registry_.find(id); + if (it == mujoco_world_registry_.end()) { + return nullptr; + } + return it->second.lock(); +} + +void MotorManager::setActiveJoints(const std::string& motor_manager_id, + ActiveJointSelection group_joints) +{ + std::lock_guard lock(registry_mutex_); + active_joints_[motor_manager_id] = std::move(group_joints); +} + +void MotorManager::clearActiveJoints() +{ + std::lock_guard lock(registry_mutex_); + active_joints_.clear(); +} + +bool MotorManager::selectActiveMotors_( + const std::string& group_name, + const google::protobuf::RepeatedPtrField& source, + std::vector& selected) const +{ + selected.clear(); + + ActiveJointSelection selection; + { + std::lock_guard lock(registry_mutex_); + const auto it = active_joints_.find(id_); + if (it != active_joints_.end()) { + selection = it->second; + } + } + + if (selection.empty()) { + CMVR_LOG(WARNING) << "[MotorManager] No active joints selected for motor manager " << id_ + << ", motor group " << group_name << " will not initialize motors."; + return false; + } + + const auto group_it = selection.find(group_name); + if (group_it == selection.end() || group_it->second.empty()) { + return false; + } + + for (const auto& motor_cfg : source) { + if (group_it->second.count(motor_cfg.joint_name()) > 0) { + selected.push_back(motor_cfg); + } + } + + if (selected.size() != group_it->second.size()) { + std::unordered_set found; + for (const auto& motor_cfg : selected) { + found.insert(motor_cfg.joint_name()); + } + for (const auto& joint_name : group_it->second) { + if (found.count(joint_name) == 0) { + CMVR_LOG(ERROR) << "[MotorManager] active joint '" << joint_name + << "' not found in motor group '" << group_name << "'"; + return false; + } + } + } + return !selected.empty(); +} + +bool MotorManager::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, + std::vector& selected) const +{ + if (selected.empty() || !group_cfg.has_joint_limits()) { + return true; + } + + const auto& limits_cfg = group_cfg.joint_limits(); + if (!limits_cfg.enable()) { + if (group_cfg.bus_type() == config::MOTOR_BUS_MUJOCO) { + constexpr double kDisabledLimit = 1e6; + for (auto& motor_cfg : selected) { + motor_cfg.set_limit_q_lb(-kDisabledLimit); + motor_cfg.set_limit_q_ub(kDisabledLimit); + motor_cfg.set_limit_qd(kDisabledLimit); + motor_cfg.set_limit_qdd(kDisabledLimit); + } + } + return true; + } + + std::unordered_map limits_by_joint; + if (limits_cfg.source() == config::JOINT_LIMIT_SOURCE_CUSTOM) { + limits_by_joint.reserve(static_cast(limits_cfg.joints_size())); + for (const auto& limit : limits_cfg.joints()) { + if (!limit.joint_name().empty()) { + limits_by_joint[limit.joint_name()] = limit; + } + } + } else if (limits_cfg.source() == config::JOINT_LIMIT_SOURCE_URDF) { + const auto urdf_path = + cmvr::ConfigHelper::resolveResourceFile(limits_cfg.urdf_path()); + if (urdf_path.empty()) { + CMVR_LOG(ERROR) << "[MotorManager] joint_limits.urdf_path is empty for motor group: " + << group_cfg.id(); + return false; + } + + UrdfParser parser; + std::string error; + if (!parser.loadModel(urdf_path, &error)) { + CMVR_LOG(ERROR) << "[MotorManager] Failed to load joint limit URDF: " << error; + return false; + } + const auto& model = parser.model(); + for (pinocchio::JointIndex joint_id = 1; joint_id < model.njoints; ++joint_id) { + if (model.nqs[joint_id] != 1 || model.nvs[joint_id] != 1) { + continue; + } + config::JointLimitConfig limit; + limit.set_joint_name(model.names[joint_id]); + limit.set_q_lb(model.lowerPositionLimit[model.idx_qs[joint_id]]); + limit.set_q_ub(model.upperPositionLimit[model.idx_qs[joint_id]]); + limit.set_qd(model.velocityLimit[model.idx_vs[joint_id]]); + limits_by_joint[limit.joint_name()] = limit; + } + } else { + CMVR_LOG(ERROR) << "[MotorManager] unsupported joint limit source"; + return false; + } + + for (auto& motor_cfg : selected) { + const auto it = limits_by_joint.find(motor_cfg.joint_name()); + if (it == limits_by_joint.end()) { + CMVR_LOG(ERROR) << "[MotorManager] missing joint limit for " << motor_cfg.joint_name() + << " in motor group: " << group_cfg.id(); + return false; + } + const auto& limit = it->second; + if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) || + !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() || + limit.qd() <= 0.0 || !std::isfinite(limit.qdd()) || + limit.qdd() < 0.0) { + CMVR_LOG(ERROR) << "[MotorManager] invalid joint limit for " << motor_cfg.joint_name(); + return false; + } + motor_cfg.set_limit_q_lb(limit.q_lb()); + motor_cfg.set_limit_q_ub(limit.q_ub()); + motor_cfg.set_limit_qd(std::abs(limit.qd())); + if (limit.qdd() > 0.0) { + motor_cfg.set_limit_qdd(std::abs(limit.qdd())); + } + } + return true; +} + +std::shared_ptr MotorManager::createBusRuntime_( + const config::MotorGroupConfig& group_cfg) const +{ + switch (group_cfg.bus_type()) { + case config::MOTOR_BUS_CAN: + return std::make_shared(); + case config::MOTOR_BUS_MUJOCO: + return std::make_shared(); + case config::MOTOR_BUS_ETHERCAT: + return std::make_shared(); + default: + CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: " + << config::MotorBusType_Name(group_cfg.bus_type()) + << ", group=" << group_cfg.id(); + return nullptr; + } +} + +std::vector> MotorManager::createMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const +{ + if (group_cfg.vendor() == config::MOTOR_VENDOR_UNKNOWN || + group_cfg.protocol() == config::MOTOR_PROTOCOL_UNKNOWN) { + CMVR_LOG(ERROR) << "[MotorManager] missing vendor/protocol: " << group_cfg.id(); + return {}; + } + + switch (group_cfg.bus_type()) { + case config::MOTOR_BUS_CAN: + return createCanMotors_(group_cfg, motor_cfgs, bus_runtime); + case config::MOTOR_BUS_MUJOCO: + return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime); + case config::MOTOR_BUS_ETHERCAT: + return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime); + default: + CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: " + << config::MotorBusType_Name(group_cfg.bus_type()) + << ", group=" << group_cfg.id(); + return {}; + } +} + +std::vector> MotorManager::createCanMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const +{ + auto can_bus_runtime = std::dynamic_pointer_cast(bus_runtime); + if (!can_bus_runtime || !can_bus_runtime->sender() || !can_bus_runtime->messageManager()) { + CMVR_LOG(ERROR) << "[MotorManager] missing CAN bus runtime: " << group_cfg.id(); + return {}; + } + if (!group_cfg.has_can()) { + CMVR_LOG(ERROR) << "[MotorManager] missing CAN config: " << group_cfg.id(); + return {}; + } + if (group_cfg.vendor() != config::MOTOR_VENDOR_TI5 || + group_cfg.protocol() != config::MOTOR_PROTOCOL_CANOPEN) { + CMVR_LOG(ERROR) << "[MotorManager] unsupported CAN motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return {}; + } + + auto protocol = std::make_shared( + can_bus_runtime->sender(), can_bus_runtime->messageManager()); + + std::vector> motors; + 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: " + << cfg.joint_name(); + return {}; + } + motors.push_back(std::move(motor)); + } + return motors; +} + +std::vector> MotorManager::createMujocoMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const +{ + auto mujoco_bus_runtime = std::dynamic_pointer_cast(bus_runtime); + if (!mujoco_bus_runtime || !mujoco_bus_runtime->mujocoWorld()) { + CMVR_LOG(ERROR) << "[MotorManager] missing MuJoCo bus runtime: " << group_cfg.id(); + return {}; + } + if (!group_cfg.has_mujoco()) { + CMVR_LOG(ERROR) << "[MotorManager] missing MuJoCo config: " << group_cfg.id(); + return {}; + } + if (group_cfg.vendor() != config::MOTOR_VENDOR_MUJOCO || + group_cfg.protocol() != config::MOTOR_PROTOCOL_MUJOCO) { + CMVR_LOG(ERROR) << "[MotorManager] unsupported MuJoCo motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return {}; + } + + std::vector> motors; + motors.reserve(motor_cfgs.size()); + for (const auto& cfg : motor_cfgs) { + auto motor = std::make_shared( + cfg.joint_name(), + mujoco_bus_runtime->mujocoWorld(), + static_cast(cfg.id())); + motor->setLimitQ(cfg.limit_q_ub(), cfg.limit_q_lb()); + motor->setLimitQd(cfg.limit_qd()); + if (cfg.limit_qdd() > 0.0) { + motor->setLimitQdd(cfg.limit_qdd(), -cfg.limit_qdd()); + } + if (!motor->init()) { + CMVR_LOG(ERROR) << "[MotorManager] failed to init MuJoCo motor: " + << cfg.joint_name(); + return {}; + } + motors.push_back(std::move(motor)); + } + return motors; +} + +std::vector> MotorManager::createEthercatMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const +{ + auto ethercat_bus_runtime = std::dynamic_pointer_cast(bus_runtime); + if (!ethercat_bus_runtime) { + CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT bus runtime: " << group_cfg.id(); + return {}; + } + if (!group_cfg.has_ethercat()) { + CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << 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 " + << motor_cfg.id() << " in group: " << group_cfg.id(); + return {}; + } + } + + 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 {}; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/motor_manager.h b/cmvr-es/devices/motor/motor_manager.h deleted file mode 100644 index c81f40dd..00000000 --- a/cmvr-es/devices/motor/motor_manager.h +++ /dev/null @@ -1,57 +0,0 @@ -// -// Created by lgv on 2025/8/1. -// - - -#pragma once - -#include "abstract_motor.h" - -#include -#include -#include -#include - -namespace cmvr { - namespace device { - - class MotorManager { - public: - void addMotor(uint8_t node_id, std::shared_ptr motor) { - std::lock_guard lock(mutex_); - motors_[node_id] = std::move(motor); - } - - void addMotor(std::shared_ptr motor) { - std::lock_guard lock(mutex_); - motors_map_[motor->jointName()] = std::move(motor); - } - - std::shared_ptr getMotor(uint8_t node_id) const { - auto it = motors_.find(node_id); - if (it != motors_.end()) { - return it->second; - } - return nullptr; - } - - std::shared_ptr getMotor(const std::string& joint_name) const { - auto it = motors_map_.find(joint_name); - if (it != motors_map_.end()) { - return it->second; - } - return nullptr; - } - - const std::unordered_map>& motorsMap() const { - return motors_map_; - } - - private: - mutable std::mutex mutex_; - std::unordered_map> motors_; - std::unordered_map> motors_map_; - }; - - } // namespace device -} // namespace cmvr diff --git a/cmvr-es/devices/motor/motor_manager_test.cpp b/cmvr-es/devices/motor/motor_manager_test.cpp deleted file mode 100644 index 3bf862eb..00000000 --- a/cmvr-es/devices/motor/motor_manager_test.cpp +++ /dev/null @@ -1,120 +0,0 @@ -#include "common/base/logging/logger.h" -// -// Created by lgv on 2025/8/1. -// - -#include "motor/motor_manager.h" -#include "gtest/gtest.h" -#include "motor/ti5_motor/ti5_motor.h" -#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" - - -#include "canbus/can_comm/can_sender.h" -#include "canbus/can_comm/message_manager.h" -#include "canbus/can_client/socket/socket_can_client_raw.h" - -using namespace cmvr::device; -using namespace cmvr::msgs; - -TEST(MotorMangerTest,MyTest) { - - uint8_t id = 22; - cmvr::config::SocketCanConfig can_cfg; - can_cfg.set_channel_id(0); - cmvr::config::Ti5MotorConfig motor_cfg; - motor_cfg.set_id(id); - motor_cfg.set_joint_name("test_motor"); - motor_cfg.set_limit_q_lb(-3.14); - motor_cfg.set_limit_q_ub(3.14); - motor_cfg.set_limit_qd(3.0); - - // 1 === 初始化公共组件 === - auto can_client = std::make_shared(can_cfg); - auto sender = std::make_shared>(); - auto receiver = std::make_shared>(); - auto message_manager = std::make_shared>(); - - - can_client->init(); - auto ret = sender->Init(can_client.get(), false); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "Failed to init can sender."; - } - - - ret = receiver->Init(can_client.get(), message_manager.get(), false); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "Failed to init can receiver."; - } - - // 2 == 创建协议 === - auto canopen_protocol = std::make_shared(sender, message_manager); - - // 3 === 创建电机 === - auto motor = std::make_shared(motor_cfg); - motor->setProtocol(canopen_protocol); - - // 4 === 添加电机到 MotorManager=== - auto manager = std::make_shared(); - manager->addMotor(id,motor); - - - // 5 === 启动通讯 === - can_client->start(); - ret = sender->Start(); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "Failed to start can sender."; - } - - ret = receiver->Start(); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "Failed to start can receiver."; - } - - // 6 === 控制电机 === - auto motor_3 = manager->getMotor(id); - motor_3->init(); - - // motor_3->calibrateZeroQ(); - - - - - // motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY); - // motor_3->setTarget(-0.5); - // std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION); - // motor_3->setTarget(0,1.6); - - // std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - // - // motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION); - // motor_3->setTarget(0,1.6); - // motor_3->calibrateZeroQ(); - // motor_3->setMode(RUN_MODE_PROFILE_POSITION); - // motor_3->setLimitQ(30.14,-40.14); - // motor_3->setQ(-30); - // std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - // motor_3->brake(); - // std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - // motor_3->setQ(-30); - // motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION); - // std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - // motor_3->setQ(-30.14); - // manager->init(id); - // manager->setMode(id,RUN_MODE_CYCLIC_SYNC_POSITION); - // manager->setQ(id,3.14); - - while (true) { - - auto mode = motor_3->getMode(); - auto q = motor_3->getQ(); - auto qd = motor_3->getQd(); - std::cout << q << ", " << qd << std::endl; - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - // motor_3->getQ(q); - - } - - CMVR_LOG(INFO) << "Testing MotorManger"; -} diff --git a/cmvr-es/devices/motor/motor_system/CMakeLists.txt b/cmvr-es/devices/motor/motor_system/CMakeLists.txt deleted file mode 100644 index ca0babd2..00000000 --- a/cmvr-es/devices/motor/motor_system/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -add_library(motor_system SHARED - src/motor_system.cpp -) - -target_include_directories(motor_system PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(motor_system - PUBLIC - cmvr_es::proto - PRIVATE - cmvr_es::device::canbus - cmvr_es::device::ti5motor - cmvr_es::device::mujoco_motor - cmvr_es::ik_solver - glog -) - -add_library(cmvr_es::device::motor_system ALIAS motor_system) -install(TARGETS motor_system LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/motor_system/include/motor_system.h b/cmvr-es/devices/motor/motor_system/include/motor_system.h deleted file mode 100644 index 2e9083f2..00000000 --- a/cmvr-es/devices/motor/motor_system/include/motor_system.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef CMVR_ES_MOTOR_SYSTEM_H -#define CMVR_ES_MOTOR_SYSTEM_H - -#include -#include -#include -#include -#include -#include - -#include "devices/abstract_device.h" -#include "devices/canbus/abstract_canbus.h" -#include "canbus/can_comm/can_receiver.h" -#include "canbus/can_comm/can_sender.h" -#include "canbus/can_comm/message_manager.h" -#include "cmvr/config/motor_config/motor_config.pb.h" -#include "cmvr/msgs/robot_detail.pb.h" -#include "motor/motor_manager.h" - -namespace cmvr::device { - -class MujocoJointBridge; - -class MotorSystem final : public AbstractDevice { -public: - MotorSystem(std::string id, const config::MotorConfig& cfg); - ~MotorSystem() override = default; - - DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; } - std::string typeName() const override { return "MotorSystem"; } - - bool init() override; - bool stop() override; - - std::shared_ptr manager() const { return motor_manager_; } - static std::shared_ptr managerFor(const std::string& id); - static std::shared_ptr mujocoBridgeFor(const std::string& id); - static void setActiveJoints(const std::string& motor_system_id, - std::unordered_map> group_joints); - static void clearActiveJoints(); - -private: - using ActiveJointSelection = std::unordered_map>; - - struct MotorGroupRuntime { - std::string name; - config::MotorBusType bus_type{config::MOTOR_BUS_UNKNOWN}; - bool enabled{false}; - std::shared_ptr client; - std::shared_ptr> sender; - std::shared_ptr> receiver; - std::shared_ptr> message_manager; - std::shared_ptr mujoco_bridge; - std::vector motor_cfgs; - }; - - bool initMotorGroup_(MotorGroupRuntime& group); - bool initMotors_(const MotorGroupRuntime& group); - bool selectActiveMotors_(const std::string& group_name, - const google::protobuf::RepeatedPtrField& source, - std::vector& selected) const; - bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, - std::vector& selected) const; - static bool makeSocketCanConfig_(const config::MotorGroupConfig& group_cfg, - config::SocketCanConfig& config); - -private: - config::MotorConfig cfg_; - std::vector motor_groups_; - std::shared_ptr motor_manager_{nullptr}; - bool initialized_{false}; - - static std::mutex registry_mutex_; - static std::unordered_map> registry_; - static std::unordered_map> mujoco_bridge_registry_; - static std::unordered_map active_joints_; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_MOTOR_SYSTEM_H diff --git a/cmvr-es/devices/motor/motor_system/src/motor_system.cpp b/cmvr-es/devices/motor/motor_system/src/motor_system.cpp deleted file mode 100644 index c2201bb9..00000000 --- a/cmvr-es/devices/motor/motor_system/src/motor_system.cpp +++ /dev/null @@ -1,411 +0,0 @@ -#include "motor/motor_system/include/motor_system.h" - -#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 "devices/motor/mujoco_motor/include/mujoco_motor.h" -#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" -#include "motor/ti5_motor/ti5_motor.h" - -namespace cmvr::device { - -std::mutex MotorSystem::registry_mutex_; -std::unordered_map> MotorSystem::registry_; -std::unordered_map> MotorSystem::mujoco_bridge_registry_; -std::unordered_map MotorSystem::active_joints_; - -MotorSystem::MotorSystem(std::string id, const config::MotorConfig& cfg) - : cfg_(cfg) -{ - id_ = std::move(id); - if (!cfg_.id().empty() && cfg_.id() != id_) { - CMVR_LOG(ERROR) << "[MotorSystem] config id '" << cfg_.id() - << "' does not match device id '" << id_ << "'"; - id_.clear(); - } -} - -bool MotorSystem::init() -{ - if (initialized_) { - return true; - } - - motor_manager_ = std::make_shared(); - if (id_.empty()) { - CMVR_LOG(ERROR) << "[MotorSystem] id is empty"; - return false; - } - motor_groups_.clear(); - motor_groups_.reserve(cfg_.motor_groups_size()); - - bool all_ok = true; - for (const auto& motor_group_cfg : cfg_.motor_groups()) { - MotorGroupRuntime group; - group.name = motor_group_cfg.id(); - if (group.name.empty()) { - CMVR_LOG(ERROR) << "[MotorSystem] motor group id is empty in system: " << id_; - continue; - } - group.bus_type = motor_group_cfg.bus_type(); - group.enabled = selectActiveMotors_(group.name, motor_group_cfg.motors(), group.motor_cfgs); - const bool selected_active_group = group.enabled; - if (group.enabled && !applyConfiguredJointLimits_(motor_group_cfg, group.motor_cfgs)) { - group.enabled = false; - } - if (group.enabled) { - switch (group.bus_type) { - case config::MOTOR_BUS_CAN: { - config::SocketCanConfig can_config; - if (!makeSocketCanConfig_(motor_group_cfg, can_config)) { - group.enabled = false; - break; - } - group.client = std::make_shared(can_config); - group.sender = std::make_shared>(); - group.receiver = std::make_shared>(); - group.message_manager = std::make_shared>(); - break; - } - case config::MOTOR_BUS_MUJOCO: - group.mujoco_bridge = std::make_shared(group.motor_cfgs.size()); - break; - case config::MOTOR_BUS_ETHERCAT: - CMVR_LOG(ERROR) << "[MotorSystem] EtherCAT motor group is configured but runtime is not implemented: " << group.name; - group.enabled = false; - break; - case config::MOTOR_BUS_UNKNOWN: - default: - CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor bus type for group: " << group.name; - group.enabled = false; - break; - } - } - if (selected_active_group && !group.enabled) { - all_ok = false; - } - motor_groups_.emplace_back(std::move(group)); - } - - std::vector> tasks; - for (auto& group : motor_groups_) { - if (!group.enabled) { - continue; - } - if (group.bus_type != config::MOTOR_BUS_MUJOCO) { - if (!initMotorGroup_(group)) { - group.enabled = false; - all_ok = false; - continue; - } - } - tasks.push_back(std::async(std::launch::async, [this, &group]() { - return initMotors_(group); - })); - } - - for (auto& task : tasks) { - all_ok = task.get() && all_ok; - } - if (!all_ok) { - CMVR_LOG(ERROR) << "[MotorSystem] motor initialization failed: " << id_; - return false; - } - - initialized_ = true; - { - std::lock_guard lock(registry_mutex_); - registry_[id_] = motor_manager_; - for (const auto& group : motor_groups_) { - if (group.mujoco_bridge) { - mujoco_bridge_registry_[id_] = group.mujoco_bridge; - break; - } - } - } - CMVR_LOG(INFO) << "[MotorSystem] Initialized motor system: " << id_; - return true; -} - -bool MotorSystem::stop() -{ - if (!motor_manager_) { - return true; - } - for (const auto& [joint_name, motor] : motor_manager_->motorsMap()) { - if (motor) { - motor->torqueOff(); - } - } - return true; -} - -std::shared_ptr MotorSystem::managerFor(const std::string& id) -{ - std::lock_guard lock(registry_mutex_); - const auto it = registry_.find(id); - if (it == registry_.end()) { - return nullptr; - } - return it->second.lock(); -} - -std::shared_ptr MotorSystem::mujocoBridgeFor(const std::string& id) -{ - std::lock_guard lock(registry_mutex_); - const auto it = mujoco_bridge_registry_.find(id); - if (it == mujoco_bridge_registry_.end()) { - return nullptr; - } - return it->second.lock(); -} - -void MotorSystem::setActiveJoints(const std::string& motor_system_id, - ActiveJointSelection group_joints) -{ - std::lock_guard lock(registry_mutex_); - active_joints_[motor_system_id] = std::move(group_joints); -} - -void MotorSystem::clearActiveJoints() -{ - std::lock_guard lock(registry_mutex_); - active_joints_.clear(); -} - -bool MotorSystem::selectActiveMotors_( - const std::string& group_name, - const google::protobuf::RepeatedPtrField& source, - std::vector& selected) const -{ - selected.clear(); - - ActiveJointSelection selection; - { - std::lock_guard lock(registry_mutex_); - const auto it = active_joints_.find(id_); - if (it != active_joints_.end()) { - selection = it->second; - } - } - - if (selection.empty()) { - CMVR_LOG(WARNING) << "[MotorSystem] No active joints selected for motor system " << id_ - << ", motor group " << group_name << " will not initialize motors."; - return false; - } - - const auto group_it = selection.find(group_name); - if (group_it == selection.end() || group_it->second.empty()) { - return false; - } - - for (const auto& motor_cfg : source) { - if (group_it->second.count(motor_cfg.joint_name()) > 0) { - selected.push_back(motor_cfg); - } - } - - if (selected.size() != group_it->second.size()) { - std::unordered_set found; - for (const auto& motor_cfg : selected) { - found.insert(motor_cfg.joint_name()); - } - for (const auto& joint_name : group_it->second) { - if (found.count(joint_name) == 0) { - CMVR_LOG(ERROR) << "[MotorSystem] active joint '" << joint_name - << "' not found in motor group '" << group_name << "'"; - return false; - } - } - } - return !selected.empty(); -} - -bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, - std::vector& selected) const -{ - if (selected.empty() || !group_cfg.has_joint_limits()) { - return true; - } - - const auto& limits_cfg = group_cfg.joint_limits(); - const auto source = limits_cfg.source(); - if (source == config::JOINT_LIMIT_SOURCE_UNKNOWN) { - return true; - } - - std::unordered_map limits_by_joint; - if (source == config::JOINT_LIMIT_SOURCE_CUSTOM) { - limits_by_joint.reserve(static_cast(limits_cfg.joints_size())); - for (const auto& limit : limits_cfg.joints()) { - if (!limit.joint_name().empty()) { - limits_by_joint[limit.joint_name()] = limit; - } - } - } else if (source == config::JOINT_LIMIT_SOURCE_URDF) { - const auto urdf_path = - cmvr::ConfigHelper::resolveResourceFile(limits_cfg.urdf_path()); - if (urdf_path.empty()) { - CMVR_LOG(ERROR) << "[MotorSystem] joint_limits.urdf_path is empty for motor group: " - << group_cfg.id(); - return false; - } - - UrdfParser parser; - std::string error; - if (!parser.loadModel(urdf_path, &error)) { - CMVR_LOG(ERROR) << "[MotorSystem] Failed to load joint limit URDF: " << error; - return false; - } - const auto& model = parser.model(); - for (pinocchio::JointIndex joint_id = 1; joint_id < model.njoints; ++joint_id) { - if (model.nqs[joint_id] != 1 || model.nvs[joint_id] != 1) { - continue; - } - config::JointLimitConfig limit; - limit.set_joint_name(model.names[joint_id]); - limit.set_q_lb(model.lowerPositionLimit[model.idx_qs[joint_id]]); - limit.set_q_ub(model.upperPositionLimit[model.idx_qs[joint_id]]); - limit.set_qd(model.velocityLimit[model.idx_vs[joint_id]]); - limits_by_joint[limit.joint_name()] = limit; - } - } else { - CMVR_LOG(ERROR) << "[MotorSystem] unsupported joint limit source"; - return false; - } - - for (auto& motor_cfg : selected) { - const auto it = limits_by_joint.find(motor_cfg.joint_name()); - if (it == limits_by_joint.end()) { - CMVR_LOG(ERROR) << "[MotorSystem] missing joint limit for " << motor_cfg.joint_name() - << " in motor group: " << group_cfg.id(); - return false; - } - const auto& limit = it->second; - if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) || - !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() || - limit.qd() <= 0.0 || !std::isfinite(limit.qdd()) || - limit.qdd() < 0.0) { - CMVR_LOG(ERROR) << "[MotorSystem] invalid joint limit for " << motor_cfg.joint_name(); - return false; - } - motor_cfg.set_limit_q_lb(limit.q_lb()); - motor_cfg.set_limit_q_ub(limit.q_ub()); - motor_cfg.set_limit_qd(std::abs(limit.qd())); - if (limit.qdd() > 0.0) { - motor_cfg.set_limit_qdd(std::abs(limit.qdd())); - } - } - return true; -} - -bool MotorSystem::makeSocketCanConfig_(const config::MotorGroupConfig& group_cfg, - config::SocketCanConfig& config) -{ - if (!group_cfg.has_can()) { - CMVR_LOG(ERROR) << "[MotorSystem] CAN motor group missing can config: " << group_cfg.id(); - return false; - } - config = group_cfg.can(); - return true; -} - -bool MotorSystem::initMotorGroup_(MotorGroupRuntime& group) -{ - if (group.bus_type != config::MOTOR_BUS_CAN) { - CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor group runtime: " << group.name; - return false; - } - if (!group.client || !group.sender || !group.receiver || !group.message_manager) { - CMVR_LOG(ERROR) << "[MotorSystem] Invalid motor group runtime: " << group.name; - return false; - } - - if (!group.client->init()) { - CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN client: " << group.name; - return false; - } - - auto ret = group.sender->Init(group.client.get(), false); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN sender: " << group.name; - return false; - } - - ret = group.receiver->Init(group.client.get(), group.message_manager.get(), false); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN receiver: " << group.name; - return false; - } - - if (!group.client->start()) { - CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN client: " << group.name; - return false; - } - - ret = group.sender->Start(); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN sender: " << group.name; - return false; - } - - ret = group.receiver->Start(); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN receiver: " << group.name; - return false; - } - return true; -} - -bool MotorSystem::initMotors_(const MotorGroupRuntime& group) -{ - switch (group.bus_type) { - case config::MOTOR_BUS_CAN: { - auto protocol = std::make_shared(group.sender, group.message_manager); - for (const auto& cfg : group.motor_cfgs) { - auto motor = std::make_shared(cfg); - motor->setProtocol(protocol); - if (!motor->init()) { - CMVR_LOG(ERROR) << "[MotorSystem] Failed to init motor: " << cfg.joint_name(); - return false; - } - motor_manager_->addMotor(motor); - } - return true; - } - case config::MOTOR_BUS_MUJOCO: { - if (!group.mujoco_bridge) { - CMVR_LOG(ERROR) << "[MotorSystem] MuJoCo motor group missing joint bridge: " << group.name; - return false; - } - for (std::size_t i = 0; i < group.motor_cfgs.size(); ++i) { - const auto& cfg = group.motor_cfgs[i]; - auto motor = std::make_shared( - i, - cfg.joint_name(), - group.mujoco_bridge, - static_cast(cfg.id())); - motor->setLimitQ(cfg.limit_q_ub(), cfg.limit_q_lb()); - motor->setLimitQd(cfg.limit_qd()); - motor_manager_->addMotor(motor); - } - return true; - } - case config::MOTOR_BUS_ETHERCAT: - case config::MOTOR_BUS_UNKNOWN: - default: - CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor init runtime: " << group.name; - return false; - } -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/mujoco_motor/CMakeLists.txt b/cmvr-es/devices/motor/mujoco_motor/CMakeLists.txt deleted file mode 100644 index d4c9271d..00000000 --- a/cmvr-es/devices/motor/mujoco_motor/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -add_library(mujoco_motor SHARED - src/mujoco_motor.cpp -) - -target_include_directories(mujoco_motor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(mujoco_motor - PUBLIC - cmvr_es::proto -) - -add_library(cmvr_es::device::mujoco_motor ALIAS mujoco_motor) -install(TARGETS mujoco_motor LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h b/cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h deleted file mode 100644 index 6a868be8..00000000 --- a/cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef CMVR_ES_MUJOCO_JOINT_BRIDGE_H -#define CMVR_ES_MUJOCO_JOINT_BRIDGE_H - -#include -#include -#include -#include -#include - -#include "cmvr/msgs/motor.pb.h" - -namespace cmvr::device { - -class MujocoJointBridge { -public: - struct CommandState { - std::vector mode; - std::vector position; - std::vector velocity; - }; - - explicit MujocoJointBridge(std::size_t dof); - - std::size_t dof() const noexcept { return dof_; } - - void setMode(std::size_t index, msgs::RunMode mode); - msgs::RunMode getMode(std::size_t index) const; - - void setPositionTarget(std::size_t index, double position, double velocity); - void setVelocityTarget(std::size_t index, double velocity); - - CommandState commands() const; - - void publishMeasured(const std::vector& position, - const std::vector& velocity); - double position(std::size_t index) const; - double velocity(std::size_t index) const; - - void markReady(bool valid); - bool waitUntilReady(std::chrono::milliseconds timeout); - -private: - void validateIndex_(std::size_t index) const; - - const std::size_t dof_{0}; - mutable std::mutex mutex_; - std::condition_variable ready_cv_; - CommandState commands_; - std::vector measured_position_; - std::vector measured_velocity_; - bool ready_{false}; - bool valid_{false}; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_MUJOCO_JOINT_BRIDGE_H diff --git a/cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp b/cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp deleted file mode 100644 index a48d29db..00000000 --- a/cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp +++ /dev/null @@ -1,225 +0,0 @@ -#include "devices/motor/mujoco_motor/include/mujoco_motor.h" - -#include -#include -#include - -#include "common/base/logging/logger.h" - -namespace cmvr::device { - -MujocoJointBridge::MujocoJointBridge(const std::size_t dof) - : dof_(dof) -{ - if (dof_ == 0) { - CMVR_LOG(ERROR) << "MujocoJointBridge dof must be positive"; - return; - } - commands_.mode.assign(dof_, msgs::RUN_MODE_UNSPECIFIED); - commands_.position.assign(dof_, 0.0); - commands_.velocity.assign(dof_, 0.0); - measured_position_.assign(dof_, 0.0); - measured_velocity_.assign(dof_, 0.0); -} - -void MujocoJointBridge::validateIndex_(const std::size_t index) const -{ - if (index >= dof_) { - CMVR_LOG(ERROR) << "MujocoJointBridge joint index out of range"; - } -} - -void MujocoJointBridge::setMode(const std::size_t index, const msgs::RunMode mode) -{ - validateIndex_(index); - if (index >= dof_) { - return; - } - std::lock_guard lock(mutex_); - commands_.mode[index] = mode; -} - -msgs::RunMode MujocoJointBridge::getMode(const std::size_t index) const -{ - validateIndex_(index); - if (index >= dof_) { - return msgs::RUN_MODE_UNSPECIFIED; - } - std::lock_guard lock(mutex_); - return commands_.mode[index]; -} - -void MujocoJointBridge::setPositionTarget(const std::size_t index, - const double position, - const double velocity) -{ - validateIndex_(index); - if (index >= dof_) { - return; - } - std::lock_guard lock(mutex_); - commands_.position[index] = position; - commands_.velocity[index] = velocity; -} - -void MujocoJointBridge::setVelocityTarget(const std::size_t index, const double velocity) -{ - validateIndex_(index); - if (index >= dof_) { - return; - } - std::lock_guard lock(mutex_); - commands_.velocity[index] = velocity; -} - -MujocoJointBridge::CommandState MujocoJointBridge::commands() const -{ - std::lock_guard lock(mutex_); - return commands_; -} - -void MujocoJointBridge::publishMeasured(const std::vector& position, - const std::vector& velocity) -{ - if (position.size() != dof_ || velocity.size() != dof_) { - CMVR_LOG(ERROR) << "MujocoJointBridge measured vectors size mismatch"; - return; - } - std::lock_guard lock(mutex_); - measured_position_ = position; - measured_velocity_ = velocity; -} - -double MujocoJointBridge::position(const std::size_t index) const -{ - validateIndex_(index); - if (index >= dof_) { - return 0.0; - } - std::lock_guard lock(mutex_); - return measured_position_[index]; -} - -double MujocoJointBridge::velocity(const std::size_t index) const -{ - validateIndex_(index); - if (index >= dof_) { - return 0.0; - } - std::lock_guard lock(mutex_); - return measured_velocity_[index]; -} - -void MujocoJointBridge::markReady(const bool valid) -{ - { - std::lock_guard lock(mutex_); - ready_ = true; - valid_ = valid; - } - ready_cv_.notify_all(); -} - -bool MujocoJointBridge::waitUntilReady(const std::chrono::milliseconds timeout) -{ - std::unique_lock lock(mutex_); - return ready_cv_.wait_for(lock, timeout, [this] { return ready_; }) && valid_; -} - -MujocoMotor::MujocoMotor(const std::size_t joint_index, - std::string joint_name, - std::shared_ptr bridge, - const std::uint8_t node_id) - : AbstractMotor(node_id == 0 ? static_cast(joint_index + 1) : node_id), - joint_index_(joint_index), - bridge_(std::move(bridge)) -{ - if (!bridge_) { - CMVR_LOG(ERROR) << "MujocoMotor bridge is null"; - return; - } - if (joint_index_ >= bridge_->dof()) { - CMVR_LOG(ERROR) << "MujocoMotor joint index out of range"; - bridge_.reset(); - return; - } - - info_.id = id(); - info_.joint_name = std::move(joint_name); - id_ = info_.joint_name; -} - -void MujocoMotor::setMode(const msgs::RunMode mode) -{ - bridge_->setMode(joint_index_, mode); -} - -msgs::RunMode MujocoMotor::getMode() -{ - return bridge_->getMode(joint_index_); -} - -void MujocoMotor::setLimitQ(const double ub, const double lb) -{ - info_.limit_q_ub = ub; - info_.limit_q_lb = lb; -} - -void MujocoMotor::setLimitQd(const double qd) -{ - info_.limit_qd = qd; -} - -void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd) -{ - limit_qdd_upper_ = u_qdd; - limit_qdd_lower_ = l_qdd; -} - -void MujocoMotor::brake() -{ - bridge_->setMode(joint_index_, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - bridge_->setPositionTarget(joint_index_, bridge_->position(joint_index_), 0.0); -} - -void MujocoMotor::setQ(const double q) -{ - bridge_->setPositionTarget(joint_index_, q, 0.0); -} - -void MujocoMotor::setTarget(const double q, const double qd) -{ - bridge_->setPositionTarget(joint_index_, q, qd); -} - -void MujocoMotor::setTarget(const double qd) -{ - bridge_->setVelocityTarget(joint_index_, qd); -} - -bool MujocoMotor::calibrateZeroQ() -{ - return true; -} - -bool MujocoMotor::reachedTargetQ() -{ - return true; -} - -void MujocoMotor::setQd(const double qd) -{ - bridge_->setVelocityTarget(joint_index_, qd); -} - -double MujocoMotor::getQ() -{ - return bridge_->position(joint_index_); -} - -double MujocoMotor::getQd() -{ - return bridge_->velocity(joint_index_); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt b/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt deleted file mode 100644 index 8585f5c6..00000000 --- a/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -add_library(ti5motor SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_sdo_response.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_tpdo1.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_tpdo2.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_rpdo1.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_rpdo2.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/canopen/ti5_motor_canopen_protocol.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/ti5_motor.cpp - -) - - -target_include_directories(ti5motor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -add_library(cmvr_es::device::ti5motor ALIAS ti5motor) - -target_link_libraries(ti5motor - PRIVATE - cmvr_es::device::canbus - cmvr_es::proto - glog -) - -install(TARGETS ti5motor LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/ti5_motor/ti5_motor.cpp b/cmvr-es/devices/motor/ti5_motor/ti5_motor.cpp deleted file mode 100644 index f6925239..00000000 --- a/cmvr-es/devices/motor/ti5_motor/ti5_motor.cpp +++ /dev/null @@ -1,5 +0,0 @@ -// -// Created by lgv on 2025/8/1. -// - -#include "ti5_motor.h" \ No newline at end of file diff --git a/cmvr-es/manager/device_manager/CMakeLists.txt b/cmvr-es/manager/device_manager/CMakeLists.txt index d1547b13..c43db8d8 100644 --- a/cmvr-es/manager/device_manager/CMakeLists.txt +++ b/cmvr-es/manager/device_manager/CMakeLists.txt @@ -13,8 +13,9 @@ target_link_libraries(device_manager PRIVATE cmvr_es::device::microphone cmvr_es::device::dexhand cmvr::device::head_esp32 - cmvr_es::device::motor_system + cmvr_es::device::motor_manager cmvr_es::device::arm + cmvr_es::mujoco_viewer osqp # cmvr_es::device::aubo_robot ) diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index f80bc179..d680dded 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -51,6 +51,7 @@ namespace cmvr::device { void log_device_plan_() const; void pre_scan_robot_arm_dependencies_() const; void init_devices_(); + void configure_mujoco_viewer_pip_(); }; } // cmvr diff --git a/cmvr-es/manager/device_manager/src/device_factory.cpp b/cmvr-es/manager/device_manager/src/device_factory.cpp index ccb5b564..b1bfe45e 100644 --- a/cmvr-es/manager/device_manager/src/device_factory.cpp +++ b/cmvr-es/manager/device_manager/src/device_factory.cpp @@ -10,7 +10,7 @@ #include "devices/camera/camera_factory.h" #include "devices/dexhand/dexhand_factory.h" #include "devices/microphone/microphone_factory.h" -#include "devices/motor/motor_system/include/motor_system.h" +#include "devices/motor/manager/include/motor_manager.h" #include "devices/speaker/speaker_factory.h" #include "common/config/config_files.h" @@ -21,8 +21,11 @@ #include "cmvr/config/dexhand_config/dexhand_config.pb.h" #include "cmvr/config/microphone_config/microphone_config.pb.h" #include "cmvr/config/motor_config/motor_config.pb.h" +#include "cmvr/config/mujoco_config/mujoco_world_config.pb.h" #include "cmvr/config/speaker_config/speaker_config.pb.h" #include "common/base/logging/logger.h" +#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" using namespace std; using namespace cmvr::device; @@ -225,11 +228,11 @@ DeviceFactory::DeviceFactory() registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM, [](const auto& entry) { if (entry.id().empty()) { - CMVR_LOG(ERROR) << "[DeviceFactory]: MotorSystem id is required"; + CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required"; return DeviceRecord{}; } if (entry.config_file().empty()) { - CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorSystem ID: " << entry.id(); + CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorManager ID: " << entry.id(); return DeviceRecord{}; } config::MotorRootConfig root_cfg; @@ -238,7 +241,7 @@ DeviceFactory::DeviceFactory() return DeviceRecord{}; } CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success"; - auto device = std::make_shared(entry.id(), root_cfg.motor()); + auto device = std::make_shared(entry.id(), root_cfg.motor()); DeviceRecord record; record.id = entry.id(); record.kind = device->kind(); @@ -249,6 +252,30 @@ DeviceFactory::DeviceFactory() registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM, createRobotArmRecord); + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD, + [](const auto& entry) { + return createFactoryRecordFromCollection( + entry, + [](const auto& root) -> decltype(auto) { + return (root.worlds()); + }, + [](const auto& cfg) { + return std::make_shared(cfg); + }); + }); + + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_VIEWER, + [](const auto& entry) { + return createFactoryRecordFromCollection( + entry, + [](const auto& root) -> decltype(auto) { + return (root.viewers()); + }, + [](const auto& cfg) { + return std::make_shared(cfg); + }); + }); + registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_BIO_HEAD_ROBOT, [](const auto& entry) { return createSingleDeviceRecord( diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index ef889f6d..d957afb4 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -10,11 +10,13 @@ #include "devices/battery/abstract_battery.h" #include "devices/biohead/abstract_biohead.h" #include "devices/camera/abstract_camera.h" +#include "devices/camera/mujoco_camera/include/mujoco_camera.h" #include "devices/dexhand/abstract_dexhand.h" #include "devices/gripper/abstract_gripper.h" #include "devices/microphone/abstract_microphone.h" -#include "devices/motor/motor_system/include/motor_system.h" +#include "devices/motor/manager/include/motor_manager.h" #include "devices/speaker/abstract_speaker.h" +#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" #include "common/config/config_files.h" #include "cmvr/config/arm_config/arm_config.pb.h" #include "cmvr/config/motor_config/motor_config.pb.h" @@ -48,12 +50,27 @@ const char* deviceTypeToString(const cmvr::config::DeviceConfigEntry::DeviceType return "DEVICE_TYPE_SPEAKER"; case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_AGV: return "DEVICE_TYPE_AGV"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD: + return "DEVICE_TYPE_MUJOCO_WORLD"; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_VIEWER: + return "DEVICE_TYPE_MUJOCO_VIEWER"; case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN: default: return "DEVICE_TYPE_UNKNOWN"; } } +bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group, + const std::string& joint_name) +{ + for (const auto& motor : motor_group.motors().motors()) { + if (motor.joint_name() == joint_name) { + return true; + } + } + return false; +} + } // namespace template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); @@ -65,7 +82,8 @@ template std::shared_ptr DeviceManager::getDevice(const std::st template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); std::shared_ptr DeviceManager::instance_ = nullptr; @@ -81,6 +99,7 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { pre_scan_robot_arm_dependencies_(); logSection("Initialize Devices"); init_devices_(); + configure_mujoco_viewer_pip_(); } DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) { @@ -102,7 +121,7 @@ DeviceManager& DeviceManager::getInstance() { void DeviceManager::destroyInstance() { std::lock_guard lock(init_mutex_); instance_.reset(); - MotorSystem::clearActiveJoints(); + MotorManager::clearActiveJoints(); } void DeviceManager::start(){ @@ -246,11 +265,11 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const continue; } if (entry.id().empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorSystem device id is empty"; + CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty"; return; } if (entry.config_file().empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorSystem config_file is empty: " << entry.id(); + CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id(); return; } @@ -260,7 +279,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const return; } if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) { - CMVR_LOG(ERROR) << "[DeviceManager]: MotorSystem entry id '" << entry.id() + CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id() << "' does not match config id '" << root_cfg.motor().id() << "'"; return; } @@ -324,7 +343,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const const auto motor_root_it = motor_roots.find(motor_config.motor_system_id()); if (motor_root_it == motor_roots.end()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() - << "' depends on disabled or missing MotorSystem: " + << "' depends on disabled or missing MotorManager: " << motor_config.motor_system_id(); return; } @@ -352,13 +371,8 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const if (allowed_groups.count(group_id) == 0) { continue; } - for (const auto& motor : motor_group.motors()) { - if (motor.joint_name() == joint_name) { - matched_group = group_id; - break; - } - } - if (!matched_group.empty()) { + if (motorGroupHasJoint(motor_group, joint_name)) { + matched_group = group_id; break; } } @@ -373,9 +387,9 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } } - MotorSystem::clearActiveJoints(); + MotorManager::clearActiveJoints(); for (auto& [motor_system_id, group_selection] : selections) { - MotorSystem::setActiveJoints(motor_system_id, std::move(group_selection)); + MotorManager::setActiveJoints(motor_system_id, std::move(group_selection)); } } @@ -423,3 +437,40 @@ void DeviceManager::init_devices_() { devices_.emplace(record.id, std::move(record)); } } + +void DeviceManager::configure_mujoco_viewer_pip_() +{ + for (const auto& [camera_id, camera_record] : devices_) { + if (camera_record.type_name != "MujocoCamera") { + continue; + } + + auto camera = std::dynamic_pointer_cast(camera_record.device); + if (!camera) { + continue; + } + + const auto& camera_config = camera->config(); + if (!camera_config.viewer_pip().enable()) { + continue; + } + + for (const auto& [viewer_id, viewer_record] : devices_) { + if (viewer_record.type_name != "MujocoViewerDevice") { + continue; + } + + auto viewer = getDevice(viewer_id); + if (viewer && viewer->setPiPCameraConfig(camera_config)) { + camera->setFetchRgbdFn([viewer](std::vector& rgb, + std::vector& depth, + int& width, + int& height, + uint64_t& frame_id) { + return viewer->getPiPCameraRGBD(rgb, depth, width, height, frame_id); + }); + break; + } + } + } +} diff --git a/docs/ethercat_motor_tutorial.md b/docs/ethercat_motor_tutorial.md new file mode 100644 index 00000000..5720c0bc --- /dev/null +++ b/docs/ethercat_motor_tutorial.md @@ -0,0 +1,237 @@ +# EtherCAT 电机接入教程 + +这份文档只说明新增一种 EtherCAT 电机需要改哪里、怎么写。 + +## 1. 增加 vendor + +修改 `protos/cmvr/config/motor_config/motor_config.proto`： + +```proto +enum MotorVendor { + MOTOR_VENDOR_UNKNOWN = 0; + MOTOR_VENDOR_TI5 = 1; + MOTOR_VENDOR_MUJOCO = 2; + MOTOR_VENDOR_XXX = 3; +} +``` + +`MOTOR_VENDOR_XXX` 改成真实厂商名，例如 `MOTOR_VENDOR_FOO`。不要复用 `TI5`。 + +## 2. 写电机配置 + +新增配置文件： + +```text +cmvr-es/config/devices/motor/ethercat_motors.pb.txt +``` + +示例： + +```proto +motor { + id: "ethercat_motors" + + motor_groups { + id: "right_arm_ethercat" + bus_type: MOTOR_BUS_ETHERCAT + vendor: MOTOR_VENDOR_XXX + protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402 + tool_frame: "R_FINGER_TIP" + + ethercat { + master_id: "eth0" + cycle_us: 1000 + slaves { motor_id: 1 slave_index: 0 vendor_id: 0x00000000 product_code: 0x00000000 } + slaves { motor_id: 2 slave_index: 1 vendor_id: 0x00000000 product_code: 0x00000000 } + slaves { motor_id: 3 slave_index: 2 vendor_id: 0x00000000 product_code: 0x00000000 } + slaves { motor_id: 4 slave_index: 3 vendor_id: 0x00000000 product_code: 0x00000000 } + slaves { motor_id: 5 slave_index: 4 vendor_id: 0x00000000 product_code: 0x00000000 } + slaves { motor_id: 6 slave_index: 5 vendor_id: 0x00000000 product_code: 0x00000000 } + slaves { motor_id: 7 slave_index: 6 vendor_id: 0x00000000 product_code: 0x00000000 } + } + + 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" } + motors { id: 2 joint_name: "R_SHOULDER_R" } + motors { id: 3 joint_name: "R_SHOULDER_Y" } + motors { id: 4 joint_name: "R_ELBOW_R" } + motors { id: 5 joint_name: "R_WRIST_P" } + motors { id: 6 joint_name: "R_WRIST_Y" } + motors { id: 7 joint_name: "R_WRIST_R" } + } + } +} +``` + +`motors.motors.id` 是系统内的电机逻辑 id。`ethercat.slaves.motor_id` 必须和它对应。 + +`joint_limits` 按 `joint_name` 读取，和 CAN、MuJoCo 电机保持同一个风格。 + +## 3. 注册设备 + +修改： + +```text +cmvr-es/config/manager/device_manager.pb.txt +``` + +加入： + +```proto +devices { + id: "ethercat_motors" + type: MOTOR_SYSTEM + enable: true + config_file: "devices/motor/ethercat_motors.pb.txt" +} +``` + +## 4. 机械臂使用 EtherCAT group + +修改机械臂配置，例如： + +```text +cmvr-es/config/devices/arm/arm.pb.txt +``` + +把 motor backend 改成： + +```proto +motor { + motor_system_id: "ethercat_motors" + motor_group_ids: "right_arm_ethercat" + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" +} +``` + +## 5. 实现 bus runtime + +EtherCAT 总线资源放在： + +```text +cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h +cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp +``` + +`EthercatMotorBusRuntime` 负责： + +```text +读取 ethercat 配置 +初始化 EtherCAT master +扫描/校验 slave_index、vendor_id、product_code +启动 cyclic loop +保存 command/feedback buffer +停止 cyclic loop +``` + +bus runtime 不创建具体电机，也不关心厂商；它只保存总线连接、线程和数据缓存。 + +## 6. 增加具体电机 driver + +新增目录： + +```text +cmvr-es/devices/motor/drivers/xxx_ethercat/ + CMakeLists.txt + include/xxx_ethercat_motor.h + include/xxx_ethercat_motor_protocol.h + src/xxx_ethercat_motor.cpp + src/xxx_ethercat_motor_protocol.cpp +``` + +`XxxEthercatMotor` 继承 `AbstractMotor`。 + +`XxxEthercatMotorProtocol` 继承 `MotorProtocolInterface`，把 `setTarget`、`setQd`、`getQ` 等接口转换成 EtherCAT command/feedback。 + +## 7. 在 MotorManager 里创建 EtherCAT 电机 + +修改： + +```text +cmvr-es/devices/motor/manager/src/motor_manager.cpp +``` + +在 `MotorManager::createEthercatMotors_()` 里按 `vendor + protocol` 创建具体电机： + +```cpp +auto ethercat_bus_runtime = + std::dynamic_pointer_cast(bus_runtime); + +if (group_cfg.vendor() == config::MOTOR_VENDOR_XXX && + group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { + auto protocol = std::make_shared(ethercat_bus_runtime); + + std::vector> motors; + motors.reserve(motor_cfgs.size()); + for (const auto& cfg : motor_cfgs) { + auto motor = std::make_shared(cfg); + motor->setProtocol(protocol); + if (!motor->init()) { + return {}; + } + motors.push_back(std::move(motor)); + } + return motors; +} +``` + +## 8. 加入 CMake + +修改： + +```text +cmvr-es/devices/motor/manager/CMakeLists.txt +``` + +给 `motor_manager` 链接新增的具体 EtherCAT 电机 target。 + +修改： + +```text +cmvr-es/devices/motor/CMakeLists.txt +``` + +加入： + +```cmake +add_subdirectory(drivers/xxx_ethercat) +``` + +## 9. 验证配置 + +先验证 proto： + +```bash +./output/bin/protoc \ + --encode=cmvr.config.MotorRootConfig \ + -I protos \ + protos/cmvr/config/motor_config/motor_config.proto \ + < cmvr-es/config/devices/motor/ethercat_motors.pb.txt \ + > /tmp/ethercat_motors.pb.bin +``` + +再编译： + +```bash +cmake --build cmake-build-debug --target mujoco_manual_ui_test +``` + +真机联调时先只验证初始化日志：master 打开、slave 数量、vendor/product 校验、cyclic loop 启动、每个 motor 注册成功。然后再下发运动命令。 diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index f69fae94..976f7d26 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/protos/cmvr/config/arm_config/arm_config.proto @@ -1,12 +1,11 @@ syntax = "proto3"; package cmvr.config; -import "cmvr/common/geometry.proto"; -import "cmvr/config/joint_limits_config.proto"; import "cmvr/config/lawba_ik_config.proto"; import "cmvr/config/pinocchio_dls_ik_config.proto"; import "cmvr/config/pinocchio_qp_ik_config.proto"; import "cmvr/config/srs_ik_config.proto"; +import "cmvr/config/cartesian_motion_validation_config.proto"; enum ToppraPathType { TOPPRA_PATH_TYPE_UNKNOWN = 0; @@ -45,23 +44,6 @@ message VendorRobotArmBackendConfig { string password = 10; } -message QpJointLimitAvoidanceConfig { - bool enable = 1; - double margin_ratio = 2; - double gain = 3; - double max_push = 4; - double weight = 5; -} - -message CartesianVelocityQpConfig { - JointLimitsConfig joint_limits = 1; - .cmvr.common.Vec6 twist_tracking_weight = 2; - double qdot_regularization = 3; - double prev_qdot_regularization = 4; - double solver_eps = 5; - QpJointLimitAvoidanceConfig joint_limit_avoidance = 6; -} - message SpeedLPlannerConfig { double linear_velocity_max = 1; double linear_acceleration_max = 2; @@ -69,16 +51,14 @@ message SpeedLPlannerConfig { double angular_velocity_max = 4; double angular_acceleration_max = 5; double angular_jerk_max = 6; - repeated double joint_acceleration_max = 7; double linear_target_replan_threshold = 8; double angular_target_replan_threshold = 9; double linear_reverse_cos_threshold = 10; double linear_reverse_switch_speed_threshold = 11; - double normal_direction_deviation_deg = 12; - double mild_direction_deviation_deg = 13; - double severe_direction_deviation_deg = 14; - double linear_min_speed_ratio = 15; - CartesianVelocityQpConfig qp = 16; + optional bool enforce_joint_acceleration_limits = 17; + CartesianLineDeviationCheckConfig line_deviation_check = 18; + JointVelocityCheckConfig joint_velocity_check = 19; + CartesianVelocityFeasibilityCheckConfig cartesian_velocity_feasibility_check = 20; } message CartesianVelocityControllerConfig { @@ -86,6 +66,7 @@ message CartesianVelocityControllerConfig { double stop_twist_norm = 2; double stop_command_velocity_norm = 3; double stop_measured_velocity_norm = 4; + double stop_acceleration = 5; } message ToppraJointMotionPlannerConfig { @@ -105,13 +86,14 @@ message MoveLPlannerConfig { double sample_period_s = 1; double position_gain = 2; double rotation_gain = 3; - CartesianVelocityQpConfig qp = 4; + CartesianLineDeviationCheckConfig line_deviation_check = 4; + JointContinuityCheckConfig joint_continuity_check = 5; + CartesianStepFeasibilityCheckConfig cartesian_step_feasibility_check = 6; } message MoveLConfig { oneof algorithm { - MoveLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1; - MoveLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2; + MoveLPlannerConfig pinocchio_cartesian_motion_planner = 1; } } @@ -123,8 +105,7 @@ message SpeedLControllerConfig { message SpeedLConfig { oneof algorithm { - SpeedLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1; - SpeedLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2; + SpeedLPlannerConfig pinocchio_cartesian_motion_planner = 1; } SpeedLControllerConfig speed_l_controller = 10; @@ -142,13 +123,11 @@ message ArmKinematicsConfig { message ArmMotionConfig { MoveJConfig move_j = 1; MoveLConfig move_l = 2; - reserved 3; SpeedLConfig speed_l = 4; } message RobotArmConfig { string id = 1; - reserved 2; oneof backend { MotorRobotArmBackendConfig motor = 10; 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 21a36747..c238d2bf 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -19,6 +19,8 @@ message DeviceConfigEntry { DEVICE_TYPE_MICROPHONE = 15; DEVICE_TYPE_SPEAKER = 16; DEVICE_TYPE_AGV = 17; + DEVICE_TYPE_MUJOCO_WORLD = 18; + DEVICE_TYPE_MUJOCO_VIEWER = 19; } string id = 1; diff --git a/protos/cmvr/config/joint_limits_config.proto b/protos/cmvr/config/joint_limits_config.proto index 319eed63..c3da751b 100644 --- a/protos/cmvr/config/joint_limits_config.proto +++ b/protos/cmvr/config/joint_limits_config.proto @@ -2,7 +2,7 @@ syntax = "proto3"; package cmvr.config; enum JointLimitSource { - JOINT_LIMIT_SOURCE_UNKNOWN = 0; + JOINT_LIMIT_SOURCE_UNSPECIFIED = 0; JOINT_LIMIT_SOURCE_URDF = 1; JOINT_LIMIT_SOURCE_CUSTOM = 2; } @@ -19,4 +19,25 @@ message JointLimitsConfig { JointLimitSource source = 1; repeated JointLimitConfig joints = 2; string urdf_path = 3; + bool enable = 4; +} + +message JointSoftLimitConfig { + bool enable = 1; + double margin_ratio = 2; + double min_margin_rad = 3; +} + +message JointLimitAvoidanceConfig { + bool enable = 1; + double gain = 2; + double margin_ratio = 3; + double max_push = 4; + double weight = 5; +} + +message JointLimitPolicyConfig { + JointLimitsConfig limits = 1; + JointSoftLimitConfig soft_limit = 2; + JointLimitAvoidanceConfig avoidance = 3; } diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index c049b087..e6d88215 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -3,7 +3,7 @@ package cmvr.config; import "cmvr/config/joint_limits_config.proto"; -message Ti5MotorConfig { +message MotorConfigItem { int32 id = 1; string joint_name = 2; double limit_q_lb = 3; @@ -12,6 +12,17 @@ message Ti5MotorConfig { double limit_qdd = 6; } +message MotorList { + repeated MotorConfigItem motors = 1; +} + +message EthercatSlaveConfig { + int32 motor_id = 1; + int32 slave_index = 2; + uint32 vendor_id = 3; + uint32 product_code = 4; +} + message SocketCanConfig { string dev_id = 1; int32 channel_id = 2; @@ -20,6 +31,11 @@ message SocketCanConfig { message EtherCATConfig { string master_id = 1; int32 cycle_us = 2; + repeated EthercatSlaveConfig slaves = 10; +} + +message MujocoMotorGroupConfig { + string world_id = 1; } enum MotorBusType { @@ -29,18 +45,34 @@ enum MotorBusType { MOTOR_BUS_MUJOCO = 3; } +enum MotorVendor { + MOTOR_VENDOR_UNKNOWN = 0; + MOTOR_VENDOR_TI5 = 1; + MOTOR_VENDOR_MUJOCO = 2; +} + +enum MotorProtocol { + MOTOR_PROTOCOL_UNKNOWN = 0; + MOTOR_PROTOCOL_CANOPEN = 1; + MOTOR_PROTOCOL_ETHERCAT_CIA402 = 2; + MOTOR_PROTOCOL_MUJOCO = 3; +} + message MotorGroupConfig { string id = 1; MotorBusType bus_type = 2; - string tool_frame = 3; + MotorVendor vendor = 3; + MotorProtocol protocol = 4; + string tool_frame = 5; oneof bus_config { SocketCanConfig can = 10; EtherCATConfig ethercat = 11; + MujocoMotorGroupConfig mujoco = 12; } - repeated Ti5MotorConfig motors = 20; JointLimitsConfig joint_limits = 30; + MotorList motors = 40; } message MotorConfig {