fix(speedl):fix speedl stop overshoot
This commit is contained in:
parent
141e9813c2
commit
dbac432565
@ -99,6 +99,11 @@ private:
|
||||
bool speedl_line_check_active_{false};
|
||||
bool speedl_line_deviation_warned_{false};
|
||||
bool speedl_line_direction_warned_{false};
|
||||
|
||||
// speedL 是否已经进入停止阶段。
|
||||
// 停止阶段不要每 1 ms 用 measured twist 重新点燃 Cartesian planner。
|
||||
bool speedl_stop_active_{false};
|
||||
|
||||
bool speedl_configured_{false};
|
||||
};
|
||||
|
||||
|
||||
@ -127,6 +127,7 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne
|
||||
speedl_line_check_active_ = false;
|
||||
speedl_line_deviation_warned_ = false;
|
||||
speedl_line_direction_warned_ = false;
|
||||
speedl_stop_active_ = false;
|
||||
speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0);
|
||||
speedl_configured_ = true;
|
||||
return true;
|
||||
@ -894,15 +895,43 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
|
||||
return false;
|
||||
}
|
||||
|
||||
const Eigen::Matrix<double, 6, 1> target_twist = common::math::velocityToVector(target_velocity);
|
||||
const bool is_stop_command = target_twist.squaredNorm() <= 1e-12;
|
||||
const Eigen::Matrix<double, 6, 1> target_twist =
|
||||
common::math::velocityToVector(target_velocity);
|
||||
|
||||
const bool is_stop_command =
|
||||
target_twist.squaredNorm() <= 1e-12;
|
||||
|
||||
if (is_stop_command) {
|
||||
twist_limiter_.synchronize(measured_twist_base, dt, true);
|
||||
} else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
|
||||
twist_limiter_.initialize(Eigen::Matrix<double, 6, 1>::Zero());
|
||||
if (!speedl_stop_active_) {
|
||||
// 只在 stop 边沿执行一次。
|
||||
//
|
||||
// 非常重要：
|
||||
// 不再调用
|
||||
// twist_limiter_.synchronize(measured_twist_base, dt, true);
|
||||
//
|
||||
// 停止应当从“上一拍已经发送出去的 command twist”
|
||||
// 连续规划到 0，而不是每 1 ms 被 measured twist 重新点燃。
|
||||
speedl_stop_active_ = true;
|
||||
|
||||
twist_limiter_.stop();
|
||||
}
|
||||
twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame));
|
||||
speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool);
|
||||
} else {
|
||||
// 收到新的非零 speedL，退出停止状态。
|
||||
speedl_stop_active_ = false;
|
||||
|
||||
// 从静止开始一个新的 speedL command。
|
||||
if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
|
||||
twist_limiter_.initialize(
|
||||
Eigen::Matrix<double, 6, 1>::Zero());
|
||||
}
|
||||
|
||||
twist_limiter_.setTargetTwist(
|
||||
target_twist,
|
||||
common::math::toPlannerFrame(frame));
|
||||
}
|
||||
|
||||
speedl_command_twist_base_ =
|
||||
twist_limiter_.update(dt, base_R_tool);
|
||||
if (!updateAndValidateSpeedLLineDeviation_(q_measured,
|
||||
is_stop_command,
|
||||
speedl_command_twist_base_.head<3>())) {
|
||||
|
||||
@ -32,3 +32,14 @@ target_link_libraries(toppra_multi_waypoint_test
|
||||
gtest
|
||||
gtest_main
|
||||
)
|
||||
|
||||
add_executable(s_curve_velocity_planner_stop_test
|
||||
motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(s_curve_velocity_planner_stop_test
|
||||
PRIVATE
|
||||
cmvr_es::base_motion
|
||||
gtest
|
||||
gtest_main
|
||||
)
|
||||
|
||||
@ -201,7 +201,12 @@ void CartesianTwistLimiter::setTargetTwist(const Twist& target_twist, CartesianF
|
||||
|
||||
void CartesianTwistLimiter::stop()
|
||||
{
|
||||
// target_twist_input_.setZero();
|
||||
target_twist_input_.setZero();
|
||||
target_twist_base_.setZero();
|
||||
|
||||
linear_norm_planner_.setTargetVelocity(0.0);
|
||||
angular_norm_planner_.setTargetVelocity(0.0);
|
||||
}
|
||||
|
||||
void CartesianTwistLimiter::emergencyStop(double emergency_acceleration,
|
||||
|
||||
@ -114,13 +114,29 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity,
|
||||
updateIsMovingFlag();
|
||||
}
|
||||
|
||||
void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity,
|
||||
void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity,
|
||||
double acceleration)
|
||||
{
|
||||
const double measured_velocity = clamp(velocity, -max_velocity_, max_velocity_);
|
||||
const double measured_acceleration =
|
||||
double measured_acceleration =
|
||||
clamp(acceleration, -max_acceleration_, max_acceleration_);
|
||||
|
||||
// When the target is zero this planner is also used for Cartesian speed
|
||||
// magnitudes. A magnitude is non-negative, while the finite-difference
|
||||
// derivative of the measured magnitude is signed. Feeding a large negative
|
||||
// measured acceleration into a signed 1-D velocity planner can generate a
|
||||
// profile that crosses through zero and becomes negative before returning to
|
||||
// zero. The twist limiter then multiplies that negative "norm" by the
|
||||
// current direction, which reverses and amplifies the Cartesian command.
|
||||
//
|
||||
// For feedback resynchronization during a stop, synchronize the measured
|
||||
// speed only and restart the stop profile with zero scalar acceleration.
|
||||
// This avoids noise-sensitive stop replans and preserves a non-overshooting
|
||||
// deceleration profile for speed-magnitude users.
|
||||
if (std::abs(state_.target_velocity) <= VELOCITY_THRESHOLD) {
|
||||
measured_acceleration = 0.0;
|
||||
}
|
||||
|
||||
if (!state_.has_active_profile &&
|
||||
std::abs(measured_velocity - state_.target_velocity) <= VELOCITY_THRESHOLD) {
|
||||
state_.velocity = state_.target_velocity;
|
||||
|
||||
@ -0,0 +1,77 @@
|
||||
#include <algorithm>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h"
|
||||
|
||||
namespace cmvr {
|
||||
namespace {
|
||||
|
||||
void finishActiveProfile(SCurveVelocityPlanner1D& planner, double dt)
|
||||
{
|
||||
for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
|
||||
planner.update(dt);
|
||||
}
|
||||
ASSERT_FALSE(planner.hasActiveProfile());
|
||||
}
|
||||
|
||||
TEST(SCurveVelocityPlannerStopTest, FeedbackResyncDoesNotReverseSpeedMagnitude)
|
||||
{
|
||||
SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0);
|
||||
constexpr double kDt = 0.001;
|
||||
constexpr double kInitialSpeed = 0.04;
|
||||
|
||||
planner.initialize(kInitialSpeed, 0.0);
|
||||
planner.setTargetVelocity(0.0);
|
||||
finishActiveProfile(planner, kDt);
|
||||
|
||||
// Reproduce the speedL stop feedback case: the command profile has already
|
||||
// reached zero, but the measured TCP still has residual speed. A 1 kHz
|
||||
// finite difference may report a large negative scalar acceleration.
|
||||
planner.synchronizeAndReplan(kInitialSpeed, -3.0);
|
||||
|
||||
double max_speed = planner.getVelocity();
|
||||
double min_speed = planner.getVelocity();
|
||||
for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
|
||||
const double speed = planner.update(kDt);
|
||||
max_speed = std::max(max_speed, speed);
|
||||
min_speed = std::min(min_speed, speed);
|
||||
}
|
||||
|
||||
EXPECT_GE(min_speed, -1e-9);
|
||||
EXPECT_LE(max_speed, kInitialSpeed + 1e-9);
|
||||
EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9);
|
||||
}
|
||||
|
||||
TEST(SCurveVelocityPlannerStopTest, LargerMeasuredDecelerationDoesNotIncreaseStopSpeed)
|
||||
{
|
||||
constexpr double kDt = 0.001;
|
||||
constexpr double kInitialSpeed = 0.04;
|
||||
const double measured_accelerations[] = {-0.5, -1.0, -2.0, -3.0};
|
||||
|
||||
for (const double measured_acceleration : measured_accelerations) {
|
||||
SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0);
|
||||
planner.initialize(kInitialSpeed, 0.0);
|
||||
planner.setTargetVelocity(0.0);
|
||||
finishActiveProfile(planner, kDt);
|
||||
|
||||
planner.synchronizeAndReplan(kInitialSpeed, measured_acceleration);
|
||||
|
||||
double max_speed = planner.getVelocity();
|
||||
double min_speed = planner.getVelocity();
|
||||
for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
|
||||
const double speed = planner.update(kDt);
|
||||
max_speed = std::max(max_speed, speed);
|
||||
min_speed = std::min(min_speed, speed);
|
||||
}
|
||||
|
||||
EXPECT_GE(min_speed, -1e-9) << "measured_acceleration=" << measured_acceleration;
|
||||
EXPECT_LE(max_speed, kInitialSpeed + 1e-9)
|
||||
<< "measured_acceleration=" << measured_acceleration;
|
||||
EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9)
|
||||
<< "measured_acceleration=" << measured_acceleration;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace cmvr
|
||||
@ -137,9 +137,9 @@ arm {
|
||||
cartesian_velocity_feasibility_check {
|
||||
enable: true
|
||||
min_linear_speed_ratio: 0.2
|
||||
max_linear_direction_deviation_deg: 5.0
|
||||
max_linear_direction_deviation_deg: 70
|
||||
min_angular_speed_ratio: 0.2
|
||||
max_angular_direction_deviation_deg: 5.0
|
||||
max_angular_direction_deviation_deg: 70
|
||||
min_desired_linear_speed: 1e-4
|
||||
min_desired_angular_speed: 1e-4
|
||||
}
|
||||
@ -151,7 +151,7 @@ arm {
|
||||
stop_twist_norm: 1e-9
|
||||
stop_command_velocity_norm: 1e-3
|
||||
stop_measured_velocity_norm: 1e-2
|
||||
stop_acceleration: 0.5
|
||||
stop_acceleration: 5
|
||||
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
|
||||
stop_timeout_s: 2.0
|
||||
}
|
||||
|
||||
@ -118,7 +118,7 @@ device_manager {
|
||||
devices {
|
||||
id: "right_arm"
|
||||
type: DEVICE_TYPE_ROBOT_ARM
|
||||
config_file: "devices/arm/arm.pb.txt"
|
||||
config_file: "devices/arm/arm_qp.pb.txt"
|
||||
enable: false
|
||||
}
|
||||
|
||||
|
||||
@ -0,0 +1,352 @@
|
||||
#include "arm/motor_robot_arm/include/motor_robot_arm.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "algorithms/kinematics/ik_solver/ik_solver_factory.h"
|
||||
#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
|
||||
#include "common/io/proto_file_io.h"
|
||||
#include "devices/motor/manager/include/motor_manager.h"
|
||||
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
namespace {
|
||||
|
||||
constexpr std::size_t kDof = 7;
|
||||
constexpr std::array<const char*, kDof> kJointNames = {
|
||||
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R",
|
||||
"R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
|
||||
constexpr std::array<double, kDof> kInitialJointPosition = {
|
||||
-0.2423, 1.2929, 1.61, 1.58, -2.8792, 0.1150, -0.08};
|
||||
constexpr std::array<double, 7> kAccelerationSweep = {
|
||||
0.1, 0.3, 0.5, 1.0, 2.0, 3.0, 5.0};
|
||||
constexpr double kCommandVelocity = 0.04;
|
||||
constexpr double kCommandDurationS = 2.0;
|
||||
constexpr double kSamplePeriodS = 0.001;
|
||||
|
||||
std::filesystem::path findProjectRoot()
|
||||
{
|
||||
const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml";
|
||||
auto search = [&](std::filesystem::path current) {
|
||||
while (!current.empty()) {
|
||||
if (std::filesystem::exists(current / marker)) {
|
||||
return current;
|
||||
}
|
||||
const auto parent = current.parent_path();
|
||||
if (parent == current) {
|
||||
break;
|
||||
}
|
||||
current = parent;
|
||||
}
|
||||
return std::filesystem::path{};
|
||||
};
|
||||
|
||||
auto root = search(std::filesystem::current_path());
|
||||
if (!root.empty()) {
|
||||
return root;
|
||||
}
|
||||
return search(std::filesystem::path(__FILE__).parent_path());
|
||||
}
|
||||
|
||||
void setKinematicsUrdfPath(config::RobotArmConfig& arm_config,
|
||||
const std::string& urdf_path)
|
||||
{
|
||||
switch (arm_config.kinematics().algorithm_case()) {
|
||||
case config::ArmKinematicsConfig::kPinocchioDlsIkSolver:
|
||||
arm_config.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path(urdf_path);
|
||||
break;
|
||||
case config::ArmKinematicsConfig::kPinocchioQpIkSolver:
|
||||
arm_config.mutable_kinematics()->mutable_pinocchio_qp_ik_solver()->set_urdf_path(urdf_path);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
struct Sample {
|
||||
double time_s{0.0};
|
||||
std::vector<double> q;
|
||||
std::vector<double> qd;
|
||||
CartesianVelocity commanded_twist;
|
||||
};
|
||||
|
||||
struct Metrics {
|
||||
double max_qd_before_stop{0.0};
|
||||
double max_qd_after_stop{0.0};
|
||||
double max_qdd{0.0};
|
||||
double max_tcp_linear_speed_before_stop{0.0};
|
||||
double max_tcp_linear_speed_after_stop{0.0};
|
||||
double max_tcp_linear_acceleration{0.0};
|
||||
};
|
||||
|
||||
double vectorNorm(const std::vector<double>& value)
|
||||
{
|
||||
double sum = 0.0;
|
||||
for (const double item : value) {
|
||||
sum += item * item;
|
||||
}
|
||||
return std::sqrt(sum);
|
||||
}
|
||||
|
||||
double linearSpeed(const Eigen::Matrix<double, 6, 1>& twist)
|
||||
{
|
||||
return twist.head<3>().norm();
|
||||
}
|
||||
|
||||
std::string numberForFile(const double value)
|
||||
{
|
||||
std::ostringstream stream;
|
||||
stream << std::fixed << std::setprecision(3) << value;
|
||||
auto result = stream.str();
|
||||
std::replace(result.begin(), result.end(), '.', '_');
|
||||
return result;
|
||||
}
|
||||
|
||||
void writeCsv(const std::filesystem::path& path,
|
||||
const std::vector<Sample>& samples,
|
||||
const double stop_time_s,
|
||||
const cmvr::PinocchioIKBase& solver,
|
||||
Metrics& metrics)
|
||||
{
|
||||
std::ofstream output(path);
|
||||
ASSERT_TRUE(output.is_open()) << "failed to open CSV: " << path;
|
||||
|
||||
output << "time_s,phase";
|
||||
for (const auto* name : kJointNames) {
|
||||
output << "," << name << "_q";
|
||||
}
|
||||
for (const auto* name : kJointNames) {
|
||||
output << "," << name << "_qd";
|
||||
}
|
||||
output << ",command_vx,command_vy,command_vz,command_wx,command_wy,command_wz"
|
||||
<< ",tcp_vx,tcp_vy,tcp_vz,tcp_wx,tcp_wy,tcp_wz,tcp_linear_speed,tcp_linear_acceleration";
|
||||
output << '\n';
|
||||
|
||||
Eigen::Matrix<double, 6, 1> previous_tcp_twist = Eigen::Matrix<double, 6, 1>::Zero();
|
||||
bool have_previous_tcp = false;
|
||||
for (const auto& sample : samples) {
|
||||
Eigen::Matrix<double, 6, 1> tcp_twist = Eigen::Matrix<double, 6, 1>::Zero();
|
||||
ASSERT_TRUE(solver.computeTwistBaseAtQ(sample.q, sample.qd, true, tcp_twist));
|
||||
const bool after_stop = stop_time_s >= 0.0 && sample.time_s >= stop_time_s;
|
||||
const double qd_norm = vectorNorm(sample.qd);
|
||||
const double tcp_speed = linearSpeed(tcp_twist);
|
||||
double tcp_acceleration = 0.0;
|
||||
if (have_previous_tcp) {
|
||||
tcp_acceleration = (tcp_twist.head<3>() - previous_tcp_twist.head<3>()).norm() /
|
||||
std::max(kSamplePeriodS, sample.time_s -
|
||||
(samples[&sample - samples.data() - 1].time_s));
|
||||
}
|
||||
previous_tcp_twist = tcp_twist;
|
||||
have_previous_tcp = true;
|
||||
|
||||
if (after_stop) {
|
||||
metrics.max_qd_after_stop = std::max(metrics.max_qd_after_stop, qd_norm);
|
||||
metrics.max_tcp_linear_speed_after_stop =
|
||||
std::max(metrics.max_tcp_linear_speed_after_stop, tcp_speed);
|
||||
} else {
|
||||
metrics.max_qd_before_stop = std::max(metrics.max_qd_before_stop, qd_norm);
|
||||
metrics.max_tcp_linear_speed_before_stop =
|
||||
std::max(metrics.max_tcp_linear_speed_before_stop, tcp_speed);
|
||||
}
|
||||
metrics.max_tcp_linear_acceleration =
|
||||
std::max(metrics.max_tcp_linear_acceleration, tcp_acceleration);
|
||||
|
||||
if (&sample != samples.data()) {
|
||||
const auto& previous = samples[&sample - samples.data() - 1];
|
||||
const double dt = std::max(kSamplePeriodS, sample.time_s - previous.time_s);
|
||||
for (std::size_t i = 0; i < kDof; ++i) {
|
||||
metrics.max_qdd = std::max(metrics.max_qdd,
|
||||
std::abs(sample.qd[i] - previous.qd[i]) / dt);
|
||||
}
|
||||
}
|
||||
|
||||
output << std::setprecision(9) << sample.time_s << ',' << (after_stop ? "stop" : "run");
|
||||
for (const double value : sample.q) {
|
||||
output << ',' << value;
|
||||
}
|
||||
for (const double value : sample.qd) {
|
||||
output << ',' << value;
|
||||
}
|
||||
output << ',' << sample.commanded_twist.vx
|
||||
<< ',' << sample.commanded_twist.vy
|
||||
<< ',' << sample.commanded_twist.vz
|
||||
<< ',' << sample.commanded_twist.wx
|
||||
<< ',' << sample.commanded_twist.wy
|
||||
<< ',' << sample.commanded_twist.wz;
|
||||
for (Eigen::Index i = 0; i < 6; ++i) {
|
||||
output << ',' << tcp_twist[i];
|
||||
}
|
||||
output << ',' << tcp_speed << ',' << tcp_acceleration << '\n';
|
||||
}
|
||||
}
|
||||
|
||||
class MotorRobotArmSpeedLStopTest : public ::testing::Test {
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
project_root_ = findProjectRoot();
|
||||
ASSERT_FALSE(project_root_.empty());
|
||||
|
||||
config::MujocoWorldRootConfig world_root_config;
|
||||
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
|
||||
(project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(),
|
||||
&world_root_config));
|
||||
ASSERT_GT(world_root_config.worlds_size(), 0);
|
||||
auto world_config = world_root_config.worlds(0);
|
||||
world_config.set_model_path(
|
||||
(project_root_ / "model/xiaoyan_description/dual_arm.xml").string());
|
||||
world_device_ = std::make_shared<simulate::MujocoWorldDevice>(world_config);
|
||||
ASSERT_TRUE(world_device_->init());
|
||||
ASSERT_TRUE(world_device_->start());
|
||||
|
||||
config::MotorRootConfig motor_root_config;
|
||||
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
|
||||
(project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(),
|
||||
&motor_root_config));
|
||||
motor_system_ = std::make_shared<MotorManager>(
|
||||
"right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
|
||||
ASSERT_TRUE(motor_system_->init());
|
||||
|
||||
config::ArmRootConfig root_config;
|
||||
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
|
||||
(project_root_ / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt").string(),
|
||||
&root_config));
|
||||
ASSERT_GT(root_config.arm().robot_arms_size(), 0);
|
||||
arm_config_ = root_config.arm().robot_arms(0);
|
||||
setKinematicsUrdfPath(
|
||||
arm_config_, (project_root_ / "model/xiaoyan_description/dual_arm.urdf").string());
|
||||
arm_ = std::make_unique<MotorRobotArm>(arm_config_);
|
||||
ASSERT_TRUE(arm_->init());
|
||||
|
||||
analysis_solver_ = IKSolverFactory::create(arm_config_.kinematics());
|
||||
ASSERT_TRUE(analysis_solver_);
|
||||
ASSERT_TRUE(analysis_solver_->init());
|
||||
analysis_pinocchio_solver_ = std::dynamic_pointer_cast<cmvr::PinocchioIKBase>(analysis_solver_);
|
||||
ASSERT_TRUE(analysis_pinocchio_solver_);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
if (arm_) {
|
||||
arm_->stop();
|
||||
}
|
||||
if (motor_system_) {
|
||||
motor_system_->stop();
|
||||
}
|
||||
if (world_device_) {
|
||||
world_device_->stop();
|
||||
}
|
||||
}
|
||||
|
||||
std::filesystem::path project_root_;
|
||||
config::RobotArmConfig arm_config_;
|
||||
std::shared_ptr<simulate::MujocoWorldDevice> world_device_;
|
||||
std::shared_ptr<MotorManager> motor_system_;
|
||||
std::unique_ptr<MotorRobotArm> arm_;
|
||||
std::shared_ptr<cmvr::IKSolver> analysis_solver_;
|
||||
std::shared_ptr<cmvr::PinocchioIKBase> analysis_pinocchio_solver_;
|
||||
};
|
||||
|
||||
TEST_F(MotorRobotArmSpeedLStopTest, SweepAccelerationAndDirection)
|
||||
{
|
||||
ASSERT_TRUE(world_device_);
|
||||
ASSERT_TRUE(world_device_->world());
|
||||
|
||||
const std::vector<double> initial(kInitialJointPosition.begin(), kInitialJointPosition.end());
|
||||
MotionOptions move_options;
|
||||
move_options.velocity = 2.0;
|
||||
move_options.acceleration = 3.0;
|
||||
ASSERT_TRUE(arm_->moveJ(JointPositionCommand{initial}, move_options).ok());
|
||||
|
||||
for (const double acceleration : kAccelerationSweep) {
|
||||
for (const double direction : {1.0, -1.0}) {
|
||||
ASSERT_FALSE(arm_->busy());
|
||||
std::vector<Sample> samples;
|
||||
samples.reserve(5000);
|
||||
std::atomic<double> stop_time_s{-1.0};
|
||||
Result command_result = Result::failure(ArmErrorCode::UnknownError, "not run");
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
|
||||
std::thread command_thread([&] {
|
||||
CartesianVelocity velocity;
|
||||
velocity.vx = direction * kCommandVelocity;
|
||||
command_result = arm_->speedL(velocity,
|
||||
acceleration,
|
||||
kCommandDurationS,
|
||||
FrameType::Base);
|
||||
stop_time_s.store(std::chrono::duration<double>(
|
||||
std::chrono::steady_clock::now() - start_time).count());
|
||||
});
|
||||
|
||||
while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() < 5.0) {
|
||||
const double time_s = std::chrono::duration<double>(
|
||||
std::chrono::steady_clock::now() - start_time).count();
|
||||
const auto state = arm_->getJointState();
|
||||
ASSERT_EQ(state.position.size(), kDof);
|
||||
ASSERT_EQ(state.velocity.size(), kDof);
|
||||
samples.push_back(Sample{time_s,
|
||||
state.position,
|
||||
state.velocity,
|
||||
arm_->getSpeedLCommandTwistBase()});
|
||||
|
||||
const double stop = stop_time_s.load();
|
||||
if (stop >= 0.0 && time_s > stop + 0.8 && !arm_->busy()) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
command_thread.join();
|
||||
|
||||
ASSERT_TRUE(command_result.ok()) << command_result.message;
|
||||
const double stop = stop_time_s.load();
|
||||
ASSERT_GT(stop, 1.8);
|
||||
ASSERT_LT(stop, 2.5);
|
||||
ASSERT_FALSE(arm_->busy());
|
||||
|
||||
const auto csv_path = std::filesystem::path("/tmp") /
|
||||
("speedl_stop_acc_" + numberForFile(acceleration) +
|
||||
"_" + (direction > 0.0 ? "pos" : "neg") + ".csv");
|
||||
Metrics metrics;
|
||||
writeCsv(csv_path, samples, stop, *analysis_pinocchio_solver_, metrics);
|
||||
std::cout << "[SpeedLStopTest] acceleration=" << acceleration
|
||||
<< ", direction=" << direction
|
||||
<< ", csv=" << csv_path
|
||||
<< ", max_qd_before=" << metrics.max_qd_before_stop
|
||||
<< ", max_qd_after=" << metrics.max_qd_after_stop
|
||||
<< ", max_qdd=" << metrics.max_qdd
|
||||
<< ", max_tcp_speed_before=" << metrics.max_tcp_linear_speed_before_stop
|
||||
<< ", max_tcp_speed_after=" << metrics.max_tcp_linear_speed_after_stop
|
||||
<< ", max_tcp_acceleration=" << metrics.max_tcp_linear_acceleration
|
||||
<< std::endl;
|
||||
|
||||
// Stopping must not create a new velocity peak. A small tolerance
|
||||
// allows one 1 ms feedback sample of transport jitter.
|
||||
EXPECT_LE(metrics.max_qd_after_stop,
|
||||
metrics.max_qd_before_stop + 0.25)
|
||||
<< "post-stop joint velocity peak for acceleration=" << acceleration
|
||||
<< ", direction=" << direction;
|
||||
EXPECT_LE(metrics.max_tcp_linear_speed_after_stop,
|
||||
metrics.max_tcp_linear_speed_before_stop + 0.01)
|
||||
<< "post-stop TCP velocity peak for acceleration=" << acceleration
|
||||
<< ", direction=" << direction;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace cmvr::device
|
||||
Loading…
Reference in New Issue
Block a user