669 lines
28 KiB
C++
669 lines
28 KiB
C++
#include "inspire.h"
|
|
#include "param.h"
|
|
|
|
#include "dds/Publisher.h"
|
|
#include "dds/Subscription.h"
|
|
#include <unitree/idl/go2/MotorCmds_.hpp>
|
|
#include <unitree/idl/go2/MotorStates_.hpp>
|
|
#include <unitree/common/thread/recurrent_thread.hpp>
|
|
|
|
#include <dirent.h>
|
|
#include <fstream>
|
|
#include <algorithm>
|
|
#include <vector>
|
|
#include <cstdlib>
|
|
#include <cmath>
|
|
#include <chrono>
|
|
|
|
// One detected CH340 (1a86:7523) USB-RS485 hand adapter.
|
|
struct HandPort
|
|
{
|
|
std::string dev; // /dev/ttyUSBn — NOT stable across replug/reboot
|
|
std::string path; // USB topology path, e.g. "1-2.2.3" — stable per physical socket
|
|
};
|
|
|
|
// Find the two CH340 hand adapters and order them by PHYSICAL USB PATH.
|
|
//
|
|
// This used to sort by ttyUSB name and assign right = ports[1]. Both dongles are
|
|
// 1a86:7523 and report the SAME USB serial, so the tty number is the only thing telling
|
|
// them apart — and the kernel hands those out in enumeration order, which changes across
|
|
// replugs, reboots and hub hiccups. When it flips, left and right silently trade places:
|
|
// nothing errors, the dashboard's port check still says "ok", and you drive the wrong hand.
|
|
// Observed live on 2026-07-30 — software "L.*" was in fact the robot's RIGHT hand.
|
|
//
|
|
// The USB topology path IS stable as long as a dongle stays in the same socket, so we
|
|
// order by that instead. Override explicitly with INSPIRE_RIGHT_PATH / INSPIRE_LEFT_PATH
|
|
// (e.g. INSPIRE_RIGHT_PATH=1-2.2.3) when the sockets are known.
|
|
static std::vector<HandPort> findCH340Ports()
|
|
{
|
|
std::vector<std::string> ttys;
|
|
if (DIR *d = opendir("/sys/class/tty"))
|
|
{
|
|
for (struct dirent *e; (e = readdir(d));)
|
|
{
|
|
std::string n = e->d_name;
|
|
if (n.rfind("ttyUSB", 0) == 0) ttys.push_back(n);
|
|
}
|
|
closedir(d);
|
|
}
|
|
std::vector<HandPort> ports;
|
|
for (const auto &t : ttys)
|
|
{
|
|
// Resolve the device symlink, then walk UP to the USB device dir (first
|
|
// ancestor that has an idVendor file) and check for the CH340 id.
|
|
char real[4096];
|
|
std::string link = "/sys/class/tty/" + t + "/device";
|
|
if (!realpath(link.c_str(), real)) continue;
|
|
std::string dir = real;
|
|
while (dir.size() > 1)
|
|
{
|
|
std::ifstream vf(dir + "/idVendor");
|
|
if (vf.good())
|
|
{
|
|
std::string vid, pid; vf >> vid;
|
|
std::ifstream pf(dir + "/idProduct"); pf >> pid;
|
|
if (vid == "1a86" && pid == "7523")
|
|
{
|
|
// The USB device dir's basename is the topology path ("1-2.2.3").
|
|
auto s = dir.find_last_of('/');
|
|
ports.push_back({"/dev/" + t, s == std::string::npos ? dir : dir.substr(s + 1)});
|
|
}
|
|
break;
|
|
}
|
|
auto slash = dir.find_last_of('/');
|
|
if (slash == std::string::npos || slash == 0) break;
|
|
dir.resize(slash);
|
|
}
|
|
}
|
|
std::sort(ports.begin(), ports.end(),
|
|
[](const HandPort &a, const HandPort &b) { return a.path < b.path; });
|
|
return ports;
|
|
}
|
|
|
|
class InspireRunner
|
|
{
|
|
public:
|
|
/// One opened port plus the RS-485 id that actually answered on it (-1 = nothing answered).
|
|
struct Opened
|
|
{
|
|
std::shared_ptr<inspire::InspireHand> hand;
|
|
SerialPort::SharedPtr port;
|
|
int id = -1;
|
|
};
|
|
|
|
InspireRunner()
|
|
{
|
|
// Dynamically detect the two CH340 hand adapters (port-independent — survives
|
|
// being moved to a hub / different USB ports). A generic udev rule grants 0666.
|
|
auto ports = findCH340Ports();
|
|
if (ports.size() < 2)
|
|
{
|
|
std::cerr << "ERROR: found " << ports.size()
|
|
<< " CH340 hand adapter(s), need 2. Plug in both USB-RS485 dongles."
|
|
<< std::endl;
|
|
exit(1);
|
|
}
|
|
// --- Work out which physical hand is on which port -----------------------------------
|
|
//
|
|
// The RH56 has NO handedness register — it cannot tell you it is a left or a right unit
|
|
// (checked: only HAND_ID 1000 exists, which is just the RS-485 slave address). Both
|
|
// dongles are CH340 1a86:7523 with the same USB serial too, so there is nothing
|
|
// intrinsic to key on. We therefore try three things, best first:
|
|
//
|
|
// 1. RS-485 ID. If the hands have been given DIFFERENT ids (see example/hand_setid),
|
|
// that is a permanent, wiring-independent identity: id 1 = RIGHT, id 2 = LEFT.
|
|
// This is the real fix — immune to enumeration order, replugs and hub changes.
|
|
// 2. INSPIRE_RIGHT_PATH / INSPIRE_LEFT_PATH env — pin by physical USB socket.
|
|
// 3. USB topology path order — stable per socket, unlike the ttyUSB number that this
|
|
// used to sort by (which silently swapped left and right; observed 2026-07-30).
|
|
Opened A = openAndIdentify(ports[0].dev, ports[0].path.c_str());
|
|
Opened B = openAndIdentify(ports[1].dev, ports[1].path.c_str());
|
|
|
|
int ri = 1, li = 0; // default: USB-path order (ports[1] = right)
|
|
const char *how = "usb path order";
|
|
|
|
if (A.id > 0 && B.id > 0 && A.id != B.id)
|
|
{
|
|
// Distinct ids => unambiguous identity, whatever the ports did.
|
|
ri = (A.id == 1) ? 0 : 1;
|
|
li = 1 - ri;
|
|
how = "RS-485 id (id1=right, id2=left)";
|
|
}
|
|
else if (std::getenv("INSPIRE_RIGHT_PATH") || std::getenv("INSPIRE_LEFT_PATH"))
|
|
{
|
|
if (const char *rp = std::getenv("INSPIRE_RIGHT_PATH"))
|
|
for (size_t i = 0; i < ports.size(); i++)
|
|
if (ports[i].path == rp) { ri = (int)i; li = (int)(1 - i); }
|
|
if (const char *lp = std::getenv("INSPIRE_LEFT_PATH"))
|
|
for (size_t i = 0; i < ports.size(); i++)
|
|
if (ports[i].path == lp) { li = (int)i; ri = (int)(1 - i); }
|
|
how = "INSPIRE_*_PATH env";
|
|
}
|
|
else if (std::getenv("INSPIRE_SWAP"))
|
|
{
|
|
ri = 0; li = 1;
|
|
how = "usb path order + INSPIRE_SWAP";
|
|
}
|
|
|
|
Opened &R = (ri == 0) ? A : B;
|
|
Opened &L = (li == 0) ? A : B;
|
|
serial1 = R.port; righthand = R.hand;
|
|
serial2 = L.port; lefthand = L.hand;
|
|
|
|
std::cout << "Inspire hands identified by " << how << ":\n"
|
|
<< " RIGHT = " << ports[ri].dev << " (usb " << ports[ri].path
|
|
<< ", rs485 id " << R.id << ")\n"
|
|
<< " LEFT = " << ports[li].dev << " (usb " << ports[li].path
|
|
<< ", rs485 id " << L.id << ")" << std::endl;
|
|
if (!(A.id > 0 && B.id > 0 && A.id != B.id))
|
|
std::cout << " NOTE: both hands answer on the same RS-485 id, so identity is inferred\n"
|
|
" from wiring. To make it self-identifying and permanent, run once:\n"
|
|
" ./build/hand_setid <left-hand-port> 1 2\n"
|
|
" (gives the LEFT hand id 2; then left/right can never swap again)"
|
|
<< std::endl;
|
|
|
|
// Recover the actuators on startup: clear any latched fault (stall/overcurrent) and
|
|
// ensure a non-zero speed/force (e.g. after the bad serial state from two services
|
|
// fighting over the bus, which can leave fingers unresponsive).
|
|
for (auto h : {righthand, lefthand})
|
|
{
|
|
h->ClearError();
|
|
h->SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
|
|
h->SetForce(500, 500, 500, 500, 500, 500);
|
|
}
|
|
|
|
calibrateForceOffset();
|
|
|
|
// dds
|
|
handcmd = std::make_shared<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>>(
|
|
"rt/" + param::ns + "/cmd");
|
|
handcmd->msg_.cmds().resize(12);
|
|
handstate = std::make_unique<unitree::robot::RealTimePublisher<unitree_go::msg::dds_::MotorStates_>>(
|
|
"rt/" + param::ns + "/state");
|
|
handstate->msg_.states().resize(12);
|
|
|
|
// Start running.
|
|
// 20ms, not 10ms: a cycle is 2 position writes + 4 register reads over a 115200 half-
|
|
// duplex bus, which cannot finish in 10ms. Asking for 100Hz just made the thread run
|
|
// back-to-back with no idle, and the measured publish rate was 28Hz. 50Hz is honest and
|
|
// achievable, and leaves headroom so a retry doesn't push the whole loop late.
|
|
thread = std::make_shared<unitree::common::RecurrentThread>(
|
|
20000, std::bind(&InspireRunner::run, this)
|
|
);
|
|
}
|
|
|
|
/**
|
|
* @brief Learn each finger's resting force, and raise that finger's force limit past it.
|
|
*
|
|
* The RH56 stops driving a finger the instant its MEASURED force reaches the force limit.
|
|
* So a finger whose force-sensor zero has drifted above the limit can never be driven: it
|
|
* draws 0 current and reads exactly like a dead motor. On this G1's right hand, R.mid rests
|
|
* at ~856g and R.index at ~570g against a 500g limit — both were mis-diagnosed as dead
|
|
* actuators until they ran their full range once the limit was raised to 1000g.
|
|
*
|
|
* We measure the offset with the hand open and unloaded, then (a) subtract it from the
|
|
* published force so touch-detection works from a true zero, and (b) add it to that
|
|
* finger's limit so the gate still trips at the same REAL force.
|
|
*/
|
|
void calibrateForceOffset()
|
|
{
|
|
std::cout << "Calibrating finger force baseline (hand open, unloaded) ..." << std::endl;
|
|
Eigen::Matrix<double, 6, 1> open6;
|
|
open6.setOnes();
|
|
for (int k = 0; k < 10; k++)
|
|
{
|
|
righthand->SetPosition(open6);
|
|
lefthand->SetPosition(open6);
|
|
usleep(30000);
|
|
}
|
|
|
|
Eigen::Matrix<double, 12, 1> acc;
|
|
acc.setZero();
|
|
int n = 0;
|
|
Eigen::Matrix<double, 6, 1> ftmp;
|
|
for (int k = 0; k < 12; k++)
|
|
{
|
|
bool ok = true;
|
|
Eigen::Matrix<double, 12, 1> one;
|
|
if (righthand->GetForce(ftmp) == 0) one.block<6, 1>(0, 0) = ftmp; else ok = false;
|
|
if (lefthand->GetForce(ftmp) == 0) one.block<6, 1>(6, 0) = ftmp; else ok = false;
|
|
if (ok) { acc += one; n++; }
|
|
usleep(20000);
|
|
}
|
|
if (!n)
|
|
{
|
|
std::cerr << " WARNING: no force readings - skipping offset calibration. "
|
|
"Fingers with a drifted sensor zero will not move." << std::endl;
|
|
foff.setZero();
|
|
applyForce(reqForceR, 0);
|
|
applyForce(reqForceL, 6);
|
|
return;
|
|
}
|
|
foff = acc / n;
|
|
|
|
static const char *FN[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
|
|
for (int lo = 0; lo <= 6; lo += 6)
|
|
{
|
|
double req = (lo == 0) ? reqForceR : reqForceL;
|
|
bool flagged = false;
|
|
for (int i = lo; i < lo + 6; i++)
|
|
{
|
|
if (std::abs(foff(i)) > req - kForceMargin)
|
|
{
|
|
if (!flagged)
|
|
{
|
|
std::cout << " " << (lo == 0 ? "RIGHT" : "LEFT")
|
|
<< " hand force-sensor ZERO HAS DRIFTED:" << std::endl;
|
|
flagged = true;
|
|
}
|
|
std::cout << " " << FN[i - lo] << " rests at " << (int)foff(i)
|
|
<< "g vs a " << (int)req << "g limit -> would read as DEAD (0 current). "
|
|
<< "Raising its limit." << std::endl;
|
|
}
|
|
}
|
|
if (flagged)
|
|
std::cout << " (proper fix: re-zero the force sensor with the hand unloaded)"
|
|
<< std::endl;
|
|
applyForce(req, lo);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Open a hand's serial port and confirm it actually ANSWERS, reopening if it doesn't.
|
|
*
|
|
* Measured repeatedly on this robot: after a (re)start one of the two RS-485 buses comes up
|
|
* mute — every read for that hand fails for the whole session while the other is perfect,
|
|
* and WHICH one is random (5 restarts: R dead, L dead, both ok, R dead, L dead). The port
|
|
* always enumerates and open() always succeeds; the adapter just never replies, so nothing
|
|
* upstream notices. Closing and reopening clears it.
|
|
*
|
|
* Probing here costs a few hundred ms at startup and turns "half the robot is silently dead
|
|
* until someone notices" into "it retried and told you".
|
|
*/
|
|
/// SerialPort::Init calls exit(-1) if open() fails, so never hand it a path that is not
|
|
/// currently openable — that turns a transient into a service-wide crash.
|
|
static bool canOpen(const std::string &dev)
|
|
{
|
|
int fd = ::open(dev.c_str(), O_RDWR | O_NOCTTY);
|
|
if (fd < 0) return false;
|
|
::close(fd);
|
|
return true;
|
|
}
|
|
|
|
/// Re-resolve a hand's tty from its (stable) USB path. Reopening a CH340 repeatedly can
|
|
/// make the adapters re-enumerate, so a device name captured at startup can go stale
|
|
/// mid-probe — which is exactly how the earlier probe crash-looped the service.
|
|
static std::string resolveByPath(const std::string &usbpath, const std::string &fallback)
|
|
{
|
|
for (const auto &p : findCH340Ports())
|
|
if (p.path == usbpath) return p.dev;
|
|
return fallback;
|
|
}
|
|
|
|
Opened openAndIdentify(const std::string &dev_in, const char *usbpath)
|
|
{
|
|
Opened o;
|
|
std::string dev = dev_in;
|
|
// Two attempts, not four: each reopen churns the USB bus and can renumber BOTH adapters.
|
|
// One retry recovers a genuinely stalled link; more just destabilises the other hand.
|
|
for (int attempt = 1; attempt <= 2; attempt++)
|
|
{
|
|
dev = resolveByPath(usbpath, dev);
|
|
if (!canOpen(dev))
|
|
{
|
|
std::cerr << " " << dev << " (usb " << usbpath << ") cannot be opened right now"
|
|
<< std::endl;
|
|
usleep(300000);
|
|
dev = resolveByPath(usbpath, dev);
|
|
if (!canOpen(dev))
|
|
{
|
|
// Everything downstream dereferences righthand/lefthand unconditionally, so a
|
|
// null hand would segfault. Exit clearly instead and let the supervisor retry —
|
|
// this self-heals as soon as the adapter is back.
|
|
std::cerr << "FATAL: usb " << usbpath << " has no openable tty (" << dev << "). "
|
|
"Adapter unplugged or re-enumerating. Exiting; supervisor will retry."
|
|
<< std::endl;
|
|
exit(1);
|
|
}
|
|
}
|
|
o.port = std::make_shared<SerialPort>(dev, B115200, 6);
|
|
usleep(120000); // let the CH340 settle after open
|
|
|
|
// Scan the RS-485 ids. Normally only id 1 is used, but giving the two hands distinct
|
|
// ids makes them self-identifying, so look for any of them.
|
|
for (int id = 1; id <= 4 && !o.hand; id++)
|
|
{
|
|
auto h = std::make_shared<inspire::InspireHand>(o.port, (uint8_t)id);
|
|
Eigen::Matrix<double, 6, 1> q;
|
|
for (int t = 0; t < 2; t++)
|
|
if (h->GetPosition(q) == 0) { o.hand = h; o.id = id; break; }
|
|
}
|
|
if (o.hand)
|
|
{
|
|
std::cout << " " << dev << " (usb " << usbpath << ") answered at rs485 id " << o.id
|
|
<< (attempt > 1 ? " [after reopen #" + std::to_string(attempt) + "]" : "")
|
|
<< std::endl;
|
|
return o;
|
|
}
|
|
std::cerr << " " << dev << " (usb " << usbpath << ") is SILENT - reopening ("
|
|
<< attempt << "/2)" << std::endl;
|
|
o.port.reset(); // close before retrying
|
|
usleep(250000); // brief release; longer churns the bus
|
|
}
|
|
// Give the caller a usable object regardless, so one dead hand does not take the other
|
|
// down. The runtime watchdog keeps retrying and the warning fires once.
|
|
dev = resolveByPath(usbpath, dev);
|
|
if (!canOpen(dev))
|
|
{
|
|
std::cerr << "FATAL: usb " << usbpath << " has no openable tty (" << dev << ") after "
|
|
"probing. Exiting; supervisor will retry." << std::endl;
|
|
exit(1);
|
|
}
|
|
o.port = std::make_shared<SerialPort>(dev, B115200, 6);
|
|
o.hand = std::make_shared<inspire::InspireHand>(o.port, 1);
|
|
o.id = -1;
|
|
std::cerr << " WARNING: " << dev << " (usb " << usbpath << ") never answered. Check its "
|
|
"RS-485 cable/power. Continuing so the other hand still works." << std::endl;
|
|
return o;
|
|
}
|
|
|
|
/**
|
|
* @brief Refresh the diagnostic registers for both hands.
|
|
*
|
|
* These are what turn "the finger will not move" from a guess into an answer:
|
|
* ERROR - locked rotor / over-temp / over-current, LATCHED until CLEAR_ERROR
|
|
* STATUS - what the driver thinks it is doing
|
|
* CURRENT- 0mA means it never energised; >0 means it is trying and blocked
|
|
* FORCE_SET - the limit the measured force is being compared against
|
|
*/
|
|
void refreshDiag()
|
|
{
|
|
uint8_t e[6], st[6], tp[6];
|
|
Eigen::Matrix<double, 6, 1> cu, fs;
|
|
for (int lo = 0; lo <= 6; lo += 6)
|
|
{
|
|
auto h = (lo == 0) ? righthand : lefthand;
|
|
if (h->GetError(e) == 0) for (int i = 0; i < 6; i++) dErr[lo + i] = e[i];
|
|
if (h->GetStatus(st)== 0) for (int i = 0; i < 6; i++) dSta[lo + i] = st[i];
|
|
if (h->GetTemp(tp) == 0) for (int i = 0; i < 6; i++) dTmp[lo + i] = tp[i];
|
|
if (h->GetCurrent(cu) == 0) dCur.block<6,1>(lo,0) = cu;
|
|
if (h->GetForceSet(fs) == 0) dFset.block<6,1>(lo,0) = fs;
|
|
}
|
|
}
|
|
|
|
/// Poll both hands' ERROR registers and auto-clear any latched fault.
|
|
void checkFaults()
|
|
{
|
|
static const char *FN[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
|
|
for (int lo = 0; lo <= 6; lo += 6)
|
|
{
|
|
auto h = (lo == 0) ? righthand : lefthand;
|
|
uint8_t err[6] = {0};
|
|
if (h->GetError(err) != 0) continue; // read failed; the link watchdog covers that
|
|
|
|
std::string hit;
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
if (!err[i]) continue;
|
|
hit += std::string(FN[i]) + "(0x";
|
|
const char *hex = "0123456789ABCDEF";
|
|
hit += hex[(err[i] >> 4) & 0xF];
|
|
hit += hex[err[i] & 0xF];
|
|
if (err[i] & 0x01) hit += " locked-rotor";
|
|
if (err[i] & 0x02) hit += " over-temp";
|
|
if (err[i] & 0x04) hit += " over-current";
|
|
if (err[i] & 0x08) hit += " abnormal";
|
|
if (err[i] & 0x10) hit += " comms";
|
|
hit += ") ";
|
|
}
|
|
if (hit.empty()) continue;
|
|
|
|
faultClears[lo == 0 ? 0 : 1]++;
|
|
std::cerr << "FAULT on " << (lo == 0 ? "RIGHT" : "LEFT") << " hand: " << hit
|
|
<< "-> sending CLEAR_ERROR (clear #" << faultClears[lo == 0 ? 0 : 1] << ")"
|
|
<< std::endl;
|
|
h->ClearError();
|
|
// A stall also drops the speed/force config on some units; restore both so the finger
|
|
// actually moves again instead of silently staying at zero speed.
|
|
h->SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
|
|
applyForce(lo == 0 ? reqForceR : reqForceL, lo);
|
|
}
|
|
}
|
|
|
|
/// Per-finger force limit = requested + that finger's sensor offset, capped at the 1000g max.
|
|
void applyForce(double req, int lo)
|
|
{
|
|
uint16_t f[6];
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
double v = req + std::abs(foff(lo + i));
|
|
f[i] = (uint16_t)(v > kForceMax ? kForceMax : (v < 0 ? 0 : v));
|
|
}
|
|
auto h = (lo == 0) ? righthand : lefthand;
|
|
h->SetForce(f[0], f[1], f[2], f[3], f[4], f[5]);
|
|
}
|
|
|
|
void run()
|
|
{
|
|
// Set command (write BOTH hands first)
|
|
if(!handcmd->isTimeout())
|
|
{
|
|
for(int i(0); i<12; i++)
|
|
{
|
|
qcmd(i) = handcmd->msg_.cmds()[i].q();
|
|
}
|
|
// Optional grip-force override via the cmd's kp field (<=0 = keep default).
|
|
// Lets the teacher drop to a low, back-drivable force so the fingers can be
|
|
// moved by hand for recording, then restore the strong default afterwards.
|
|
// Only re-sent when the requested value changes (avoids serial spam).
|
|
// Applied through applyForce() so each finger's sensor offset is still added on top
|
|
// — otherwise an override would re-gate a drifted finger back to "dead".
|
|
double rf = handcmd->msg_.cmds()[0].kp();
|
|
double lf = handcmd->msg_.cmds()[6].kp();
|
|
if(rf > 0 && rf != lastForceR)
|
|
{ reqForceR = rf; applyForce(rf, 0); lastForceR = rf; }
|
|
if(lf > 0 && lf != lastForceL)
|
|
{ reqForceL = lf; applyForce(lf, 6); lastForceL = lf; }
|
|
|
|
righthand->SetPosition(qcmd.block<6, 1>(0, 0));
|
|
lefthand->SetPosition(qcmd.block<6, 1>(6, 0));
|
|
}
|
|
|
|
// Recv state. GetPosition() is self-retrying: it flushes stale RX and re-sends the
|
|
// angleAct query, recovering when the hand is briefly busy after a write (a single
|
|
// one-shot query gets ignored and reads back all-zeros — worst inside Docker / a tight
|
|
// loop). A short settle after the writes still helps the very first query land.
|
|
Eigen::Matrix<double, 6, 1> qtemp;
|
|
usleep(1500); // settle after the writes (was 3000)
|
|
|
|
if(righthand->GetPosition(qtemp) == 0)
|
|
{
|
|
qstate.block<6, 1>(0, 0) = qtemp;
|
|
posFailR = 0;
|
|
everOkR = true;
|
|
}
|
|
else
|
|
{
|
|
posFailR++;
|
|
for(int i(0); i<6; i++)
|
|
{
|
|
handstate->msg_.states()[i].lost()++;
|
|
}
|
|
}
|
|
|
|
if(lefthand->GetPosition(qtemp) == 0)
|
|
{
|
|
qstate.block<6, 1>(6, 0) = qtemp;
|
|
posFailL = 0;
|
|
everOkL = true;
|
|
}
|
|
else
|
|
{
|
|
posFailL++;
|
|
for(int i(0); i<6; i++)
|
|
{
|
|
handstate->msg_.states()[i+6].lost()++;
|
|
}
|
|
}
|
|
|
|
// Force (grams, signed) — published as tau_est. A push on a finger registers
|
|
// here even though the non-backdrivable finger can't move; used for
|
|
// push-to-teach. Reuses the robust re-query reader.
|
|
//
|
|
// A FAILED read used to silently keep the previous value and republish it as if it were
|
|
// fresh, with no counter (positions had one, force did not). A frozen force channel is
|
|
// indistinguishable from a genuinely still finger, so push-to-teach would quietly stop
|
|
// detecting touches and nothing would say so. Now failures bump lost() and get logged.
|
|
Eigen::Matrix<double, 6, 1> ftemp;
|
|
bool fr = (righthand->GetForce(ftemp) == 0);
|
|
if(fr) fstate.block<6, 1>(0, 0) = ftemp;
|
|
bool fl = (lefthand->GetForce(ftemp) == 0);
|
|
if(fl) fstate.block<6, 1>(6, 0) = ftemp;
|
|
|
|
if(!fr) { forceFailR++; for(int i(0); i<6; i++) handstate->msg_.states()[i].lost()++; }
|
|
else { forceFailR = 0; everOkR = true; }
|
|
if(!fl) { forceFailL++; for(int i(6); i<12; i++) handstate->msg_.states()[i].lost()++; }
|
|
else { forceFailL = 0; everOkL = true; }
|
|
|
|
if(forceFailR == kStaleWarn)
|
|
std::cerr << "WARNING: right hand force reads failing - published force is STALE "
|
|
"(push-to-teach will not detect touches)" << std::endl;
|
|
if(forceFailL == kStaleWarn)
|
|
std::cerr << "WARNING: left hand force reads failing - published force is STALE "
|
|
"(push-to-teach will not detect touches)" << std::endl;
|
|
|
|
// A hand whose reads all fail is either (a) a stale link that a reopen would fix, or
|
|
// (b) physically gone — unplugged, unpowered, or not answering on RS-485.
|
|
//
|
|
// Only (a) is worth exiting for. Exiting on (b) would boot-loop forever: the supervisor
|
|
// restarts us every 3s, the hand is still absent, we exit again. So restart ONLY if this
|
|
// hand was working earlier in this process — that is the signature of a link that died
|
|
// and might come back on a reopen. If it has never answered since startup, stay up and
|
|
// keep serving the healthy hand, loudly and once.
|
|
bool deadR = (forceFailR > kDeadCycles) && (posFailR > kDeadCycles);
|
|
bool deadL = (forceFailL > kDeadCycles) && (posFailL > kDeadCycles);
|
|
if((deadR && everOkR) || (deadL && everOkL))
|
|
{
|
|
std::cerr << "FATAL: " << ((deadR && everOkR) ? "right" : "left")
|
|
<< " hand serial link DIED after working (" << kDeadCycles
|
|
<< " consecutive failed reads). Exiting so the supervisor can restart and "
|
|
"reopen/re-detect the adapters." << std::endl;
|
|
exit(1);
|
|
}
|
|
if(deadR && !everOkR && !warnedNeverR)
|
|
{ warnedNeverR = true;
|
|
std::cerr << "WARNING: RIGHT hand has NEVER answered since startup - check its RS-485 "
|
|
"cable/power. Continuing with the left hand only." << std::endl; }
|
|
if(deadL && !everOkL && !warnedNeverL)
|
|
{ warnedNeverL = true;
|
|
std::cerr << "WARNING: LEFT hand has NEVER answered since startup - check its RS-485 "
|
|
"cable/power. Continuing with the right hand only." << std::endl; }
|
|
|
|
if(handstate->trylock())
|
|
{
|
|
for(int i(0); i<12; i++)
|
|
{
|
|
handstate->msg_.states()[i].q() = qstate(i);
|
|
// Piggyback the diagnostics on fields this hand never populates, so no IDL
|
|
// change is needed: dq=current(mA), ddq=force limit, temperature=degC,
|
|
// reserve[0]=ERROR bits, reserve[1]=STATUS.
|
|
handstate->msg_.states()[i].dq() = dCur(i);
|
|
handstate->msg_.states()[i].ddq() = dFset(i);
|
|
handstate->msg_.states()[i].temperature() = dTmp[i];
|
|
handstate->msg_.states()[i].reserve()[0] = dErr[i];
|
|
handstate->msg_.states()[i].reserve()[1] = dSta[i];
|
|
// Subtract the calibrated resting offset so a drifted sensor zero doesn't look
|
|
// like a permanent 850g press, and touch deltas start from a true zero.
|
|
handstate->msg_.states()[i].tau_est() = fstate(i) - foff(i);
|
|
}
|
|
handstate->unlockAndPublish();
|
|
}
|
|
|
|
// Fault watchdog. ClearError() used to run ONLY in the constructor, so a finger that
|
|
// stalled mid-session stayed latched and dead until the whole service was restarted —
|
|
// it ignored every target at every force limit, and a release did not reset it, while
|
|
// still reporting a plausible angle and a low force. Poll the ERROR register ~1x/s and
|
|
// clear it automatically, naming the finger so a repeat offender is visible.
|
|
if(cycles % 50 == 0)
|
|
{
|
|
refreshDiag();
|
|
checkFaults();
|
|
}
|
|
|
|
// Report the rate we ACTUALLY achieve, so the gap between the requested period and the
|
|
// serial bus's real throughput is visible instead of assumed.
|
|
if(++cycles % 250 == 0)
|
|
{
|
|
auto now = std::chrono::steady_clock::now();
|
|
double dt = std::chrono::duration<double>(now - lastReport).count();
|
|
if(dt > 0)
|
|
{
|
|
pubHz = 250.0 / dt;
|
|
std::cout << "[inspire_g1] " << pubHz << " Hz" << std::endl;
|
|
}
|
|
lastReport = now;
|
|
}
|
|
}
|
|
|
|
unitree::common::ThreadPtr thread;
|
|
|
|
// inspire
|
|
SerialPort::SharedPtr serial1;
|
|
SerialPort::SharedPtr serial2;
|
|
std::shared_ptr<inspire::InspireHand> lefthand;
|
|
std::shared_ptr<inspire::InspireHand> righthand;
|
|
Eigen::Matrix<double, 12, 1> qcmd, qstate, fstate;
|
|
double lastForceR = -1, lastForceL = -1; // last grip force applied per hand (via cmd kp)
|
|
|
|
// Force-sensor zero offset per finger, measured at startup with the hand open+unloaded.
|
|
// Subtracted from published force, and added to each finger's force limit.
|
|
Eigen::Matrix<double, 12, 1> foff = Eigen::Matrix<double, 12, 1>::Zero();
|
|
double reqForceR = 500.0, reqForceL = 500.0; // grip force asked for, before offset
|
|
static constexpr double kForceMax = 1000.0; // RH56 hardware max
|
|
static constexpr double kForceMargin = 250.0; // headroom kept above a drifted zero
|
|
|
|
// Read-failure tracking. Consecutive failures per hand; force used to have no counter at
|
|
// all, so a dead force channel republished stale values indefinitely and looked healthy.
|
|
int forceFailR = 0, forceFailL = 0, posFailR = 0, posFailL = 0;
|
|
static constexpr int kStaleWarn = 25; // ~0.5s at 50Hz -> warn once
|
|
static constexpr int kDeadCycles = 150; // ~3s of total silence -> link is gone, restart
|
|
|
|
// Has this hand EVER answered since this process started? Distinguishes a link that died
|
|
// (worth exiting for, a reopen may fix it) from a hand that was never there (exiting would
|
|
// just boot-loop every 3s). Set on any successful read.
|
|
bool everOkR = false, everOkL = false;
|
|
bool warnedNeverR = false, warnedNeverL = false; // warn once, not every cycle
|
|
|
|
// Slow-poll diagnostics: ERROR/STATUS/TEMP/CURRENT/FORCE_SET, refreshed ~1x/s and
|
|
// published in MotorState_ fields this hand does not otherwise use. Costs one extra
|
|
// register read per second per hand, not per cycle.
|
|
uint8_t dErr[12] = {0}, dSta[12] = {0}, dTmp[12] = {0};
|
|
Eigen::Matrix<double, 12, 1> dCur = Eigen::Matrix<double, 12, 1>::Zero();
|
|
Eigen::Matrix<double, 12, 1> dFset = Eigen::Matrix<double, 12, 1>::Zero();
|
|
double pubHz = 0.0;
|
|
|
|
long cycles = 0;
|
|
long faultClears[2] = {0, 0}; // per hand; a climbing count = something keeps stalling
|
|
std::chrono::steady_clock::time_point lastReport = std::chrono::steady_clock::now();
|
|
|
|
// dds
|
|
std::unique_ptr<unitree::robot::RealTimePublisher<unitree_go::msg::dds_::MotorStates_>> handstate;
|
|
std::shared_ptr<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>> handcmd;
|
|
};
|
|
|
|
int main(int argc, char ** argv)
|
|
{
|
|
auto vm = param::helper(argc, argv);
|
|
unitree::robot::ChannelFactory::Instance()->Init(0, param::network);
|
|
|
|
std::cout << " --- Unitree Robotics --- " << std::endl;
|
|
std::cout << " Inspire Hand Controller " << std::endl;
|
|
|
|
InspireRunner runner;
|
|
|
|
while (true)
|
|
{
|
|
sleep(1);
|
|
}
|
|
return 0;
|
|
} |