update
This commit is contained in:
parent
9a4f2ee6d4
commit
ee9a8022d0
@ -14,9 +14,9 @@
|
|||||||
<!-- <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
|
<!-- <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
|
||||||
<!-- <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
|
<!-- <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
|
||||||
<!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
|
<!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
|
||||||
<!-- <RealsenseCamera id="cam4" serial="243122075614" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
|
<!-- <RealsenseCamera id="cam4" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
|
||||||
<!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
|
<!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
|
||||||
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" codec="H265"/>-->
|
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
|
||||||
</Camera>
|
</Camera>
|
||||||
|
|
||||||
<DexHand>
|
<DexHand>
|
||||||
@ -36,19 +36,20 @@
|
|||||||
urdf="/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
|
urdf="/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
|
||||||
baseLink="PELVIS_S"
|
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"
|
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"
|
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,R_FINGER_TIP,R_CAM"
|
||||||
bufferSize="50">
|
bufferSize="50"
|
||||||
|
verbose="false">
|
||||||
<CanManger id="" devId="">
|
<CanManger id="" devId="">
|
||||||
<LeftArmCan id = " " devId = " " channelId ="0">
|
<LeftArmCan id = " " devId = " " channelId ="0">
|
||||||
<Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<!-- <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||||
<Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<!-- <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||||
<Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<!-- <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||||
<Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<!-- <Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||||
<Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<!-- <Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||||
<Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<!-- <Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||||
<Motor id="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<!-- <Motor id="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||||
</LeftArmCan>
|
</LeftArmCan>
|
||||||
<RightArmCan id = " " devId = " " channelId ="1">
|
<RightArmCan id = " " devId = " " channelId ="1" toolFrame="R_FINGER_TIP">
|
||||||
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||||
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||||
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||||
@ -68,25 +69,17 @@
|
|||||||
|
|
||||||
<BioHead>
|
<BioHead>
|
||||||
<!-- <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">-->
|
<!-- <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">-->
|
||||||
|
<!-- <!– 配置左眉毛，舵机通道 0~3 –>-->
|
||||||
|
<!-- <EyeBrow serial="64:0~3" offest="90 90 90 90"-->
|
||||||
|
<!-- jLmtUp="170 170 170 170" jLmtLow="10 10 10 10"/>-->
|
||||||
|
|
||||||
<!-- <!– 眉毛 –>-->
|
<!-- <!– 配置眼睛，舵机通道 4~9 –>-->
|
||||||
<!-- <EyeBrow serial="64:0~3"-->
|
<!-- <Eye serial="64:4~9" offest="90 90 90 90 90 90"-->
|
||||||
<!-- offest="90 90 90 90"-->
|
<!-- jLmtUp="170 170 170 170 170 170" jLmtLow="10 10 10 10 10 10"/>-->
|
||||||
<!-- jLmtUp="120 120 120 120"-->
|
|
||||||
<!-- jLmtLow="60 60 60 60"/>-->
|
|
||||||
|
|
||||||
<!-- <!– 眼睛 –>-->
|
|
||||||
<!-- <Eye serial="64:4~9"-->
|
|
||||||
<!-- offest="90 90 90 90 90 90"-->
|
|
||||||
<!-- jLmtUp="170 90 90 122 170 170"-->
|
|
||||||
<!-- jLmtLow="90 58 58 90 10 10"/>-->
|
|
||||||
|
|
||||||
<!-- <!– 嘴巴 –>-->
|
|
||||||
<!-- <Mouth serial="65:0~9"-->
|
|
||||||
<!-- offest="90 90 90 90 90 90 90 90 90 90"-->
|
|
||||||
<!-- jLmtUp="110 130 130 130 110 130 130 130 100 100"-->
|
|
||||||
<!-- jLmtLow="70 90 70 70 70 90 70 70 80 80"/>-->
|
|
||||||
|
|
||||||
|
<!-- <!– 配置嘴巴，舵机通道 0~8 –>-->
|
||||||
|
<!-- <Mouth serial="65:0~8" offest="90 90 90 90 90 90 90 90 90"-->
|
||||||
|
<!-- jLmtUp="170 170 170 170 170 170 170 170 170" jLmtLow="10 10 10 10 10 10 10 10 10"/>-->
|
||||||
<!-- </esp32>-->
|
<!-- </esp32>-->
|
||||||
</BioHead >
|
</BioHead >
|
||||||
|
|
||||||
@ -139,7 +132,7 @@
|
|||||||
|
|
||||||
</MonitorManager>
|
</MonitorManager>
|
||||||
|
|
||||||
<gRPCServer port="50051">
|
<gRPCServer port="50052">
|
||||||
|
|
||||||
</gRPCServer>
|
</gRPCServer>
|
||||||
|
|
||||||
|
|||||||
@ -550,268 +550,6 @@ 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>
|
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 {
|
try {
|
||||||
@ -1006,7 +744,6 @@ void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirectio
|
|||||||
if (rsm_.load() == ROBOT_RUNNING) {
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||||||
flash_cmd_.store(true);
|
flash_cmd_.store(true);
|
||||||
eStop();
|
eStop();
|
||||||
LOG(ERROR) << "机器人正在运行，停止当前运动";
|
|
||||||
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||||||
rsm_.store(ROBOT_RUNNING);
|
rsm_.store(ROBOT_RUNNING);
|
||||||
|
|
||||||
@ -1549,27 +1286,88 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
|
|||||||
template<int DOF>
|
template<int DOF>
|
||||||
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
|
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
|
||||||
try {
|
try {
|
||||||
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
|
// 1. 状态机检查：仅允许在 ESTOP/READY/TOROFF 状态启动
|
||||||
auto ok = check_joint_traj_(traj, dt);
|
if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY && rsm_.load() != ROBOT_TOROFF) {
|
||||||
if (!ok) { throw runtime_error("joint traj invalid"); }
|
throw runtime_error("followJointTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")");
|
||||||
|
}
|
||||||
|
|
||||||
rsm_.store(ROBOT_RUNNING);
|
// 2. 轨迹合法性检查
|
||||||
// TODO: need to optimize callback loop
|
if (!check_joint_traj_(traj, dt)) {
|
||||||
for (auto i = 0; i < traj.size(); i++) {
|
throw runtime_error("followJointTrajectory: invalid trajectory");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3. 初始化：切换电机模式为CSP，清空缓冲，更新状态机
|
||||||
|
std::lock_guard<std::mutex> exec_lock(exec_mtx_); // 防止多线程指令冲突
|
||||||
|
CSP_buffer_->clear(); // 清空CSP模式缓冲
|
||||||
|
rsm_.store(ROBOT_RUNNING);
|
||||||
|
|
||||||
|
// 3.1 预配置所有电机为CSP模式（避免轨迹执行中切换模式导致延迟）
|
||||||
|
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);
|
||||||
|
LOG(INFO) << "Motor " << motor->jointName() << " switched to CSP mode";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 4. 轨迹执行：使用定时器按dt间隔发送轨迹点
|
||||||
|
std::shared_ptr<FDTimer> traj_timer = std::make_shared<FDTimer>();
|
||||||
|
std::atomic<size_t> waypoint_idx(0); // 当前执行的轨迹点索引（原子变量防线程竞争）
|
||||||
|
std::atomic<bool> traj_completed(false); // 轨迹是否完成
|
||||||
|
|
||||||
|
// 4.1 定时器回调：发送当前轨迹点
|
||||||
|
traj_timer->start(
|
||||||
|
std::chrono::nanoseconds(static_cast<long long>(dt * 1e9)), // dt转换为纳秒
|
||||||
|
[this, &traj, &waypoint_idx, &traj_completed, traj_timer]() {
|
||||||
|
// 检查轨迹中断（外部指令触发）
|
||||||
if (flash_cmd_.load()) {
|
if (flash_cmd_.load()) {
|
||||||
flash_cmd_.store(false);
|
flash_cmd_.store(false);
|
||||||
LOG(INFO) << "followJointTrajectory is canceled";
|
traj_timer->stop();
|
||||||
|
traj_completed.store(true);
|
||||||
|
rsm_.store(ROBOT_ESTOP);
|
||||||
|
LOG(INFO) << "followJointTrajectory: interrupted by external command";
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
servoJ(traj[i], dt);
|
|
||||||
this_thread::sleep_for(chrono::milliseconds((int) dt));
|
// 检查轨迹是否完成
|
||||||
|
size_t current_idx = waypoint_idx.load();
|
||||||
|
if (current_idx >= traj.size()) {
|
||||||
|
traj_timer->stop();
|
||||||
|
traj_completed.store(true);
|
||||||
|
rsm_.store(ROBOT_ESTOP);
|
||||||
|
LOG(INFO) << "followJointTrajectory: trajectory completed";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 4.2 发送当前轨迹点的关节指令
|
||||||
|
const auto& current_waypoint = traj[current_idx];
|
||||||
|
for (const auto& joint : current_waypoint) {
|
||||||
|
auto motor = motor_manager_->getMotor(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";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 4.3 推进轨迹点索引
|
||||||
|
waypoint_idx.fetch_add(1);
|
||||||
}
|
}
|
||||||
rsm_.store(ROBOT_ESTOP);
|
);
|
||||||
} else {
|
|
||||||
throw runtime_error("rsm invalid");
|
// 4.4 等待轨迹完成或中断
|
||||||
|
while (!traj_completed.load()) {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用
|
||||||
}
|
}
|
||||||
} catch (exception &e) {
|
|
||||||
throw runtime_error(e.what());
|
} catch (std::exception &e) {
|
||||||
|
// 异常处理：停止轨迹，重置状态机
|
||||||
|
rsm_.store(ROBOT_ESTOP);
|
||||||
|
LOG(ERROR) << "followJointTrajectory failed: " << e.what();
|
||||||
|
throw runtime_error("followJointTrajectory error: " + std::string(e.what()));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1577,25 +1375,214 @@ template<int DOF>
|
|||||||
void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
|
void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
|
||||||
std::vector<std::vector<cmvr::ctrl::PoseTarget> > &targets, double dt) {
|
std::vector<std::vector<cmvr::ctrl::PoseTarget> > &targets, double dt) {
|
||||||
try {
|
try {
|
||||||
|
// 1. 基础校验：状态机与轨迹合法性
|
||||||
if (rsm_.load() == ROBOT_RUNNING) {
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||||||
flash_cmd_.store(true);
|
flash_cmd_.store(true);
|
||||||
eStop();
|
eStop(); // 中断当前运动
|
||||||
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
throw runtime_error("followPoseTrajectory: robot is running, interrupted");
|
||||||
rsm_.store(ROBOT_RUNNING);
|
|
||||||
// TODO:
|
|
||||||
// 1. set Timer(dt)
|
|
||||||
// 2. for each timestamp, use Cyclic Synchronous Position (CSP) Mode to set joint position
|
|
||||||
// 3. if flash_cmd_ is set, set it to false and exit
|
|
||||||
// 3. join timer
|
|
||||||
rsm_.store(ROBOT_READY);
|
|
||||||
} else {
|
|
||||||
throw runtime_error("rsm invalid");
|
|
||||||
}
|
}
|
||||||
} catch (exception &e) {
|
if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY) {
|
||||||
throw runtime_error(e.what());
|
throw runtime_error("followPoseTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")");
|
||||||
|
}
|
||||||
|
if (targets.empty()) {
|
||||||
|
throw runtime_error("followPoseTrajectory: pose trajectory is empty");
|
||||||
|
}
|
||||||
|
if (dt <= 0 || dt > 0.1) {
|
||||||
|
throw runtime_error("followPoseTrajectory: invalid dt=" + std::to_string(dt) + " (must be 0 < dt ≤ 0.1)");
|
||||||
|
}
|
||||||
|
// // 检查基座链接有效性（依赖机器人模型接口）
|
||||||
|
// int base_link_idx = m_robot_->GetLinkIdx(base_link);
|
||||||
|
// if (base_link_idx == -1) {
|
||||||
|
// throw runtime_error("followPoseTrajectory: invalid base link: " + base_link);
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
// 2. 关键步骤1：获取当前关节状态，解算“轨迹第一个点”的关节配置（作为后续IK基准）
|
||||||
|
std::lock_guard<std::mutex> exec_lock(exec_mtx_);
|
||||||
|
CSP_buffer_->clear(); // 清空CSP缓冲，避免指令冲突
|
||||||
|
|
||||||
|
// 2.1 获取当前关节角度（初始化机器人状态）
|
||||||
|
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"];
|
||||||
|
m_state_->SetQ(q_current);
|
||||||
|
m_robot_->ComputeForwardKinematics(m_state_); // 更新当前正运动学状态
|
||||||
|
|
||||||
|
|
||||||
|
// 2.2 解算“轨迹第一个点”的关节配置（q_first，作为后续所有IK的初始值）
|
||||||
|
const auto& first_pose_targets = targets[0]; // 轨迹第一个点的位姿目标
|
||||||
|
Eigen::Vector<double, DOF> q_first; // 轨迹第一个点的关节配置（IK基准）
|
||||||
|
bool ik_first_ok = m_cctrl_->compute(
|
||||||
|
m_state_, // 当前机器人状态（作为IK初始值）
|
||||||
|
base_link, // 基座链接
|
||||||
|
first_pose_targets, // 第一个点的位姿目标
|
||||||
|
dt, // 控制周期（用于速度限制）
|
||||||
|
ctrl::CartesianController<DOF>::Mode::Position, // 位置控制模式
|
||||||
|
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)");
|
||||||
|
}
|
||||||
|
LOG(INFO) << "followPoseTrajectory: first waypoint IK solved successfully, q_first=" << q_first.transpose();
|
||||||
|
|
||||||
|
|
||||||
|
// 3. 关键步骤2：从当前位置移动到“轨迹第一个点”（过渡运动）
|
||||||
|
rsm_.store(ROBOT_RUNNING); // 切换状态为运行中
|
||||||
|
LOG(INFO) << "followPoseTrajectory: moving from current position to first waypoint...";
|
||||||
|
|
||||||
|
// 3.1 配置电机为CSP模式（用于过渡运动和后续轨迹）
|
||||||
|
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);
|
||||||
|
LOG(INFO) << "followPoseTrajectory: motor " << motor->jointName() << " switched to CSP mode";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3.2 执行“当前→第一个点”的过渡运动（匀速逼近，避免冲击）
|
||||||
|
const double TRANSITION_VEL = 0.5; // 过渡运动速度（1rad/s，可根据需求调整）
|
||||||
|
bool transition_completed = false;
|
||||||
|
auto transition_start_time = std::chrono::high_resolution_clock::now();
|
||||||
|
|
||||||
|
while (!transition_completed && !flash_cmd_.load()) {
|
||||||
|
// 3.2.1 计算当前应到达的关节位置（匀速插值）
|
||||||
|
auto now = std::chrono::high_resolution_clock::now();
|
||||||
|
double elapsed = std::chrono::duration<double>(now - transition_start_time).count();
|
||||||
|
Eigen::Vector<double, DOF> 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];
|
||||||
|
auto motor = motor_manager_->getMotor(joint_name);
|
||||||
|
if (motor) {
|
||||||
|
motor->setQd(TRANSITION_VEL); // 过渡运动速度
|
||||||
|
motor->setQ(q_transition[i]); // 当前过渡位置
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3.2.3 检查过渡运动是否完成（所有关节到达目标）
|
||||||
|
transition_completed = true;
|
||||||
|
for (size_t i = 0; i < DOF; ++i) {
|
||||||
|
const std::string& joint_name = joint_names_[i];
|
||||||
|
auto motor = motor_manager_->getMotor(joint_name);
|
||||||
|
if (motor && !motor->reachedTargetQ()) { // 精度阈值：0.0001rad（≈0.0057°）
|
||||||
|
transition_completed = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3.2.4 控制过渡运动频率（与后续轨迹一致）
|
||||||
|
std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast<long long>(dt * 1e9)));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3.2.5 过渡运动中断处理
|
||||||
|
if (flash_cmd_.load()) {
|
||||||
|
flash_cmd_.store(false);
|
||||||
|
rsm_.store(ROBOT_ESTOP);
|
||||||
|
throw runtime_error("followPoseTrajectory: transition to first waypoint interrupted");
|
||||||
|
}
|
||||||
|
LOG(INFO) << "followPoseTrajectory: reached first waypoint, start trajectory execution";
|
||||||
|
|
||||||
|
|
||||||
|
// 4. 关键步骤3：执行轨迹（所有点的IK均以q_first为初始值）
|
||||||
|
std::shared_ptr<FDTimer> traj_timer = std::make_shared<FDTimer>();
|
||||||
|
std::atomic<size_t> waypoint_idx(0); // 当前执行的轨迹点索引（从0开始，即第一个点）
|
||||||
|
std::atomic<bool> traj_completed(false); // 轨迹是否完成
|
||||||
|
Eigen::Vector<double, DOF> last_valid_q = q_first; // 上一次有效的关节配置（容错用）
|
||||||
|
|
||||||
|
// 4.1 初始化机器人状态为第一个点（确保轨迹起始状态正确）
|
||||||
|
m_state_->SetQ(q_first);
|
||||||
|
m_robot_->ComputeForwardKinematics(m_state_);
|
||||||
|
|
||||||
|
// 4.2 定时器回调：按dt间隔解算IK并发送指令（IK初始值固定为q_first）
|
||||||
|
traj_timer->start(
|
||||||
|
std::chrono::nanoseconds(static_cast<long long>(dt * 1e9)), // 定时器周期=控制周期dt
|
||||||
|
[this, &base_link, &targets, &waypoint_idx, &traj_completed, &last_valid_q, &q_first, traj_timer, dt]() {
|
||||||
|
// 4.2.1 检查外部中断
|
||||||
|
if (flash_cmd_.load()) {
|
||||||
|
flash_cmd_.store(false);
|
||||||
|
traj_timer->stop();
|
||||||
|
traj_completed.store(true);
|
||||||
|
rsm_.store(ROBOT_ESTOP);
|
||||||
|
LOG(INFO) << "followPoseTrajectory: trajectory interrupted by external command";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 4.2.2 检查轨迹是否完成
|
||||||
|
size_t current_idx = waypoint_idx.load();
|
||||||
|
if (current_idx >= targets.size()) {
|
||||||
|
traj_timer->stop();
|
||||||
|
traj_completed.store(true);
|
||||||
|
rsm_.store(ROBOT_ESTOP);
|
||||||
|
LOG(INFO) << "followPoseTrajectory: trajectory executed completely";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 4.2.3 解算当前轨迹点的IK（关键：初始值固定为q_first）
|
||||||
|
const auto& current_pose_targets = targets[current_idx];
|
||||||
|
Eigen::Vector<double, DOF> q_cmd; // 当前点的关节目标
|
||||||
|
|
||||||
|
// 临时更新机器人状态为q_first（确保IK初始值固定）
|
||||||
|
m_state_->SetQ(q_first);
|
||||||
|
m_robot_->ComputeForwardKinematics(m_state_);
|
||||||
|
|
||||||
|
bool ik_ok = m_cctrl_->compute(
|
||||||
|
m_state_, // IK初始值：固定为q_first
|
||||||
|
base_link, // 基座链接
|
||||||
|
current_pose_targets, // 当前点的位姿目标
|
||||||
|
dt, // 控制周期
|
||||||
|
ctrl::CartesianController<DOF>::Mode::Position,
|
||||||
|
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() << ")";
|
||||||
|
q_cmd = last_valid_q;
|
||||||
|
} else {
|
||||||
|
last_valid_q = q_cmd; // 更新有效配置
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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];
|
||||||
|
auto motor = motor_manager_->getMotor(joint_name);
|
||||||
|
if (motor) {
|
||||||
|
motor->setQd(TRAJ_VEL); // 轨迹执行速度
|
||||||
|
motor->setQ(q_cmd[i]); // 关节目标位置
|
||||||
|
LOG(INFO) << "followPoseTrajectory: waypoint " << current_idx
|
||||||
|
<< ", joint " << joint_name << " -> pos=" << q_cmd[i] << " rad";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 4.2.6 推进轨迹点索引
|
||||||
|
waypoint_idx.fetch_add(1);
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
// 4.3 等待轨迹执行完成
|
||||||
|
while (!traj_completed.load()) {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用
|
||||||
|
}
|
||||||
|
|
||||||
|
} catch (std::exception &e) {
|
||||||
|
// 异常处理：重置状态机，确保机器人安全
|
||||||
|
rsm_.store(ROBOT_ESTOP);
|
||||||
|
LOG(ERROR) << "followPoseTrajectory failed: " << e.what();
|
||||||
|
throw runtime_error("followPoseTrajectory error: " + std::string(e.what()));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
template<int DOF>
|
template<int DOF>
|
||||||
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
|
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
|
||||||
for (const auto &j: joints) {
|
for (const auto &j: joints) {
|
||||||
@ -1944,7 +1931,7 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
|
|||||||
}
|
}
|
||||||
|
|
||||||
try {
|
try {
|
||||||
const double CONTROL_PERIOD = 1.0 / 100.0; // 控制周期保持不变
|
const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变
|
||||||
msgs::Pose3d current_pose = fk(base_link, ee_link);
|
msgs::Pose3d current_pose = fk(base_link, ee_link);
|
||||||
|
|
||||||
// 1. 初始化当前和目标位姿矩阵
|
// 1. 初始化当前和目标位姿矩阵
|
||||||
@ -2032,26 +2019,6 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
|
|||||||
|
|
||||||
cartesian_trajectory.push_back(T_interp);
|
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. 预先计算所有轨迹点的关节位置
|
// 6. 预先计算所有轨迹点的关节位置
|
||||||
std::vector<Eigen::Vector<double, DOF>> joint_positions;
|
std::vector<Eigen::Vector<double, DOF>> joint_positions;
|
||||||
@ -2264,7 +2231,33 @@ void HumanoidRobot<DOF>::generateSTrapezoidalProfile(double total_distance, doub
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template<int DOF>
|
||||||
|
cmvr::math::Pose3d HumanoidRobot<DOF>::getTransform(std::string &base_link, std::string &target_link)
|
||||||
|
{
|
||||||
|
// 获取gRPC生成的Pose3d消息
|
||||||
|
auto grpc_pose = fk(base_link, target_link);
|
||||||
|
|
||||||
|
// 转换为cmvr::math::Pose3d
|
||||||
|
cmvr::math::Pose3d math_pose;
|
||||||
|
|
||||||
|
// 转换位置信息
|
||||||
|
math_pose.position.x = grpc_pose.position().x();
|
||||||
|
math_pose.position.y = grpc_pose.position().y();
|
||||||
|
math_pose.position.z = grpc_pose.position().z();
|
||||||
|
|
||||||
|
// 转换四元数
|
||||||
|
math_pose.quaternion.w = grpc_pose.quaternion().w();
|
||||||
|
math_pose.quaternion.x = grpc_pose.quaternion().x();
|
||||||
|
math_pose.quaternion.y = grpc_pose.quaternion().y();
|
||||||
|
math_pose.quaternion.z = grpc_pose.quaternion().z();
|
||||||
|
|
||||||
|
// 转换欧拉角
|
||||||
|
math_pose.euler.rx = grpc_pose.euler().rx();
|
||||||
|
math_pose.euler.ry = grpc_pose.euler().ry();
|
||||||
|
math_pose.euler.rz = grpc_pose.euler().rz();
|
||||||
|
|
||||||
|
return math_pose;
|
||||||
|
}
|
||||||
|
|
||||||
template class cmvr::device::HumanoidRobot<7>;
|
template class cmvr::device::HumanoidRobot<7>;
|
||||||
template class cmvr::device::HumanoidRobot<14>;
|
template class cmvr::device::HumanoidRobot<14>;
|
||||||
|
|||||||
@ -63,7 +63,7 @@ namespace cmvr::device{
|
|||||||
void getJointQ(std::unordered_map<std::string,double> &joint_qs) const override;
|
void getJointQ(std::unordered_map<std::string,double> &joint_qs) const override;
|
||||||
|
|
||||||
void getState(RobotState &state) override;
|
void getState(RobotState &state) override;
|
||||||
math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}
|
math::Pose3d getTransform(std::string &bask_link, std::string &target_link) override;
|
||||||
|
|
||||||
/* torque on and off */
|
/* torque on and off */
|
||||||
void torqueOn() override;
|
void torqueOn() override;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user