SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(moveL): use trajectory velocity and reduce planning overhead

Browse Source
This commit is contained in:
lgv 2026-07-01 10:17:42 +08:00
parent 5e53e017a5
commit 94ee690796
7 changed files with 296 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h
View File

@ -88,6 +88,12 @@ public:
Eigen::MatrixXd& jacobian_base,
Eigen::Matrix3d& base_R_ee);
bool computePoseAndJacobianBaseAtQ(const std::vector<double>& q_chain,
bool is_tcp,
Eigen::Matrix4d& pose_base,
Eigen::MatrixXd& jacobian_base,
Eigen::Matrix3d& base_R_ee);
bool computeTwistBaseAtQ(const std::vector<double>& q_chain,
const std::vector<double>& qdot_chain,
bool is_tcp,

21
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp
View File

@ -438,12 +438,21 @@ bool PinocchioDlsIKSolver::solveVelocityBase(const Eigen::MatrixXd& jacobian_bas
Eigen::VectorXd qdot = jacobian_base.transpose() * ldlt.solve(target_twist_base);
const Eigen::Map<const Eigen::VectorXd> q_chain(q_chain_std.data(), chain_q_dof_);
const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain);
if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
const Eigen::Matrix<double,6,6> A_inv =
ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
const Eigen::MatrixXd J_pinv = jacobian_base.transpose() * A_inv;
qdot += projectToNullspace(J_pinv, jacobian_base, qdot_avoid);
const auto& avoidance = joint_limit_policy_.avoidance();
if (!jointLimitsDisabled() &&
avoidance.enable() &&
avoidance.gain() > 0.0 &&
chain_v_dof_ == chain_q_dof_ &&
q_chain.size() == chain_q_dof_ &&
joint_pos_lower_limits_.size() == chain_q_dof_ &&
joint_pos_upper_limits_.size() == chain_q_dof_) {
const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain);
if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
const Eigen::Matrix<double,6,6> A_inv =
ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
const Eigen::MatrixXd J_pinv = jacobian_base.transpose() * A_inv;
qdot += projectToNullspace(J_pinv, jacobian_base, qdot_avoid);
}
}
qdot = cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max);

46
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp
View File

