Compare commits
No commits in common. "ffccdea26da7f3d88a130130f7acab8d4dc153fd" and "96e3bcf62472692287146fcf71e4e8426c1085ca" have entirely different histories.
ffccdea26d
...
96e3bcf624
@ -11,7 +11,3 @@ target_link_libraries(arm_control
|
||||
|
||||
add_library(cmvr_es::algorithms::arm_control ALIAS arm_control)
|
||||
install(TARGETS arm_control LIBRARY DESTINATION lib)
|
||||
|
||||
add_executable(cartesian_velocity_controller_test src/cartesian_velocity_controller_test.cpp)
|
||||
target_link_libraries(cartesian_velocity_controller_test PRIVATE
|
||||
cmvr_es::algorithms::arm_control gtest gtest_main pthread)
|
||||
|
||||
@ -46,9 +46,6 @@ public:
|
||||
double duration,
|
||||
FrameType frame);
|
||||
Result stop(std::optional<double> acceleration = std::nullopt);
|
||||
Result speedL(const CartesianVelocity& velocity, const SpeedLOptions& options,
|
||||
double duration, FrameType frame);
|
||||
SpeedLReference getReference() const;
|
||||
void shutdown();
|
||||
|
||||
bool busy() const { return busy_.load(); }
|
||||
@ -80,9 +77,6 @@ private:
|
||||
CartesianVelocity target_twist_{};
|
||||
FrameType target_frame_{FrameType::Base};
|
||||
double target_acceleration_{0.25};
|
||||
SpeedLOptions target_options_{};
|
||||
SpeedLReference reference_{};
|
||||
CartesianVelocity command_twist_snapshot_{};
|
||||
std::uint64_t command_version_{0};
|
||||
std::atomic<bool> busy_{false};
|
||||
};
|
||||
|
||||
@ -61,26 +61,16 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
|
||||
const double duration,
|
||||
const FrameType frame)
|
||||
{
|
||||
SpeedLOptions options;
|
||||
options.acceleration = acceleration;
|
||||
return speedL(velocity, options, duration, frame);
|
||||
}
|
||||
|
||||
Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
|
||||
const SpeedLOptions& options,
|
||||
const double duration, const FrameType frame)
|
||||
{
|
||||
const double acceleration = options.acceleration;
|
||||
if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 ||
|
||||
!std::isfinite(acceleration) || acceleration <= 0.0 ||
|
||||
!std::isfinite(duration) || duration < 0.0 || !std::isfinite(twistNorm_(velocity)) ||
|
||||
(options.linear_jerk && (!std::isfinite(*options.linear_jerk) || *options.linear_jerk <= 0.0))) {
|
||||
if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
|
||||
return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
|
||||
}
|
||||
if (!worker_ || !worker_->joinable()) {
|
||||
if (busy_.exchange(true)) {
|
||||
return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
|
||||
}
|
||||
} else {
|
||||
// speedL is a streaming command: an existing worker may receive a new target.
|
||||
busy_.store(true);
|
||||
}
|
||||
|
||||
ensureWorkerStarted_();
|
||||
@ -91,10 +81,7 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
|
||||
target_twist_ = velocity;
|
||||
target_acceleration_ = acceleration;
|
||||
target_frame_ = frame;
|
||||
target_options_ = options;
|
||||
reference_ = {};
|
||||
command_active_ = true;
|
||||
busy_.store(true);
|
||||
command_version = ++command_version_;
|
||||
}
|
||||
cv_.notify_all();
|
||||
@ -156,14 +143,10 @@ void CartesianVelocityController::shutdown()
|
||||
|
||||
CartesianVelocity CartesianVelocityController::getCommandTwistBase() const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return command_twist_snapshot_;
|
||||
}
|
||||
|
||||
SpeedLReference CartesianVelocityController::getReference() const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return reference_;
|
||||
if (!planner_) {
|
||||
return {};
|
||||
}
|
||||
return planner_->getSpeedLCommandTwistBase();
|
||||
}
|
||||
|
||||
void CartesianVelocityController::ensureWorkerStarted_()
|
||||
@ -184,9 +167,6 @@ void CartesianVelocityController::workerLoop_()
|
||||
CartesianVelocity target_twist;
|
||||
double acceleration = 0.25;
|
||||
FrameType target_frame = FrameType::Base;
|
||||
SpeedLOptions options;
|
||||
std::uint64_t applied_version = 0;
|
||||
bool capture_reference = false;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
cv_.wait(lock, [&]() {
|
||||
@ -215,13 +195,9 @@ void CartesianVelocityController::workerLoop_()
|
||||
target_twist = target_twist_;
|
||||
acceleration = target_acceleration_;
|
||||
target_frame = target_frame_;
|
||||
options = target_options_;
|
||||
options.acceleration = acceleration;
|
||||
applied_version = command_version_;
|
||||
capture_reference = options.capture_reference && !reference_.valid;
|
||||
}
|
||||
|
||||
if (!planner_->updateSpeedLLimits(options)) {
|
||||
if (!planner_->updateSpeedLAcceleration(acceleration)) {
|
||||
if (twistNorm_(target_twist) < config_.stop_twist_norm) {
|
||||
abortCommand_();
|
||||
break;
|
||||
@ -245,13 +221,6 @@ void CartesianVelocityController::workerLoop_()
|
||||
}
|
||||
|
||||
std::vector<double> qd_cmd;
|
||||
SpeedLReference reference;
|
||||
if (capture_reference &&
|
||||
!planner_->captureSpeedLReference(q_now, target_twist, target_frame, reference)) {
|
||||
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] cannot capture motion reference";
|
||||
abortCommand_();
|
||||
break;
|
||||
}
|
||||
if (!planner_->speedLStep(target_twist, dt, q_now, qd_now, qd_cmd, target_frame)) {
|
||||
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] speedLStep failed, target_twist=["
|
||||
<< target_twist.vx << ", " << target_twist.vy << ", "
|
||||
@ -280,32 +249,15 @@ void CartesianVelocityController::workerLoop_()
|
||||
continue;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
command_twist_snapshot_ = planner_->getSpeedLCommandTwistBase();
|
||||
if (capture_reference && command_version_ == applied_version) {
|
||||
reference.command_version = applied_version;
|
||||
reference_ = reference;
|
||||
// Lock a captured Tool-frame translation in Base for the
|
||||
// entire command, matching its distance reference axis.
|
||||
target_twist_ = reference.target_base;
|
||||
target_frame_ = FrameType::Base;
|
||||
}
|
||||
}
|
||||
|
||||
if (twistNorm_(target_twist) < config_.stop_twist_norm &&
|
||||
velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm &&
|
||||
velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
// A newer target may have arrived during planning/I/O.
|
||||
if (command_version_ != applied_version) continue;
|
||||
command_active_ = false;
|
||||
// Serialize the final zero and busy transition with new
|
||||
// submissions, not only the version comparison.
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
}
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
break;
|
||||
}
|
||||
|
||||
@ -327,8 +279,6 @@ void CartesianVelocityController::requestStop_(const std::optional<double> accel
|
||||
target_frame_ = FrameType::Base;
|
||||
target_acceleration_ = acceleration.has_value() ? *acceleration
|
||||
: config_.stop_acceleration;
|
||||
target_options_.acceleration = target_acceleration_;
|
||||
target_options_.capture_reference = false;
|
||||
command_active_ = true;
|
||||
++command_version_;
|
||||
}
|
||||
@ -342,9 +292,9 @@ void CartesianVelocityController::abortCommand_()
|
||||
command_active_ = false;
|
||||
target_twist_ = {};
|
||||
target_frame_ = FrameType::Base;
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
}
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
}
|
||||
|
||||
void CartesianVelocityController::sendZero_()
|
||||
|
||||
@ -1,127 +0,0 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include "algorithms/controllers/arm_control/include/cartesian_velocity_controller.h"
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <limits>
|
||||
|
||||
namespace cmvr::device {
|
||||
namespace {
|
||||
using namespace std::chrono_literals;
|
||||
class Planner final : public CartesianMotionPlanner {
|
||||
public:
|
||||
bool configureSpeedL(const config::SpeedLPlannerConfig&, std::size_t) override { return true; }
|
||||
bool configureMoveL(const config::MoveLPlannerConfig&) override { return true; }
|
||||
bool planMoveL(const CartesianPose&, const std::vector<double>&, const std::vector<double>&,
|
||||
double, double, double, FrameType, CartesianJointTrajectory&) override { return false; }
|
||||
bool updateSpeedLAcceleration(double) override { return true; }
|
||||
bool updateSpeedLLimits(const SpeedLOptions& options) override {
|
||||
applied_jerk.store(options.linear_jerk.value_or(10.0)); return true;
|
||||
}
|
||||
bool captureSpeedLReference(const std::vector<double>&, const CartesianVelocity& target,
|
||||
FrameType, SpeedLReference& ref) override {
|
||||
ref.valid = true;
|
||||
ref.tcp_pose_base.y = .5;
|
||||
ref.target_base.vx = target.vy;
|
||||
return true;
|
||||
}
|
||||
bool speedLStep(const CartesianVelocity& v, double, const std::vector<double>&,
|
||||
const std::vector<double>&, std::vector<double>& out, FrameType) override {
|
||||
current = v; out = {v.vy}; return true;
|
||||
}
|
||||
CartesianVelocity getSpeedLCommandTwistBase() const override { return current; }
|
||||
CartesianVelocity current;
|
||||
std::atomic<double> applied_jerk{0.0};
|
||||
};
|
||||
TEST(CartesianVelocityController, CompletedStopCannotClearNewReversal) {
|
||||
std::mutex mutex;
|
||||
std::condition_variable cv;
|
||||
bool forward_sent = false, block_zero = false, zero_entered = false;
|
||||
bool release_zero = false, reverse_sent = false;
|
||||
auto planner = std::make_shared<Planner>();
|
||||
CartesianVelocityController controller({}, planner, 1,
|
||||
[](auto& q, auto& qd) { q = {0}; qd = {0}; return true; },
|
||||
[&](const JointVelocityCommand& command, double) {
|
||||
std::unique_lock<std::mutex> lock(mutex);
|
||||
forward_sent |= command.velocity[0] > 0;
|
||||
reverse_sent |= command.velocity[0] < 0;
|
||||
if (command.velocity[0] == 0 && block_zero && !zero_entered) {
|
||||
zero_entered = true; cv.notify_all();
|
||||
cv.wait_for(lock, 2s, [&] { return release_zero; });
|
||||
}
|
||||
cv.notify_all(); return Result::success();
|
||||
});
|
||||
CartesianVelocity v; v.vy = .08;
|
||||
ASSERT_TRUE(controller.speedL(v, 3, 0, FrameType::Base).ok());
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex);
|
||||
ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return forward_sent; }));
|
||||
block_zero = true;
|
||||
}
|
||||
ASSERT_TRUE(controller.stop(3).ok());
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex);
|
||||
ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return zero_entered; }));
|
||||
}
|
||||
v.vy = -.08;
|
||||
ASSERT_TRUE(controller.speedL(v, 3, 0, FrameType::Base).ok());
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex);
|
||||
release_zero = true; cv.notify_all();
|
||||
EXPECT_TRUE(cv.wait_for(lock, 1s, [&] { return reverse_sent; }));
|
||||
}
|
||||
EXPECT_TRUE(controller.busy());
|
||||
controller.shutdown();
|
||||
}
|
||||
TEST(CartesianVelocityController, PublishesReferenceAfterSendAndKeepsCommandLimits) {
|
||||
std::mutex mutex;
|
||||
std::condition_variable cv;
|
||||
bool entered = false, release = false;
|
||||
auto planner = std::make_shared<Planner>();
|
||||
CartesianVelocityController controller({}, planner, 1,
|
||||
[](auto& q, auto& qd) { q = {0}; qd = {0}; return true; },
|
||||
[&](const JointVelocityCommand&, double) {
|
||||
std::unique_lock<std::mutex> lock(mutex);
|
||||
if (!entered) {
|
||||
entered = true; cv.notify_all();
|
||||
cv.wait_for(lock, 2s, [&] { return release; });
|
||||
}
|
||||
return Result::success();
|
||||
});
|
||||
CartesianVelocity v; v.vy = -.08;
|
||||
SpeedLOptions options;
|
||||
options.acceleration = 3;
|
||||
options.linear_jerk = 60;
|
||||
options.capture_reference = true;
|
||||
ASSERT_TRUE(controller.speedL(v, options, 0, FrameType::Tool).ok());
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex);
|
||||
ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return entered; }));
|
||||
EXPECT_FALSE(controller.getReference().valid);
|
||||
release = true; cv.notify_all();
|
||||
}
|
||||
const auto deadline = std::chrono::steady_clock::now() + 1s;
|
||||
while (!controller.getReference().valid && std::chrono::steady_clock::now() < deadline)
|
||||
std::this_thread::sleep_for(1ms);
|
||||
const auto ref = controller.getReference();
|
||||
EXPECT_TRUE(ref.valid);
|
||||
EXPECT_GT(ref.command_version, 0U);
|
||||
EXPECT_DOUBLE_EQ(ref.tcp_pose_base.y, .5);
|
||||
EXPECT_DOUBLE_EQ(ref.target_base.vx, -.08);
|
||||
EXPECT_DOUBLE_EQ(planner->applied_jerk.load(), 60);
|
||||
controller.shutdown();
|
||||
}
|
||||
TEST(CartesianVelocityController, RejectsInvalidMotionLimitsBeforeStarting) {
|
||||
auto planner = std::make_shared<Planner>();
|
||||
CartesianVelocityController controller({}, planner, 1,
|
||||
[](auto&, auto&) { return false; },
|
||||
[](const auto&, double) { return Result::success(); });
|
||||
SpeedLOptions options;
|
||||
options.linear_jerk = std::numeric_limits<double>::quiet_NaN();
|
||||
EXPECT_FALSE(controller.speedL({}, options, 0, FrameType::Base).ok());
|
||||
options.linear_jerk = -1;
|
||||
EXPECT_FALSE(controller.speedL({}, options, 0, FrameType::Base).ok());
|
||||
EXPECT_FALSE(controller.busy());
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -25,11 +25,3 @@ target_link_libraries(toppra_joint_motion_planner_test
|
||||
gtest
|
||||
gtest_main
|
||||
)
|
||||
|
||||
add_executable(pinocchio_speedl_limits_test
|
||||
cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp
|
||||
)
|
||||
target_compile_definitions(pinocchio_speedl_limits_test PRIVATE
|
||||
CMVR_TEST_SOURCE_DIR="${PROJECT_SOURCE_DIR}")
|
||||
target_link_libraries(pinocchio_speedl_limits_test PRIVATE
|
||||
cmvr_es::algorithms::arm_motion gtest gtest_main)
|
||||
|
||||
@ -44,11 +44,6 @@ public:
|
||||
FrameType frame) = 0;
|
||||
|
||||
virtual bool updateSpeedLAcceleration(double acceleration) = 0;
|
||||
virtual bool updateSpeedLLimits(const SpeedLOptions& options) {
|
||||
return !options.linear_jerk && updateSpeedLAcceleration(options.acceleration);
|
||||
}
|
||||
virtual bool captureSpeedLReference(const std::vector<double>&,
|
||||
const CartesianVelocity&, FrameType, SpeedLReference&) { return false; }
|
||||
virtual CartesianVelocity getSpeedLCommandTwistBase() const = 0;
|
||||
};
|
||||
|
||||
|
||||
@ -37,9 +37,6 @@ public:
|
||||
FrameType frame) override;
|
||||
|
||||
bool updateSpeedLAcceleration(double acceleration) override;
|
||||
bool updateSpeedLLimits(const SpeedLOptions& options) override;
|
||||
bool captureSpeedLReference(const std::vector<double>& q, const CartesianVelocity& target,
|
||||
FrameType frame, SpeedLReference& reference) override;
|
||||
CartesianVelocity getSpeedLCommandTwistBase() const override;
|
||||
|
||||
private:
|
||||
@ -99,8 +96,6 @@ private:
|
||||
Eigen::Vector3d speedl_line_start_tcp_base_{Eigen::Vector3d::Zero()};
|
||||
Eigen::Vector3d speedl_line_direction_base_{Eigen::Vector3d::Zero()};
|
||||
double speedl_applied_acceleration_{0.25};
|
||||
double speedl_applied_angular_acceleration_{0.25};
|
||||
double speedl_applied_linear_jerk_{-1.0};
|
||||
bool speedl_line_check_active_{false};
|
||||
bool speedl_line_deviation_warned_{false};
|
||||
bool speedl_line_direction_warned_{false};
|
||||
|
||||
@ -121,7 +121,6 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne
|
||||
}
|
||||
|
||||
cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_);
|
||||
speedl_applied_linear_jerk_ = -1.0;
|
||||
|
||||
prev_qdot_command_.assign(dof, 0.0);
|
||||
speedl_command_twist_base_.setZero();
|
||||
@ -130,7 +129,6 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne
|
||||
speedl_line_direction_warned_ = false;
|
||||
speedl_stop_active_ = false;
|
||||
speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0);
|
||||
speedl_applied_angular_acceleration_ = positiveOr(speedl_config_.angular_acceleration_max(), 5.0);
|
||||
speedl_configured_ = true;
|
||||
return true;
|
||||
}
|
||||
@ -922,8 +920,7 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
|
||||
speedl_stop_active_ = false;
|
||||
|
||||
// 从静止开始一个新的 speedL command。
|
||||
if (!twist_limiter_.isMoving() &&
|
||||
speedl_command_twist_base_.squaredNorm() <= 1e-12) {
|
||||
if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
|
||||
twist_limiter_.initialize(
|
||||
Eigen::Matrix<double, 6, 1>::Zero());
|
||||
}
|
||||
@ -1252,60 +1249,18 @@ std::string PinocchioCartesianMotionPlanner::describeJointLimitCandidates_(
|
||||
|
||||
bool PinocchioCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration)
|
||||
{
|
||||
SpeedLOptions options;
|
||||
options.acceleration = acceleration;
|
||||
return updateSpeedLLimits(options);
|
||||
}
|
||||
|
||||
bool PinocchioCartesianMotionPlanner::updateSpeedLLimits(const SpeedLOptions& options)
|
||||
{
|
||||
const double jerk_max = positiveOr(speedl_config_.linear_jerk_max(), 10.0);
|
||||
const double requested_jerk = options.linear_jerk.value_or(jerk_max);
|
||||
// Validate before clamping: invalid requests must not become valid limits.
|
||||
if (!speedl_configured_ || !std::isfinite(options.acceleration) || options.acceleration <= 0.0 ||
|
||||
!std::isfinite(requested_jerk) || requested_jerk <= 0.0) {
|
||||
if (!speedl_configured_ || acceleration <= 0.0) {
|
||||
return false;
|
||||
}
|
||||
const double acceleration = std::min(options.acceleration,
|
||||
positiveOr(speedl_config_.linear_acceleration_max(), 5.0));
|
||||
const double angular_acceleration = std::min(options.acceleration,
|
||||
positiveOr(speedl_config_.angular_acceleration_max(), 5.0));
|
||||
const double jerk = std::min(requested_jerk, jerk_max);
|
||||
if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9 &&
|
||||
std::abs(speedl_applied_angular_acceleration_ - angular_acceleration) <= 1e-9 &&
|
||||
std::abs(speedl_applied_linear_jerk_ - jerk) <= 1e-9) {
|
||||
if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) {
|
||||
return true;
|
||||
}
|
||||
|
||||
twist_limiter_.setLinearConstraints(
|
||||
positiveOr(speedl_config_.linear_velocity_max(), .55),
|
||||
acceleration, jerk);
|
||||
twist_limiter_.setAngularConstraints(
|
||||
positiveOr(speedl_config_.angular_velocity_max(), 1.0),
|
||||
angular_acceleration, positiveOr(speedl_config_.angular_jerk_max(), 12.0));
|
||||
cartesian_motion::updateTwistLimiterAcceleration(
|
||||
twist_limiter_,
|
||||
speedl_config_,
|
||||
acceleration);
|
||||
speedl_applied_acceleration_ = acceleration;
|
||||
speedl_applied_angular_acceleration_ = angular_acceleration;
|
||||
speedl_applied_linear_jerk_ = jerk;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PinocchioCartesianMotionPlanner::captureSpeedLReference(
|
||||
const std::vector<double>& q, const CartesianVelocity& target,
|
||||
const FrameType frame, SpeedLReference& reference)
|
||||
{
|
||||
Eigen::Matrix4d pose;
|
||||
if (!solver_ || !solver_->fk(q, pose, true) || !pose.allFinite()) return false;
|
||||
auto twist = common::math::velocityToVector(target);
|
||||
if (frame == FrameType::Tool) {
|
||||
const Eigen::Matrix3d rotation = pose.block<3, 3>(0, 0);
|
||||
twist.head<3>() = rotation * twist.head<3>().eval();
|
||||
twist.tail<3>() = rotation * twist.tail<3>().eval();
|
||||
} else if (frame != FrameType::Base) {
|
||||
return false;
|
||||
}
|
||||
reference.tcp_pose_base = common::math::matrixToPose(pose);
|
||||
reference.target_base = common::math::vectorToVelocity(twist);
|
||||
reference.valid = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -1,157 +0,0 @@
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
#include <limits>
|
||||
|
||||
#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
|
||||
#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h"
|
||||
#include "common/io/proto_file_io.h"
|
||||
#include "common/math/transform_math.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
namespace {
|
||||
constexpr double kDt = .001;
|
||||
using Twist = Eigen::Matrix<double, 6, 1>;
|
||||
|
||||
// Exercise the production planner and real URDF kinematics, without motor I/O.
|
||||
class PinocchioSpeedLLimits : public ::testing::Test {
|
||||
protected:
|
||||
void SetUp() override {
|
||||
const auto root = std::filesystem::path(CMVR_TEST_SOURCE_DIR);
|
||||
config::ArmRootConfig arms;
|
||||
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
|
||||
(root / "cmvr-es/config/devices/arm/arm.pb.txt").string(), &arms));
|
||||
ASSERT_GT(arms.arm().robot_arms_size(), 0);
|
||||
auto ik = arms.arm().robot_arms(0).kinematics().pinocchio_dls_ik_solver();
|
||||
ik.set_urdf_path((root / "model/xiaoyan_description/dual_arm.urdf").string());
|
||||
solver_ = std::make_shared<PinocchioDlsIKSolver>(ik);
|
||||
ASSERT_TRUE(solver_->init());
|
||||
planner_ = std::make_unique<PinocchioCartesianMotionPlanner>(solver_);
|
||||
limits_.set_linear_velocity_max(.2);
|
||||
limits_.set_linear_acceleration_max(.4);
|
||||
limits_.set_linear_jerk_max(2);
|
||||
limits_.set_angular_velocity_max(1);
|
||||
limits_.set_angular_acceleration_max(.8);
|
||||
limits_.set_angular_jerk_max(4);
|
||||
limits_.set_enforce_joint_acceleration_limits(false);
|
||||
ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
|
||||
}
|
||||
|
||||
struct Peaks { double velocity{0}, acceleration{0}, jerk{0}; };
|
||||
Peaks sample(const CartesianVelocity& target, const SpeedLOptions& options,
|
||||
bool angular = false, int steps = 2200, bool legacy_api = false) {
|
||||
Peaks peaks;
|
||||
Twist previous = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase());
|
||||
Twist previous_acceleration = Twist::Zero();
|
||||
for (int i = 0; i < steps; ++i) {
|
||||
const bool updated = legacy_api
|
||||
? planner_->updateSpeedLAcceleration(options.acceleration)
|
||||
: planner_->updateSpeedLLimits(options);
|
||||
if (!updated || !planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)) {
|
||||
ADD_FAILURE() << "Planning failed at sample " << i;
|
||||
break;
|
||||
}
|
||||
const Twist velocity = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase());
|
||||
const Twist acceleration = (velocity - previous) / kDt;
|
||||
const Twist jerk = (acceleration - previous_acceleration) / kDt;
|
||||
const int offset = angular ? 3 : 0;
|
||||
peaks.velocity = std::max(peaks.velocity, velocity.segment<3>(offset).norm());
|
||||
peaks.acceleration = std::max(peaks.acceleration, acceleration.segment<3>(offset).norm());
|
||||
peaks.jerk = std::max(peaks.jerk, jerk.segment<3>(offset).norm());
|
||||
previous = velocity;
|
||||
previous_acceleration = acceleration;
|
||||
}
|
||||
return peaks;
|
||||
}
|
||||
|
||||
void expectPeaks(const Peaks& p, double velocity, double acceleration, double jerk) {
|
||||
// These trajectories contain plateaus: limits must be reached, as well
|
||||
// as obeyed, so an unintended smaller cap cannot pass the test.
|
||||
EXPECT_NEAR(p.velocity, velocity, 1e-8);
|
||||
EXPECT_NEAR(p.acceleration, acceleration, 1e-8);
|
||||
EXPECT_NEAR(p.jerk, jerk, 1e-6);
|
||||
}
|
||||
|
||||
std::shared_ptr<PinocchioDlsIKSolver> solver_;
|
||||
std::unique_ptr<PinocchioCartesianMotionPlanner> planner_;
|
||||
config::SpeedLPlannerConfig limits_;
|
||||
const std::vector<double> q_{.25, 1, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0};
|
||||
const std::vector<double> qd_ = std::vector<double>(7, 0);
|
||||
std::vector<double> command_;
|
||||
};
|
||||
|
||||
TEST_F(PinocchioSpeedLLimits, ExcessiveRequestsRespectAllLinearLimits) {
|
||||
SpeedLOptions options;
|
||||
options.acceleration = 60;
|
||||
options.linear_jerk = 60;
|
||||
CartesianVelocity target;
|
||||
target.vx = .6;
|
||||
target.vy = .8;
|
||||
expectPeaks(sample(target, options), .2, .4, 2);
|
||||
}
|
||||
|
||||
TEST_F(PinocchioSpeedLLimits, LowerRequestsRemainEffective) {
|
||||
SpeedLOptions options;
|
||||
options.acceleration = .15;
|
||||
options.linear_jerk = .8;
|
||||
CartesianVelocity target;
|
||||
target.vy = 1;
|
||||
expectPeaks(sample(target, options), .2, .15, .8);
|
||||
}
|
||||
|
||||
TEST_F(PinocchioSpeedLLimits, OmittedJerkRestoresArmLimit) {
|
||||
SpeedLOptions options;
|
||||
options.acceleration = .4;
|
||||
options.linear_jerk = .3;
|
||||
ASSERT_TRUE(planner_->updateSpeedLLimits(options));
|
||||
options.linear_jerk.reset();
|
||||
CartesianVelocity target;
|
||||
target.vy = 1;
|
||||
expectPeaks(sample(target, options), .2, .4, 2);
|
||||
}
|
||||
|
||||
TEST_F(PinocchioSpeedLLimits, LegacyAccelerationAndStopAreCapped) {
|
||||
SpeedLOptions options;
|
||||
options.acceleration = 60;
|
||||
CartesianVelocity target;
|
||||
target.vy = 1;
|
||||
expectPeaks(sample(target, options, false, 1200, true), .2, .4, 2);
|
||||
const auto stop = sample({}, options, false, 1200, true);
|
||||
EXPECT_LE(stop.velocity, .2);
|
||||
EXPECT_NEAR(stop.acceleration, .4, 1e-8);
|
||||
EXPECT_NEAR(stop.jerk, 2, 1e-6);
|
||||
EXPECT_NEAR(planner_->getSpeedLCommandTwistBase().vy, 0, 1e-12);
|
||||
}
|
||||
|
||||
TEST_F(PinocchioSpeedLLimits, AngularAccelerationHasItsOwnCapAndCache) {
|
||||
limits_.set_linear_acceleration_max(.2);
|
||||
ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
|
||||
SpeedLOptions options;
|
||||
options.acceleration = .3;
|
||||
ASSERT_TRUE(planner_->updateSpeedLLimits(options));
|
||||
// Linear effective acceleration stays at .2, but angular must change.
|
||||
options.acceleration = .6;
|
||||
CartesianVelocity target;
|
||||
target.wy = 2;
|
||||
expectPeaks(sample(target, options, true), 1, .6, 4);
|
||||
ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size()));
|
||||
options.acceleration = 60;
|
||||
expectPeaks(sample(target, options, true), 1, .8, 4);
|
||||
}
|
||||
|
||||
TEST_F(PinocchioSpeedLLimits, InvalidRequestsAreRejectedBeforeClamping) {
|
||||
for (double invalid : {0.0, -1.0, std::numeric_limits<double>::infinity(),
|
||||
std::numeric_limits<double>::quiet_NaN()}) {
|
||||
SpeedLOptions options;
|
||||
options.acceleration = invalid;
|
||||
EXPECT_FALSE(planner_->updateSpeedLLimits(options));
|
||||
EXPECT_FALSE(planner_->updateSpeedLAcceleration(invalid));
|
||||
options.acceleration = .3;
|
||||
options.linear_jerk = invalid;
|
||||
EXPECT_FALSE(planner_->updateSpeedLLimits(options));
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
} // namespace cmvr::device
|
||||
@ -1,8 +1,6 @@
|
||||
#ifndef CMVR_ES_TWIST_LIMITER_CONFIG_H
|
||||
#define CMVR_ES_TWIST_LIMITER_CONFIG_H
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
|
||||
#include "cmvr/config/arm_config/arm_config.pb.h"
|
||||
#include "common/config/config_files.h"
|
||||
@ -30,8 +28,6 @@ inline void configureTwistLimiterFromSpeedLConfig(
|
||||
? config.linear_reverse_cos_threshold()
|
||||
: -0.8660254037844386,
|
||||
positiveOr(config.linear_reverse_switch_speed_threshold(), 1e-3));
|
||||
limiter.setContinuousLinearReversal(!config.has_continuous_linear_reversal() ||
|
||||
config.continuous_linear_reversal());
|
||||
limiter.initialize(Eigen::Matrix<double, 6, 1>::Zero());
|
||||
}
|
||||
|
||||
@ -43,10 +39,10 @@ inline void updateTwistLimiterAcceleration(
|
||||
using cmvr::common::config::positiveOr;
|
||||
|
||||
limiter.setLinearConstraints(positiveOr(config.linear_velocity_max(), 0.55),
|
||||
std::min(acceleration, positiveOr(config.linear_acceleration_max(), 5.0)),
|
||||
acceleration,
|
||||
positiveOr(config.linear_jerk_max(), 10.0));
|
||||
limiter.setAngularConstraints(positiveOr(config.angular_velocity_max(), 1.0),
|
||||
std::min(acceleration, positiveOr(config.angular_acceleration_max(), 5.0)),
|
||||
acceleration,
|
||||
positiveOr(config.angular_jerk_max(), 12.0));
|
||||
}
|
||||
|
||||
|
||||
@ -20,10 +20,6 @@ target_link_libraries(base_motion PUBLIC
|
||||
)
|
||||
|
||||
add_library(cmvr_es::base_motion ALIAS base_motion)
|
||||
add_executable(cartesian_twist_limiter_reversal_test
|
||||
cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp)
|
||||
target_link_libraries(cartesian_twist_limiter_reversal_test PRIVATE
|
||||
cmvr_es::base_motion gtest gtest_main)
|
||||
install(TARGETS base_motion LIBRARY DESTINATION lib)
|
||||
|
||||
add_executable(toppra_multi_waypoint_test
|
||||
|
||||
@ -16,8 +16,7 @@ enum class CartesianFrame
|
||||
* @brief 6维末端 twist 在线限幅器（基于 SCurveVelocityPlanner1D）
|
||||
*
|
||||
* 线速度部分：
|
||||
* - 固定轴上的有符号速度使用 SCurveVelocityPlanner1D 做 jerk-limited 规划
|
||||
* - 同轴反向连续过零，保留加速度；可显式选择旧的停止后换向策略
|
||||
* - 模长使用 SCurveVelocityPlanner1D 做 jerk-limited 速度规划
|
||||
* - 运动中锁定当前方向，不做方向插值
|
||||
* - 若目标方向与当前方向不共线，则采用“先减速到0，再切方向”的 switch policy
|
||||
*
|
||||
@ -68,10 +67,6 @@ public:
|
||||
void setLinearReverseSwitchPolicy(double cos_threshold,
|
||||
double switch_speed_threshold);
|
||||
|
||||
// Same-axis reversal uses a signed velocity without resetting acceleration
|
||||
// at zero. Disable only to reproduce the legacy stop-then-switch policy.
|
||||
void setContinuousLinearReversal(bool enabled) { continuous_linear_reversal_ = enabled; }
|
||||
|
||||
/**
|
||||
* @brief 初始化当前 twist 状态
|
||||
*
|
||||
@ -125,7 +120,7 @@ public:
|
||||
*
|
||||
* 作用：
|
||||
* - 更新当前执行方向
|
||||
* - 线速度使用固定轴投影（保留正负号），角速度使用模长
|
||||
* - 用测得模长与模长加速度同步两个 planner
|
||||
* - keep_target=true 时：
|
||||
* - 若测量状态仍贴着当前 profile，则保持当前 profile
|
||||
* - 否则由 planner 内部从测量状态重规划到当前目标
|
||||
@ -183,7 +178,6 @@ private:
|
||||
double linear_reverse_switch_speed_threshold_;
|
||||
double angular_switch_speed_threshold_;
|
||||
bool emergency_stop_active_;
|
||||
bool continuous_linear_reversal_{true};
|
||||
|
||||
// 目标/当前状态
|
||||
Twist target_twist_input_;
|
||||
|
||||
@ -249,8 +249,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base,
|
||||
const Eigen::Vector3d v_meas = measured_twist_base.head<3>();
|
||||
const Eigen::Vector3d w_meas = measured_twist_base.tail<3>();
|
||||
|
||||
const bool signed_linear = continuous_linear_reversal_ && current_linear_dir_base_.norm() > EPSILON;
|
||||
const double v_norm = signed_linear ? v_meas.dot(current_linear_dir_base_) : v_meas.norm();
|
||||
const double v_norm = v_meas.norm();
|
||||
const double w_norm = w_meas.norm();
|
||||
|
||||
double v_acc = 0.0;
|
||||
@ -274,7 +273,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base,
|
||||
last_measured_twist_base_ = measured_twist_base;
|
||||
has_measured_sync_ = true;
|
||||
|
||||
// 线速度按固定轴投影保留正负号；角速度仍按模长同步 planner。
|
||||
// 用测得模长/模长加速度同步 planner 当前状态。
|
||||
// keep_target=true:
|
||||
// - 若测量值仍贴着当前 profile，则继续沿旧 profile 走
|
||||
// - 否则从测量状态重规划到当前目标
|
||||
@ -290,7 +289,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base,
|
||||
target_twist_base_.setZero();
|
||||
}
|
||||
|
||||
if (!signed_linear && v_norm > EPSILON) {
|
||||
if (v_norm > EPSILON) {
|
||||
current_linear_dir_base_ = v_meas / v_norm;
|
||||
last_target_linear_dir_base_ = current_linear_dir_base_;
|
||||
}
|
||||
@ -465,18 +464,7 @@ CartesianTwistLimiter::update(double dt, const Eigen::Matrix3d& base_R_tool)
|
||||
const bool must_switch_axis =
|
||||
!same_axis || dir_dot < linear_reverse_cos_threshold_;
|
||||
|
||||
if (continuous_linear_reversal_ && same_axis) {
|
||||
// Keep the axis fixed: the scalar profile carries the direction sign.
|
||||
// Crossing zero is an interior point, with continuous acceleration.
|
||||
linear_norm_planner_.setTargetVelocity(v_des.dot(current_linear_dir_base_));
|
||||
} else if (continuous_linear_reversal_ &&
|
||||
(std::abs(v_cur_norm) > EPSILON ||
|
||||
std::abs(linear_norm_planner_.getAcceleration()) > EPSILON ||
|
||||
linear_norm_planner_.hasActiveProfile())) {
|
||||
// A different axis may only be adopted after the old profile settles.
|
||||
linear_norm_planner_.setTargetVelocity(0.0);
|
||||
} else if (!continuous_linear_reversal_ && must_switch_axis &&
|
||||
v_cur_norm > linear_reverse_switch_speed_threshold_) {
|
||||
if (must_switch_axis && v_cur_norm > linear_reverse_switch_speed_threshold_) {
|
||||
linear_norm_planner_.setTargetVelocity(0.0);
|
||||
} else {
|
||||
current_linear_dir_base_ = v_target_dir;
|
||||
|
||||
@ -1,97 +0,0 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
|
||||
|
||||
namespace cmvr {
|
||||
namespace {
|
||||
using Twist = CartesianTwistLimiter::Twist;
|
||||
constexpr double dt = .001;
|
||||
Twist y(double v) { Twist t = Twist::Zero(); t.y() = v; return t; }
|
||||
struct Motion { double peak{0}, reverse_time{-1}, return_time{-1}; };
|
||||
Motion reverse(bool continuous, int approach_ticks) {
|
||||
CartesianTwistLimiter limiter;
|
||||
limiter.setLinearConstraints(.55, 5, 10);
|
||||
limiter.setContinuousLinearReversal(continuous);
|
||||
limiter.initialize(approach_ticks == 0 ? y(.08) : Twist::Zero());
|
||||
limiter.setTargetTwist(y(.08), CartesianFrame::Base);
|
||||
for (int i = 0; i < approach_ticks; ++i) limiter.update(dt, Eigen::Matrix3d::Identity());
|
||||
limiter.setLinearConstraints(.55, 3, 10);
|
||||
limiter.setTargetTwist(y(-.08), CartesianFrame::Base);
|
||||
Motion result;
|
||||
double position = 0;
|
||||
for (int i = 1; i <= 1500; ++i) {
|
||||
const auto v = limiter.update(dt, Eigen::Matrix3d::Identity());
|
||||
position += v.y() * dt;
|
||||
result.peak = std::max(result.peak, position);
|
||||
if (v.y() < 0 && result.reverse_time < 0) result.reverse_time = i * dt;
|
||||
if (result.reverse_time > 0 && position <= 0 && result.return_time < 0) result.return_time = i * dt;
|
||||
if (continuous) {
|
||||
EXPECT_LE(limiter.getAccelerationBase().norm(), 3.0 + 1e-7);
|
||||
EXPECT_LE(limiter.getJerkBase().norm(), 10.0 + 1e-6);
|
||||
EXPECT_NEAR(v.x(), 0, 1e-12);
|
||||
EXPECT_NEAR(v.z(), 0, 1e-12);
|
||||
}
|
||||
}
|
||||
EXPECT_NEAR(limiter.getTwistBase().y(), -.08, 1e-9);
|
||||
return result;
|
||||
}
|
||||
TEST(CartesianTwistReversal, ContinuousReversalReducesTimeAndForwardTravel) {
|
||||
for (const int ticks : {0, 80, 120}) {
|
||||
SCOPED_TRACE(ticks);
|
||||
const auto legacy = reverse(false, ticks);
|
||||
const auto continuous = reverse(true, ticks);
|
||||
EXPECT_GT(continuous.reverse_time, 0);
|
||||
EXPECT_LT(continuous.reverse_time, legacy.reverse_time);
|
||||
EXPECT_LT(continuous.return_time, legacy.return_time);
|
||||
EXPECT_LT(continuous.peak, legacy.peak);
|
||||
}
|
||||
}
|
||||
TEST(CartesianTwistReversal, ExactZeroCrossingIsStillMovingAndKeepsAcceleration) {
|
||||
CartesianTwistLimiter limiter;
|
||||
limiter.setLinearConstraints(1, 1, 4);
|
||||
limiter.initialize(y(.02));
|
||||
limiter.setTargetTwist(y(-.02), CartesianFrame::Base);
|
||||
for (int i = 0; i < 100; ++i) limiter.update(dt, Eigen::Matrix3d::Identity());
|
||||
EXPECT_NEAR(limiter.getTwistBase().y(), 0, 1e-12);
|
||||
EXPECT_TRUE(limiter.isMoving());
|
||||
EXPECT_LT(limiter.getAccelerationBase().y(), -.39);
|
||||
EXPECT_LT(limiter.update(dt, Eigen::Matrix3d::Identity()).y(), -.0003);
|
||||
}
|
||||
TEST(CartesianTwistReversal, StopFromEitherDirectionSettlesWithoutReversing) {
|
||||
for (double v : {.08, -.08}) {
|
||||
CartesianTwistLimiter limiter;
|
||||
limiter.setLinearConstraints(1, 3, 10);
|
||||
limiter.initialize(y(v));
|
||||
limiter.stop();
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
EXPECT_GE(limiter.update(dt, Eigen::Matrix3d::Identity()).y() * v, -1e-12);
|
||||
}
|
||||
EXPECT_FALSE(limiter.isMoving());
|
||||
EXPECT_NEAR(limiter.getTwistBase().norm(), 0, 1e-12);
|
||||
}
|
||||
}
|
||||
TEST(CartesianTwistReversal, FeedbackPreservesNegativeVelocityOnLockedAxis) {
|
||||
CartesianTwistLimiter limiter;
|
||||
limiter.setLinearConstraints(1, 3, 10);
|
||||
limiter.initialize(y(.08));
|
||||
limiter.setTargetTwist(y(-.08), CartesianFrame::Base);
|
||||
for (int i = 0; i < 400; ++i) limiter.update(dt, Eigen::Matrix3d::Identity());
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
limiter.synchronize(y(-.08), dt, true);
|
||||
EXPECT_NEAR(limiter.update(dt, Eigen::Matrix3d::Identity()).y(), -.08, 1e-9);
|
||||
}
|
||||
}
|
||||
TEST(CartesianTwistReversal, NonCollinearChangeStopsBeforeSwitchingAxis) {
|
||||
CartesianTwistLimiter limiter;
|
||||
limiter.setLinearConstraints(1, 3, 10);
|
||||
limiter.initialize(y(.08));
|
||||
Twist x = Twist::Zero(); x.x() = .08;
|
||||
limiter.setTargetTwist(x, CartesianFrame::Base);
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
const auto v = limiter.update(dt, Eigen::Matrix3d::Identity());
|
||||
EXPECT_FALSE(v.x() > 1e-9 && std::abs(v.y()) > 1e-9);
|
||||
EXPECT_LE(limiter.getJerkBase().norm(), 10 + 1e-6);
|
||||
}
|
||||
EXPECT_NEAR(limiter.getTwistBase().x(), .08, 1e-9);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -2,7 +2,6 @@
|
||||
#define CMVR_ES_ARM_TYPES_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@ -65,24 +64,6 @@ struct CartesianVelocity {
|
||||
double wz{0.0};
|
||||
};
|
||||
|
||||
struct SpeedLOptions {
|
||||
// Requested acceleration, capped separately by the arm's linear/angular maxima.
|
||||
double acceleration{0.5};
|
||||
// Requested linear jerk (m/s^3), capped by the arm's configured maximum.
|
||||
// Unset uses that maximum.
|
||||
std::optional<double> linear_jerk;
|
||||
// Capture the first applied command's measured TCP position and target
|
||||
// direction in Base. Useful for distance-based motion without caller FK.
|
||||
bool capture_reference{false};
|
||||
};
|
||||
|
||||
struct SpeedLReference {
|
||||
bool valid{false};
|
||||
std::uint64_t command_version{0};
|
||||
CartesianPose tcp_pose_base{};
|
||||
CartesianVelocity target_base{};
|
||||
};
|
||||
|
||||
struct CartesianWrench {
|
||||
double fx{0.0};
|
||||
double fy{0.0};
|
||||
|
||||
@ -211,6 +211,7 @@ arm {
|
||||
gain: 0.2
|
||||
margin_ratio: 0.15
|
||||
max_push: 0.25
|
||||
weight: 0.05
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -109,9 +109,9 @@ arm {
|
||||
|
||||
speed_l {
|
||||
pinocchio_cartesian_motion_planner {
|
||||
linear_velocity_max: 0.8
|
||||
linear_acceleration_max: 10.0
|
||||
linear_jerk_max: 30.0
|
||||
linear_velocity_max: 0.55
|
||||
linear_acceleration_max: 5.0
|
||||
linear_jerk_max: 10.0
|
||||
angular_velocity_max: 1.0
|
||||
angular_acceleration_max: 5.0
|
||||
angular_jerk_max: 12.0
|
||||
|
||||
@ -28,8 +28,6 @@ dexhand {
|
||||
resultant_length: 3
|
||||
poll_interval_ms: 5
|
||||
response_timeout_ms: 200
|
||||
# 触觉数据最大有效期；超过此时间报不可用，不继续返回旧力值。
|
||||
max_sample_age_ms: 50
|
||||
response_header_bytes: 14
|
||||
tactile_rows: 1
|
||||
tactile_cols: 51
|
||||
|
||||
@ -96,22 +96,20 @@ touch_screen_task {
|
||||
speed_l {
|
||||
twist_tool { x: 0.0 y: -0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 5.0
|
||||
linear_jerk: 10.0
|
||||
max_distance_m: 0.03
|
||||
}
|
||||
tactile {
|
||||
finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
|
||||
region: TOUCH_SCREEN_TACTILE_REGION_TIP
|
||||
criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
|
||||
force_threshold: 0.1 # N
|
||||
force_threshold: 1.0
|
||||
}
|
||||
dwell_time_s: 0.0
|
||||
}
|
||||
|
||||
retract {
|
||||
twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 5.0
|
||||
linear_jerk: 10.0 # m/s^3，约束接触后的减速和连续换向。
|
||||
acceleration: 3.0
|
||||
# TCP 后退目标距离，单位为米。
|
||||
distance_m: 0.03
|
||||
}
|
||||
|
||||
@ -105,14 +105,13 @@ touch_screen_task {
|
||||
speed_l {
|
||||
twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 6.0
|
||||
linear_jerk: 60.0 # m/s^3，接近阶段请求值，受机械臂 linear_jerk_max 限制。
|
||||
max_distance_m: 0.02
|
||||
}
|
||||
tactile {
|
||||
finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
|
||||
region: TOUCH_SCREEN_TACTILE_REGION_TIP
|
||||
criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
|
||||
force_threshold: 0.1 # N
|
||||
force_threshold: 1.0
|
||||
}
|
||||
dwell_time_s: 0.0
|
||||
}
|
||||
@ -120,7 +119,6 @@ touch_screen_task {
|
||||
retract {
|
||||
twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 4.0
|
||||
linear_jerk: 60.0 # m/s^3，保持仿真机械臂原有的 jerk 上限。
|
||||
# TCP 后退目标距离，单位为米。
|
||||
distance_m: 0.05
|
||||
}
|
||||
|
||||
@ -19,11 +19,6 @@ target_link_libraries(motor_robot_arm
|
||||
add_library(cmvr_es::device::motor_robot_arm ALIAS motor_robot_arm)
|
||||
install(TARGETS motor_robot_arm LIBRARY DESTINATION lib)
|
||||
|
||||
add_executable(speedl_reversal_mujoco_test src/speedl_reversal_mujoco_test.cpp)
|
||||
target_link_libraries(speedl_reversal_mujoco_test PRIVATE
|
||||
cmvr_es::device::motor_robot_arm cmvr_es::device::motor_manager
|
||||
cmvr_es::device::mujoco_motor_driver cmvr_es::proto pthread)
|
||||
|
||||
add_executable(motor_robot_arm_mujoco_test
|
||||
src/motor_robot_arm_mujoco_test.cpp
|
||||
)
|
||||
|
||||
@ -1,77 +0,0 @@
|
||||
# speedL 连续换向与 MuJoCo 验证
|
||||
|
||||
同轴线速度换向使用固定轴上的有符号 S 曲线，从当前速度和加速度直接规划到反向目标。过零时保留加速度，不重置规划器。非同轴变向先停止再换轴；角速度仍使用原有策略。
|
||||
|
||||
`SpeedLPlannerConfig.continuous_linear_reversal` 默认 true；设为 false 可以复现旧的同轴停止后换向策略，用于同条件比较。
|
||||
|
||||
`MotorRobotArm::speedL(velocity, SpeedLOptions, duration, frame)` 可以为本条命令指定 `linear_jerk`（m/s³）。未指定时使用机器人配置的 jerk；普通 `speedL(velocity, acceleration, duration, frame)` 仍可直接使用。控制线程将速度、加速度、jerk 作为同一命令读取。正常停止的收尾操作检查命令版本，避免清除之后提交的运动命令。
|
||||
|
||||
机械臂的 `SpeedLPlannerConfig` 提供规划上限：速度保持按配置限幅；线加速度、角加速度分别取请求 `acceleration` 与各自配置上限的较小值；线性 jerk 取请求值与 `linear_jerk_max` 的较小值。省略 jerk 时使用配置上限。普通 `stopL` 的加速度也遵循此规则。未配置或无效的上限继续使用规划器默认值（线性 0.55/5/10，角向 1/5/12）。
|
||||
|
||||
例如机械臂配置加速度 5、jerk 10 时，回撤请求 60/60 实际按 5/10 规划，请求 3/6 则按 3/6 规划。任务参数不能提高机械臂上限。
|
||||
|
||||
接近阶段也可在 `touch.speed_l` 内配置 `linear_jerk`（m/s³），与 `retract.linear_jerk` 独立设置。两者都必须为有限正数，省略时使用机械臂上限；执行时都取请求值与上限的较小值。配置示例：
|
||||
|
||||
```protobuf
|
||||
touch {
|
||||
speed_l {
|
||||
twist_tool { x: 0 y: -0.08 z: 0 rx: 0 ry: 0 rz: 0 }
|
||||
acceleration: 5.0
|
||||
linear_jerk: 10.0
|
||||
max_distance_m: 0.03
|
||||
}
|
||||
# tactile 和 dwell_time_s 等其他必填项仍按任务配置填写。
|
||||
}
|
||||
```
|
||||
|
||||
`capture_reference=true` 时，控制线程从第一拍关节状态计算 TCP 位置及 Base 下的目标方向，成功发送速度后发布 `SpeedLReference`；Tool 目标随后固定到这一 Base 方向。新命令提交时旧参考立即失效。尚不支持这些选项的机械臂后端明确返回不支持。
|
||||
|
||||
触屏任务在 `dwell_time_s=0` 时检测到阈值就提交回撤，随后才记录日志，跳过停止/停留中间目标。回撤距离为沿回撤方向的有符号位移，继续前压不会算成回撤。`retract.linear_jerk` 控制整个减速、过零及反向加速过程，并受机械臂上限限制。实机触控任务当前接近和回撤都请求 10 m/s³；DLS 机械臂配置上限为 10，QP 配置上限为 30，MuJoCo 配置上限为 60 m/s³，执行时取所加载配置与任务请求的较小值。
|
||||
|
||||
触屏任务的 `[RETRACT]`、`[RETRACT_DONE]` 日志包含 `max_forward_after_retract_mm`：以回撤首个控制周期锁存的实测 TCP 为起点，统计沿回撤反方向的最大正位移，单位毫米。每次回撤清零，后续后退不抵消已记录的峰值。统计复用任务每次回撤步骤的 FK 位置采样，不额外阻塞触觉触发后的命令提交。它是采样到的最大值，不包括触发到首个回撤控制周期之前的移动，也不包括非零 dwell 阶段的移动；短暂峰值可能落在两次任务采样之间。已取得回撤参考后发生失败时，`[RETRACT_FAILED]` 也会打印已记录的最大值。
|
||||
|
||||
## 实际仿真测试
|
||||
|
||||
测试创建 MuJoCo 世界、MuJoCo 电机组和 MotorRobotArm，不创建真实硬件或视觉任务。使用 `dual_arm.xml` 的位置执行器驱动动力学；没有通过直接修改关节状态模拟运动。
|
||||
|
||||
先 MoveJ 到已有运动测试姿态，然后调用 `speedL(vy=+0.08)`，运动过程中直接调用 `speedL(vy=-0.08)`。默认 Base 坐标系，`--tool-frame true` 可验证 Tool 坐标系。测量来自 MuJoCo `R_FINGER_TIP_SITE` 的位置和雅可比乘实际 qvel；不是规划速度的积分。
|
||||
|
||||
在仓库根目录运行（需要已配置 `cmake-build-debug`；可用 `CMVR_BUILD_DIR` 覆盖）：
|
||||
|
||||
```bash
|
||||
# 匀速阶段换向：接近加速度 5，回撤加速度 3，jerk 均为 10。
|
||||
./script/test_speedl_reversal_mujoco.sh --csv /tmp/reversal-new.csv
|
||||
|
||||
# 同一新版本中选择旧换向策略，比较算法本身。
|
||||
./script/test_speedl_reversal_mujoco.sh --legacy true --csv /tmp/reversal-legacy.csv
|
||||
|
||||
# 仍在加速时，命令 vy 首次达到 0.032 m/s 即反向。
|
||||
./script/test_speedl_reversal_mujoco.sh --trigger-speed 0.032 --csv /tmp/reversal-accelerating.csv
|
||||
|
||||
# 上限仍为 10，回撤请求 60 将被限制为 10。
|
||||
./script/test_speedl_reversal_mujoco.sh --reverse-jerk 60 --reverse-acceleration 60 \
|
||||
--capture-reference true --csv /tmp/reversal-capped.csv
|
||||
|
||||
# 将仿真机械臂 jerk 上限设为 60；接近请求 10，回撤请求 60。
|
||||
./script/test_speedl_reversal_mujoco.sh --jerk 60 --approach-jerk 10 \
|
||||
--reverse-jerk 60 --capture-reference true \
|
||||
--csv /tmp/reversal-jerk60.csv
|
||||
|
||||
# Tool 坐标系和触屏任务使用的参数接口。
|
||||
./script/test_speedl_reversal_mujoco.sh --tool-frame true --jerk 60 --approach-jerk 10 --reverse-jerk 60 \
|
||||
--capture-reference true --csv /tmp/reversal-tool.csv
|
||||
```
|
||||
|
||||
默认前进 500 ms 后换向；`--approach-ms` 可修改，`--trigger-speed` 非零时优先按命令速度触发。默认速度为 0.08 m/s，可用 `--speed` 修改；`--jerk` 设置仿真机械臂 jerk 上限，`--approach-jerk` 和 `--reverse-jerk` 分别设置接近、回撤请求（省略时使用上限）。`--reverse-acceleration` 设置回撤加速度请求，默认 3，机械臂上限为 5。输出同时标注请求值和限幅值。测试采样目标周期和仿真步长均为 1 ms；线程由操作系统调度，CSV 同时记录 wall time 和 simulation time。
|
||||
|
||||
`max_forward_mm` 是反向调用前采样点之后，实际 TCP 沿前进轴的最大正位移；`peak_at_ms` 是到达该最远点的时间；`actual_reverse_ms` 要求至少连续 5 次采样的轴向速度小于 -0.0001 m/s；`return_to_origin_ms` 是确认反向后返回调用时位置的时间。Tool 模式的 CSV 速度字段也表示沿锁定前进轴的投影。
|
||||
|
||||
测试要求换向前实际速度为正、之后产生反向速度并退回起点，且控制器未提前退出；使用起点锁存时还要求参考有效。失败返回非零。该自由空间实验不包含屏幕接触、触觉延迟或硅胶形变，不能把返回起点时间直接当作屏幕抬起事件时间。
|
||||
|
||||
## 回归测试
|
||||
|
||||
构建并运行 `cartesian_twist_limiter_reversal_test`、`s_curve_velocity_planner_stop_test` 和 `cartesian_velocity_controller_test`。覆盖匀速/加速中换向、速度与 jerk 连续性、恰好过零、普通停止、非同轴转向、负速度反馈、停止/回撤竞争、成功发送后才发布回撤参考及非法参数拒绝。
|
||||
|
||||
`pinocchio_speedl_limits_test` 使用真实 URDF 和生产规划器，采样输出速度并差分验证实际规划的速度、加速度、jerk 上限；覆盖超限请求、较小请求、省略 jerk、旧接口及停止、独立角加速度限幅和非法请求。测试不初始化电机。
|
||||
|
||||
运行时应优先加载当前构建的项目动态库，测试脚本已处理；不要把新可执行文件与 `output/lib` 的旧项目库混用。
|
||||
@ -62,9 +62,6 @@ public:
|
||||
double duration,
|
||||
FrameType frame = FrameType::Base) override;
|
||||
Result stopL(std::optional<double> acceleration = std::nullopt) override;
|
||||
Result speedL(const CartesianVelocity& velocity, const SpeedLOptions& options,
|
||||
double duration, FrameType frame = FrameType::Base) override;
|
||||
SpeedLReference getSpeedLReference() const override;
|
||||
Result stopMotion() override;
|
||||
|
||||
Result startServoMode(const ServoOptions& options) override;
|
||||
|
||||
@ -748,21 +748,6 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
|
||||
return cartesian_velocity_controller_->stop(acceleration);
|
||||
}
|
||||
|
||||
Result MotorRobotArm::speedL(const CartesianVelocity& velocity, const SpeedLOptions& options,
|
||||
const double duration, const FrameType frame)
|
||||
{
|
||||
if (const auto stopped = safetyStopResult_("speedL")) return *stopped;
|
||||
if (busy_.load() || !cartesian_velocity_controller_) {
|
||||
return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy or velocity controller is unavailable");
|
||||
}
|
||||
return cartesian_velocity_controller_->speedL(velocity, options, duration, frame);
|
||||
}
|
||||
|
||||
SpeedLReference MotorRobotArm::getSpeedLReference() const
|
||||
{
|
||||
return cartesian_velocity_controller_ ? cartesian_velocity_controller_->getReference() : SpeedLReference{};
|
||||
}
|
||||
|
||||
Result MotorRobotArm::stopMotion()
|
||||
{
|
||||
const auto cartesian_stop = stopCartesianMotionAndWait_();
|
||||
|
||||
@ -1,196 +0,0 @@
|
||||
#include "arm/motor_robot_arm/include/motor_robot_arm.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"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
|
||||
// Headless, actuator-driven MuJoCo experiment. Never creates real motor drivers.
|
||||
namespace {
|
||||
using namespace cmvr;
|
||||
using namespace cmvr::device;
|
||||
using Clock = std::chrono::steady_clock;
|
||||
|
||||
template<class T> T readConfig(const std::filesystem::path& path) {
|
||||
T config;
|
||||
if (!ProtoMessageIo::getProtoFromAsciiFile(path.string(), &config)) {
|
||||
throw std::runtime_error("Cannot read " + path.string());
|
||||
}
|
||||
return config;
|
||||
}
|
||||
void require(const Result& result) {
|
||||
if (!result.ok()) throw std::runtime_error(result.message);
|
||||
}
|
||||
struct Sample {
|
||||
double sim_time;
|
||||
Eigen::Vector3d position;
|
||||
Eigen::Vector3d velocity;
|
||||
Eigen::Matrix3d rotation;
|
||||
};
|
||||
Sample sample(const std::shared_ptr<simulate::MujocoWorld>& world, int site) {
|
||||
std::lock_guard<std::mutex> lock(world->mutex());
|
||||
const auto* model = world->model();
|
||||
const auto* data = world->data();
|
||||
std::vector<mjtNum> jac(3 * model->nv);
|
||||
mj_jacSite(model, data, jac.data(), nullptr, site);
|
||||
Sample s{data->time, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity()};
|
||||
for (int axis = 0; axis < 3; ++axis) {
|
||||
s.position[axis] = data->site_xpos[site * 3 + axis];
|
||||
for (int j = 0; j < 3; ++j) s.rotation(axis, j) = data->site_xmat[site * 9 + axis * 3 + j];
|
||||
for (int j = 0; j < model->nv; ++j) s.velocity[axis] += jac[axis * model->nv + j] * data->qvel[j];
|
||||
}
|
||||
return s;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
try {
|
||||
const auto root = std::filesystem::current_path();
|
||||
double approach_ms = 500.0, jerk = 10.0, speed = 0.08, reverse_jerk = 0.0, trigger_speed = 0.0;
|
||||
double approach_jerk = 0.0, reverse_acceleration = 3.0;
|
||||
bool legacy = false, capture = false, tool_frame = false;
|
||||
std::string csv_path = "/tmp/speedl-reversal.csv";
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
const std::string arg = argv[i];
|
||||
if (++i >= argc) throw std::runtime_error("Missing value for " + arg);
|
||||
if (arg == "--approach-ms") approach_ms = std::stod(argv[i]);
|
||||
else if (arg == "--jerk") jerk = std::stod(argv[i]);
|
||||
else if (arg == "--speed") speed = std::stod(argv[i]);
|
||||
else if (arg == "--reverse-jerk") reverse_jerk = std::stod(argv[i]);
|
||||
else if (arg == "--approach-jerk") approach_jerk = std::stod(argv[i]);
|
||||
else if (arg == "--reverse-acceleration") reverse_acceleration = std::stod(argv[i]);
|
||||
else if (arg == "--trigger-speed") trigger_speed = std::stod(argv[i]);
|
||||
else if (arg == "--legacy") legacy = std::string(argv[i]) == "true";
|
||||
else if (arg == "--capture-reference") capture = std::string(argv[i]) == "true";
|
||||
else if (arg == "--tool-frame") tool_frame = std::string(argv[i]) == "true";
|
||||
else if (arg == "--csv") csv_path = argv[i];
|
||||
else throw std::runtime_error("Unknown option " + arg);
|
||||
}
|
||||
if (!(approach_ms > 0 && approach_ms <= 1000 && jerk > 0 && speed > 0 && speed <= .1 &&
|
||||
reverse_jerk >= 0 && trigger_speed >= 0 && trigger_speed <= speed &&
|
||||
approach_jerk >= 0 && reverse_acceleration > 0 && std::isfinite(jerk) &&
|
||||
std::isfinite(reverse_jerk) && std::isfinite(approach_jerk) && std::isfinite(reverse_acceleration))) {
|
||||
throw std::runtime_error("Invalid experiment parameters");
|
||||
}
|
||||
auto worlds = readConfig<config::MujocoWorldRootConfig>(root / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt");
|
||||
auto wc = worlds.worlds(0);
|
||||
wc.set_model_path((root / "model/xiaoyan_description/dual_arm.xml").string());
|
||||
simulate::MujocoWorldDevice world_device(wc);
|
||||
if (!world_device.init() || !world_device.start()) throw std::runtime_error("MuJoCo start failed");
|
||||
auto motors = readConfig<config::MotorRootConfig>(root / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt");
|
||||
auto manager = std::make_shared<MotorManager>("right_arm_mujoco_motors", motors.motor(), "right_arm_mujoco_motors");
|
||||
manager->init();
|
||||
const auto world = world_device.world();
|
||||
const int site = mj_name2id(world->model(), mjOBJ_SITE, "R_FINGER_TIP_SITE");
|
||||
if (site < 0) throw std::runtime_error("TCP site not found");
|
||||
auto arms = readConfig<config::ArmRootConfig>(root / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt");
|
||||
auto ac = arms.arm().robot_arms(0);
|
||||
ac.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path(
|
||||
(root / "model/xiaoyan_description/dual_arm.urdf").string());
|
||||
auto* planner_config = ac.mutable_motion()->mutable_speed_l()->mutable_pinocchio_cartesian_motion_planner();
|
||||
// Match physical-arm limits instead of the simulation's faster defaults.
|
||||
planner_config->set_linear_velocity_max(.55);
|
||||
planner_config->set_linear_acceleration_max(5.0);
|
||||
planner_config->set_linear_jerk_max(jerk);
|
||||
planner_config->set_continuous_linear_reversal(!legacy);
|
||||
MotorRobotArm arm(ac);
|
||||
if (!arm.init()) throw std::runtime_error("Simulated arm initialization failed");
|
||||
MotionOptions move;
|
||||
move.velocity = 1.0;
|
||||
move.acceleration = 3.0;
|
||||
require(arm.moveJ(JointPositionCommand{{.25, 1.0, M_PI/2, M_PI/2, -M_PI/2, 0, 0}}, move));
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(300));
|
||||
|
||||
std::ofstream csv(csv_path);
|
||||
if (!csv) throw std::runtime_error("Cannot open CSV");
|
||||
csv << "wall_ms,sim_ms,after_reverse,x_m,y_m,z_m,vy_m_s,forward_displacement_m,command_vy_m_s\n" << std::setprecision(12);
|
||||
CartesianVelocity forward;
|
||||
forward.vy = speed;
|
||||
const auto frame = tool_frame ? FrameType::Tool : FrameType::Base;
|
||||
const Eigen::Vector3d axis = tool_frame ? Eigen::Vector3d(sample(world, site).rotation.col(1))
|
||||
: Eigen::Vector3d::UnitY();
|
||||
SpeedLOptions forward_options;
|
||||
forward_options.acceleration = 5.0;
|
||||
if (approach_jerk > 0) forward_options.linear_jerk = approach_jerk;
|
||||
require(arm.speedL(forward, forward_options, 0.0, frame));
|
||||
const auto begin = Clock::now();
|
||||
auto next = begin;
|
||||
auto reverse_time = begin;
|
||||
Sample origin{};
|
||||
bool reversed = false;
|
||||
bool reference_valid = !capture;
|
||||
double peak = 0, peak_ms = 0, return_ms = -1, reverse_ms = -1;
|
||||
int negative_samples = 0;
|
||||
while (Clock::now() - begin < std::chrono::milliseconds(static_cast<int>(approach_ms) + 1000)) {
|
||||
auto now = Clock::now();
|
||||
auto s = sample(world, site);
|
||||
const auto command = arm.getSpeedLCommandTwistBase();
|
||||
const double command_vy = Eigen::Vector3d(command.vx, command.vy, command.vz).dot(axis);
|
||||
const double actual_vy = s.velocity.dot(axis);
|
||||
const double wall_ms = std::chrono::duration<double, std::milli>(now - begin).count();
|
||||
if (!reversed && (trigger_speed > 0 ? command_vy >= trigger_speed : wall_ms >= approach_ms)) {
|
||||
origin = s;
|
||||
reverse_time = Clock::now();
|
||||
CartesianVelocity backward;
|
||||
backward.vy = -speed;
|
||||
SpeedLOptions options;
|
||||
options.acceleration = reverse_acceleration;
|
||||
options.capture_reference = capture;
|
||||
if (reverse_jerk > 0) options.linear_jerk = reverse_jerk;
|
||||
require(arm.speedL(backward, options, 0.0, frame));
|
||||
reversed = true;
|
||||
}
|
||||
const double elapsed = std::chrono::duration<double, std::milli>(now - reverse_time).count();
|
||||
const double displacement = reversed ? (s.position - origin.position).dot(axis) : 0.0;
|
||||
if (reversed) {
|
||||
if (capture) {
|
||||
const auto ref = arm.getSpeedLReference();
|
||||
reference_valid |= ref.valid && Eigen::Vector3d(ref.target_base.vx,
|
||||
ref.target_base.vy, ref.target_base.vz).dot(axis) < 0 &&
|
||||
std::isfinite(ref.tcp_pose_base.y) && ref.command_version != 0;
|
||||
}
|
||||
if (displacement > peak) { peak = displacement; peak_ms = elapsed; }
|
||||
negative_samples = actual_vy < -1e-4 ? negative_samples + 1 : 0;
|
||||
if (negative_samples >= 5 && reverse_ms < 0) reverse_ms = elapsed;
|
||||
if (reverse_ms >= 0 && displacement <= 0 && return_ms < 0) return_ms = elapsed;
|
||||
}
|
||||
csv << wall_ms << ',' << s.sim_time * 1000 << ',' << reversed << ','
|
||||
<< s.position.x() << ',' << s.position.y() << ',' << s.position.z() << ','
|
||||
<< actual_vy << ',' << displacement << ',' << command_vy << '\n';
|
||||
next += std::chrono::milliseconds(1);
|
||||
std::this_thread::sleep_until(next);
|
||||
}
|
||||
const bool active = arm.busy();
|
||||
require(arm.stopL(3.0));
|
||||
const auto stop_deadline = Clock::now() + std::chrono::seconds(3);
|
||||
while (arm.busy() && Clock::now() < stop_deadline) std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
std::cout << std::fixed << std::setprecision(3)
|
||||
<< "REVERSAL legacy=" << legacy << " approach_ms=" << approach_ms
|
||||
<< " frame=" << (tool_frame ? "Tool" : "Base")
|
||||
<< " trigger_speed=" << trigger_speed << " speed_m_s=" << speed << " jerk_m_s3=" << jerk
|
||||
<< " approach_jerk_m_s3=" << std::min(approach_jerk > 0 ? approach_jerk : jerk, jerk)
|
||||
<< " requested_reverse_acceleration_m_s2=" << reverse_acceleration
|
||||
<< " reverse_acceleration_m_s2=" << std::min(reverse_acceleration, 5.0)
|
||||
<< " requested_reverse_jerk_m_s3=" << (reverse_jerk > 0 ? reverse_jerk : jerk)
|
||||
<< " reverse_jerk_m_s3=" << std::min(reverse_jerk > 0 ? reverse_jerk : jerk, jerk)
|
||||
<< " actual_vy_at_reverse=" << origin.velocity.dot(axis)
|
||||
<< " max_forward_mm=" << peak * 1000 << " peak_at_ms=" << peak_ms
|
||||
<< " actual_reverse_ms=" << reverse_ms << " return_to_origin_ms=" << return_ms
|
||||
<< " remained_active=" << active << " reference_valid=" << reference_valid << " csv=" << csv_path << '\n';
|
||||
arm.stop();
|
||||
manager->stop();
|
||||
world_device.stop();
|
||||
return active && reference_valid && reversed && origin.velocity.dot(axis) > .001 && reverse_ms > 0 && return_ms > 0 ? 0 : 1;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Experiment failed: " << e.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@ -58,16 +58,6 @@ public:
|
||||
double duration,
|
||||
FrameType frame = FrameType::Base) = 0;
|
||||
virtual Result stopL(std::optional<double> acceleration = std::nullopt) = 0;
|
||||
virtual Result speedL(const CartesianVelocity& velocity,
|
||||
const SpeedLOptions& options,
|
||||
double duration,
|
||||
FrameType frame = FrameType::Base) {
|
||||
if (options.linear_jerk || options.capture_reference) {
|
||||
return Result::failure(ArmErrorCode::UnsupportedCommand, "speedL options are not supported by this arm");
|
||||
}
|
||||
return speedL(velocity, options.acceleration, duration, frame);
|
||||
}
|
||||
virtual SpeedLReference getSpeedLReference() const { return {}; }
|
||||
virtual Result stopMotion() = 0;
|
||||
|
||||
virtual Result moveP(const CartesianPose& target,
|
||||
|
||||
@ -9,7 +9,6 @@
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
@ -44,17 +43,6 @@ namespace cmvr::device {
|
||||
|
||||
using ResultantForce = TactilePoint;
|
||||
|
||||
// Physical force, separate from device-specific integer tactile data.
|
||||
struct ForceNewtons {
|
||||
double fx{0.0};
|
||||
double fy{0.0};
|
||||
double fz{0.0};
|
||||
|
||||
double magnitude() const {
|
||||
return std::hypot(fx, fy, fz);
|
||||
}
|
||||
};
|
||||
|
||||
enum class FingerType {
|
||||
PINKY,
|
||||
RING,
|
||||
@ -159,11 +147,6 @@ namespace cmvr::device {
|
||||
virtual std::vector<TactileRegionData> getSensorData() = 0;
|
||||
virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0;
|
||||
virtual ResultantForce getResultantForce(FingerType finger, TactileRegion region) = 0;
|
||||
// Backends must provide a documented conversion; raw pressure counts
|
||||
// cannot be assumed to represent newtons.
|
||||
virtual ForceNewtons getResultantForceNewtons(FingerType, TactileRegion) {
|
||||
throw std::runtime_error(typeName() + " does not provide force in newtons.");
|
||||
}
|
||||
|
||||
virtual void setPositions(const std::vector<int>&) {
|
||||
CMVR_LOG(ERROR) << "[AbstractDexHand] setPositions is not supported by this dexhand abstraction.";
|
||||
|
||||
@ -12,12 +12,3 @@ target_link_libraries(px_6ax_gen3
|
||||
)
|
||||
|
||||
install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib)
|
||||
|
||||
# Sensor-only executable: no DeviceManager, arm initialization or calibration.
|
||||
add_executable(px_6ax_gen3_real_test src/px_6ax_gen3_real_test.cpp)
|
||||
target_link_libraries(px_6ax_gen3_real_test PRIVATE
|
||||
px_6ax_gen3 cmvr_es::proto cmvr_es::logging pthread)
|
||||
|
||||
add_executable(px_6ax_gen3_test src/px_6ax_gen3_test.cpp)
|
||||
target_link_libraries(px_6ax_gen3_test PRIVATE
|
||||
px_6ax_gen3 cmvr_es::proto cmvr_es::logging gtest gtest_main pthread)
|
||||
|
||||
@ -1,51 +0,0 @@
|
||||
# PX-6AX GEN3 传感器读取与真实 USB 测试
|
||||
|
||||
真实测试只创建 PX6AXGen3 和 POSIX 串口，不初始化机械臂或 DeviceManager；禁用自动标定，传输层只允许 `0xFB` 读取命令。
|
||||
|
||||
在仓库根目录运行：
|
||||
|
||||
```bash
|
||||
# 每次读取都发起一个真实请求：检查串口应答耗时和按压力值。
|
||||
./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \
|
||||
--mode sync --duration-s 30 --csv /tmp/paxini-sync.csv --raw-log /tmp/paxini-frames.log
|
||||
|
||||
# 与触屏任务相同：后台轮询，前台读取最新有效缓存。
|
||||
./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \
|
||||
--mode stream --duration-s 30 --csv /tmp/paxini-stream.csv
|
||||
```
|
||||
|
||||
也可把 `--port` 指定为 `/dev/serial/by-id/` 下的稳定设备链接。设备端口取决于连接顺序；测试参数不会更改机器人部署配置里的串口。`module-id=2` 对应协议设备地址 3。
|
||||
|
||||
脚本会构建真实测试并优先加载本次构建的驱动和 protobuf。默认构建目录为 `cmake-build-debug`，可用 `CMVR_BUILD_DIR` 覆盖；构建目录须已完成 CMake 配置。串口应可读写，运行测试前退出占用同一串口的程序。
|
||||
|
||||
测试期间可用手轻按、松开传感器，终端每 100 ms 显示一次力值，CSV 记录每次 getter 调用。按 Ctrl-C 可结束。CSV 的无效读数留空，不能当作零力。进程在初始化失败、没有有效数据或存在读取失败时返回非零退出码。
|
||||
|
||||
- `sync` 的 `read_ms` 是一次驱动请求/应答调用的耗时；默认每 5 ms 请求一次。
|
||||
- `stream` 的 `read_ms` 是读取缓存耗时，成功次数包含重复快照，不能据此推算传感器实际更新频率。
|
||||
- `force_N`、`fz_N` 和 CSV 中的三个力分量均由驱动直接返回，单位为 N；例如原始值 1 对应 0.1 N、108 对应 10.8 N。传感器标称输出频率 83.3 Hz；请求频率可以高于内部测量更新频率。
|
||||
- 串口往返时间不包含“物理接触到传感器产生非零输出”的全部时间。需要按压试验或外部同步信号才能测量接触检测延迟。
|
||||
- `--raw-log` 会记录原始 TX/RX 和单调时钟时间，用于对照手册定位帧问题；写日志会给时间测量带来少量开销。
|
||||
|
||||
## 驱动行为
|
||||
|
||||
`getResultantForceNewtons()` 将合力寄存器的三个原始分量各乘以 0.1，返回以 N 为单位的浮点力值；原有 `getResultantForce()` 保留原始整数。触屏任务和 USB 测试使用牛顿接口，不再额外换算。触屏任务的 `force_threshold`、力值日志和 `lastTouchPressureSum()` 均使用 N；阈值 `0.1` 与旧版原始值阈值 `1.0` 对应相同力度。FZ 判据比较法向力，MAGNITUDE 判据比较三轴合力大小，多区域按原逻辑累加。
|
||||
|
||||
MuJoCo 零值触觉后端也实现牛顿接口。尚无确定换算系数的其他后端（如 RH56DFTP）调用该接口会明确报错，不会把原始压力计数当成 N。
|
||||
|
||||
合力应答按 `14 字节头部 + 3 字节数据 + 1 字节 LRC` 完整读取。验证帧长度、设备地址、预留位、功能码、寄存器地址、字节数及 LRC，并支持分片、请求回显和噪声后的重新定位。
|
||||
|
||||
读状态字节属于内部调试信息（手册 5.3.5）；实测正常读应答为 `0x01`，不能套用写应答 `0x00=成功` 的规则。自动标定的写应答要求完整 15 字节并且状态为 0。
|
||||
|
||||
`max_sample_age_ms` 默认 50 ms，可在设备配置中调整。每类数据分别记录请求开始时间：较晚返回的旧请求不能被重新标记为新数据。此值独立于 `response_timeout_ms`（默认 200 ms）。后台读取接口遇到过期或失效快照会抛出异常，不等待串口补读、不返回旧力值或伪造零值；触屏任务现有的异常捕获会将其识别为触觉不可用。
|
||||
|
||||
通信失败会使快照失效，后台继续尝试恢复，只有通过验证的新应答才能恢复有效数据。`stop()` 清除缓存；停止或故障状态下的 getter 报错，显式 `init()` / `start()` 后才能恢复。初始化重试失败会返回 false。
|
||||
|
||||
## 自动回归测试
|
||||
|
||||
真实设备无法稳定制造的坏帧和超时，用可注入的串口实现验证：
|
||||
|
||||
```bash
|
||||
cmake --build cmake-build-debug --target px_6ax_gen3_test -j 4
|
||||
LD_LIBRARY_PATH="$PWD/cmake-build-debug:$PWD/cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3:$PWD/cmake-build-debug/cmvr-es/hardware:$PWD/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \
|
||||
./cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3/px_6ax_gen3_test
|
||||
```
|
||||
@ -39,8 +39,6 @@ namespace cmvr::device {
|
||||
};
|
||||
|
||||
explicit PX6AXGen3(const config::PX6AXGen3& cfg);
|
||||
PX6AXGen3(const config::PX6AXGen3& cfg,
|
||||
std::unique_ptr<::cmvr::AbstractSerialTransport> serial);
|
||||
~PX6AXGen3() override;
|
||||
|
||||
std::string typeName() const override { return "PX6AXGen3"; }
|
||||
@ -56,10 +54,7 @@ namespace cmvr::device {
|
||||
void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
|
||||
std::vector<TactileRegionData> getSensorData() override;
|
||||
TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
|
||||
// Throws when no fresh, validated sample is available. A failed read is
|
||||
// never represented as a zero force (which would mean no contact).
|
||||
ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
|
||||
ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override;
|
||||
|
||||
private:
|
||||
struct SensorSnapshot {
|
||||
@ -69,8 +64,6 @@ namespace cmvr::device {
|
||||
int cols{0};
|
||||
bool tactile_valid{false};
|
||||
bool resultant_valid{false};
|
||||
std::chrono::steady_clock::time_point tactile_request_time{};
|
||||
std::chrono::steady_clock::time_point resultant_request_time{};
|
||||
};
|
||||
|
||||
static PollingReadMode parsePollingReadMode(config::PX6AXGen3PollingReadMode mode);
|
||||
@ -88,19 +81,17 @@ namespace cmvr::device {
|
||||
|
||||
bool isSupportedRegion(FingerType finger, TactileRegion region) const;
|
||||
bool isSnapshotReady(bool require_tactile, bool require_resultant) const;
|
||||
bool isSampleFresh(std::chrono::steady_clock::time_point request_time) const;
|
||||
TactileRegionData buildSupportedRegionSnapshot() const;
|
||||
std::pair<bool, bool> resolvePollingReadSelection() const;
|
||||
|
||||
void clearOperationalError();
|
||||
void handleRefreshFailure(const std::string& error);
|
||||
void handleRefreshFailure(const std::string& error, bool had_valid_snapshot);
|
||||
void transitionTo(Status next_state);
|
||||
void enterFault(const std::string& error);
|
||||
bool isOperationalState(Status lifecycle) const;
|
||||
|
||||
std::unique_ptr<::cmvr::AbstractSerialTransport> serial_;
|
||||
config::PX6AXGen3 config_;
|
||||
bool config_valid_{false};
|
||||
|
||||
mutable std::mutex lifecycle_mutex_;
|
||||
Status lifecycle_state_{Status::CREATED};
|
||||
@ -118,7 +109,6 @@ namespace cmvr::device {
|
||||
int tactile_rows_{1};
|
||||
int tactile_cols_{0};
|
||||
int response_timeout_ms_{200};
|
||||
std::chrono::milliseconds max_sample_age_{50};
|
||||
FingerType tactile_finger_{FingerType::INDEX};
|
||||
TactileRegion tactile_region_{TactileRegion::TIP};
|
||||
PollingReadMode polling_read_mode_{PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE};
|
||||
|
||||
@ -125,13 +125,32 @@ namespace {
|
||||
return frame;
|
||||
}
|
||||
|
||||
// UART read reply: 14-byte header, N payload bytes, one LRC byte.
|
||||
constexpr size_t kResponseHeaderBytes = 14U;
|
||||
constexpr size_t kResponseOverheadBytes = kResponseHeaderBytes + 1U;
|
||||
std::vector<uint8_t> extractPayload(const std::vector<uint8_t>& response,
|
||||
const size_t frame_offset,
|
||||
const size_t response_header_bytes,
|
||||
const size_t payload_length) {
|
||||
if (response.size() < frame_offset + response_header_bytes + payload_length) {
|
||||
CMVR_LOG(ERROR) << "PX-6AX GEN3 response shorter than expected payload window.";
|
||||
return {};
|
||||
}
|
||||
return std::vector<uint8_t>(
|
||||
response.begin() + static_cast<std::ptrdiff_t>(frame_offset + response_header_bytes),
|
||||
response.begin() + static_cast<std::ptrdiff_t>(frame_offset + response_header_bytes + payload_length));
|
||||
}
|
||||
|
||||
uint16_t readLe16(const std::vector<uint8_t>& bytes, const size_t offset) {
|
||||
return static_cast<uint16_t>(bytes[offset]) |
|
||||
static_cast<uint16_t>(static_cast<uint16_t>(bytes[offset + 1U]) << 8U);
|
||||
size_t findResponseFrameOffset(const std::vector<uint8_t>& response,
|
||||
const size_t expected_frame_bytes) {
|
||||
if (response.size() < expected_frame_bytes) {
|
||||
return std::string::npos;
|
||||
}
|
||||
|
||||
for (size_t offset = 0; offset + expected_frame_bytes <= response.size(); ++offset) {
|
||||
if (response[offset] == 0xAA && response[offset + 1] == 0x55) {
|
||||
return offset;
|
||||
}
|
||||
}
|
||||
|
||||
return std::string::npos;
|
||||
}
|
||||
|
||||
std::string previewBytesHex(const std::vector<uint8_t>& data, const size_t max_bytes = 32U) {
|
||||
@ -152,83 +171,39 @@ namespace {
|
||||
return stream.str();
|
||||
}
|
||||
|
||||
std::vector<uint8_t> transact(cmvr::AbstractSerialTransport& serial,
|
||||
const std::vector<uint8_t>& request,
|
||||
const size_t payload_length,
|
||||
const std::chrono::milliseconds timeout) {
|
||||
if (!serial.flushInput()) {
|
||||
throw std::runtime_error("Failed to flush PX6AXGen3 input: " + serial.lastError());
|
||||
}
|
||||
if (!serial.write(request)) {
|
||||
throw std::runtime_error("Failed to send PX6AXGen3 request: " + serial.lastError());
|
||||
}
|
||||
|
||||
const size_t expected_size = kResponseOverheadBytes + payload_length;
|
||||
// Permit a request echo/noisy prefix, but bound resynchronization work.
|
||||
const size_t max_response_bytes = expected_size + 4096U;
|
||||
std::vector<uint8_t> readFramedResponse(cmvr::AbstractSerialTransport& serial,
|
||||
const size_t expected_frame_bytes,
|
||||
const size_t max_prefix_bytes,
|
||||
const std::chrono::milliseconds timeout,
|
||||
const std::string& response_name) {
|
||||
const auto deadline = std::chrono::steady_clock::now() + timeout;
|
||||
std::vector<uint8_t> response;
|
||||
size_t offset = 0;
|
||||
std::string error = "Incomplete PX6AXGen3 response";
|
||||
while (response.size() < max_response_bytes) {
|
||||
const bool read_ok = serial.read(expected_frame_bytes, timeout, response);
|
||||
auto frame_offset = findResponseFrameOffset(response, expected_frame_bytes);
|
||||
|
||||
while (frame_offset == std::string::npos &&
|
||||
response.size() < expected_frame_bytes + max_prefix_bytes) {
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
if (now >= deadline) {
|
||||
break;
|
||||
}
|
||||
std::vector<uint8_t> chunk;
|
||||
const auto remaining = std::max(std::chrono::milliseconds(1),
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now));
|
||||
// Read available bytes; never wait for a guessed length before
|
||||
// identifying the header. This also handles split headers/echoes.
|
||||
const bool read_ok = serial.read(1U, remaining, chunk);
|
||||
if (chunk.size() > max_response_bytes - response.size()) {
|
||||
throw std::runtime_error("PX6AXGen3 response exceeds resynchronization limit");
|
||||
}
|
||||
response.insert(response.end(), chunk.begin(), chunk.end());
|
||||
while (offset + kResponseHeaderBytes <= response.size()) {
|
||||
if (response[offset] != 0xAA || response[offset + 1U] != 0x55) {
|
||||
++offset;
|
||||
continue;
|
||||
}
|
||||
// Frame length counts data[4] through data[N+13], excluding LRC.
|
||||
if (readLe16(response, offset + 2U) != payload_length + 10U ||
|
||||
response[offset + 4U] != request[4] ||
|
||||
response[offset + 5U] != 0x00 ||
|
||||
!std::equal(request.begin() + 6, request.begin() + 13,
|
||||
response.begin() + static_cast<std::ptrdiff_t>(offset + 6U))) {
|
||||
error = "PX6AXGen3 response does not match request (length/address/function)";
|
||||
++offset;
|
||||
continue;
|
||||
}
|
||||
if (response.size() - offset < expected_size) {
|
||||
break;
|
||||
}
|
||||
uint8_t sum = 0;
|
||||
for (size_t i = offset; i < offset + expected_size; ++i) {
|
||||
sum = static_cast<uint8_t>(sum + response[i]);
|
||||
}
|
||||
if (sum != 0U) {
|
||||
error = "Invalid PX6AXGen3 response LRC";
|
||||
++offset;
|
||||
continue;
|
||||
}
|
||||
// Manual 5.3.5: read status is internal/debug information;
|
||||
// hardware returns 0x01 on normal force reads. Only write ACKs
|
||||
// define 0x00 as success (5.4.2), so do not apply it to reads.
|
||||
if (request[6] == 0x79 && response[offset + 13U] != 0x00) {
|
||||
throw std::runtime_error("PX6AXGen3 returned status " +
|
||||
std::to_string(response[offset + 13U]));
|
||||
}
|
||||
return std::vector<uint8_t>(
|
||||
response.begin() + static_cast<std::ptrdiff_t>(offset + kResponseHeaderBytes),
|
||||
response.begin() + static_cast<std::ptrdiff_t>(offset + kResponseHeaderBytes + payload_length));
|
||||
}
|
||||
if (!read_ok || chunk.empty()) {
|
||||
|
||||
std::vector<uint8_t> extra_bytes;
|
||||
const auto remaining_timeout = std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now);
|
||||
if (!serial.read(1U, remaining_timeout, extra_bytes)) {
|
||||
break;
|
||||
}
|
||||
|
||||
response.insert(response.end(), extra_bytes.begin(), extra_bytes.end());
|
||||
frame_offset = findResponseFrameOffset(response, expected_frame_bytes);
|
||||
}
|
||||
throw std::runtime_error(error + "; " + serial.lastError() +
|
||||
", raw=" + previewBytesHex(response));
|
||||
|
||||
if (!read_ok && frame_offset == std::string::npos) {
|
||||
CMVR_LOG(ERROR) << "Failed to read " << response_name << ": " << serial.lastError()
|
||||
<< ", raw=" << previewBytesHex(response);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
std::array<int, 3> parseResultantPayload(const std::vector<uint8_t>& payload) {
|
||||
@ -328,28 +303,21 @@ PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadModeName(std::string value
|
||||
}
|
||||
|
||||
PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg)
|
||||
: PX6AXGen3(cfg, std::make_unique<::cmvr::PosixSerialTransport>()) {}
|
||||
|
||||
PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg,
|
||||
std::unique_ptr<::cmvr::AbstractSerialTransport> serial)
|
||||
: serial_(std::move(serial)), config_(cfg) {
|
||||
: serial_(std::make_unique<::cmvr::PosixSerialTransport>()),
|
||||
config_(cfg) {
|
||||
id_ = config_.id();
|
||||
port_name_ = config_.serial_port();
|
||||
if (!config_.sensor_model().empty()) {
|
||||
sensor_model_ = config_.sensor_model();
|
||||
}
|
||||
module_id_ = config_.module_id();
|
||||
if (!serial_ || module_id_ < 0 || module_id_ > 254) {
|
||||
enterFault("PX6AXGen3 requires a serial transport and module_id in [0, 254].");
|
||||
return;
|
||||
}
|
||||
module_id_ = std::max(0, config_.module_id());
|
||||
device_address_ = module_id_ + 1;
|
||||
if (config_.baud_rate() > 0) {
|
||||
baud_rate_ = config_.baud_rate();
|
||||
}
|
||||
distributed_length_ = config_.distributed_length();
|
||||
if (distributed_length_ <= 0 || distributed_length_ > 65525 || distributed_length_ % 3 != 0) {
|
||||
enterFault("PX6AXGen3 distributed_length must be a positive multiple of 3 fitting the UART frame.");
|
||||
if (distributed_length_ <= 0) {
|
||||
enterFault("PX6AXGen3 requires config.distributed_length to be explicitly configured.");
|
||||
return;
|
||||
}
|
||||
if (config_.resultant_length() > 0) {
|
||||
@ -358,15 +326,6 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg,
|
||||
if (config_.response_header_bytes() > 0) {
|
||||
response_header_bytes_ = config_.response_header_bytes();
|
||||
}
|
||||
if (resultant_length_ != 3 || response_header_bytes_ != kResponseHeaderBytes ||
|
||||
config_.resultant_length() < 0 || config_.response_header_bytes() < 0 ||
|
||||
config_.max_sample_age_ms() < 0) {
|
||||
enterFault("PX6AXGen3 requires resultant_length=3, response_header_bytes=14 and a nonnegative max_sample_age_ms.");
|
||||
return;
|
||||
}
|
||||
if (config_.max_sample_age_ms() > 0) {
|
||||
max_sample_age_ = std::chrono::milliseconds(config_.max_sample_age_ms());
|
||||
}
|
||||
if (config_.tactile_rows() > 0) {
|
||||
tactile_rows_ = config_.tactile_rows();
|
||||
}
|
||||
@ -391,7 +350,6 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg,
|
||||
poll_interval_ = std::chrono::milliseconds(config_.poll_interval_ms());
|
||||
}
|
||||
initializeSnapshot();
|
||||
config_valid_ = true;
|
||||
}
|
||||
|
||||
PX6AXGen3::~PX6AXGen3() {
|
||||
@ -399,15 +357,15 @@ PX6AXGen3::~PX6AXGen3() {
|
||||
}
|
||||
|
||||
bool PX6AXGen3::init() {
|
||||
if (!config_valid_) {
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
ensureConnected();
|
||||
if (!isOperationalState(state())) {
|
||||
return false;
|
||||
}
|
||||
refreshSensorDataWithRetry(
|
||||
5,
|
||||
std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2)));
|
||||
const auto [tactile, resultant] = resolvePollingReadSelection();
|
||||
return isOperationalState(state()) && isSnapshotReady(tactile, resultant);
|
||||
return isOperationalState(state());
|
||||
} catch (const std::exception& e) {
|
||||
enterFault("[PX6AXGen3](init): " + std::string(e.what()));
|
||||
return false;
|
||||
@ -415,10 +373,8 @@ bool PX6AXGen3::init() {
|
||||
}
|
||||
|
||||
bool PX6AXGen3::start() {
|
||||
if (!config_valid_) {
|
||||
return false;
|
||||
}
|
||||
if (polling_thread_running_.exchange(true, std::memory_order_acq_rel)) {
|
||||
transitionTo(Status::STREAMING);
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -427,22 +383,19 @@ bool PX6AXGen3::start() {
|
||||
polling_thread_.join();
|
||||
}
|
||||
|
||||
bool requested_polling;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(polling_mutex_);
|
||||
requested_polling = requested_polling_;
|
||||
ensureConnected();
|
||||
if (!isOperationalState(state())) {
|
||||
polling_thread_running_.store(false, std::memory_order_release);
|
||||
return false;
|
||||
}
|
||||
if (requested_polling) {
|
||||
if (requested_polling_) {
|
||||
refreshSensorDataWithRetry(
|
||||
5,
|
||||
std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2)));
|
||||
} else {
|
||||
std::lock_guard<std::mutex> lock(refresh_mutex_);
|
||||
ensureConnected();
|
||||
}
|
||||
|
||||
transitionTo(Status::STREAMING);
|
||||
polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this);
|
||||
transitionTo(Status::STREAMING);
|
||||
polling_cv_.notify_all();
|
||||
return true;
|
||||
} catch (const std::exception& e) {
|
||||
@ -460,8 +413,6 @@ bool PX6AXGen3::stop() {
|
||||
polling_thread_.join();
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(refresh_mutex_);
|
||||
initializeSnapshot();
|
||||
closeConnection();
|
||||
|
||||
if (state() != Status::FAULT) {
|
||||
@ -483,13 +434,10 @@ std::string PX6AXGen3::lastError() const {
|
||||
void PX6AXGen3::getState(DexHandState& state_out) {
|
||||
DexHandState next_state{};
|
||||
next_state.is_initialized = isOperationalState(state());
|
||||
const auto [tactile, resultant] = resolvePollingReadSelection();
|
||||
const bool ready = isSnapshotReady(tactile, resultant);
|
||||
next_state.is_initialized = next_state.is_initialized && ready;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
if (latest_snapshot_.resultant_valid && isSampleFresh(latest_snapshot_.resultant_request_time)) {
|
||||
if (latest_snapshot_.resultant_valid) {
|
||||
next_state.hands[0].force = latest_snapshot_.resultant_force_tenths[2];
|
||||
}
|
||||
}
|
||||
@ -497,8 +445,6 @@ void PX6AXGen3::getState(DexHandState& state_out) {
|
||||
const auto error = lastError();
|
||||
if (!error.empty()) {
|
||||
next_state.hands[0].error_message.push_back(error);
|
||||
} else if (!ready) {
|
||||
next_state.hands[0].error_message.push_back("PX6AXGen3 sample is unavailable or stale.");
|
||||
}
|
||||
|
||||
state_out = std::move(next_state);
|
||||
@ -522,8 +468,7 @@ void PX6AXGen3::setTactilePollingRegions(const std::vector<TactileRegionKey>& re
|
||||
}
|
||||
polling_cv_.notify_all();
|
||||
|
||||
if (!regions.empty() && isOperationalState(state()) &&
|
||||
!polling_thread_running_.load(std::memory_order_acquire)) {
|
||||
if (!regions.empty() && isOperationalState(state())) {
|
||||
refreshSensorData();
|
||||
}
|
||||
}
|
||||
@ -537,7 +482,8 @@ std::vector<TactileRegionData> PX6AXGen3::getSensorData() {
|
||||
|
||||
TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion region) {
|
||||
if (!isSupportedRegion(finger, region)) {
|
||||
throw std::invalid_argument("PX6AXGen3 requested tactile region is not configured.");
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data.";
|
||||
return {};
|
||||
}
|
||||
|
||||
ensureSensorReady(true, true, false);
|
||||
@ -546,14 +492,16 @@ TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion regi
|
||||
|
||||
PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, TactileRegion region) {
|
||||
if (!isSupportedRegion(finger, region)) {
|
||||
throw std::invalid_argument("PX6AXGen3 requested resultant-force region is not configured.");
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data.";
|
||||
return {};
|
||||
}
|
||||
|
||||
ensureSensorReady(true, false, true);
|
||||
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
if (!latest_snapshot_.resultant_valid || !isSampleFresh(latest_snapshot_.resultant_request_time)) {
|
||||
throw std::runtime_error("PX6AXGen3 resultant-force sample is unavailable or stale.");
|
||||
if (!latest_snapshot_.resultant_valid) {
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 resultant-force snapshot is not ready.";
|
||||
return {};
|
||||
}
|
||||
|
||||
return ResultantForce{
|
||||
@ -563,28 +511,29 @@ PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, Tactil
|
||||
};
|
||||
}
|
||||
|
||||
PX6AXGen3::ForceNewtons PX6AXGen3::getResultantForceNewtons(FingerType finger, TactileRegion region) {
|
||||
const auto raw = getResultantForce(finger, region);
|
||||
// PX-6AX GEN3 manual 5.6.2: one resultant-force LSB is 0.1 N.
|
||||
constexpr double kNewtonsPerCount = 0.1;
|
||||
return {raw.fx * kNewtonsPerCount, raw.fy * kNewtonsPerCount, raw.fz * kNewtonsPerCount};
|
||||
}
|
||||
|
||||
void PX6AXGen3::initializeSnapshot() {
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
latest_snapshot_ = SensorSnapshot{};
|
||||
}
|
||||
|
||||
void PX6AXGen3::ensureConnected() {
|
||||
if (!config_valid_ || port_name_.empty()) {
|
||||
throw std::runtime_error("PX6AXGen3 serial/configuration is invalid.");
|
||||
if (port_name_.empty()) {
|
||||
enterFault("PX6AXGen3 serial port is not configured.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!serial_) {
|
||||
serial_ = std::make_unique<::cmvr::PosixSerialTransport>();
|
||||
}
|
||||
|
||||
if (!serial_->isOpen()) {
|
||||
if (!serial_->open(::cmvr::AbstractSerialTransport::Config{port_name_, baud_rate_})) {
|
||||
throw std::runtime_error("Failed to open PX6AXGen3 serial transport: " + serial_->lastError());
|
||||
enterFault("Failed to open PX-6AX GEN3 serial transport: " + serial_->lastError());
|
||||
return;
|
||||
}
|
||||
calibration_performed_ = false;
|
||||
}
|
||||
|
||||
calibrateIfRequested();
|
||||
if (!isOperationalState(state())) {
|
||||
transitionTo(Status::INITIALIZED);
|
||||
@ -604,9 +553,27 @@ void PX6AXGen3::calibrateIfRequested() {
|
||||
if (!auto_calibrate_ || calibration_performed_) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto frame = buildCommandFrame(CommandType::CALIBRATION, device_address_, distributed_length_);
|
||||
// Write acknowledgment has a complete 14-byte header and LRC, no payload.
|
||||
transact(*serial_, frame, 0U, std::chrono::milliseconds(response_timeout_ms_));
|
||||
if (!serial_->flushInput()) {
|
||||
enterFault("Failed to flush serial input before calibration: " + serial_->lastError());
|
||||
return;
|
||||
}
|
||||
if (!serial_->write(frame)) {
|
||||
enterFault("Failed to send calibration command: " + serial_->lastError());
|
||||
return;
|
||||
}
|
||||
const auto response = readFramedResponse(
|
||||
*serial_,
|
||||
2U,
|
||||
frame.size(),
|
||||
std::chrono::milliseconds(response_timeout_ms_),
|
||||
"calibration response");
|
||||
if (findResponseFrameOffset(response, 2U) == std::string::npos) {
|
||||
enterFault("PX-6AX GEN3 calibration command did not receive a valid acknowledgment. raw=" +
|
||||
previewBytesHex(response));
|
||||
return;
|
||||
}
|
||||
calibration_performed_ = true;
|
||||
}
|
||||
|
||||
@ -617,51 +584,117 @@ void PX6AXGen3::refreshSensorData() {
|
||||
|
||||
void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_resultant) {
|
||||
if (!read_distributed && !read_resultant) {
|
||||
throw std::invalid_argument("PX6AXGen3 refresh requires at least one data type.");
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 refreshSensorData requires at least one data type to read.";
|
||||
return;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> refresh_lock(refresh_mutex_);
|
||||
const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant);
|
||||
|
||||
try {
|
||||
ensureConnected();
|
||||
// Publish force first: a slower distributed read must not postpone a
|
||||
// contact measurement. Each channel has its own request timestamp.
|
||||
if (read_resultant) {
|
||||
const auto frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_);
|
||||
const auto request_time = std::chrono::steady_clock::now();
|
||||
const auto payload = transact(*serial_, frame, resultant_length_,
|
||||
std::chrono::milliseconds(response_timeout_ms_));
|
||||
if (!isSampleFresh(request_time)) {
|
||||
throw std::runtime_error("PX6AXGen3 resultant-force response arrived too late.");
|
||||
}
|
||||
const auto force = parseResultantPayload(payload);
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
latest_snapshot_.resultant_force_tenths = force;
|
||||
latest_snapshot_.resultant_request_time = request_time;
|
||||
latest_snapshot_.resultant_valid = true;
|
||||
if (!isOperationalState(state())) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<TactilePoint> tactile_points;
|
||||
int rows = 0;
|
||||
int cols = 0;
|
||||
bool tactile_valid = false;
|
||||
|
||||
if (read_distributed) {
|
||||
const auto frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_);
|
||||
const auto request_time = std::chrono::steady_clock::now();
|
||||
const auto payload = transact(*serial_, frame, distributed_length_,
|
||||
std::chrono::milliseconds(response_timeout_ms_));
|
||||
if (!isSampleFresh(request_time)) {
|
||||
throw std::runtime_error("PX6AXGen3 distributed-force response arrived too late.");
|
||||
const auto distributed_frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_);
|
||||
if (!serial_->flushInput()) {
|
||||
handleRefreshFailure("Failed to flush serial input before distributed-force read: " + serial_->lastError(), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
auto points = parseDistributedPayload(payload);
|
||||
const auto [rows, cols] = resolveMatrixShape(tactile_rows_, tactile_cols_, points.size());
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
latest_snapshot_.tactile_points = std::move(points);
|
||||
if (!serial_->write(distributed_frame)) {
|
||||
handleRefreshFailure("Failed to send distributed-force command: " + serial_->lastError(), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
const size_t distributed_frame_bytes =
|
||||
static_cast<size_t>(response_header_bytes_) + static_cast<size_t>(distributed_length_);
|
||||
const auto distributed_response = readFramedResponse(
|
||||
*serial_,
|
||||
distributed_frame_bytes,
|
||||
distributed_frame.size(),
|
||||
std::chrono::milliseconds(response_timeout_ms_),
|
||||
"distributed tactile response");
|
||||
const auto distributed_frame_offset =
|
||||
findResponseFrameOffset(distributed_response, distributed_frame_bytes);
|
||||
if (distributed_frame_offset == std::string::npos) {
|
||||
handleRefreshFailure("Invalid distributed tactile response header. raw=" + previewBytesHex(distributed_response), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
|
||||
tactile_points = parseDistributedPayload(extractPayload(
|
||||
distributed_response,
|
||||
distributed_frame_offset,
|
||||
static_cast<size_t>(response_header_bytes_),
|
||||
static_cast<size_t>(distributed_length_)));
|
||||
std::tie(rows, cols) = resolveMatrixShape(tactile_rows_, tactile_cols_, tactile_points.size());
|
||||
tactile_valid = !tactile_points.empty();
|
||||
}
|
||||
|
||||
std::array<int, 3> resultant_force_tenths{};
|
||||
bool resultant_valid = false;
|
||||
if (read_resultant) {
|
||||
try {
|
||||
const auto resultant_frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_);
|
||||
if (!serial_->flushInput()) {
|
||||
handleRefreshFailure("Failed to flush serial input before resultant-force read: " + serial_->lastError(), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
if (!serial_->write(resultant_frame)) {
|
||||
handleRefreshFailure("Failed to send resultant-force command: " + serial_->lastError(), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
const size_t resultant_frame_bytes =
|
||||
static_cast<size_t>(response_header_bytes_) + static_cast<size_t>(resultant_length_);
|
||||
const auto resultant_response = readFramedResponse(
|
||||
*serial_,
|
||||
resultant_frame_bytes,
|
||||
resultant_frame.size(),
|
||||
std::chrono::milliseconds(response_timeout_ms_),
|
||||
"resultant-force response");
|
||||
const auto resultant_frame_offset =
|
||||
findResponseFrameOffset(resultant_response, resultant_frame_bytes);
|
||||
if (resultant_frame_offset != std::string::npos) {
|
||||
resultant_force_tenths = parseResultantPayload(extractPayload(
|
||||
resultant_response,
|
||||
resultant_frame_offset,
|
||||
static_cast<size_t>(response_header_bytes_),
|
||||
static_cast<size_t>(resultant_length_)));
|
||||
resultant_valid = true;
|
||||
} else {
|
||||
handleRefreshFailure("Invalid resultant-force response header. raw=" + previewBytesHex(resultant_response), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
if (!read_distributed) {
|
||||
handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
CMVR_LOG(WARNING) << "[PX6AXGen3] Failed to refresh resultant force: " << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
if (read_distributed) {
|
||||
latest_snapshot_.tactile_points = std::move(tactile_points);
|
||||
latest_snapshot_.rows = rows;
|
||||
latest_snapshot_.cols = cols;
|
||||
latest_snapshot_.tactile_request_time = request_time;
|
||||
latest_snapshot_.tactile_valid = true;
|
||||
latest_snapshot_.tactile_valid = tactile_valid;
|
||||
}
|
||||
if (read_resultant) {
|
||||
latest_snapshot_.resultant_force_tenths = resultant_force_tenths;
|
||||
latest_snapshot_.resultant_valid = resultant_valid;
|
||||
}
|
||||
|
||||
clearOperationalError();
|
||||
} catch (const std::exception& e) {
|
||||
handleRefreshFailure(e.what());
|
||||
// Let initialization retries and synchronous callers see the failure.
|
||||
// The polling thread catches it and continues reconnecting in background.
|
||||
throw;
|
||||
handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
@ -682,7 +715,7 @@ void PX6AXGen3::refreshSensorDataWithRetry(const int max_attempts,
|
||||
}
|
||||
}
|
||||
|
||||
throw std::runtime_error(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error);
|
||||
handleRefreshFailure(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error, isSnapshotReady(true, true));
|
||||
}
|
||||
|
||||
void PX6AXGen3::pollingLoop() {
|
||||
@ -721,21 +754,18 @@ void PX6AXGen3::pollingLoop() {
|
||||
void PX6AXGen3::ensureSensorReady(const bool allow_background,
|
||||
const bool require_tactile,
|
||||
const bool require_resultant) {
|
||||
const auto lifecycle = state();
|
||||
if (!config_valid_ || lifecycle == Status::STOPPED || lifecycle == Status::FAULT) {
|
||||
throw std::runtime_error("PX6AXGen3 is not operational: " + lastError());
|
||||
}
|
||||
const auto [polls_tactile, polls_resultant] = resolvePollingReadSelection();
|
||||
const bool background_covers_request =
|
||||
(!require_tactile || polls_tactile) &&
|
||||
(!require_resultant || polls_resultant);
|
||||
if (allow_background && background_covers_request &&
|
||||
polling_thread_running_.load(std::memory_order_acquire)) {
|
||||
// The getter checks freshness while copying under snapshot_mutex_.
|
||||
// Never block the control loop on serial I/O to replace a stale sample.
|
||||
return;
|
||||
const bool background_ready = allow_background &&
|
||||
background_covers_request &&
|
||||
polling_thread_running_.load(std::memory_order_acquire) &&
|
||||
isSnapshotReady(require_tactile, require_resultant);
|
||||
|
||||
if (!background_ready) {
|
||||
refreshSensorData(require_tactile, require_resultant);
|
||||
}
|
||||
refreshSensorData(require_tactile, require_resultant);
|
||||
}
|
||||
|
||||
bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion region) const {
|
||||
@ -744,15 +774,8 @@ bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion r
|
||||
|
||||
bool PX6AXGen3::isSnapshotReady(const bool require_tactile, const bool require_resultant) const {
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
return (!require_tactile || (latest_snapshot_.tactile_valid &&
|
||||
isSampleFresh(latest_snapshot_.tactile_request_time))) &&
|
||||
(!require_resultant || (latest_snapshot_.resultant_valid &&
|
||||
isSampleFresh(latest_snapshot_.resultant_request_time)));
|
||||
}
|
||||
|
||||
bool PX6AXGen3::isSampleFresh(const std::chrono::steady_clock::time_point request_time) const {
|
||||
return request_time != std::chrono::steady_clock::time_point{} &&
|
||||
std::chrono::steady_clock::now() - request_time <= max_sample_age_;
|
||||
return (!require_tactile || latest_snapshot_.tactile_valid) &&
|
||||
(!require_resultant || latest_snapshot_.resultant_valid);
|
||||
}
|
||||
|
||||
TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const {
|
||||
@ -762,8 +785,9 @@ TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const {
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
if (!latest_snapshot_.tactile_valid || !isSampleFresh(latest_snapshot_.tactile_request_time)) {
|
||||
throw std::runtime_error("PX6AXGen3 tactile sample is unavailable or stale.");
|
||||
if (!latest_snapshot_.tactile_valid) {
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 tactile snapshot is not ready.";
|
||||
return {};
|
||||
}
|
||||
*snapshot = latest_snapshot_.tactile_points;
|
||||
rows = latest_snapshot_.rows;
|
||||
@ -799,15 +823,21 @@ void PX6AXGen3::clearOperationalError() {
|
||||
}
|
||||
}
|
||||
|
||||
void PX6AXGen3::handleRefreshFailure(const std::string& error) {
|
||||
// Invalidate before any close/reconnect work. Keep the worker alive so a
|
||||
// subsequent valid transaction can restore service, but never expose old data.
|
||||
initializeSnapshot();
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
|
||||
last_error_ = error;
|
||||
}
|
||||
void PX6AXGen3::handleRefreshFailure(const std::string& error, const bool had_valid_snapshot) {
|
||||
closeConnection();
|
||||
|
||||
if (had_valid_snapshot) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
|
||||
if (lifecycle_state_ == Status::INITIALIZED || lifecycle_state_ == Status::STREAMING) {
|
||||
last_error_ = error;
|
||||
}
|
||||
}
|
||||
CMVR_LOG(WARNING) << error;
|
||||
return;
|
||||
}
|
||||
|
||||
enterFault(error);
|
||||
}
|
||||
|
||||
void PX6AXGen3::transitionTo(const Status next_state) {
|
||||
@ -827,7 +857,6 @@ void PX6AXGen3::enterFault(const std::string& error) {
|
||||
|
||||
polling_thread_running_.store(false, std::memory_order_release);
|
||||
polling_cv_.notify_all();
|
||||
initializeSnapshot();
|
||||
closeConnection();
|
||||
|
||||
CMVR_LOG(ERROR) << error;
|
||||
|
||||
@ -1,201 +0,0 @@
|
||||
#include "../include/px_6ax_gen3.h"
|
||||
#include "hardware/include/posix_serial_transport.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <csignal>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace {
|
||||
using Clock = std::chrono::steady_clock;
|
||||
volatile std::sig_atomic_t interrupted = 0;
|
||||
void interrupt(int) { interrupted = 1; }
|
||||
|
||||
// Wrap the actual POSIX serial transport; optionally capture every transmitted
|
||||
// and received byte to diagnose framing/USB latency without a second reader.
|
||||
class TraceTransport final : public cmvr::AbstractSerialTransport {
|
||||
public:
|
||||
explicit TraceTransport(const std::string& path) {
|
||||
if (!path.empty()) {
|
||||
trace_.open(path);
|
||||
if (!trace_) throw std::runtime_error("Cannot open raw log: " + path);
|
||||
}
|
||||
}
|
||||
bool open(const Config& cfg) override { return serial_.open(cfg); }
|
||||
bool close() override { return serial_.close(); }
|
||||
bool isOpen() const override { return serial_.isOpen(); }
|
||||
bool flushInput() override { return serial_.flushInput(); }
|
||||
bool write(const std::vector<uint8_t>& data) override {
|
||||
// Only permit sensor read requests, even if driver defaults change.
|
||||
if (data.size() != 14 || data[6] != 0xFB) {
|
||||
throw std::runtime_error("Real test only permits force read commands.");
|
||||
}
|
||||
const bool ok = serial_.write(data);
|
||||
record(ok ? "TX" : "TX_FAILED", data);
|
||||
return ok;
|
||||
}
|
||||
bool read(size_t n, std::chrono::milliseconds timeout, std::vector<uint8_t>& data) override {
|
||||
const bool ok = serial_.read(n, timeout, data);
|
||||
record(ok ? "RX" : "RX_FAILED", data);
|
||||
return ok;
|
||||
}
|
||||
std::string lastError() const override { return serial_.lastError(); }
|
||||
private:
|
||||
void record(const char* direction, const std::vector<uint8_t>& data) {
|
||||
if (!trace_.is_open()) return;
|
||||
trace_ << std::fixed << std::setprecision(3)
|
||||
<< std::chrono::duration<double, std::milli>(Clock::now() - begin_).count()
|
||||
<< " ms " << direction;
|
||||
for (const auto byte : data) {
|
||||
trace_ << ' ' << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte);
|
||||
}
|
||||
trace_ << std::dec << std::setfill(' ') << '\n';
|
||||
trace_.flush();
|
||||
}
|
||||
cmvr::PosixSerialTransport serial_;
|
||||
std::ofstream trace_;
|
||||
Clock::time_point begin_{Clock::now()};
|
||||
};
|
||||
|
||||
struct Options {
|
||||
std::string port{"/dev/ttyACM0"};
|
||||
std::string mode{"sync"};
|
||||
std::string csv;
|
||||
std::string raw_log;
|
||||
int module_id{2};
|
||||
int poll_ms{5};
|
||||
int max_age_ms{50};
|
||||
int timeout_ms{200};
|
||||
double duration_s{10.0};
|
||||
};
|
||||
|
||||
void usage() {
|
||||
std::cout << "Usage: px_6ax_gen3_real_test [--port /dev/ttyACM0] [--module-id 2]\n"
|
||||
" [--duration-s 10] [--mode sync|stream] [--poll-ms 5]\n"
|
||||
" [--max-age-ms 50] [--timeout-ms 200] [--csv samples.csv]\n"
|
||||
" [--raw-log frames.log]\n"
|
||||
"sync: each read sends a new force request; latency is request/response time.\n"
|
||||
"stream: exercise background polling and cached getters used by the task;\n"
|
||||
" getter counts include repeated samples, not sensor update frequency.\n"
|
||||
"Only reads the sensor. No calibration or robot commands. Ctrl-C stops.\n";
|
||||
}
|
||||
|
||||
Options parseOptions(int argc, char** argv) {
|
||||
Options o;
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
const std::string arg = argv[i];
|
||||
if (arg == "--help") { usage(); std::exit(0); }
|
||||
if (++i >= argc) throw std::invalid_argument("Missing value for " + arg);
|
||||
const std::string value = argv[i];
|
||||
if (arg == "--port") o.port = value;
|
||||
else if (arg == "--mode") o.mode = value;
|
||||
else if (arg == "--module-id") o.module_id = std::stoi(value);
|
||||
else if (arg == "--duration-s") o.duration_s = std::stod(value);
|
||||
else if (arg == "--poll-ms") o.poll_ms = std::stoi(value);
|
||||
else if (arg == "--max-age-ms") o.max_age_ms = std::stoi(value);
|
||||
else if (arg == "--timeout-ms") o.timeout_ms = std::stoi(value);
|
||||
else if (arg == "--csv") o.csv = value;
|
||||
else if (arg == "--raw-log") o.raw_log = value;
|
||||
else throw std::invalid_argument("Unknown option: " + arg);
|
||||
}
|
||||
if ((o.mode != "sync" && o.mode != "stream") || !std::isfinite(o.duration_s) ||
|
||||
o.duration_s <= 0 || o.poll_ms <= 0 || o.max_age_ms <= 0 || o.timeout_ms <= 0) {
|
||||
throw std::invalid_argument("Invalid mode, duration or timing option");
|
||||
}
|
||||
return o;
|
||||
}
|
||||
|
||||
double percentile(const std::vector<double>& sorted, double fraction) {
|
||||
return sorted.empty() ? 0.0 : sorted[static_cast<size_t>((sorted.size() - 1) * fraction)];
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
try {
|
||||
const auto options = parseOptions(argc, argv);
|
||||
std::signal(SIGINT, interrupt);
|
||||
std::signal(SIGTERM, interrupt);
|
||||
cmvr::config::PX6AXGen3 cfg;
|
||||
cfg.set_serial_port(options.port);
|
||||
cfg.set_module_id(options.module_id);
|
||||
cfg.set_baud_rate(921600);
|
||||
cfg.set_distributed_length(153);
|
||||
cfg.set_resultant_length(3);
|
||||
cfg.set_poll_interval_ms(options.poll_ms);
|
||||
cfg.set_response_timeout_ms(options.timeout_ms);
|
||||
cfg.set_max_sample_age_ms(options.max_age_ms);
|
||||
cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE);
|
||||
cfg.set_auto_calibrate(false);
|
||||
cmvr::device::PX6AXGen3 sensor(cfg, std::make_unique<TraceTransport>(options.raw_log));
|
||||
std::ofstream csv;
|
||||
if (!options.csv.empty()) {
|
||||
csv.open(options.csv);
|
||||
if (!csv) throw std::runtime_error("Cannot open CSV: " + options.csv);
|
||||
csv << "elapsed_ms,read_ms,valid,fx_N,fy_N,fz_N\n";
|
||||
}
|
||||
std::cout << "port=" << options.port << " module_id=" << options.module_id
|
||||
<< " device_address=" << options.module_id + 1 << " mode=" << options.mode
|
||||
<< " max_sample_age_ms=" << options.max_age_ms << '\n';
|
||||
if (!sensor.init()) throw std::runtime_error("Sensor init failed: " + sensor.lastError());
|
||||
if (options.mode == "stream" && !sensor.start()) {
|
||||
throw std::runtime_error("Sensor start failed: " + sensor.lastError());
|
||||
}
|
||||
const auto begin = Clock::now();
|
||||
auto next = begin;
|
||||
auto next_print = begin;
|
||||
std::vector<double> durations;
|
||||
size_t failures = 0;
|
||||
double max_fz = 0.0;
|
||||
while (!interrupted && std::chrono::duration<double>(Clock::now() - begin).count() < options.duration_s) {
|
||||
const auto read_begin = Clock::now();
|
||||
cmvr::device::AbstractDexHand::ForceNewtons force;
|
||||
bool valid = true;
|
||||
std::string error;
|
||||
try {
|
||||
force = sensor.getResultantForceNewtons(cmvr::device::AbstractDexHand::FingerType::INDEX,
|
||||
cmvr::device::AbstractDexHand::TactileRegion::TIP);
|
||||
} catch (const std::exception& e) {
|
||||
valid = false;
|
||||
error = e.what();
|
||||
++failures;
|
||||
}
|
||||
const auto now = Clock::now();
|
||||
const double read_ms = std::chrono::duration<double, std::milli>(now - read_begin).count();
|
||||
const double elapsed_ms = std::chrono::duration<double, std::milli>(now - begin).count();
|
||||
if (valid) { durations.push_back(read_ms); max_fz = std::max(max_fz, force.fz); }
|
||||
if (csv.is_open()) {
|
||||
csv << std::fixed << std::setprecision(3) << elapsed_ms << ',' << read_ms << ',' << valid;
|
||||
if (valid) csv << ',' << force.fx << ',' << force.fy << ',' << force.fz;
|
||||
else csv << ",,,";
|
||||
csv << '\n';
|
||||
}
|
||||
if (now >= next_print) {
|
||||
std::cout << std::fixed << std::setprecision(3) << "t_ms=" << elapsed_ms
|
||||
<< " read_ms=" << read_ms;
|
||||
if (valid) std::cout << " force_N=[" << force.fx << ',' << force.fy << ',' << force.fz
|
||||
<< "] fz_N=" << force.fz;
|
||||
else std::cout << " UNAVAILABLE: " << error;
|
||||
std::cout << std::endl;
|
||||
next_print = now + std::chrono::milliseconds(100);
|
||||
}
|
||||
next += std::chrono::milliseconds(options.poll_ms);
|
||||
if (next < now) next = now;
|
||||
std::this_thread::sleep_until(next);
|
||||
}
|
||||
const double elapsed_s = std::chrono::duration<double>(Clock::now() - begin).count();
|
||||
sensor.stop();
|
||||
std::sort(durations.begin(), durations.end());
|
||||
std::cout << "SUMMARY mode=" << options.mode << " valid_reads=" << durations.size()
|
||||
<< " unavailable_reads=" << failures << " valid_reads_per_s=" << durations.size() / elapsed_s
|
||||
<< " p50_ms=" << percentile(durations, .5) << " p95_ms=" << percentile(durations, .95)
|
||||
<< " max_ms=" << percentile(durations, 1) << " max_fz_N=" << max_fz << '\n';
|
||||
return durations.empty() || failures != 0 ? 1 : 0;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << e.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@ -1,318 +0,0 @@
|
||||
#include "../include/px_6ax_gen3.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <functional>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace {
|
||||
using Sensor = cmvr::device::PX6AXGen3;
|
||||
using Bytes = std::vector<uint8_t>;
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
void checksum(Bytes& frame) {
|
||||
unsigned sum = 0;
|
||||
for (size_t i = 0; i + 1 < frame.size(); ++i) sum += frame[i];
|
||||
frame.back() = static_cast<uint8_t>(-sum);
|
||||
}
|
||||
|
||||
Bytes reply(const Bytes& request) {
|
||||
const bool is_write = request[6] == 0x79;
|
||||
const size_t n = is_write ? 0U : request[11] | (size_t(request[12]) << 8U);
|
||||
Bytes frame{0xAA, 0x55, static_cast<uint8_t>((n + 10U) & 0xffU),
|
||||
static_cast<uint8_t>((n + 10U) >> 8U)};
|
||||
frame.insert(frame.end(), request.begin() + 4, request.begin() + 13);
|
||||
// Real device returns status=1 on reads; zero is only the write ACK status.
|
||||
frame.push_back(is_write ? 0 : 1);
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
frame.push_back(i % 3U == 0 ? 0x80 : (i % 3U == 1 ? 0x7f : 10));
|
||||
}
|
||||
frame.push_back(0);
|
||||
checksum(frame);
|
||||
return frame;
|
||||
}
|
||||
|
||||
class ScriptedTransport final : public cmvr::AbstractSerialTransport {
|
||||
public:
|
||||
bool open(const Config&) override { opened = true; return true; }
|
||||
bool close() override { opened = false; return true; }
|
||||
bool isOpen() const override { return opened; }
|
||||
bool flushInput() override { pending.clear(); return true; }
|
||||
bool write(const Bytes& request) override {
|
||||
++writes;
|
||||
unsigned sum = 0;
|
||||
for (auto byte : request) sum += byte;
|
||||
EXPECT_EQ(sum % 256, 0U);
|
||||
pending = dropping.load() ? Bytes{} : make_reply(request);
|
||||
return true;
|
||||
}
|
||||
bool read(size_t, std::chrono::milliseconds timeout, Bytes& out) override {
|
||||
out.clear();
|
||||
if (pending.empty()) {
|
||||
waiting.store(true);
|
||||
std::this_thread::sleep_for(timeout);
|
||||
waiting.store(false);
|
||||
return false;
|
||||
}
|
||||
const size_t n = std::min(chunk_size, pending.size());
|
||||
out.assign(pending.begin(), pending.begin() + n);
|
||||
pending.erase(pending.begin(), pending.begin() + n);
|
||||
return true;
|
||||
}
|
||||
std::string lastError() const override { return "scripted timeout"; }
|
||||
std::function<Bytes(const Bytes&)> make_reply{reply};
|
||||
size_t chunk_size{256};
|
||||
std::atomic<int> writes{0};
|
||||
std::atomic<bool> dropping{false};
|
||||
std::atomic<bool> waiting{false};
|
||||
private:
|
||||
bool opened{false};
|
||||
Bytes pending;
|
||||
};
|
||||
|
||||
cmvr::config::PX6AXGen3 config() {
|
||||
cmvr::config::PX6AXGen3 cfg;
|
||||
cfg.set_serial_port("scripted");
|
||||
cfg.set_module_id(2);
|
||||
cfg.set_distributed_length(153);
|
||||
cfg.set_poll_interval_ms(5);
|
||||
cfg.set_response_timeout_ms(5);
|
||||
cfg.set_max_sample_age_ms(50);
|
||||
cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE);
|
||||
return cfg;
|
||||
}
|
||||
|
||||
Sensor::ResultantForce force(Sensor& sensor) {
|
||||
return sensor.getResultantForce(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
|
||||
}
|
||||
|
||||
Sensor::ForceNewtons forceNewtons(cmvr::device::AbstractDexHand& sensor) {
|
||||
return sensor.getResultantForceNewtons(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
|
||||
}
|
||||
|
||||
bool waitFor(const std::function<bool()>& predicate) {
|
||||
const auto deadline = std::chrono::steady_clock::now() + 1s;
|
||||
do {
|
||||
if (predicate()) return true;
|
||||
std::this_thread::sleep_for(1ms);
|
||||
} while (std::chrono::steady_clock::now() < deadline);
|
||||
return predicate();
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, ParsesRealReadStatusAndSignedForcesAcrossByteFragments) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->chunk_size = 1;
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
const auto f = force(sensor);
|
||||
EXPECT_EQ(f.fx, -128);
|
||||
EXPECT_EQ(f.fy, 127);
|
||||
EXPECT_EQ(f.fz, 10);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, NewtonInterfacePreservesSignsAndComputesPhysicalMagnitude) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
serial->make_reply = [](const Bytes& request) {
|
||||
auto frame = reply(request);
|
||||
frame[14] = 0xfd; // -3 raw = -0.3 N
|
||||
frame[15] = 4;
|
||||
frame[16] = 12;
|
||||
checksum(frame);
|
||||
return frame;
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
const auto writes_before = transport->writes.load();
|
||||
const auto f = forceNewtons(sensor);
|
||||
EXPECT_EQ(transport->writes.load(), writes_before + 1);
|
||||
EXPECT_DOUBLE_EQ(f.fx, -0.3);
|
||||
EXPECT_DOUBLE_EQ(f.fy, 0.4);
|
||||
EXPECT_DOUBLE_EQ(f.fz, 1.2);
|
||||
EXPECT_DOUBLE_EQ(f.magnitude(), 1.3);
|
||||
EXPECT_EQ(force(sensor).fz, 12); // Raw interface remains unchanged.
|
||||
}
|
||||
|
||||
class NewtonForce : public testing::TestWithParam<int> {};
|
||||
TEST_P(NewtonForce, ConvertsRawFzOnceAndPreservesPointOneNewtonTrigger) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
const int raw_fz = GetParam();
|
||||
serial->make_reply = [raw_fz](const Bytes& request) {
|
||||
auto frame = reply(request);
|
||||
frame[14] = 0;
|
||||
frame[15] = 0;
|
||||
frame[16] = static_cast<uint8_t>(raw_fz);
|
||||
checksum(frame);
|
||||
return frame;
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
const auto f = forceNewtons(sensor);
|
||||
EXPECT_DOUBLE_EQ(f.fz, static_cast<double>(raw_fz) / 10.0);
|
||||
EXPECT_DOUBLE_EQ(f.magnitude(), f.fz);
|
||||
EXPECT_EQ(f.fz >= 0.1, raw_fz >= 1);
|
||||
EXPECT_EQ(force(sensor).fz, raw_fz);
|
||||
}
|
||||
INSTANTIATE_TEST_SUITE_P(PhysicalUnits, NewtonForce, testing::Values(0, 1, 10, 108, 255));
|
||||
|
||||
TEST(PX6AXGen3, ResynchronizesAfterEchoNoiseAndInvalidFrame) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->chunk_size = 7;
|
||||
serial->make_reply = [](const Bytes& request) {
|
||||
auto bad = reply(request);
|
||||
bad.back() ^= 1;
|
||||
Bytes frames{0xAA, 0x00};
|
||||
frames.insert(frames.end(), request.begin(), request.end());
|
||||
frames.insert(frames.end(), bad.begin(), bad.end());
|
||||
const auto valid = reply(request);
|
||||
frames.insert(frames.end(), valid.begin(), valid.end());
|
||||
return frames;
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
EXPECT_EQ(force(sensor).fz, 10);
|
||||
}
|
||||
|
||||
class InvalidFrame : public testing::TestWithParam<int> {};
|
||||
TEST_P(InvalidFrame, RejectsResponseAndFailsInitializationAfterRetries) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
const int kind = GetParam();
|
||||
serial->make_reply = [kind](const Bytes& request) {
|
||||
auto frame = reply(request);
|
||||
if (kind == 0) { frame.pop_back(); return frame; } // missing LRC
|
||||
if (kind == 1) { frame.back() ^= 1; return frame; } // bad LRC
|
||||
// Frame size, device, reserved, function, register, returned byte count.
|
||||
const int offsets[]{2, 4, 5, 6, 7, 11};
|
||||
frame[offsets[kind - 2]] ^= 1;
|
||||
checksum(frame); // Valid LRC must not bypass request matching.
|
||||
return frame;
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
EXPECT_FALSE(sensor.init());
|
||||
EXPECT_EQ(transport->writes.load(), 5);
|
||||
EXPECT_EQ(sensor.state(), Sensor::Status::FAULT);
|
||||
EXPECT_FALSE(sensor.lastError().empty());
|
||||
EXPECT_THROW(force(sensor), std::runtime_error);
|
||||
}
|
||||
INSTANTIATE_TEST_SUITE_P(ProtocolValidation, InvalidFrame, testing::Range(0, 8));
|
||||
|
||||
TEST(PX6AXGen3, RetriesTransientStartupFailure) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
int count = 0;
|
||||
serial->make_reply = [&count](const Bytes& request) {
|
||||
++count;
|
||||
return count < 3 ? Bytes{} : reply(request);
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
EXPECT_EQ(count, 3);
|
||||
EXPECT_TRUE(sensor.lastError().empty());
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, StaleGetterFailsWithoutWaitingForSerialAndRecovers) {
|
||||
auto cfg = config();
|
||||
cfg.set_response_timeout_ms(200);
|
||||
cfg.set_max_sample_age_ms(30);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
ASSERT_TRUE(sensor.start());
|
||||
ASSERT_EQ(force(sensor).fz, 10);
|
||||
transport->dropping.store(true);
|
||||
ASSERT_TRUE(waitFor([&] { return transport->waiting.load(); }));
|
||||
std::this_thread::sleep_for(40ms);
|
||||
const auto begin = std::chrono::steady_clock::now();
|
||||
EXPECT_THROW(force(sensor), std::runtime_error);
|
||||
EXPECT_THROW(forceNewtons(sensor), std::runtime_error);
|
||||
EXPECT_LT(std::chrono::steady_clock::now() - begin, 50ms);
|
||||
cmvr::device::DexHandState state;
|
||||
sensor.getState(state);
|
||||
EXPECT_FALSE(state.is_initialized);
|
||||
EXPECT_FALSE(state.hands[0].error_message.empty());
|
||||
transport->dropping.store(false);
|
||||
ASSERT_TRUE(waitFor([&] {
|
||||
try { return force(sensor).fz == 10; }
|
||||
catch (const std::exception&) { return false; }
|
||||
}));
|
||||
sensor.stop();
|
||||
const auto writes = transport->writes.load();
|
||||
EXPECT_THROW(force(sensor), std::runtime_error);
|
||||
EXPECT_THROW(forceNewtons(sensor), std::runtime_error);
|
||||
EXPECT_EQ(transport->writes.load(), writes);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, ReadFailureInvalidatesPreviouslyValidSample) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
transport->dropping.store(true);
|
||||
EXPECT_THROW(force(sensor), std::runtime_error);
|
||||
cmvr::device::DexHandState state;
|
||||
sensor.getState(state);
|
||||
EXPECT_FALSE(state.is_initialized);
|
||||
transport->dropping.store(false);
|
||||
EXPECT_EQ(force(sensor).fz, 10);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, RejectsLateResponseEvenWithValidChecksum) {
|
||||
auto cfg = config();
|
||||
cfg.set_max_sample_age_ms(2);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->make_reply = [](const Bytes& request) {
|
||||
std::this_thread::sleep_for(5ms);
|
||||
return reply(request);
|
||||
};
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
EXPECT_FALSE(sensor.init());
|
||||
EXPECT_NE(sensor.lastError().find("too late"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, DistributedDataIsValidatedAndParsed) {
|
||||
auto cfg = config();
|
||||
cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_FORCE);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->chunk_size = 5;
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
auto data = sensor.getSensorData(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
|
||||
ASSERT_TRUE(data.valid());
|
||||
ASSERT_EQ(data.view.pointCount(), 51);
|
||||
EXPECT_EQ(data.view.at(0, 50).fx, -128);
|
||||
EXPECT_EQ(data.view.at(0, 50).fz, 10);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, CalibrationRequiresFullSuccessfulWriteAcknowledgment) {
|
||||
auto cfg = config();
|
||||
cfg.set_auto_calibrate(true);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->make_reply = [](const Bytes& request) {
|
||||
if (request[6] == 0x79) return Bytes{0xAA, 0x55};
|
||||
return reply(request);
|
||||
};
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
EXPECT_FALSE(sensor.init());
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, CalibrationAcceptsFullWriteAcknowledgment) {
|
||||
auto cfg = config();
|
||||
cfg.set_auto_calibrate(true);
|
||||
Sensor sensor(cfg, std::make_unique<ScriptedTransport>());
|
||||
ASSERT_TRUE(sensor.init());
|
||||
EXPECT_EQ(force(sensor).fz, 10);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, InvalidConfigurationCannotBeResurrectedByInitOrStart) {
|
||||
auto cfg = config();
|
||||
cfg.set_response_header_bytes(15);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
EXPECT_FALSE(sensor.init());
|
||||
EXPECT_FALSE(sensor.start());
|
||||
EXPECT_EQ(transport->writes.load(), 0);
|
||||
}
|
||||
}
|
||||
@ -38,7 +38,6 @@ public:
|
||||
std::vector<TactileRegionData> getSensorData() override;
|
||||
TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
|
||||
ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
|
||||
ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override;
|
||||
|
||||
private:
|
||||
TactileRegionData makeRegionData(FingerType finger, TactileRegion region);
|
||||
|
||||
@ -83,11 +83,6 @@ ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce(
|
||||
return TactilePoint::fromFz(0);
|
||||
}
|
||||
|
||||
ZeroSimTouchDexHand::ForceNewtons ZeroSimTouchDexHand::getResultantForceNewtons(
|
||||
const FingerType, const TactileRegion) {
|
||||
return {};
|
||||
}
|
||||
|
||||
ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData(
|
||||
const FingerType finger, const TactileRegion region) {
|
||||
tactile_points_[0] = TactilePoint::fromFz(0);
|
||||
|
||||
@ -95,7 +95,6 @@ public:
|
||||
|
||||
int targetU() const;
|
||||
int targetV() const;
|
||||
// Selected force criterion summed over requested tactile regions, in N.
|
||||
double lastTouchPressureSum() const;
|
||||
int lastTouchNonzeroCount() const;
|
||||
int lastActiveTagId() const;
|
||||
@ -184,8 +183,8 @@ private:
|
||||
int pbvs_debug_count_{0};
|
||||
int last_active_tag_id_{-1};
|
||||
|
||||
double last_touch_pressure_sum_{0.0}; // N
|
||||
double last_touch_resultant_fz_{0.0}; // N
|
||||
double last_touch_pressure_sum_{0.0};
|
||||
double last_touch_resultant_fz_{0.0};
|
||||
int last_touch_nonzero_count_{0};
|
||||
Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
|
||||
Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
|
||||
@ -200,10 +199,6 @@ private:
|
||||
Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
|
||||
bool retract_start_position_valid_{false};
|
||||
Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()};
|
||||
Eigen::Vector3d retract_direction_base_{Eigen::Vector3d::Zero()};
|
||||
// Sampled maximum TCP displacement opposite to the retract direction,
|
||||
// relative to the first applied retract command's measured TCP position.
|
||||
double max_forward_after_retract_m_{0.0};
|
||||
bool have_last_T_B_G_{false};
|
||||
Eigen::Matrix4d last_T_B_G_{Eigen::Matrix4d::Identity()};
|
||||
double max_T_B_G_translation_delta_m_{0.0};
|
||||
|
||||
@ -116,15 +116,15 @@ bool isTouchTriggered(const TouchScreenTaskConfig& config,
|
||||
return resultant_force_value >= config.touch().tactile().force_threshold();
|
||||
}
|
||||
|
||||
double tactileForceValue(const device::AbstractDexHand::ForceNewtons& point,
|
||||
double tactileForceValue(const device::AbstractDexHand::TactilePoint& point,
|
||||
const cmvr::config::TouchScreenTactileCriterion criterion) {
|
||||
switch (criterion) {
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ:
|
||||
return point.fz;
|
||||
return static_cast<double>(point.fz);
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE:
|
||||
return point.magnitude();
|
||||
}
|
||||
return point.fz;
|
||||
return static_cast<double>(point.fz);
|
||||
}
|
||||
|
||||
Eigen::Matrix3d rotationFromTargetEuler(const double rx,
|
||||
@ -988,9 +988,6 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
|
||||
!std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 ||
|
||||
!cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() ||
|
||||
!std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 ||
|
||||
(retract.has_linear_jerk() &&
|
||||
(!std::isfinite(retract.linear_jerk()) || retract.linear_jerk() <= 0.0)) ||
|
||||
cmvr::common::math::toEigenVec6(retract.twist_tool()).head<3>().norm() <= 1e-9 ||
|
||||
!std::isfinite(retract.distance_m()) || retract.distance_m() <= 0.0) {
|
||||
return false;
|
||||
}
|
||||
@ -1061,8 +1058,6 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
|
||||
return twist.allFinite() &&
|
||||
std::isfinite(speed_l.acceleration()) &&
|
||||
speed_l.acceleration() > 0.0 &&
|
||||
(!speed_l.has_linear_jerk() ||
|
||||
(std::isfinite(speed_l.linear_jerk()) && speed_l.linear_jerk() > 0.0)) &&
|
||||
std::isfinite(speed_l.max_distance_m()) &&
|
||||
speed_l.max_distance_m() >= 0.0;
|
||||
}
|
||||
@ -1643,28 +1638,6 @@ bool TouchScreenTask::stepRetracting() {
|
||||
const auto now = Clock::now();
|
||||
const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
|
||||
const double retract_distance_m = config_.retract().distance_m();
|
||||
if (!retract_start_position_valid_) {
|
||||
const auto reference = arm_->getSpeedLReference();
|
||||
if (!reference.valid) {
|
||||
if (!arm_->busy() || elapsed > 1.0) {
|
||||
enterFailed(Status::ROBOT_STATE_FAILED);
|
||||
return false;
|
||||
}
|
||||
return true; // First controller tick has not captured the origin yet.
|
||||
}
|
||||
retract_start_position_base_ << reference.tcp_pose_base.x,
|
||||
reference.tcp_pose_base.y,
|
||||
reference.tcp_pose_base.z;
|
||||
retract_direction_base_ << reference.target_base.vx,
|
||||
reference.target_base.vy, reference.target_base.vz;
|
||||
if (!retract_start_position_base_.allFinite() || !retract_direction_base_.allFinite() ||
|
||||
retract_direction_base_.norm() <= 1e-9) {
|
||||
enterFailed(Status::ROBOT_STATE_FAILED);
|
||||
return false;
|
||||
}
|
||||
retract_direction_base_.normalize();
|
||||
retract_start_position_valid_ = true;
|
||||
}
|
||||
Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
|
||||
if (!retract_start_position_valid_ ||
|
||||
!readCurrentTouchPointPositionBase(current_position_base)) {
|
||||
@ -1678,23 +1651,14 @@ bool TouchScreenTask::stepRetracting() {
|
||||
|
||||
const Eigen::Vector3d delta_base =
|
||||
current_position_base - retract_start_position_base_;
|
||||
const double traveled_distance_m = delta_base.dot(retract_direction_base_);
|
||||
// Keep the forward peak: subsequent backward travel must not cancel it.
|
||||
// Reuse the existing measured-position sample, without delaying reversal.
|
||||
max_forward_after_retract_m_ =
|
||||
std::max(max_forward_after_retract_m_, -traveled_distance_m);
|
||||
const double traveled_distance_m = delta_base.norm();
|
||||
if (traveled_distance_m < retract_distance_m) {
|
||||
if (!arm_->busy()) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
}
|
||||
if (std::chrono::duration<double>(now - last_retract_log_time_).count() >= 0.2) {
|
||||
last_retract_log_time_ = now;
|
||||
const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{};
|
||||
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
|
||||
<< ", distance=" << traveled_distance_m
|
||||
<< "/" << retract_distance_m
|
||||
<< ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0
|
||||
<< ", cmd_base=[" << cmd_base.vx << ", "
|
||||
<< cmd_base.vy << ", " << cmd_base.vz << ", "
|
||||
<< cmd_base.wx << ", " << cmd_base.wy << ", "
|
||||
@ -1712,13 +1676,10 @@ bool TouchScreenTask::stepRetracting() {
|
||||
const bool have_final_delta = retract_start_position_valid_ && have_final_position;
|
||||
if (have_final_delta) {
|
||||
final_delta_base = final_position_base - retract_start_position_base_;
|
||||
max_forward_after_retract_m_ = std::max(max_forward_after_retract_m_,
|
||||
-final_delta_base.dot(retract_direction_base_));
|
||||
}
|
||||
if (have_final_delta) {
|
||||
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
|
||||
<< ", target_distance_m=" << retract_distance_m
|
||||
<< ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0
|
||||
<< ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
|
||||
<< ", final_tcp_delta_base=[" << final_delta_base.x() << ", "
|
||||
<< final_delta_base.y() << ", " << final_delta_base.z()
|
||||
@ -1726,7 +1687,6 @@ bool TouchScreenTask::stepRetracting() {
|
||||
} else {
|
||||
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
|
||||
<< ", target_distance_m=" << retract_distance_m
|
||||
<< ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0
|
||||
<< ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
|
||||
<< ", final_tcp_delta_base=unavailable";
|
||||
}
|
||||
@ -1898,12 +1858,6 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const {
|
||||
}
|
||||
|
||||
bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) {
|
||||
if (config_.touch().dwell_time_s() <= 0.0) {
|
||||
// Submit reversal in this tick, before any synchronous FK or logging.
|
||||
if (!startRetractPhase(Phase::DONE, Status::DONE)) return false;
|
||||
logTouchPressure(true);
|
||||
return true;
|
||||
}
|
||||
if (stop_forward_motion) {
|
||||
const auto result = arm_->stopL();
|
||||
if (!result.ok()) {
|
||||
@ -1945,12 +1899,9 @@ bool TouchScreenTask::startTouchPhase() {
|
||||
last_status_ = Status::ROBOT_STATE_FAILED;
|
||||
return false;
|
||||
}
|
||||
device::SpeedLOptions options;
|
||||
options.acceleration = speed_l.acceleration();
|
||||
if (speed_l.has_linear_jerk()) options.linear_jerk = speed_l.linear_jerk();
|
||||
const auto result = arm_->speedL(toCartesianVelocity(
|
||||
cmvr::common::math::toEigenVec6(speed_l.twist_tool())),
|
||||
options,
|
||||
speed_l.acceleration(),
|
||||
0.0,
|
||||
device::FrameType::Tool);
|
||||
if (!result.ok()) {
|
||||
@ -2006,17 +1957,24 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
|
||||
}
|
||||
|
||||
const auto& retract = config_.retract();
|
||||
retract_start_position_valid_ = false;
|
||||
retract_direction_base_.setZero();
|
||||
max_forward_after_retract_m_ = 0.0;
|
||||
retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_);
|
||||
if (!retract_start_position_valid_) {
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] cannot read TCP start pose";
|
||||
return false;
|
||||
}
|
||||
const auto retract_cmd = toCartesianVelocity(
|
||||
cmvr::common::math::toEigenVec6(retract.twist_tool()));
|
||||
device::SpeedLOptions options;
|
||||
options.acceleration = retract.acceleration();
|
||||
if (retract.has_linear_jerk()) options.linear_jerk = retract.linear_jerk();
|
||||
options.capture_reference = true;
|
||||
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
|
||||
<< retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
|
||||
<< ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
|
||||
<< "], acceleration=" << retract.acceleration()
|
||||
<< ", distance_m=" << retract.distance_m()
|
||||
<< ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
|
||||
<< retract_start_position_base_.y() << ", "
|
||||
<< retract_start_position_base_.z() << "]";
|
||||
|
||||
const auto result = arm_->speedL(retract_cmd,
|
||||
options,
|
||||
retract.acceleration(),
|
||||
0.0,
|
||||
device::FrameType::Tool);
|
||||
if (!result.ok()) {
|
||||
@ -2032,20 +1990,11 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
|
||||
last_retract_log_time_ = phase_start_time_;
|
||||
retract_command_started_ = true;
|
||||
last_status_ = Status::RETRACTING;
|
||||
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] submitted twist_tool=["
|
||||
<< retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
|
||||
<< "], acceleration=" << retract.acceleration()
|
||||
<< ", linear_jerk=" << (retract.has_linear_jerk() ? retract.linear_jerk() : 0.0)
|
||||
<< ", distance_m=" << retract.distance_m();
|
||||
return true;
|
||||
}
|
||||
|
||||
void TouchScreenTask::enterFailed(const Status status) {
|
||||
stopPbvsMotion();
|
||||
if (phase_ == Phase::RETRACTING && retract_start_position_valid_) {
|
||||
CMVR_LOG(WARNING) << "[TouchScreenTask][RETRACT_FAILED] status=" << statusToString(status)
|
||||
<< ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0;
|
||||
}
|
||||
phase_ = Phase::FAILED;
|
||||
setCoordinateOverlayEnabled(true);
|
||||
touch_command_started_ = false;
|
||||
@ -2073,10 +2022,9 @@ bool TouchScreenTask::updateTouchPressure() {
|
||||
double resultant_fz = 0.0;
|
||||
try {
|
||||
for (const auto& tactile_region : tactile_regions) {
|
||||
const auto resultant_force = dexhand_->getResultantForceNewtons(
|
||||
tactile_region.first, tactile_region.second);
|
||||
const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second);
|
||||
resultant_value += tactileForceValue(resultant_force, tactile.criterion());
|
||||
resultant_fz += resultant_force.fz;
|
||||
resultant_fz += static_cast<double>(resultant_force.fz);
|
||||
}
|
||||
} catch (...) {
|
||||
return false;
|
||||
@ -2095,9 +2043,9 @@ void TouchScreenTask::logTouchPressure(const bool force) {
|
||||
return;
|
||||
}
|
||||
last_touch_pressure_log_time_ = now;
|
||||
CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz_N=" << last_touch_resultant_fz_
|
||||
<< ", criterion_value_N=" << last_touch_pressure_sum_
|
||||
<< ", threshold_N=" << config_.touch().tactile().force_threshold()
|
||||
CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << last_touch_resultant_fz_
|
||||
<< ", criterion_value=" << last_touch_pressure_sum_
|
||||
<< ", threshold=" << config_.touch().tactile().force_threshold()
|
||||
<< ", triggered=" << isTouchTriggered(config_, last_touch_pressure_sum_);
|
||||
}
|
||||
|
||||
|
||||
@ -1,5 +1,4 @@
|
||||
#include "gtest/gtest.h"
|
||||
#include <google/protobuf/text_format.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
@ -307,45 +306,6 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TouchScreenTaskTest, TouchSpeedLJerkParsesFromTextConfig) {
|
||||
cmvr::config::TouchScreenTaskRootConfig parsed;
|
||||
ASSERT_TRUE(google::protobuf::TextFormat::ParseFromString(
|
||||
"touch_screen_task { touch { speed_l { acceleration: 5 linear_jerk: 10 } } }", &parsed));
|
||||
ASSERT_TRUE(parsed.touch_screen_task().touch().speed_l().has_linear_jerk());
|
||||
EXPECT_DOUBLE_EQ(parsed.touch_screen_task().touch().speed_l().linear_jerk(), 10);
|
||||
|
||||
const auto project_root = findProjectRoot();
|
||||
ASSERT_FALSE(project_root.empty());
|
||||
for (const auto* file_name : {"touch_screen_task.pb.txt", "touch_screen_task_mujoco.pb.txt"}) {
|
||||
cmvr::config::TouchScreenTaskRootConfig root;
|
||||
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
|
||||
(project_root / "cmvr-es/config/tasks/touch_screen_task" / file_name).string(), &root));
|
||||
cmvr::task::TouchScreenTask task(root.touch_screen_task());
|
||||
EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED)
|
||||
<< file_name;
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TouchScreenTaskTest, TouchSpeedLJerkIsOptionalAndMustBeFinitePositive) {
|
||||
auto config = loadMujocoTouchConfig(findProjectRoot());
|
||||
config.mutable_touch()->mutable_speed_l()->clear_linear_jerk();
|
||||
{
|
||||
cmvr::task::TouchScreenTask task(config);
|
||||
EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED);
|
||||
}
|
||||
for (const double jerk : {.5, 10.0, 60.0}) {
|
||||
config.mutable_touch()->mutable_speed_l()->set_linear_jerk(jerk);
|
||||
cmvr::task::TouchScreenTask task(config);
|
||||
EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED);
|
||||
}
|
||||
for (const double jerk : {0.0, -1.0, std::numeric_limits<double>::infinity(),
|
||||
std::numeric_limits<double>::quiet_NaN()}) {
|
||||
config.mutable_touch()->mutable_speed_l()->set_linear_jerk(jerk);
|
||||
cmvr::task::TouchScreenTask task(config);
|
||||
EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::INVALID_CONFIG);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TouchScreenTaskTest, CoordinateFrameProjectionUsesBgrAxisColors) {
|
||||
cv::Mat image = cv::Mat::zeros(240, 320, CV_8UC3);
|
||||
cmvr::device::Rs2Intrinsics intrinsics{};
|
||||
|
||||
@ -46,11 +46,6 @@ message VendorRobotArmBackendConfig {
|
||||
}
|
||||
|
||||
message SpeedLPlannerConfig {
|
||||
// Default true: preserve acceleration through same-axis velocity reversal.
|
||||
optional bool continuous_linear_reversal = 21;
|
||||
// Arm-level speedL limits. Command requests may lower, but not raise them.
|
||||
// Linear units: m/s, m/s^2, m/s^3. Angular units: rad/s, rad/s^2, rad/s^3.
|
||||
// Unset/nonpositive values use the planner defaults.
|
||||
double linear_velocity_max = 1;
|
||||
double linear_acceleration_max = 2;
|
||||
double linear_jerk_max = 3;
|
||||
|
||||
@ -36,9 +36,6 @@ message PX6AXGen3{
|
||||
string tactile_region = 16;
|
||||
string sensor_name = 17;
|
||||
PX6AXGen3PollingReadMode polling_read_mode = 18;
|
||||
// Maximum age since the sample's request was sent. 0 uses 50 ms.
|
||||
// Independent of response_timeout_ms: stale cached data must fail promptly.
|
||||
int32 max_sample_age_ms = 19;
|
||||
}
|
||||
|
||||
message ZeroSimTouchDexHand {
|
||||
|
||||
@ -97,9 +97,6 @@ message TouchScreenTouchSpeedLConfig {
|
||||
.cmvr.common.Vec6 twist_tool = 1;
|
||||
optional double acceleration = 2;
|
||||
optional double max_distance_m = 3;
|
||||
// Requested linear jerk during approach, m/s^3, capped by the arm's
|
||||
// linear_jerk_max. Unset uses that arm limit.
|
||||
optional double linear_jerk = 4;
|
||||
}
|
||||
|
||||
message TouchScreenTouchMoveLConfig {
|
||||
@ -115,8 +112,6 @@ message TouchScreenTactileTriggerConfig {
|
||||
optional TouchScreenFingerType finger = 1;
|
||||
optional TouchScreenTactileRegion region = 2;
|
||||
optional TouchScreenTactileCriterion criterion = 3;
|
||||
// Threshold in newtons (N), applied to the selected force criterion summed
|
||||
// over the requested tactile regions. Equality also triggers contact.
|
||||
optional double force_threshold = 4;
|
||||
}
|
||||
|
||||
@ -132,15 +127,11 @@ message TouchScreenTaskTouchConfig {
|
||||
|
||||
message TouchScreenTaskRetractConfig {
|
||||
.cmvr.common.Vec6 twist_tool = 1;
|
||||
// Requested acceleration, capped by the arm's speedL acceleration limits.
|
||||
optional double acceleration = 2;
|
||||
reserved 3;
|
||||
reserved "duration_s";
|
||||
// Signed displacement along the retract direction before stopping, in meters.
|
||||
// Distance traveled by the TCP before the retract motion stops, in meters.
|
||||
optional double distance_m = 4;
|
||||
// Requested linear jerk for braking and reversal, m/s^3, capped by the arm's
|
||||
// linear_jerk_max. Unset uses that arm limit.
|
||||
optional double linear_jerk = 5;
|
||||
}
|
||||
|
||||
message TouchScreenTaskConfig {
|
||||
|
||||
@ -1,12 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
# Real USB sensor test. The executable only sends force-read requests.
|
||||
# Example: ./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --duration-s 20 --mode sync
|
||||
repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}"
|
||||
cmake --build "$build_dir" --target px_6ax_gen3_real_test -j 4
|
||||
sensor_build_dir="$build_dir/cmvr-es/devices/dexhand/px_6ax_gen3"
|
||||
# Put build libraries ahead of installed ones so this tests the current driver/proto.
|
||||
exec env LD_LIBRARY_PATH="$build_dir:$sensor_build_dir:$build_dir/cmvr-es/hardware:$repo_root/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \
|
||||
"$sensor_build_dir/px_6ax_gen3_real_test" "$@"
|
||||
@ -1,22 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
# Headless MuJoCo only. Never initializes the physical robot.
|
||||
repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}"
|
||||
cmake --build "$build_dir" --target speedl_reversal_mujoco_test -j 4
|
||||
cd "$repo_root"
|
||||
python3 - "$build_dir" "$@" <<'PY'
|
||||
from pathlib import Path
|
||||
import os
|
||||
import sys
|
||||
|
||||
build = Path(sys.argv[1]).resolve()
|
||||
# Prefer every freshly built project library over output/lib's installed copy.
|
||||
paths = sorted({str(path.parent) for path in build.rglob('*.so')})
|
||||
paths.append(str(Path('output/lib').resolve()))
|
||||
env = os.environ.copy()
|
||||
env['LD_LIBRARY_PATH'] = ':'.join(paths + [env.get('LD_LIBRARY_PATH', '')])
|
||||
binary = build / 'cmvr-es/devices/arm/motor_robot_arm/speedl_reversal_mujoco_test'
|
||||
os.execve(str(binary), [str(binary), *sys.argv[2:]], env)
|
||||
PY
|
||||
Loading…
Reference in New Issue
Block a user