test:add speedL test
This commit is contained in:
parent
5a751bb0d8
commit
a16a108363
@ -108,11 +108,11 @@ namespace cmvr::device{
|
||||
|
||||
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 bool speedL(const std::vector<double> &xd, double acceleration = 0.25, double time = 0.0) { throw std::runtime_error("Not implemented"); }
|
||||
virtual void stopSpeedL() { 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 followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
|
||||
|
||||
virtual void followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
|
||||
|
||||
@ -38,6 +38,7 @@ target_link_libraries(humanoid_robot_test
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
matplot
|
||||
cmvr_es::proto
|
||||
${OpenCV_LIBS}
|
||||
ccd
|
||||
|
||||
@ -22,6 +22,7 @@
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
@ -33,6 +34,7 @@
|
||||
|
||||
#include "planner/joint_space_planner/include/joint_space_planner_creator.h"
|
||||
#include "planner/joint_space_planner/include/joint_space_planner.h"
|
||||
#include "ik_solver/include/pinocchio_dls_ik_solver.h"
|
||||
|
||||
namespace cmvr::device{
|
||||
|
||||
@ -91,10 +93,13 @@ namespace cmvr::device{
|
||||
|
||||
|
||||
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 followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) override;
|
||||
void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) override;
|
||||
bool speedL(const std::vector<double> &xd, double acceleration = 0.25, double time = 0.0) override;
|
||||
void stopSpeedL() override;
|
||||
Eigen::Matrix<double, 6, 1> getSpeedLCommandTwistBase();
|
||||
|
||||
|
||||
|
||||
|
||||
void servoJ(std::vector<JointPoint> &joints, double dt) override;
|
||||
void servoJ(std::vector<JointPoint> &joints, double vel, double dt) override;
|
||||
@ -111,6 +116,11 @@ 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);
|
||||
void sendJointVelocityCommand_(const std::vector<JointVelocityCommand>& cmd);
|
||||
bool updateSpeedLAccelerationConfig_(double acceleration);
|
||||
void ensureSpeedLWorkerStarted_();
|
||||
void stopSpeedLWorker_();
|
||||
void speedLWorkerLoop_();
|
||||
|
||||
|
||||
|
||||
@ -195,6 +205,18 @@ namespace cmvr::device{
|
||||
std::shared_ptr<MotorManager> motor_manager_{nullptr};
|
||||
|
||||
std::shared_ptr<JointSpacePlanner> joint_space_planner_{nullptr};
|
||||
std::shared_ptr<PinocchioDlsIKSolver> ik_solver_{nullptr};
|
||||
PinocchioDlsIKSolver::SpeedLConfig speedl_config_{};
|
||||
std::unique_ptr<std::thread> speedl_thread_;
|
||||
std::mutex speedl_mutex_;
|
||||
std::condition_variable speedl_cv_;
|
||||
std::atomic<bool> speedl_stop_requested_{false};
|
||||
bool speedl_command_active_{false};
|
||||
Eigen::Matrix<double, 6, 1> speedl_target_twist_{Eigen::Matrix<double, 6, 1>::Zero()};
|
||||
Eigen::Matrix<double, 6, 1> speedl_last_command_twist_base_{Eigen::Matrix<double, 6, 1>::Zero()};
|
||||
double speedl_target_acceleration_{0.25};
|
||||
double speedl_applied_acceleration_{0.25};
|
||||
std::uint64_t speedl_command_version_{0};
|
||||
|
||||
// 每个电机组的锁和执行状态
|
||||
std::mutex left_arm_mutex_, right_arm_mutex_, head_mutex_, waist_mutex_;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -18,10 +18,12 @@
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include <glog/logging.h>
|
||||
#include <matplot/matplot.h>
|
||||
#include "../../../../device_manager/include/device_manager.h"
|
||||
#include <libgen.h>
|
||||
|
||||
#include "controller/include/ibvs_controller.h"
|
||||
#include "ik_solver/include/pinocchio_dls_ik_solver.h"
|
||||
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
|
||||
#include "../include/humanoid_robot.h"
|
||||
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
|
||||
@ -257,6 +259,277 @@ TEST(HumanoidRobotTest,speedJTest) {
|
||||
|
||||
//
|
||||
}
|
||||
|
||||
TEST(HumanoidRobotTest, speedLSmokeTest) {
|
||||
const XmlNode config("/home/lgv/cmvr/0-workspace/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
|
||||
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||
|
||||
auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
|
||||
ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>";
|
||||
|
||||
const std::vector<double> twist_pos = {0.03, 0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
const std::vector<double> twist_neg = {-0.03, 0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
const double acceleration = 0.20;
|
||||
const double segment_time = 2.0;
|
||||
const double settle_time = 1.0;
|
||||
const double sample_dt = 0.02;
|
||||
|
||||
std::vector<double> t_trace;
|
||||
std::vector<double> target_vx_trace;
|
||||
std::vector<double> command_vx_trace;
|
||||
std::vector<double> command_vy_trace;
|
||||
std::vector<double> command_vz_trace;
|
||||
std::vector<double> command_speed_trace;
|
||||
|
||||
double t_now = 0.0;
|
||||
auto sample_phase = [&](const std::vector<double>& target_twist,
|
||||
double duration,
|
||||
const char* phase) {
|
||||
LOG(INFO) << "speedLSmokeTest phase: " << phase;
|
||||
ASSERT_NO_THROW(robot->speedL(target_twist, acceleration, 0.0));
|
||||
const int steps = static_cast<int>(std::ceil(duration / sample_dt));
|
||||
for (int i = 0; i < steps; ++i) {
|
||||
const Eigen::Matrix<double, 6, 1> cmd_twist = robot->getSpeedLCommandTwistBase();
|
||||
t_trace.push_back(t_now);
|
||||
target_vx_trace.push_back(target_twist[0]);
|
||||
command_vx_trace.push_back(cmd_twist[0]);
|
||||
command_vy_trace.push_back(cmd_twist[1]);
|
||||
command_vz_trace.push_back(cmd_twist[2]);
|
||||
command_speed_trace.push_back(cmd_twist.head<3>().norm());
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(sample_dt));
|
||||
t_now += sample_dt;
|
||||
}
|
||||
};
|
||||
|
||||
sample_phase(twist_pos, segment_time, "+X");
|
||||
sample_phase(twist_neg, segment_time, "-X");
|
||||
|
||||
LOG(INFO) << "speedLSmokeTest phase: stop";
|
||||
ASSERT_NO_THROW(robot->stopSpeedL());
|
||||
const int settle_steps = static_cast<int>(std::ceil(settle_time / sample_dt));
|
||||
for (int i = 0; i < settle_steps; ++i) {
|
||||
const Eigen::Matrix<double, 6, 1> cmd_twist = robot->getSpeedLCommandTwistBase();
|
||||
t_trace.push_back(t_now);
|
||||
target_vx_trace.push_back(0.0);
|
||||
command_vx_trace.push_back(cmd_twist[0]);
|
||||
command_vy_trace.push_back(cmd_twist[1]);
|
||||
command_vz_trace.push_back(cmd_twist[2]);
|
||||
command_speed_trace.push_back(cmd_twist.head<3>().norm());
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(sample_dt));
|
||||
t_now += sample_dt;
|
||||
}
|
||||
|
||||
ASSERT_FALSE(t_trace.empty());
|
||||
|
||||
using namespace matplot;
|
||||
auto fig = figure(true);
|
||||
fig->size(1600, 1000);
|
||||
fig->font_size(16);
|
||||
|
||||
auto ax1 = subplot(2, 1, 0);
|
||||
hold(ax1, true);
|
||||
auto l_target = plot(ax1, t_trace, target_vx_trace, "k--");
|
||||
l_target->line_width(2.0f);
|
||||
auto l_cmd_x = plot(ax1, t_trace, command_vx_trace, "r-");
|
||||
l_cmd_x->line_width(2.0f);
|
||||
auto l_cmd_y = plot(ax1, t_trace, command_vy_trace, "g-");
|
||||
l_cmd_y->line_width(2.0f);
|
||||
auto l_cmd_z = plot(ax1, t_trace, command_vz_trace, "b-");
|
||||
l_cmd_z->line_width(2.0f);
|
||||
title(ax1, "speedL target vx vs command vxyz");
|
||||
xlabel(ax1, "time [s]");
|
||||
ylabel(ax1, "linear cmd [m/s]");
|
||||
legend(ax1, {"target vx", "command vx", "command vy", "command vz"});
|
||||
grid(ax1, true);
|
||||
|
||||
auto ax2 = subplot(2, 1, 1);
|
||||
hold(ax2, true);
|
||||
auto l_norm = plot(ax2, t_trace, command_speed_trace, "m-");
|
||||
l_norm->line_width(2.0f);
|
||||
title(ax2, "speedL command speed norm");
|
||||
xlabel(ax2, "time [s]");
|
||||
ylabel(ax2, "norm [m/s]");
|
||||
legend(ax2, {"||command v||"});
|
||||
grid(ax2, true);
|
||||
|
||||
show(fig);
|
||||
}
|
||||
|
||||
TEST(HumanoidRobotTest, SpeedLOpenLoopPlannerPlot) {
|
||||
cmvr::PinocchioDlsIKSolver solver(
|
||||
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
|
||||
"PELVIS_S",
|
||||
"R_WRIST_R_S",
|
||||
"R_FINGER_TIP_FIXED");
|
||||
ASSERT_TRUE(solver.init()) << "PinocchioDlsIKSolver init failed";
|
||||
|
||||
cmvr::PinocchioDlsIKSolver::SpeedLConfig speedl_config;
|
||||
speedl_config.linear_velocity_max = 0.55;
|
||||
speedl_config.linear_acceleration_max = 0.80;
|
||||
speedl_config.linear_jerk_max = 3.30;
|
||||
speedl_config.angular_velocity_max = 1.00;
|
||||
speedl_config.angular_acceleration_max = 3.00;
|
||||
speedl_config.angular_jerk_max = 12.0;
|
||||
speedl_config.joint_acceleration_max = std::vector<double>(7, 8.0);
|
||||
speedl_config.linear_target_replan_threshold = 1e-4;
|
||||
speedl_config.angular_target_replan_threshold = 1e-4;
|
||||
speedl_config.linear_reverse_cos_threshold = -0.8660254037844386;
|
||||
speedl_config.linear_reverse_switch_speed_threshold = 1e-3;
|
||||
ASSERT_TRUE(solver.configureSpeedL(speedl_config)) << "configureSpeedL failed";
|
||||
|
||||
std::vector<double> q_init = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0};
|
||||
solver.update_joints_state(q_init);
|
||||
|
||||
const double dt = 0.002;
|
||||
const int log_every = 50;
|
||||
const double segment_time = 1.5;
|
||||
const double stop_time = 1.0;
|
||||
const double settle_time = 1.0;
|
||||
const double linear_speed_cmd = 0.1;
|
||||
const double z_speed_cmd = 0.10;
|
||||
const double total_time = 2.0 * segment_time + stop_time;
|
||||
const int active_steps = static_cast<int>(std::ceil(total_time / dt));
|
||||
const int settle_steps = static_cast<int>(std::ceil(settle_time / dt));
|
||||
const int total_steps = active_steps + settle_steps;
|
||||
|
||||
int ok_steps = 0;
|
||||
std::vector<double> t_trace;
|
||||
std::vector<double> target_vy_trace;
|
||||
std::vector<double> target_vz_trace;
|
||||
std::vector<double> command_vx_trace;
|
||||
std::vector<double> command_vy_trace;
|
||||
std::vector<double> command_vz_trace;
|
||||
std::vector<std::vector<double>> qd_cmd_trace(kRightArmJointNames.size());
|
||||
t_trace.reserve(total_steps);
|
||||
target_vy_trace.reserve(total_steps);
|
||||
target_vz_trace.reserve(total_steps);
|
||||
command_vx_trace.reserve(total_steps);
|
||||
command_vy_trace.reserve(total_steps);
|
||||
command_vz_trace.reserve(total_steps);
|
||||
for (auto& v : qd_cmd_trace) v.reserve(total_steps);
|
||||
|
||||
for (int step = 0; step < total_steps; ++step) {
|
||||
const double t = static_cast<double>(step) * dt;
|
||||
|
||||
Eigen::Matrix<double, 6, 1> target_twist = Eigen::Matrix<double, 6, 1>::Zero();
|
||||
if (t < segment_time) {
|
||||
target_twist[1] = linear_speed_cmd;
|
||||
target_twist[2] = z_speed_cmd;
|
||||
|
||||
} else if (t < 2.0 * segment_time) {
|
||||
target_twist[1] = -linear_speed_cmd;
|
||||
target_twist[2] = -z_speed_cmd;
|
||||
} else if (t < total_time) {
|
||||
target_twist.setZero();
|
||||
} else {
|
||||
target_twist.setZero();
|
||||
}
|
||||
|
||||
std::vector<double> qd_cmd;
|
||||
ASSERT_TRUE(solver.speedLStep(target_twist,
|
||||
dt,
|
||||
qd_cmd,
|
||||
cmvr::CartesianFrame::Base,
|
||||
true))
|
||||
<< "speedLStep failed at step " << step;
|
||||
|
||||
t_trace.push_back(t);
|
||||
target_vy_trace.push_back(target_twist[1]);
|
||||
target_vz_trace.push_back(target_twist[2]);
|
||||
command_vx_trace.push_back(solver.getSpeedLCommandTwistBase()[0]);
|
||||
command_vy_trace.push_back(solver.getSpeedLCommandTwistBase()[1]);
|
||||
command_vz_trace.push_back(solver.getSpeedLCommandTwistBase()[2]);
|
||||
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
|
||||
qd_cmd_trace[i].push_back(i < qd_cmd.size() ? qd_cmd[i] : 0.0);
|
||||
}
|
||||
|
||||
++ok_steps;
|
||||
|
||||
if ((step % log_every) == 0) {
|
||||
std::cout << "[SPEEDL_OPEN_LOOP] step=" << step
|
||||
<< " t=" << t
|
||||
<< " target_vy=" << target_twist[1]
|
||||
<< " target_vz=" << target_twist[2]
|
||||
<< " phase="
|
||||
<< (t < segment_time ? "pos" :
|
||||
(t < 2.0 * segment_time ? "neg" :
|
||||
(t < total_time ? "stop" : "settle")))
|
||||
<< " cmd_qd=";
|
||||
for (const auto& v : qd_cmd) {
|
||||
std::cout << v << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
if (!t_trace.empty()) {
|
||||
using namespace matplot;
|
||||
auto fig = figure(true);
|
||||
fig->size(1600, 1200);
|
||||
fig->font_size(16);
|
||||
|
||||
auto ax1 = subplot(3, 1, 0);
|
||||
hold(ax1, true);
|
||||
auto l_target_y = plot(ax1, t_trace, target_vy_trace, "k--");
|
||||
l_target_y->line_width(2.0f);
|
||||
auto l_target_z = plot(ax1, t_trace, target_vz_trace, "c--");
|
||||
l_target_z->line_width(2.0f);
|
||||
auto l_cmd_x = plot(ax1, t_trace, command_vx_trace, "r-");
|
||||
l_cmd_x->line_width(2.0f);
|
||||
auto l_cmd_y = plot(ax1, t_trace, command_vy_trace, "g-");
|
||||
l_cmd_y->line_width(2.0f);
|
||||
auto l_cmd_z = plot(ax1, t_trace, command_vz_trace, "b-");
|
||||
l_cmd_z->line_width(2.0f);
|
||||
title(ax1, "speedL target vy/vz vs command vxyz");
|
||||
xlabel(ax1, "time [s]");
|
||||
ylabel(ax1, "linear cmd [m/s]");
|
||||
legend(ax1, {"target vy", "target vz", "command vx", "command vy", "command vz"});
|
||||
grid(ax1, true);
|
||||
|
||||
auto ax2 = subplot(3, 1, 1);
|
||||
hold(ax2, true);
|
||||
std::vector<std::string> joint_labels;
|
||||
joint_labels.reserve(kRightArmJointNames.size());
|
||||
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
|
||||
auto line = plot(ax2, t_trace, qd_cmd_trace[i]);
|
||||
line->line_width(1.8f);
|
||||
joint_labels.emplace_back(kRightArmJointNames[i]);
|
||||
}
|
||||
title(ax2, "solver qd_cmd");
|
||||
xlabel(ax2, "time [s]");
|
||||
ylabel(ax2, "joint vel [rad/s]");
|
||||
legend(ax2, joint_labels);
|
||||
grid(ax2, true);
|
||||
|
||||
auto ax3 = subplot(3, 1, 2);
|
||||
hold(ax3, true);
|
||||
std::vector<double> qd_norm_trace;
|
||||
qd_norm_trace.reserve(t_trace.size());
|
||||
for (size_t k = 0; k < t_trace.size(); ++k) {
|
||||
double norm = 0.0;
|
||||
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
|
||||
const double v = qd_cmd_trace[i][k];
|
||||
norm += v * v;
|
||||
}
|
||||
qd_norm_trace.push_back(std::sqrt(norm));
|
||||
}
|
||||
auto qd_norm_line = plot(ax3, t_trace, qd_norm_trace, "m-");
|
||||
qd_norm_line->line_width(2.0f);
|
||||
title(ax3, "solver qd_cmd norm");
|
||||
xlabel(ax3, "time [s]");
|
||||
ylabel(ax3, "norm [rad/s]");
|
||||
legend(ax3, {"||qd_cmd||"});
|
||||
grid(ax3, true);
|
||||
|
||||
show(fig);
|
||||
}
|
||||
|
||||
EXPECT_GT(ok_steps, 0) << "No successful open-loop speedL steps.";
|
||||
}
|
||||
|
||||
TEST(HumanoidRobotTest,IBVSWithRealRobot) {
|
||||
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
|
||||
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||
@ -440,4 +713,3 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
|
||||
|
||||
EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps.";
|
||||
}
|
||||
|
||||
|
||||
@ -9,7 +9,7 @@
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cartesian_space_planner/include/cartesian_twist_limiter.h"
|
||||
#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h"
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
@ -39,6 +39,9 @@ public:
|
||||
|
||||
std::vector<double> joint_velocity_max; ///< size=chain_v_dof_，为空则仅用 URDF limit
|
||||
std::vector<double> joint_acceleration_max; ///< size=chain_v_dof_，为空则不做 joint accel 限制
|
||||
bool enable_joint_soft_limit_velocity{true};
|
||||
double joint_soft_limit_margin{0.05};
|
||||
double joint_hard_limit_margin{0.03};
|
||||
|
||||
double linear_target_replan_threshold{1e-4};
|
||||
double angular_target_replan_threshold{1e-4};
|
||||
@ -101,7 +104,11 @@ public:
|
||||
/**
|
||||
* @brief 配置 speedL task-space / joint-space 限幅参数。
|
||||
*
|
||||
* 调用后会重置 speedL 内部运行态，但保留本次配置。
|
||||
* 仅更新 speedL 配置，不主动重置当前运行态。
|
||||
*
|
||||
* 使用注意：
|
||||
* 1) 运行中再次调用本接口时，当前 target / planner state / 上一拍 qdot 缓存会被保留。
|
||||
* 2) 如果你需要“重新配置并从静止状态重新开始”，请先调用 resetSpeedL()，再调用 configureSpeedL()。
|
||||
*/
|
||||
bool configureSpeedL(const SpeedLConfig& config);
|
||||
|
||||
@ -154,7 +161,9 @@ public:
|
||||
* 1) 只有在能拿到真实 q、但暂时拿不到真实 qd 时再用这个版本。
|
||||
* 2) 这里的 qdot_measured 是 speedl_prev_qdot_cmd_ 的近似值；下游存在饱和、延迟或丢包时，
|
||||
* 这个近似会偏乐观。
|
||||
* 3) 一旦能拿到真实 qdot，应切回带 q_measured/qdot_measured 的闭环版本。
|
||||
* 3) 为避免把伪造的 qdot_measured 再同步回 task-space planner，这个版本不会执行
|
||||
* CartesianTwistLimiter::synchronize(...)。
|
||||
* 4) 一旦能拿到真实 qdot，应切回带 q_measured/qdot_measured 的闭环版本。
|
||||
*/
|
||||
bool speedLStep(const Eigen::Matrix<double,6,1>& target_twist,
|
||||
double dt,
|
||||
@ -174,7 +183,8 @@ public:
|
||||
* 2) 该版本依赖内部预测状态，会随时间累计漂移；不适合长时间连续运行。
|
||||
* 3) 仅当 chain_dof_ == chain_v_dof_ 时才允许使用，因为内部使用简单的 q += qdot * dt
|
||||
* 欧拉推进。
|
||||
* 4) 一旦恢复真实 q 或 qd，请立即切回上面两个 overload。
|
||||
* 4) 该版本同样不会执行 CartesianTwistLimiter::synchronize(...)。
|
||||
* 5) 一旦恢复真实 q 或 qd，请立即切回上面两个 overload。
|
||||
*/
|
||||
bool speedLStep(const Eigen::Matrix<double,6,1>& target_twist,
|
||||
double dt,
|
||||
@ -235,11 +245,22 @@ private:
|
||||
Eigen::VectorXd* q_full_out = nullptr);
|
||||
|
||||
Eigen::VectorXd applyJointVelocityLimits(const Eigen::VectorXd& qdot_des) const;
|
||||
Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain,
|
||||
const Eigen::VectorXd& qdot_des);
|
||||
|
||||
Eigen::VectorXd applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des,
|
||||
const Eigen::VectorXd& qdot_reference,
|
||||
double dt) const;
|
||||
|
||||
bool speedLStepImpl(const Eigen::Matrix<double,6,1>& target_twist,
|
||||
double dt,
|
||||
const std::vector<double>& q_measured,
|
||||
const std::vector<double>& qdot_measured,
|
||||
std::vector<double>& qdot_cmd,
|
||||
CartesianFrame input_frame,
|
||||
bool is_tcp,
|
||||
bool enable_twist_sync);
|
||||
|
||||
private:
|
||||
int chain_q_start_{0};
|
||||
int chain_dof_{0};
|
||||
@ -273,6 +294,7 @@ private:
|
||||
Eigen::Matrix<double,6,1> speedl_twist_measured_base_{Eigen::Matrix<double,6,1>::Zero()};
|
||||
Eigen::Matrix<double,6,1> speedl_twist_command_base_{Eigen::Matrix<double,6,1>::Zero()};
|
||||
Eigen::Matrix<double,6,1> speedl_twist_executed_base_{Eigen::Matrix<double,6,1>::Zero()};
|
||||
bool speedl_joint_soft_limit_active_{false};
|
||||
};
|
||||
|
||||
} // namespace cmvr
|
||||
|
||||
@ -15,6 +15,7 @@
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
@ -709,12 +710,6 @@ bool PinocchioDlsIKSolver::configureSpeedL(const SpeedLConfig& config)
|
||||
speedl_twist_limiter_.setLinearReverseSwitchPolicy(config.linear_reverse_cos_threshold,
|
||||
config.linear_reverse_switch_speed_threshold);
|
||||
|
||||
speedl_twist_limiter_.reset();
|
||||
speedl_prev_qdot_cmd_.assign(chain_v_dof_, 0.0);
|
||||
speedl_twist_measured_base_.setZero();
|
||||
speedl_twist_command_base_.setZero();
|
||||
speedl_twist_executed_base_.setZero();
|
||||
|
||||
speedl_configured_ = true;
|
||||
return true;
|
||||
}
|
||||
@ -726,6 +721,7 @@ void PinocchioDlsIKSolver::resetSpeedL()
|
||||
speedl_twist_measured_base_.setZero();
|
||||
speedl_twist_command_base_.setZero();
|
||||
speedl_twist_executed_base_.setZero();
|
||||
speedl_joint_soft_limit_active_ = false;
|
||||
}
|
||||
|
||||
bool PinocchioDlsIKSolver::resolveSpeedLEeFrame(bool is_tcp, pinocchio::FrameIndex& ee_id) const
|
||||
@ -847,6 +843,89 @@ Eigen::VectorXd PinocchioDlsIKSolver::applyJointVelocityLimits(const Eigen::Vect
|
||||
return gamma * qdot_des;
|
||||
}
|
||||
|
||||
Eigen::VectorXd PinocchioDlsIKSolver::applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain,
|
||||
const Eigen::VectorXd& qdot_des)
|
||||
{
|
||||
if (!speedl_config_.enable_joint_soft_limit_velocity ||
|
||||
q_chain.size() != chain_dof_ ||
|
||||
qdot_des.size() != chain_v_dof_ ||
|
||||
chain_dof_ != chain_v_dof_ ||
|
||||
joint_pos_lower_limits_.size() != chain_dof_ ||
|
||||
joint_pos_upper_limits_.size() != chain_dof_) {
|
||||
return qdot_des;
|
||||
}
|
||||
|
||||
const double hard_margin = std::max(1e-4, speedl_config_.joint_hard_limit_margin);
|
||||
const double soft_margin = std::max(hard_margin + 1e-4, speedl_config_.joint_soft_limit_margin);
|
||||
|
||||
auto apply_scalar = [&](double q,
|
||||
double v,
|
||||
double q_min,
|
||||
double q_max) -> double {
|
||||
if (q_max <= q_min) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
if (v < 0.0) {
|
||||
const double q_hard = q_min + hard_margin;
|
||||
const double q_soft = q_min + soft_margin;
|
||||
if (q <= q_hard) {
|
||||
return 0.0;
|
||||
}
|
||||
if (q < q_soft) {
|
||||
const double s = std::clamp((q - q_hard) / (q_soft - q_hard), 0.0, 1.0);
|
||||
return v * s;
|
||||
}
|
||||
} else if (v > 0.0) {
|
||||
const double q_hard = q_max - hard_margin;
|
||||
const double q_soft = q_max - soft_margin;
|
||||
if (q >= q_hard) {
|
||||
return 0.0;
|
||||
}
|
||||
if (q > q_soft) {
|
||||
const double s = std::clamp((q_hard - q) / (q_hard - q_soft), 0.0, 1.0);
|
||||
return v * s;
|
||||
}
|
||||
}
|
||||
return v;
|
||||
};
|
||||
|
||||
Eigen::VectorXd qdot_limited = qdot_des;
|
||||
bool clamped_any = false;
|
||||
std::ostringstream oss;
|
||||
|
||||
for (int i = 0; i < chain_v_dof_; ++i) {
|
||||
const double v_before = qdot_des[i];
|
||||
const double v_after = apply_scalar(q_chain[i],
|
||||
v_before,
|
||||
joint_pos_lower_limits_[i],
|
||||
joint_pos_upper_limits_[i]);
|
||||
qdot_limited[i] = v_after;
|
||||
|
||||
if (std::abs(v_after - v_before) > 1e-9) {
|
||||
clamped_any = true;
|
||||
if (oss.tellp() > 0) {
|
||||
oss << " | ";
|
||||
}
|
||||
const std::string joint_name =
|
||||
(i < static_cast<int>(chain_joint_names_.size())) ? chain_joint_names_[i] : ("joint_" + std::to_string(i));
|
||||
oss << joint_name
|
||||
<< " q=" << q_chain[i]
|
||||
<< " v:" << v_before << "->" << v_after;
|
||||
}
|
||||
}
|
||||
|
||||
if (clamped_any && !speedl_joint_soft_limit_active_) {
|
||||
std::cerr << "[PinocchioDlsIKSolver] speedL soft joint-limit velocity clamp active: "
|
||||
<< oss.str() << "\n";
|
||||
} else if (!clamped_any && speedl_joint_soft_limit_active_) {
|
||||
std::cerr << "[PinocchioDlsIKSolver] speedL soft joint-limit velocity clamp released\n";
|
||||
}
|
||||
speedl_joint_soft_limit_active_ = clamped_any;
|
||||
|
||||
return qdot_limited;
|
||||
}
|
||||
|
||||
Eigen::VectorXd PinocchioDlsIKSolver::applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des,
|
||||
const Eigen::VectorXd& qdot_reference,
|
||||
double dt) const
|
||||
@ -885,6 +964,25 @@ bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix<double,6,1>& target_tw
|
||||
std::vector<double>& qdot_cmd,
|
||||
CartesianFrame input_frame,
|
||||
bool is_tcp)
|
||||
{
|
||||
return speedLStepImpl(target_twist,
|
||||
dt,
|
||||
q_measured,
|
||||
qdot_measured,
|
||||
qdot_cmd,
|
||||
input_frame,
|
||||
is_tcp,
|
||||
true);
|
||||
}
|
||||
|
||||
bool PinocchioDlsIKSolver::speedLStepImpl(const Eigen::Matrix<double,6,1>& target_twist,
|
||||
double dt,
|
||||
const std::vector<double>& q_measured,
|
||||
const std::vector<double>& qdot_measured,
|
||||
std::vector<double>& qdot_cmd,
|
||||
CartesianFrame input_frame,
|
||||
bool is_tcp,
|
||||
bool enable_twist_sync)
|
||||
{
|
||||
if (!initialized_) {
|
||||
std::cerr << "[PinocchioDlsIKSolver] speedLStep failed: solver not initialized\n";
|
||||
@ -928,8 +1026,9 @@ bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix<double,6,1>& target_tw
|
||||
return false;
|
||||
}
|
||||
|
||||
// 1) 用 measured twist 同步 task-space limiter
|
||||
speedl_twist_limiter_.synchronize(speedl_twist_measured_base_, dt, true);
|
||||
if (enable_twist_sync) {
|
||||
speedl_twist_limiter_.synchronize(speedl_twist_measured_base_, dt, true);
|
||||
}
|
||||
|
||||
// 2) 设置目标 twist
|
||||
speedl_twist_limiter_.setTargetTwist(target_twist, input_frame);
|
||||
@ -965,6 +1064,9 @@ bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix<double,6,1>& target_tw
|
||||
// 5) joint velocity limit（整体缩放）
|
||||
qdot_des = applyJointVelocityLimits(qdot_des);
|
||||
|
||||
// 5.1) joint soft position-limit velocity clamp（逐轴压缩靠近限位且继续往外的速度）
|
||||
qdot_des = applyJointSoftLimitVelocity(q_chain, qdot_des);
|
||||
|
||||
// 6) joint acceleration limit（相对 measured qdot 整体缩放）
|
||||
const Eigen::Map<const Eigen::VectorXd> qdot_meas_vec(qdot_measured.data(), chain_v_dof_);
|
||||
qdot_des = applyJointAccelerationLimits(qdot_des, qdot_meas_vec, dt);
|
||||
@ -1012,13 +1114,14 @@ bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix<double,6,1>& target_tw
|
||||
qdot_measured = speedl_prev_qdot_cmd_;
|
||||
}
|
||||
|
||||
return speedLStep(target_twist,
|
||||
dt,
|
||||
q_measured,
|
||||
qdot_measured,
|
||||
qdot_cmd,
|
||||
input_frame,
|
||||
is_tcp);
|
||||
return speedLStepImpl(target_twist,
|
||||
dt,
|
||||
q_measured,
|
||||
qdot_measured,
|
||||
qdot_cmd,
|
||||
input_frame,
|
||||
is_tcp,
|
||||
false);
|
||||
}
|
||||
|
||||
bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix<double,6,1>& target_twist,
|
||||
|
||||
@ -1534,9 +1534,11 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) {
|
||||
Eigen::Matrix<double,6,1> target_twist =
|
||||
Eigen::Matrix<double,6,1>::Zero();
|
||||
if (t < segment_time) {
|
||||
target_twist[5] = -linear_speed_cmd;
|
||||
target_twist[1] = linear_speed_cmd;
|
||||
target_twist[2] = 0.1;
|
||||
} else if (t < 2.0 * segment_time) {
|
||||
target_twist[5] = linear_speed_cmd;
|
||||
target_twist[1] = -linear_speed_cmd;
|
||||
target_twist[2] = -0.1;
|
||||
} else {
|
||||
target_twist.setZero();
|
||||
}
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
|
||||
|
||||
add_library(planner STATIC
|
||||
add_library(planner SHARED
|
||||
joint_space_planner/src/joint_space_planner.cpp
|
||||
joint_space_planner/src/joint_space_planner_creator.cpp
|
||||
joint_space_planner/src/toppra_bspline.cpp
|
||||
|
||||
@ -143,7 +143,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context,
|
||||
auto cart = static_cast<device::RobotCartesian>(request->cart());
|
||||
|
||||
robot->setToolFrame(ee_link);
|
||||
robot->speedL(cart,dir,vel,acc);
|
||||
// robot->speedL(cart,dir,vel,acc);
|
||||
}catch (const std::exception& e) {
|
||||
response->mutable_header()->set_success(false);
|
||||
response->mutable_header()->set_error_message(e.what());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user