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>
|
template<int DOF>
|
||||||
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
|
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;
|
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
|
||||||
cur_joints_angle.reserve(cmd.size());
|
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
|
||||||
|
}
|
||||||
std::vector<std::shared_ptr<AbstractMotor> > motors;
|
motor->setQ(j.rad);
|
||||||
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);
|
//3. wait for completion
|
||||||
|
bool completion = true;
|
||||||
std::vector<double> q_start, q_goal;
|
do {
|
||||||
q_start.reserve(cmd.size());
|
completion = true;
|
||||||
q_goal.reserve(cmd.size());
|
for (const auto &j: cmd) {
|
||||||
for (auto &jp: cmd) {
|
auto motor = motor_manager_->getMotor(j.joint_name);
|
||||||
auto it = cur_joints_angle.find(jp.joint_name);
|
if (motor != nullptr) {
|
||||||
if (it == cur_joints_angle.end()) {
|
if (!motor->reachedTargetQ()) {
|
||||||
LOG(ERROR) << "missing current angle for " << jp.joint_name;
|
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;
|
return;
|
||||||
}
|
}
|
||||||
q_start.push_back(it->second); // 当前读到的弧度
|
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||||||
q_goal.push_back(jp.rad); // 目标弧度
|
} while (!completion);
|
||||||
}
|
} catch (exception &e) {
|
||||||
if (q_start.size() != q_goal.size()) return;
|
throw runtime_error(e.what());
|
||||||
|
|
||||||
|
|
||||||
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]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// ——睡到下一拍 (k+1)*dt——
|
|
||||||
++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>::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>
|
template<int DOF>
|
||||||
void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
|
void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
|
||||||
auto motor = motor_manager_->getMotor(joint_name);
|
auto motor = motor_manager_->getMotor(joint_name);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user