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

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

@@ -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,

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

@@ -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);
+    const auto& avoidance = joint_limit_policy_.avoidance();
-    if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
+    if (!jointLimitsDisabled() &&
-        const Eigen::Matrix<double,6,6> A_inv =
+        avoidance.enable() &&
-            ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
+        avoidance.gain() > 0.0 &&
-        const Eigen::MatrixXd J_pinv = jacobian_base.transpose() * A_inv;
+        chain_v_dof_ == chain_q_dof_ &&
-        qdot += projectToNullspace(J_pinv, jacobian_base, qdot_avoid);
+        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);

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

@@ -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,

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

@@ -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,
+double elapsedMs(const std::chrono::steady_clock::time_point start,
-                                 const Eigen::Vector3d& actual)
+                 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);
+        copyEigenToStdVector(q_current, q_std);
-        Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity();
+        if (cached_current_pose_valid) {
-        if (!solver_->fk(q_std, current_pose_base, true)) {
+            current_pose_base = cached_current_pose_base;
-            return false;
+            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,
+        if (!measure_validation([&]() {
-                                          linear_direction,
+                return checkMoveLPlanLineDeviation_(p_start,
-                                          p_current,
+                                                    linear_direction,
-                                          (p_current - p_start).norm(),
+                                                    p_current,
-                                          line_deviation_warned,
+                                                    (p_current - p_start).norm(),
-                                          line_direction_warned,
+                                                    line_deviation_warned,
-                                          stop_reason)) {
+                                                    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;
+        if (!measure_planning(solve_velocity_ms, [&]() {
-        Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
+                return solver_->solveVelocityBase(jacobian_base,
-        if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) {
+                                                  target_twist_base,
+                                                  q_std,
+                                                  qdot_std,
+                                                  std::numeric_limits<double>::infinity());
+            })) {
             return false;
         }
 
-        std::vector<double> qdot_std;
+        Eigen::Map<const Eigen::VectorXd> qdot(qdot_std.data(),
-        if (!solver_->solveVelocityBase(jacobian_base,
+                                               static_cast<Eigen::Index>(qdot_std.size()));
-                                        target_twist_base,
+        Eigen::VectorXd qdot_limited = qdot;
-                                        q_std,
-                                        qdot_std,
-                                        std::numeric_limits<double>::infinity())) {
-            return false;
-        }
-
-        Eigen::VectorXd qdot = toEigenVector(qdot_std);
         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;
+        const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot_limited;
-        if (!checkMoveLPlanCartesianStepFeasibility_(target_twist_base,
+        if (!measure_validation([&]() {
-                                                     achieved_twist_base,
+                return checkMoveLPlanCartesianStepFeasibility_(target_twist_base,
-                                                     s,
+                                                               achieved_twist_base,
-                                                     stop_reason)) {
+                                                               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;
+        const Eigen::VectorXd q_next = q_current + qdot_limited * step_dt;
-        if (!checkMoveLPlanJointContinuity_(q_current,
+        if (!measure_validation([&]() {
-                                            q_next,
+                return checkMoveLPlanJointContinuity_(q_current,
-                                            qdot,
+                                                      q_next,
-                                            prev_qdot,
+                                                      qdot_limited,
-                                            step_dt,
+                                                      prev_qdot,
-                                            s,
+                                                      step_dt,
-                                            stop_reason)) {
+                                                      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,
+        if (!measure_validation([&]() {
-                                                q_next,
+                return checkMoveLPlanJointPositionLimits_(q_current,
-                                                qdot,
+                                                          q_next,
-                                                step_dt,
+                                                          qdot_limited,
-                                                s,
+                                                          step_dt,
-                                                stop_reason)) {
+                                                          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);
+        copyEigenToStdVector(q_next, q_next_std);
-        Eigen::Matrix4d next_pose_base = Eigen::Matrix4d::Identity();
+        if (!measure_planning(next_pose_jacobian_ms, [&]() {
-        if (!solver_->fk(q_next_std, next_pose_base, true)) {
+                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,
+        if (!measure_validation([&]() {
-                                          linear_direction,
+                return checkMoveLPlanLineDeviation_(p_start,
-                                          p_next,
+                                                    linear_direction,
-                                          (p_next - p_start).norm(),
+                                                    p_next,
-                                          line_deviation_warned,
+                                                    (p_next - p_start).norm(),
-                                          line_direction_warned,
+                                                    line_deviation_warned,
-                                          stop_reason)) {
+                                                    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="

cmvr-es/config/manager/device_manager.pb.txt

@@ -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
   }
 

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp

@@ -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;
+        const auto& position = trajectory.position[i];
-        joint_cmd.position = trajectory.position[i];
+        const auto& velocity = trajectory.velocity[i];
-        const auto result = servoJ(joint_cmd);
+        if (position.size() != motors.size() || velocity.size() != motors.size()) {
-        if (!result.ok()) {
             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);

cmvr-es/test/mujoco_manual_ui_test.cpp

@@ -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;
+    auto move_l_target = arm->getTcpPose(cmvr::device::FrameType::Base);
-    speed_l_velocity.vz = 0.19;
+    move_l_target.z += 0.05;
-    // speed_l_velocity.wy = 0.19;
+    // move_l_target.ry += 0.5;
 
-    CMVR_LOG(INFO) << "[MujocoManualUiTest] speedL +X";
+    cmvr::device::MotionOptions cartesian_options;
-    result = arm->speedL(speed_l_velocity, 100.0, 1.0, cmvr::device::FrameType::Base);
+    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 = [&]() {