fix(touch): support repeated MuJoCo touch runs
This commit is contained in:
parent
e05a03e075
commit
724da3d000
@ -53,6 +53,8 @@ public:
|
||||
private:
|
||||
void ensureWorkerStarted_();
|
||||
void workerLoop_();
|
||||
void requestStop_(std::optional<double> acceleration = std::nullopt);
|
||||
void abortCommand_();
|
||||
void sendZero_();
|
||||
|
||||
static double velocityNorm_(const std::vector<double>& velocity);
|
||||
|
||||
@ -61,8 +61,13 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
|
||||
if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
|
||||
return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
|
||||
}
|
||||
if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
|
||||
return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
|
||||
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_();
|
||||
@ -103,15 +108,7 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
|
||||
if (!worker_ || !worker_->joinable()) {
|
||||
return Result::success();
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
target_twist_ = {};
|
||||
target_frame_ = FrameType::Base;
|
||||
target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
|
||||
command_active_ = true;
|
||||
++command_version_;
|
||||
}
|
||||
cv_.notify_all();
|
||||
requestStop_(acceleration);
|
||||
return Result::success();
|
||||
}
|
||||
|
||||
@ -192,27 +189,26 @@ void CartesianVelocityController::workerLoop_()
|
||||
}
|
||||
|
||||
if (!planner_->updateSpeedLAcceleration(acceleration)) {
|
||||
if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
command_active_ = false;
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
if (twistNorm_(target_twist) < config_.stop_twist_norm) {
|
||||
abortCommand_();
|
||||
break;
|
||||
}
|
||||
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
|
||||
<< acceleration;
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
return;
|
||||
requestStop_();
|
||||
continue;
|
||||
}
|
||||
|
||||
std::vector<double> q_now;
|
||||
std::vector<double> qd_now;
|
||||
if (!read_state_(q_now, qd_now)) {
|
||||
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
return;
|
||||
if (twistNorm_(target_twist) < config_.stop_twist_norm) {
|
||||
abortCommand_();
|
||||
break;
|
||||
}
|
||||
requestStop_();
|
||||
continue;
|
||||
}
|
||||
|
||||
std::vector<double> qd_cmd;
|
||||
@ -222,9 +218,12 @@ void CartesianVelocityController::workerLoop_()
|
||||
<< target_twist.vz << ", " << target_twist.wx << ", "
|
||||
<< target_twist.wy << ", " << target_twist.wz
|
||||
<< "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
return;
|
||||
if (twistNorm_(target_twist) < config_.stop_twist_norm) {
|
||||
abortCommand_();
|
||||
break;
|
||||
}
|
||||
requestStop_();
|
||||
continue;
|
||||
}
|
||||
|
||||
JointVelocityCommand velocity_command;
|
||||
@ -233,9 +232,12 @@ void CartesianVelocityController::workerLoop_()
|
||||
if (!send_result.ok()) {
|
||||
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
|
||||
<< send_result.message;
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
return;
|
||||
if (twistNorm_(target_twist) < config_.stop_twist_norm) {
|
||||
abortCommand_();
|
||||
break;
|
||||
}
|
||||
requestStop_();
|
||||
continue;
|
||||
}
|
||||
|
||||
if (twistNorm_(target_twist) < config_.stop_twist_norm &&
|
||||
@ -260,6 +262,32 @@ void CartesianVelocityController::workerLoop_()
|
||||
busy_.store(false);
|
||||
}
|
||||
|
||||
void CartesianVelocityController::requestStop_(const std::optional<double> acceleration)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
target_twist_ = {};
|
||||
target_frame_ = FrameType::Base;
|
||||
target_acceleration_ = acceleration.has_value() ? *acceleration
|
||||
: config_.stop_acceleration;
|
||||
command_active_ = true;
|
||||
++command_version_;
|
||||
}
|
||||
cv_.notify_all();
|
||||
}
|
||||
|
||||
void CartesianVelocityController::abortCommand_()
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
command_active_ = false;
|
||||
target_twist_ = {};
|
||||
target_frame_ = FrameType::Base;
|
||||
}
|
||||
sendZero_();
|
||||
busy_.store(false);
|
||||
}
|
||||
|
||||
void CartesianVelocityController::sendZero_()
|
||||
{
|
||||
if (!send_velocity_) {
|
||||
|
||||
@ -932,7 +932,11 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
|
||||
qdot = applyJointAccelerationLimits_(qdot, reference, dt);
|
||||
}
|
||||
const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
|
||||
if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
|
||||
// During a stop, the limiter intentionally commands a near-zero residual
|
||||
// twist while the measured arm can still be moving in a different direction.
|
||||
// Direction and speed-ratio checks are not meaningful for that transient.
|
||||
if (!is_stop_command &&
|
||||
!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
|
||||
achieved_twist_base,
|
||||
toEigenVector(q_measured),
|
||||
qdot)) {
|
||||
|
||||
@ -38,4 +38,9 @@ dexhand {
|
||||
auto_calibrate: false
|
||||
}
|
||||
}
|
||||
|
||||
dexhands {
|
||||
id: "mujoco_zero_touch_dexhand"
|
||||
zero_sim_touch {}
|
||||
}
|
||||
}
|
||||
|
||||
@ -8,35 +8,35 @@ device_manager {
|
||||
id: "mujoco_world"
|
||||
type: DEVICE_TYPE_MUJOCO_WORLD
|
||||
config_file: "devices/mujoco/mujoco_world.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_motors"
|
||||
type: DEVICE_TYPE_MOTOR_SYSTEM
|
||||
config_file: "devices/motor/mujoco_motors.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_right_arm"
|
||||
type: DEVICE_TYPE_ROBOT_ARM
|
||||
config_file: "devices/arm/arm_mujoco_qp.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_viewer"
|
||||
type: DEVICE_TYPE_MUJOCO_VIEWER
|
||||
config_file: "devices/mujoco/mujoco_viewer.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_hand_cam"
|
||||
type: DEVICE_TYPE_CAMERA
|
||||
config_file: "devices/camera/camera.pb.txt"
|
||||
enable: false
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
@ -68,6 +68,13 @@ device_manager {
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "mujoco_zero_touch_dexhand"
|
||||
type: DEVICE_TYPE_DEXHAND
|
||||
config_file: "devices/dexhand/dexhand.pb.txt"
|
||||
enable: true
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "ti5_motors"
|
||||
type: DEVICE_TYPE_MOTOR_SYSTEM
|
||||
@ -100,7 +107,7 @@ device_manager {
|
||||
id: "huayan_arm"
|
||||
type: DEVICE_TYPE_ROBOT_ARM
|
||||
config_file: "devices/arm/huayan_arm.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
|
||||
@ -4,8 +4,8 @@ task_manager {
|
||||
type: TASK_TYPE_TOUCH_SCREEN
|
||||
run_mode: TASK_RUN_MODE_PERIODIC_STEP
|
||||
control_period_s: 0.001
|
||||
config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
|
||||
enable: false
|
||||
config_file: "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt"
|
||||
enable: true
|
||||
}
|
||||
tasks {
|
||||
id: "grpc_server"
|
||||
@ -20,6 +20,6 @@ task_manager {
|
||||
run_mode: TASK_RUN_MODE_PERIODIC_STEP
|
||||
control_period_s: 0.002
|
||||
config_file: "tasks/self_collision_task/self_collision_task.pb.txt"
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
}
|
||||
|
||||
@ -2,9 +2,9 @@ touch_screen_task {
|
||||
id: "touch_screen"
|
||||
|
||||
devices {
|
||||
arm_id: "right_arm_mujoco"
|
||||
arm_id: "mujoco_right_arm"
|
||||
dexhand_id: "mujoco_zero_touch_dexhand"
|
||||
camera_id: "hand_cam"
|
||||
camera_id: "mujoco_hand_cam"
|
||||
}
|
||||
|
||||
initialization {
|
||||
@ -90,8 +90,8 @@ touch_screen_task {
|
||||
}
|
||||
|
||||
retract {
|
||||
twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 8.0
|
||||
duration_s: 5.0
|
||||
twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 5.0
|
||||
duration_s: 2.0
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
add_subdirectory(rh56dftp_dexhand)
|
||||
add_subdirectory(px_6ax_gen3)
|
||||
add_subdirectory(zero_sim_touch_dexhand)
|
||||
|
||||
add_library(dexhand INTERFACE)
|
||||
|
||||
@ -9,6 +10,7 @@ target_link_libraries(dexhand
|
||||
INTERFACE
|
||||
cmvr_es::device::rh56dftp_dexhand
|
||||
cmvr_es::device::px_6ax_gen3
|
||||
cmvr_es::device::zero_sim_touch_dexhand
|
||||
cmvr_es::proto
|
||||
)
|
||||
|
||||
|
||||
@ -10,6 +10,7 @@
|
||||
#include "devices/dexhand/abstract_dexhand.h"
|
||||
#include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h"
|
||||
#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
|
||||
#include "devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
@ -32,6 +33,10 @@ public:
|
||||
return std::make_shared<PX6AXGen3>(
|
||||
backendWithId_(cfg.id(), cfg.px_6ax_gen3()));
|
||||
|
||||
case config::DexHandDeviceConfig::kZeroSimTouch:
|
||||
return std::make_shared<ZeroSimTouchDexHand>(
|
||||
backendWithId_(cfg.id(), cfg.zero_sim_touch()));
|
||||
|
||||
case config::DexHandDeviceConfig::BACKEND_NOT_SET:
|
||||
default:
|
||||
{
|
||||
|
||||
@ -0,0 +1,13 @@
|
||||
add_library(zero_sim_touch_dexhand SHARED src/zero_sim_touch_dexhand.cpp)
|
||||
|
||||
target_include_directories(zero_sim_touch_dexhand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
add_library(cmvr_es::device::zero_sim_touch_dexhand ALIAS zero_sim_touch_dexhand)
|
||||
|
||||
target_link_libraries(zero_sim_touch_dexhand
|
||||
PRIVATE
|
||||
cmvr_es::proto
|
||||
glog
|
||||
)
|
||||
|
||||
install(TARGETS zero_sim_touch_dexhand LIBRARY DESTINATION lib)
|
||||
@ -0,0 +1,57 @@
|
||||
#ifndef CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
|
||||
#define CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
|
||||
|
||||
#include <array>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
|
||||
#include "../../abstract_dexhand.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
class ZeroSimTouchDexHand final : public AbstractDexHand {
|
||||
public:
|
||||
using FingerType = AbstractDexHand::FingerType;
|
||||
using ResultantForce = AbstractDexHand::ResultantForce;
|
||||
using TactilePoint = AbstractDexHand::TactilePoint;
|
||||
using TactileRegion = AbstractDexHand::TactileRegion;
|
||||
using TactileRegionKey = AbstractDexHand::TactileRegionKey;
|
||||
using TactileRegionData = AbstractDexHand::TactileRegionData;
|
||||
using Status = AbstractDexHand::Status;
|
||||
|
||||
explicit ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg);
|
||||
~ZeroSimTouchDexHand() override = default;
|
||||
|
||||
std::string typeName() const override { return "ZeroSimTouchDexHand"; }
|
||||
bool init() override;
|
||||
bool start() override;
|
||||
bool stop() override;
|
||||
|
||||
Status state() const override;
|
||||
std::string lastError() const override;
|
||||
void getState(DexHandState& state) override;
|
||||
|
||||
void setAngles(const std::vector<int>& finger_joint_angles) override;
|
||||
void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
|
||||
std::vector<TactileRegionData> getSensorData() override;
|
||||
TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
|
||||
ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
|
||||
|
||||
private:
|
||||
TactileRegionData makeRegionData(FingerType finger, TactileRegion region);
|
||||
|
||||
mutable std::mutex mutex_;
|
||||
std::vector<TactileRegionKey> polling_regions_;
|
||||
std::array<TactilePoint, 1> tactile_points_{};
|
||||
Status lifecycle_state_{Status::CREATED};
|
||||
std::string last_error_;
|
||||
};
|
||||
|
||||
// Keep the historical spelling available to callers that used the test double.
|
||||
using ZeroSImTouchDexHand = ZeroSimTouchDexHand;
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
#endif // CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
|
||||
@ -0,0 +1,96 @@
|
||||
#include "../include/zero_sim_touch_dexhand.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
ZeroSimTouchDexHand::ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg) {
|
||||
id_ = cfg.id();
|
||||
}
|
||||
|
||||
bool ZeroSimTouchDexHand::init() {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
lifecycle_state_ = Status::STREAMING;
|
||||
last_error_.clear();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ZeroSimTouchDexHand::start() {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
lifecycle_state_ = Status::STREAMING;
|
||||
last_error_.clear();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ZeroSimTouchDexHand::stop() {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
lifecycle_state_ = Status::STOPPED;
|
||||
return true;
|
||||
}
|
||||
|
||||
ZeroSimTouchDexHand::Status ZeroSimTouchDexHand::state() const {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return lifecycle_state_;
|
||||
}
|
||||
|
||||
std::string ZeroSimTouchDexHand::lastError() const {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return last_error_;
|
||||
}
|
||||
|
||||
void ZeroSimTouchDexHand::getState(DexHandState& state) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
state = DexHandState{};
|
||||
state.is_initialized = lifecycle_state_ == Status::INITIALIZED ||
|
||||
lifecycle_state_ == Status::STREAMING;
|
||||
if (!last_error_.empty()) {
|
||||
state.hands[0].error_message.push_back(last_error_);
|
||||
}
|
||||
}
|
||||
|
||||
void ZeroSimTouchDexHand::setAngles(const std::vector<int>&) {
|
||||
// The simulation has no finger actuators; commands are intentionally ignored.
|
||||
}
|
||||
|
||||
void ZeroSimTouchDexHand::setTactilePollingRegions(
|
||||
const std::vector<TactileRegionKey>& regions) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
polling_regions_ = regions;
|
||||
}
|
||||
|
||||
std::vector<ZeroSimTouchDexHand::TactileRegionData> ZeroSimTouchDexHand::getSensorData() {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (polling_regions_.empty()) {
|
||||
polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP});
|
||||
}
|
||||
|
||||
std::vector<TactileRegionData> data;
|
||||
data.reserve(polling_regions_.size());
|
||||
for (const auto& region : polling_regions_) {
|
||||
data.push_back(makeRegionData(region.first, region.second));
|
||||
}
|
||||
return data;
|
||||
}
|
||||
|
||||
ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::getSensorData(
|
||||
const FingerType finger, const TactileRegion region) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return makeRegionData(finger, region);
|
||||
}
|
||||
|
||||
ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce(
|
||||
const FingerType, const TactileRegion) {
|
||||
return TactilePoint::fromFz(0);
|
||||
}
|
||||
|
||||
ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData(
|
||||
const FingerType finger, const TactileRegion region) {
|
||||
tactile_points_[0] = TactilePoint::fromFz(0);
|
||||
TactileMatrixView view;
|
||||
view.data = tactile_points_.data();
|
||||
view.rows = 1;
|
||||
view.cols = 1;
|
||||
return {finger, region, view, "mujoco_touch_tip"};
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
@ -19,22 +19,22 @@ target_link_libraries(task
|
||||
add_library(cmvr_es::task ALIAS task)
|
||||
install(TARGETS task LIBRARY DESTINATION lib)
|
||||
|
||||
#add_executable(touch_screen_task_test
|
||||
# touch_screen_task/src/touch_screen_task_test.cpp
|
||||
#)
|
||||
#
|
||||
#target_link_libraries(touch_screen_task_test PRIVATE
|
||||
# cmvr_es::task
|
||||
# cmvr_es::device::arm
|
||||
# cmvr_es::device::motor_manager
|
||||
# cmvr_es::device::mujoco_motor_driver
|
||||
# cmvr_es::device::mujoco_camera
|
||||
# cmvr_es::mujoco_viewer
|
||||
# cmvr_es::proto
|
||||
# cmvr_es::device_manager
|
||||
# cmvr_es::service
|
||||
# gtest
|
||||
# gtest_main
|
||||
# pthread
|
||||
# glog
|
||||
#)
|
||||
add_executable(touch_screen_task_test
|
||||
touch_screen_task/src/touch_screen_task_test.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(touch_screen_task_test PRIVATE
|
||||
cmvr_es::task
|
||||
cmvr_es::device::arm
|
||||
cmvr_es::device::motor_manager
|
||||
cmvr_es::device::mujoco_motor_driver
|
||||
cmvr_es::device::mujoco_camera
|
||||
cmvr_es::mujoco_viewer
|
||||
cmvr_es::proto
|
||||
cmvr_es::device_manager
|
||||
cmvr_es::service
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
)
|
||||
|
||||
@ -385,6 +385,7 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
|
||||
bool TouchScreenTask::touch(const int u, const int v) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (isBusyUnlocked()) {
|
||||
last_status_ = Status::TASK_BUSY;
|
||||
return false;
|
||||
}
|
||||
return startFromPixelUnlocked(u, v);
|
||||
|
||||
@ -4,7 +4,6 @@
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <filesystem>
|
||||
#include <iostream>
|
||||
@ -28,10 +27,10 @@
|
||||
#include "devices/arm/robot_arm_factory.h"
|
||||
#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
|
||||
#include "devices/motor/manager/include/motor_manager.h"
|
||||
#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
#include "manager/task_manager/include/task_manager.h"
|
||||
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
|
||||
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
|
||||
|
||||
namespace {
|
||||
|
||||
@ -68,178 +67,6 @@ std::filesystem::path findProjectRoot()
|
||||
return search(std::filesystem::path(__FILE__).parent_path());
|
||||
}
|
||||
|
||||
class TouchMujocoViewer final : public cmvr::MuJocoViewer {
|
||||
public:
|
||||
TouchMujocoViewer(const std::string& model_path,
|
||||
std::shared_ptr<cmvr::device::MujocoJointBridge> bridge)
|
||||
: MuJocoViewer(model_path.c_str()), bridge_(std::move(bridge))
|
||||
{
|
||||
position_actuator_ids_.fill(-1);
|
||||
qpos_ids_.fill(-1);
|
||||
qvel_ids_.fill(-1);
|
||||
}
|
||||
|
||||
std::shared_ptr<cmvr::device::MujocoCamera> waitCameraReady(
|
||||
const std::chrono::milliseconds timeout)
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(camera_mutex_);
|
||||
if (!camera_cv_.wait_for(lock, timeout, [this] { return camera_ != nullptr; })) {
|
||||
return nullptr;
|
||||
}
|
||||
return camera_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void initOnce(mjModel* model, mjData* data) override
|
||||
{
|
||||
setupCamera(2.5, -160.0, -25.0);
|
||||
enablePiPCamera("hand_cam");
|
||||
|
||||
bool valid = true;
|
||||
for (std::size_t i = 0; i < kDof; ++i) {
|
||||
const std::string actuator_name = std::string(kJointNames[i]) + "_pos";
|
||||
position_actuator_ids_[i] = mj_name2id(model, mjOBJ_ACTUATOR, actuator_name.c_str());
|
||||
const int joint_id = mj_name2id(model, mjOBJ_JOINT, kJointNames[i]);
|
||||
if (position_actuator_ids_[i] < 0 || joint_id < 0) {
|
||||
valid = false;
|
||||
continue;
|
||||
}
|
||||
qpos_ids_[i] = model->jnt_qposadr[joint_id];
|
||||
qvel_ids_[i] = model->jnt_dofadr[joint_id];
|
||||
position_reference_[i] = data->qpos[qpos_ids_[i]];
|
||||
}
|
||||
|
||||
const int hand_cam_id = mj_name2id(model, mjOBJ_CAMERA, "hand_cam");
|
||||
if (hand_cam_id < 0) {
|
||||
valid = false;
|
||||
}
|
||||
|
||||
auto camera = std::make_shared<cmvr::device::MujocoCamera>(
|
||||
[this](std::vector<unsigned char>& rgb,
|
||||
std::vector<float>& depth,
|
||||
int& width,
|
||||
int& height,
|
||||
std::uint64_t& frame_id) {
|
||||
return getPiPCameraRGBD(rgb, depth, width, height, frame_id);
|
||||
});
|
||||
if (hand_cam_id >= 0) {
|
||||
camera->setFovyDeg(model->cam_fovy[hand_cam_id]);
|
||||
}
|
||||
camera->setConsumeNewFrameOnly(false);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(camera_mutex_);
|
||||
camera_ = std::move(camera);
|
||||
}
|
||||
camera_cv_.notify_all();
|
||||
bridge_->markReady(valid);
|
||||
}
|
||||
|
||||
void controlCallback(mjModel* model, mjData* data) override
|
||||
{
|
||||
std::vector<double> measured_position(kDof, 0.0);
|
||||
std::vector<double> measured_velocity(kDof, 0.0);
|
||||
for (std::size_t i = 0; i < kDof; ++i) {
|
||||
if (qpos_ids_[i] >= 0) {
|
||||
measured_position[i] = data->qpos[qpos_ids_[i]];
|
||||
}
|
||||
if (qvel_ids_[i] >= 0) {
|
||||
measured_velocity[i] = data->qvel[qvel_ids_[i]];
|
||||
}
|
||||
}
|
||||
bridge_->publishMeasured(measured_position, measured_velocity);
|
||||
|
||||
const auto commands = bridge_->commands();
|
||||
for (std::size_t i = 0; i < kDof; ++i) {
|
||||
const int actuator_id = position_actuator_ids_[i];
|
||||
if (actuator_id < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (commands.mode[i] == cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
|
||||
if (last_mode_[i] != cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY && qpos_ids_[i] >= 0) {
|
||||
position_reference_[i] = data->qpos[qpos_ids_[i]];
|
||||
}
|
||||
position_reference_[i] += commands.velocity[i] * model->opt.timestep;
|
||||
} else {
|
||||
position_reference_[i] = commands.position[i];
|
||||
}
|
||||
|
||||
const double lower = model->actuator_ctrlrange[2 * actuator_id];
|
||||
const double upper = model->actuator_ctrlrange[2 * actuator_id + 1];
|
||||
position_reference_[i] = std::clamp(position_reference_[i], lower, upper);
|
||||
data->ctrl[actuator_id] = position_reference_[i];
|
||||
last_mode_[i] = commands.mode[i];
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<cmvr::device::MujocoJointBridge> bridge_;
|
||||
std::array<int, kDof> position_actuator_ids_{};
|
||||
std::array<int, kDof> qpos_ids_{};
|
||||
std::array<int, kDof> qvel_ids_{};
|
||||
std::array<double, kDof> position_reference_{};
|
||||
std::array<cmvr::msgs::RunMode, kDof> last_mode_{};
|
||||
std::mutex camera_mutex_;
|
||||
std::condition_variable camera_cv_;
|
||||
std::shared_ptr<cmvr::device::MujocoCamera> camera_{nullptr};
|
||||
};
|
||||
|
||||
class ZeroTouchDexHand final : public cmvr::device::AbstractDexHand {
|
||||
public:
|
||||
ZeroTouchDexHand()
|
||||
{
|
||||
id_ = "mujoco_zero_touch_dexhand";
|
||||
}
|
||||
|
||||
std::string typeName() const override { return "ZeroTouchDexHand"; }
|
||||
Status state() const override { return Status::STREAMING; }
|
||||
std::string lastError() const override { return {}; }
|
||||
|
||||
void setAngles(const std::vector<int>& finger_joint_angles) override
|
||||
{
|
||||
(void)finger_joint_angles;
|
||||
}
|
||||
|
||||
void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override
|
||||
{
|
||||
polling_regions_ = regions;
|
||||
}
|
||||
|
||||
std::vector<TactileRegionData> getSensorData() override
|
||||
{
|
||||
std::vector<TactileRegionData> data;
|
||||
if (polling_regions_.empty()) {
|
||||
polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP});
|
||||
}
|
||||
data.reserve(polling_regions_.size());
|
||||
for (const auto& region : polling_regions_) {
|
||||
data.push_back(getSensorData(region.first, region.second));
|
||||
}
|
||||
return data;
|
||||
}
|
||||
|
||||
TactileRegionData getSensorData(const FingerType finger, const TactileRegion region) override
|
||||
{
|
||||
tactile_points_[0] = getResultantForce(finger, region);
|
||||
TactileMatrixView view;
|
||||
view.data = tactile_points_.data();
|
||||
view.rows = 1;
|
||||
view.cols = 1;
|
||||
return {finger, region, view, "mujoco_touch_tip"};
|
||||
}
|
||||
|
||||
ResultantForce getResultantForce(const FingerType finger, const TactileRegion region) override
|
||||
{
|
||||
(void)finger;
|
||||
(void)region;
|
||||
return TactilePoint::fromFz(0);
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<TactileRegionKey> polling_regions_;
|
||||
std::array<TactilePoint, 1> tactile_points_{};
|
||||
};
|
||||
|
||||
struct TagCenterPixel {
|
||||
int tag_id{-1};
|
||||
int u{0};
|
||||
@ -459,16 +286,26 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
|
||||
cmvr::config::DeviceManagerConfig device_manager_config;
|
||||
device_manager_config.set_name("touch_screen_mujoco_test");
|
||||
device_manager_config.set_version("test");
|
||||
auto* world_entry = device_manager_config.add_devices();
|
||||
world_entry->set_id("mujoco_world");
|
||||
world_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD);
|
||||
world_entry->set_config_file("devices/mujoco/mujoco_world.pb.txt");
|
||||
world_entry->set_enable(true);
|
||||
auto* motor_entry = device_manager_config.add_devices();
|
||||
motor_entry->set_id("mujoco_motors");
|
||||
motor_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
|
||||
motor_entry->set_config_file("devices/motor/mujoco_motors.pb.txt");
|
||||
motor_entry->set_enable(true);
|
||||
auto* arm_entry = device_manager_config.add_devices();
|
||||
arm_entry->set_id("right_arm_mujoco");
|
||||
arm_entry->set_id("mujoco_right_arm");
|
||||
arm_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM);
|
||||
arm_entry->set_config_file("devices/arm/arm_mujoco.pb.txt");
|
||||
arm_entry->set_enable(true);
|
||||
auto* dexhand_entry = device_manager_config.add_devices();
|
||||
dexhand_entry->set_id("mujoco_zero_touch_dexhand");
|
||||
dexhand_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND);
|
||||
dexhand_entry->set_config_file("devices/dexhand/dexhand.pb.txt");
|
||||
dexhand_entry->set_enable(true);
|
||||
|
||||
auto& device_manager = cmvr::device::DeviceManager::getInstance(device_manager_config);
|
||||
auto motor_system = device_manager.getDevice<cmvr::device::MotorManager>("mujoco_motors");
|
||||
@ -476,19 +313,48 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: mujoco_motors";
|
||||
return;
|
||||
}
|
||||
auto bridge = cmvr::device::MotorManager::mujocoBridgeFor("mujoco_motors");
|
||||
if (!bridge) {
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco bridge not found: mujoco_motors";
|
||||
auto world = cmvr::device::MotorManager::mujocoWorldFor("mujoco_motors");
|
||||
if (!world || !world->isLoaded() || !world->isRunning()) {
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco world is not ready: mujoco_world";
|
||||
return;
|
||||
}
|
||||
auto arm = device_manager.getDevice<cmvr::device::RobotArm>("right_arm_mujoco");
|
||||
auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
|
||||
if (!arm) {
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: right_arm_mujoco";
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: mujoco_right_arm";
|
||||
return;
|
||||
}
|
||||
// The task config still uses the historical ID; keep it as a test-only alias.
|
||||
device_manager.registerDevice("right_arm_mujoco", arm);
|
||||
|
||||
TouchMujocoViewer viewer(
|
||||
(project_root / "model/xiaoyan_description/dual_arm.xml").string(), bridge);
|
||||
cmvr::MuJocoViewer viewer(world);
|
||||
viewer.setupCamera(2.5, -160.0, -25.0);
|
||||
viewer.enablePiPCamera("hand_cam");
|
||||
|
||||
auto camera = std::make_shared<cmvr::device::MujocoCamera>(
|
||||
[&viewer](std::vector<unsigned char>& rgb,
|
||||
std::vector<float>& depth,
|
||||
int& width,
|
||||
int& height,
|
||||
std::uint64_t& frame_id) {
|
||||
return viewer.getPiPCameraRGBD(rgb, depth, width, height, frame_id);
|
||||
});
|
||||
camera->setConsumeNewFrameOnly(true);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(world->mutex());
|
||||
const auto* model = world->model();
|
||||
const int hand_cam_id = model == nullptr
|
||||
? -1
|
||||
: mj_name2id(model, mjOBJ_CAMERA, "hand_cam");
|
||||
if (hand_cam_id < 0) {
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MuJoCo camera not found: hand_cam";
|
||||
return;
|
||||
}
|
||||
camera->setFovyDeg(model->cam_fovy[hand_cam_id]);
|
||||
}
|
||||
if (!camera->init()) {
|
||||
CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Failed to initialize MuJoCo camera";
|
||||
return;
|
||||
}
|
||||
|
||||
struct Outcome {
|
||||
bool init_ok{false};
|
||||
@ -506,17 +372,8 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
|
||||
|
||||
std::thread scenario([&] {
|
||||
try {
|
||||
if (!bridge->waitUntilReady(std::chrono::seconds(10))) {
|
||||
throw std::runtime_error("MuJoCo right-arm joints, actuators, or hand_cam are not ready");
|
||||
}
|
||||
auto camera = viewer.waitCameraReady(std::chrono::seconds(10));
|
||||
if (!camera) {
|
||||
throw std::runtime_error("MuJoCo camera is not ready");
|
||||
}
|
||||
|
||||
const auto touch_config = loadMujocoTouchConfig(project_root);
|
||||
device_manager.registerDevice(touch_config.devices().camera_id(), camera);
|
||||
device_manager.registerDevice(std::make_shared<ZeroTouchDexHand>());
|
||||
|
||||
cmvr::config::TaskManagerConfig task_manager_config;
|
||||
auto* task_entry = task_manager_config.add_tasks();
|
||||
@ -533,6 +390,7 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
|
||||
throw std::runtime_error("TouchScreenTask not found: " + touch_config.id());
|
||||
}
|
||||
outcome.init_ok = task->lastStatus() != cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED;
|
||||
|
||||
task_manager.startRunTask();
|
||||
|
||||
TagCenterPixel target_pixel;
|
||||
@ -548,10 +406,31 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
|
||||
<< ", v=" << target_pixel.v
|
||||
<< std::endl;
|
||||
|
||||
std::cout << "[TouchScreenTaskMujocoTest] before task->touch, phase="
|
||||
<< cmvr::task::TouchScreenTask::phaseToString(task->phase())
|
||||
<< ", status="
|
||||
<< cmvr::task::TouchScreenTask::statusToString(task->lastStatus())
|
||||
<< std::endl;
|
||||
const auto touch_start = std::chrono::steady_clock::now();
|
||||
outcome.touch_ok = task->touch(target_pixel.u, target_pixel.v);
|
||||
std::cout << "[TouchScreenTaskMujocoTest] after task->touch, ok="
|
||||
<< (outcome.touch_ok ? 1 : 0)
|
||||
<< ", elapsed_ms="
|
||||
<< std::chrono::duration<double, std::milli>(
|
||||
std::chrono::steady_clock::now() - touch_start)
|
||||
.count()
|
||||
<< ", phase="
|
||||
<< cmvr::task::TouchScreenTask::phaseToString(task->phase())
|
||||
<< ", status="
|
||||
<< cmvr::task::TouchScreenTask::statusToString(task->lastStatus())
|
||||
<< std::endl;
|
||||
if (!outcome.touch_ok) {
|
||||
outcome.final_status = task->lastStatus();
|
||||
return;
|
||||
task_manager.stopRunTask();
|
||||
viewer.requestStop();
|
||||
throw std::runtime_error(
|
||||
"task->touch failed, status=" +
|
||||
std::string(cmvr::task::TouchScreenTask::statusToString(outcome.final_status)));
|
||||
}
|
||||
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(20);
|
||||
@ -604,6 +483,7 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
|
||||
cmvr::task::TaskManager::getInstance().stopRunTask();
|
||||
} catch (...) {
|
||||
}
|
||||
viewer.requestStop();
|
||||
outcome.worker_error = error.what();
|
||||
std::cerr << "[TouchScreenTaskMujocoTest] scenario error: " << outcome.worker_error
|
||||
<< "\nClose the MuJoCo viewer window to finish gtest." << std::endl;
|
||||
|
||||
@ -38,6 +38,10 @@ message PX6AXGen3{
|
||||
PX6AXGen3PollingReadMode polling_read_mode = 18;
|
||||
}
|
||||
|
||||
message ZeroSimTouchDexHand {
|
||||
string id = 1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
message DexHandDeviceConfig {
|
||||
@ -47,6 +51,7 @@ message DexHandDeviceConfig {
|
||||
oneof backend {
|
||||
RH56DFTPDexHandConfig rh56dftp = 10;
|
||||
PX6AXGen3 px_6ax_gen3 = 11;
|
||||
ZeroSimTouchDexHand zero_sim_touch = 12;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user