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

add dexhand sensordata

Browse Source
This commit is contained in:
xtkuang 2026-09-30 10:14:20 +08:00
parent fbd67dab6a
commit 69b06d80c6
6 changed files with 253 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -203,21 +203,21 @@ device_manager {
id: "dual_arm_ethercat_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/dual_arm_ethercat_motors.pb.txt"
enable: true
enable: false
}
devices {
id: "right_ethercat_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/ethercat_dual_arm.pb.txt"
enable: true
enable: false
}
devices {
id: "left_ethercat_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/ethercat_dual_arm.pb.txt"
enable: true
enable: false
}
}

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

@ -5,7 +5,7 @@ task_manager {
run_mode: TASK_RUN_MODE_PERIODIC_STEP
control_period_s: 0.001
config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
enable: true
enable: false
}
tasks {
id: "grpc_server"

14
cmvr-es/devices/dexhand/oymotion_ap001/include/oymotion_ap001.h
View File

@ -2,8 +2,10 @@
#define CMVR_ES_OYMOTION_AP001_H
#include <array>
#include <atomic>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
@ -41,6 +43,9 @@ private:
static OYMotionAP001* fromSdkContext(void* context);
bool openSocket();
bool checkSdkResult(uint8_t result, uint8_t remote_error, const char* operation);
void startTactilePolling();
void stopTactilePolling();
void tactilePollingLoop();
void setFault(const std::string& message);
void setState(Status state);
std::array<uint16_t, MOTOR_COUNT> normalizedPositions(const std::vector<int>& values, int maximum) const;
@ -57,7 +62,16 @@ private:
std::string last_error_;
std::array<uint8_t, MOTOR_COUNT> speeds_{{255, 255, 255, 255, 255, 255}};
std::array<uint16_t, MOTOR_COUNT> current_positions_{};
// OHand SDK reports the physical joint angle in degrees * 100.
std::array<int16_t, MOTOR_COUNT> current_angles_{};
std::vector<TactileRegionKey> tactile_regions_;
static constexpr size_t INDEX_TACTILE_COUNT = 60;
std::atomic<bool> tactile_running_{false};
std::thread tactile_thread_;
mutable std::mutex tactile_mutex_;
std::array<TactilePoint, INDEX_TACTILE_COUNT> index_tactile_{};
bool index_tactile_valid_{false};
uint64_t index_tactile_sequence_{0};
};
} // namespace cmvr::device

245
cmvr-es/devices/dexhand/oymotion_ap001/src/oymotion_ap001.cpp
View File

