136 lines
5.0 KiB
C++
136 lines
5.0 KiB
C++
/**
|
|
* @file hand_forcetest.cpp
|
|
* @brief Read BOTH the actual angle (0x0A) and the force sensor (0x2E) of the
|
|
* right hand while it holds open, to see whether pushing a finger by hand
|
|
* registers as a FORCE change even though the position can't move
|
|
* (non-backdrivable). Robust re-query read (flush + resend + scan).
|
|
*
|
|
* g++ -std=c++17 -Iinclude example/hand_forcetest.cpp -o /tmp/hft
|
|
* /tmp/hft /dev/ttyUSB1 (right hand; needs the port free)
|
|
*/
|
|
#include "inspire.h"
|
|
#include <cstdio>
|
|
#include <cstdint>
|
|
#include <memory>
|
|
#include <vector>
|
|
#include <unistd.h>
|
|
|
|
// Robust read of a 6-value register (angleAct 0x0A or forceAct 0x2E).
|
|
static int robustRead(SerialPort &sp, uint8_t addr, uint8_t id,
|
|
Eigen::Matrix<double, 6, 1> &out, double scale, bool sign)
|
|
{
|
|
std::vector<uint8_t> q = {0xEB, 0x90, id, 0x04, 0x11, addr, 0x06, 0x0C, 0x00};
|
|
uint8_t s = 0;
|
|
for (size_t i = 2; i < q.size() - 1; i++) s += q[i];
|
|
q.back() = s;
|
|
|
|
uint8_t buf[64];
|
|
for (int att = 0; att < 8; att++)
|
|
{
|
|
sp.flush();
|
|
sp.send(q.data(), q.size());
|
|
usleep(8000);
|
|
size_t n = sp.recv(buf, sizeof(buf));
|
|
for (size_t off = 0; off + 20 <= n; off++)
|
|
{
|
|
if (!((buf[off] == 0x90 && buf[off + 1] == 0xEB) ||
|
|
(buf[off] == 0xEB && buf[off + 1] == 0x90))) continue;
|
|
if (buf[off + 4] != 0x11 || buf[off + 5] != addr) continue;
|
|
uint8_t cs = 0;
|
|
for (int i = 2; i < 19; i++) cs += buf[off + i];
|
|
if (buf[off + 19] != cs) continue;
|
|
const uint8_t *b = buf + off;
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
uint16_t raw = b[7 + 2 * i] | (b[8 + 2 * i] << 8);
|
|
out(i) = (sign ? (double)(int16_t)raw : (double)raw) * scale;
|
|
}
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
const char *pr = argc > 1 ? argv[1] : "/dev/ttyUSB1";
|
|
auto sp = std::make_shared<SerialPort>(pr, B115200, 6);
|
|
inspire::InspireHand right(sp, 1);
|
|
right.ClearError();
|
|
right.SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
|
|
right.SetForce(500, 500, 500, 500, 500, 500);
|
|
|
|
Eigen::Matrix<double, 6, 1> qopen, pos, force;
|
|
for (int i = 0; i < 6; i++) qopen(i) = 1.0;
|
|
|
|
// DIAGNOSTIC: mimic inspire_g1's tight loop (SetPos, settle, GetPos, GetForce,
|
|
// no inter-cycle sleep) and count how often each read fails + whether force
|
|
// actually changes. This reproduces the "force frozen" symptom.
|
|
if (argc > 2 && std::string(argv[2]) == "diag")
|
|
{
|
|
int posFail = 0, forceFail = 0, N = 200;
|
|
double fmin[6], fmax[6];
|
|
for (int i = 0; i < 6; i++) { fmin[i] = 1e9; fmax[i] = -1e9; }
|
|
for (int k = 0; k < N; k++)
|
|
{
|
|
right.SetPosition(qopen);
|
|
usleep(3000);
|
|
if (robustRead(*sp, 0x0A, 1, pos, 1.0 / 1000.0, false) != 0) posFail++;
|
|
if (robustRead(*sp, 0x2E, 1, force, 1.0, true) != 0) forceFail++;
|
|
else for (int i = 0; i < 6; i++) { if (force(i) < fmin[i]) fmin[i] = force(i); if (force(i) > fmax[i]) fmax[i] = force(i); }
|
|
}
|
|
printf("tight loop N=%d: GetPosition fails=%d GetForce fails=%d\n", N, posFail, forceFail);
|
|
printf("force range over run (g): ");
|
|
for (int i = 0; i < 6; i++) printf("[%.0f..%.0f] ", fmin[i], fmax[i]);
|
|
printf("\n");
|
|
return 0;
|
|
}
|
|
|
|
const char *NM[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
|
|
const double THRESH = 100.0; // grams of deviation from baseline = a deliberate push
|
|
|
|
// Drive open + hold, and calibrate each finger's resting-force baseline (~2s).
|
|
printf("Calibrating resting baseline — DON'T touch the fingers for 2s...\n");
|
|
double base[6] = {0, 0, 0, 0, 0, 0};
|
|
int nb = 0;
|
|
for (int k = 0; k < 10; k++)
|
|
{
|
|
right.SetPosition(qopen);
|
|
usleep(3000);
|
|
if (robustRead(*sp, 0x2E, 1, force, 1.0, true) == 0)
|
|
{
|
|
for (int i = 0; i < 6; i++) base[i] += force(i);
|
|
nb++;
|
|
}
|
|
usleep(150000);
|
|
}
|
|
for (int i = 0; i < 6; i++) base[i] = nb ? base[i] / nb : 0;
|
|
printf("Baseline(g): ");
|
|
for (int i = 0; i < 6; i++) printf("%s=%.0f ", NM[i], base[i]);
|
|
printf("\n\nNow PUSH one right finger at a time — it flags which finger and how hard:\n\n");
|
|
|
|
for (int k = 0; k < 120; k++)
|
|
{
|
|
right.SetPosition(qopen);
|
|
usleep(3000);
|
|
int rf = robustRead(*sp, 0x2E, 1, force, 1.0, true);
|
|
if (rf == 0)
|
|
{
|
|
char line[256]; int p = 0; bool pushed = false;
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
double dev = force(i) - base[i];
|
|
if (dev > THRESH || dev < -THRESH)
|
|
{
|
|
pushed = true;
|
|
p += sprintf(line + p, "%s:%+.0fg ", NM[i], dev);
|
|
}
|
|
}
|
|
if (pushed) printf("k%3d PUSH -> %s\n", k, line);
|
|
}
|
|
usleep(120000);
|
|
}
|
|
printf("(done)\n");
|
|
return 0;
|
|
}
|