test:movej
This commit is contained in:
parent
119e29e22d
commit
aeae44b8a4
@ -173,138 +173,137 @@ void HumanoidRobot<DOF>::eStop() {
|
||||
}
|
||||
}
|
||||
|
||||
// template<int DOF>
|
||||
// void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
|
||||
// try {
|
||||
// for (const auto &j: cmd) {
|
||||
// auto motor = motor_manager_->getMotor(j.joint_name);
|
||||
// if (motor != nullptr) {
|
||||
// // PPM 模式下 这个实际速度会超30% 左右
|
||||
// motor->setQd(vel);
|
||||
//
|
||||
// if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
|
||||
// motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
|
||||
// }
|
||||
// motor->setQ(j.rad);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// //3. wait for completion
|
||||
// bool completion = true;
|
||||
// do {
|
||||
// completion = true;
|
||||
// for (const auto &j: cmd) {
|
||||
// auto motor = motor_manager_->getMotor(j.joint_name);
|
||||
// if (motor != nullptr) {
|
||||
// if (!motor->reachedTargetQ()) {
|
||||
// completion = false;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// // 4. while waiting, check flash_cmd_, if it is true, set it false then exit
|
||||
// if (flash_cmd_.load()) {
|
||||
// flash_cmd_.store(false);
|
||||
// return;
|
||||
// }
|
||||
// std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||||
// } while (!completion);
|
||||
// } catch (exception &e) {
|
||||
// throw runtime_error(e.what());
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
template<int DOF>
|
||||
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
|
||||
if (cmd.empty()) return;
|
||||
try {
|
||||
for (const auto &j: cmd) {
|
||||
auto motor = motor_manager_->getMotor(j.joint_name);
|
||||
if (motor != nullptr) {
|
||||
// PPM 模式下 这个实际速度会超30% 左右
|
||||
motor->setQd(vel);
|
||||
|
||||
std::unordered_map<std::string, double> cur_joints_angle;
|
||||
cur_joints_angle.reserve(cmd.size());
|
||||
|
||||
std::vector<std::shared_ptr<AbstractMotor> > motors;
|
||||
motors.reserve(cmd.size());
|
||||
|
||||
for (auto &jp: cmd) {
|
||||
auto motor = motor_manager_->getMotor(jp.joint_name);
|
||||
motors.push_back(motor);
|
||||
// 切 CSP
|
||||
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||||
}
|
||||
// 为 map 预填 key
|
||||
cur_joints_angle.emplace(jp.joint_name, 0.0);
|
||||
}
|
||||
|
||||
getJointQ(cur_joints_angle);
|
||||
|
||||
std::vector<double> q_start, q_goal;
|
||||
q_start.reserve(cmd.size());
|
||||
q_goal.reserve(cmd.size());
|
||||
for (auto &jp: cmd) {
|
||||
auto it = cur_joints_angle.find(jp.joint_name);
|
||||
if (it == cur_joints_angle.end()) {
|
||||
LOG(ERROR) << "missing current angle for " << jp.joint_name;
|
||||
return;
|
||||
}
|
||||
q_start.push_back(it->second); // 当前读到的弧度
|
||||
q_goal.push_back(jp.rad); // 目标弧度
|
||||
}
|
||||
if (q_start.size() != q_goal.size()) return;
|
||||
|
||||
|
||||
TrajPtr traj;
|
||||
joint_space_planner_->setPathType(PathType::Quintic);
|
||||
joint_space_planner_->setSymmetricLimits(std::vector<double>(cmd.size(), vel),
|
||||
std::vector<double>(cmd.size(), acc));
|
||||
|
||||
|
||||
bool ok = joint_space_planner_->plan(q_start, q_goal, traj);
|
||||
if (!ok) {
|
||||
LOG(ERROR) << "planner plan() failed";
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// 10ms
|
||||
const double dt = 0.01;
|
||||
auto samples = joint_space_planner_->sampleTrajectory(traj, dt);
|
||||
if (samples.size() < 2) {
|
||||
LOG(ERROR) << "trajectory too short";
|
||||
return;
|
||||
}
|
||||
|
||||
const auto t0 = std::chrono::steady_clock::now();
|
||||
|
||||
// k=1 开始
|
||||
size_t k = 0;
|
||||
const size_t K = samples.size();
|
||||
|
||||
while (k < K) {
|
||||
// ——根据墙钟计算“应当在的拍次”，用于追赶——
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
double elapsed = std::chrono::duration<double>(now - t0).count();
|
||||
size_t k_should = static_cast<size_t>(std::floor(elapsed / dt)); // 向下取整更保守
|
||||
if (k_should >= K) k_should = K - 1;
|
||||
if (k < k_should) k = k_should; // 追赶，避免越拖越晚
|
||||
|
||||
// ——下发第 k 个采样点——
|
||||
const auto &s = samples[k];
|
||||
// 这里最好在进入循环前做过维度检查：s.q/qd 与 motors.size() 相等
|
||||
for (size_t i = 0; i < motors.size(); ++i) {
|
||||
motors[i]->setQd(s.qd[i]);
|
||||
motors[i]->setQ(s.q[i]);
|
||||
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
|
||||
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
|
||||
}
|
||||
motor->setQ(j.rad);
|
||||
}
|
||||
}
|
||||
|
||||
// ——睡到下一拍 (k+1)*dt——
|
||||
++k; // 先递增，下一拍时间点明确
|
||||
if (k < K) {
|
||||
auto next_t = t0 + std::chrono::duration<double>(k * dt);
|
||||
std::this_thread::sleep_until(next_t);
|
||||
}
|
||||
//3. wait for completion
|
||||
bool completion = true;
|
||||
do {
|
||||
completion = true;
|
||||
for (const auto &j: cmd) {
|
||||
auto motor = motor_manager_->getMotor(j.joint_name);
|
||||
if (motor != nullptr) {
|
||||
if (!motor->reachedTargetQ()) {
|
||||
completion = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
// 4. while waiting, check flash_cmd_, if it is true, set it false then exit
|
||||
if (flash_cmd_.load()) {
|
||||
flash_cmd_.store(false);
|
||||
return;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||||
} while (!completion);
|
||||
} catch (exception &e) {
|
||||
throw runtime_error(e.what());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// template<int DOF>
|
||||
// void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
|
||||
// if (cmd.empty()) return;
|
||||
//
|
||||
// std::unordered_map<std::string, double> cur_joints_angle;
|
||||
// cur_joints_angle.reserve(cmd.size());
|
||||
//
|
||||
// std::vector<std::shared_ptr<AbstractMotor> > motors;
|
||||
// motors.reserve(cmd.size());
|
||||
//
|
||||
// for (auto &jp: cmd) {
|
||||
// auto motor = motor_manager_->getMotor(jp.joint_name);
|
||||
// motors.push_back(motor);
|
||||
// // 切 CSP
|
||||
// if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||||
// motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||||
// }
|
||||
// // 为 map 预填 key
|
||||
// cur_joints_angle.emplace(jp.joint_name, 0.0);
|
||||
// }
|
||||
//
|
||||
// getJointQ(cur_joints_angle);
|
||||
//
|
||||
// std::vector<double> q_start, q_goal;
|
||||
// q_start.reserve(cmd.size());
|
||||
// q_goal.reserve(cmd.size());
|
||||
// for (auto &jp: cmd) {
|
||||
// auto it = cur_joints_angle.find(jp.joint_name);
|
||||
// if (it == cur_joints_angle.end()) {
|
||||
// LOG(ERROR) << "missing current angle for " << jp.joint_name;
|
||||
// return;
|
||||
// }
|
||||
// q_start.push_back(it->second); // 当前读到的弧度
|
||||
// q_goal.push_back(jp.rad); // 目标弧度
|
||||
// }
|
||||
// if (q_start.size() != q_goal.size()) return;
|
||||
//
|
||||
//
|
||||
// TrajPtr traj;
|
||||
// joint_space_planner_->setPathType(PathType::Quintic);
|
||||
// joint_space_planner_->setSymmetricLimits(std::vector<double>(cmd.size(), vel),
|
||||
// std::vector<double>(cmd.size(), acc));
|
||||
//
|
||||
//
|
||||
// bool ok = joint_space_planner_->plan(q_start, q_goal, traj);
|
||||
// if (!ok) {
|
||||
// LOG(ERROR) << "planner plan() failed";
|
||||
// return;
|
||||
// }
|
||||
//
|
||||
//
|
||||
// // 10ms
|
||||
// const double dt = 0.01;
|
||||
// auto samples = joint_space_planner_->sampleTrajectory(traj, dt);
|
||||
// if (samples.size() < 2) {
|
||||
// LOG(ERROR) << "trajectory too short";
|
||||
// return;
|
||||
// }
|
||||
//
|
||||
// const auto t0 = std::chrono::steady_clock::now();
|
||||
//
|
||||
// // k=0 开始
|
||||
// size_t k = 0;
|
||||
// const size_t K = samples.size();
|
||||
//
|
||||
// while (k < K) {
|
||||
// // // ——根据墙钟计算“应当在的拍次”，用于追赶——
|
||||
// // auto now = std::chrono::steady_clock::now();
|
||||
// // double elapsed = std::chrono::duration<double>(now - t0).count();
|
||||
// // size_t k_should = static_cast<size_t>(std::floor(elapsed / dt)); // 向下取整更保守
|
||||
// // if (k_should >= K) k_should = K - 1;
|
||||
// // if (k < k_should) k = k_should; // 追赶，避免越拖越晚
|
||||
//
|
||||
// // ——下发第 k 个采样点——
|
||||
// const auto &s = samples[k];
|
||||
//
|
||||
// for (size_t i = 0; i < motors.size(); ++i) {
|
||||
// motors[i]->setQd(s.qd[i]);
|
||||
// motors[i]->setQ(s.q[i]);
|
||||
// }
|
||||
//
|
||||
// ++k;
|
||||
// if (k < K) {
|
||||
// auto next_t = t0 + std::chrono::duration<double>(k * dt);
|
||||
// std::this_thread::sleep_until(next_t);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
template<int DOF>
|
||||
void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
|
||||
auto motor = motor_manager_->getMotor(joint_name);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user