From 94ee690796770967bcba688d8f857c67a30aba6a Mon Sep 17 00:00:00 2001 From: lgv Date: Wed, 1 Jul 2026 10:17:42 +0800 Subject: [PATCH] fix(moveL): use trajectory velocity and reduce planning overhead --- .../pinocchio/include/pinocchio_ik_base.h | 6 + .../pinocchio/src/pinocchio_dls_ik_solver.cpp | 21 +- .../pinocchio/src/pinocchio_ik_base.cpp | 46 ++++ .../pinocchio_cartesian_motion_planner.cpp | 241 +++++++++++++----- cmvr-es/config/manager/device_manager.pb.txt | 2 +- .../motor_robot_arm/src/motor_robot_arm.cpp | 30 ++- cmvr-es/test/mujoco_manual_ui_test.cpp | 52 ++-- 7 files changed, 296 insertions(+), 102 deletions(-) diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h index b8c8c928..d9b81444 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h +++ b/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& q_chain, + bool is_tcp, + Eigen::Matrix4d& pose_base, + Eigen::MatrixXd& jacobian_base, + Eigen::Matrix3d& base_R_ee); + bool computeTwistBaseAtQ(const std::vector& q_chain, const std::vector& qdot_chain, bool is_tcp, diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp index 188801d2..bccc0236 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp +++ b/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 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 A_inv = - ldlt.solve(Eigen::Matrix::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 A_inv = + ldlt.solve(Eigen::Matrix::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); diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp index a15cbef6..823073a0 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp @@ -265,6 +265,52 @@ bool PinocchioIKBase::computeJacobianBaseAtQ(const std::vector& q_chain, nullptr); } +bool PinocchioIKBase::computePoseAndJacobianBaseAtQ( + const std::vector& 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(q_chain_std.size()) != chain_q_dof_) { + CMVR_LOG(ERROR) << "[PinocchioIKBase] computePoseAndJacobianBaseAtQ: q size mismatch"; + return false; + } + + const Eigen::Map 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 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& q_chain, const std::vector& qdot_chain, const pinocchio::FrameIndex ee_id, diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp index ab29231d..329c7b96 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp +++ b/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 +#include #include #include #include @@ -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(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(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::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 q_std(q_start.size(), 0.0); + std::vector q_next_std(q_start.size(), 0.0); + std::vector 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& dst) { + dst.resize(static_cast(src.size())); + Eigen::Map(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 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::infinity()); + })) { return false; } - std::vector qdot_std; - if (!solver_->solveVelocityBase(jacobian_base, - target_twist_base, - q_std, - qdot_std, - std::numeric_limits::infinity())) { - return false; - } - - Eigen::VectorXd qdot = toEigenVector(qdot_std); + Eigen::Map qdot(qdot_std.data(), + static_cast(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(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 achieved_twist_base = jacobian_base * qdot; - if (!checkMoveLPlanCartesianStepFeasibility_(target_twist_base, - achieved_twist_base, - s, - stop_reason)) { + const Eigen::Matrix 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 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=" diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 2672abfe..5a3ff6be 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/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 } diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 9e1fdae2..77e8abbc 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/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> motors; + motors.reserve(joint_names_.size()); + { + std::lock_guard 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::duration(dt_segment)); std::this_thread::sleep_until(next_deadline); diff --git a/cmvr-es/test/mujoco_manual_ui_test.cpp b/cmvr-es/test/mujoco_manual_ui_test.cpp index 1d4838b9..a0773538 100644 --- a/cmvr-es/test/mujoco_manual_ui_test.cpp +++ b/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; - 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 = [&]() {