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
|