189 lines
6.4 KiB
C++
189 lines
6.4 KiB
C++
/**
|
|
* @file hand_gestures.cpp
|
|
* @brief Named gestures for the Inspire RH56 hands on G1, over DDS (rt/inspire/cmd).
|
|
*
|
|
* Requires the service running in another terminal: ./inspire_g1
|
|
*
|
|
* Usage:
|
|
* ./hand_gestures # interactive menu (pick gestures repeatedly)
|
|
* ./hand_gestures <gesture> [iface] # run one gesture and exit
|
|
* gesture = open | close | box | thumbup | handshake | combo | demo
|
|
*
|
|
* Per-hand DOF order: [pinky, ring, middle, index, thumb_bend, thumb_rotation]
|
|
* q in [0,1]: 0 = closed, 1 = open. Indices 0-5 = right hand, 6-11 = left hand.
|
|
*
|
|
* NOTE: this controls the HAND (fingers) only. Raising the arm is a separate
|
|
* subsystem (G1 arm joints) and is not driven here.
|
|
*/
|
|
|
|
#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 <unitree/common/thread/thread.hpp>
|
|
|
|
#include <eigen3/Eigen/Dense>
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <mutex>
|
|
#include <unistd.h>
|
|
|
|
using Vec6 = Eigen::Matrix<float, 6, 1>;
|
|
|
|
class HandController
|
|
{
|
|
public:
|
|
HandController() { Init(); }
|
|
|
|
void set(const Vec6 &right, const Vec6 &left)
|
|
{
|
|
for (size_t i = 0; i < 6; i++)
|
|
{
|
|
cmd.cmds()[i].q() = right(i);
|
|
cmd.cmds()[i + 6].q() = left(i);
|
|
}
|
|
handcmd->Write(cmd);
|
|
}
|
|
void setBoth(const Vec6 &q) { set(q, q); }
|
|
|
|
unitree_go::msg::dds_::MotorCmds_ cmd;
|
|
unitree_go::msg::dds_::MotorStates_ state;
|
|
|
|
private:
|
|
void Init()
|
|
{
|
|
handcmd = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>("rt/inspire/cmd");
|
|
handcmd->InitChannel();
|
|
cmd.cmds().resize(12);
|
|
handstate = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::MotorStates_>>("rt/inspire/state");
|
|
handstate->InitChannel([this](const void *message) {
|
|
std::lock_guard<std::mutex> lock(mtx);
|
|
state = *(unitree_go::msg::dds_::MotorStates_ *)message;
|
|
});
|
|
state.states().resize(12);
|
|
}
|
|
|
|
std::mutex mtx;
|
|
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::MotorCmds_> handcmd;
|
|
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::MotorStates_> handstate;
|
|
};
|
|
|
|
static Vec6 V(float pinky, float ring, float middle, float index, float thumb_bend, float thumb_rot)
|
|
{
|
|
Vec6 v; v << pinky, ring, middle, index, thumb_bend, thumb_rot; return v;
|
|
}
|
|
static void hold(int ms) { usleep(ms * 1000); }
|
|
|
|
// --- Pose library (tune freely; values are [0,1], 0=closed 1=open) ---
|
|
static const Vec6 OPEN = V(1, 1, 1, 1, 1, 1);
|
|
static const Vec6 CLOSE = V(0, 0, 0, 0, 0, 0);
|
|
static const Vec6 BOX = V(0.5, 0.5, 0.5, 0.5, 0.4, 0.1);
|
|
static const Vec6 THUMBUP = V(0, 0, 0, 0, 1, 1);
|
|
static const Vec6 SHAKE_READY = V(0.85, 0.85, 0.85, 0.85, 0.85, 0.6);
|
|
static const Vec6 SHAKE_GRIP = V(0.2, 0.2, 0.2, 0.2, 0.25, 0.1);
|
|
|
|
// --- Gestures (social ones use the RIGHT hand, left held open) ---
|
|
static void g_pose(HandController &h, const Vec6 &q, const char *name, int ms = 1500)
|
|
{
|
|
std::cout << " [" << name << "]" << std::endl;
|
|
h.setBoth(q);
|
|
hold(ms);
|
|
}
|
|
static void g_thumbup(HandController &h, int ms = 2000)
|
|
{
|
|
std::cout << " [thumbup] (right hand)" << std::endl;
|
|
h.set(THUMBUP, OPEN);
|
|
hold(ms);
|
|
}
|
|
static void g_handshake(HandController &h)
|
|
{
|
|
std::cout << " [handshake] (right hand)" << std::endl;
|
|
h.set(SHAKE_READY, OPEN); hold(1200); // reach out, open
|
|
h.set(SHAKE_GRIP, OPEN); hold(800); // grip
|
|
for (int k = 0; k < 3; k++) // a few firm "shakes"
|
|
{
|
|
h.set(SHAKE_GRIP * 0.85f, OPEN); hold(180);
|
|
h.set(SHAKE_GRIP, OPEN); hold(180);
|
|
}
|
|
h.set(THUMBUP, OPEN); hold(1500); // finish with a thumbs-up
|
|
h.set(OPEN, OPEN); hold(600); // relax
|
|
}
|
|
static void g_demo(HandController &h)
|
|
{
|
|
g_pose(h, OPEN, "open");
|
|
g_pose(h, CLOSE, "close");
|
|
g_pose(h, BOX, "box");
|
|
g_pose(h, OPEN, "open");
|
|
g_thumbup(h);
|
|
g_handshake(h);
|
|
g_pose(h, OPEN, "open");
|
|
}
|
|
|
|
// Dispatch by name or menu number. Returns false only on quit.
|
|
static bool runGesture(HandController &h, const std::string &g)
|
|
{
|
|
if (g == "open" || g == "1") g_pose(h, OPEN, "open");
|
|
else if (g == "close" || g == "2") g_pose(h, CLOSE, "close");
|
|
else if (g == "box" || g == "3") g_pose(h, BOX, "box");
|
|
else if (g == "thumbup" || g == "4") g_thumbup(h);
|
|
else if (g == "handshake" || g == "5") g_handshake(h);
|
|
else if (g == "combo" || g == "6") { g_thumbup(h); g_handshake(h); }
|
|
else if (g == "demo" || g == "7") g_demo(h);
|
|
else if (g == "q" || g == "quit" || g == "0") return false;
|
|
else std::cout << " unknown option: '" << g << "'" << std::endl;
|
|
return true;
|
|
}
|
|
|
|
static void printMenu()
|
|
{
|
|
std::cout << "\n=== Inspire Hand Gestures ===\n"
|
|
<< " 1) open\n"
|
|
<< " 2) close (fist)\n"
|
|
<< " 3) box (power grasp)\n"
|
|
<< " 4) thumbup (right hand)\n"
|
|
<< " 5) handshake (right hand, ends thumbs-up)\n"
|
|
<< " 6) combo (thumbup + handshake)\n"
|
|
<< " 7) demo (run all)\n"
|
|
<< " q) quit\n"
|
|
<< "Select: " << std::flush;
|
|
}
|
|
|
|
static std::string trim(std::string s)
|
|
{
|
|
const char *ws = " \t\r\n";
|
|
size_t a = s.find_first_not_of(ws);
|
|
size_t b = s.find_last_not_of(ws);
|
|
return a == std::string::npos ? "" : s.substr(a, b - a + 1);
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
std::string arg1 = argc > 1 ? argv[1] : "";
|
|
std::string iface = argc > 2 ? argv[2] : "";
|
|
|
|
unitree::robot::ChannelFactory::Instance()->Init(0, iface);
|
|
auto hand = std::make_shared<HandController>();
|
|
usleep(300000); // let DDS match the service
|
|
|
|
// Direct mode: run one gesture and exit (backward compatible).
|
|
if (!arg1.empty())
|
|
{
|
|
runGesture(*hand, arg1);
|
|
std::cout << "done." << std::endl;
|
|
return 0;
|
|
}
|
|
|
|
// Interactive menu: fire gestures repeatedly over one DDS connection.
|
|
std::string line;
|
|
while (true)
|
|
{
|
|
printMenu();
|
|
if (!std::getline(std::cin, line)) break; // EOF / Ctrl-D
|
|
line = trim(line);
|
|
if (line.empty()) continue;
|
|
if (!runGesture(*hand, line)) break;
|
|
}
|
|
std::cout << "bye." << std::endl;
|
|
return 0;
|
|
}
|