227 lines
8.7 KiB
C++
227 lines
8.7 KiB
C++
//
|
||
// Created by xtkuang on 2025/5/8.
|
||
//
|
||
|
||
#ifndef CMVR_ES_ABSTRACT_DEXHAND_H
|
||
#define CMVR_ES_ABSTRACT_DEXHAND_H
|
||
#pragma once
|
||
|
||
#include "abstract_device.h"
|
||
|
||
namespace cmvr::device{
|
||
// 单个触觉点数据（16位无符号整数）
|
||
using TactilePoint = uint16_t;
|
||
|
||
// 手指触觉数据结构体（支持从数组任意位置解析）
|
||
struct FingerTactileData {
|
||
std::vector<std::vector<TactilePoint>> data; // 触觉数据二维数组
|
||
int rows; // 行数
|
||
int cols; // 列数
|
||
int byteSize; // 总字节数（rows * cols * 2字节/点）
|
||
std::string name; // 部位名称
|
||
|
||
// 初始化数据数组
|
||
void init() {
|
||
data.resize(rows, std::vector<TactilePoint>(cols, 0));
|
||
}
|
||
|
||
// 从 std::vector<uint16_t> 中赋值数据
|
||
void assignFromVector(const std::vector<uint16_t>& values, int startIndex = 0) {
|
||
int index = startIndex;
|
||
for (int i = 0; i < rows; ++i) {
|
||
for (int j = 0; j < cols; ++j) {
|
||
if (index < values.size()) {
|
||
data[i][j] = values[index];
|
||
index++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
};
|
||
|
||
// 手掌触觉数据结构体（支持从数组任意位置解析）
|
||
struct PalmTactileData {
|
||
std::vector<std::vector<TactilePoint>> data; // 触觉数据二维数组
|
||
int rows = 8; // 行数（掌心固定为8行）
|
||
int cols = 14; // 列数（掌心固定为14列）
|
||
int byteSize = 224; // 总字节数（8*14*2=224）
|
||
std::string name = "掌心"; // 部位名称
|
||
|
||
// 初始化数据数组
|
||
void init() {
|
||
data.resize(rows, std::vector<TactilePoint>(cols, 0));
|
||
}
|
||
|
||
// 从 std::vector<uint16_t> 中赋值数据
|
||
void assignFromVector(const std::vector<uint16_t>& values, int startIndex = 0) {
|
||
int index = startIndex;
|
||
for (int j = 0; j < cols; ++j) {
|
||
for (int i = rows - 1; i >= 0; --i) {
|
||
if (index < values.size()) {
|
||
data[i][j] = values[index];
|
||
index++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
};
|
||
|
||
// 手指枚举（方便按手指类型解析）
|
||
enum FingerType {
|
||
PINKY, // 小拇指
|
||
RING, // 无名指
|
||
MIDDLE, // 中指
|
||
INDEX, // 食指
|
||
THUMB // 大拇指
|
||
};
|
||
|
||
// 整只手的触觉传感器数据
|
||
struct HandTactileSensors {
|
||
// 五指的触觉数据（每根手指包含指端、指尖、指腹）
|
||
struct {
|
||
FingerTactileData tip; // 指端
|
||
FingerTactileData finger; // 指尖
|
||
FingerTactileData pad; // 指腹
|
||
} pinky, ring, middle, index; // 小拇指、无名指、中指、食指
|
||
|
||
// 大拇指（特殊：多一个指中）
|
||
struct {
|
||
FingerTactileData tip; // 指端
|
||
FingerTactileData finger; // 指尖
|
||
FingerTactileData middle; // 指中
|
||
FingerTactileData pad; // 指腹
|
||
} thumb;
|
||
|
||
// 掌心触觉数据
|
||
PalmTactileData palm;
|
||
|
||
// 初始化所有传感器数据（设置相对偏移量）
|
||
void init() {
|
||
// 小拇指（总370byte）
|
||
pinky.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "小拇指指端"};
|
||
pinky.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "小拇指指尖"};
|
||
pinky.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "小拇指指腹"};
|
||
|
||
// 无名指（总370byte）
|
||
ring.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "无名指指端"};
|
||
ring.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "无名指指尖"};
|
||
ring.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "无名指指腹"};
|
||
|
||
// 中指（总370byte）
|
||
middle.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "中指指端"};
|
||
middle.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "中指指尖"};
|
||
middle.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "中指指腹"};
|
||
|
||
// 食指（总370byte）
|
||
index.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "食指指端"};
|
||
index.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "食指指尖"};
|
||
index.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "食指指腹"};
|
||
|
||
// 大拇指（总420byte）
|
||
thumb.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指端"};
|
||
thumb.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指尖"};
|
||
thumb.middle = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指中"};
|
||
thumb.pad = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指指腹"};
|
||
|
||
// 掌心（总224byte）
|
||
palm = { .name = "掌心"};
|
||
|
||
// 初始化所有数据数组
|
||
pinky.tip.init();
|
||
pinky.finger.init();
|
||
pinky.pad.init();
|
||
|
||
ring.tip.init();
|
||
ring.finger.init();
|
||
ring.pad.init();
|
||
|
||
middle.tip.init();
|
||
middle.finger.init();
|
||
middle.pad.init();
|
||
|
||
index.tip.init();
|
||
index.finger.init();
|
||
index.pad.init();
|
||
|
||
thumb.tip.init();
|
||
thumb.finger.init();
|
||
thumb.middle.init();
|
||
thumb.pad.init();
|
||
|
||
palm.init();
|
||
}
|
||
// 按手指类型解析整个手指的数据
|
||
void parseFinger(FingerType fingerType, const std::vector<uint16_t>& values) {
|
||
switch (fingerType) {
|
||
case PINKY:
|
||
pinky.tip.assignFromVector(values);
|
||
pinky.finger.assignFromVector(values, 9);
|
||
pinky.pad.assignFromVector(values, 9 + 96);
|
||
break;
|
||
|
||
case RING:
|
||
ring.tip.assignFromVector(values);
|
||
ring.finger.assignFromVector(values, 9);
|
||
ring.pad.assignFromVector(values, 9 + 96);
|
||
break;
|
||
|
||
case MIDDLE:
|
||
middle.tip.assignFromVector(values);
|
||
middle.finger.assignFromVector(values, 9);
|
||
middle.pad.assignFromVector(values, 9 + 96);
|
||
break;
|
||
|
||
case INDEX:
|
||
index.tip.assignFromVector(values);
|
||
index.finger.assignFromVector(values, 9);
|
||
index.pad.assignFromVector(values, 9 + 96);
|
||
break;
|
||
|
||
case THUMB:
|
||
thumb.tip.assignFromVector(values);
|
||
thumb.finger.assignFromVector(values, 9);
|
||
thumb.middle.assignFromVector(values, 9 + 96);
|
||
thumb.pad.assignFromVector(values, 9 + 96 + 9);
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 解析手心（掌心）数据
|
||
void parsePalm(const std::vector<uint16_t>& values) {
|
||
palm.assignFromVector(values);
|
||
}
|
||
|
||
// 获取手指名称（用于调试）
|
||
std::string getFingerName(FingerType fingerType) {
|
||
switch (fingerType) {
|
||
case PINKY: return "小拇指";
|
||
case RING: return "无名指";
|
||
case MIDDLE: return "中指";
|
||
case INDEX: return "食指";
|
||
case THUMB: return "大拇指";
|
||
default: return "未知";
|
||
}
|
||
}
|
||
};
|
||
class AbstractDexHand: public AbstractDevice{
|
||
public:
|
||
explicit AbstractDexHand(const XmlNode& cfg): AbstractDevice(cfg) {}
|
||
~AbstractDexHand() override = default;
|
||
|
||
virtual void getState(DexHandState &state) {}
|
||
|
||
// control
|
||
virtual void setPositions(const std::vector<int>& finger_joint_targets) {}
|
||
virtual void setAngles(const std::vector<int>& finger_joint_angles) {}
|
||
virtual void setVelocities(const std::vector<int>& finger_joint_velocities) {}
|
||
virtual void setPresetAct(int action_id) {}
|
||
virtual void execPresetAct(int action_id) {}
|
||
virtual void setForce(const std::vector<int>& finger_joint_force) {}
|
||
virtual HandTactileSensors& getSensorData() { return hand_tactile_sensors_;}
|
||
protected:
|
||
DexHandState status_;
|
||
HandTactileSensors hand_tactile_sensors_;
|
||
};
|
||
}
|
||
|
||
#endif //CMVR_ES_ABSTRACT_DEXHAND_H
|