209 lines
9.1 KiB
C++
209 lines
9.1 KiB
C++
/**
|
|
* @file hand_bridge.cpp
|
|
* @brief Local TCP <-> DDS bridge for the web dashboard.
|
|
* - "v0 v1 ... v11\n" -> publish MotorCmds_ to rt/inspire/cmd (q in [0,1], or <0 = release)
|
|
* - "R\n" -> reply with the 12 latest rt/inspire/state q values (read-back / capture)
|
|
* - "S\n" -> reply with 24 values: 12 angles then 12 forces (joint tracker)
|
|
* Persistent publisher + state subscriber. Needs ./inspire_g1 running.
|
|
*
|
|
* ./hand_bridge [iface] [port] (default eth0 7799)
|
|
*/
|
|
#include <unitree/idl/go2/MotorCmds_.hpp>
|
|
#include <unitree/idl/go2/MotorStates_.hpp>
|
|
#include <unitree/robot/channel/channel_publisher.hpp>
|
|
#include <unitree/robot/channel/channel_subscriber.hpp>
|
|
|
|
#include <arpa/inet.h>
|
|
#include <netinet/in.h>
|
|
#include <sys/socket.h>
|
|
#include <unistd.h>
|
|
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <cstdio>
|
|
#include <iostream>
|
|
#include <mutex>
|
|
#include <cmath>
|
|
#include <sstream>
|
|
#include <chrono>
|
|
#include <thread>
|
|
#include <atomic>
|
|
#include <cstdlib>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
std::string iface = argc > 1 ? argv[1] : "eth0";
|
|
int port = argc > 2 ? std::atoi(argv[2]) : 7799;
|
|
|
|
unitree::robot::ChannelFactory::Instance()->Init(0, iface);
|
|
auto pub = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>("rt/inspire/cmd");
|
|
pub->InitChannel();
|
|
unitree_go::msg::dds_::MotorCmds_ cmd;
|
|
cmd.cmds().resize(12);
|
|
|
|
// Subscribe to hand state for read-back / capture.
|
|
std::mutex mtx;
|
|
double state[12] = {0};
|
|
double force[12] = {0};
|
|
double cur[12] = {0}, fset[12] = {0}; // dq = current mA, ddq = force limit
|
|
double temp[12] = {0}, err[12] = {0}, sta[12] = {0}, lost[12] = {0};
|
|
double rate = 0; int rn = 0; auto rt0 = std::chrono::steady_clock::now();
|
|
|
|
// ---- force-follow (admittance) --------------------------------------------------
|
|
// The RH56 cannot be back-driven, so pushing a finger moves it 0.000. This closes the
|
|
// loop in software: read the press, drive the finger that way, hold when you stop. It
|
|
// lives here rather than in the web layer because the loop needs the 30Hz state stream
|
|
// and the command publisher in the same place -- polling over HTTP is far too slow.
|
|
std::atomic<bool> follow{false};
|
|
std::atomic<double> fgain{0.0006}, fdead{60.0}, fmaxrate{0.35}, fforce{400.0};
|
|
double ftarget[12] = {0}, fbase[12] = {0};
|
|
std::atomic<bool> fcal{false}; // tau_est, in grams — the joint tracker needs BOTH angle and
|
|
// force, because 'force moved but angle did not' is the whole
|
|
// signature of a finger that cannot be back-driven.
|
|
auto sub = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::MotorStates_>>("rt/inspire/state");
|
|
sub->InitChannel([&](const void *m) {
|
|
auto s = (const unitree_go::msg::dds_::MotorStates_ *)m;
|
|
std::lock_guard<std::mutex> lk(mtx);
|
|
for (int i = 0; i < 12 && i < (int)s->states().size(); i++) {
|
|
state[i] = s->states()[i].q();
|
|
force[i] = s->states()[i].tau_est();
|
|
cur[i] = s->states()[i].dq(); // mA
|
|
fset[i] = s->states()[i].ddq(); // grip force limit
|
|
temp[i] = s->states()[i].temperature();
|
|
err[i] = s->states()[i].reserve()[0];
|
|
sta[i] = s->states()[i].reserve()[1];
|
|
lost[i] = s->states()[i].lost();
|
|
}
|
|
// Measure the ACTUAL publish rate here — a hand whose bus has died keeps serving
|
|
// cached values, so a healthy-looking readout with a collapsed rate is the tell.
|
|
if (++rn >= 20) {
|
|
auto now = std::chrono::steady_clock::now();
|
|
double dt = std::chrono::duration<double>(now - rt0).count();
|
|
if (dt > 0) rate = rn / dt;
|
|
rn = 0; rt0 = now;
|
|
}
|
|
});
|
|
|
|
// Admittance thread: only publishes while follow is on, so it never fights the
|
|
// dashboard, the recorder, or anything else driving the hand.
|
|
std::thread ftick([&]{
|
|
auto prev = std::chrono::steady_clock::now();
|
|
while (true) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(33));
|
|
auto now = std::chrono::steady_clock::now();
|
|
double dt = std::chrono::duration<double>(now - prev).count(); prev = now;
|
|
if (!follow.load()) continue;
|
|
if (dt > 0.2) dt = 0.2;
|
|
{
|
|
std::lock_guard<std::mutex> lk(mtx);
|
|
if (fcal.exchange(false)) { // arm: zero on the current force
|
|
for (int i = 0; i < 12; i++) { fbase[i] = force[i]; ftarget[i] = state[i]; }
|
|
}
|
|
for (int i = 0; i < 12; i++) {
|
|
double dev = force[i] - fbase[i];
|
|
if (std::abs(dev) <= fdead.load()) continue;
|
|
dev -= (dev > 0 ? fdead.load() : -fdead.load());
|
|
double step = -fgain.load() * dev * dt; // press pad (+force) -> close
|
|
double cap = fmaxrate.load() * dt;
|
|
if (step > cap) step = cap;
|
|
if (step < -cap) step = -cap;
|
|
ftarget[i] += step;
|
|
if (ftarget[i] < 0.0) ftarget[i] = 0.0;
|
|
if (ftarget[i] > 1.0) ftarget[i] = 1.0;
|
|
}
|
|
for (int i = 0; i < 12; i++) {
|
|
cmd.cmds()[i].q() = ftarget[i];
|
|
cmd.cmds()[i].kp() = fforce.load();
|
|
}
|
|
}
|
|
pub->Write(cmd);
|
|
}
|
|
});
|
|
ftick.detach();
|
|
|
|
int srv = socket(AF_INET, SOCK_STREAM, 0);
|
|
int opt = 1;
|
|
setsockopt(srv, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
|
|
sockaddr_in addr{};
|
|
addr.sin_family = AF_INET;
|
|
addr.sin_addr.s_addr = inet_addr("127.0.0.1");
|
|
addr.sin_port = htons(port);
|
|
if (bind(srv, (sockaddr *)&addr, sizeof(addr)) < 0) { perror("bind"); return 1; }
|
|
listen(srv, 8);
|
|
std::cout << "hand_bridge listening on 127.0.0.1:" << port << " (iface " << iface << ")" << std::endl;
|
|
|
|
char buf[1024];
|
|
while (true)
|
|
{
|
|
int c = accept(srv, nullptr, nullptr);
|
|
if (c < 0) continue;
|
|
int n = read(c, buf, sizeof(buf) - 1);
|
|
if (n > 0)
|
|
{
|
|
buf[n] = 0;
|
|
if (buf[0] == 'F' || buf[0] == 'f') // follow: "F 1 gain dead maxrate force" | "F 0"
|
|
{
|
|
std::istringstream iss(buf + 1);
|
|
int on = 0; double g = 0, d = 0, mr = 0, fo = 0;
|
|
iss >> on;
|
|
if (iss >> g && g > 0) fgain.store(g);
|
|
if (iss >> d && d > 0) fdead.store(d);
|
|
if (iss >> mr && mr > 0) fmaxrate.store(mr);
|
|
if (iss >> fo && fo > 0) fforce.store(fo);
|
|
if (on) fcal.store(true); // re-zero the baseline each time it arms
|
|
follow.store(on != 0);
|
|
std::string o = std::string("follow ") + (on ? "on" : "off") + "\n";
|
|
(void)!write(c, o.c_str(), o.size());
|
|
}
|
|
else if (buf[0] == 'D' || buf[0] == 'd') // full diagnostics: 8x12 + rate
|
|
{
|
|
std::string out;
|
|
{ std::lock_guard<std::mutex> lk(mtx);
|
|
char t[32];
|
|
const double *blocks[8] = {state, force, cur, fset, temp, err, sta, lost};
|
|
for (int b = 0; b < 8; b++)
|
|
for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", blocks[b][i]); out += t; }
|
|
snprintf(t, sizeof(t), "%.2f ", rate); out += t; }
|
|
out += "\n";
|
|
(void)!write(c, out.c_str(), out.size());
|
|
}
|
|
else if (buf[0] == 'S' || buf[0] == 's') // angle + force: 24 values
|
|
{
|
|
std::string out;
|
|
{ std::lock_guard<std::mutex> lk(mtx);
|
|
char t[32];
|
|
for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", state[i]); out += t; }
|
|
for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.1f ", force[i]); out += t; } }
|
|
out += "\n";
|
|
(void)!write(c, out.c_str(), out.size());
|
|
}
|
|
else if (buf[0] == 'R' || buf[0] == 'r') // read-back request (12 angles, legacy)
|
|
{
|
|
std::string out;
|
|
{ std::lock_guard<std::mutex> lk(mtx);
|
|
char t[32];
|
|
for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", state[i]); out += t; } }
|
|
out += "\n";
|
|
(void)!write(c, out.c_str(), out.size());
|
|
}
|
|
else
|
|
{
|
|
std::istringstream iss(buf);
|
|
std::vector<float> v; float x;
|
|
while (iss >> x) v.push_back(x);
|
|
if (v.size() == 12)
|
|
{
|
|
for (int i = 0; i < 12; i++) cmd.cmds()[i].q() = v[i];
|
|
for (int k = 0; k < 3; k++) { pub->Write(cmd); usleep(8000); }
|
|
(void)!write(c, "ok\n", 3);
|
|
}
|
|
else (void)!write(c, "err\n", 4);
|
|
}
|
|
}
|
|
close(c);
|
|
}
|
|
return 0;
|
|
}
|