371 lines
11 KiB
C++
371 lines
11 KiB
C++
#ifndef INSPIRE_H
|
|
#define INSPIRE_H
|
|
|
|
#include <eigen3/Eigen/Dense>
|
|
#include <vector>
|
|
|
|
#include "SerialPort.h"
|
|
|
|
namespace inspire
|
|
{
|
|
|
|
class InspireHand
|
|
{
|
|
public:
|
|
InspireHand(SerialPort::SharedPtr serial = nullptr, id_t id = 0)
|
|
: serial_(serial), id(id)
|
|
{
|
|
if(!serial)
|
|
serial_ = std::make_shared<SerialPort>("/dev/ttyUSB0", B115200);
|
|
|
|
}
|
|
|
|
void ChangeID(uint8_t before, uint8_t now)
|
|
{
|
|
std::vector<uint8_t> cmd = {0xEB, 0x90, before, 0x04, 0x12, 0xE8, 0x03, now, 0x00};
|
|
cmd.back() = CheckSum(cmd.data(), cmd.size());
|
|
serial_->send(cmd.data(), cmd.size());
|
|
|
|
usleep(5000);
|
|
serial_->recv(recvBuff, 9);
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Set the finger position
|
|
* -1: no change
|
|
* [0, 1] 0: close
|
|
* ID: pinky, ring, middle, index, thumb_bend, thumb_rotate
|
|
*/
|
|
int16_t SetPosition(const Eigen::Matrix<double, 6, 1> & q)
|
|
{
|
|
Eigen::Matrix<int16_t, 6, 1> q_int16;
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
// 0xFFFF = "no change" (per the docstring above): the hand IGNORES this joint and keeps
|
|
// driving it to its previous target. It does NOT release the motor — an earlier comment
|
|
// here claimed it did, and a hand-pose recording mode was built on that before the
|
|
// operator found the finger still locked. There is no release command on the RH56, and
|
|
// the drive is a non-backdrivable worm gear, so no force setting frees it either.
|
|
if (q(i) < 0.0) q_int16(i) = -1;
|
|
else { double v = q(i) * 1000.0; q_int16(i) = (int16_t)(v < 0 ? 0 : (v > 1000 ? 1000 : v)); }
|
|
}
|
|
|
|
uint8_t cmd[20];
|
|
cmd[0] = 0xEB; // head
|
|
cmd[1] = 0x90;
|
|
cmd[2] = id; // ID
|
|
cmd[3] = 0x0F; // data length
|
|
cmd[4] = 0x12; // write
|
|
cmd[5] = 0xCE; // address
|
|
cmd[6] = 0x05;
|
|
|
|
cmd[7] = q_int16(0) & 0xFF;
|
|
cmd[8] = (q_int16(0) >> 8) & 0xFF;
|
|
cmd[9] = q_int16(1) & 0xFF;
|
|
cmd[10] = (q_int16(1) >> 8) & 0xFF;
|
|
cmd[11] = q_int16(2) & 0xFF;
|
|
cmd[12] = (q_int16(2) >> 8) & 0xFF;
|
|
cmd[13] = q_int16(3) & 0xFF;
|
|
cmd[14] = (q_int16(3) >> 8) & 0xFF;
|
|
cmd[15] = q_int16(4) & 0xFF;
|
|
cmd[16] = (q_int16(4) >> 8) & 0xFF;
|
|
cmd[17] = q_int16(5) & 0xFF;
|
|
cmd[18] = (q_int16(5) >> 8) & 0xFF;
|
|
|
|
cmd[19] = CheckSum(cmd, 20);
|
|
|
|
serial_->send(cmd, 20);
|
|
usleep(1500); // was 5000 - recv() polls via select(), and reads flush() first
|
|
serial_->recv(recvBuff, 9);
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief Get the finger position
|
|
*
|
|
* ID: pinky, ring, middle, index, thumb_bend, thumb_rotate
|
|
*/
|
|
int16_t GetPosition(Eigen::Matrix<double, 6, 1> & q)
|
|
{
|
|
const uint8_t *b = Query12(0x0A); // angleAct
|
|
if (!b) return 1;
|
|
for (int i = 0; i < 6; i++)
|
|
q(i) = (b[7 + 2 * i] | (b[8 + 2 * i] << 8)) / 1000.;
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief Set the finger velocity
|
|
*/
|
|
void SetVelocity(int16_t v0, int16_t v1, int16_t v2, int16_t v3, int16_t v4, int16_t v5)
|
|
{
|
|
uint8_t cmd[20];
|
|
cmd[0] = 0xEB; // head
|
|
cmd[1] = 0x90;
|
|
cmd[2] = id; // ID
|
|
cmd[3] = 0x0F; // data length
|
|
cmd[4] = 0x12; // write
|
|
cmd[5] = 0xF2; // address
|
|
cmd[6] = 0x05;
|
|
|
|
cmd[7] = v0 & 0xFF;
|
|
cmd[8] = (v0 >> 8) & 0xFF;
|
|
cmd[9] = v1 & 0xFF;
|
|
cmd[10] = (v1 >> 8) & 0xFF;
|
|
cmd[11] = v2 & 0xFF;
|
|
cmd[12] = (v2 >> 8) & 0xFF;
|
|
cmd[13] = v3 & 0xFF;
|
|
cmd[14] = (v3 >> 8) & 0xFF;
|
|
cmd[15] = v4 & 0xFF;
|
|
cmd[16] = (v4 >> 8) & 0xFF;
|
|
cmd[17] = v5 & 0xFF;
|
|
cmd[18] = (v5 >> 8) & 0xFF;
|
|
|
|
cmd[19] = CheckSum(cmd, 20);
|
|
|
|
serial_->send(cmd, 20);
|
|
usleep(1500); // was 5000 - recv() polls via select(), and reads flush() first
|
|
serial_->recv(recvBuff, 9);
|
|
}
|
|
|
|
/**
|
|
* @brief Get the force control threshold
|
|
*
|
|
* [0, 1000] Unit: g
|
|
*/
|
|
void SetForce(uint16_t f0, uint16_t f1, uint16_t f2, uint16_t f3, uint16_t f4, uint16_t f5)
|
|
{
|
|
uint8_t cmd[20];
|
|
cmd[0] = 0xEB; // head
|
|
cmd[1] = 0x90;
|
|
cmd[2] = id; // ID
|
|
cmd[3] = 0x0F; // data length
|
|
cmd[4] = 0x12; // write
|
|
cmd[5] = 0xDA; // address
|
|
cmd[6] = 0x05;
|
|
|
|
cmd[7] = f0 & 0xFF;
|
|
cmd[8] = (f0 >> 8) & 0xFF;
|
|
cmd[9] = f1 & 0xFF;
|
|
cmd[10] = (f1 >> 8) & 0xFF;
|
|
cmd[11] = f2 & 0xFF;
|
|
cmd[12] = (f2 >> 8) & 0xFF;
|
|
cmd[13] = f3 & 0xFF;
|
|
cmd[14] = (f3 >> 8) & 0xFF;
|
|
cmd[15] = f4 & 0xFF;
|
|
cmd[16] = (f4 >> 8) & 0xFF;
|
|
cmd[17] = f5 & 0xFF;
|
|
cmd[18] = (f5 >> 8) & 0xFF;
|
|
|
|
cmd[19] = CheckSum(cmd, 20);
|
|
|
|
serial_->send(cmd, 20);
|
|
usleep(1500); // was 5000 - recv() polls via select(), and reads flush() first
|
|
serial_->recv(recvBuff, 9);
|
|
}
|
|
|
|
/**
|
|
* @brief Get the force of each finger
|
|
*
|
|
* The force is in the unit of g, [0 - 1000], convert to N
|
|
*/
|
|
int16_t GetForce(Eigen::Matrix<double, 6, 1> & f)
|
|
{
|
|
// Force sensor register 0x2E. Output = grams, SIGNED. Used for push-to-teach: a hand
|
|
// push shows up as a force deviation even though the finger can't back-drive.
|
|
const uint8_t *b = Query12(0x2E);
|
|
if (!b) return 1;
|
|
for (int i = 0; i < 6; i++)
|
|
f(i) = (double)(int16_t)(b[7 + 2 * i] | (b[8 + 2 * i] << 8));
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief Read the per-finger ERROR register (1606).
|
|
*
|
|
* Bits: 0x01 locked rotor, 0x02 over-temperature, 0x04 over-current,
|
|
* 0x08 abnormal operation, 0x10 communication error.
|
|
*
|
|
* The RH56 LATCHES these: once a finger stalls it refuses every subsequent motion
|
|
* command — any target, any force limit, and a release (q<0) does not reset it — until
|
|
* CLEAR_ERROR is sent. It keeps reporting a low force and a valid angle throughout, so
|
|
* from the outside it just looks like a finger that stopped listening.
|
|
*/
|
|
int16_t GetError(uint8_t out[6])
|
|
{
|
|
const uint8_t *b = QueryReg(1606, 6);
|
|
if (!b) return 1;
|
|
for (int i = 0; i < 6; i++) out[i] = b[7 + i];
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief Per-finger STATUS (1612). 0 unclenching, 1 grasping, 2 reached position,
|
|
* 3 reached force, 5 current protection, 6 locked rotor, 7 fault.
|
|
*/
|
|
int16_t GetStatus(uint8_t out[6])
|
|
{
|
|
const uint8_t *b = QueryReg(1612, 6);
|
|
if (!b) return 1;
|
|
for (int i = 0; i < 6; i++) out[i] = b[7 + i];
|
|
return 0;
|
|
}
|
|
|
|
/// Per-finger motor temperature in degrees C (1618).
|
|
int16_t GetTemp(uint8_t out[6])
|
|
{
|
|
const uint8_t *b = QueryReg(1618, 6);
|
|
if (!b) return 1;
|
|
for (int i = 0; i < 6; i++) out[i] = b[7 + i];
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief Per-finger motor CURRENT in mA (1594, 12 bytes / 6 int16).
|
|
*
|
|
* The single number that separates "dead" from "force-gated": a finger commanded to move
|
|
* that draws 0 mA is not being driven at all, whereas one drawing current against a
|
|
* mechanical block is trying and failing. Reading only angle and force cannot tell those
|
|
* apart, which is how healthy fingers were once written off as dead actuators.
|
|
*/
|
|
int16_t GetCurrent(Eigen::Matrix<double, 6, 1> &c)
|
|
{
|
|
const uint8_t *b = QueryReg(1594, 12);
|
|
if (!b) return 1;
|
|
for (int i = 0; i < 6; i++)
|
|
c(i) = (double)(int16_t)(b[7 + 2 * i] | (b[8 + 2 * i] << 8));
|
|
return 0;
|
|
}
|
|
|
|
/// Per-finger grip force LIMIT currently in the hand (FORCE_SET 1498). Shown next to the
|
|
/// measured force so "force >= limit -> driver refuses to move" is visible, not deduced.
|
|
int16_t GetForceSet(Eigen::Matrix<double, 6, 1> &f)
|
|
{
|
|
const uint8_t *b = QueryReg(1498, 12);
|
|
if (!b) return 1;
|
|
for (int i = 0; i < 6; i++)
|
|
f(i) = (double)(int16_t)(b[7 + 2 * i] | (b[8 + 2 * i] << 8));
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief Clear error
|
|
*
|
|
* When the Inspire Hand has a fault such as a stall, overcurrent, or abnormality,
|
|
* the fault can be cleared by the clear fault command.
|
|
*/
|
|
void ClearError()
|
|
{
|
|
uint8_t cmd[9];
|
|
cmd[0] = 0xEB; // head
|
|
cmd[1] = 0x90;
|
|
cmd[2] = id; // ID
|
|
cmd[3] = 0x04; // data length
|
|
cmd[4] = 0x12; // write
|
|
cmd[5] = 0xEC; // address
|
|
cmd[6] = 0x03;
|
|
cmd[7] = 0x01;
|
|
cmd[8] = CheckSum(cmd, 9);
|
|
|
|
serial_->send(cmd, 9);
|
|
usleep(5000);
|
|
serial_->recv(recvBuff, 9);
|
|
}
|
|
|
|
/**
|
|
* @brief Force sensor calibration
|
|
*
|
|
* @attention The Inspire Hand must be in an unloaded state during calibration
|
|
*/
|
|
void Calibration()
|
|
{
|
|
std::vector<uint8_t> cmd = {0xEB, 0x90, id, 0x04, 0x12, 0x2F, 0x06, 0x01, 0x00};
|
|
cmd.back() = CheckSum(cmd.data(), cmd.size());
|
|
serial_->send(cmd.data(), cmd.size());
|
|
|
|
usleep(5000);
|
|
serial_->recv(recvBuff, 9); // First frame
|
|
sleep(10); // The calibration process takes about 6 seconds
|
|
serial_->recv(recvBuff, 9); // Second frame
|
|
}
|
|
|
|
uint8_t id = 1;
|
|
private:
|
|
/**
|
|
* @brief Send a 12-byte read query for `addr` and wait for its reply, POLLING the port
|
|
* rather than sleeping a fixed interval.
|
|
*
|
|
* The previous version slept a flat 8ms per attempt whether or not the reply had already
|
|
* landed. With four of these per control cycle (2 hands x angle+force) that alone cost
|
|
* ~32ms, which is why the 100Hz RecurrentThread actually published at a measured 28Hz.
|
|
* select() inside recv() already returns the instant bytes arrive, so we accumulate until
|
|
* a complete, checksum-valid frame is present or the budget expires — typically ~3ms.
|
|
*
|
|
* Still re-queries on failure: if the hand is briefly busy after a write it ignores a
|
|
* single query and a one-shot read returns all-zeros. We only accept a reply whose cmd
|
|
* (0x11) and address match, so a stray write-ack or echo can never be mis-parsed as 0.
|
|
*
|
|
* @return pointer to the 20-byte frame inside recvBuff, or nullptr if it never arrived.
|
|
*/
|
|
const uint8_t* QueryReg(uint16_t addr, uint8_t nbytes, int budget_us = 12000)
|
|
{
|
|
uint8_t query[9] = {0xEB, 0x90, id, 0x04, 0x11,
|
|
(uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8), nbytes, 0x00};
|
|
query[8] = CheckSum(query, 9);
|
|
const size_t frame = 7u + nbytes + 1u; // header+addr + data + checksum
|
|
|
|
for (int attempt = 0; attempt < 4; attempt++)
|
|
{
|
|
serial_->flush();
|
|
serial_->send(query, 9);
|
|
|
|
size_t n = 0;
|
|
auto t0 = std::chrono::steady_clock::now();
|
|
while (n + 1 < sizeof(recvBuff))
|
|
{
|
|
ssize_t got = serial_->recv(recvBuff + n, sizeof(recvBuff) - n);
|
|
if (got > 0)
|
|
{
|
|
n += (size_t)got;
|
|
for (size_t off = 0; off + frame <= n; off++)
|
|
{
|
|
if (!((recvBuff[off] == 0x90 && recvBuff[off + 1] == 0xEB) ||
|
|
(recvBuff[off] == 0xEB && recvBuff[off + 1] == 0x90))) continue;
|
|
if (recvBuff[off + 4] != 0x11) continue;
|
|
uint16_t got_addr = recvBuff[off + 5] | (recvBuff[off + 6] << 8);
|
|
if (got_addr != addr) continue;
|
|
if (recvBuff[off + frame - 1] != CheckSum(recvBuff + off, frame)) continue;
|
|
return recvBuff + off;
|
|
}
|
|
}
|
|
auto el = std::chrono::duration_cast<std::chrono::microseconds>(
|
|
std::chrono::steady_clock::now() - t0).count();
|
|
if (el >= budget_us) break;
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
/// 12-byte reads (angleAct 0x060A, forceAct 0x062E) keep their short spelling.
|
|
const uint8_t* Query12(uint8_t addrLo, int budget_us = 12000)
|
|
{
|
|
return QueryReg((uint16_t)(0x0600 | addrLo), 12, budget_us);
|
|
}
|
|
|
|
uint8_t CheckSum(const uint8_t* data, uint8_t len)
|
|
{
|
|
uint8_t sum = 0;
|
|
for (int i = 2; i < len - 1; i++)
|
|
{
|
|
sum += data[i];
|
|
}
|
|
return sum;
|
|
}
|
|
|
|
SerialPort::SharedPtr serial_;
|
|
uint8_t recvBuff[1024];
|
|
};
|
|
|
|
} // namespace inspire
|
|
#endif // INSPIRE_H
|