62 lines
2.4 KiB
C++
62 lines
2.4 KiB
C++
/**
|
|
* @file hand_pose.cpp
|
|
* @brief Live finger-pose tuner for the Inspire hand (right hand). Needs ./inspire_g1.
|
|
*
|
|
* ./hand_pose [networkInterface] (default iface: eth0)
|
|
*
|
|
* Then type 6 numbers (q in [0,1], 0=closed 1=open):
|
|
* pinky ring middle index thumb_bend thumb_rot
|
|
* e.g. 0 0 0 0 1 1 -> try a thumbs-up and tweak the last two values
|
|
* Shortcuts: open | close | q(uit)
|
|
* The pose is applied to the RIGHT hand (left held open) and holds until the next entry.
|
|
*/
|
|
#include <unitree/idl/go2/MotorCmds_.hpp>
|
|
#include <unitree/robot/channel/channel_publisher.hpp>
|
|
|
|
#include <iostream>
|
|
#include <sstream>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <memory>
|
|
#include <unistd.h>
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
const std::string iface = argc > 1 ? argv[1] : "eth0";
|
|
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);
|
|
usleep(300000);
|
|
|
|
// right = indices 0-5, left = 6-11 (held open)
|
|
auto apply = [&](const std::vector<float> &r) {
|
|
for (int i = 0; i < 6; i++) { cmd.cmds()[i].q() = r[i]; cmd.cmds()[i + 6].q() = 1.0f; }
|
|
for (int n = 0; n < 12; n++) { pub->Write(cmd); usleep(30000); } // ~0.36s of repeats so it lands + moves
|
|
};
|
|
|
|
std::cout << "Inspire RIGHT-hand pose tuner. Order: pinky ring middle index thumb_bend thumb_rot\n"
|
|
<< "q in [0,1], 0=closed 1=open. Type 6 numbers, or 'open'/'close'/'q'.\n";
|
|
|
|
std::string line;
|
|
while (true) {
|
|
std::cout << "pose> " << std::flush;
|
|
if (!std::getline(std::cin, line)) break;
|
|
if (line == "q" || line == "quit") break;
|
|
if (line == "open") { apply({1, 1, 1, 1, 1, 1}); continue; }
|
|
if (line == "close") { apply({0, 0, 0, 0, 0, 0}); continue; }
|
|
|
|
std::istringstream iss(line);
|
|
std::vector<float> v; float x;
|
|
while (iss >> x) v.push_back(x);
|
|
if (v.size() != 6) { std::cout << " enter exactly 6 numbers (or open/close/q)\n"; continue; }
|
|
apply(v);
|
|
std::cout << " applied: pinky=" << v[0] << " ring=" << v[1] << " middle=" << v[2]
|
|
<< " index=" << v[3] << " thumb_bend=" << v[4] << " thumb_rot=" << v[5] << "\n";
|
|
}
|
|
std::cout << "bye.\n";
|
|
return 0;
|
|
}
|