103 lines
3.9 KiB
C++
103 lines
3.9 KiB
C++
/**
|
|
* @file greet_common.hpp
|
|
* @brief Shared arm-action + Inspire-hand greeting routines (handshake / thumbup).
|
|
*
|
|
* Arm uses Unitree's built-in actions (G1ArmActionClient, balance-aware).
|
|
* Hand uses the Inspire DDS bridge -> REQUIRES ./inspire_g1 running for fingers.
|
|
*/
|
|
#pragma once
|
|
|
|
#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
|
|
#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
|
|
#include <unitree/idl/go2/MotorCmds_.hpp>
|
|
#include <unitree/robot/channel/channel_publisher.hpp>
|
|
|
|
#include <eigen3/Eigen/Dense>
|
|
#include <iostream>
|
|
#include <memory>
|
|
#include <string>
|
|
#include <unistd.h>
|
|
|
|
using Vec6 = Eigen::Matrix<float, 6, 1>;
|
|
|
|
// Inspire hand command publisher (open-loop; needs inspire_g1 running).
|
|
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 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); }
|
|
pub_->Write(cmd_);
|
|
}
|
|
private:
|
|
unitree_go::msg::dds_::MotorCmds_ cmd_;
|
|
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::MotorCmds_> pub_;
|
|
};
|
|
|
|
inline Vec6 gv(float p, float r, float m, float i, float tb, float tr) { Vec6 v; v << p, r, m, i, tb, tr; return v; }
|
|
inline void ghold(int ms) { usleep(ms * 1000); }
|
|
|
|
// q in [0,1], 0=closed 1=open; order pinky,ring,middle,index,thumb_bend,thumb_rot
|
|
static const Vec6 G_OPEN = gv(1, 1, 1, 1, 1, 1);
|
|
static const Vec6 G_THUMBUP = gv(0, 0, 0, 0, 1, 1);
|
|
static const Vec6 G_SHAKE_READY = gv(0.85, 0.85, 0.85, 0.85, 0.85, 0.6);
|
|
static const Vec6 G_SHAKE_GRIP = gv(0.2, 0.2, 0.2, 0.2, 0.25, 0.1);
|
|
|
|
// Execute a built-in arm action; print a helpful message on failure.
|
|
inline bool arm_do(unitree::robot::g1::G1ArmActionClient &arm, int id, const char *name)
|
|
{
|
|
using namespace unitree::robot::g1;
|
|
std::cout << "arm: " << name << " (" << id << ") ..." << std::endl;
|
|
int32_t ret = arm.ExecuteAction(id);
|
|
if (ret != 0) {
|
|
std::cerr << " arm action '" << name << "' failed, code " << ret;
|
|
if (ret == UT_ROBOT_ARM_ACTION_ERR_ARMSDK) std::cerr << " (rt/arm_sdk occupied)";
|
|
else if (ret == UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID) std::cerr << " (need fsm {500,501,801})";
|
|
else if (ret == UT_ROBOT_ARM_ACTION_ERR_HOLDING) std::cerr << " (arm holding; send release 99)";
|
|
std::cerr << std::endl;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// arm reaches out -> hand grips -> thumbs-up -> arm releases
|
|
inline void runHandshake(unitree::robot::g1::G1ArmActionClient &arm, Hand &hand)
|
|
{
|
|
hand.set(G_SHAKE_READY, G_OPEN); // present an open hand
|
|
if (!arm_do(arm, 27, "shake hand")) { hand.set(G_OPEN, G_OPEN); return; }
|
|
ghold(300);
|
|
std::cout << "hand: grip" << std::endl;
|
|
hand.set(G_SHAKE_GRIP, G_OPEN); ghold(1500);
|
|
std::cout << "hand: thumbs-up" << std::endl;
|
|
hand.set(G_THUMBUP, G_OPEN); ghold(1200);
|
|
hand.set(G_OPEN, G_OPEN); ghold(400);
|
|
arm_do(arm, 99, "release arm");
|
|
}
|
|
|
|
// raise right hand -> hand thumbs-up -> arm releases
|
|
inline void runThumbup(unitree::robot::g1::G1ArmActionClient &arm, Hand &hand)
|
|
{
|
|
if (!arm_do(arm, 23, "right hand up")) { hand.set(G_OPEN, G_OPEN); return; }
|
|
ghold(300);
|
|
std::cout << "hand: thumbs-up" << std::endl;
|
|
hand.set(G_THUMBUP, G_OPEN); ghold(2500);
|
|
hand.set(G_OPEN, G_OPEN); ghold(400);
|
|
arm_do(arm, 99, "release arm");
|
|
}
|
|
|
|
// Shared setup: init DDS + arm client + hand, then return the arm client.
|
|
inline std::shared_ptr<unitree::robot::g1::G1ArmActionClient> greet_init(const std::string &iface)
|
|
{
|
|
unitree::robot::ChannelFactory::Instance()->Init(0, iface);
|
|
auto arm = std::make_shared<unitree::robot::g1::G1ArmActionClient>();
|
|
arm->Init();
|
|
arm->SetTimeout(10.f);
|
|
return arm;
|
|
}
|