@ -265,6 +265,52 @@ bool PinocchioIKBase::computeJacobianBaseAtQ(const std::vector<double>& q_chain,
nullptr);
}
bool PinocchioIKBase::computePoseAndJacobianBaseAtQ(
const std::vector<double>& q_chain_std,
const bool is_tcp,
Eigen::Matrix4d& pose_base,
Eigen::MatrixXd& jacobian_base,
Eigen::Matrix3d& base_R_ee)
{
if (!data_) {
CMVR_LOG(ERROR) << "[PinocchioIKBase] computePoseAndJacobianBaseAtQ called before pinocchio init";
return false;
}
if (static_cast<int>(q_chain_std.size()) != chain_q_dof_) {
CMVR_LOG(ERROR) << "[PinocchioIKBase] computePoseAndJacobianBaseAtQ: q size mismatch";
return false;
}
const Eigen::Map<const Eigen::VectorXd> q_chain(q_chain_std.data(), chain_q_dof_);
Eigen::VectorXd q_full;
if (!buildFullQFromChain(q_chain, q_full, "computePoseAndJacobianBaseAtQ")) {
return false;
}
updateKinematics(q_full);
const pinocchio::FrameIndex ee_id = (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_;
const pinocchio::SE3& oM_base = getBasePoseWorld();
const pinocchio::SE3& oM_ee = data_->oMf[ee_id];
const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
pose_base = se3ToMatrix4(base_M_ee);
base_R_ee = base_M_ee.rotation();
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_world(6, model_.nv);
pinocchio::computeFrameJacobian(model_, *data_, q_full,
ee_id,
pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
jacobian_world);
jacobian_base = extractChainJacobian(jacobian_world);
const Eigen::Matrix3d R_bo = oM_base.rotation().transpose();
jacobian_base.topRows(3) = R_bo * jacobian_base.topRows(3);
jacobian_base.bottomRows(3) = R_bo * jacobian_base.bottomRows(3);
return true;
}
bool PinocchioIKBase::computeMeasuredTwistBase(const std::vector<double>& q_chain,
const std::vector<double>& qdot_chain,
const pinocchio::FrameIndex ee_id,

241
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp
View File

@ -1,6 +1,7 @@
#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <Eigen/Geometry>
#include <limits>
@ -27,10 +28,10 @@ using cmvr::device::cartesian_motion::toStdVector;
namespace {
double lineDirectionDeviationDeg(const Eigen::Vector3d& expected,
const Eigen::Vector3d& actual)
double elapsedMs(const std::chrono::steady_clock::time_point start,
const std::chrono::steady_clock::time_point end)
{
return directionDeviationDeg(expected, actual);
return std::chrono::duration<double, std::milli>(end - start).count();
}
} // namespace
@ -152,6 +153,70 @@ bool PinocchioCartesianMotionPlanner::planMoveL(const CartesianPose& target,
const FrameType frame,
CartesianJointTrajectory& trajectory)
{
const auto plan_start_time = std::chrono::steady_clock::now();
double validation_ms = 0.0;
double current_pose_jacobian_ms = 0.0;
double solve_velocity_ms = 0.0;
double next_pose_jacobian_ms = 0.0;
std::size_t loop_count = 0;
struct PlanMoveLTimingLog {
std::chrono::steady_clock::time_point start_time;
double& validation_ms;
double& current_pose_jacobian_ms;
double& solve_velocity_ms;
double& next_pose_jacobian_ms;
std::size_t& loop_count;
CartesianJointTrajectory& trajectory;
~PlanMoveLTimingLog()
{
const auto end_time = std::chrono::steady_clock::now();
const double total_ms = elapsedMs(start_time, end_time);
const double kinematics_ms = current_pose_jacobian_ms + next_pose_jacobian_ms;
const double measured_planning_ms = kinematics_ms + solve_velocity_ms;
const double planning_ms = std::max(0.0, total_ms - validation_ms);
const double misc_ms = std::max(0.0, planning_ms - measured_planning_ms);
const double loop_count_d = static_cast<double>(loop_count);
const double per_loop_ms = loop_count_d > 0.0 ? planning_ms / loop_count_d : 0.0;
std::ostringstream oss;
oss << "[PinocchioCartesianMotionPlanner][moveL][plan] timing: total_ms="
<< total_ms
<< ", planning_ms=" << planning_ms
<< ", validation_ms=" << validation_ms
<< ", kinematics_ms=" << kinematics_ms
<< ", ik_ms=" << solve_velocity_ms
<< ", misc_ms=" << misc_ms
<< ", loops=" << loop_count
<< ", per_loop_ms=" << per_loop_ms
<< ", points=" << trajectory.position.size()
<< ", planned_path_m=" << trajectory.planned_path_length
<< ", executable_path_m=" << trajectory.executable_path_length
<< ", truncated=" << trajectory.truncated;
if (trajectory.truncated && !trajectory.truncation_reason.empty()) {
oss << ", reason=" << trajectory.truncation_reason;
}
CMVR_LOG(INFO) << oss.str();
}
} timing_log{plan_start_time,
validation_ms,
current_pose_jacobian_ms,
solve_velocity_ms,
next_pose_jacobian_ms,
loop_count,
trajectory};
auto measure_validation = [&validation_ms](auto&& validator) {
const auto start_time = std::chrono::steady_clock::now();
const bool result = validator();
validation_ms += elapsedMs(start_time, std::chrono::steady_clock::now());
return result;
};
auto measure_planning = [](double& accumulator_ms, auto&& operation) {
const auto start_time = std::chrono::steady_clock::now();
const bool result = operation();
accumulator_ms += elapsedMs(start_time, std::chrono::steady_clock::now());
return result;
};
trajectory = {};
const double dt = positiveOr(movel_config_.sample_period_s(), 0.001);
if (!solver_ || q_start.empty() ||
@ -199,6 +264,11 @@ bool PinocchioCartesianMotionPlanner::planMoveL(const CartesianPose& target,
if (profile.total_time <= 0.0) {
return false;
}
const auto reserve_count =
static_cast<std::size_t>(std::ceil(profile.total_time / dt)) + 2U;
trajectory.position.reserve(reserve_count);
trajectory.velocity.reserve(reserve_count);
trajectory.time.reserve(reserve_count);
Eigen::VectorXd q_current = toEigenVector(q_start);
Eigen::Vector3d linear_direction = Eigen::Vector3d::Zero();
@ -213,11 +283,28 @@ bool PinocchioCartesianMotionPlanner::planMoveL(const CartesianPose& target,
bool line_direction_warned = false;
double executable_path_length = 0.0;
Eigen::VectorXd prev_qdot = Eigen::VectorXd::Zero(q_current.size());
Eigen::Matrix4d cached_current_pose_base = Eigen::Matrix4d::Identity();
Eigen::MatrixXd cached_current_jacobian_base;
Eigen::Matrix3d cached_current_base_R_tool = Eigen::Matrix3d::Identity();
bool cached_current_pose_valid = false;
std::vector<double> q_std(q_start.size(), 0.0);
std::vector<double> q_next_std(q_start.size(), 0.0);
std::vector<double> qdot_std(q_start.size(), 0.0);
Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity();
Eigen::MatrixXd jacobian_base;
Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
Eigen::Matrix4d next_pose_base = Eigen::Matrix4d::Identity();
Eigen::Matrix3d next_base_R_tool = Eigen::Matrix3d::Identity();
auto copyEigenToStdVector = [](const Eigen::VectorXd& src, std::vector<double>& dst) {
dst.resize(static_cast<std::size_t>(src.size()));
Eigen::Map<Eigen::VectorXd>(dst.data(), src.size()) = src;
};
double previous_time = 0.0;
for (double t = std::min(dt, profile.total_time);
t <= profile.total_time + 1e-9;
t = std::min(t + dt, profile.total_time)) {
++loop_count;
const double step_dt = std::max(1e-6, t - previous_time);
previous_time = t;
@ -225,22 +312,35 @@ bool PinocchioCartesianMotionPlanner::planMoveL(const CartesianPose& target,
const double sd = std::max(0.0, curve.getVelocityAtTime(profile, t));
const double ratio = clamp(s / path_length, 0.0, 1.0);
const std::vector<double> q_std = toStdVector(q_current);
Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity();
if (!solver_->fk(q_std, current_pose_base, true)) {
return false;
copyEigenToStdVector(q_current, q_std);
if (cached_current_pose_valid) {
current_pose_base = cached_current_pose_base;
jacobian_base.swap(cached_current_jacobian_base);
base_R_tool = cached_current_base_R_tool;
} else {
if (!measure_planning(current_pose_jacobian_ms, [&]() {
return solver_->computePoseAndJacobianBaseAtQ(q_std,
true,
current_pose_base,
jacobian_base,
base_R_tool);
})) {
return false;
}
}
const Eigen::Vector3d p_current = current_pose_base.block<3, 1>(0, 3);
const Eigen::Vector3d p_desired = p_start + ratio * dp;
std::string stop_reason;
if (!checkMoveLPlanLineDeviation_(p_start,
linear_direction,
p_current,
(p_current - p_start).norm(),
line_deviation_warned,
line_direction_warned,
stop_reason)) {
if (!measure_validation([&]() {
return checkMoveLPlanLineDeviation_(p_start,
linear_direction,
p_current,
(p_current - p_start).norm(),
line_deviation_warned,
line_direction_warned,
stop_reason);
})) {
trajectory.truncated = true;
trajectory.truncation_reason = std::move(stop_reason);
trajectory.executable_path_length = executable_path_length;
@ -260,84 +360,94 @@ bool PinocchioCartesianMotionPlanner::planMoveL(const CartesianPose& target,
(total_rotation_vector / path_length) * sd + rotation_gain * rotation_error;
}
Eigen::MatrixXd jacobian_base;
Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) {
if (!measure_planning(solve_velocity_ms, [&]() {
return solver_->solveVelocityBase(jacobian_base,
target_twist_base,
q_std,
qdot_std,
std::numeric_limits<double>::infinity());
})) {
return false;
}
std::vector<double> qdot_std;
if (!solver_->solveVelocityBase(jacobian_base,
target_twist_base,
q_std,
qdot_std,
std::numeric_limits<double>::infinity())) {
return false;
}
Eigen::VectorXd qdot = toEigenVector(qdot_std);
Eigen::Map<const Eigen::VectorXd> qdot(qdot_std.data(),
static_cast<Eigen::Index>(qdot_std.size()));
Eigen::VectorXd qdot_limited = qdot;
if (!qd_max.empty()) {
double scale = 1.0;
for (Eigen::Index i = 0; i < qdot.size(); ++i) {
for (Eigen::Index i = 0; i < qdot_limited.size(); ++i) {
const double limit = std::abs(qd_max[static_cast<std::size_t>(i)]);
if (limit <= 0.0 || !std::isfinite(limit)) {
continue;
}
const double value = std::abs(qdot[i]);
const double value = std::abs(qdot_limited[i]);
if (value > limit) {
scale = std::min(scale, limit / value);
}
}
qdot *= scale;
qdot_limited *= scale;
}
const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
if (!checkMoveLPlanCartesianStepFeasibility_(target_twist_base,
achieved_twist_base,
s,
stop_reason)) {
const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot_limited;
if (!measure_validation([&]() {
return checkMoveLPlanCartesianStepFeasibility_(target_twist_base,
achieved_twist_base,
s,
stop_reason);
})) {
trajectory.truncated = true;
trajectory.truncation_reason = std::move(stop_reason);
trajectory.executable_path_length = executable_path_length;
break;
}
const Eigen::VectorXd q_next = q_current + qdot * step_dt;
if (!checkMoveLPlanJointContinuity_(q_current,
q_next,
qdot,
prev_qdot,
step_dt,
s,
stop_reason)) {
const Eigen::VectorXd q_next = q_current + qdot_limited * step_dt;
if (!measure_validation([&]() {
return checkMoveLPlanJointContinuity_(q_current,
q_next,
qdot_limited,
prev_qdot,
step_dt,
s,
stop_reason);
})) {
trajectory.truncated = true;
trajectory.truncation_reason = std::move(stop_reason);
trajectory.executable_path_length = executable_path_length;
break;
}
if (!checkMoveLPlanJointPositionLimits_(q_current,
q_next,
qdot,
step_dt,
s,
stop_reason)) {
if (!measure_validation([&]() {
return checkMoveLPlanJointPositionLimits_(q_current,
q_next,
qdot_limited,
step_dt,
s,
stop_reason);
})) {
trajectory.truncated = true;
trajectory.truncation_reason = std::move(stop_reason);
trajectory.executable_path_length = executable_path_length;
break;
}
const std::vector<double> q_next_std = toStdVector(q_next);
Eigen::Matrix4d next_pose_base = Eigen::Matrix4d::Identity();
if (!solver_->fk(q_next_std, next_pose_base, true)) {
copyEigenToStdVector(q_next, q_next_std);
if (!measure_planning(next_pose_jacobian_ms, [&]() {
return solver_->computePoseAndJacobianBaseAtQ(q_next_std,
true,
next_pose_base,
cached_current_jacobian_base,
next_base_R_tool);
})) {
return false;
}
const Eigen::Vector3d p_next = next_pose_base.block<3, 1>(0, 3);
if (!checkMoveLPlanLineDeviation_(p_start,
linear_direction,
p_next,
(p_next - p_start).norm(),
line_deviation_warned,
line_direction_warned,
stop_reason)) {
if (!measure_validation([&]() {
return checkMoveLPlanLineDeviation_(p_start,
linear_direction,
p_next,
(p_next - p_start).norm(),
line_deviation_warned,
line_direction_warned,
stop_reason);
})) {
trajectory.truncated = true;
trajectory.truncation_reason = std::move(stop_reason);
trajectory.executable_path_length = executable_path_length;
@ -345,10 +455,13 @@ bool PinocchioCartesianMotionPlanner::planMoveL(const CartesianPose& target,
}
q_current = q_next;
prev_qdot = qdot;
prev_qdot = qdot_limited;
cached_current_pose_base = next_pose_base;
cached_current_base_R_tool = next_base_R_tool;
cached_current_pose_valid = true;
trajectory.position.push_back(toStdVector(q_current));
trajectory.velocity.push_back(toStdVector(qdot));
trajectory.velocity.push_back(toStdVector(qdot_limited));
trajectory.time.push_back(t);
executable_path_length = s;
trajectory.executable_path_length = executable_path_length;
@ -630,7 +743,7 @@ bool PinocchioCartesianMotionPlanner::checkMoveLPlanLineDeviation_(
}
const Eigen::Vector3d actual_direction = (p_current - p_start) / traveled;
const double deviation_deg = lineDirectionDeviationDeg(line_direction, actual_direction);
const double deviation_deg = directionDeviationDeg(line_direction, actual_direction);
const double stop_deg = positiveOr(config.line_direction_stop_deg(), 45.0);
if (deviation_deg >= stop_deg) {
CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] line direction deviation exceeds stop threshold: deviation_deg="
@ -685,7 +798,7 @@ bool PinocchioCartesianMotionPlanner::updateAndValidateSpeedLLineDeviation_(
if (!reset_line && speedl_line_direction_base_.squaredNorm() > 1e-12) {
const double reset_deg = positiveOr(config.line_direction_reset_deg(), 10.0);
const double command_change_deg =
lineDirectionDeviationDeg(speedl_line_direction_base_, command_direction);
directionDeviationDeg(speedl_line_direction_base_, command_direction);
reset_line = command_change_deg >= reset_deg;
}
@ -725,7 +838,7 @@ bool PinocchioCartesianMotionPlanner::updateAndValidateSpeedLLineDeviation_(
const Eigen::Vector3d actual_direction = delta / traveled;
const double deviation_deg =
lineDirectionDeviationDeg(speedl_line_direction_base_, actual_direction);
directionDeviationDeg(speedl_line_direction_base_, actual_direction);
const double stop_deg = positiveOr(config.line_direction_stop_deg(), 45.0);
if (deviation_deg >= stop_deg) {
CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] line direction deviation too large: deviation_deg="

2
cmvr-es/config/manager/device_manager.pb.txt
View File

@ -20,7 +20,7 @@ device_manager {
devices {
id: "mujoco_right_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm_mujoco.pb.txt"
config_file: "devices/arm/arm_mujoco_qp.pb.txt"
enable: true
}

30
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
View File

@ -706,22 +706,42 @@ bool MotorRobotArm::configureAlgorithms_()
bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory)
{
if (trajectory.position.empty() || trajectory.time.size() != trajectory.position.size()) {
if (trajectory.position.empty() ||
trajectory.velocity.size() != trajectory.position.size() ||
trajectory.time.size() != trajectory.position.size()) {
return false;
}
if (trajectory.position.size() == 1) {
return true;
}
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(joint_names_.size());
{
std::lock_guard<std::mutex> lock(mutex_);
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
return false;
}
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
}
motors.push_back(std::move(motor));
}
}
auto next_deadline = std::chrono::steady_clock::now();
for (std::size_t i = 1; i < trajectory.position.size(); ++i) {
const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]);
JointPositionCommand joint_cmd;
joint_cmd.position = trajectory.position[i];
const auto result = servoJ(joint_cmd);
if (!result.ok()) {
const auto& position = trajectory.position[i];
const auto& velocity = trajectory.velocity[i];
if (position.size() != motors.size() || velocity.size() != motors.size()) {
return false;
}
for (std::size_t j = 0; j < motors.size(); ++j) {
motors[j]->setTarget(position[j], velocity[j]);
}
next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(dt_segment));
std::this_thread::sleep_until(next_deadline);

52
cmvr-es/test/mujoco_manual_ui_test.cpp
View File

@ -81,39 +81,39 @@ ManualUiRunResult runManualUiTest(
return {};
}
// std::this_thread::sleep_for(std::chrono::seconds(1));
//
// auto move_l_target = arm->getTcpPose(cmvr::device::FrameType::Base);
// // move_l_target.z += 1.50;
// move_l_target.ry += 0.5;
//
// cmvr::device::MotionOptions cartesian_options;
// cartesian_options.velocity = 0.19;
// cartesian_options.acceleration = 100.0;
// cartesian_options.jerk = 500.0;
//
// CMVR_LOG(INFO) << "[MujocoManualUiTest] moveL +X";
// result = arm->moveL(move_l_target, cartesian_options, cmvr::device::FrameType::Base);
// if (!result.ok()) {
// return {false, result.message};
// }
//
// if (stop_requested.load()) {
// return {};
// }
std::this_thread::sleep_for(std::chrono::seconds(1));
cmvr::device::CartesianVelocity speed_l_velocity;
speed_l_velocity.vz = 0.19;
// speed_l_velocity.wy = 0.19;
auto move_l_target = arm->getTcpPose(cmvr::device::FrameType::Base);
move_l_target.z += 0.05;
// move_l_target.ry += 0.5;
CMVR_LOG(INFO) << "[MujocoManualUiTest] speedL +X";
result = arm->speedL(speed_l_velocity, 100.0, 1.0, cmvr::device::FrameType::Base);
cmvr::device::MotionOptions cartesian_options;
cartesian_options.velocity = 0.01;
cartesian_options.acceleration = 10.0;
cartesian_options.jerk = 50.0;
CMVR_LOG(INFO) << "[MujocoManualUiTest] moveL +X";
result = arm->moveL(move_l_target, cartesian_options, cmvr::device::FrameType::Base);
if (!result.ok()) {
return {false, result.message};
}
if (stop_requested.load()) {
return {};
}
// std::this_thread::sleep_for(std::chrono::seconds(1));
//
// cmvr::device::CartesianVelocity speed_l_velocity;
// speed_l_velocity.vz = 0.19;
// // speed_l_velocity.wy = 0.19;
//
// CMVR_LOG(INFO) << "[MujocoManualUiTest] speedL +X";
// result = arm->speedL(speed_l_velocity, 100.0, 1.0, cmvr::device::FrameType::Base);
// if (!result.ok()) {
// return {false, result.message};
// }
// auto last_pose = arm->getTcpPose(cmvr::device::FrameType::Base);
// auto last_time = std::chrono::steady_clock::now();
// auto read_sim_time = [&]() {