update moveL
This commit is contained in:
parent
fb3d6622f4
commit
25ec465e0b
@ -77,6 +77,9 @@ namespace cmvr::device{
|
|||||||
|
|
||||||
virtual void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
virtual void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
|
||||||
|
virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
|
virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
|
virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|||||||
@ -929,6 +929,316 @@ cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(const std::string &base_link, const st
|
|||||||
return pose;
|
return pose;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void printTrajectoryInfo(
|
||||||
|
const std::vector<Eigen::Matrix4d>& trajectory,
|
||||||
|
const std::vector<double>& times,
|
||||||
|
const std::vector<double>& velocities,
|
||||||
|
double total_distance) {
|
||||||
|
|
||||||
|
std::cout << "\n===================================== 轨迹详细信息 =====================================" << std::endl;
|
||||||
|
std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl;
|
||||||
|
std::cout << "总运动时间: " << std::fixed << std::setprecision(3) << times.back() << "s" << std::endl;
|
||||||
|
std::cout << "轨迹点总数: " << trajectory.size() << " 个" << std::endl;
|
||||||
|
std::cout << "-----------------------------------------------------------------------------------------" << std::endl;
|
||||||
|
std::cout << std::setw(4) << "序号" << " | "
|
||||||
|
<< std::setw(8) << "时间(s)" << " | "
|
||||||
|
<< std::setw(10) << "x(m)" << " | "
|
||||||
|
<< std::setw(10) << "y(m)" << " | "
|
||||||
|
<< std::setw(10) << "z(m)" << " | "
|
||||||
|
<< std::setw(12) << "速度(m/s)" << " | "
|
||||||
|
<< std::setw(16) << "到起点距离(m)" << std::endl;
|
||||||
|
std::cout << "-----------------------------------------------------------------------------------------" << std::endl;
|
||||||
|
|
||||||
|
Eigen::Vector3d start_pos(trajectory[0](0,3), trajectory[0](1,3), trajectory[0](2,3));
|
||||||
|
for (size_t idx = 0; idx < trajectory.size(); ++idx) {
|
||||||
|
const auto& T = trajectory[idx];
|
||||||
|
Eigen::Vector3d pos(T(0,3), T(1,3), T(2,3));
|
||||||
|
double dist_from_start = (pos - start_pos).norm();
|
||||||
|
|
||||||
|
std::cout << std::setw(4) << idx << " | "
|
||||||
|
<< std::fixed << std::setprecision(3) << std::setw(8) << times[idx] << " | "
|
||||||
|
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.x() << " | "
|
||||||
|
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.y() << " | "
|
||||||
|
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.z() << " | "
|
||||||
|
<< std::fixed << std::setprecision(6) << std::setw(12) << velocities[idx] << " | "
|
||||||
|
<< std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl;
|
||||||
|
}
|
||||||
|
std::cout << "=========================================================================================\n" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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"];
|
||||||
|
|
||||||
|
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();
|
||||||
|
|
||||||
|
// 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());
|
||||||
|
|
||||||
|
// 3. 调用moveL执行直线运动到目标位姿
|
||||||
|
moveL(base_link, ee_link, target_pose, vel, acc);
|
||||||
|
|
||||||
|
} catch (const std::exception &e) {
|
||||||
|
LOG(ERROR) << "moveDeltaL failed: " << e.what();
|
||||||
|
throw std::runtime_error(std::string("moveDeltaL error: ") + e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
template<int DOF>
|
||||||
|
void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &ee_link,
|
||||||
|
msgs::Pose3d target_pose, double vel, double acc) {
|
||||||
|
if (vel <= 0 || acc <= 0) {
|
||||||
|
throw std::runtime_error("moveL: vel and acc must be positive");
|
||||||
|
}
|
||||||
|
|
||||||
|
try {
|
||||||
|
const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变
|
||||||
|
msgs::Pose3d current_pose = fk(base_link, ee_link);
|
||||||
|
|
||||||
|
// 1. 初始化当前和目标位姿矩阵
|
||||||
|
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::Matrix4d T_target = Eigen::Matrix4d::Identity();
|
||||||
|
T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
|
||||||
|
target_pose.euler().rx(), target_pose.euler().ry(), target_pose.euler().rz()
|
||||||
|
);
|
||||||
|
T_target(0, 3) = target_pose.position().x();
|
||||||
|
T_target(1, 3) = target_pose.position().y();
|
||||||
|
T_target(2, 3) = target_pose.position().z();
|
||||||
|
|
||||||
|
// 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"];
|
||||||
|
|
||||||
|
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.w_posrot = 0.5;
|
||||||
|
target_ik_check.weight = 1.0;
|
||||||
|
target_ik_check.link_name = ee_link;
|
||||||
|
|
||||||
|
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中的高迭代次数
|
||||||
|
if (!ik_solvable) {
|
||||||
|
throw std::runtime_error("moveL: Target pose is unreachable");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3. 计算位置差值（与main一致，保持姿态不变）
|
||||||
|
Eigen::Vector3d delta_pos = T_target.block<3, 1>(0, 3) - T_current.block<3, 1>(0, 3);
|
||||||
|
double total_distance = delta_pos.norm();
|
||||||
|
|
||||||
|
if (total_distance < 1e-6) {
|
||||||
|
LOG(INFO) << "moveL: Target is already reached";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 4. 基于路径长度的均匀插值（核心修改点）
|
||||||
|
// 计算所需的插值点数（根据速度和控制周期计算）
|
||||||
|
double move_time = total_distance / vel; // 总移动时间
|
||||||
|
size_t num_points = std::max(2ul, static_cast<size_t>(ceil(move_time / CONTROL_PERIOD)));
|
||||||
|
double step_distance = total_distance / num_points; // 每个点的距离间隔
|
||||||
|
|
||||||
|
LOG(INFO) << "moveL: Planning trajectory - points=" << num_points
|
||||||
|
<< ", total distance=" << total_distance << "m, move time=" << move_time << "s";
|
||||||
|
|
||||||
|
// 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）
|
||||||
|
|
||||||
|
Eigen::Matrix4d T_interp = T_current; // 复制起点姿态（保持不变）
|
||||||
|
// 仅位置按比例插值
|
||||||
|
T_interp(0, 3) = T_current(0, 3) + s * delta_pos.x();
|
||||||
|
T_interp(1, 3) = T_current(1, 3) + s * delta_pos.y();
|
||||||
|
T_interp(2, 3) = T_current(2, 3) + s * delta_pos.z();
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
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) {
|
||||||
|
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求解参数（使用main中的高迭代次数）
|
||||||
|
int max_iter = 10000;
|
||||||
|
double tolerance = 1e-6;
|
||||||
|
|
||||||
|
// 求解IK，以上一个状态作为初始值（与main一致）
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
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");
|
||||||
|
}
|
||||||
|
// 右臂第二个关节不能超过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;
|
||||||
|
|
||||||
|
|
||||||
|
// 更新状态（与main一致，保证连续性）
|
||||||
|
m_state_->SetQ(q_current);
|
||||||
|
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;
|
||||||
|
joint_command.push_back(jp);
|
||||||
|
}
|
||||||
|
servoJ(joint_command, vel, CONTROL_PERIOD);
|
||||||
|
|
||||||
|
// 检查中断
|
||||||
|
if (flash_cmd_.load()) {
|
||||||
|
flash_cmd_.store(false);
|
||||||
|
LOG(INFO) << "moveL: Interrupted by external command";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 控制时间节奏
|
||||||
|
auto expected_time = loop_start_time + std::chrono::nanoseconds(
|
||||||
|
static_cast<long long>(i * CONTROL_PERIOD * 1e9)
|
||||||
|
);
|
||||||
|
auto now = std::chrono::high_resolution_clock::now();
|
||||||
|
if (now < expected_time) {
|
||||||
|
std::this_thread::sleep_until(expected_time);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 最终状态更新
|
||||||
|
m_state_->SetQ(q_current);
|
||||||
|
m_robot_->ComputeForwardKinematics(m_state_);
|
||||||
|
rsm_.store(ROBOT_READY);
|
||||||
|
LOG(INFO) << "moveL: Trajectory completed successfully";
|
||||||
|
|
||||||
|
} catch (const std::exception &e) {
|
||||||
|
LOG(ERROR) << "moveL failed: " << e.what();
|
||||||
|
rsm_.store(ROBOT_ERROR);
|
||||||
|
throw std::runtime_error(std::string("moveL error: ") + e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
template class cmvr::device::HumanoidRobot<7>;
|
template class cmvr::device::HumanoidRobot<7>;
|
||||||
template class cmvr::device::HumanoidRobot<14>;
|
template class cmvr::device::HumanoidRobot<14>;
|
||||||
template class cmvr::device::HumanoidRobot<20>;
|
template class cmvr::device::HumanoidRobot<20>;
|
||||||
|
|||||||
@ -63,7 +63,12 @@ namespace cmvr::device{
|
|||||||
void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override;
|
void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override;
|
||||||
void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override ;
|
void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override ;
|
||||||
void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;
|
void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;
|
||||||
|
|
||||||
void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override;
|
void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override;
|
||||||
|
void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) override;
|
||||||
|
void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) override;
|
||||||
|
|
||||||
|
|
||||||
void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override;
|
void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override;
|
||||||
void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) override;
|
void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) override;
|
||||||
|
|
||||||
|
|||||||
@ -19,7 +19,7 @@ DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot confi
|
|||||||
using namespace cmvr::device;
|
using namespace cmvr::device;
|
||||||
TEST(HumanoidRobotTest,MyRobotTest) {
|
TEST(HumanoidRobotTest,MyRobotTest) {
|
||||||
|
|
||||||
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
|
||||||
const XmlNode config(config_path);
|
const XmlNode config(config_path);
|
||||||
|
|
||||||
if (!config.hasChild("DeviceManager")){
|
if (!config.hasChild("DeviceManager")){
|
||||||
@ -47,7 +47,7 @@ TEST(HumanoidRobotTest,MyRobotTest) {
|
|||||||
// {"L_WRIST_Y", 0.0},
|
// {"L_WRIST_Y", 0.0},
|
||||||
// {"L_WRIST_R", 0.0},
|
// {"L_WRIST_R", 0.0},
|
||||||
//
|
//
|
||||||
// {"R_SHOULDER_P", 0.0},
|
// {"R_SHOULDER_P", 0.0}
|
||||||
// {"R_SHOULDER_R", 0.0},
|
// {"R_SHOULDER_R", 0.0},
|
||||||
// {"R_SHOULDER_Y", 0.0},
|
// {"R_SHOULDER_Y", 0.0},
|
||||||
// {"R_ELBOW_R", 0.0},
|
// {"R_ELBOW_R", 0.0},
|
||||||
@ -430,4 +430,45 @@ TEST(HumanoidRobotTest,MoveIKTest) {
|
|||||||
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
||||||
// }
|
// }
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(HumanoidRobotTest,MoveLTest) {
|
||||||
|
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<AbstractRobot>("hc01");
|
||||||
|
|
||||||
|
|
||||||
|
try {
|
||||||
|
// 定义基座和末端链路名（需与URDF模型中的链路名一致）
|
||||||
|
std::string base_link = "PELVIS_S"; // 基座坐标系
|
||||||
|
std::string ee_link = "R_WRIST_R_S"; // 末端执行器坐标系
|
||||||
|
|
||||||
|
// 定义目标位姿（直线运动的终点与当前的差值）
|
||||||
|
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_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执行直线运动
|
||||||
|
robot->moveDeltaL(base_link, ee_link, delta_pose, vel, acc);
|
||||||
|
LOG(INFO) << "直线运动完成！";
|
||||||
|
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
// 捕获异常（如IK解算失败、状态非法等）
|
||||||
|
LOG(ERROR) << "moveL调用失败：" << e.what();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
Loading…
Reference in New Issue
Block a user