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

update

Browse Source
This commit is contained in:
tankaitao 2025-09-11 16:36:14 +08:00
parent 25ec465e0b
commit 823e235ee0
2 changed files with 643 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

763
src/devices/robot/humanoid_robot/humanoid_robot.cpp
View File

@ -5,6 +5,7 @@
#include "humanoid_robot.h"
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "motor/ti5_motor/ti5_motor.h"
#include "utils/base/abstract_interpolation.h"
using namespace std;
@ -246,7 +247,7 @@ HumanoidRobot<DOF>::~HumanoidRobot() {
{"WAIST_P", 0}
};
this->moveJ(cmd,0.8);
// this->moveJ(cmd,0.8);
this->torqueOff();
}
@ -549,35 +550,456 @@ void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vecto
}
}
// template<int DOF>
// void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &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<double, DOF> 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<Eigen::Matrix4d> 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<int>(move_time * 100); // 100Hz控制频率
//
// // 存储所有插值点的关节角度
// std::vector<Eigen::Vector<double, DOF>> joint_trajectory;
// joint_trajectory.reserve(num_points + 1);
//
// // 记录上一次成功的关节角度
// Eigen::Vector<double, DOF> 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<double>(i) / num_points;
//
// // 创建插值后的目标
// std::vector<cmvr::ctrl::PoseTarget> 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);
// Eigen::Matrix3d target_rot_matrix = targets[j].T_target.block<3, 3>(0, 0);
// Eigen::Quaterniond current_rot(current_rot_matrix);
// Eigen::Quaterniond target_rot(target_rot_matrix);
// Eigen::Quaterniond interp_rot = current_rot.slerp(t, target_rot);
//
// // 更新目标位姿
// interpolated_targets[j].T_target.setIdentity();
// interpolated_targets[j].T_target.block<3, 3>(0, 0) = interp_rot.toRotationMatrix();
// interpolated_targets[j].T_target.block<3, 1>(0, 3) = interp_pos;
// }
//
// // 求解逆运动学
// Eigen::Vector<double, DOF> q_cmd;
// bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002,
// ctrl::CartesianController<DOF>::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];
// }
// }
//
// // 计算每个关节的最大角度变化
// Eigen::Vector<double, DOF> max_angle_change = Eigen::Vector<double, DOF>::Zero();
// for (int i = 1; i < joint_trajectory.size(); i++) {
// Eigen::Vector<double, DOF> delta = joint_trajectory[i] - joint_trajectory[i-1];
// for (int j = 0; j < DOF; j++) {
// if (std::abs(delta[j]) > std::abs(max_angle_change[j])) {
// max_angle_change[j] = delta[j];
// }
// }
// }
//
// // 计算每个关节所需的时间比例因子
// Eigen::Vector<double, DOF> time_scale_factors = Eigen::Vector<double, DOF>::Ones();
// for (int j = 0; j < DOF; j++) {
// if (std::abs(max_angle_change[j]) > 1e-6) {
// // 根据关节的最大速度和加速度限制计算时间比例因子
// double max_vel = 1.0; // 假设最大角速度 1 rad/s
// double max_acc = 2.0; // 假设最大角加速度 2 rad/s²
//
// double required_time_vel = std::abs(max_angle_change[j]) / max_vel;
// double required_time_acc = std::sqrt(std::abs(max_angle_change[j]) / max_acc);
//
// double required_time = std::max(required_time_vel, required_time_acc);
// time_scale_factors[j] = required_time / move_time;
// }
// }
//
// // 取最大的时间比例因子作为整体时间缩放因子
// double max_time_scale = time_scale_factors.maxCoeff();
// if (max_time_scale > 1.0) {
// // 需要延长运动时间
// move_time *= max_time_scale;
// num_points = static_cast<int>(move_time * 100);
// LOG(INFO) << "Adjusted move time: " << move_time << " seconds";
// }
//
// // 执行轨迹
// for (int i = 0; i <= num_points; i++) {
// if (flash_cmd_.load()) {
// flash_cmd_.store(false);
// break;
// }
//
// // 计算当前时间点的索引（考虑时间缩放）
// int idx = static_cast<int>(i / max_time_scale);
// if (idx >= joint_trajectory.size()) {
// idx = joint_trajectory.size() - 1;
// }
//
// Eigen::Vector<double, DOF> q_cmd = joint_trajectory[idx];
//
// // 发送关节命令 - 控制所有7个关节
// std::vector<JointPoint> 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]}
// };
//
// // 计算每个关节的角度变化
// std::vector<double> angle_changes(joint_points.size(), 0.0);
// if (i > 0) {
// int prev_idx = static_cast<int>((i-1) / max_time_scale);
// if (prev_idx >= joint_trajectory.size()) {
// prev_idx = joint_trajectory.size() - 1;
// }
//
// Eigen::Vector<double, DOF> prev_q = joint_trajectory[prev_idx];
// for (size_t j = 0; j < joint_points.size(); j++) {
// angle_changes[j] = std::abs(q_cmd[7 + j] - prev_q[7 + j]);
// }
// }
//
// // 设置每个关节的速度和位置
// 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) {
// // 根据关节的角度变化计算实际速度
// double actual_vel = vel;
// if (i > 0) {
// actual_vel = angle_changes[j] / (move_time / num_points);
// }
//
// motor->setQd(actual_vel);
// if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
// motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
// }
// 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);
//
// rsm_.store(ROBOT_READY);
// } else {
// throw std::runtime_error("rsm invalid");
// }
// } catch (std::exception &e) {
// rsm_.store(ROBOT_ESTOP);
// throw std::runtime_error(e.what());
// }
// }
template<int DOF>
void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
double acc) {
void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, 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) {
return;
}
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
rsm_.store(ROBOT_RUNNING);
// TODO:
// 1. interpolate line waypoint by vel and acc
// 2. for each waypoint, call cartesian controller to solve joint positions
// 3. for each waypoint, call motor Cyclic Synchronous Position (CSP) command with Timer
// 4. in the loop, check flash_cmd_, if it is true, set it false then exit
// Eigen::Vector<double, DOF> q_cmd;
// m_state_->SetQ(state_.joint_positions);
// bool ok = m_cctrl_.compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position, q_cmd, 60, 1e-4);
// if (!ok) {
// throw runtime_error("solve IK failed");
// }
// 获取当前关节状态
Eigen::Vector<double, DOF> 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<Eigen::Matrix4d> 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<int>(move_time * 100); // 100Hz控制频率
// 存储所有插值点的关节角度
std::vector<Eigen::Vector<double, DOF>> joint_trajectory;
joint_trajectory.reserve(num_points + 1);
// 记录上一次成功的关节角度
Eigen::Vector<double, DOF> 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<double>(i) / num_points;
// 创建插值后的目标（只做位置插值，旋转保持不变）
std::vector<cmvr::ctrl::PoseTarget> 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<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002,
ctrl::CartesianController<DOF>::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<double, DOF> q_cmd = joint_trajectory[i];
// 发送关节命令
std::vector<JointPoint> 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);
rsm_.store(ROBOT_READY);
} else {
throw runtime_error("rsm invalid");
throw std::runtime_error("rsm invalid");
}
} catch (exception &e) {
throw runtime_error(e.what());
} catch (std::exception &e) {
rsm_.store(ROBOT_ESTOP);
throw std::runtime_error(e.what());
}
}
template<int DOF>
void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) {
try {
@ -972,29 +1394,27 @@ template<int DOF>
void HumanoidRobot<DOF>::moveDeltaL(const std::string &base_link, const std::string &ee_link,
msgs::Pose3d delta_pose, double vel, double acc) {
try {
Eigen::Vector<double, DOF> 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_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;
// 1. 计算末端当前位姿（通过FK）
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();
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();
// 2. 计算目标位姿 = 当前位姿 + 相对偏移（位置/姿态分别叠加）
msgs::Pose3d target_pose;
// 位置偏移（米）
target_pose.mutable_position()->set_x(current_pose.position().x() + delta_pose.position().x());
target_pose.mutable_position()->set_y(current_pose.position().y() + delta_pose.position().y());
target_pose.mutable_position()->set_z(current_pose.position().z() + delta_pose.position().z());
// 姿态偏移（弧度，ZYX欧拉角）
target_pose.mutable_euler()->set_rx(current_pose.euler().rx() + delta_pose.euler().rx());
target_pose.mutable_euler()->set_ry(current_pose.euler().ry() + delta_pose.euler().ry());
target_pose.mutable_euler()->set_rz(current_pose.euler().rz() + delta_pose.euler().rz());
@ -1036,21 +1456,25 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
T_target(1, 3) = target_pose.position().y();
T_target(2, 3) = target_pose.position().z();
// 保存起始姿态，确保整个运动过程中姿态保持不变
Eigen::Matrix3d start_orientation = T_current.block<3, 3>(0, 0);
// 2. 获取当前关节配置并验证目标可达性
Eigen::Vector<double, DOF> 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_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);
m_robot_->ComputeForwardKinematics(m_state_);
// 验证目标点可达性
// 验证目标点可达性 - 使用起始姿态，确保姿态不变
cmvr::ctrl::PoseTarget target_ik_check;
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.link_name = ee_link;
@ -1058,12 +1482,12 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
Eigen::Vector<double, DOF> q_cmd_check;
bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position,
q_cmd_check, 10000, 1e-6); // 使用main中的高迭代次数
q_cmd_check, 10000, 1e-6);
if (!ik_solvable) {
throw std::runtime_error("moveL: Target pose is unreachable");
throw std::runtime_error("moveL: Target pose is unreachable with constant orientation");
}
// 3. 计算位置差值（与main一致，保持姿态不变）
// 3. 计算位置差值（保持姿态不变）
Eigen::Vector3d delta_pos = T_target.block<3, 1>(0, 3) - T_current.block<3, 1>(0, 3);
double total_distance = delta_pos.norm();
@ -1072,22 +1496,27 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
return;
}
// 4. 基于路径长度的均匀插值（核心修改点）
// 计算所需的插值点数（根据速度和控制周期计算）
double move_time = total_distance / vel; // 总移动时间
// 4. 基于S曲线速度规划的时间规划
// 计算总时间和插值点数
double move_time = calculateMoveTime(total_distance, vel, acc);
size_t num_points = std::max(2ul, static_cast<size_t>(ceil(move_time / CONTROL_PERIOD)));
double step_distance = total_distance / num_points; // 每个点的距离间隔
// 生成时间轴和距离比例
std::vector<double> time_points;
std::vector<double> distance_ratios;
generateSTrapezoidalProfile(total_distance, vel, acc, move_time, num_points,
time_points, distance_ratios);
LOG(INFO) << "moveL: Planning trajectory - points=" << num_points
<< ", total distance=" << total_distance << "m, move time=" << move_time << "s";
// 5. 生成均匀分布的轨迹点（仅位置变化，姿态保持与起点一致）
// 5. 生成轨迹点（位置线性插值，姿态保持不变）
std::vector<Eigen::Matrix4d> cartesian_trajectory;
for (size_t i = 0; i <= num_points; ++i) {
double s = static_cast<double>(i) / num_points; // 基于距离的插值系数（0~1）
double s = distance_ratios[i]; // 使用S曲线规划的距离比例
Eigen::Matrix4d T_interp = T_current; // 复制起点姿态（保持不变）
Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity();
T_interp.block<3, 3>(0, 0) = start_orientation; // 保持起始姿态不变
// 仅位置按比例插值
T_interp(0, 3) = T_current(0, 3) + s * delta_pos.x();
T_interp(1, 3) = T_current(1, 3) + s * delta_pos.y();
@ -1096,45 +1525,12 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
cartesian_trajectory.push_back(T_interp);
}
// 6. 打印所有轨迹点信息
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) << step_distance << "m" << 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::setw(4) << "序号" << " | "
<< std::setw(10) << "x(m)" << " | "
<< std::setw(10) << "y(m)" << " | "
<< std::setw(10) << "z(m)" << " | "
<< std::setw(16) << "到起点距离(m)" << " | "
<< std::setw(16) << "与上一点距离(m)" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" << std::endl;
// 6. 预先计算所有轨迹点的关节位置
std::vector<Eigen::Vector<double, DOF>> joint_positions;
joint_positions.push_back(q_current); // 起始位置
double prev_distance = 0.0;
for (size_t idx = 0; idx < cartesian_trajectory.size(); ++idx) {
const auto& T = cartesian_trajectory[idx];
Eigen::Vector3d pos(T(0,3), T(1,3), T(2,3));
Eigen::Vector3d delta_from_start = pos - T_current.block<3,1>(0,3);
double current_distance = delta_from_start.norm();
double segment_distance = (idx == 0) ? current_distance : current_distance - prev_distance;
std::cout << std::setw(4) << 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(16) << current_distance << " | "
<< std::fixed << std::setprecision(6) << std::setw(16) << segment_distance << std::endl;
prev_distance = current_distance;
}
std::cout << "=========================================================================================\n" << std::endl;
// 7. 执行IK求解（与main逻辑一致，逐步更新状态）
auto loop_start_time = std::chrono::high_resolution_clock::now();
for (size_t i = 0; i < cartesian_trajectory.size(); ++i) {
// 预先计算所有关节位置
for (size_t i = 1; i < cartesian_trajectory.size(); ++i) {
const auto& T_interp = cartesian_trajectory[i];
// 构造当前目标
@ -1144,68 +1540,78 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
current_target.w_posrot = 0.5;
current_target.weight = 1.0;
// IK求解参数（使用main中的高迭代次数）
int max_iter = 10000;
double tolerance = 1e-6;
// 求解IK，以上一个状态作为初始值（与main一致）
// 使用前一点的位置作为初始值求解IK
Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
q_current, max_iter, tolerance);
// 失败重试机制
if (!ok) {
LOG(WARNING) << "Retrying IK for point " << i;
ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
q_current, max_iter * 2, tolerance * 10);
}
q_next, 10000, 1e-6);
if (!ok) {
LOG(ERROR) << "IK failed at point " << i;
LOG(ERROR) << "Target pos: (" << T_interp(0,3) << ","
<< T_interp(1,3) << "," << T_interp(2,3) << ")";
throw std::runtime_error("moveL: IK failed during trajectory execution");
LOG(WARNING) << "Pre-computation IK failed at point " << i << ", using previous point";
q_next = joint_positions.back();
}
// 右臂第二个关节不能超过90°
if (q_current[8] > 1.5708)
q_current[8] = 1.5708;
// 新增：打印IK求解得到的关节角度
std::cout << "\n===================================== 关节角度信息 (点 " << i << ") =====================================" << std::endl;
std::cout << "左手臂关节角度（弧度）：" << std::endl;
std::cout << " L_SHOULDER_P: " << std::fixed << std::setprecision(6) << q_current[0] << std::endl;
std::cout << " L_SHOULDER_R: " << std::fixed << std::setprecision(6) << q_current[1] << std::endl;
std::cout << " L_SHOULDER_Y: " << std::fixed << std::setprecision(6) << q_current[2] << std::endl;
std::cout << " L_ELBOW_R: " << std::fixed << std::setprecision(6) << q_current[3] << std::endl;
std::cout << " L_WRIST_P: " << std::fixed << std::setprecision(6) << q_current[4] << std::endl;
std::cout << " L_WRIST_Y: " << std::fixed << std::setprecision(6) << q_current[5] << std::endl;
std::cout << " L_WRIST_R: " << std::fixed << std::setprecision(6) << q_current[6] << std::endl;
std::cout << "\n右手臂关节角度（弧度）：" << std::endl;
std::cout << " R_SHOULDER_P: " << std::fixed << std::setprecision(6) << q_current[7] << std::endl;
std::cout << " R_SHOULDER_R: " << std::fixed << std::setprecision(6) << q_current[8] << std::endl;
std::cout << " R_SHOULDER_Y: " << std::fixed << std::setprecision(6) << q_current[9] << std::endl;
std::cout << " R_ELBOW_R: " << std::fixed << std::setprecision(6) << q_current[10] << std::endl;
std::cout << " R_WRIST_P: " << std::fixed << std::setprecision(6) << q_current[11] << std::endl;
std::cout << " R_WRIST_Y: " << std::fixed << std::setprecision(6) << q_current[12] << std::endl;
std::cout << " R_WRIST_R: " << std::fixed << std::setprecision(6) << q_current[13] << std::endl;
std::cout << "====================================================================================================\n" << std::endl;
joint_positions.push_back(q_next);
}
// 7. 计算每个点的关节速度
std::vector<Eigen::Vector<double, DOF>> joint_velocities;
joint_velocities.push_back(Eigen::Vector<double, DOF>::Zero()); // 起始速度为零
// 更新状态（与main一致，保证连续性）
m_state_->SetQ(q_current);
for (size_t i = 1; i < joint_positions.size(); ++i) {
double dt = time_points[i] - time_points[i-1];
Eigen::Vector<double, DOF> vel = (joint_positions[i] - joint_positions[i-1]) / dt;
joint_velocities.push_back(vel);
}
// 8. 打印轨迹信息
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 << "保持姿态不变" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" << std::endl;
// 9. 执行轨迹
auto loop_start_time = std::chrono::high_resolution_clock::now();
for (size_t i = 0; i < cartesian_trajectory.size(); ++i) {
// 获取当前点的关节位置和速度
Eigen::Vector<double, DOF> q_cmd = joint_positions[i];
Eigen::Vector<double, DOF> q_vel = joint_velocities[i];
// 更新状态
m_state_->SetQ(q_cmd);
m_robot_->ComputeForwardKinematics(m_state_);
// 发送关节命令
// 发送关节命令 - 为每个电机单独设置位置和速度
std::vector<JointPoint> joint_command;
for (size_t j = 0; j < DOF; ++j) {
JointPoint jp;
jp.joint_name = joint_names_[j];
jp.rad = q_current[j];
jp.vel = vel;
jp.rad = q_cmd[j];
jp.vel = std::abs(q_vel[j]); // 使用计算出的关节速度
joint_command.push_back(jp);
}
servoJ(joint_command, vel, CONTROL_PERIOD);
// 计算当前点应该执行的时间
double expected_time = time_points[i];
// servoJ(joint_command, vel, expected_time);
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);
}
motor->setQd(j.vel);
motor->setQ(j.rad);
}
}
// 检查中断
if (flash_cmd_.load()) {
@ -1214,18 +1620,21 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
return;
}
// 控制时间节奏
auto expected_time = loop_start_time + std::chrono::nanoseconds(
static_cast<long long>(i * CONTROL_PERIOD * 1e9)
// 控制时间节奏 - 使用精确的时间规划
auto expected_time_point = loop_start_time + std::chrono::nanoseconds(
static_cast<long long>(expected_time * 1e9)
);
auto now = std::chrono::high_resolution_clock::now();
if (now < expected_time) {
std::this_thread::sleep_until(expected_time);
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<std::chrono::milliseconds>(now - expected_time_point).count() << "ms";
}
}
// 最终状态更新
m_state_->SetQ(q_current);
m_state_->SetQ(joint_positions.back());
m_robot_->ComputeForwardKinematics(m_state_);
rsm_.store(ROBOT_READY);
LOG(INFO) << "moveL: Trajectory completed successfully";
@ -1238,7 +1647,99 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
}
// 辅助函数：计算运动时间
template<int DOF>
double HumanoidRobot<DOF>::calculateMoveTime(double distance, double vel, double acc) {
// 计算加速和减速所需的时间和距离
double acc_time = vel / acc;
double acc_distance = 0.5 * acc * acc_time * acc_time;
// 如果加速距离超过总距离的一半，需要调整最大速度
if (2 * acc_distance > distance) {
// 三角形速度曲线：加速然后直接减速
double max_reachable_vel = std::sqrt(acc * distance);
return 2 * max_reachable_vel / acc;
} else {
// 梯形速度曲线：加速-匀速-减速
double constant_distance = distance - 2 * acc_distance;
double constant_time = constant_distance / vel;
return 2 * acc_time + constant_time;
}
}
// 辅助函数：生成S曲线轨迹规划
template<int DOF>
void HumanoidRobot<DOF>::generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc,
double total_time, size_t num_points,
std::vector<double>& time_points,
std::vector<double>& distance_ratios) {
time_points.clear();
distance_ratios.clear();
// 计算加速和减速阶段的时间
double acc_time = max_vel / max_acc;
double acc_distance = 0.5 * max_acc * acc_time * acc_time;
// 确定实际的速度曲线形状
if (2 * acc_distance > total_distance) {
// 三角形速度曲线
double actual_max_vel = std::sqrt(max_acc * total_distance);
acc_time = actual_max_vel / max_acc;
acc_distance = 0.5 * max_acc * acc_time * acc_time;
double dec_time = acc_time;
// 生成时间点和距离比例
for (size_t i = 0; i <= num_points; ++i) {
double t = static_cast<double>(i) / num_points * total_time;
time_points.push_back(t);
if (t <= acc_time) {
// 加速阶段
double s = 0.5 * max_acc * t * t;
distance_ratios.push_back(s / total_distance);
} else {
// 减速阶段
double dec_start_time = total_time - dec_time;
double dec_elapsed = t - dec_start_time;
double s = acc_distance + actual_max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed;
distance_ratios.push_back(s / total_distance);
}
}
} else {
// 梯形速度曲线
double constant_time = (total_distance - 2 * acc_distance) / max_vel;
double dec_time = acc_time;
// 生成时间点和距离比例
for (size_t i = 0; i <= num_points; ++i) {
double t = static_cast<double>(i) / num_points * total_time;
time_points.push_back(t);
if (t <= acc_time) {
// 加速阶段
double s = 0.5 * max_acc * t * t;
distance_ratios.push_back(s / total_distance);
} else if (t <= acc_time + constant_time) {
// 匀速阶段
double s = acc_distance + max_vel * (t - acc_time);
distance_ratios.push_back(s / total_distance);
} else {
// 减速阶段
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;
distance_ratios.push_back(s / total_distance);
}
}
}
}
template class cmvr::device::HumanoidRobot<7>;
template class cmvr::device::HumanoidRobot<14>;
template class cmvr::device::HumanoidRobot<20>;

12
src/devices/robot/humanoid_robot/humanoid_robot.h
View File

@ -90,10 +90,20 @@ namespace cmvr::device{
int exec_CSP_cmd_(std::vector<int> &ids, std::vector<double> &pos, int dt);
int exec_CSV_cmd_(std::vector<int> &ids, std::vector<double> &vel, int dt);
int exec_CSC_cmd_(std::vector<int> &ids, std::vector<double> &cur, int dt);
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);
void generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc,
double total_time, size_t num_points,
std::vector<double>& time_points,
std::vector<double>& distance_ratios);
msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link);
private: