diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index 62a0adf0..701c6335 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -16,13 +16,13 @@ - + - - - + + + @@ -37,8 +37,7 @@ baseLink="PELVIS_S" jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R" linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S" - bufferSize="50" - verbose="false"> + bufferSize="50"> @@ -47,7 +46,7 @@ - + @@ -69,23 +68,31 @@ - - - - - - + + + + + + + + + + + + + + + + + - - - - + @@ -132,7 +139,7 @@ - + diff --git a/include/devices/state_define.h b/include/devices/state_define.h index 4115c8fa..0835fe65 100644 --- a/include/devices/state_define.h +++ b/include/devices/state_define.h @@ -41,9 +41,21 @@ namespace cmvr::device{ // ------------------------------------- robot ------------------------------------- typedef enum { - FORWARD, BACKWARD + FORWARD, BACKWARD, + X_POSITIVE, // X轴正向 + X_NEGATIVE, // X轴负向 + Y_POSITIVE, // Y轴正向 + Y_NEGATIVE, // Y轴负向 + Z_POSITIVE, // Z轴正向 + Z_NEGATIVE, // Z轴负向 + // 角度方向也可以添加 + ROTATE_X, // 绕X轴旋转 + ROTATE_Y, // 绕Y轴旋转 + ROTATE_Z // 绕Z轴旋转 } RobotJointIndexDirection; + + /// robot cartesian index typedef enum { X, Y, Z, RX, RY, RZ diff --git a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp index ddd886db..69eaf9a2 100644 --- a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp +++ b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp @@ -206,7 +206,12 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { break; } - + case RUN_MODE_PROFILE_VELOCITY: { + cw.enable_operation = 1; + cw.switch_on = 1; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + break; + } default: // TODO: Handle unspecified or unknown mode break; @@ -419,18 +424,22 @@ bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { } void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) { - uint32_t speed = ((std::abs(qd) * RADTODEG) * GearRatio * 100.0) / 360.0; - + auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; switch (cur_mode_[node_id]) { case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: case msgs::RUN_MODE_PROFILE_POSITION: { auto it = last_Qd_.find(node_id); if (it == last_Qd_.end() || it->second != speed) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed,0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),0); last_Qd_[node_id] = speed; } break; } + case msgs::RUN_MODE_PROFILE_VELOCITY: { + // 在速度模式下，直接设置目标速度 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0); + break; + } default: break; } diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index a69d9109..290c4085 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -1006,38 +1006,546 @@ void HumanoidRobot::speedJ(std::string &joint_name, RobotJointIndexDirectio if (rsm_.load() == ROBOT_RUNNING) { flash_cmd_.store(true); eStop(); + LOG(ERROR) << "机器人正在运行，停止当前运动"; } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) { rsm_.store(ROBOT_RUNNING); - // TODO: - // 1. set joint speed and acc - // 2. set joint speed by PROFILE VELOCITY MODE (PVM) - // rsm_.store(ROBOT_READY); -> should not set rsm_ to ready because motor is 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("rsm invalid"); + throw runtime_error("无效的机器人状态，无法进行速度控制"); } } catch (exception &e) { - throw runtime_error(e.what()); + rsm_.store(ROBOT_ESTOP); // 出错时，设置为紧急停止状态 + LOG(ERROR) << "speedJ 控制失败: " << e.what(); + throw runtime_error("speedJ 控制失败: " + string(e.what())); } } +// template +// void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) { +// if (vel <= 0 || acc <= 0) { +// throw std::runtime_error("speedL: vel and acc must be positive"); +// } +// rsm_.store(ROBOT_RUNNING); +// try { +// const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变 +// +// // 定义基座和末端执行器链接 +// std::string base_link = "PELVIS_S"; +// std::string ee_link = "R_WRIST_R_S"; +// +// // 获取当前末端执行器位姿 +// msgs::Pose3d current_pose = fk(base_link, ee_link); +// +// // 初始化当前位姿矩阵 +// Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity(); +// T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix( +// current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz() +// ); +// T_current(0, 3) = current_pose.position().x(); +// T_current(1, 3) = current_pose.position().y(); +// T_current(2, 3) = current_pose.position().z(); +// +// // 获取目标笛卡尔速度方向 +// Eigen::Vector3d direction = Eigen::Vector3d::Zero(); +// +// // 根据方向设置笛卡尔速度 +// if (cart == RobotCartesian::X) { +// direction.x() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0; +// } else if (cart == RobotCartesian::Y) { +// direction.y() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0; +// } else if (cart == RobotCartesian::Z) { +// direction.z() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0; +// } +// +// // 规范化方向向量 +// direction.normalize(); +// +// // 计算目标位姿（基于当前位姿和方向） +// Eigen::Matrix4d T_target = T_current; +// T_target(0, 3) += direction.x() * vel * CONTROL_PERIOD; // 小位移 +// T_target(1, 3) += direction.y() * vel * CONTROL_PERIOD; +// T_target(2, 3) += direction.z() * vel * CONTROL_PERIOD; +// +// // 创建目标位姿 +// cmvr::ctrl::PoseTarget target; +// target.T_target = T_target; +// target.link_name = ee_link; +// target.w_posrot = 0.5; // 位置和旋转权重 +// target.weight = 1.0; +// +// // 创建队列用于存储关节速度 +// std::queue> velocity_queue; +// std::mutex queue_mutex; +// std::condition_variable queue_cv; +// std::atomic ik_completed{false}; +// std::atomic stop_requested{false}; +// +// // IK计算线程（使用CartesianController的Velocity模式） +// std::thread ik_thread([&]() { +// try { +// // 获取当前关节位置 +// auto q_map_current = getJointQ(); +// Eigen::Vector current_joint_positions; +// for (int i = 0; i < DOF; ++i) { +// current_joint_positions[i] = q_map_current[joint_names_[i]]; +// } +// +// // 设置初始状态 +// m_state_->SetQ(current_joint_positions); +// m_robot_->ComputeForwardKinematics(m_state_); +// +// LOG(INFO) << "Current robot state: " << rsm_.load(); +// LOG(INFO) << "stop_requested: " << stop_requested.load(); +// +// // IK计算循环 +// while (rsm_.load() == ROBOT_RUNNING ) { +// // 使用CartesianController计算关节速度 +// Eigen::Vector joint_velocities; +// bool ok = m_cctrl_->compute(m_state_, base_link, {target}, CONTROL_PERIOD, +// ctrl::CartesianController::Mode::Velocity, +// joint_velocities, 10000, 1e-6); +// +// if (!ok) { +// LOG(WARNING) << "IK计算失败，使用上一个有效速度"; +// // 可以使用上一个有效的速度或者零速度 +// continue; +// } +// +// LOG(INFO) << "IK计算得到的关节速度："; +// for (int i = 0; i < DOF; ++i) { +// LOG(INFO) << "关节 " << joint_names_[i] << " 速度: " << joint_velocities[i]; +// } +// +// // 将关节速度加入队列 +// { +// std::lock_guard lock(queue_mutex); +// velocity_queue.push(joint_velocities); +// } +// queue_cv.notify_one(); +// +// // 更新目标位姿（基于当前位姿和方向） +// msgs::Pose3d current_pose = fk(base_link, ee_link); +// T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix( +// current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz() +// ); +// T_current(0, 3) = current_pose.position().x(); +// T_current(1, 3) = current_pose.position().y(); +// T_current(2, 3) = current_pose.position().z(); +// +// T_target = T_current; +// T_target(0, 3) += direction.x() * vel * CONTROL_PERIOD; +// T_target(1, 3) += direction.y() * vel * CONTROL_PERIOD; +// T_target(2, 3) += direction.z() * vel * CONTROL_PERIOD; +// +// target.T_target = T_target; +// +// // 等待控制周期 +// std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(CONTROL_PERIOD * 1000))); +// } +// +// ik_completed.store(true); +// queue_cv.notify_one(); +// +// } catch (const std::exception& e) { +// LOG(ERROR) << "IK thread error: " << e.what(); +// ik_completed.store(true); +// queue_cv.notify_one(); +// } +// }); +// +// // 控制执行线程（周期同步速度模式） +// std::thread control_thread([&]() { +// try { +// // 设置所有电机为周期同步速度模式 +// for (int i = 0; i < DOF; ++i) { +// auto motor = motor_manager_->getMotor(joint_names_[i]); +// if (motor != nullptr) { +// if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { +// motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); +// } +// } +// } +// +// // 控制循环 +// while (rsm_.load() == ROBOT_RUNNING && !stop_requested.load()) { +// Eigen::Vector joint_velocities; +// bool has_velocity = false; +// +// // 从队列中获取关节速度 +// { +// std::unique_lock lock(queue_mutex); +// if (queue_cv.wait_for(lock, std::chrono::milliseconds(100), +// [&] { return !velocity_queue.empty() || ik_completed.load(); })) { +// if (!velocity_queue.empty()) { +// joint_velocities = velocity_queue.front(); +// velocity_queue.pop(); +// has_velocity = true; +// } +// } +// } +// +// if (has_velocity) { +// // 发送关节速度命令 +// for (int j = 0; j < DOF; ++j) { +// auto motor = motor_manager_->getMotor(joint_names_[j]); +// // if (motor != nullptr) { +// // motor->setQd(joint_velocities[j]); +// // } +// } +// } +// +// // 检查是否完成 +// { +// std::lock_guard lock(queue_mutex); +// if (ik_completed.load() && velocity_queue.empty()) { +// break; +// } +// } +// +// // 控制时间节奏 +// std::this_thread::sleep_for(std::chrono::milliseconds(10)); +// } +// +// // 停止所有关节 +// for (int i = 0; i < DOF; ++i) { +// auto motor = motor_manager_->getMotor(joint_names_[i]); +// if (motor != nullptr) { +// motor->setQd(0); +// } +// } +// +// } catch (const std::exception& e) { +// LOG(ERROR) << "Control thread error: " << e.what(); +// } +// }); +// +// LOG(INFO) << "开始在笛卡尔空间进行速度控制，方向：" << dir << "，速度：" << vel; +// +// // 等待停止命令 +// while (rsm_.load() == ROBOT_RUNNING && !flash_cmd_.load()) { +// std::this_thread::sleep_for(std::chrono::milliseconds(100)); +// } +// +// // 通知线程停止 +// stop_requested.store(true); +// { +// std::lock_guard lock(queue_mutex); +// ik_completed.store(true); +// } +// queue_cv.notify_all(); +// +// // 等待线程结束 +// if (ik_thread.joinable()) { +// ik_thread.join(); +// } +// if (control_thread.joinable()) { +// control_thread.join(); +// } +// +// LOG(INFO) << "speedL速度控制完成，所有电机已停止。"; +// +// } catch (const std::exception &e) { +// LOG(ERROR) << "speedL failed: " << e.what(); +// rsm_.store(ROBOT_ERROR); +// throw std::runtime_error(std::string("speedL error: ") + e.what()); +// } +// } + + template void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) { + if (vel <= 0 || acc <= 0) { + throw std::runtime_error("speedL: vel and acc must be positive"); + } + 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); - // TODO: ??? - // rsm_.store(ROBOT_READY); -> should not set rsm_ to ready because motor is running - } else { - throw runtime_error("rsm invalid"); + const double CONTROL_PERIOD = 1.0 / 100; // 控制周期保持不变 + + // 定义基座和末端执行器链接 + std::string base_link = "PELVIS_S"; + std::string ee_link = "R_WRIST_R_S"; + + msgs::Pose3d current_pose = fk(base_link, ee_link); + + // 初始化当前位姿矩阵 + Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity(); + T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix( + current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz() + ); + T_current(0, 3) = current_pose.position().x(); + T_current(1, 3) = current_pose.position().y(); + T_current(2, 3) = current_pose.position().z(); + + // 获取目标笛卡尔速度方向 + Eigen::Vector3d direction = Eigen::Vector3d::Zero(); + bool is_rotation = false; + + // 根据方向设置笛卡尔速度 + switch (dir) { + case RobotJointIndexDirection::X_POSITIVE: + direction.x() = 1.0; + break; + case RobotJointIndexDirection::X_NEGATIVE: + direction.x() = -1.0; + break; + case RobotJointIndexDirection::Y_POSITIVE: + direction.y() = 1.0; + break; + case RobotJointIndexDirection::Y_NEGATIVE: + direction.y() = -1.0; + break; + case RobotJointIndexDirection::Z_POSITIVE: + direction.z() = 1.0; + break; + case RobotJointIndexDirection::Z_NEGATIVE: + direction.z() = -1.0; + break; + case RobotJointIndexDirection::ROTATE_X: + direction.x() = 1.0; + is_rotation = true; + break; + case RobotJointIndexDirection::ROTATE_Y: + direction.y() = 1.0; + is_rotation = true; + break; + case RobotJointIndexDirection::ROTATE_Z: + direction.z() = 1.0; + is_rotation = true; + break; + case RobotJointIndexDirection::FORWARD: + // 保持向后兼容 + if (cart == RobotCartesian::X) direction.x() = 1.0; + else if (cart == RobotCartesian::Y) direction.y() = 1.0; + else if (cart == RobotCartesian::Z) direction.z() = 1.0; + break; + case RobotJointIndexDirection::BACKWARD: + // 保持向后兼容 + if (cart == RobotCartesian::X) direction.x() = -1.0; + else if (cart == RobotCartesian::Y) direction.y() = -1.0; + else if (cart == RobotCartesian::Z) direction.z() = -1.0; + break; + default: + throw std::runtime_error("speedL: unknown direction"); } - } catch (exception &e) { - throw runtime_error(e.what()); + + // 计算末端执行器的总运动时间 + double move_time = calculateMoveTime(direction.norm(), vel, acc); + + // 使用S曲线速度规划 + size_t num_points = std::max(2ul, static_cast(ceil(move_time / CONTROL_PERIOD))); + std::vector time_points; + std::vector distance_ratios; + generateSTrapezoidalProfile(direction.norm(), vel, acc, move_time, num_points, + time_points, distance_ratios); + + LOG(INFO) << "speedL: Planning trajectory - points=" << num_points + << ", total distance=" << direction.norm() << "m, move time=" << move_time << "s"; + + // 生成轨迹点 + std::vector cartesian_trajectory; + for (size_t i = 0; i <= num_points; ++i) { + double s = distance_ratios[i]; // 使用S曲线规划的距离比例 + + Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity(); + + if (is_rotation) { + // 旋转运动：保持位置不变，旋转姿态 + T_interp.block<3, 1>(0, 3) = T_current.block<3, 1>(0, 3); // 位置不变 + + // 计算旋转矩阵 + double angle = s * direction.norm(); // 旋转角度 + Eigen::AngleAxisd rotation(angle, direction.normalized()); + Eigen::Matrix3d R_current = T_current.block<3, 3>(0, 0); + Eigen::Matrix3d R_interp = rotation * R_current; + T_interp.block<3, 3>(0, 0) = R_interp; + } else { + // 平移运动：保持姿态不变，移动位置 + T_interp.block<3, 3>(0, 0) = T_current.block<3, 3>(0, 0); // 保持姿态不变 + T_interp(0, 3) = T_current(0, 3) + s * direction.x(); + T_interp(1, 3) = T_current(1, 3) + s * direction.y(); + T_interp(2, 3) = T_current(2, 3) + s * direction.z(); + } + + cartesian_trajectory.push_back(T_interp); + } + + // 预先计算所有关节位置和速度 + std::vector> joint_positions; + std::vector> joint_velocities; + + // 获取当前关节位置 + auto q_map_current = getJointQ(); + Eigen::Vector q_current; + // 根据你的关节名称填充q_current + for (int i = 0; i < DOF; ++i) { + q_current[i] = q_map_current[joint_names_[i]]; + } + joint_positions.push_back(q_current); + joint_velocities.push_back(Eigen::Vector::Zero()); // 起始速度为零 + + // 预先计算所有关节位置 + for (size_t i = 1; i < cartesian_trajectory.size(); ++i) { + const auto& T_interp = cartesian_trajectory[i]; + + // 构造当前目标 + cmvr::ctrl::PoseTarget current_target; + current_target.T_target = T_interp; + current_target.link_name = ee_link; + current_target.w_posrot = 0.5; + current_target.weight = 1.0; + + // 使用前一点的位置作为初始值求解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); + + if (!ok) { + LOG(WARNING) << "IK计算失败，使用上一个有效点"; + q_next = joint_positions.back(); + } + + LOG(INFO) << "IK计算结果 (q_next): " << q_next.transpose(); + joint_positions.push_back(q_next); + } + + // 计算每个点的关节速度 + 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; + joint_velocities.push_back(vel); + } + + // 创建队列用于存储轨迹点 + std::queue, Eigen::Vector>> trajectory_queue; + std::mutex queue_mutex; + std::condition_variable queue_cv; + std::atomic trajectory_completed{false}; + + // 轨迹点计算线程 + std::thread trajectory_thread([&]() { + try { + // 计算并将轨迹点放入队列 + for (size_t i = 0; i < joint_positions.size(); ++i) { + { + std::lock_guard lock(queue_mutex); + trajectory_queue.push({joint_positions[i], joint_velocities[i]}); + } + queue_cv.notify_one(); // 通知控制线程 + + // 控制节奏 + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(CONTROL_PERIOD * 1000))); + } + + trajectory_completed.store(true); + queue_cv.notify_one(); + } catch (const std::exception &e) { + LOG(ERROR) << "Trajectory thread error: " << e.what(); + trajectory_completed.store(true); + queue_cv.notify_one(); + } + }); + + // 控制执行线程 + std::thread control_thread([&]() { + try { + while (!trajectory_completed.load() || !trajectory_queue.empty()) { + // 从队列中获取轨迹点 + std::pair, Eigen::Vector> point; + bool has_point = false; + + { + std::unique_lock lock(queue_mutex); + if (queue_cv.wait_for(lock, std::chrono::milliseconds(100), + [&] { return !trajectory_queue.empty() || trajectory_completed.load(); })) { + if (!trajectory_queue.empty()) { + point = trajectory_queue.front(); + trajectory_queue.pop(); + has_point = true; + } + } + } + + if (has_point) { + // 更新关节命令 + m_state_->SetQ(point.first); + m_robot_->ComputeForwardKinematics(m_state_); + + // 发送关节命令 - 为每个电机单独设置位置和速度 + for (int j = 0; j < DOF; ++j) { + auto motor = motor_manager_->getMotor(joint_names_[j]); + if (motor != nullptr) { + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + } + motor->setQd(point.second[j]); + motor->setQ(point.first[j]); + + // 打印发送的关节命令 + LOG(INFO) << "Sending setQ: joint[" << joint_names_[j] << "] = " + << point.first[j] << ", joint_velocity = " + << point.second[j]; + } + } + } + + // 控制时间节奏 + std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 控制周期 + } + rsm_.store(ROBOT_READY); // 运动完成 + } catch (const std::exception &e) { + LOG(ERROR) << "Control thread error: " << e.what(); + rsm_.store(ROBOT_ERROR); + } + }); + + // 等待线程完成 + if (trajectory_thread.joinable()) { + trajectory_thread.join(); + } + if (control_thread.joinable()) { + control_thread.join(); + } + + LOG(INFO) << "speedL trajectory execution completed successfully."; + + } catch (const std::exception &e) { + LOG(ERROR) << "speedL failed: " << e.what(); + rsm_.store(ROBOT_ERROR); + throw std::runtime_error(std::string("speedL error: ") + e.what()); } } + + + template void HumanoidRobot::followJointTrajectory(std::vector > &traj, double dt) { try { @@ -1436,7 +1944,7 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & } try { - const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变 + const double CONTROL_PERIOD = 1.0 / 100.0; // 控制周期保持不变 msgs::Pose3d current_pose = fk(base_link, ee_link); // 1. 初始化当前和目标位姿矩阵 @@ -1524,6 +2032,26 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & cartesian_trajectory.push_back(T_interp); } + // 在这里保存轨迹点到文件 + std::ofstream trajectory_file("/home/tankaitao/cmvr-es/trajectory_points.csv"); + if (!trajectory_file.is_open()) { + throw std::runtime_error("Failed to open trajectory file"); + } + + // 写入 CSV 文件头 + trajectory_file << "x,y,z\n"; + + // 写入轨迹点位置 + for (size_t i = 0; i < cartesian_trajectory.size(); ++i) { + Eigen::Matrix4d T = cartesian_trajectory[i]; + Eigen::Vector3d position = T.block<3, 1>(0, 3); // 获取位置 + + trajectory_file << position.x() << "," << position.y() << "," << position.z() << "\n"; + } + + trajectory_file.close(); + LOG(INFO) << "Trajectory points saved to /home/tankaitao/cmvr-es/trajectory_points.csv"; + // 6. 预先计算所有轨迹点的关节位置 std::vector> joint_positions; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index 96dd4289..0764c8a2 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -17,6 +17,19 @@ #include "canbus/can_comm/message_manager.h" #include "cmvr/msgs/robot_detail.pb.h" #include "motor/motor_manager.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace cmvr::device{ @@ -93,10 +106,26 @@ namespace cmvr::device{ + + + // 添加这三个函数的声明 + bool computeJacobian(const std::shared_ptr>& state, + const std::string& base_link, + const std::string& ee_link, + Eigen::Matrix& J); + + Eigen::Vector3d getEndEffectorPosition(const std::shared_ptr>& state, + const std::string& ee_link); + + Eigen::Matrix3d getEndEffectorOrientation(const std::shared_ptr>& state, + const std::string& ee_link); + + + public: // 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz); - static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R);\ + static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R); double calculateMoveTime(double distance, double vel, double acc); @@ -128,6 +157,29 @@ namespace cmvr::device{ std::shared_ptr> CSV_buffer_; std::shared_ptr> CSC_buffer_; + + struct ControlPoint { + Eigen::Vector joint_positions; + Eigen::Vector joint_velocities; + double timestamp; + ControlPoint() : timestamp(0.0) {} + ControlPoint(const Eigen::Vector& pos, const Eigen::Vector& vel, double time): joint_positions(pos), joint_velocities(vel), timestamp(time) {} + }; + // 线程间通信的队列和同步变量 + std::queue control_queue_; + std::mutex queue_mutex_; + std::condition_variable queue_cv_; + std::atomic trajectory_completed_{false}; + std::atomic control_thread_ready_{false}; + std::atomic stop_requested_{false}; + + // 线程对象 + std::unique_ptr trajectory_thread_; + std::unique_ptr control_thread_; + + + + private: std::vector l_motors_cfg_; std::vector r_motors_cfg_; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index 1e55b715..7d317126 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -13,6 +13,8 @@ #include "cmvr/msgs/can_card_parameter.grpc.pb.h" #include "cmvr/msgs/geometry.pb.h" #include "robot/humanoid_robot/humanoid_robot.h" +#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" +#include "utils/base/abstract_interpolation.h" // 定义一个命令行参数 --config_path DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file"); @@ -432,6 +434,7 @@ TEST(HumanoidRobotTest,MoveIKTest) { } + TEST(HumanoidRobotTest,MoveLTest) { std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml"; const XmlNode config(config_path); @@ -452,15 +455,19 @@ TEST(HumanoidRobotTest,MoveLTest) { // 定义目标位姿（直线运动的终点与当前的差值） cmvr::msgs::Pose3d delta_pose; - delta_pose.mutable_position()->set_x(0.05); - delta_pose.mutable_position()->set_y(0.05); - delta_pose.mutable_position()->set_z(0.05); + delta_pose.mutable_position()->set_x(0.2); + delta_pose.mutable_position()->set_y(0); + delta_pose.mutable_position()->set_z(0); delta_pose.mutable_euler()->set_rx(0); delta_pose.mutable_euler()->set_ry(0); delta_pose.mutable_euler()->set_rz(0); // 4. 设置运动参数（速度单位：m/s，加速度单位：m/s²） double vel = 0.1; // 最大线速度 0.1m/s + + + + double acc = 0.05; // 加速度 0.05m/s² // 调用moveL执行直线运动 @@ -471,4 +478,82 @@ TEST(HumanoidRobotTest,MoveLTest) { // 捕获异常（如IK解算失败、状态非法等） LOG(ERROR) << "moveL调用失败：" << e.what(); } +} + + +TEST(HumanoidRobotTest, SpeedJTest) { + // 配置文件路径 + std::string config_path = "/home/tankaitao/cmvr-es/config/cabin_robot.xml"; + const XmlNode config(config_path); + + // 检查配置文件是否包含 DeviceManager 节点 + if (!config.hasChild("DeviceManager")) { + LOG(ERROR) << "Device Manager node not found"; + } + + // 获取 DeviceManager 配置并初始化设备管理器 + auto dmgr_cfg = config.getChild("DeviceManager"); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + + // 获取机器人实例 + auto robot = dmgr.getDevice("hc01"); + + // 设置目标关节和相关参数 + std::string joint_name = "L_WRIST_R"; // 目标关节名 + RobotJointIndexDirection dir = RobotJointIndexDirection::Y_NEGATIVE; // 方向 + double vel = -0.1; // 速度 (rad/s) + double acc = 1; // 加速度 (rad/s²) + + try { + // 调用 speedJ 函数进行关节运动 + robot->speedJ(joint_name, dir, vel, acc); + + } catch (const std::exception& e) { + LOG(ERROR) << "speedJ test failed: " << e.what(); + FAIL() << "Exception thrown during speedJ test: " << e.what(); + } +} + + + +TEST(HumanoidRobotTest, SpeedLTest) { + std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml"; + const XmlNode config(config_path); + + if (!config.hasChild("DeviceManager")) { + LOG(ERROR) << "Device Manager node not found"; + } + auto dmgr_cfg = config.getChild("DeviceManager"); + auto &dmgr = DeviceManager::getInstance(dmgr_cfg); + + auto robot = dmgr.getDevice("hc01"); + + try { + // 定义运动方向（笛卡尔坐标系） + RobotCartesian cart_direction = RobotCartesian::Y; // 沿X轴移动 + + // 定义运动方向（正向或反向） + RobotJointIndexDirection move_direction = RobotJointIndexDirection::Y_NEGATIVE; // 正向 + + // 设置运动参数（速度单位：m/s，加速度单位：m/s²） + double vel = 0.02; // 速度 0.1m/s + double acc = 0.05; // 加速度 0.05m/s² + + LOG(INFO) << "开始执行速度控制，方向：X轴正向，速度：" << vel << "m/s，加速度：" << acc << "m/s²"; + + // 调用speedL执行速度控制 + robot->speedL(cart_direction, move_direction, vel, acc); + + // 等待一段时间让机器人运动 + std::this_thread::sleep_for(std::chrono::seconds(5)); + + // 停止运动（假设有停止函数） + // robot->stopSpeedL(); + + LOG(INFO) << "速度控制完成！"; + + } catch (const std::exception& e) { + // 捕获异常（如IK解算失败、状态非法等） + LOG(ERROR) << "speedL调用失败：" << e.what(); + } } \ No newline at end of file diff --git a/src/utils/controller/cartesian_controller.cpp b/src/utils/controller/cartesian_controller.cpp index 8e7e6e57..b5a4a479 100644 --- a/src/utils/controller/cartesian_controller.cpp +++ b/src/utils/controller/cartesian_controller.cpp @@ -79,9 +79,23 @@ bool CartesianController::compute( case Mode::Velocity: { const double Kp = 1.0; // [rad/s] per rad error out_cmd = Kp * (q_des - q_init); + std::cout << "=================== q_des = " << q_des + << "q_init = " << q_init << std::endl; const auto qd_lim = robot_->GetLimitQdotUpper(state); - for (int i = 0; i < DOF; ++i) + for (int i = 0; i < DOF; ++i) { + // 打印原始的速度命令和限制值 + std::cout << "Before clamping - out_cmd[" << i << "] = " << out_cmd[i] + << ", qd_lim(" << i << ") = " << qd_lim(i) + << ", -qd_lim(" << i << ") = " << -qd_lim(i) << std::endl; + + // 对 out_cmd 进行限速 out_cmd[i] = std::clamp(out_cmd[i], -qd_lim(i), qd_lim(i)); + + // 打印被clamp之后的值 + std::cout << "After clamping - out_cmd[" << i << "] = " << out_cmd[i] + << ", qd_lim(" << i << ") = " << qd_lim(i) + << ", -qd_lim(" << i << ") = " << -qd_lim(i) << std::endl; + } return ik_ok; } case Mode::Torque: {