@ -21,6 +21,43 @@ constexpr int kPositionInputMaximum = 2000;
constexpr int kAngleInputMaximum = 1000;
constexpr int kVelocityInputMaximum = 1000;
constexpr int kForceInputMaximum = 3000;
constexpr uint8_t kIndexTactileSensorId = 1;
constexpr int kIndexTactileRows = 12;
constexpr int kIndexTactileCols = 5;
constexpr auto kTactilePollingPeriod = std::chrono::microseconds(6667);
// cmvr-es exposes dex-hand joints as:
// pinky, ring, middle, index, thumb flexion, thumb rotation.
// OHand SDK uses:
// thumb flexion, index, middle, ring, pinky, thumb rotation.
constexpr std::array<size_t, OYMotionAP001::MOTOR_COUNT> kApiToSdkMotor{
4, 3, 2, 1, 0, 5};
template <typename T>
std::array<T, OYMotionAP001::MOTOR_COUNT> apiToSdkMotorOrder(
const std::array<T, OYMotionAP001::MOTOR_COUNT>& api_values) {
std::array<T, OYMotionAP001::MOTOR_COUNT> sdk_values{};
for (size_t api_index = 0; api_index < kApiToSdkMotor.size(); ++api_index) {
sdk_values[kApiToSdkMotor[api_index]] = api_values[api_index];
}
return sdk_values;
}
template <typename T>
std::array<T, OYMotionAP001::MOTOR_COUNT> sdkToApiMotorOrder(
const std::array<T, OYMotionAP001::MOTOR_COUNT>& sdk_values) {
std::array<T, OYMotionAP001::MOTOR_COUNT> api_values{};
for (size_t api_index = 0; api_index < kApiToSdkMotor.size(); ++api_index) {
api_values[api_index] = sdk_values[kApiToSdkMotor[api_index]];
}
return api_values;
}
struct TactileSnapshot {
std::array<AbstractDexHand::TactilePoint,
static_cast<size_t>(kIndexTactileRows * kIndexTactileCols)>
values{};
};
}
OYMotionAP001::OYMotionAP001(const config::OYMotionAP001Config& config) : config_(config) {
@ -89,11 +126,21 @@ bool OYMotionAP001::init() {
if (!checkSdkResult(HAND_GetProtocolVersion(sdk_context_, hand_id_, &major, &minor, &remote_error), remote_error, "HAND_GetProtocolVersion")) return false;
CMVR_LOG(INFO) << "[OYMotionAP001] connected: id=" << id_ << ", interface=" << can_interface_ << ", hand_id=" << static_cast<int>(hand_id_) << ", protocol=" << static_cast<int>(major) << "." << static_cast<int>(minor);
setState(Status::INITIALIZED);
// DeviceManager constructs and initializes devices during startup, but the
// application does not call DeviceManager::start(). Start the independent
// tactile sampler as soon as the AP001 connection is ready.
startTactilePolling();
return true;
}
bool OYMotionAP001::start() { if (!init()) return false; setState(Status::STREAMING); return true; }
bool OYMotionAP001::start() {
if (!init()) return false;
setState(Status::STREAMING);
startTactilePolling();
return true;
}
bool OYMotionAP001::stop() {
stopTactilePolling();
std::lock_guard<std::mutex> io_lock(io_mutex_);
if (sdk_context_) { HAND_FreeContext(sdk_context_); sdk_context_ = nullptr; }
if (socket_fd_ >= 0) { close(socket_fd_); socket_fd_ = -1; }
@ -108,13 +155,49 @@ void OYMotionAP001::getState(DexHandState& output) {
next.is_initialized = state() == Status::INITIALIZED || state() == Status::STREAMING;
std::lock_guard<std::mutex> io_lock(io_mutex_);
if (sdk_context_) {
std::array<uint16_t, MOTOR_COUNT> targets{}, positions{};
uint8_t count = 0, remote_error = 0;
if (HAND_GetFingerPosAll(sdk_context_, hand_id_, targets.data(), positions.data(), &count, &remote_error) == HAND_RESP_SUCCESS) current_positions_ = positions;
std::array<uint16_t, MOTOR_COUNT> target_positions{};
std::array<uint16_t, MOTOR_COUNT> current_positions{};
// The SDK treats motor_cnt as an in/out buffer-capacity parameter.
uint8_t position_count = static_cast<uint8_t>(MOTOR_COUNT);
uint8_t position_remote_error = 0;
const uint8_t position_result = HAND_GetFingerPosAll(
sdk_context_, hand_id_, target_positions.data(), current_positions.data(),
&position_count, &position_remote_error);
if (position_result != HAND_RESP_SUCCESS) {
CMVR_LOG(ERROR) << "[OYMotionAP001] HAND_GetFingerPosAll failed: result="
<< static_cast<int>(position_result)
<< ", remote_error=" << static_cast<int>(position_remote_error);
} else if (position_count != MOTOR_COUNT) {
CMVR_LOG(ERROR) << "[OYMotionAP001] HAND_GetFingerPosAll returned unexpected motor count: expected="
<< MOTOR_COUNT << ", actual=" << static_cast<int>(position_count);
} else {
current_positions_ = sdkToApiMotorOrder(current_positions);
}
std::array<int16_t, MOTOR_COUNT> target_angles{};
std::array<int16_t, MOTOR_COUNT> current_angles{};
// The SDK treats motor_cnt as an in/out buffer-capacity parameter.
uint8_t angle_count = static_cast<uint8_t>(MOTOR_COUNT);
uint8_t angle_remote_error = 0;
const uint8_t angle_result = HAND_GetFingerAngleAll(
sdk_context_, hand_id_, target_angles.data(), current_angles.data(),
&angle_count, &angle_remote_error);
if (angle_result != HAND_RESP_SUCCESS) {
CMVR_LOG(ERROR) << "[OYMotionAP001] HAND_GetFingerAngleAll failed: result="
<< static_cast<int>(angle_result)
<< ", remote_error=" << static_cast<int>(angle_remote_error);
} else if (angle_count != MOTOR_COUNT) {
CMVR_LOG(ERROR) << "[OYMotionAP001] HAND_GetFingerAngleAll returned unexpected motor count: expected="
<< MOTOR_COUNT << ", actual=" << static_cast<int>(angle_count);
} else {
current_angles_ = sdkToApiMotorOrder(current_angles);
}
}
for (size_t i = 0; i < MOTOR_COUNT; ++i) {
next.hands[i].position = static_cast<int>(current_positions_[i]) * kPositionInputMaximum / 65535;
next.hands[i].angle = static_cast<int>(current_positions_[i]) * kAngleInputMaximum / 65535;
next.hands[i].angle = current_angles_[i];
next.hands[i].speed = static_cast<int>(speeds_[i]) * kVelocityInputMaximum / 255;
}
const auto error = lastError();
@ -130,14 +213,20 @@ std::array<uint16_t, OYMotionAP001::MOTOR_COUNT> OYMotionAP001::normalizedPositi
}
void OYMotionAP001::setPositions(const std::vector<int>& values) {
auto positions = normalizedPositions(values, kPositionInputMaximum);
std::lock_guard<std::mutex> lock(io_mutex_); uint8_t remote_error = 0;
checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds_.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll");
auto positions = apiToSdkMotorOrder(
normalizedPositions(values, kPositionInputMaximum));
std::lock_guard<std::mutex> lock(io_mutex_);
auto speeds = apiToSdkMotorOrder(speeds_);
uint8_t remote_error = 0;
checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll");
}
void OYMotionAP001::setAngles(const std::vector<int>& values) {
auto positions = normalizedPositions(values, kAngleInputMaximum);
std::lock_guard<std::mutex> lock(io_mutex_); uint8_t remote_error = 0;
checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds_.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll(angle normalization)");
auto positions = apiToSdkMotorOrder(
normalizedPositions(values, kAngleInputMaximum));
std::lock_guard<std::mutex> lock(io_mutex_);
auto speeds = apiToSdkMotorOrder(speeds_);
uint8_t remote_error = 0;
checkSdkResult(HAND_SetFingerPosAll(sdk_context_, hand_id_, positions.data(), speeds.data(), MOTOR_COUNT, &remote_error), remote_error, "HAND_SetFingerPosAll(angle normalization)");
}
void OYMotionAP001::setVelocities(const std::vector<int>& values) {
if (values.size() != MOTOR_COUNT) throw std::invalid_argument("OYMotionAP001 expects exactly 6 velocities");
@ -147,14 +236,136 @@ void OYMotionAP001::setVelocities(const std::vector<int>& values) {
void OYMotionAP001::setForce(const std::vector<int>& values) {
if (values.size() != MOTOR_COUNT) throw std::invalid_argument("OYMotionAP001 expects exactly 6 force values");
std::lock_guard<std::mutex> lock(io_mutex_);
for (size_t i = 0; i < MOTOR_COUNT; ++i) { uint8_t remote_error = 0; if (!checkSdkResult(HAND_SetFingerForceTarget(sdk_context_, hand_id_, static_cast<uint8_t>(i), static_cast<uint16_t>(std::clamp(values[i], 0, kForceInputMaximum)), &remote_error), remote_error, "HAND_SetFingerForceTarget")) return; }
for (size_t api_index = 0; api_index < MOTOR_COUNT; ++api_index) {
uint8_t remote_error = 0;
if (!checkSdkResult(
HAND_SetFingerForceTarget(
sdk_context_, hand_id_,
static_cast<uint8_t>(kApiToSdkMotor[api_index]),
static_cast<uint16_t>(std::clamp(
values[api_index], 0, kForceInputMaximum)),
&remote_error),
remote_error, "HAND_SetFingerForceTarget")) {
return;
}
}
}
void OYMotionAP001::setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) { tactile_regions_ = regions; }
std::vector<AbstractDexHand::TactileRegionData> OYMotionAP001::getSensorData() { return {}; }
AbstractDexHand::TactileRegionData OYMotionAP001::getSensorData(FingerType, TactileRegion) { return {}; }
AbstractDexHand::ResultantForce OYMotionAP001::getResultantForce(FingerType, TactileRegion) { return {}; }
AbstractDexHand::ForceNewtons OYMotionAP001::getResultantForceNewtons(FingerType, TactileRegion) { return {}; }
void OYMotionAP001::setTactilePollingRegions(
const std::vector<TactileRegionKey>& regions) {
std::lock_guard<std::mutex> lock(io_mutex_);
tactile_regions_ = regions;
}
void OYMotionAP001::startTactilePolling() {
bool expected = false;
if (!tactile_running_.compare_exchange_strong(expected, true)) return;
tactile_thread_ = std::thread(&OYMotionAP001::tactilePollingLoop, this);
}
void OYMotionAP001::stopTactilePolling() {
tactile_running_.store(false);
if (tactile_thread_.joinable()) tactile_thread_.join();
}
void OYMotionAP001::tactilePollingLoop() {
auto next_poll = std::chrono::steady_clock::now();
auto stats_start = next_poll;
uint64_t successful_reads = 0;
uint64_t failed_reads = 0;
while (tactile_running_.load()) {
std::array<uint8_t, INDEX_TACTILE_COUNT> raw_force{};
uint8_t entry_count = static_cast<uint8_t>(raw_force.size());
uint8_t remote_error = 0;
uint8_t result = HAND_RESP_TIMEOUT;
{
std::lock_guard<std::mutex> lock(io_mutex_);
if (sdk_context_) {
result = HAND_GetFingerForce(
sdk_context_, hand_id_, kIndexTactileSensorId, &entry_count,
raw_force.data(), &remote_error);
}
}
if (result == HAND_RESP_SUCCESS && entry_count == raw_force.size()) {
std::lock_guard<std::mutex> lock(tactile_mutex_);
for (size_t point = 0; point < raw_force.size(); ++point) {
index_tactile_[point] = TactilePoint::fromFz(raw_force[point]);
}
index_tactile_valid_ = true;
++index_tactile_sequence_;
++successful_reads;
} else {
++failed_reads;
}
const auto now = std::chrono::steady_clock::now();
const auto stats_elapsed = now - stats_start;
if (stats_elapsed >= std::chrono::seconds(5)) {
const double seconds =
std::chrono::duration<double>(stats_elapsed).count();
CMVR_LOG(INFO) << "[OYMotionAP001] index tactile acquisition: rate="
<< successful_reads / seconds << " Hz, successful="
<< successful_reads << ", failed=" << failed_reads;
stats_start = now;
successful_reads = 0;
failed_reads = 0;
}
next_poll += kTactilePollingPeriod;
if (next_poll > now) {
std::this_thread::sleep_until(next_poll);
} else {
next_poll = now;
}
}
}
std::vector<AbstractDexHand::TactileRegionData> OYMotionAP001::getSensorData() {
auto snapshot = std::make_shared<TactileSnapshot>();
{
std::lock_guard<std::mutex> lock(tactile_mutex_);
if (!index_tactile_valid_) return {};
snapshot->values = index_tactile_;
}
return {TactileRegionData{
FingerType::INDEX, TactileRegion::PAD,
TactileMatrixView{snapshot->values.data(), kIndexTactileRows,
kIndexTactileCols},
"食指触觉", snapshot}};
}
AbstractDexHand::TactileRegionData OYMotionAP001::getSensorData(
FingerType finger, TactileRegion region) {
if (finger != FingerType::INDEX || region != TactileRegion::PAD) return {};
auto regions = getSensorData();
return regions.empty() ? TactileRegionData{} : regions.front();
}
AbstractDexHand::ResultantForce OYMotionAP001::getResultantForce(
FingerType finger, TactileRegion region) {
const auto tactile_data = getSensorData(finger, region);
if (!tactile_data.valid()) return {};
ResultantForce resultant{};
for (int row = 0; row < tactile_data.view.rows; ++row) {
const auto* values = tactile_data.view.rowData(row);
for (int col = 0; col < tactile_data.view.cols; ++col) {
resultant.fx += values[col].fx;
resultant.fy += values[col].fy;
resultant.fz += values[col].fz;
}
}
return resultant;
}
AbstractDexHand::ForceNewtons OYMotionAP001::getResultantForceNewtons(
FingerType, TactileRegion) {
throw std::runtime_error(
"OYMotionAP001 tactile values are raw sensor counts; no newton calibration is available.");
}
bool OYMotionAP001::checkSdkResult(uint8_t result, uint8_t remote_error, const char* operation) {
if (result == HAND_RESP_SUCCESS) return true;

1
cmvr-es/service/CMakeLists.txt
View File

@ -20,6 +20,7 @@ target_link_libraries(service PRIVATE
cmvr_es::proto
osqp
cmvr_es::device_manager
cmvr_es::device::oymotion_ap001
cmvr_es::task_manager
cmvr_es::algorithms::controller
cmvr_es::task

7
cmvr-es/service/grpc/src/grpc_dexhand_service.cpp
View File

@ -11,6 +11,7 @@
#include <thread>
#include <vector>
#include "devices/dexhand/oymotion_ap001/include/oymotion_ap001.h"
#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
#include "service/grpc/action/include/control_command_arbiter.h"
#include "service/grpc/action/include/control_command_guard.h"
@ -470,6 +471,10 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
}
maybeConfigureRh56FullTactilePolling(dev);
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id;
const auto stream_interval =
std::dynamic_pointer_cast<OYMotionAP001>(dev)
? std::chrono::microseconds(6667)
: std::chrono::microseconds(33000);
while (!context->IsCancelled())
{
@ -483,7 +488,7 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(33));
std::this_thread::sleep_for(stream_interval);
}
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): finished, id=" << dev_id;
return grpc::Status::OK;