186 lines
6.3 KiB
C++
186 lines
6.3 KiB
C++
/**
|
|
* @file hand_record.cpp
|
|
* @brief Record & replay named hand poses (12 joint values: right 0-5, left 6-11).
|
|
*
|
|
* You sculpt a shape live (close/open individual fingers), name it, and it's saved
|
|
* to a file. Records the COMMANDED values (the serial read-back is unreliable on
|
|
* these CH340 adapters, and the hand holds whatever it's commanded anyway).
|
|
* Needs ./inspire_g1 running.
|
|
*
|
|
* ./hand_record [iface] interactive recorder (default iface eth0)
|
|
* ./hand_record play <name> [iface] command a saved pose and hold
|
|
* ./hand_record list list saved poses
|
|
*
|
|
* Poses file: $HOME/DFX_inspire_service/hand_poses.txt (line: "name v0 .. v11")
|
|
* Per-hand joint order: pinky ring middle index thumb_bend thumb_rot. q in [0,1].
|
|
*/
|
|
#include <unitree/idl/go2/MotorCmds_.hpp>
|
|
#include <unitree/robot/channel/channel_publisher.hpp>
|
|
|
|
#include <array>
|
|
#include <cstdlib>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <map>
|
|
#include <sstream>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <unistd.h>
|
|
|
|
using Pose = std::array<float, 12>;
|
|
|
|
static std::string posesFile()
|
|
{
|
|
const char *h = getenv("HOME");
|
|
return std::string(h ? h : ".") + "/DFX_inspire_service/hand_poses.txt";
|
|
}
|
|
|
|
static std::map<std::string, Pose> loadPoses()
|
|
{
|
|
std::map<std::string, Pose> m;
|
|
std::ifstream f(posesFile());
|
|
std::string line;
|
|
while (std::getline(f, line))
|
|
{
|
|
if (line.empty() || line[0] == '#') continue;
|
|
std::istringstream iss(line);
|
|
std::string name; iss >> name;
|
|
Pose v{}; int i = 0; float x;
|
|
while (i < 12 && (iss >> x)) v[i++] = x;
|
|
if (i == 12) m[name] = v;
|
|
}
|
|
return m;
|
|
}
|
|
|
|
static void savePose(const std::string &name, const Pose &v)
|
|
{
|
|
auto m = loadPoses();
|
|
m[name] = v; // add or overwrite
|
|
std::ofstream f(posesFile(), std::ios::trunc);
|
|
f << "# name <right: pinky ring middle index thumb_bend thumb_rot> <left: same>\n";
|
|
for (const auto &kv : m)
|
|
{
|
|
f << kv.first;
|
|
for (float x : kv.second) f << " " << x;
|
|
f << "\n";
|
|
}
|
|
}
|
|
|
|
class Hand
|
|
{
|
|
public:
|
|
Hand()
|
|
{
|
|
pub_ = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>("rt/inspire/cmd");
|
|
pub_->InitChannel();
|
|
cmd_.cmds().resize(12);
|
|
}
|
|
void apply(const Pose &v)
|
|
{
|
|
for (int i = 0; i < 12; i++) cmd_.cmds()[i].q() = v[i];
|
|
for (int n = 0; n < 8; n++) { pub_->Write(cmd_); usleep(20000); } // ~0.16s so it lands
|
|
}
|
|
private:
|
|
unitree_go::msg::dds_::MotorCmds_ cmd_;
|
|
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::MotorCmds_> pub_;
|
|
};
|
|
|
|
static void printPose(const Pose &v)
|
|
{
|
|
std::cout << " R:";
|
|
for (int i = 0; i < 6; i++) std::cout << " " << v[i];
|
|
std::cout << " L:";
|
|
for (int i = 6; i < 12; i++) std::cout << " " << v[i];
|
|
std::cout << std::endl;
|
|
}
|
|
|
|
static void printMenu()
|
|
{
|
|
std::cout <<
|
|
"\n--- hand_record --- q in [0,1] (tune to taste), order: pinky ring middle index thumb_bend thumb_rot\n"
|
|
" r <6 nums> set RIGHT hand l <6 nums> set LEFT hand b <6 nums> set BOTH\n"
|
|
" f <0-11> <v> set one joint (0-5 right, 6-11 left) open | close all\n"
|
|
" save <name> save current shape load <name> recall list | print | q\n";
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
std::string a1 = argc > 1 ? argv[1] : "";
|
|
|
|
if (a1 == "list")
|
|
{
|
|
auto m = loadPoses();
|
|
std::cout << m.size() << " saved pose(s):\n";
|
|
for (const auto &kv : m) { std::cout << " " << kv.first << ":"; printPose(kv.second); }
|
|
return 0;
|
|
}
|
|
if (a1 == "play")
|
|
{
|
|
if (argc < 3) { std::cerr << "usage: hand_record play <name> [iface]\n"; return 1; }
|
|
std::string name = argv[2], iface = argc > 3 ? argv[3] : "eth0";
|
|
auto m = loadPoses();
|
|
if (!m.count(name)) { std::cerr << "no pose '" << name << "'. try: hand_record list\n"; return 1; }
|
|
unitree::robot::ChannelFactory::Instance()->Init(0, iface);
|
|
Hand hand; usleep(300000);
|
|
std::cout << "playing '" << name << "'\n"; printPose(m[name]);
|
|
hand.apply(m[name]); usleep(1500000);
|
|
return 0;
|
|
}
|
|
|
|
// interactive
|
|
std::string iface = a1.empty() ? "eth0" : a1;
|
|
unitree::robot::ChannelFactory::Instance()->Init(0, iface);
|
|
Hand hand; usleep(300000);
|
|
|
|
Pose cur; cur.fill(1.0f); // start open
|
|
hand.apply(cur);
|
|
printMenu();
|
|
|
|
std::string line;
|
|
while (true)
|
|
{
|
|
std::cout << "rec> " << std::flush;
|
|
if (!std::getline(std::cin, line)) break;
|
|
std::istringstream iss(line);
|
|
std::string op; iss >> op;
|
|
if (op.empty()) continue;
|
|
|
|
if (op == "q" || op == "quit") break;
|
|
else if (op == "open") { cur.fill(1.0f); hand.apply(cur); printPose(cur); }
|
|
else if (op == "close") { cur.fill(0.0f); hand.apply(cur); printPose(cur); }
|
|
else if (op == "print") { printPose(cur); }
|
|
else if (op == "list") { for (const auto &kv : loadPoses()) { std::cout << " " << kv.first << ":"; printPose(kv.second); } }
|
|
else if (op == "r" || op == "l" || op == "b")
|
|
{
|
|
std::vector<float> v; float x; while (iss >> x) v.push_back(x);
|
|
if (v.size() != 6) { std::cout << " need 6 numbers\n"; continue; }
|
|
if (op == "r" || op == "b") for (int i = 0; i < 6; i++) cur[i] = v[i];
|
|
if (op == "l" || op == "b") for (int i = 0; i < 6; i++) cur[i + 6] = v[i];
|
|
hand.apply(cur); printPose(cur);
|
|
}
|
|
else if (op == "f")
|
|
{
|
|
int idx; float val;
|
|
if (!(iss >> idx >> val) || idx < 0 || idx > 11) { std::cout << " usage: f <0-11> <val>\n"; continue; }
|
|
cur[idx] = val; hand.apply(cur); printPose(cur);
|
|
}
|
|
else if (op == "save")
|
|
{
|
|
std::string name; iss >> name;
|
|
if (name.empty()) { std::cout << " usage: save <name>\n"; continue; }
|
|
savePose(name, cur);
|
|
std::cout << " saved '" << name << "' -> " << posesFile() << "\n";
|
|
}
|
|
else if (op == "load")
|
|
{
|
|
std::string name; iss >> name;
|
|
auto m = loadPoses();
|
|
if (!m.count(name)) { std::cout << " no pose '" << name << "'\n"; continue; }
|
|
cur = m[name]; hand.apply(cur); printPose(cur);
|
|
}
|
|
else std::cout << " ? see menu\n";
|
|
}
|
|
std::cout << "bye.\n";
|
|
return 0;
|
|
}
|