diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index 1fb3350e..54ae96de 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -40,16 +40,16 @@ bufferSize="50" verbose="false"> - - - - - - - - + + + + + + + + - + @@ -58,7 +58,12 @@ - + + + + + + diff --git a/python/vision_servo/robot_warpper_test.py b/python/vision_servo/robot_warpper_test.py index 3d9fcc86..8369c10a 100644 --- a/python/vision_servo/robot_warpper_test.py +++ b/python/vision_servo/robot_warpper_test.py @@ -16,28 +16,7 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") -# JOINT_POSITIONS = [ -# [-0.1756037771702, 1.1800090074539, 1.8522456884384, 1.5761110782623, -2.7331359386444, 0.2563314139843, -0.0606026798487], -# [-0.1923076510429, 1.1099290847778, 1.8832376003265, 1.7514026165009, -2.6219944953918, 0.1934144645929, 0.2889180481434], -# [0.2704087197781, 0.9557962417603, 1.7136577367783, 1.6711657047272, -2.5147924423218, 0.0584972538054, 0.783962905407], -# [0.383531242609, 0.7108500599861, 1.7350907325745, 1.8691725730896, -2.2797479629517, -0.1232690215111, 1.2245988845825], -# [0.3922453224659, 0.7737803459167, 1.8068689107895, 1.6336191892624, -2.1118586063385, -0.0338453501463, 1.1426001787186], -# [0.2443187087774, 0.7818641066551, 1.9731577634811, 1.5233805179596, -1.872123837471, 0.0405574627221, 1.1054704189301], -# [0.1276659220457, 0.9156659245491, 2.0081288814545, 1.4481414556503, -1.8519257307053, 0.2532368600368, 0.8394842743874], -# [0.1861536949873, 1.0141937732697, 1.8355123996735, 1.1459348201752, -2.2625010013580, 0.5969776511192, 0.2645072638988], -# [-0.0192848723382, 0.9415585398674, 1.8732609748840, 1.1809715032578, -2.6499755382538, 0.6354467868805, -0.2003904730082], -# [-0.1747295111418, 1.0453623533249, 1.8939573764801, 1.4446462392807, -2.6536157131195, 0.5147834420204, -0.1335265636444], -# [-0.0191576723009, 1.0618592500687, 1.8731262683868, 1.4747705459595, -2.4312825202942, 0.3830471336842, 0.2654716968536], -# [0.1436645090580, 0.8961057662964, 1.9278227090836, 1.5483351945877, -2.1520030498505, 0.2300240248442, 0.7717508077621], -# [-0.2458887547255, 1.3182551860809, 1.8035455942154, 1.7302685976028, -2.8433899879456, 0.0447540767491, -0.0295310281217], -# [-0.1628440171480, 1.1346882581711, 1.7724473476410, 1.8686875104904, -2.8621222972870, 0.0011371960863, 0.3351055085659], -# [0.2227641940117, 0.9846931695938, 1.5547094345093, 1.9238842725754, -2.8571910858154, -0.1052865162492, 0.8961741328239], -# [0.4523145258427, 0.7738097310066, 1.5341449975967, 1.9425786733627, -2.5897336006165, -0.1147941574454, 1.2599183320999], -# [0.6323996782303, 0.7300637960434, 1.3266047239304, 2.0460503101349, -2.7566838264465, -0.1049704179168, 1.4412622451782], -# [0.6435400247574, 0.7136778831482, 1.3097128868103, 2.0483627319336, -2.7511403560638, -0.1215622797608, 1.5219746828079], -# [0.0131992585957, 1.1347451210022, 1.4958723783493, 1.7922105789185, -3.1400320529938, -0.2474720925093, 0.4145260155201], -# [-0.1823624074459, 1.4246475696564, 1.7136197090149, 1.6952623128891, -2.9659118652344, -0.1499162465334, -0.0364842526615], -# ] + # # # while True: @@ -48,19 +27,21 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") # js = robot.getJointQ('right') # print(js) # 控制左臂关节 -# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) +# robot.calibrateZeroQ("R_WRIST_Y") +# robot.calibrateZeroQ("R_WRIST_R") +# time.sleep(3) +robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # # -# time.sleep(200) -# robot.calibrateZeroQ("R_WRIST_Y") -# robot.calibrateZeroQ("R_WRIST_R") +time.sleep(10) + # # robot.torqueOff("R_WRIST_R") # time.sleep(10) # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # time.sleep(20) # # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) -# # robot.torqueOff() +# robot.torqueOn() # # time.sleep(500) # robot.torqueOff("WAIST_P") # time.sleep(30) diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 831fd804..22ce27e6 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -37,28 +37,56 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { CSC_buffer_ = make_shared >(cfg.getAttrDefault("bufferSize", 50)); + // 定义 lambda 函数，用于读取 XML 节点 enable 属性 + auto readEnable = [](const XmlNode &node) -> bool { + std::string enable_str = node.getAttrString("enable"); // 默认 false + return (enable_str == "true" || enable_str == "1"); + }; auto can_cfg = cfg.getChild("CanManger"); auto l_can_cfg = can_cfg.getChild("LeftArmCan"); - l_motors_cfg_ = l_can_cfg.getChildren("Motor"); - l_can_client_ = std::make_shared(l_can_cfg); - l_can_sender_ = std::make_shared >(); - l_can_receiver_ = std::make_shared >(); - l_message_manager_ = std::make_shared >(); + left_arm_enabled_ = readEnable(l_can_cfg); + + if (left_arm_enabled_) { + l_motors_cfg_ = l_can_cfg.getChildren("Motor"); + l_can_client_ = std::make_shared(l_can_cfg); + l_can_sender_ = std::make_shared >(); + l_can_receiver_ = std::make_shared >(); + l_message_manager_ = std::make_shared >(); + } + auto r_can_cfg = can_cfg.getChild("RightArmCan"); - r_motors_cfg_ = r_can_cfg.getChildren("Motor"); - r_can_client_ = std::make_shared(r_can_cfg); - r_can_sender_ = std::make_shared >(); - r_can_receiver_ = std::make_shared >(); - r_message_manager_ = std::make_shared >(); + right_arm_enabled_ = readEnable(r_can_cfg); + if (right_arm_enabled_) { + r_motors_cfg_ = r_can_cfg.getChildren("Motor"); + r_can_client_ = std::make_shared(r_can_cfg); + r_can_sender_ = std::make_shared >(); + r_can_receiver_ = std::make_shared >(); + r_message_manager_ = std::make_shared >(); + } + auto waist_can_cfg = can_cfg.getChild("WaistCan"); - waist_motors_cfg_ = waist_can_cfg.getChildren("Motor"); - waist_can_client_ = std::make_shared(waist_can_cfg); - waist_can_sender_ = std::make_shared >(); - waist_can_receiver_ = std::make_shared >(); - waist_message_manager_ = std::make_shared >(); + waist_enabled_ = readEnable(waist_can_cfg); + if (waist_enabled_) { + waist_motors_cfg_ = waist_can_cfg.getChildren("Motor"); + waist_can_client_ = std::make_shared(waist_can_cfg); + waist_can_sender_ = std::make_shared >(); + waist_can_receiver_ = std::make_shared >(); + waist_message_manager_ = std::make_shared >(); + } + + auto head_can_cfg = can_cfg.getChild("HeadCan"); + head_enabled_ = readEnable(head_can_cfg); + if (head_enabled_) { + head_motors_cfg_ = head_can_cfg.getChildren("Motor"); + head_can_client_ = std::make_shared(head_can_cfg); + head_can_sender_ = std::make_shared >(); + head_can_receiver_ = std::make_shared >(); + head_message_manager_ = std::make_shared >(); + } + upd_timer_ = make_shared(); upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), @@ -72,150 +100,100 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { template void HumanoidRobot::init() { - // 1 === 初始化公共组件 === - l_can_client_->init(); - r_can_client_->init(); - waist_can_client_->init(); - auto ret = l_can_sender_->Init(l_can_client_.get(), false); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can sender."; - } - ret = r_can_sender_->Init(r_can_client_.get(), false); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can sender."; - } - ret = waist_can_sender_->Init(waist_can_client_.get(), false); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can sender."; - } + struct Limb { + std::string name; + bool enabled; + std::shared_ptr client; + std::shared_ptr > sender; + std::shared_ptr > receiver; + std::shared_ptr > message_manager; + std::vector motor_cfgs; + }; + std::vector limbs{ + { + "WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_, + waist_motors_cfg_ + }, + { + "LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_, + l_motors_cfg_ + }, + { + "RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_, + r_motors_cfg_ + }, + { + "HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_, + head_motors_cfg_ + } + }; - ret = l_can_receiver_->Init(l_can_client_.get(), l_message_manager_.get(), false); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can receiver."; - } - ret = r_can_receiver_->Init(r_can_client_.get(), r_message_manager_.get(), false); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can receiver."; - } - ret = waist_can_receiver_->Init(waist_can_client_.get(), waist_message_manager_.get(), false); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to init can receiver."; - } - - - // 2 === 启动通讯 === - l_can_client_->start(); - ret = l_can_sender_->Start(); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can sender."; - } - - r_can_client_->start(); - ret = r_can_sender_->Start(); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can sender."; - } - - waist_can_client_->start(); - ret = waist_can_sender_->Start(); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can sender."; - } - - ret = l_can_receiver_->Start(); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can receiver."; - } - - ret = r_can_receiver_->Start(); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can receiver."; - } - - ret = waist_can_receiver_->Start(); - if (ret != ErrorCode::OK) { - LOG(ERROR) << "Failed to start can receiver."; - } - - // 3 == 创建协议 === - auto l_canopen_protocol = std::make_shared(l_can_sender_, l_message_manager_); - auto r_canopen_protocol = std::make_shared(r_can_sender_, r_message_manager_); - auto waist_canopen_protocol = std::make_shared(waist_can_sender_, waist_message_manager_); - - // 4 === 创建 MotorManager === + // 创建 MotorManager motor_manager_ = std::make_shared(); - // for (const auto& cfg : r_motors_cfg_) { - // auto motor = std::make_shared(cfg); - // motor->setProtocol(r_canopen_protocol); - // motor->init(); // 耗时操作 - // motor_manager_->addMotor(motor); - // } - // - // for (const auto& cfg : l_motors_cfg_) { - // auto motor = std::make_shared(cfg); - // motor->setProtocol(l_canopen_protocol); - // motor->init(); // 耗时操作 - // motor_manager_->addMotor(motor); - // } + std::vector > tasks; - // 5 === 并行创建电机 === - auto left_task = std::async(std::launch::async, [&] { - LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing LEFT motors..."; - for (const auto &cfg: l_motors_cfg_) { - auto motor = std::make_shared(cfg); - motor->setProtocol(l_canopen_protocol); - motor->init(); - motor_manager_->addMotor(motor); + for (auto &limb: limbs) { + if (!limb.enabled) continue; + + + if (limb.client) limb.client->init(); + + // 2. 初始化 Sender / Receiver（如果有） + if (limb.sender && limb.receiver && limb.client) { + auto ret = limb.sender->Init(limb.client.get(), false); + if (ret != ErrorCode::OK) + LOG(ERROR) << "Failed to init " << limb.name << " CAN sender."; + + ret = limb.receiver->Init(limb.client.get(), limb.message_manager.get(), false); + if (ret != ErrorCode::OK) + LOG(ERROR) << "Failed to init " << limb.name << " CAN receiver."; + + limb.client->start(); + ret = limb.sender->Start(); + if (ret != ErrorCode::OK) + LOG(ERROR) << "Failed to start " << limb.name << " CAN sender."; + + ret = limb.receiver->Start(); + if (ret != ErrorCode::OK) + LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver."; } - }); - auto right_task = std::async(std::launch::async, [&] { - LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing RIGHT motors..."; - for (const auto &cfg: r_motors_cfg_) { - auto motor = std::make_shared(cfg); - motor->setProtocol(r_canopen_protocol); - motor->init(); - motor_manager_->addMotor(motor); + // 3. 创建协议（如果有CAN） + std::shared_ptr protocol = nullptr; + if (limb.sender && limb.message_manager) { + protocol = std::make_shared(limb.sender, limb.message_manager); } - }); - auto waist_task = std::async(std::launch::async, [&] { - LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing waist motors..."; - for (const auto &cfg: waist_motors_cfg_) { - auto motor = std::make_shared(cfg); - motor->setProtocol(waist_canopen_protocol); - motor->init(); - motor_manager_->addMotor(motor); + // 4. 并行初始化电机 + if (!limb.motor_cfgs.empty()) { + tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] { + LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name << + " motors..."; + for (const auto &cfg: limb.motor_cfgs) { + auto motor = std::make_shared(cfg); + if (protocol) motor->setProtocol(protocol); + motor->init(); + motor_manager_->addMotor(motor); + } + })); } - }); + } - // 等待两个线程完成 - left_task.get(); - right_task.get(); - waist_task.get(); - - motor_manager_->getMotor("WAIST_Y")->setQ(0); - motor_manager_->getMotor("WAIST_P")->setQ(0); + // 等待所有任务完成 + for (auto &task: tasks) task.get(); rsm_.store(ROBOT_ESTOP); - LOG(INFO) << "All motors initialized successfully."; + LOG(INFO) << "All enabled motors initialized successfully."; } template void HumanoidRobot::torqueOff() { try { - if (rsm_.load() == ROBOT_RUNNING) { - throw runtime_error("robot is running"); - } - - if (rsm_.load() != ROBOT_TOROFF) { - for (const auto &pair: motor_manager_->motorsMap()) { - if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P" ) - pair.second->torqueOff(); - } - rsm_.store(ROBOT_TOROFF); + for (const auto &pair: motor_manager_->motorsMap()) { + if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P") + pair.second->torqueOff(); } } catch (std::exception &e) { throw runtime_error(e.what()); @@ -227,30 +205,7 @@ template HumanoidRobot::~HumanoidRobot() { // TODO: close can interfaces upd_timer_->stop(); - - std::vector cmd = { - // {"L_SHOULDER_P", 0.0}, - // {"L_SHOULDER_R", -1.31873}, - // {"L_SHOULDER_Y", 0.0}, - // {"L_ELBOW_R", -0.537621}, - // {"L_WRIST_P", 0.0}, - // {"L_WRIST_Y", 0.000183204}, - // {"L_WRIST_R", 0.0225797}, - - {"R_SHOULDER_P", -0.0201069}, - {"R_SHOULDER_R", 1.46698}, - {"R_SHOULDER_Y", 1.45894}, - {"R_ELBOW_R", 0.159681}, - {"R_WRIST_P", 0.0808349}, - {"R_WRIST_Y", -0.138279}, - {"R_WRIST_R", -0.243169}, - - {"WAIST_Y", 0}, - {"WAIST_P", 0} - }; - - // this->moveJ(cmd,0.8); - this->torqueOff(); + // this->torqueOff(); } template @@ -264,10 +219,10 @@ std::vector HumanoidRobot::getJointNames() { } template -std::unordered_map HumanoidRobot::getJointQ() const{ +std::unordered_map HumanoidRobot::getJointQ() const { std::unordered_map joint_qs; - for (const auto &pair : motor_manager_->motorsMap()) { + for (const auto &pair: motor_manager_->motorsMap()) { auto motor = pair.second; joint_qs[motor->jointName()] = motor->getQ(); } @@ -276,7 +231,7 @@ std::unordered_map HumanoidRobot::getJointQ() const{ template void HumanoidRobot::getJointQ(std::unordered_map &joint_qs) const { - for (auto &pair : joint_qs) { + for (auto &pair: joint_qs) { auto motor = motor_manager_->getMotor(pair.first); if (motor) { pair.second = motor->getQ(); @@ -287,20 +242,19 @@ void HumanoidRobot::getJointQ(std::unordered_map &join } - template std::vector HumanoidRobot::getLinkNames() { return link_names_; } template -void HumanoidRobot::getJointsState(std::vector& states) { +void HumanoidRobot::getJointsState(std::vector &states) { try { lock_guard lock(exec_mtx_); states.clear(); JointState state; - for (const auto &pair : motor_manager_->motorsMap()) { + for (const auto &pair: motor_manager_->motorsMap()) { auto motor = pair.second; state.name = motor->jointName(); state.position = motor->getQ(); @@ -317,7 +271,6 @@ void HumanoidRobot::getState(RobotState &state) { try { lock_guard lock(exec_mtx_); // TODO: copy m_state_ date into state - } catch (exception &e) { throw runtime_error(e.what()); } @@ -342,65 +295,50 @@ void HumanoidRobot::torqueOff(const std::string &joint_name) { template void HumanoidRobot::eStop() { - if (rsm_.load() != ROBOT_ESTOP) { - CSP_buffer_->clear(); - CSV_buffer_->clear(); - CSC_buffer_->clear(); - for (const auto &pair: motor_manager_->motorsMap()) { - pair.second->brake(); - } - rsm_.store(ROBOT_ESTOP); + CSP_buffer_->clear(); + CSV_buffer_->clear(); + CSC_buffer_->clear(); + for (const auto &pair: motor_manager_->motorsMap()) { + pair.second->brake(); } } template void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { try { - if (rsm_.load() == ROBOT_RUNNING) { - flash_cmd_.store(true); - eStop(); - } - if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) { - rsm_.store(ROBOT_RUNNING); + for (const auto &j: cmd) { + auto motor = motor_manager_->getMotor(j.joint_name); + if (motor != nullptr) { + // PPM 模式下 这个实际速度会超30% 左右 + motor->setQd(vel); + if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { + motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + } + motor->setQ(j.rad); + } + } + + //3. wait for completion + bool completion = true; + do { + completion = true; for (const auto &j: cmd) { auto motor = motor_manager_->getMotor(j.joint_name); if (motor != nullptr) { - // PPM 模式下 这个实际速度会超30% 左右 - motor->setQd(vel); - - if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { - motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + if (!motor->reachedTargetQ()) { + completion = false; + break; } - motor->setQ(j.rad); } } - - //3. wait for completion - bool completion = true; - do { - completion = true; - for (const auto &j: cmd) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - if (!motor->reachedTargetQ()) { - completion = false; - break; - } - } - } - // 4. while waiting, check flash_cmd_, if it is true, set it false then exit - if (flash_cmd_.load()) { - flash_cmd_.store(false); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } while (!completion); - - rsm_.store(ROBOT_ESTOP); - } else { - throw runtime_error("rsm invalid"); - } + // 4. while waiting, check flash_cmd_, if it is true, set it false then exit + if (flash_cmd_.load()) { + flash_cmd_.store(false); + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } while (!completion); } catch (exception &e) { throw runtime_error(e.what()); } @@ -413,405 +351,359 @@ void HumanoidRobot::calibrateZeroQ(const std::string &joint_name) { } template -void HumanoidRobot::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) { +void HumanoidRobot::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, + double acc) { + try { + // update m_state_ + Eigen::Vector q_init; + auto q_map = getJointQ(); + q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], + q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], + q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], + q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - try { - if (rsm_.load() == ROBOT_RUNNING) { - flash_cmd_.store(true); - eStop(); + // LOG(INFO) << "q_init: " << q_init; + m_state_->SetQ(q_init); + m_robot_->ComputeForwardKinematics(m_state_); + + + Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); + T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); + // 输入为弧度 + T_target(0, 3) = pose.position().x(); + T_target(1, 3) = pose.position().y(); + T_target(2, 3) = pose.position().z(); + + cmvr::ctrl::PoseTarget target; + target.T_target = T_target; + target.w_posrot = 0.5; + target.weight = 1.0; + target.link_name = ee_link; + + // slove ik + Eigen::Vector q_cmd; + bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, + ctrl::CartesianController::Mode::Position, + q_cmd, 10000, 1e-6); + if (!ok) { + throw runtime_error("solve IK failed"); } - if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) { - rsm_.store(ROBOT_RUNNING); + std::vector joint_points{ + {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, + {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, + {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, + {"R_WRIST_R", q_cmd[13]} + }; + for (const auto &j: joint_points) { + auto motor = motor_manager_->getMotor(j.joint_name); + if (motor != nullptr) { + // PPM 模式下 这个实际速度会超30% 左右 - - // update m_state_ - Eigen::Vector q_init; - auto q_map = getJointQ(); - q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - - // LOG(INFO) << "q_init: " << q_init; - m_state_->SetQ(q_init); - m_robot_->ComputeForwardKinematics(m_state_); - - - Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); - T_target.block<3,3>(0,0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); // 输入为弧度 - T_target(0,3) = pose.position().x(); - T_target(1,3) = pose.position().y(); - T_target(2,3) = pose.position().z(); - - cmvr::ctrl::PoseTarget target; - target.T_target = T_target; - target.w_posrot = 0.5; - target.weight = 1.0; - target.link_name = ee_link; - - // slove ik - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController::Mode::Position, - q_cmd, 10000, 1e-6); - if (!ok) { - throw runtime_error("solve IK failed"); + if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { + motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + } + motor->setQd(vel); + motor->setQ(j.rad); } - std::vector joint_points{ - {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, - {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, - {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, - {"R_WRIST_R", q_cmd[13]} - }; + } + + //3. wait for completion + bool completion = true; + do { + completion = true; for (const auto &j: joint_points) { auto motor = motor_manager_->getMotor(j.joint_name); if (motor != nullptr) { - // PPM 模式下 这个实际速度会超30% 左右 - - if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { - motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); - } - motor->setQd(vel); - motor->setQ(j.rad); - } - } - - //3. wait for completion - bool completion = true; - do { - completion = true; - for (const auto &j: joint_points) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - if (!motor->reachedTargetQ()) { - completion = false; - break; - } - } - } - // 4. while waiting, check flash_cmd_, if it is true, set it false then exit - if (flash_cmd_.load()) { - flash_cmd_.store(false); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } while (!completion); - rsm_.store(ROBOT_READY); - } else { - throw runtime_error("rsm invalid"); - } - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - - -template -void HumanoidRobot::moveJ_IK(const std::string &base_link, const std::vector &targets, double vel, - double acc) { - try { - if (rsm_.load() == ROBOT_RUNNING) { - flash_cmd_.store(true); - eStop(); - } - if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) { - rsm_.store(ROBOT_RUNNING); - - - // update m_state_ - Eigen::Vector q_init; - auto q_map = getJointQ(); - q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - - LOG(INFO) << "q_init: " << q_init; - m_state_->SetQ(q_init); - m_robot_->ComputeForwardKinematics(m_state_); - - // slove ik - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController::Mode::Position, - q_cmd, 10000, 1e-6); - if (!ok) { - throw runtime_error("solve IK failed"); - } - std::vector joint_points{ - {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, - {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, - {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, - {"R_WRIST_R", q_cmd[13]} - }; - // for (const auto &j: joint_points) { - // auto motor = motor_manager_->getMotor(j.joint_name); - // if (motor != nullptr) { - // // PPM 模式下 这个实际速度会超30% 左右 - // motor->setQd(vel); - // if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { - // motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); - // } - // motor->setQ(j.rad); - // } - // } - // - // //3. wait for completion - // bool completion = true; - // do { - // completion = true; - // for (const auto &j: joint_points) { - // auto motor = motor_manager_->getMotor(j.joint_name); - // if (motor != nullptr) { - // if (!motor->reachedTargetQ()) { - // completion = false; - // break; - // } - // } - // } - // // 4. while waiting, check flash_cmd_, if it is true, set it false then exit - // if (flash_cmd_.load()) { - // flash_cmd_.store(false); - // return; - // } - // std::this_thread::sleep_for(std::chrono::milliseconds(2)); - // } while (!completion); - rsm_.store(ROBOT_READY); - } else { - throw runtime_error("rsm invalid"); - } - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - -template -void HumanoidRobot::moveL(std::string &base_link, std::vector &targets, double vel, double acc) { - try { - if (rsm_.load() == ROBOT_RUNNING) { - flash_cmd_.store(true); - eStop(); - return; - } - if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) { - rsm_.store(ROBOT_RUNNING); - - // 获取当前关节状态 - Eigen::Vector q_init; - auto q_map = getJointQ(); - q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - - LOG(INFO) << "q_init: " << q_init; - m_state_->SetQ(q_init); - m_robot_->ComputeForwardKinematics(m_state_); - - // 获取基座链接索引 - auto base_idx = m_robot_->GetLinkIdx(base_link); - - // 获取当前末端位姿 - 使用前向运动学计算 - std::vector current_poses; - for (const auto& target : targets) { - auto ee_idx = m_robot_->GetLinkIdx(target.link_name); - - // 使用正向运动学计算当前位姿 - Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); - current_poses.push_back(T); - - // 打印当前末端执行器的 XYZ 和欧拉角 - if (&target == &targets.front()) { - Eigen::Vector3d position = T.block<3, 1>(0, 3); - Eigen::Matrix3d rotation = T.block<3, 3>(0, 0); - Eigen::Vector3d euler = rotationMatrixToEulerZYX(rotation); - - LOG(INFO) << "Starting point (Initial position): " - << "X: " << position[0] << ", Y: " << position[1] << ", Z: " << position[2]; - LOG(INFO) << "Starting orientation (Euler angles): " - << "RX: " << euler[0] << ", RY: " << euler[1] << ", RZ: " << euler[2]; - } - } - - // 计算最大距离和插值点数 - double max_distance = 0.0; - for (size_t i = 0; i < targets.size(); i++) { - Eigen::Vector3d current_pos = current_poses[i].block<3, 1>(0, 3); - Eigen::Vector3d target_pos = targets[i].T_target.block<3, 1>(0, 3); - double distance = (target_pos - current_pos).norm(); - max_distance = std::max(max_distance, distance); - } - - // 基于速度和距离计算插值点数 - double move_time = max_distance / vel; - int num_points = static_cast(move_time * 100); // 100Hz控制频率 - - // 存储所有插值点的关节角度 - std::vector> joint_trajectory; - joint_trajectory.reserve(num_points + 1); - - // 记录上一次成功的关节角度 - Eigen::Vector last_success_q = q_init; - - // 预先计算所有插值点的逆运动学 - for (int i = 0; i <= num_points; i++) { - if (flash_cmd_.load()) { - flash_cmd_.store(false); - rsm_.store(ROBOT_READY); - return; - } - - double t = static_cast(i) / num_points; - - // 创建插值后的目标（只做位置插值，旋转保持不变） - std::vector interpolated_targets = targets; - for (size_t j = 0; j < targets.size(); j++) { - // 位置线性插值 - Eigen::Vector3d current_pos = current_poses[j].block<3, 1>(0, 3); - Eigen::Vector3d target_pos = targets[j].T_target.block<3, 1>(0, 3); - Eigen::Vector3d interp_pos = current_pos + t * (target_pos - current_pos); - - // 保持旋转不变 - Eigen::Matrix3d current_rot_matrix = current_poses[j].block<3, 3>(0, 0); - interpolated_targets[j].T_target.setIdentity(); - interpolated_targets[j].T_target.block<3, 3>(0, 0) = current_rot_matrix; - interpolated_targets[j].T_target.block<3, 1>(0, 3) = interp_pos; - } - - // 求解逆运动学 - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002, - ctrl::CartesianController::Mode::Position, - q_cmd, 10000, 1e-6); - - if (!ok) { - LOG(WARNING) << "IK failed at point " << i << ", using last successful configuration"; - q_cmd = last_success_q; - } else { - last_success_q = q_cmd; - } - - joint_trajectory.push_back(q_cmd); - - // 获取当前末端执行器的位置 (通过正向运动学) - m_state_->SetQ(q_cmd); - m_robot_->ComputeForwardKinematics(m_state_); - - // 获取当前末端执行器的位姿 (变换矩阵 T) - auto ee_idx = m_robot_->GetLinkIdx(targets[0].link_name); - Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); - - // 从变换矩阵中提取 XYZ 坐标 - Eigen::Vector3d end_effector_pos = T.block<3, 1>(0, 3); - - // 打印 IK 解算出的 XYZ 位置 - if (i % 10 == 0) { // 每10个点打印一次，避免日志过多 - LOG(INFO) << "IK solution at point " << i << " : " - << "X: " << end_effector_pos[0] << ", Y: " << end_effector_pos[1] << ", Z: " << end_effector_pos[2]; - } - } - - // 执行轨迹 - for (int i = 0; i <= num_points; i++) { - if (flash_cmd_.load()) { - flash_cmd_.store(false); - break; - } - - // 获取当前时间点的关节角度 - Eigen::Vector q_cmd = joint_trajectory[i]; - - // 发送关节命令 - std::vector joint_points{ - {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, - {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, - {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, - {"R_WRIST_R", q_cmd[13]} - }; - - // 设置每个关节的速度和位置 - for (size_t j = 0; j < joint_points.size(); j++) { - auto& joint_point = joint_points[j]; - auto motor = motor_manager_->getMotor(joint_point.joint_name); - if (motor != nullptr) { - motor->setQ(joint_point.rad); - } - } - - // 等待一段时间，控制频率 - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - // 等待最终位置到达 - 检查所有关节 - bool completion = true; - do { - completion = true; - for (const auto& name : joint_names_) { - auto motor = motor_manager_->getMotor(name); - if (motor != nullptr && !motor->reachedTargetQ()) { + if (!motor->reachedTargetQ()) { completion = false; break; } } - if (flash_cmd_.load()) { - flash_cmd_.store(false); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } while (!completion); + } + // 4. while waiting, check flash_cmd_, if it is true, set it false then exit + if (flash_cmd_.load()) { + flash_cmd_.store(false); + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } while (!completion); + } catch (exception &e) { + throw runtime_error(e.what()); + } +} - rsm_.store(ROBOT_READY); - } else { - throw std::runtime_error("rsm invalid"); + +template +void HumanoidRobot::moveJ_IK(const std::string &base_link, const std::vector &targets, + double vel, + double acc) { + try { + // update m_state_ + Eigen::Vector q_init; + auto q_map = getJointQ(); + q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], + q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], + q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], + q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; + + LOG(INFO) << "q_init: " << q_init; + m_state_->SetQ(q_init); + m_robot_->ComputeForwardKinematics(m_state_); + + // slove ik + Eigen::Vector q_cmd; + bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController::Mode::Position, + q_cmd, 10000, 1e-6); + if (!ok) { + throw runtime_error("solve IK failed"); } + std::vector joint_points{ + {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, + {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, + {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, + {"R_WRIST_R", q_cmd[13]} + }; + // for (const auto &j: joint_points) { + // auto motor = motor_manager_->getMotor(j.joint_name); + // if (motor != nullptr) { + // // PPM 模式下 这个实际速度会超30% 左右 + // motor->setQd(vel); + // if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { + // motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + // } + // motor->setQ(j.rad); + // } + // } + // + // //3. wait for completion + // bool completion = true; + // do { + // completion = true; + // for (const auto &j: joint_points) { + // auto motor = motor_manager_->getMotor(j.joint_name); + // if (motor != nullptr) { + // if (!motor->reachedTargetQ()) { + // completion = false; + // break; + // } + // } + // } + // // 4. while waiting, check flash_cmd_, if it is true, set it false then exit + // if (flash_cmd_.load()) { + // flash_cmd_.store(false); + // return; + // } + // std::this_thread::sleep_for(std::chrono::milliseconds(2)); + // } while (!completion); + } catch (exception &e) { + throw runtime_error(e.what()); + } +} + +template +void HumanoidRobot::moveL(std::string &base_link, std::vector &targets, double vel, + double acc) { + try { + // 获取当前关节状态 + Eigen::Vector q_init; + auto q_map = getJointQ(); + q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], + q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], + q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], + q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; + + LOG(INFO) << "q_init: " << q_init; + m_state_->SetQ(q_init); + m_robot_->ComputeForwardKinematics(m_state_); + + // 获取基座链接索引 + auto base_idx = m_robot_->GetLinkIdx(base_link); + + // 获取当前末端位姿 - 使用前向运动学计算 + std::vector current_poses; + for (const auto &target: targets) { + auto ee_idx = m_robot_->GetLinkIdx(target.link_name); + + // 使用正向运动学计算当前位姿 + Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); + current_poses.push_back(T); + + // 打印当前末端执行器的 XYZ 和欧拉角 + if (&target == &targets.front()) { + Eigen::Vector3d position = T.block<3, 1>(0, 3); + Eigen::Matrix3d rotation = T.block<3, 3>(0, 0); + Eigen::Vector3d euler = rotationMatrixToEulerZYX(rotation); + + LOG(INFO) << "Starting point (Initial position): " + << "X: " << position[0] << ", Y: " << position[1] << ", Z: " << position[2]; + LOG(INFO) << "Starting orientation (Euler angles): " + << "RX: " << euler[0] << ", RY: " << euler[1] << ", RZ: " << euler[2]; + } + } + + // 计算最大距离和插值点数 + double max_distance = 0.0; + for (size_t i = 0; i < targets.size(); i++) { + Eigen::Vector3d current_pos = current_poses[i].block<3, 1>(0, 3); + Eigen::Vector3d target_pos = targets[i].T_target.block<3, 1>(0, 3); + double distance = (target_pos - current_pos).norm(); + max_distance = std::max(max_distance, distance); + } + + // 基于速度和距离计算插值点数 + double move_time = max_distance / vel; + int num_points = static_cast(move_time * 100); // 100Hz控制频率 + + // 存储所有插值点的关节角度 + std::vector > joint_trajectory; + joint_trajectory.reserve(num_points + 1); + + // 记录上一次成功的关节角度 + Eigen::Vector last_success_q = q_init; + + // 预先计算所有插值点的逆运动学 + for (int i = 0; i <= num_points; i++) { + if (flash_cmd_.load()) { + flash_cmd_.store(false); + rsm_.store(ROBOT_READY); + return; + } + + double t = static_cast(i) / num_points; + + // 创建插值后的目标（只做位置插值，旋转保持不变） + std::vector interpolated_targets = targets; + for (size_t j = 0; j < targets.size(); j++) { + // 位置线性插值 + Eigen::Vector3d current_pos = current_poses[j].block<3, 1>(0, 3); + Eigen::Vector3d target_pos = targets[j].T_target.block<3, 1>(0, 3); + Eigen::Vector3d interp_pos = current_pos + t * (target_pos - current_pos); + + // 保持旋转不变 + Eigen::Matrix3d current_rot_matrix = current_poses[j].block<3, 3>(0, 0); + interpolated_targets[j].T_target.setIdentity(); + interpolated_targets[j].T_target.block<3, 3>(0, 0) = current_rot_matrix; + interpolated_targets[j].T_target.block<3, 1>(0, 3) = interp_pos; + } + + // 求解逆运动学 + Eigen::Vector q_cmd; + bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002, + ctrl::CartesianController::Mode::Position, + q_cmd, 10000, 1e-6); + + if (!ok) { + LOG(WARNING) << "IK failed at point " << i << ", using last successful configuration"; + q_cmd = last_success_q; + } else { + last_success_q = q_cmd; + } + + joint_trajectory.push_back(q_cmd); + + // 获取当前末端执行器的位置 (通过正向运动学) + m_state_->SetQ(q_cmd); + m_robot_->ComputeForwardKinematics(m_state_); + + // 获取当前末端执行器的位姿 (变换矩阵 T) + auto ee_idx = m_robot_->GetLinkIdx(targets[0].link_name); + Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); + + // 从变换矩阵中提取 XYZ 坐标 + Eigen::Vector3d end_effector_pos = T.block<3, 1>(0, 3); + + // 打印 IK 解算出的 XYZ 位置 + if (i % 10 == 0) { + // 每10个点打印一次，避免日志过多 + LOG(INFO) << "IK solution at point " << i << " : " + << "X: " << end_effector_pos[0] << ", Y: " << end_effector_pos[1] << ", Z: " << end_effector_pos + [2]; + } + } + + // 执行轨迹 + for (int i = 0; i <= num_points; i++) { + if (flash_cmd_.load()) { + flash_cmd_.store(false); + break; + } + + // 获取当前时间点的关节角度 + Eigen::Vector q_cmd = joint_trajectory[i]; + + // 发送关节命令 + std::vector joint_points{ + {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, + {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, + {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, + {"R_WRIST_R", q_cmd[13]} + }; + + // 设置每个关节的速度和位置 + for (size_t j = 0; j < joint_points.size(); j++) { + auto &joint_point = joint_points[j]; + auto motor = motor_manager_->getMotor(joint_point.joint_name); + if (motor != nullptr) { + motor->setQ(joint_point.rad); + } + } + + // 等待一段时间，控制频率 + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // 等待最终位置到达 - 检查所有关节 + bool completion = true; + do { + completion = true; + for (const auto &name: joint_names_) { + auto motor = motor_manager_->getMotor(name); + if (motor != nullptr && !motor->reachedTargetQ()) { + completion = false; + break; + } + } + if (flash_cmd_.load()) { + flash_cmd_.store(false); + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } while (!completion); } catch (std::exception &e) { - rsm_.store(ROBOT_ESTOP); throw std::runtime_error(e.what()); } } - - template void HumanoidRobot::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) { try { - if (rsm_.load() == ROBOT_RUNNING) { - flash_cmd_.store(true); - eStop(); - } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) { - rsm_.store(ROBOT_RUNNING); - - // 获取电机控制对象 - // 这里的控制函数需要根据你的实际实现来进行填充 - // 获取目标关节的电机 - auto motor = motor_manager_->getMotor(joint_name); - if (motor == nullptr) { - throw runtime_error("Motor not found for joint: " + joint_name); - } - - - // 设置电机的运行模式为速度模式 - if (motor->getMode() != msgs::RUN_MODE_PROFILE_VELOCITY) { - motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); - } - - // 设置加速度和目标速度 - motor->setQd(vel); - - - LOG(INFO) << "开始在关节 " << joint_name << " 上进行速度控制，速度：" << vel << " rad/s，加速度：" << acc << " rad/s²"; - std::this_thread::sleep_for(std::chrono::seconds(10)); - motor->setQd(0); - LOG(INFO) << "速度控制完成，电机已停止。"; - - - - - // 运动完成后不立即将 rsm_ 置为 READY，防止误操作 - } else { - throw runtime_error("无效的机器人状态，无法进行速度控制"); + // 获取电机控制对象 + // 这里的控制函数需要根据你的实际实现来进行填充 + // 获取目标关节的电机 + auto motor = motor_manager_->getMotor(joint_name); + if (motor == nullptr) { + throw runtime_error("Motor not found for joint: " + joint_name); } + + + // 设置电机的运行模式为速度模式 + if (motor->getMode() != msgs::RUN_MODE_PROFILE_VELOCITY) { + motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); + } + + // 设置加速度和目标速度 + motor->setQd(vel); + + + LOG(INFO) << "开始在关节 " << joint_name << " 上进行速度控制，速度：" << vel << " rad/s，加速度：" << acc << " rad/s²"; + std::this_thread::sleep_for(std::chrono::seconds(10)); + motor->setQd(0); + LOG(INFO) << "速度控制完成，电机已停止。"; + + + // 运动完成后不立即将 rsm_ 置为 READY，防止误操作 } catch (exception &e) { - rsm_.store(ROBOT_ESTOP); // 出错时，设置为紧急停止状态 LOG(ERROR) << "speedJ 控制失败: " << e.what(); throw runtime_error("speedJ 控制失败: " + string(e.what())); } @@ -824,8 +716,8 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di } try { - const double CONTROL_PERIOD = 1.0 / 100; // 控制周期保持不变 - const size_t MAX_QUEUE_SIZE = 10; // 队列最大缓存的轨迹点数量，防止内存溢出 + const double CONTROL_PERIOD = 1.0 / 100; // 控制周期保持不变 + const size_t MAX_QUEUE_SIZE = 10; // 队列最大缓存的轨迹点数量，防止内存溢出 // 定义基座和末端执行器链接 std::string base_link = "PELVIS_S"; @@ -903,16 +795,16 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di time_points, distance_ratios); LOG(INFO) << "speedL: Planning trajectory - points=" << num_points - << ", total distance=" << direction.norm() << "m, move time=" << move_time << "s"; + << ", total distance=" << direction.norm() << "m, move time=" << move_time << "s"; // 创建轨迹队列及同步机制 - std::queue, Eigen::Vector>> trajectory_queue; + std::queue, Eigen::Vector > > trajectory_queue; std::mutex queue_mutex; std::condition_variable queue_cv; - std::atomic planning_completed{false}; // 规划是否完成 - std::atomic execution_failed{false}; // 执行是否失败 - std::atomic planned_points{0}; // 已规划的点数 - std::atomic executed_points{0}; // 已执行的点数 + std::atomic planning_completed{false}; // 规划是否完成 + std::atomic execution_failed{false}; // 执行是否失败 + std::atomic planned_points{0}; // 已规划的点数 + std::atomic executed_points{0}; // 已执行的点数 // 获取当前关节位置（初始点） auto q_map_current = getJointQ(); @@ -925,7 +817,7 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di { std::lock_guard lock(queue_mutex); trajectory_queue.push({q_current, Eigen::Vector::Zero()}); - planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量 + planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量 } // 启动控制执行子线程（先启动子线程） @@ -937,25 +829,27 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di // 循环条件使用load()读取原子变量 while (!planning_completed.load() || !trajectory_queue.empty() && !execution_failed.load()) { // 从队列中获取轨迹点 - std::pair, Eigen::Vector> point; + std::pair, Eigen::Vector > point; bool has_point = false; { std::unique_lock lock(queue_mutex); // 等待队列中有数据或规划完成，使用load()读取原子变量 if (queue_cv.wait_for(lock, std::chrono::milliseconds(500), - [&] { return !trajectory_queue.empty() || planning_completed.load() || execution_failed.load(); })) { - + [&] { + return !trajectory_queue.empty() || planning_completed.load() || + execution_failed.load(); + })) { if (!trajectory_queue.empty()) { point = trajectory_queue.front(); trajectory_queue.pop(); has_point = true; - executed_points.store(executed_points.load() + 1); // 使用store和load操作原子变量 + executed_points.store(executed_points.load() + 1); // 使用store和load操作原子变量 } } else { // 超时，可能规划线程出现问题 LOG(WARNING) << "控制线程等待轨迹点超时"; - execution_failed.store(true); // 使用store设置原子变量 + execution_failed.store(true); // 使用store设置原子变量 break; } } @@ -964,7 +858,7 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di // 计算当前点的期望执行时间，确保按时间规划执行 auto current_time = std::chrono::high_resolution_clock::now(); std::chrono::duration elapsed = current_time - start_time; - double expected_time = (executed_points.load() - 1) * CONTROL_PERIOD; // 使用load读取原子变量 + double expected_time = (executed_points.load() - 1) * CONTROL_PERIOD; // 使用load读取原子变量 // 如果执行过快，等待到期望时间 if (elapsed.count() < expected_time) { @@ -985,32 +879,33 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di motor->setQd(point.second[j]); motor->setQ(point.first[j]); - LOG(INFO) << "执行点 " << executed_points.load() // 使用load读取原子变量 - << ": joint[" << joint_names_[j] << "] = " - << point.first[j]; + LOG(INFO) << "执行点 " << executed_points.load() // 使用load读取原子变量 + << ": joint[" << joint_names_[j] << "] = " + << point.first[j]; } } } } - if (execution_failed.load()) { // 使用load读取原子变量 + if (execution_failed.load()) { + // 使用load读取原子变量 LOG(ERROR) << "控制执行线程异常退出"; rsm_.store(ROBOT_ERROR); } else { - LOG(INFO) << "控制执行线程完成，共执行 " << executed_points.load() // 使用load读取原子变量 - << " 个轨迹点"; + LOG(INFO) << "控制执行线程完成，共执行 " << executed_points.load() // 使用load读取原子变量 + << " 个轨迹点"; rsm_.store(ROBOT_READY); } } catch (const std::exception &e) { LOG(ERROR) << "控制线程错误: " << e.what(); - execution_failed.store(true); // 使用store设置原子变量 + execution_failed.store(true); // 使用store设置原子变量 rsm_.store(ROBOT_ERROR); } }); // 主线程开始进行IK逆解和轨迹点规划（边规划边放入队列） try { - Eigen::Vector prev_q = q_current; // 上一个关节位置 + Eigen::Vector prev_q = q_current; // 上一个关节位置 double prev_time = 0.0; // 生成并规划轨迹点（从1开始，因为0已经作为初始点） @@ -1050,12 +945,12 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di Eigen::Vector q_next; bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, - ctrl::CartesianController::Mode::Position, - q_next, 10000, 1e-6); + ctrl::CartesianController::Mode::Position, + q_next, 10000, 1e-6); if (!ok) { LOG(WARNING) << "轨迹点 " << i << " IK求解失败，停止规划"; - execution_failed.store(true); // 使用store设置原子变量 + execution_failed.store(true); // 使用store设置原子变量 break; } @@ -1076,18 +971,19 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di return trajectory_queue.size() < MAX_QUEUE_SIZE || execution_failed.load(); }); - if (execution_failed.load()) { // 使用load读取原子变量 + if (execution_failed.load()) { + // 使用load读取原子变量 break; } trajectory_queue.push({q_next, q_vel}); - planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量 + planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量 prev_q = q_next; prev_time = time_points[i]; LOG(INFO) << "规划点 " << i << " 已加入队列，当前队列大小: " << trajectory_queue.size(); } - queue_cv.notify_one(); // 通知控制线程有新数据 + queue_cv.notify_one(); // 通知控制线程有新数据 // 简单的速率控制，避免规划过快，使用load读取原子变量 if (planned_points.load() - executed_points.load() > MAX_QUEUE_SIZE / 2) { @@ -1096,28 +992,28 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di } } catch (const std::exception &e) { LOG(ERROR) << "轨迹规划错误: " << e.what(); - execution_failed.store(true); // 使用store设置原子变量 + execution_failed.store(true); // 使用store设置原子变量 } // 规划完成，通知控制线程，使用store设置原子变量 planning_completed.store(true); queue_cv.notify_one(); - LOG(INFO) << "轨迹规划完成，共规划 " << planned_points.load() // 使用load读取原子变量 - << " 个轨迹点"; + LOG(INFO) << "轨迹规划完成，共规划 " << planned_points.load() // 使用load读取原子变量 + << " 个轨迹点"; // 等待控制线程完成 if (control_thread.joinable()) { control_thread.join(); } - if (execution_failed.load()) { // 使用load读取原子变量 + if (execution_failed.load()) { + // 使用load读取原子变量 LOG(ERROR) << "speedL执行失败"; rsm_.store(ROBOT_ERROR); throw std::runtime_error("speedL execution failed"); } else { LOG(INFO) << "speedL轨迹执行成功完成"; } - } catch (const std::exception &e) { LOG(ERROR) << "speedL失败: " << e.what(); rsm_.store(ROBOT_ERROR); @@ -1129,11 +1025,6 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di template void HumanoidRobot::followJointTrajectory(std::vector > &traj, double dt) { try { - // 1. 状态机检查：仅允许在 ESTOP/READY/TOROFF 状态启动 - if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY && rsm_.load() != ROBOT_TOROFF) { - throw runtime_error("followJointTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")"); - } - // 2. 轨迹合法性检查 if (!check_joint_traj_(traj, dt)) { throw runtime_error("followJointTrajectory: invalid trajectory"); @@ -1145,7 +1036,7 @@ void HumanoidRobot::followJointTrajectory(std::vectormotorsMap()) { + for (const auto &motor_pair: motor_manager_->motorsMap()) { auto motor = motor_pair.second; if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); @@ -1183,16 +1074,16 @@ void HumanoidRobot::followJointTrajectory(std::vectorgetMotor(joint.joint_name); if (motor) { // 优先使用轨迹点中的速度，若无则用默认速度（0.5 rad/s） double target_vel = (joint.vel > 0) ? joint.vel : 0.5; motor->setQd(target_vel); // 设置关节速度 - motor->setQ(joint.rad); // 设置关节目标位置 - LOG(INFO)<< "Joint " << joint.joint_name - << " -> pos=" << joint.rad << " rad, vel=" << target_vel << " rad/s"; + motor->setQ(joint.rad); // 设置关节目标位置 + LOG(INFO) << "Joint " << joint.joint_name + << " -> pos=" << joint.rad << " rad, vel=" << target_vel << " rad/s"; } } @@ -1205,7 +1096,6 @@ void HumanoidRobot::followJointTrajectory(std::vector void HumanoidRobot::followPoseTrajectory(std::string &base_link, std::vector > &targets, double dt) { try { - // 1. 基础校验：状态机与轨迹合法性 - if (rsm_.load() == ROBOT_RUNNING) { - flash_cmd_.store(true); - eStop(); // 中断当前运动 - throw runtime_error("followPoseTrajectory: robot is running, interrupted"); - } - if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY) { - throw runtime_error("followPoseTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")"); - } if (targets.empty()) { throw runtime_error("followPoseTrajectory: pose trajectory is empty"); } @@ -1247,26 +1128,28 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, // 2.1 获取当前关节角度（初始化机器人状态） Eigen::Vector q_current; auto q_map_current = getJointQ(); - q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"], + q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], + q_map_current["L_ELBOW_R"], q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], - q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"], + q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], + q_map_current["R_ELBOW_R"], q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; m_state_->SetQ(q_current); m_robot_->ComputeForwardKinematics(m_state_); // 更新当前正运动学状态 // 2.2 解算“轨迹第一个点”的关节配置（q_first，作为后续所有IK的初始值） - const auto& first_pose_targets = targets[0]; // 轨迹第一个点的位姿目标 + const auto &first_pose_targets = targets[0]; // 轨迹第一个点的位姿目标 Eigen::Vector q_first; // 轨迹第一个点的关节配置（IK基准） bool ik_first_ok = m_cctrl_->compute( - m_state_, // 当前机器人状态（作为IK初始值） - base_link, // 基座链接 - first_pose_targets, // 第一个点的位姿目标 - dt, // 控制周期（用于速度限制） + m_state_, // 当前机器人状态（作为IK初始值） + base_link, // 基座链接 + first_pose_targets, // 第一个点的位姿目标 + dt, // 控制周期（用于速度限制） ctrl::CartesianController::Mode::Position, // 位置控制模式 - q_first, // 输出：第一个点的关节配置 - 10000, // IK最大迭代次数（确保精度） - 1e-6 // IK位置精度（1mm/0.001°） + q_first, // 输出：第一个点的关节配置 + 10000, // IK最大迭代次数（确保精度） + 1e-6 // IK位置精度（1mm/0.001°） ); if (!ik_first_ok) { throw runtime_error("followPoseTrajectory: IK failed for the FIRST waypoint (unreachable target)"); @@ -1279,7 +1162,7 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, LOG(INFO) << "followPoseTrajectory: moving from current position to first waypoint..."; // 3.1 配置电机为CSP模式（用于过渡运动和后续轨迹） - for (const auto& motor_pair : motor_manager_->motorsMap()) { + for (const auto &motor_pair: motor_manager_->motorsMap()) { auto motor = motor_pair.second; if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); @@ -1296,11 +1179,12 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, // 3.2.1 计算当前应到达的关节位置（匀速插值） auto now = std::chrono::high_resolution_clock::now(); double elapsed = std::chrono::duration(now - transition_start_time).count(); - Eigen::Vector q_transition = q_current + (q_first - q_current) * std::min(elapsed * TRANSITION_VEL / (q_first - q_current).norm(), 1.0); + Eigen::Vector q_transition = q_current + (q_first - q_current) * std::min( + elapsed * TRANSITION_VEL / (q_first - q_current).norm(), 1.0); // 3.2.2 发送过渡运动关节指令 for (size_t i = 0; i < DOF; ++i) { - const std::string& joint_name = joint_names_[i]; + const std::string &joint_name = joint_names_[i]; auto motor = motor_manager_->getMotor(joint_name); if (motor) { motor->setQd(TRANSITION_VEL); // 过渡运动速度 @@ -1311,9 +1195,10 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, // 3.2.3 检查过渡运动是否完成（所有关节到达目标） transition_completed = true; for (size_t i = 0; i < DOF; ++i) { - const std::string& joint_name = joint_names_[i]; + const std::string &joint_name = joint_names_[i]; auto motor = motor_manager_->getMotor(joint_name); - if (motor && !motor->reachedTargetQ()) { // 精度阈值：0.0001rad（≈0.0057°） + if (motor && !motor->reachedTargetQ()) { + // 精度阈值：0.0001rad（≈0.0057°） transition_completed = false; break; } @@ -1367,7 +1252,7 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, } // 4.2.3 解算当前轨迹点的IK（关键：初始值固定为q_first） - const auto& current_pose_targets = targets[current_idx]; + const auto ¤t_pose_targets = targets[current_idx]; Eigen::Vector q_cmd; // 当前点的关节目标 // 临时更新机器人状态为q_first（确保IK初始值固定） @@ -1375,20 +1260,20 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, m_robot_->ComputeForwardKinematics(m_state_); bool ik_ok = m_cctrl_->compute( - m_state_, // IK初始值：固定为q_first - base_link, // 基座链接 - current_pose_targets, // 当前点的位姿目标 - dt, // 控制周期 + m_state_, // IK初始值：固定为q_first + base_link, // 基座链接 + current_pose_targets, // 当前点的位姿目标 + dt, // 控制周期 ctrl::CartesianController::Mode::Position, - q_cmd, // 输出：当前点的关节配置 - 5000, // 减少迭代次数（平衡精度与速度） - 5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽） + q_cmd, // 输出：当前点的关节配置 + 5000, // 减少迭代次数（平衡精度与速度） + 5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽） ); // 4.2.4 IK容错：失败时使用上一次有效配置 if (!ik_ok) { LOG(WARNING) << "followPoseTrajectory: IK failed at waypoint " << current_idx - << ", use last valid config (q_last_valid=" << last_valid_q.transpose() << ")"; + << ", use last valid config (q_last_valid=" << last_valid_q.transpose() << ")"; q_cmd = last_valid_q; } else { last_valid_q = q_cmd; // 更新有效配置 @@ -1397,13 +1282,13 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, // 4.2.5 发送当前点的关节指令（固定速度，可根据需求调整） const double TRAJ_VEL = 1.0; // 轨迹执行速度（1rad/s） for (size_t i = 0; i < DOF; ++i) { - const std::string& joint_name = joint_names_[i]; + const std::string &joint_name = joint_names_[i]; auto motor = motor_manager_->getMotor(joint_name); if (motor) { motor->setQd(TRAJ_VEL); // 轨迹执行速度 - motor->setQ(q_cmd[i]); // 关节目标位置 + motor->setQ(q_cmd[i]); // 关节目标位置 LOG(INFO) << "followPoseTrajectory: waypoint " << current_idx - << ", joint " << joint_name << " -> pos=" << q_cmd[i] << " rad"; + << ", joint " << joint_name << " -> pos=" << q_cmd[i] << " rad"; } } @@ -1416,7 +1301,6 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, while (!traj_completed.load()) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用 } - } catch (std::exception &e) { // 异常处理：重置状态机，确保机器人安全 rsm_.store(ROBOT_ESTOP); @@ -1446,7 +1330,6 @@ void HumanoidRobot::servoJ(std::vector &joints, double vel, dou for (const auto &j: joints) { auto motor = motor_manager_->getMotor(j.joint_name); if (motor != nullptr) { - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } @@ -1507,9 +1390,9 @@ void HumanoidRobot::servoJ(const std::string &base_link, const std::string } - template -void HumanoidRobot::servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) { +void HumanoidRobot::servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, + double vel, double acc) { try { // 1 : 计算当前位姿 auto cur_pose = fk(base_link, ee_link); @@ -1532,7 +1415,6 @@ void HumanoidRobot::servoDeltaJ(const std::string &base_link, const std::st } - template void HumanoidRobot::servoL(std::string &base_link, std::vector &targets, double dt) { try { @@ -1598,18 +1480,18 @@ template Eigen::Matrix3d HumanoidRobot::eulerZYXToRotationMatrix(double rx, double ry, double rz) { Eigen::Matrix3d R_x; R_x << 1, 0, 0, - 0, cos(rx), -sin(rx), - 0, sin(rx), cos(rx); + 0, cos(rx), -sin(rx), + 0, sin(rx), cos(rx); Eigen::Matrix3d R_y; R_y << cos(ry), 0, sin(ry), - 0, 1, 0, - -sin(ry), 0, cos(ry); + 0, 1, 0, + -sin(ry), 0, cos(ry); Eigen::Matrix3d R_z; R_z << cos(rz), -sin(rz), 0, - sin(rz), cos(rz), 0, - 0, 0, 1; + sin(rz), cos(rz), 0, + 0, 0, 1; return R_x * R_y * R_z; } @@ -1624,37 +1506,39 @@ Eigen::Vector3d HumanoidRobot::rotationMatrixToEulerZYX(const Eigen::Matrix // | -cx*sy*cz + sx*sz cx*sy*sz + sx*cz cx*cy | // 提取 ry（绕 Y 的角度） - ry = std::asin(R(0,2)); // R(0,2) = sin(ry) + ry = std::asin(R(0, 2)); // R(0,2) = sin(ry) double cy = std::cos(ry); if (std::abs(cy) > 1e-6) { // 正常情况 - rx = std::atan2(-R(1,2), R(2,2)); - rz = std::atan2(-R(0,1), R(0,0)); + rx = std::atan2(-R(1, 2), R(2, 2)); + rz = std::atan2(-R(0, 1), R(0, 0)); } else { // 万向节锁：cy ≈ 0 rx = 0; // 任意选择 if (ry > 0) { - rz = std::atan2(R(1,0), R(1,1)); + rz = std::atan2(R(1, 0), R(1, 1)); } else { - rz = std::atan2(-R(1,0), R(1,1)); + rz = std::atan2(-R(1, 0), R(1, 1)); } } return Eigen::Vector3d(rx, ry, rz); } + template -std::vector HumanoidRobot::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) { +std::vector HumanoidRobot< + DOF>::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) { // update m_state_ try { Eigen::Vector q_init; auto q_map = getJointQ(); q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; + q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], + q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], + q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; LOG(INFO) << "q_init: " << q_init; m_state_->SetQ(q_init); @@ -1662,31 +1546,31 @@ std::vector HumanoidRobot::ik(const std::string &base_link, const s Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); - T_target.block<3,3>(0,0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); // 输入为弧度 - T_target(0,3) = pose.position().x(); - T_target(1,3) = pose.position().y(); - T_target(2,3) = pose.position().z(); + T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); + // 输入为弧度 + T_target(0, 3) = pose.position().x(); + T_target(1, 3) = pose.position().y(); + T_target(2, 3) = pose.position().z(); cmvr::ctrl::PoseTarget target; target.T_target = T_target; target.w_posrot = 0.5; - target.weight = 1.0; + target.weight = 1.0; target.link_name = ee_link; // slove ik Eigen::Vector q_cmd{}; - bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController::Mode::Position, + bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, + ctrl::CartesianController::Mode::Position, q_cmd, 10000, 1e-6); if (!ok) { throw std::runtime_error("IK solve failed"); } else { return std::vector(q_cmd.data(), q_cmd.data() + q_cmd.size()); } - }catch (std::exception &e) { + } catch (std::exception &e) { throw runtime_error(e.what()); } - - } @@ -1699,9 +1583,9 @@ cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const st Eigen::Vector q; auto q_map = getJointQ(); // 类似 moveJ 中获取关节角度 q << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], - q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], - q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], - q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; + q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], + q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], + q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; // 更新状态并计算前向运动学 m_state_->SetQ(q); @@ -1709,23 +1593,22 @@ cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const st // 获取基座和末端索引 auto base_idx = m_robot_->GetLinkIdx(base_link); - auto ee_idx = m_robot_->GetLinkIdx(ee_link); + auto ee_idx = m_robot_->GetLinkIdx(ee_link); // 获取变换矩阵 Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); // 填充 Pose3d - pose.mutable_position()->set_x(T(0,3)); - pose.mutable_position()->set_y(T(1,3)); - pose.mutable_position()->set_z(T(2,3)); + pose.mutable_position()->set_x(T(0, 3)); + pose.mutable_position()->set_y(T(1, 3)); + pose.mutable_position()->set_z(T(2, 3)); // 将旋转矩阵转换为欧拉角 - Eigen::Matrix3d R = T.block<3,3>(0,0); + Eigen::Matrix3d R = T.block<3, 3>(0, 0); Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); // 你需要实现或已有此工具函数 pose.mutable_euler()->set_rx(euler(0)); pose.mutable_euler()->set_ry(euler(1)); pose.mutable_euler()->set_rz(euler(2)); - } catch (const std::exception &e) { throw std::runtime_error(std::string("FK计算失败: ") + e.what()); } @@ -1735,40 +1618,42 @@ cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const st void printTrajectoryInfo( - const std::vector& trajectory, - const std::vector& times, - const std::vector& velocities, + const std::vector &trajectory, + const std::vector ×, + const std::vector &velocities, double total_distance) { - std::cout << "\n===================================== 轨迹详细信息 =====================================" << std::endl; std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; std::cout << "总运动时间: " << std::fixed << std::setprecision(3) << times.back() << "s" << std::endl; std::cout << "轨迹点总数: " << trajectory.size() << " 个" << std::endl; - std::cout << "-----------------------------------------------------------------------------------------" << std::endl; + std::cout << "-----------------------------------------------------------------------------------------" << + std::endl; std::cout << std::setw(4) << "序号" << " | " - << std::setw(8) << "时间(s)" << " | " - << std::setw(10) << "x(m)" << " | " - << std::setw(10) << "y(m)" << " | " - << std::setw(10) << "z(m)" << " | " - << std::setw(12) << "速度(m/s)" << " | " - << std::setw(16) << "到起点距离(m)" << std::endl; - std::cout << "-----------------------------------------------------------------------------------------" << std::endl; + << std::setw(8) << "时间(s)" << " | " + << std::setw(10) << "x(m)" << " | " + << std::setw(10) << "y(m)" << " | " + << std::setw(10) << "z(m)" << " | " + << std::setw(12) << "速度(m/s)" << " | " + << std::setw(16) << "到起点距离(m)" << std::endl; + std::cout << "-----------------------------------------------------------------------------------------" << + std::endl; - Eigen::Vector3d start_pos(trajectory[0](0,3), trajectory[0](1,3), trajectory[0](2,3)); + Eigen::Vector3d start_pos(trajectory[0](0, 3), trajectory[0](1, 3), trajectory[0](2, 3)); for (size_t idx = 0; idx < trajectory.size(); ++idx) { - const auto& T = trajectory[idx]; - Eigen::Vector3d pos(T(0,3), T(1,3), T(2,3)); + const auto &T = trajectory[idx]; + Eigen::Vector3d pos(T(0, 3), T(1, 3), T(2, 3)); double dist_from_start = (pos - start_pos).norm(); std::cout << std::setw(4) << idx << " | " - << std::fixed << std::setprecision(3) << std::setw(8) << times[idx] << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.x() << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.y() << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.z() << " | " - << std::fixed << std::setprecision(6) << std::setw(12) << velocities[idx] << " | " - << std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl; + << std::fixed << std::setprecision(3) << std::setw(8) << times[idx] << " | " + << std::fixed << std::setprecision(6) << std::setw(10) << pos.x() << " | " + << std::fixed << std::setprecision(6) << std::setw(10) << pos.y() << " | " + << std::fixed << std::setprecision(6) << std::setw(10) << pos.z() << " | " + << std::fixed << std::setprecision(6) << std::setw(12) << velocities[idx] << " | " + << std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl; } - std::cout << "=========================================================================================\n" << std::endl; + std::cout << "=========================================================================================\n" << + std::endl; } @@ -1778,10 +1663,12 @@ void HumanoidRobot::moveDeltaL(const std::string &base_link, const std::str try { Eigen::Vector q_current_for_ik; auto q_map_current = getJointQ(); - q_current_for_ik << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"], - q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], - q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"], - q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; + q_current_for_ik << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], + q_map_current["L_ELBOW_R"], + q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], + q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], + q_map_current["R_ELBOW_R"], + q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; LOG(INFO) << "q_current_for_ik: " << q_current_for_ik; @@ -1789,8 +1676,8 @@ void HumanoidRobot::moveDeltaL(const std::string &base_link, const std::str msgs::Pose3d current_pose = fk(base_link, ee_link); LOG(INFO) << current_pose.mutable_position()->x() << " " << current_pose.mutable_position()->y() << " " - << current_pose.mutable_position()->z() << " " << current_pose.mutable_euler()->rx() << " " - << current_pose.mutable_euler()->ry() << " " << current_pose.mutable_euler()->rz(); + << current_pose.mutable_position()->z() << " " << current_pose.mutable_euler()->rx() << " " + << current_pose.mutable_euler()->ry() << " " << current_pose.mutable_euler()->rz(); // 2. 计算目标位姿 = 当前位姿 + 相对偏移（位置/姿态分别叠加） msgs::Pose3d target_pose; @@ -1803,7 +1690,6 @@ void HumanoidRobot::moveDeltaL(const std::string &base_link, const std::str // 3. 调用moveL执行直线运动到目标位姿 moveL(base_link, ee_link, target_pose, vel, acc); - } catch (const std::exception &e) { LOG(ERROR) << "moveDeltaL failed: " << e.what(); throw std::runtime_error(std::string("moveDeltaL error: ") + e.what()); @@ -1812,7 +1698,7 @@ void HumanoidRobot::moveDeltaL(const std::string &base_link, const std::str template void HumanoidRobot::moveL(const std::string &base_link, const std::string &ee_link, - msgs::Pose3d target_pose, double vel, double acc) { + msgs::Pose3d target_pose, double vel, double acc) { if (vel <= 0 || acc <= 0) { throw std::runtime_error("moveL: vel and acc must be positive"); } @@ -1844,10 +1730,12 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & // 2. 获取当前关节配置并验证目标可达性 Eigen::Vector q_current; auto q_map_current = getJointQ(); - q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"], - q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], - q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"], - q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; + q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], + q_map_current["L_ELBOW_R"], + q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], + q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], + q_map_current["R_ELBOW_R"], + q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; LOG(INFO) << "Current joint configuration: " << q_current; m_state_->SetQ(q_current); @@ -1858,13 +1746,13 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & target_ik_check.T_target = T_target; target_ik_check.T_target.block<3, 3>(0, 0) = start_orientation; // 使用起始姿态 target_ik_check.w_posrot = 0.5; - target_ik_check.weight = 1.0; + target_ik_check.weight = 1.0; target_ik_check.link_name = ee_link; Eigen::Vector q_cmd_check; bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002, - ctrl::CartesianController::Mode::Position, - q_cmd_check, 10000, 1e-6); + ctrl::CartesianController::Mode::Position, + q_cmd_check, 10000, 1e-6); if (!ik_solvable) { throw std::runtime_error("moveL: Target pose is unreachable with constant orientation"); } @@ -1887,10 +1775,10 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & std::vector time_points; std::vector distance_ratios; generateSTrapezoidalProfile(total_distance, vel, acc, move_time, num_points, - time_points, distance_ratios); + time_points, distance_ratios); LOG(INFO) << "moveL: Planning trajectory - points=" << num_points - << ", total distance=" << total_distance << "m, move time=" << move_time << "s"; + << ", total distance=" << total_distance << "m, move time=" << move_time << "s"; // 5. 生成轨迹点（位置线性插值，姿态保持不变） std::vector cartesian_trajectory; @@ -1908,12 +1796,12 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & } // 6. 预先计算所有轨迹点的关节位置 - std::vector> joint_positions; + std::vector > joint_positions; joint_positions.push_back(q_current); // 起始位置 // 预先计算所有关节位置 for (size_t i = 1; i < cartesian_trajectory.size(); ++i) { - const auto& T_interp = cartesian_trajectory[i]; + const auto &T_interp = cartesian_trajectory[i]; // 构造当前目标 cmvr::ctrl::PoseTarget current_target; @@ -1925,8 +1813,8 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & // 使用前一点的位置作为初始值求解IK Eigen::Vector q_next; bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, - ctrl::CartesianController::Mode::Position, - q_next, 10000, 1e-6); + ctrl::CartesianController::Mode::Position, + q_next, 10000, 1e-6); if (!ok) { throw std::runtime_error("Pre-computation IK failed"); @@ -1938,26 +1826,30 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & } // 7. 计算每个点的关节速度 - std::vector> joint_velocities; + std::vector > joint_velocities; joint_velocities.push_back(Eigen::Vector::Zero()); // 起始速度为零 for (size_t i = 1; i < joint_positions.size(); ++i) { - double dt = time_points[i] - time_points[i-1]; - Eigen::Vector vel = (joint_positions[i] - joint_positions[i-1]) / dt; + double dt = time_points[i] - time_points[i - 1]; + Eigen::Vector vel = (joint_positions[i] - joint_positions[i - 1]) / dt; joint_velocities.push_back(vel); } // 8. 打印轨迹信息 - std::cout << "\n===================================== 轨迹规划信息 =====================================" << std::endl; + std::cout << "\n===================================== 轨迹规划信息 =====================================" << + std::endl; std::cout << "轨迹点总数: " << cartesian_trajectory.size() << " 个" << std::endl; std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; std::cout << "最大速度: " << std::fixed << std::setprecision(6) << vel << "m/s" << std::endl; std::cout << "加速度: " << std::fixed << std::setprecision(6) << acc << "m/s²" << std::endl; std::cout << "总时间: " << std::fixed << std::setprecision(6) << move_time << "s" << std::endl; - std::cout << "起点位置: (x=" << T_current(0,3) << ", y=" << T_current(1,3) << ", z=" << T_current(2,3) << ")" << std::endl; - std::cout << "终点位置: (x=" << T_target(0,3) << ", y=" << T_target(1,3) << ", z=" << T_target(2,3) << ")" << std::endl; + std::cout << "起点位置: (x=" << T_current(0, 3) << ", y=" << T_current(1, 3) << ", z=" << T_current(2, 3) << ")" << + std::endl; + std::cout << "终点位置: (x=" << T_target(0, 3) << ", y=" << T_target(1, 3) << ", z=" << T_target(2, 3) << ")" << + std::endl; std::cout << "保持姿态不变" << std::endl; - std::cout << "-----------------------------------------------------------------------------------------" << std::endl; + std::cout << "-----------------------------------------------------------------------------------------" << + std::endl; // 9. 执行轨迹 auto loop_start_time = std::chrono::high_resolution_clock::now(); @@ -1987,7 +1879,6 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & for (const auto &j: joint_command) { auto motor = motor_manager_->getMotor(j.joint_name); if (motor != nullptr) { - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } @@ -2005,14 +1896,15 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & // 控制时间节奏 - 使用精确的时间规划 auto expected_time_point = loop_start_time + std::chrono::nanoseconds( - static_cast(expected_time * 1e9) - ); + static_cast(expected_time * 1e9) + ); auto now = std::chrono::high_resolution_clock::now(); if (now < expected_time_point) { std::this_thread::sleep_until(expected_time_point); } else { LOG(WARNING) << "moveL: Behind schedule at point " << i - << " by " << std::chrono::duration_cast(now - expected_time_point).count() << "ms"; + << " by " << std::chrono::duration_cast(now - expected_time_point). + count() << "ms"; } } @@ -2021,7 +1913,6 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & m_robot_->ComputeForwardKinematics(m_state_); rsm_.store(ROBOT_READY); LOG(INFO) << "moveL: Trajectory completed successfully"; - } catch (const std::exception &e) { LOG(ERROR) << "moveL failed: " << e.what(); rsm_.store(ROBOT_ERROR); @@ -2053,9 +1944,9 @@ double HumanoidRobot::calculateMoveTime(double distance, double vel, double // 辅助函数：生成S曲线轨迹规划 template void HumanoidRobot::generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc, - double total_time, size_t num_points, - std::vector& time_points, - std::vector& distance_ratios) { + double total_time, size_t num_points, + std::vector &time_points, + std::vector &distance_ratios) { time_points.clear(); distance_ratios.clear(); @@ -2112,7 +2003,7 @@ void HumanoidRobot::generateSTrapezoidalProfile(double total_distance, doub double dec_start_time = acc_time + constant_time; double dec_elapsed = t - dec_start_time; double s = acc_distance + max_vel * constant_time + - max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed; + max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed; distance_ratios.push_back(s / total_distance); } } @@ -2120,8 +2011,7 @@ void HumanoidRobot::generateSTrapezoidalProfile(double total_distance, doub } template -cmvr::math::Pose3d HumanoidRobot::getTransform(std::string &base_link, std::string &target_link) -{ +cmvr::math::Pose3d HumanoidRobot::getTransform(std::string &base_link, std::string &target_link) { // 获取gRPC生成的Pose3d消息 auto grpc_pose = fk(base_link, target_link); @@ -2150,5 +2040,3 @@ cmvr::math::Pose3d HumanoidRobot::getTransform(std::string &base_link, std: template class cmvr::device::HumanoidRobot<7>; template class cmvr::device::HumanoidRobot<14>; template class cmvr::device::HumanoidRobot<20>; - - diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index d122b4cc..fa876c1a 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -190,23 +190,33 @@ namespace cmvr::device{ std::vector l_motors_cfg_{}; std::vector r_motors_cfg_{}; std::vector waist_motors_cfg_{}; + std::vector head_motors_cfg_{}; std::shared_ptr l_can_client_{nullptr}; std::shared_ptr r_can_client_{nullptr}; std::shared_ptr waist_can_client_{nullptr}; + std::shared_ptr head_can_client_{nullptr}; std::shared_ptr> l_can_receiver_{nullptr}; std::shared_ptr> r_can_receiver_{nullptr}; std::shared_ptr> waist_can_receiver_{nullptr}; + std::shared_ptr> head_can_receiver_{nullptr}; std::shared_ptr> l_can_sender_{nullptr}; std::shared_ptr> r_can_sender_{nullptr}; std::shared_ptr> waist_can_sender_{nullptr}; + std::shared_ptr> head_can_sender_{nullptr}; std::shared_ptr> l_message_manager_{nullptr}; std::shared_ptr> r_message_manager_{nullptr}; std::shared_ptr> waist_message_manager_{nullptr}; + std::shared_ptr> head_message_manager_{nullptr}; + + bool waist_enabled_{false}; + bool right_arm_enabled_{false}; + bool left_arm_enabled_{false}; + bool head_enabled_{false}; std::shared_ptr motor_manager_{nullptr}; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index ad4ab92e..8dd19c57 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -55,7 +55,7 @@ TEST(HumanoidRobotTest,GetState) { TEST(HumanoidRobotTest,MyRobotTest) { - std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml"; + std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; const XmlNode config(config_path); if (!config.hasChild("DeviceManager")){ @@ -71,17 +71,17 @@ TEST(HumanoidRobotTest,MyRobotTest) { std::vector> traj; // robot->calibrateZeroQ("R_WRIST_P"); - robot->calibrateZeroQ("R_WRIST_Y"); + // robot->calibrateZeroQ("R_WRIST_Y"); // robot->calibrateZeroQ("R_WRIST_R"); // cmd = { - // {"L_SHOULDER_P", 0.0}, - // {"L_SHOULDER_R", 0.0}, - // {"L_SHOULDER_Y", 0.0}, - // {"L_ELBOW_R", 0.0}, - // {"L_WRIST_P", 0.0}, - // {"L_WRIST_Y", 0.0}, - // {"L_WRIST_R", 0.0}, + {"L_SHOULDER_P", 0.0}, + {"L_SHOULDER_R", 0.0}, + {"L_SHOULDER_Y", 0.0}, + {"L_ELBOW_R", 0.0}, + {"L_WRIST_P", 0.0}, + {"L_WRIST_Y", 0.0}, + {"L_WRIST_R", 0.0}, // // {"R_SHOULDER_P", 0.0} // {"R_SHOULDER_R", 0.0}, @@ -89,10 +89,15 @@ TEST(HumanoidRobotTest,MyRobotTest) { // {"R_ELBOW_R", 0.0}, // {"R_WRIST_P", 0.0}, // {"R_WRIST_Y", 0.0}, - {"R_WRIST_R", 0.0}, + // {"R_WRIST_R", 0.0}, // {"WAIST_P" ,0.0}, // {"WAIST_Y" ,0.0}, + + {"HEAD_P",0.0}, + {"HEAD_Y",0.0}, + {"HEAD_R",0.0}, + }; robot->moveJ(cmd,0.8);