初始化项目，包含fast_lio_ws和unitree_sdk2
This commit is contained in:
53
unitree_sdk2/example/g1/CMakeLists.txt
Normal file
53
unitree_sdk2/example/g1/CMakeLists.txt
Normal file
@@ -0,0 +1,53 @@
|
||||
add_executable(g1_loco_client high_level/g1_loco_client_example.cpp)
|
||||
target_link_libraries(g1_loco_client unitree_sdk2)
|
||||
|
||||
add_executable(g1_arm5_sdk_dds_example high_level/g1_arm5_sdk_dds_example.cpp)
|
||||
target_link_libraries(g1_arm5_sdk_dds_example unitree_sdk2)
|
||||
|
||||
add_executable(g1_arm7_sdk_dds_example high_level/g1_arm7_sdk_dds_example.cpp)
|
||||
target_link_libraries(g1_arm7_sdk_dds_example unitree_sdk2)
|
||||
|
||||
add_executable(g1_ankle_swing_example low_level/g1_ankle_swing_example.cpp)
|
||||
target_link_libraries(g1_ankle_swing_example unitree_sdk2)
|
||||
|
||||
add_executable(g1_audio_client_example audio/g1_audio_client_example.cpp)
|
||||
target_link_libraries(g1_audio_client_example unitree_sdk2)
|
||||
|
||||
add_executable(g1_dex3_example dex3/g1_dex3_example.cpp)
|
||||
target_link_libraries(g1_dex3_example unitree_sdk2)
|
||||
|
||||
add_executable(g1_userctrl_dds_example high_level/g1_userctrl_dds_example.cpp)
|
||||
target_link_libraries(g1_userctrl_dds_example unitree_sdk2)
|
||||
|
||||
find_package(Boost COMPONENTS program_options)
|
||||
if(Boost_FOUND)
|
||||
add_executable(g1_termination low_level/terminations.cpp)
|
||||
target_link_libraries(g1_termination unitree_sdk2 Boost::program_options)
|
||||
|
||||
add_executable(g1_arm_action_example high_level/g1_arm_action_example.cpp)
|
||||
target_link_libraries(g1_arm_action_example unitree_sdk2 Boost::program_options)
|
||||
endif()
|
||||
|
||||
find_package(yaml-cpp QUIET)
|
||||
if(yaml-cpp_FOUND)
|
||||
if (${yaml-cpp_VERSION} VERSION_GREATER_EQUAL "0.6")
|
||||
message(STATUS "Found yaml-cpp version ${yaml-cpp_VERSION}")
|
||||
add_executable(g1_dual_arm_example low_level/g1_dual_arm_example.cpp)
|
||||
target_link_libraries(g1_dual_arm_example PRIVATE unitree_sdk2 yaml-cpp)
|
||||
target_compile_definitions(g1_dual_arm_example PUBLIC BLIB_DIR="${CMAKE_CURRENT_SOURCE_DIR}/low_level/behavior_lib/")
|
||||
else()
|
||||
message(STATUS "yaml-cpp version ${yaml-cpp_VERSION} is too old, skipping build of g1_dual_arm_example.")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
add_executable(g1_agv_client_example g1d/g1_agv_client_example.cpp)
|
||||
target_link_libraries(g1_agv_client_example unitree_sdk2)
|
||||
|
||||
add_executable(g1d_arm_example g1d/g1d_arm_example.cpp)
|
||||
target_link_libraries(g1d_arm_example unitree_sdk2)
|
||||
|
||||
add_executable(g1d_height_control g1d/g1d_height_control.cpp)
|
||||
target_link_libraries(g1d_height_control unitree_sdk2)
|
||||
|
||||
add_executable(g1_hand_sdk_example high_level/g1_hand_sdk_example.cpp)
|
||||
target_link_libraries(g1_hand_sdk_example unitree_sdk2)
|
||||
175
unitree_sdk2/example/g1/audio/g1_audio_client_example.cpp
Normal file
175
unitree_sdk2/example/g1/audio/g1_audio_client_example.cpp
Normal file
@@ -0,0 +1,175 @@
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
#include <unitree/common/time/time_tool.hpp>
|
||||
#include <unitree/idl/ros2/String_.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
#include <unitree/robot/g1/audio/g1_audio_client.hpp>
|
||||
|
||||
#include "wav.hpp"
|
||||
|
||||
#define AUDIO_FILE_PATH "../example/g1/audio/test.wav"
|
||||
#define AUDIO_SUBSCRIBE_TOPIC "rt/audio_msg"
|
||||
#define GROUP_IP "239.168.123.161"
|
||||
#define PORT 5555
|
||||
|
||||
#define WAV_SECOND 5 // record seconds
|
||||
#define WAV_LEN (16000 * 2 * WAV_SECOND)
|
||||
#define WAV_LEN_ONCE (16000 * 2 * 160 / 1000)
|
||||
#define CHUNK_SIZE 96000 // 3 seconds
|
||||
int sock;
|
||||
|
||||
void asr_handler(const void *msg) {
|
||||
std_msgs::msg::dds_::String_ *resMsg = (std_msgs::msg::dds_::String_ *)msg;
|
||||
std::cout << "Topic:\"rt/audio_msg\" recv: " << resMsg->data() << std::endl;
|
||||
}
|
||||
|
||||
std::string get_local_ip_for_multicast() {
|
||||
struct ifaddrs *ifaddr, *ifa;
|
||||
char host[NI_MAXHOST];
|
||||
std::string result = "";
|
||||
|
||||
getifaddrs(&ifaddr);
|
||||
for (ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) {
|
||||
if (!ifa->ifa_addr || ifa->ifa_addr->sa_family != AF_INET) continue;
|
||||
getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in), host, NI_MAXHOST,
|
||||
NULL, 0, NI_NUMERICHOST);
|
||||
std::string ip(host);
|
||||
if (ip.find("192.168.123.") == 0) {
|
||||
result = ip;
|
||||
break;
|
||||
}
|
||||
}
|
||||
freeifaddrs(ifaddr);
|
||||
return result;
|
||||
}
|
||||
|
||||
void thread_mic(void) {
|
||||
sock = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
sockaddr_in local_addr{};
|
||||
local_addr.sin_family = AF_INET;
|
||||
local_addr.sin_port = htons(PORT);
|
||||
local_addr.sin_addr.s_addr = INADDR_ANY;
|
||||
bind(sock, (sockaddr *)&local_addr, sizeof(local_addr));
|
||||
|
||||
ip_mreq mreq{};
|
||||
inet_pton(AF_INET, GROUP_IP, &mreq.imr_multiaddr);
|
||||
std::string local_ip = get_local_ip_for_multicast();
|
||||
std::cout << "local ip: " << local_ip << std::endl;
|
||||
mreq.imr_interface.s_addr = inet_addr(local_ip.c_str());
|
||||
setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
|
||||
|
||||
int total_bytes = 0;
|
||||
std::vector<int16_t> pcm_data;
|
||||
pcm_data.reserve(WAV_LEN / 2);
|
||||
std::cout << "start record!" << std::endl;
|
||||
while (total_bytes < WAV_LEN) {
|
||||
char buffer[WAV_LEN_ONCE];
|
||||
ssize_t len = recvfrom(sock, buffer, sizeof(buffer), 0, nullptr, nullptr);
|
||||
if (len > 0) {
|
||||
size_t sample_count = len / 2;
|
||||
const int16_t *samples = reinterpret_cast<const int16_t *>(buffer);
|
||||
pcm_data.insert(pcm_data.end(), samples, samples + sample_count);
|
||||
total_bytes += len;
|
||||
}
|
||||
}
|
||||
|
||||
WriteWave("record.wav", 16000, pcm_data.data(), pcm_data.size(), 1);
|
||||
std::cout << "record finish! save to record.wav " << std::endl;
|
||||
}
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: audio_client_example [NetWorkInterface(eth0)]"
|
||||
<< std::endl;
|
||||
exit(0);
|
||||
}
|
||||
int32_t ret;
|
||||
/*
|
||||
* Initilaize ChannelFactory
|
||||
*/
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
|
||||
unitree::robot::g1::AudioClient client;
|
||||
client.Init();
|
||||
client.SetTimeout(10.0f);
|
||||
|
||||
/*ASR message Example*/
|
||||
unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_> subscriber(
|
||||
AUDIO_SUBSCRIBE_TOPIC);
|
||||
subscriber.InitChannel(asr_handler);
|
||||
|
||||
/*Volume Example*/
|
||||
uint8_t volume;
|
||||
ret = client.GetVolume(volume);
|
||||
std::cout << "GetVolume API ret:" << ret
|
||||
<< " volume = " << std::to_string(volume) << std::endl;
|
||||
ret = client.SetVolume(100);
|
||||
std::cout << "SetVolume to 100% , API ret:" << ret << std::endl;
|
||||
|
||||
/*TTS Example*/
|
||||
ret = client.TtsMaker("你好。我是宇树科技的机器人。例程启动成功",
|
||||
0); // Auto play
|
||||
std::cout << "TtsMaker API ret:" << ret << std::endl;
|
||||
unitree::common::Sleep(5);
|
||||
|
||||
ret = client.TtsMaker(
|
||||
"Hello. I'm a robot from Unitree Robotics. The example has started "
|
||||
"successfully. ",
|
||||
1); // Engilsh TTS
|
||||
std::cout << "TtsMaker API ret:" << ret << std::endl;
|
||||
unitree::common::Sleep(8);
|
||||
|
||||
/*Audio Play Example*/
|
||||
int32_t sample_rate = -1;
|
||||
int8_t num_channels = 0;
|
||||
bool filestate = false;
|
||||
std::vector<uint8_t> pcm =
|
||||
ReadWave(AUDIO_FILE_PATH, &sample_rate, &num_channels, &filestate);
|
||||
|
||||
std::cout << "wav file sample_rate = " << sample_rate
|
||||
<< " num_channels = " << std::to_string(num_channels)
|
||||
<< " filestate =" << filestate << "filesize = " << pcm.size()
|
||||
<< std::endl;
|
||||
|
||||
if (filestate && sample_rate == 16000 && num_channels == 1) {
|
||||
size_t total_size = pcm.size();
|
||||
size_t offset = 0;
|
||||
int chunk_index = 0;
|
||||
std::string stream_id =
|
||||
std::to_string(unitree::common::GetCurrentTimeMillisecond());
|
||||
|
||||
while (offset < total_size) {
|
||||
size_t remaining = total_size - offset;
|
||||
size_t current_chunk_size =
|
||||
std::min(static_cast<size_t>(CHUNK_SIZE), remaining);
|
||||
std::vector<uint8_t> chunk(pcm.begin() + offset,
|
||||
pcm.begin() + offset + current_chunk_size);
|
||||
client.PlayStream("example", stream_id, chunk);
|
||||
unitree::common::Sleep(1);
|
||||
std::cout << "Playing size: " << offset << std::endl;
|
||||
offset += current_chunk_size;
|
||||
}
|
||||
|
||||
ret = client.PlayStop(stream_id); // stop playback after transmission ends
|
||||
|
||||
} else {
|
||||
std::cout << "audio file format error, please check!" << std::endl;
|
||||
}
|
||||
|
||||
/*LED Control Example*/
|
||||
client.LedControl(0, 255, 0);
|
||||
unitree::common::Sleep(1);
|
||||
client.LedControl(0, 0, 0);
|
||||
unitree::common::Sleep(1);
|
||||
client.LedControl(0, 0, 255);
|
||||
|
||||
std::cout << "AudioClient api test finish , asr start..." << std::endl;
|
||||
|
||||
std::thread mic_t(thread_mic);
|
||||
|
||||
while (1) {
|
||||
sleep(1); // wait for asr message
|
||||
}
|
||||
mic_t.join();
|
||||
return 0;
|
||||
}
|
||||
BIN
unitree_sdk2/example/g1/audio/test.wav
Normal file
BIN
unitree_sdk2/example/g1/audio/test.wav
Normal file
Binary file not shown.
232
unitree_sdk2/example/g1/audio/wav.hpp
Normal file
232
unitree_sdk2/example/g1/audio/wav.hpp
Normal file
@@ -0,0 +1,232 @@
|
||||
#ifndef __UT_ROBOT_WAV_READER_HPP__
|
||||
#define __UT_ROBOT_WAV_READER_HPP__
|
||||
|
||||
struct WaveHeader {
|
||||
void SeekToDataChunk(std::istream &is) {
|
||||
while (is && subchunk2_id != 0x61746164) {
|
||||
is.seekg(subchunk2_size, std::istream::cur);
|
||||
is.read(reinterpret_cast<char *>(&subchunk2_id), sizeof(int32_t));
|
||||
is.read(reinterpret_cast<char *>(&subchunk2_size), sizeof(int32_t));
|
||||
}
|
||||
}
|
||||
|
||||
int32_t chunk_id;
|
||||
int32_t chunk_size;
|
||||
int32_t format;
|
||||
int32_t subchunk1_id;
|
||||
int32_t subchunk1_size;
|
||||
int16_t audio_format;
|
||||
int16_t num_channels;
|
||||
int32_t sample_rate;
|
||||
int32_t byte_rate;
|
||||
int16_t block_align;
|
||||
int16_t bits_per_sample;
|
||||
int32_t subchunk2_id; // a tag of this chunk
|
||||
int32_t subchunk2_size; // size of subchunk2
|
||||
};
|
||||
|
||||
static_assert(sizeof(WaveHeader) == 44);
|
||||
|
||||
std::vector<uint8_t> ReadWaveImpl(std::istream &is, int32_t *sampling_rate,
|
||||
int8_t *channelCount, bool *is_ok) {
|
||||
WaveHeader header{};
|
||||
is.read(reinterpret_cast<char *>(&header.chunk_id), sizeof(header.chunk_id));
|
||||
|
||||
// F F I R
|
||||
if (header.chunk_id != 0x46464952) {
|
||||
printf("Expected chunk_id RIFF. Given: 0x%08x\n", header.chunk_id);
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.chunk_size),
|
||||
sizeof(header.chunk_size));
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.format), sizeof(header.format));
|
||||
|
||||
// E V A W
|
||||
if (header.format != 0x45564157) {
|
||||
printf("Expected format WAVE. Given: 0x%08x\n", header.format);
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.subchunk1_id),
|
||||
sizeof(header.subchunk1_id));
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.subchunk1_size),
|
||||
sizeof(header.subchunk1_size));
|
||||
|
||||
if (header.subchunk1_id == 0x4b4e554a) {
|
||||
// skip junk padding
|
||||
is.seekg(header.subchunk1_size, std::istream::cur);
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.subchunk1_id),
|
||||
sizeof(header.subchunk1_id));
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.subchunk1_size),
|
||||
sizeof(header.subchunk1_size));
|
||||
}
|
||||
|
||||
if (header.subchunk1_id != 0x20746d66) {
|
||||
printf("Expected subchunk1_id 0x20746d66. Given: 0x%08x\n",
|
||||
header.subchunk1_id);
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
if (header.subchunk1_size != 16 &&
|
||||
header.subchunk1_size != 18) { // 16 for PCM
|
||||
printf("Expected subchunk1_size 16. Given: %d\n", header.subchunk1_size);
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.audio_format),
|
||||
sizeof(header.audio_format));
|
||||
|
||||
if (header.audio_format != 1) { // 1 for PCM
|
||||
printf("Expected audio_format 1. Given: %d\n", header.audio_format);
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.num_channels),
|
||||
sizeof(header.num_channels));
|
||||
|
||||
*channelCount = (int8_t)header.num_channels;
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.sample_rate),
|
||||
sizeof(header.sample_rate));
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.byte_rate),
|
||||
sizeof(header.byte_rate));
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.block_align),
|
||||
sizeof(header.block_align));
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.bits_per_sample),
|
||||
sizeof(header.bits_per_sample));
|
||||
|
||||
if (header.byte_rate !=
|
||||
(header.sample_rate * header.num_channels * header.bits_per_sample / 8)) {
|
||||
printf("Incorrect byte rate: %d. Expected: %d", header.byte_rate,
|
||||
(header.sample_rate * header.num_channels * header.bits_per_sample /
|
||||
8));
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
if (header.block_align !=
|
||||
(header.num_channels * header.bits_per_sample / 8)) {
|
||||
printf("Incorrect block align: %d. Expected: %d\n", header.block_align,
|
||||
(header.num_channels * header.bits_per_sample / 8));
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
if (header.bits_per_sample != 16) { // we support only 16 bits per sample
|
||||
printf("Expected bits_per_sample 16. Given: %d\n", header.bits_per_sample);
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
if (header.subchunk1_size == 18) {
|
||||
int16_t extra_size = -1;
|
||||
is.read(reinterpret_cast<char *>(&extra_size), sizeof(int16_t));
|
||||
if (extra_size != 0) {
|
||||
printf(
|
||||
"Extra size should be 0 for wave from NAudio. Current extra size "
|
||||
"%d\n",
|
||||
extra_size);
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
}
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.subchunk2_id),
|
||||
sizeof(header.subchunk2_id));
|
||||
|
||||
is.read(reinterpret_cast<char *>(&header.subchunk2_size),
|
||||
sizeof(header.subchunk2_size));
|
||||
|
||||
header.SeekToDataChunk(is);
|
||||
if (!is) {
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
*sampling_rate = header.sample_rate;
|
||||
|
||||
// header.subchunk2_size contains the number of bytes in the data.
|
||||
// As we assume each sample contains two bytes, so it is divided by 2 here
|
||||
std::vector<int16_t> samples(header.subchunk2_size / 2);
|
||||
|
||||
is.read(reinterpret_cast<char *>(samples.data()), header.subchunk2_size);
|
||||
if (!is) {
|
||||
*is_ok = false;
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<uint8_t> ans(samples.size() * 2);
|
||||
for (int32_t i = 0; i != static_cast<int32_t>(samples.size()); ++i) {
|
||||
ans[i * 2] = samples[i] & 0xFF;
|
||||
ans[i * 2 + 1] = (samples[i] >> 8) & 0xFF;
|
||||
}
|
||||
|
||||
*is_ok = true;
|
||||
return ans;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> ReadWave(const std::string &filename,
|
||||
int32_t *sampling_rate, int8_t *channelCount,
|
||||
bool *is_ok) {
|
||||
std::ifstream is(filename, std::ifstream::binary);
|
||||
auto samples = ReadWaveImpl(is, sampling_rate, channelCount, is_ok);
|
||||
return samples;
|
||||
}
|
||||
|
||||
bool WriteWave(const std::string &filename, int32_t sampling_rate,
|
||||
const int16_t *samples, int32_t n, uint8_t num_channels) {
|
||||
WaveHeader header{};
|
||||
header.chunk_id = 0x46464952; // FFIR
|
||||
header.format = 0x45564157; // EVAW
|
||||
header.subchunk1_id = 0x20746d66; // "fmt "
|
||||
header.subchunk1_size = 16; // 16 for PCM
|
||||
header.audio_format = 1; // PCM =1
|
||||
|
||||
int32_t bits_per_sample = 16; // int16_t
|
||||
header.num_channels = num_channels;
|
||||
header.sample_rate = sampling_rate;
|
||||
header.byte_rate = sampling_rate * num_channels * bits_per_sample / 8;
|
||||
header.block_align = num_channels * bits_per_sample / 8;
|
||||
header.bits_per_sample = bits_per_sample;
|
||||
header.subchunk2_id = 0x61746164; // atad
|
||||
header.subchunk2_size = n * num_channels * bits_per_sample / 8;
|
||||
|
||||
header.chunk_size = 36 + header.subchunk2_size;
|
||||
|
||||
std::vector<int16_t> samples_int16(n * num_channels);
|
||||
for (int32_t i = 0; i != n * num_channels; ++i) {
|
||||
samples_int16[i] = samples[i];
|
||||
}
|
||||
|
||||
std::ofstream os(filename, std::ios::binary);
|
||||
if (!os) {
|
||||
printf("Failed to create %s", filename.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
os.write(reinterpret_cast<const char *>(&header), sizeof(header));
|
||||
os.write(reinterpret_cast<const char *>(samples_int16.data()),
|
||||
samples_int16.size() * sizeof(int16_t));
|
||||
|
||||
if (!os) {
|
||||
printf("Write %s failed", filename.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
326
unitree_sdk2/example/g1/dex3/g1_dex3_example.cpp
Normal file
326
unitree_sdk2/example/g1/dex3/g1_dex3_example.cpp
Normal file
@@ -0,0 +1,326 @@
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <unitree/idl/hg/HandState_.hpp> //replace your sdk path
|
||||
#include <unitree/idl/hg/HandCmd_.hpp> //replace your sdk path
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <cmath>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
|
||||
enum State {
|
||||
INIT,
|
||||
ROTATE,
|
||||
GRIP,
|
||||
STOP,
|
||||
PRINT
|
||||
};
|
||||
|
||||
// set URDF Limits
|
||||
const float maxLimits_left[7]= { 1.05 , 1.05 , 1.75 , 0 , 0 , 0 , 0 }; // set max motor value
|
||||
const float minLimits_left[7]= { -1.05 , -0.724 , 0 , -1.57 , -1.75 , -1.57 ,-1.75};
|
||||
const float maxLimits_right[7]= { 1.05 , 0.742 , 0 , 1.57 , 1.75 , 1.57 , 1.75};
|
||||
const float minLimits_right[7]= { -1.05 , -1.05 , -1.75, 0 , 0 , 0 ,0 };
|
||||
|
||||
// Initing the dds configuration
|
||||
std::string dds_namespace = "rt/dex3/left";
|
||||
std::string sub_namespace = "rt/dex3/left/state";
|
||||
unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::HandCmd_> handcmd_publisher;
|
||||
unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::HandState_> handstate_subscriber;
|
||||
unitree_hg::msg::dds_::HandCmd_ msg;
|
||||
unitree_hg::msg::dds_::HandState_ state;
|
||||
std::atomic<State> currentState(INIT);
|
||||
std::mutex stateMutex;
|
||||
|
||||
#define MOTOR_MAX 7
|
||||
#define SENSOR_MAX 9
|
||||
uint8_t hand_id = 0;
|
||||
|
||||
typedef struct {
|
||||
uint8_t id : 4;
|
||||
uint8_t status : 3;
|
||||
uint8_t timeout: 1;
|
||||
} RIS_Mode_t;
|
||||
|
||||
// stateToString Method
|
||||
const char* stateToString(State state) {
|
||||
switch (state) {
|
||||
case INIT: return "INIT";
|
||||
case ROTATE: return "ROTATE";
|
||||
case GRIP: return "GRIP";
|
||||
case STOP: return "STOP";
|
||||
case PRINT: return "PRINT";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
// Monitor user's input
|
||||
char getNonBlockingInput() {
|
||||
struct termios oldt, newt;
|
||||
char ch;
|
||||
int oldf;
|
||||
|
||||
tcgetattr(STDIN_FILENO, &oldt);
|
||||
newt = oldt;
|
||||
newt.c_lflag &= ~(ICANON | ECHO);
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
|
||||
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
|
||||
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
|
||||
|
||||
ch = getchar();
|
||||
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
|
||||
fcntl(STDIN_FILENO, F_SETFL, oldf);
|
||||
|
||||
return ch;
|
||||
}
|
||||
|
||||
void userInputThread() {
|
||||
while (true) {
|
||||
char ch = getNonBlockingInput();
|
||||
if (ch == 'q') {
|
||||
std::cout << "Exiting..." << std::endl;
|
||||
currentState = STOP;
|
||||
break;
|
||||
} else if (ch == 'r') {
|
||||
currentState = ROTATE;
|
||||
} else if (ch == 'g') {
|
||||
currentState = GRIP;
|
||||
} else if (ch == 'p') {
|
||||
currentState = PRINT;
|
||||
} else if (ch == 's') {
|
||||
currentState = STOP;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
}
|
||||
|
||||
// this method can send kp and kd to motors
|
||||
void rotateMotors(bool isLeftHand) {
|
||||
static int _count = 1;
|
||||
static int dir = 1;
|
||||
const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
|
||||
const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;
|
||||
|
||||
for (int i = 0; i < MOTOR_MAX; i++) {
|
||||
RIS_Mode_t ris_mode;
|
||||
ris_mode.id = i;
|
||||
ris_mode.status = 0x01;
|
||||
|
||||
|
||||
uint8_t mode = 0;
|
||||
mode |= (ris_mode.id & 0x0F);
|
||||
mode |= (ris_mode.status & 0x07) << 4;
|
||||
mode |= (ris_mode.timeout & 0x01) << 7;
|
||||
msg.motor_cmd()[i].mode(mode);
|
||||
msg.motor_cmd()[i].tau(0);
|
||||
msg.motor_cmd()[i].kp(0.5);
|
||||
msg.motor_cmd()[i].kd(0.1);
|
||||
|
||||
|
||||
float range = maxLimits[i] - minLimits[i];
|
||||
float mid = (maxLimits[i] + minLimits[i]) / 2.0;
|
||||
float amplitude = range / 2.0;
|
||||
float q = mid + amplitude * sin(_count / 20000.0 * M_PI);
|
||||
|
||||
msg.motor_cmd()[i].q(q);
|
||||
}
|
||||
|
||||
handcmd_publisher->Write(msg);
|
||||
_count += dir;
|
||||
|
||||
|
||||
if (_count >= 10000) {
|
||||
dir = -1;
|
||||
}
|
||||
if (_count <= -10000) {
|
||||
dir = 1;
|
||||
}
|
||||
|
||||
usleep(100);
|
||||
}
|
||||
|
||||
// this method can send static position to motors
|
||||
void gripHand(bool isLeftHand) {
|
||||
|
||||
const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
|
||||
const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;
|
||||
|
||||
for (int i = 0; i < MOTOR_MAX; i++) {
|
||||
RIS_Mode_t ris_mode;
|
||||
ris_mode.id = i;
|
||||
ris_mode.status = 0x01;
|
||||
|
||||
|
||||
uint8_t mode = 0;
|
||||
mode |= (ris_mode.id & 0x0F);
|
||||
mode |= (ris_mode.status & 0x07) << 4;
|
||||
mode |= (ris_mode.timeout & 0x01) << 7;
|
||||
msg.motor_cmd()[i].mode(mode);
|
||||
msg.motor_cmd()[i].tau(0);
|
||||
|
||||
|
||||
float mid = (maxLimits[i] + minLimits[i]) / 2.0;
|
||||
|
||||
|
||||
msg.motor_cmd()[i].q(mid);
|
||||
msg.motor_cmd()[i].dq(0);
|
||||
msg.motor_cmd()[i].kp(1.5);
|
||||
msg.motor_cmd()[i].kd(0.1);
|
||||
}
|
||||
|
||||
|
||||
handcmd_publisher->Write(msg);
|
||||
usleep(1000000);
|
||||
}
|
||||
|
||||
// this method can send dynamic position to motors
|
||||
void stopMotors() {
|
||||
for (int i = 0; i < MOTOR_MAX; i++) {
|
||||
RIS_Mode_t ris_mode;
|
||||
ris_mode.id = i;
|
||||
ris_mode.status = 0x01;
|
||||
ris_mode.timeout = 0x01;
|
||||
|
||||
uint8_t mode = 0;
|
||||
mode |= (ris_mode.id & 0x0F);
|
||||
mode |= (ris_mode.status & 0x07) << 4;
|
||||
mode |= (ris_mode.timeout & 0x01) << 7;
|
||||
msg.motor_cmd()[i].mode(mode);
|
||||
msg.motor_cmd()[i].tau(0);
|
||||
msg.motor_cmd()[i].dq(0);
|
||||
msg.motor_cmd()[i].kp(0);
|
||||
msg.motor_cmd()[i].kd(0);
|
||||
msg.motor_cmd()[i].q(0);
|
||||
|
||||
}
|
||||
handcmd_publisher->Write(msg);
|
||||
usleep(1000000);
|
||||
}
|
||||
|
||||
// this method can subscribe dds and show the position for now
|
||||
void printState(bool isLeftHand){
|
||||
Eigen::Matrix<float, 7, 1> q;
|
||||
|
||||
const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
|
||||
const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;
|
||||
for(int i = 0; i < 7; i++)
|
||||
{
|
||||
q(i) = state.motor_state()[i].q();
|
||||
|
||||
q(i) = (q(i) - minLimits[i] ) / (maxLimits[i] - minLimits[i]);
|
||||
q(i) = std::clamp(q(i), 0.0f, 1.0f);
|
||||
}
|
||||
std::cout << "\033[2J\033[H";
|
||||
std::cout << "-- Hand State --\n";
|
||||
std::cout << "--- Current State: " << "Test" << " ---\n";
|
||||
std::cout << "Commands:\n";
|
||||
std::cout << " r - Rotate\n";
|
||||
std::cout << " g - Grip\n";
|
||||
std::cout << " t - Test\n";
|
||||
std::cout << " q - Quit\n";
|
||||
if(isLeftHand){
|
||||
std::cout << " L: " << q.transpose() << std::endl;
|
||||
}else std::cout << " R: " << q.transpose() << std::endl;
|
||||
usleep(0.1 * 1e6);
|
||||
|
||||
}
|
||||
|
||||
void StateHandler(const void *message) {
|
||||
state = *(unitree_hg::msg::dds_::HandState_ *)message;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int main(int argc, const char** argv)
|
||||
{
|
||||
std::cout << " --- Unitree Robotics --- \n";
|
||||
std::cout << " Dex3 Hand Example \n\n";
|
||||
std::string input;
|
||||
std::cout << "Please input the hand id (L for left hand, R for right hand): ";
|
||||
std::cin >> input;
|
||||
|
||||
if (input == "L") {
|
||||
hand_id = 0;
|
||||
dds_namespace = "rt/dex3/left";
|
||||
sub_namespace = "rt/lf/dex3/left/state";
|
||||
} else if (input == "R") {
|
||||
hand_id = 1;
|
||||
dds_namespace = "rt/dex3/right";
|
||||
sub_namespace = "rt/lf/dex3/right/state";
|
||||
} else {
|
||||
std::cout << "Invalid hand id. Please input 'L' or 'R'." << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (argc < 2)
|
||||
{
|
||||
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
|
||||
handcmd_publisher.reset(new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::HandCmd_>(dds_namespace + "/cmd"));
|
||||
handstate_subscriber.reset(new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::HandState_>(sub_namespace));
|
||||
handcmd_publisher->InitChannel();
|
||||
handstate_subscriber->InitChannel(
|
||||
std::bind(&StateHandler, std::placeholders::_1), 1);
|
||||
state.motor_state().resize(MOTOR_MAX);
|
||||
state.press_sensor_state().resize(SENSOR_MAX);
|
||||
msg.motor_cmd().resize(MOTOR_MAX);
|
||||
|
||||
// handcmd_publisher->msg_.motor_cmd().resize(MOTOR_MAX);
|
||||
|
||||
|
||||
std::thread inputThread(userInputThread);
|
||||
State lastState = INIT;
|
||||
while (true) {
|
||||
State state;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(stateMutex);
|
||||
state = currentState.load();
|
||||
}
|
||||
|
||||
if (state != lastState) {
|
||||
std::cout << "\n--- Current State: " << stateToString(state) << " ---\n";
|
||||
std::cout << "Commands:\n";
|
||||
std::cout << " r - Rotate\n";
|
||||
std::cout << " g - Grip\n";
|
||||
std::cout << " p - Print_state\n";
|
||||
std::cout << " q - Quit\n";
|
||||
std::cout << " s - Stop\n";
|
||||
lastState = state;
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
case INIT:
|
||||
std::cout << "Initializing..." << std::endl;
|
||||
currentState = ROTATE;
|
||||
break;
|
||||
case ROTATE:
|
||||
rotateMotors(input == "L");
|
||||
break;
|
||||
case GRIP:
|
||||
gripHand(input == "L");
|
||||
break;
|
||||
case STOP:
|
||||
stopMotors();
|
||||
break;
|
||||
case PRINT:
|
||||
printState(input == "L");
|
||||
break;
|
||||
default:
|
||||
std::cout << "Invalid state!" << std::endl;
|
||||
inputThread.join();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
53
unitree_sdk2/example/g1/g1d/g1_agv_client_example.cpp
Normal file
53
unitree_sdk2/example/g1/g1d/g1_agv_client_example.cpp
Normal file
@@ -0,0 +1,53 @@
|
||||
#include <unitree/robot/g1/agv/g1_agv_client.hpp>
|
||||
#include <iostream>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
|
||||
extern "C" {
|
||||
float sinf(float x);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
/*
|
||||
* Initilaize ChannelFactory
|
||||
*/
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0);
|
||||
unitree::robot::g1::AgvClient ac;
|
||||
|
||||
ac.SetTimeout(3.0f);
|
||||
ac.Init();
|
||||
|
||||
//Test Api - Periodic motion
|
||||
int cycle_count = 0;
|
||||
const float cycle_period = 40.0f; // 40步完成一个周期
|
||||
const float vx_amplitude = 0.3f;
|
||||
const float vyaw_amplitude = 0.3f;
|
||||
const float height_amplitude = 1.0f;
|
||||
|
||||
while (true)
|
||||
{
|
||||
// 使用正弦函数实现周期运动
|
||||
float time_phase = (cycle_count % (int)cycle_period) / cycle_period * 2.0f * M_PI;
|
||||
|
||||
// 速度周期运动: vx和vyaw使用正弦波
|
||||
float vx = vx_amplitude * sinf(time_phase);
|
||||
float vyaw = vyaw_amplitude * sinf(time_phase);
|
||||
|
||||
int32_t ret = ac.Move(vx, 0.0f, vyaw);
|
||||
std::cout << "Call Move vx:" << vx << " vyaw:" << vyaw << " ret:" << ret << std::endl;
|
||||
|
||||
usleep(50000); // 50ms
|
||||
|
||||
// 高度调节周期运动: 使用正弦波，周期与速度一致
|
||||
float height = height_amplitude * sinf(time_phase);
|
||||
|
||||
int32_t ret2 = ac.HeightAdjust(height);
|
||||
std::cout << "Call HeightAdjust height:" << height << " ret:" << ret2 << std::endl;
|
||||
|
||||
cycle_count++;
|
||||
usleep(50000); // 50ms
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
292
unitree_sdk2/example/g1/g1d/g1d_arm_example.cpp
Normal file
292
unitree_sdk2/example/g1/g1d/g1d_arm_example.cpp
Normal file
@@ -0,0 +1,292 @@
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <shared_mutex>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
#include "gamepad.hpp"
|
||||
|
||||
// DDS
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
// IDL
|
||||
#include <unitree/idl/hg/IMUState_.hpp>
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
|
||||
|
||||
static const std::string HG_CMD_TOPIC = "rt/lowcmd";
|
||||
static const std::string HG_IMU_TORSO = "rt/secondary_imu";
|
||||
static const std::string HG_STATE_TOPIC = "rt/lowstate";
|
||||
|
||||
using namespace unitree::common;
|
||||
using namespace unitree::robot;
|
||||
using namespace unitree_hg::msg::dds_;
|
||||
|
||||
template <typename T>
|
||||
class DataBuffer {
|
||||
public:
|
||||
void SetData(const T &newData) {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = std::make_shared<T>(newData);
|
||||
}
|
||||
|
||||
std::shared_ptr<const T> GetData() {
|
||||
std::shared_lock<std::shared_mutex> lock(mutex);
|
||||
return data ? data : nullptr;
|
||||
}
|
||||
|
||||
void Clear() {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = nullptr;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<T> data;
|
||||
std::shared_mutex mutex;
|
||||
};
|
||||
|
||||
const int G1D_NUM_MOTOR = 29;
|
||||
|
||||
// IMU状态结构体
|
||||
struct ImuState {
|
||||
std::array<float, 3> rpy = {};
|
||||
std::array<float, 3> omega = {};
|
||||
};
|
||||
|
||||
// 电机控制指令结构体
|
||||
struct MotorCommand {
|
||||
std::array<float, G1D_NUM_MOTOR> q_target = {};
|
||||
std::array<float, G1D_NUM_MOTOR> dq_target = {};
|
||||
std::array<float, G1D_NUM_MOTOR> kp = {};
|
||||
std::array<float, G1D_NUM_MOTOR> kd = {};
|
||||
std::array<float, G1D_NUM_MOTOR> tau_ff = {};
|
||||
};
|
||||
|
||||
// 电机状态结构体
|
||||
struct MotorState {
|
||||
std::array<float, G1D_NUM_MOTOR> q = {};
|
||||
std::array<float, G1D_NUM_MOTOR> dq = {};
|
||||
};
|
||||
|
||||
// Stiffness for all G1D Joints
|
||||
std::array<float, G1D_NUM_MOTOR> Kp{
|
||||
0, 0, 0, 0, 0, 0, // empty
|
||||
0, 0, 0, 0, 0, 0, // empty
|
||||
60, 0, 40, // waist
|
||||
40, 40, 40, 40, 40, 40, 40, // arms
|
||||
40, 40, 40, 40, 40, 40, 40 // arms
|
||||
};
|
||||
|
||||
// Damping for all G1D Joints
|
||||
std::array<float, G1D_NUM_MOTOR> Kd{
|
||||
0, 0, 0, 0, 0, 0, // empty
|
||||
0, 0, 0, 0, 0, 0, // empty
|
||||
1, 0, 1, // waist
|
||||
1, 1, 1, 1, 1, 1, 1, // arms
|
||||
1, 1, 1, 1, 1, 1, 1 // arms
|
||||
};
|
||||
|
||||
uint8_t mode_machine_;
|
||||
|
||||
Gamepad gamepad_;
|
||||
REMOTE_DATA_RX rx_;
|
||||
|
||||
DataBuffer<MotorState> motor_state_buffer_;
|
||||
DataBuffer<MotorCommand> motor_command_buffer_;
|
||||
DataBuffer<ImuState> imu_state_buffer_;
|
||||
|
||||
ThreadPtr command_writer_ptr_, control_thread_ptr_;
|
||||
|
||||
LowState_ g1d_low_state;
|
||||
IMUState_ Torso_IMU;
|
||||
|
||||
bool A_PRESSED = false;
|
||||
bool IMU_Get = false;
|
||||
|
||||
enum G1DJointIndex {
|
||||
LeftHipPitch = 0, // NOTE INVALID for g1d
|
||||
LeftHipRoll = 1, // NOTE INVALID for g1d
|
||||
LeftHipYaw = 2, // NOTE INVALID for g1d
|
||||
LeftKnee = 3, // NOTE INVALID for g1d
|
||||
LeftAnklePitch = 4, // NOTE INVALID for g1d
|
||||
LeftAnkleB = 4, // NOTE INVALID for g1d
|
||||
LeftAnkleRoll = 5, // NOTE INVALID for g1d
|
||||
LeftAnkleA = 5, // NOTE INVALID for g1d
|
||||
RightHipPitch = 6, // NOTE INVALID for g1d
|
||||
RightHipRoll = 7, // NOTE INVALID for g1d
|
||||
RightHipYaw = 8, // NOTE INVALID for g1d
|
||||
RightKnee = 9, // NOTE INVALID for g1d
|
||||
RightAnklePitch = 10, // NOTE INVALID for g1d
|
||||
RightAnkleB = 10, // NOTE INVALID for g1d
|
||||
RightAnkleRoll = 11, // NOTE INVALID for g1d
|
||||
RightAnkleA = 11, // NOTE INVALID for g1d
|
||||
WaistYaw = 12,
|
||||
WaistRoll = 13, // NOTE INVALID for g1d
|
||||
WaistA = 13, // NOTE INVALID for g1d
|
||||
WaistPitch = 14,
|
||||
WaistB = 14, // NOTE INVALID for g1d
|
||||
LeftShoulderPitch = 15,
|
||||
LeftShoulderRoll = 16,
|
||||
LeftShoulderYaw = 17,
|
||||
LeftElbow = 18,
|
||||
LeftWristRoll = 19,
|
||||
LeftWristPitch = 20,
|
||||
LeftWristYaw = 21,
|
||||
RightShoulderPitch = 22,
|
||||
RightShoulderRoll = 23,
|
||||
RightShoulderYaw = 24,
|
||||
RightElbow = 25,
|
||||
RightWristRoll = 26,
|
||||
RightWristPitch = 27,
|
||||
RightWristYaw = 28
|
||||
};
|
||||
|
||||
// CRC校验值计算
|
||||
inline uint32_t Crc32Core(uint32_t *ptr, uint32_t len) {
|
||||
uint32_t xbit = 0;
|
||||
uint32_t data = 0;
|
||||
uint32_t CRC32 = 0xFFFFFFFF;
|
||||
const uint32_t dwPolynomial = 0x04c11db7;
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
xbit = 1 << 31;
|
||||
data = ptr[i];
|
||||
for (uint32_t bits = 0; bits < 32; bits++) {
|
||||
if (CRC32 & 0x80000000) {
|
||||
CRC32 <<= 1;
|
||||
CRC32 ^= dwPolynomial;
|
||||
} else
|
||||
CRC32 <<= 1;
|
||||
if (data & xbit) CRC32 ^= dwPolynomial;
|
||||
|
||||
xbit >>= 1;
|
||||
}
|
||||
}
|
||||
return CRC32;
|
||||
};
|
||||
|
||||
// 躯干IMU消息回调函数
|
||||
void imuTorsoHandler(const void *message) {
|
||||
IMUState_ imu_torso = *(const IMUState_ *)message;
|
||||
Torso_IMU = imu_torso;
|
||||
}
|
||||
|
||||
// 底层状态消息回调函数
|
||||
void LowStateHandler(const void *message) {
|
||||
LowState_ low_state = *(const LowState_ *)message;
|
||||
g1d_low_state = low_state;
|
||||
if (low_state.crc() != Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) {
|
||||
std::cout << "[ERROR] CRC Error" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// 获取关节电机数据
|
||||
MotorState ms_tmp;
|
||||
for (int i = 0; i < G1D_NUM_MOTOR; ++i) {
|
||||
ms_tmp.q.at(i) = low_state.motor_state()[i].q();
|
||||
ms_tmp.dq.at(i) = low_state.motor_state()[i].dq();
|
||||
if (low_state.motor_state()[i].motorstate() && i <= RightAnkleRoll)
|
||||
std::cout << "[ERROR] motor " << i << " with code " << low_state.motor_state()[i].motorstate() << "\n";
|
||||
}
|
||||
motor_state_buffer_.SetData(ms_tmp);
|
||||
|
||||
// 获取IMU数据
|
||||
ImuState imu_tmp;
|
||||
imu_tmp.omega = low_state.imu_state().gyroscope();
|
||||
imu_tmp.rpy = low_state.imu_state().rpy();
|
||||
imu_state_buffer_.SetData(imu_tmp);
|
||||
IMU_Get = true;
|
||||
|
||||
// 更新遥控器状态
|
||||
memcpy(rx_.buff, &low_state.wireless_remote()[0], 40);
|
||||
gamepad_.update(rx_.RF_RX);
|
||||
if(gamepad_.A.on_press) A_PRESSED = true;
|
||||
|
||||
// 更新机型编号
|
||||
mode_machine_ = low_state.mode_machine();
|
||||
}
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: g1d_arm_example network_interface" << std::endl;
|
||||
exit(0);
|
||||
}
|
||||
std::string networkInterface = argv[1];
|
||||
ChannelFactory::Instance()->Init(0, networkInterface);
|
||||
|
||||
// Publisher
|
||||
ChannelPublisherPtr<LowCmd_> lowcmd_publisher(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
|
||||
lowcmd_publisher->InitChannel();
|
||||
|
||||
// Subscriber
|
||||
ChannelSubscriberPtr<LowState_> lowstate_subscriber(new ChannelSubscriber<LowState_>(HG_STATE_TOPIC));
|
||||
lowstate_subscriber->InitChannel(LowStateHandler, 1);
|
||||
ChannelSubscriberPtr<IMUState_> imu_subscriber(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
|
||||
imu_subscriber->InitChannel(imuTorsoHandler, 1);
|
||||
|
||||
// 等待接收到第一帧IMU数据，打印躯干IMU状态信息
|
||||
while(!IMU_Get){
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
std::cout << "Torso IMU State: " << std::endl
|
||||
<< "RPY = "
|
||||
<< Torso_IMU.rpy()[0] << ", " << Torso_IMU.rpy()[1] << ", " << Torso_IMU.rpy()[2]
|
||||
<< std::endl
|
||||
<< "Gyro = "
|
||||
<< Torso_IMU.gyroscope()[0] << ", " << Torso_IMU.gyroscope()[1] << ", " << Torso_IMU.gyroscope()[2]
|
||||
<< std::endl;
|
||||
|
||||
// 等待遥控器A键按下
|
||||
std::cout << "Press button A on the remote control to continue!" << std::endl;
|
||||
while(true){
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
if(A_PRESSED){
|
||||
A_PRESSED = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
std::cout << "A Pressed!" << std::endl;
|
||||
|
||||
// 手臂摆动
|
||||
LowCmd_ dds_low_command;
|
||||
dds_low_command.mode_pr() = static_cast<uint8_t>(0); //构造底层控制指令
|
||||
dds_low_command.mode_machine() = mode_machine_;
|
||||
for (size_t i = 0; i < G1D_NUM_MOTOR; i++) {
|
||||
dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable
|
||||
dds_low_command.motor_cmd().at(i).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(i).q() = motor_state_buffer_.GetData() -> q.at(i);
|
||||
dds_low_command.motor_cmd().at(i).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(i).kp() = Kp.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kd() = Kd.at(i);
|
||||
}
|
||||
|
||||
double control_dt = 0.002;
|
||||
double max_P = M_PI * 30.0 / 180.0;
|
||||
double t = 0.0;
|
||||
|
||||
std::cout << "Press button A on the remote control to exit the program!" << std::endl;
|
||||
while(true){
|
||||
double L_Shoulder_des = max_P * std::sin(2.0 * M_PI * t);
|
||||
double R_Shoulder_des = -max_P * std::sin(2.0 * M_PI * t);
|
||||
|
||||
dds_low_command.motor_cmd().at(LeftShoulderPitch).q() = L_Shoulder_des;
|
||||
dds_low_command.motor_cmd().at(RightShoulderPitch).q() = R_Shoulder_des;
|
||||
|
||||
dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command, (sizeof(dds_low_command) >> 2) - 1);
|
||||
lowcmd_publisher -> Write(dds_low_command); //发布底层控制指令
|
||||
|
||||
t += control_dt;
|
||||
|
||||
// 按下遥控器A键或键盘Ctrl+C退出程序
|
||||
if(A_PRESSED){
|
||||
A_PRESSED = false;
|
||||
break;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
184
unitree_sdk2/example/g1/g1d/g1d_height_control.cpp
Normal file
184
unitree_sdk2/example/g1/g1d/g1d_height_control.cpp
Normal file
@@ -0,0 +1,184 @@
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <csignal>
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include <unitree/idl/ros2/Point32_.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
#include <unitree/robot/g1/agv/g1_agv_client.hpp>
|
||||
|
||||
static const std::string G1D_HISPEED_STATE_TOPIC = "rt/hispeed_state";
|
||||
|
||||
using namespace unitree::robot;
|
||||
using namespace geometry_msgs::msg::dds_;
|
||||
|
||||
volatile std::sig_atomic_t running = 1;
|
||||
std::atomic<bool> height_received{false};
|
||||
std::atomic<float> current_height{0.0F};
|
||||
std::atomic<int64_t> last_height_time_ms{0};
|
||||
|
||||
int64_t NowMs() {
|
||||
using namespace std::chrono;
|
||||
return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
|
||||
//Process Ctrl+C interrupt signal.
|
||||
void SignalHandler(int) {
|
||||
running = 0;
|
||||
}
|
||||
|
||||
void HispeedStateHandler(const void *message) {
|
||||
const Point32_ *hispeed_state = static_cast<const Point32_ *>(message);
|
||||
|
||||
current_height.store(hispeed_state->y(), std::memory_order_relaxed);
|
||||
last_height_time_ms.store(NowMs(), std::memory_order_release);
|
||||
height_received.store(true, std::memory_order_release);
|
||||
}
|
||||
|
||||
bool WaitForHeightData() {
|
||||
while (running != 0 && !height_received.load(std::memory_order_acquire)) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
|
||||
return running != 0;
|
||||
}
|
||||
|
||||
bool HeightDataTimeout() {
|
||||
const int64_t last_time = last_height_time_ms.load(std::memory_order_acquire);
|
||||
return last_time <= 0 || NowMs() - last_time > 500;
|
||||
}
|
||||
|
||||
bool ParseFloat(const char *text, float *value) {
|
||||
char *end = nullptr;
|
||||
errno = 0;
|
||||
const float parsed = std::strtof(text, &end);
|
||||
if (text == end || *end != '\0' || errno == ERANGE || !std::isfinite(parsed)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*value = parsed;
|
||||
return true;
|
||||
}
|
||||
|
||||
void StopHeightAdjust(g1::AgvClient &agv_client) {
|
||||
const int32_t ret = agv_client.HeightAdjust(0.0F);
|
||||
if (ret != 0) {
|
||||
std::cout << std::endl << "[WARN] HeightAdjust stop command failed, ret: " << ret << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
//Use PD controller to control the robot's height.
|
||||
void height_control(g1::AgvClient &agv_client, float z) {
|
||||
constexpr float kP = 20.0F;
|
||||
constexpr float kD = 0.3F;
|
||||
constexpr float kMaxVz = 0.8F;
|
||||
constexpr float kArriveTolerance = 0.001F;
|
||||
constexpr int kArriveStableCycles = 20;
|
||||
constexpr auto kControlPeriod = std::chrono::milliseconds(50);
|
||||
|
||||
if (!WaitForHeightData()) {
|
||||
StopHeightAdjust(agv_client);
|
||||
return;
|
||||
}
|
||||
|
||||
float previous_error = z - current_height.load(std::memory_order_relaxed);
|
||||
int arrive_count = 0;
|
||||
auto previous_time = std::chrono::steady_clock::now();
|
||||
|
||||
while (running != 0) {
|
||||
if (HeightDataTimeout()) {
|
||||
StopHeightAdjust(agv_client);
|
||||
std::cout << std::endl << "[ERROR] Height data timeout." << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
float dt = std::chrono::duration<float>(now - previous_time).count();
|
||||
if (dt <= 0.0F) {
|
||||
dt = std::chrono::duration<float>(kControlPeriod).count();
|
||||
}
|
||||
previous_time = now;
|
||||
|
||||
const float height = current_height.load(std::memory_order_relaxed);
|
||||
const float error = z - height;
|
||||
const float d_error = (error - previous_error) / dt;
|
||||
previous_error = error;
|
||||
|
||||
float vz = kP * error + kD * d_error;
|
||||
vz = std::clamp(vz, -kMaxVz, kMaxVz);
|
||||
|
||||
if (std::fabs(error) <= kArriveTolerance) {
|
||||
vz = 0.0F;
|
||||
++arrive_count;
|
||||
} else {
|
||||
arrive_count = 0;
|
||||
}
|
||||
|
||||
const int32_t ret = agv_client.HeightAdjust(vz);
|
||||
std::cout << "\rG1D column height: " << std::fixed << std::setprecision(4)
|
||||
<< height << " m"
|
||||
<< " | target: " << z << " m"
|
||||
<< " | error: " << error << " m"
|
||||
<< " | vz: " << vz << " " << std::flush;
|
||||
|
||||
if (ret != 0) {
|
||||
StopHeightAdjust(agv_client);
|
||||
std::cout << std::endl << "[ERROR] HeightAdjust failed, ret: " << ret << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (arrive_count >= kArriveStableCycles) {
|
||||
StopHeightAdjust(agv_client);
|
||||
std::cout << std::endl
|
||||
<< "Target height reached: " << std::fixed << std::setprecision(4)
|
||||
<< z << " m" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(kControlPeriod);
|
||||
}
|
||||
|
||||
StopHeightAdjust(agv_client);
|
||||
}
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 3) {
|
||||
std::cout << "Usage: g1d_height_control network_interface target_height_m" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::signal(SIGINT, SignalHandler);
|
||||
|
||||
const std::string network_interface = argv[1];
|
||||
float target_height = 0.0F;
|
||||
if (!ParseFloat(argv[2], &target_height)) {
|
||||
std::cout << "Invalid target_height_m: " << argv[2] << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
ChannelFactory::Instance()->Init(0, network_interface);
|
||||
|
||||
g1::AgvClient agv_client;
|
||||
agv_client.SetTimeout(3.0F);
|
||||
agv_client.Init();
|
||||
|
||||
ChannelSubscriberPtr<Point32_> hispeed_state_subscriber(
|
||||
new ChannelSubscriber<Point32_>(G1D_HISPEED_STATE_TOPIC));
|
||||
hispeed_state_subscriber->InitChannel(HispeedStateHandler, 1);
|
||||
|
||||
std::cout << "Subscribe topic: " << G1D_HISPEED_STATE_TOPIC << std::endl;
|
||||
std::cout << "Waiting for height data..." << std::endl;
|
||||
|
||||
height_control(agv_client, target_height);
|
||||
|
||||
std::cout << std::endl << "Exit g1d_height_control." << std::endl;
|
||||
return 0;
|
||||
}
|
||||
128
unitree_sdk2/example/g1/g1d/gamepad.hpp
Normal file
128
unitree_sdk2/example/g1/g1d/gamepad.hpp
Normal file
@@ -0,0 +1,128 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace unitree::common
|
||||
{
|
||||
|
||||
// bytecode mapping for raw joystick data
|
||||
// 16b
|
||||
typedef union
|
||||
{
|
||||
struct
|
||||
{
|
||||
uint8_t R1 : 1;
|
||||
uint8_t L1 : 1;
|
||||
uint8_t start : 1;
|
||||
uint8_t select : 1;
|
||||
uint8_t R2 : 1;
|
||||
uint8_t L2 : 1;
|
||||
uint8_t F1 : 1;
|
||||
uint8_t F2 : 1;
|
||||
uint8_t A : 1;
|
||||
uint8_t B : 1;
|
||||
uint8_t X : 1;
|
||||
uint8_t Y : 1;
|
||||
uint8_t up : 1;
|
||||
uint8_t right : 1;
|
||||
uint8_t down : 1;
|
||||
uint8_t left : 1;
|
||||
} components;
|
||||
uint16_t value;
|
||||
} xKeySwitchUnion;
|
||||
|
||||
// 40 Byte (now used 24B)
|
||||
typedef struct
|
||||
{
|
||||
uint8_t head[2];
|
||||
xKeySwitchUnion btn;
|
||||
float lx;
|
||||
float rx;
|
||||
float ry;
|
||||
float L2;
|
||||
float ly;
|
||||
|
||||
uint8_t idle[16];
|
||||
} xRockerBtnDataStruct;
|
||||
|
||||
typedef union
|
||||
{
|
||||
xRockerBtnDataStruct RF_RX;
|
||||
uint8_t buff[40];
|
||||
} REMOTE_DATA_RX;
|
||||
|
||||
class Button
|
||||
{
|
||||
public:
|
||||
Button() {}
|
||||
|
||||
void update(bool state)
|
||||
{
|
||||
on_press = state ? state != pressed : false;
|
||||
on_release = state ? false : state != pressed;
|
||||
pressed = state;
|
||||
}
|
||||
|
||||
bool pressed = false;
|
||||
bool on_press = false;
|
||||
bool on_release = false;
|
||||
};
|
||||
|
||||
class Gamepad
|
||||
{
|
||||
public:
|
||||
Gamepad() {}
|
||||
|
||||
void update(xRockerBtnDataStruct &key_data)
|
||||
{
|
||||
lx = lx * (1 - smooth) + (std::fabs(key_data.lx) < dead_zone ? 0.0 : key_data.lx) * smooth;
|
||||
rx = rx * (1 - smooth) + (std::fabs(key_data.rx) < dead_zone ? 0.0 : key_data.rx) * smooth;
|
||||
ry = ry * (1 - smooth) + (std::fabs(key_data.ry) < dead_zone ? 0.0 : key_data.ry) * smooth;
|
||||
l2 = l2 * (1 - smooth) + (std::fabs(key_data.L2) < dead_zone ? 0.0 : key_data.L2) * smooth;
|
||||
ly = ly * (1 - smooth) + (std::fabs(key_data.ly) < dead_zone ? 0.0 : key_data.ly) * smooth;
|
||||
|
||||
R1.update(key_data.btn.components.R1);
|
||||
L1.update(key_data.btn.components.L1);
|
||||
start.update(key_data.btn.components.start);
|
||||
select.update(key_data.btn.components.select);
|
||||
R2.update(key_data.btn.components.R2);
|
||||
L2.update(key_data.btn.components.L2);
|
||||
F1.update(key_data.btn.components.F1);
|
||||
F2.update(key_data.btn.components.F2);
|
||||
A.update(key_data.btn.components.A);
|
||||
B.update(key_data.btn.components.B);
|
||||
X.update(key_data.btn.components.X);
|
||||
Y.update(key_data.btn.components.Y);
|
||||
up.update(key_data.btn.components.up);
|
||||
right.update(key_data.btn.components.right);
|
||||
down.update(key_data.btn.components.down);
|
||||
left.update(key_data.btn.components.left);
|
||||
}
|
||||
|
||||
float lx = 0.;
|
||||
float rx = 0.;
|
||||
float ry = 0.;
|
||||
float l2 = 0.;
|
||||
float ly = 0.;
|
||||
|
||||
float smooth = 0.03;
|
||||
float dead_zone = 0.01;
|
||||
|
||||
Button R1;
|
||||
Button L1;
|
||||
Button start;
|
||||
Button select;
|
||||
Button R2;
|
||||
Button L2;
|
||||
Button F1;
|
||||
Button F2;
|
||||
Button A;
|
||||
Button B;
|
||||
Button X;
|
||||
Button Y;
|
||||
Button up;
|
||||
Button right;
|
||||
Button down;
|
||||
Button left;
|
||||
};
|
||||
} // namespace unitree::common
|
||||
256
unitree_sdk2/example/g1/high_level/g1_arm5_sdk_dds_example.cpp
Normal file
256
unitree_sdk2/example/g1/high_level/g1_arm5_sdk_dds_example.cpp
Normal file
@@ -0,0 +1,256 @@
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
static const std::string kTopicArmSDK = "rt/arm_sdk";
|
||||
static const std::string kTopicState = "rt/lowstate";
|
||||
constexpr float kPi = 3.141592654;
|
||||
constexpr float kPi_2 = 1.57079632;
|
||||
|
||||
enum JointIndex {
|
||||
// Left leg
|
||||
kLeftHipPitch = 0,
|
||||
kLeftHipRoll = 1,
|
||||
kLeftHipYaw = 2,
|
||||
kLeftKnee = 3,
|
||||
kLeftAnkle = 4,
|
||||
kLeftAnkleRoll = 5,
|
||||
|
||||
// Right leg
|
||||
kRightHipPitch = 6,
|
||||
kRightHipRoll = 7,
|
||||
kRightHipYaw = 8,
|
||||
kRightKnee = 9,
|
||||
kRightAnkle = 10,
|
||||
kRightAnkleRoll = 11,
|
||||
|
||||
kWaistYaw = 12,
|
||||
kWaistRoll = 13,
|
||||
kWaistPitch = 14,
|
||||
|
||||
// Left arm
|
||||
kLeftShoulderPitch = 15,
|
||||
kLeftShoulderRoll = 16,
|
||||
kLeftShoulderYaw = 17,
|
||||
kLeftElbowPitch = 18,
|
||||
kLeftElbowRoll = 19,
|
||||
|
||||
// Right arm
|
||||
kRightShoulderPitch = 22,
|
||||
kRightShoulderRoll = 23,
|
||||
kRightShoulderYaw = 24,
|
||||
kRightElbowPitch = 25,
|
||||
kRightElbowRoll = 26,
|
||||
|
||||
kNotUsedJoint = 29,
|
||||
kNotUsedJoint1 = 30,
|
||||
kNotUsedJoint2 = 31,
|
||||
kNotUsedJoint3 = 32,
|
||||
kNotUsedJoint4 = 33,
|
||||
kNotUsedJoint5 = 34
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
|
||||
|
||||
unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
|
||||
arm_sdk_publisher;
|
||||
unitree_hg::msg::dds_::LowCmd_ msg;
|
||||
|
||||
arm_sdk_publisher.reset(
|
||||
new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
|
||||
kTopicArmSDK));
|
||||
arm_sdk_publisher->InitChannel();
|
||||
|
||||
unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
|
||||
low_state_subscriber;
|
||||
|
||||
// create subscriber
|
||||
unitree_hg::msg::dds_::LowState_ state_msg;
|
||||
low_state_subscriber.reset(
|
||||
new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
|
||||
kTopicState));
|
||||
low_state_subscriber->InitChannel([&](const void *msg) {
|
||||
auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
|
||||
memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
|
||||
}, 1);
|
||||
|
||||
std::array<JointIndex, 13> arm_joints = {
|
||||
JointIndex::kLeftShoulderPitch, JointIndex::kLeftShoulderRoll,
|
||||
JointIndex::kLeftShoulderYaw, JointIndex::kLeftElbowPitch,
|
||||
JointIndex::kLeftElbowRoll,
|
||||
JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
|
||||
JointIndex::kRightShoulderYaw, JointIndex::kRightElbowPitch,
|
||||
JointIndex::kRightElbowRoll,
|
||||
JointIndex::kWaistYaw,
|
||||
JointIndex::kWaistRoll,
|
||||
JointIndex::kWaistPitch};
|
||||
|
||||
float weight = 0.f;
|
||||
float weight_rate = 0.2f;
|
||||
|
||||
float kp = 60.f;
|
||||
float kd = 1.5f;
|
||||
float dq = 0.f;
|
||||
float tau_ff = 0.f;
|
||||
|
||||
float control_dt = 0.02f;
|
||||
float max_joint_velocity = 0.5f;
|
||||
|
||||
float delta_weight = weight_rate * control_dt;
|
||||
float max_joint_delta = max_joint_velocity * control_dt;
|
||||
auto sleep_time =
|
||||
std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
|
||||
|
||||
std::array<float, 13> init_pos{0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0};
|
||||
|
||||
std::array<float, 13> target_pos = {0.f, kPi_2, 0.f, kPi_2, 0.f,
|
||||
0.f, -kPi_2, 0.f, kPi_2, 0.f,
|
||||
0.f, 0.f, 0.f};
|
||||
|
||||
// wait for init
|
||||
std::cout << "Press ENTER to init arms ...";
|
||||
std::cin.get();
|
||||
|
||||
// get current joint position
|
||||
std::array<float, 13> current_jpos{};
|
||||
std::cout<<"Current joint position: ";
|
||||
for (int i = 0; i < arm_joints.size(); ++i) {
|
||||
current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
|
||||
std::cout << current_jpos.at(i) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// set init pos
|
||||
std::cout << "Initailizing arms ...";
|
||||
float init_time = 2.0f;
|
||||
int init_time_steps = static_cast<int>(init_time / control_dt);
|
||||
|
||||
for (int i = 0; i < init_time_steps; ++i) {
|
||||
// set weight
|
||||
weight = 1.0;
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
|
||||
float phase = 1.0 * i / init_time_steps;
|
||||
std::cout << "Phase: " << phase << std::endl;
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
// wait for control
|
||||
std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
|
||||
std::cin.get();
|
||||
|
||||
// start control
|
||||
std::cout << "Start arm ctrl!" << std::endl;
|
||||
float period = 5.f;
|
||||
int num_time_steps = static_cast<int>(period / control_dt);
|
||||
|
||||
std::array<float, 13> current_jpos_des{};
|
||||
|
||||
// lift arms up
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(target_pos.at(j) - current_jpos_des.at(j),
|
||||
-max_joint_delta, max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// put arms down
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
|
||||
max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// stop control
|
||||
std::cout << "Stoping arm ctrl ...";
|
||||
float stop_time = 2.0f;
|
||||
int stop_time_steps = static_cast<int>(stop_time / control_dt);
|
||||
|
||||
for (int i = 0; i < stop_time_steps; ++i) {
|
||||
// increase weight
|
||||
weight -= delta_weight;
|
||||
weight = std::clamp(weight, 0.f, 1.f);
|
||||
|
||||
// set weight
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// set weight
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(0);
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
262
unitree_sdk2/example/g1/high_level/g1_arm7_sdk_dds_example.cpp
Normal file
262
unitree_sdk2/example/g1/high_level/g1_arm7_sdk_dds_example.cpp
Normal file
@@ -0,0 +1,262 @@
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
static const std::string kTopicArmSDK = "rt/arm_sdk";
|
||||
static const std::string kTopicState = "rt/lowstate";
|
||||
constexpr float kPi = 3.141592654;
|
||||
constexpr float kPi_2 = 1.57079632;
|
||||
|
||||
enum JointIndex {
|
||||
// Left leg
|
||||
kLeftHipPitch,
|
||||
kLeftHipRoll,
|
||||
kLeftHipYaw,
|
||||
kLeftKnee,
|
||||
kLeftAnkle,
|
||||
kLeftAnkleRoll,
|
||||
|
||||
// Right leg
|
||||
kRightHipPitch,
|
||||
kRightHipRoll,
|
||||
kRightHipYaw,
|
||||
kRightKnee,
|
||||
kRightAnkle,
|
||||
kRightAnkleRoll,
|
||||
|
||||
kWaistYaw,
|
||||
kWaistRoll,
|
||||
kWaistPitch,
|
||||
|
||||
// Left arm
|
||||
kLeftShoulderPitch,
|
||||
kLeftShoulderRoll,
|
||||
kLeftShoulderYaw,
|
||||
kLeftElbow,
|
||||
kLeftWristRoll,
|
||||
kLeftWristPitch,
|
||||
kLeftWristYaw,
|
||||
// Right arm
|
||||
kRightShoulderPitch,
|
||||
kRightShoulderRoll,
|
||||
kRightShoulderYaw,
|
||||
kRightElbow,
|
||||
kRightWristRoll,
|
||||
kRightWristPitch,
|
||||
kRightWristYaw,
|
||||
|
||||
kNotUsedJoint,
|
||||
kNotUsedJoint1,
|
||||
kNotUsedJoint2,
|
||||
kNotUsedJoint3,
|
||||
kNotUsedJoint4,
|
||||
kNotUsedJoint5
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
|
||||
|
||||
unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
|
||||
arm_sdk_publisher;
|
||||
unitree_hg::msg::dds_::LowCmd_ msg;
|
||||
|
||||
arm_sdk_publisher.reset(
|
||||
new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
|
||||
kTopicArmSDK));
|
||||
arm_sdk_publisher->InitChannel();
|
||||
|
||||
unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
|
||||
low_state_subscriber;
|
||||
|
||||
// create subscriber
|
||||
unitree_hg::msg::dds_::LowState_ state_msg;
|
||||
low_state_subscriber.reset(
|
||||
new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
|
||||
kTopicState));
|
||||
low_state_subscriber->InitChannel([&](const void *msg) {
|
||||
auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
|
||||
memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
|
||||
}, 1);
|
||||
|
||||
std::array<JointIndex, 17> arm_joints = {
|
||||
JointIndex::kLeftShoulderPitch, JointIndex::kLeftShoulderRoll,
|
||||
JointIndex::kLeftShoulderYaw, JointIndex::kLeftElbow,
|
||||
JointIndex::kLeftWristRoll, JointIndex::kLeftWristPitch,
|
||||
JointIndex::kLeftWristYaw,
|
||||
JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
|
||||
JointIndex::kRightShoulderYaw, JointIndex::kRightElbow,
|
||||
JointIndex::kRightWristRoll, JointIndex::kRightWristPitch,
|
||||
JointIndex::kRightWristYaw,
|
||||
JointIndex::kWaistYaw,
|
||||
JointIndex::kWaistRoll,
|
||||
JointIndex::kWaistPitch};
|
||||
|
||||
float weight = 0.f;
|
||||
float weight_rate = 0.2f;
|
||||
|
||||
float kp = 60.f;
|
||||
float kd = 1.5f;
|
||||
float dq = 0.f;
|
||||
float tau_ff = 0.f;
|
||||
|
||||
float control_dt = 0.02f;
|
||||
float max_joint_velocity = 0.5f;
|
||||
|
||||
float delta_weight = weight_rate * control_dt;
|
||||
float max_joint_delta = max_joint_velocity * control_dt;
|
||||
auto sleep_time =
|
||||
std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
|
||||
|
||||
std::array<float, 17> init_pos{0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0};
|
||||
|
||||
std::array<float, 17> target_pos = {0.f, kPi_2, 0.f, kPi_2, 0.f, 0.f, 0.f,
|
||||
0.f, -kPi_2, 0.f, kPi_2, 0.f, 0.f, 0.f,
|
||||
0, 0, 0};
|
||||
|
||||
// wait for init
|
||||
std::cout << "Press ENTER to init arms ...";
|
||||
std::cin.get();
|
||||
|
||||
// get current joint position
|
||||
std::array<float, 17> current_jpos{};
|
||||
std::cout<<"Current joint position: ";
|
||||
for (int i = 0; i < arm_joints.size(); ++i) {
|
||||
current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
|
||||
std::cout << current_jpos.at(i) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// set init pos
|
||||
std::cout << "Initailizing arms ...";
|
||||
float init_time = 2.0f;
|
||||
int init_time_steps = static_cast<int>(init_time / control_dt);
|
||||
|
||||
for (int i = 0; i < init_time_steps; ++i) {
|
||||
// increase weight
|
||||
weight = 1.0;
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
|
||||
float phase = 1.0 * i / init_time_steps;
|
||||
std::cout << "Phase: " << phase << std::endl;
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
// wait for control
|
||||
std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
|
||||
std::cin.get();
|
||||
|
||||
// start control
|
||||
std::cout << "Start arm ctrl!" << std::endl;
|
||||
float period = 5.f;
|
||||
int num_time_steps = static_cast<int>(period / control_dt);
|
||||
|
||||
std::array<float, 17> current_jpos_des{};
|
||||
|
||||
// lift arms up
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(target_pos.at(j) - current_jpos_des.at(j),
|
||||
-max_joint_delta, max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// put arms down
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
|
||||
max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// stop control
|
||||
std::cout << "Stoping arm ctrl ...";
|
||||
float stop_time = 2.0f;
|
||||
int stop_time_steps = static_cast<int>(stop_time / control_dt);
|
||||
|
||||
for (int i = 0; i < stop_time_steps; ++i) {
|
||||
// increase weight
|
||||
weight -= delta_weight;
|
||||
weight = std::clamp(weight, 0.f, 1.f);
|
||||
|
||||
// set weight
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
|
||||
// set weight
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(0);
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
85
unitree_sdk2/example/g1/high_level/g1_arm_action_example.cpp
Normal file
85
unitree_sdk2/example/g1/high_level/g1_arm_action_example.cpp
Normal file
@@ -0,0 +1,85 @@
|
||||
/**
|
||||
* @file g1_arm_action_example.cpp
|
||||
* @brief This example demonstrates how to use the G1 Arm Action Client to execute predefined arm actions.
|
||||
*/
|
||||
#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
|
||||
#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
|
||||
#include <boost/program_options.hpp>
|
||||
|
||||
namespace po = boost::program_options;
|
||||
using namespace unitree::robot;
|
||||
using namespace unitree::robot::g1;
|
||||
|
||||
int main(int argc, const char** argv)
|
||||
{
|
||||
std::cout << " --- Unitree Robotics --- \n";
|
||||
std::cout << " G1 Arm Action Example \n\n";
|
||||
|
||||
// Parse command line arguments
|
||||
po::options_description desc("Unitree G1 Arm Action Example.");
|
||||
desc.add_options()
|
||||
("help,h", "show help message")
|
||||
("network,n", po::value<std::string>()->default_value(""), "dds network interface")
|
||||
("list,l", "list all supported actions")
|
||||
("id,i", po::value<int>(), "action id to execute, 0 to list all supported actions")
|
||||
("name", po::value<std::string>(), "custom action name to execute")
|
||||
("stop", "stop the current custom action")
|
||||
;
|
||||
po::variables_map vm;
|
||||
po::store(po::parse_command_line(argc, argv, desc), vm);
|
||||
po::notify(vm);
|
||||
if(argc < 2 || vm.count("help")) {
|
||||
std::cout << desc << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// DDS Init
|
||||
ChannelFactory::Instance()->Init(0, vm["network"].as<std::string>());
|
||||
|
||||
auto client = std::make_shared<G1ArmActionClient>();
|
||||
client->Init();
|
||||
// Attention: unitree actions are all less than 10s,
|
||||
// but the custom actions may be longer.
|
||||
client->SetTimeout(10.f);
|
||||
|
||||
if(vm.count("list") || (vm.count("id") && vm["id"].as<int>() == 0)){
|
||||
std::string action_list_data;
|
||||
int32_t ret = client->GetActionList(action_list_data);
|
||||
if (ret != 0) {
|
||||
std::cerr << "Failed to get action list, error code: " << ret << "\n";
|
||||
}
|
||||
std::cout << "Available actions:\n" << action_list_data << std::endl;
|
||||
} else if (vm.count("id")) {
|
||||
int32_t ret = client->ExecuteAction(vm["id"].as<int>());
|
||||
if(ret != 0) {
|
||||
switch (ret)
|
||||
{
|
||||
case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
|
||||
std::cout << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC << std::endl;
|
||||
break;
|
||||
case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
|
||||
std::cout << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << std::endl;
|
||||
break;
|
||||
case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
|
||||
std::cout << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << std::endl;
|
||||
break;
|
||||
case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
|
||||
std::cout << "The actions are only supported in fsm id {500, 501, 801}" << std::endl;
|
||||
std::cout << "You can subscribe the topic rt/sportmodestate to check the fsm id." << std::endl;
|
||||
std::cout << "And in the state 801, the actions are only supported in the fsm mode {0, 3}." << std::endl;
|
||||
std::cout << "If an error is still returned at this point, ignore this action.";
|
||||
break;
|
||||
default:
|
||||
std::cerr << "Execute action failed, error code: " << ret << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else if (vm.count("name")) {
|
||||
int32_t ret = client->ExecuteAction(vm["name"].as<std::string>());
|
||||
if(ret != 0) std::cout << "Execute custom action failed, error code: " << ret << std::endl;
|
||||
} else if (vm.count("stop")) {
|
||||
int32_t ret = client->StopCustomAction();
|
||||
}
|
||||
|
||||
return 0;
|
||||
};
|
||||
76
unitree_sdk2/example/g1/high_level/g1_hand_sdk_example.cpp
Normal file
76
unitree_sdk2/example/g1/high_level/g1_hand_sdk_example.cpp
Normal file
@@ -0,0 +1,76 @@
|
||||
// G1 Hand SDK Control Example
|
||||
//
|
||||
// Motor_real = weight * Hand_SDK + (1 - weight) * G1_Cmd
|
||||
//
|
||||
// Demonstrates how a user process injects commands for the 4 hand motors
|
||||
// into ai_sport via the DDS topic `rt/hand_sdk`. This example takes full
|
||||
// control of the hand (weight = 1.0) and toggles tau between +0.3 and
|
||||
// -0.3 every second to alternate between "close" and "open".
|
||||
//
|
||||
//
|
||||
// Run:
|
||||
// ./hand_sdk_example # use default network interface
|
||||
// ./hand_sdk_example eth0 # specify a network interface
|
||||
|
||||
#include <unitree/robot/channel/channel_factory.hpp>
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/idl/go2/MotorCmds_.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
class HandSdk {
|
||||
public:
|
||||
static constexpr int kMotorNum = 4;
|
||||
|
||||
explicit HandSdk(const std::string& topic = "rt/hand_sdk")
|
||||
: publisher_(std::make_shared<unitree::robot::ChannelPublisher<
|
||||
unitree_go::msg::dds_::MotorCmds_>>(topic)) {
|
||||
publisher_->InitChannel();
|
||||
msg_.cmds().resize(kMotorNum);
|
||||
}
|
||||
|
||||
// weight is in [0, 1] and is encoded as `weight * 100` (uint8) in cmds[0].mode.
|
||||
float weight() const {
|
||||
return std::clamp(msg_.cmds()[0].mode() / 100.f, 0.f, 1.f);
|
||||
}
|
||||
void set_weight(float w) {
|
||||
msg_.cmds()[0].mode(
|
||||
static_cast<uint8_t>(std::clamp(w, 0.f, 1.f) * 100.f));
|
||||
}
|
||||
|
||||
// Positive tau closes the hand, negative tau opens it.
|
||||
void set_tau(float tau) {
|
||||
for (int i = 0; i < kMotorNum; ++i) {
|
||||
msg_.cmds()[i].tau(tau);
|
||||
}
|
||||
}
|
||||
|
||||
void write() { publisher_->Write(msg_); }
|
||||
|
||||
private:
|
||||
std::shared_ptr<unitree::robot::ChannelPublisher<
|
||||
unitree_go::msg::dds_::MotorCmds_>> publisher_;
|
||||
unitree_go::msg::dds_::MotorCmds_ msg_;
|
||||
};
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc > 1) {
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
|
||||
} else {
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0);
|
||||
}
|
||||
|
||||
HandSdk hand_sdk;
|
||||
hand_sdk.set_weight(1.0f);
|
||||
|
||||
float tau = 0.3f;
|
||||
while (true) {
|
||||
tau = -tau;
|
||||
hand_sdk.set_tau(tau);
|
||||
hand_sdk.write();
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
273
unitree_sdk2/example/g1/high_level/g1_loco_client_example.cpp
Normal file
273
unitree_sdk2/example/g1/high_level/g1_loco_client_example.cpp
Normal file
@@ -0,0 +1,273 @@
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include <unitree/robot/g1/loco/g1_loco_api.hpp>
|
||||
#include <unitree/robot/g1/loco/g1_loco_client.hpp>
|
||||
|
||||
std::vector<float> stringToFloatVector(const std::string &str) {
|
||||
std::vector<float> result;
|
||||
std::stringstream ss(str);
|
||||
float num;
|
||||
while (ss >> num) {
|
||||
result.push_back(num);
|
||||
// ignore any trailing whitespace
|
||||
ss.ignore();
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
std::map<std::string, std::string> args = {{"network_interface", "lo"}};
|
||||
|
||||
std::map<std::string, std::string> values;
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
std::string arg = argv[i];
|
||||
if (arg.substr(0, 2) == "--") {
|
||||
size_t pos = arg.find("=");
|
||||
std::string key, value;
|
||||
if (pos != std::string::npos) {
|
||||
key = arg.substr(2, pos - 2);
|
||||
value = arg.substr(pos + 1);
|
||||
|
||||
if (value.front() == '"' && value.back() == '"') {
|
||||
value = value.substr(1, value.length() - 2);
|
||||
}
|
||||
} else {
|
||||
key = arg.substr(2);
|
||||
value = "";
|
||||
}
|
||||
if (args.find(key) != args.end()) {
|
||||
args[key] = value;
|
||||
} else {
|
||||
args.insert({{key, value}});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0,
|
||||
args["network_interface"]);
|
||||
|
||||
unitree::robot::g1::LocoClient client;
|
||||
|
||||
client.Init();
|
||||
client.SetTimeout(10.f);
|
||||
|
||||
for (const auto &arg_pair : args) {
|
||||
std::cout << "Processing command: [" << arg_pair.first << "] with param: ["
|
||||
<< arg_pair.second << "] ..." << std::endl;
|
||||
if (arg_pair.first == "network_interface") {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_fsm_id") {
|
||||
int fsm_id;
|
||||
client.GetFsmId(fsm_id);
|
||||
std::cout << "current fsm_id: " << fsm_id << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_fsm_mode") {
|
||||
int fsm_mode;
|
||||
client.GetFsmMode(fsm_mode);
|
||||
std::cout << "current fsm_mode: " << fsm_mode << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_balance_mode") {
|
||||
int balance_mode;
|
||||
client.GetBalanceMode(balance_mode);
|
||||
std::cout << "current balance_mode: " << balance_mode << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_swing_height") {
|
||||
float swing_height;
|
||||
client.GetSwingHeight(swing_height);
|
||||
std::cout << "current swing_height: " << swing_height << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_stand_height") {
|
||||
float stand_height;
|
||||
client.GetStandHeight(stand_height);
|
||||
std::cout << "current stand_height: " << stand_height << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_phase") {
|
||||
std::vector<float> phase;
|
||||
client.GetPhase(phase);
|
||||
std::cout << "current phase: (";
|
||||
for (const auto &p : phase) {
|
||||
std::cout << p << ", ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_fsm_id") {
|
||||
int fsm_id = std::stoi(arg_pair.second);
|
||||
client.SetFsmId(fsm_id);
|
||||
std::cout << "set fsm_id to " << fsm_id << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_balance_mode") {
|
||||
int balance_mode = std::stoi(arg_pair.second);
|
||||
client.SetBalanceMode(balance_mode);
|
||||
std::cout << "set balance_mode to " << balance_mode << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_swing_height") {
|
||||
float swing_height = std::stof(arg_pair.second);
|
||||
client.SetSwingHeight(swing_height);
|
||||
std::cout << "set swing_height to " << swing_height << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_stand_height") {
|
||||
float stand_height = std::stof(arg_pair.second);
|
||||
client.SetStandHeight(stand_height);
|
||||
std::cout << "set stand_height to " << stand_height << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_velocity") {
|
||||
std::vector<float> param = stringToFloatVector(arg_pair.second);
|
||||
auto param_size = param.size();
|
||||
float vx, vy, omega, duration;
|
||||
if (param_size == 3) {
|
||||
vx = param.at(0);
|
||||
vy = param.at(1);
|
||||
omega = param.at(2);
|
||||
duration = 1.f;
|
||||
} else if (param_size == 4) {
|
||||
vx = param.at(0);
|
||||
vy = param.at(1);
|
||||
omega = param.at(2);
|
||||
duration = param.at(3);
|
||||
} else {
|
||||
std::cerr << "Invalid param size for method SetVelocity: " << param_size
|
||||
<< std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
client.SetVelocity(vx, vy, omega, duration);
|
||||
std::cout << "set velocity to " << arg_pair.second << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "damp") {
|
||||
client.Damp();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "start") {
|
||||
client.Start();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "squat") {
|
||||
client.Squat();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "sit") {
|
||||
client.Sit();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "stand_up") {
|
||||
client.StandUp();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "zero_torque") {
|
||||
client.ZeroTorque();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "stop_move") {
|
||||
client.StopMove();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "high_stand") {
|
||||
client.HighStand();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "low_stand") {
|
||||
client.LowStand();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "balance_stand") {
|
||||
client.BalanceStand();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "continous_gait") {
|
||||
bool flag;
|
||||
if (arg_pair.second == "true") {
|
||||
flag = true;
|
||||
} else if (arg_pair.second == "false") {
|
||||
flag = false;
|
||||
} else {
|
||||
std::cerr << "invalid argument: " << arg_pair.second << std::endl;
|
||||
return 1;
|
||||
}
|
||||
client.ContinuousGait(flag);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "switch_move_mode") {
|
||||
bool flag;
|
||||
if (arg_pair.second == "true") {
|
||||
flag = true;
|
||||
} else if (arg_pair.second == "false") {
|
||||
flag = false;
|
||||
} else {
|
||||
std::cerr << "invalid argument: " << arg_pair.second << std::endl;
|
||||
return 1;
|
||||
}
|
||||
client.SwitchMoveMode(flag);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "move") {
|
||||
std::vector<float> param = stringToFloatVector(arg_pair.second);
|
||||
auto param_size = param.size();
|
||||
float vx, vy, omega;
|
||||
if (param_size == 3) {
|
||||
vx = param.at(0);
|
||||
vy = param.at(1);
|
||||
omega = param.at(2);
|
||||
} else {
|
||||
std::cerr << "Invalid param size for method SetVelocity: " << param_size
|
||||
<< std::endl;
|
||||
return 1;
|
||||
}
|
||||
client.Move(vx, vy, omega);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_task_id") {
|
||||
int task_id = std::stoi(arg_pair.second);
|
||||
client.SetTaskId(task_id);
|
||||
std::cout << "set task_id to " << task_id << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "shake_hand") {
|
||||
client.ShakeHand(0);
|
||||
std::cout << "Shake hand starts! Waiting for 10 s for ending"
|
||||
<< std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::seconds(10));
|
||||
std::cout << "Shake hand ends!" << std::endl;
|
||||
client.ShakeHand(1);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "wave_hand") {
|
||||
client.WaveHand();
|
||||
std::cout << "wave hand" << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "wave_hand_with_turn") {
|
||||
client.WaveHand(true);
|
||||
std::cout << "wave hand with turn" << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_speed_mode") {
|
||||
client.SetSpeedMode(std::stoi(arg_pair.second));
|
||||
std::cout << "set speed mode" << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_mimic_motion") {
|
||||
std::string data;
|
||||
client.GetMimicMotion(data);
|
||||
std::cout << "Mimic motion: " << data << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
248
unitree_sdk2/example/g1/high_level/g1_userctrl_dds_example.cpp
Normal file
248
unitree_sdk2/example/g1/high_level/g1_userctrl_dds_example.cpp
Normal file
@@ -0,0 +1,248 @@
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
#include "unitree/robot/g1/loco/g1_loco_api.hpp"
|
||||
#include "unitree/robot/g1/loco/g1_loco_client.hpp"
|
||||
|
||||
static const std::string kTopicUserCtrl = "rt/user_lowcmd";
|
||||
static const std::string kTopicState = "rt/lowstate";
|
||||
|
||||
enum JointIndex {
|
||||
// Left leg
|
||||
kLeftHipPitch,
|
||||
kLeftHipRoll,
|
||||
kLeftHipYaw,
|
||||
kLeftKnee,
|
||||
kLeftAnkle,
|
||||
kLeftAnkleRoll,
|
||||
|
||||
// Right leg
|
||||
kRightHipPitch,
|
||||
kRightHipRoll,
|
||||
kRightHipYaw,
|
||||
kRightKnee,
|
||||
kRightAnkle,
|
||||
kRightAnkleRoll,
|
||||
|
||||
kWaistYaw,
|
||||
kWaistRoll,
|
||||
kWaistPitch,
|
||||
|
||||
// Left arm
|
||||
kLeftShoulderPitch,
|
||||
kLeftShoulderRoll,
|
||||
kLeftShoulderYaw,
|
||||
kLeftElbow,
|
||||
kLeftWristRoll,
|
||||
kLeftWristPitch,
|
||||
kLeftWristYaw,
|
||||
// Right arm
|
||||
kRightShoulderPitch,
|
||||
kRightShoulderRoll,
|
||||
kRightShoulderYaw,
|
||||
kRightElbow,
|
||||
kRightWristRoll,
|
||||
kRightWristPitch,
|
||||
kRightWristYaw,
|
||||
|
||||
kNotUsedJoint,
|
||||
kNotUsedJoint1,
|
||||
kNotUsedJoint2,
|
||||
kNotUsedJoint3,
|
||||
kNotUsedJoint4,
|
||||
kNotUsedJoint5
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
|
||||
|
||||
unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
|
||||
user_ctrl_publisher;
|
||||
unitree_hg::msg::dds_::LowCmd_ msg;
|
||||
|
||||
user_ctrl_publisher.reset(
|
||||
new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
|
||||
kTopicUserCtrl));
|
||||
user_ctrl_publisher->InitChannel();
|
||||
|
||||
unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
|
||||
low_state_subscriber;
|
||||
|
||||
// create subscriber
|
||||
unitree_hg::msg::dds_::LowState_ state_msg;
|
||||
low_state_subscriber.reset(
|
||||
new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
|
||||
kTopicState));
|
||||
low_state_subscriber->InitChannel([&](const void *msg) {
|
||||
auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
|
||||
memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
|
||||
}, 1);
|
||||
|
||||
unitree::robot::g1::LocoClient client;
|
||||
client.Init();
|
||||
client.SetTimeout(5.f);
|
||||
int current_fsm_id;
|
||||
client.GetFsmId(current_fsm_id);
|
||||
if(current_fsm_id != 1){
|
||||
std::cout << "Current fsm is not PASSIVE, exitting ..." <<std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
float kp = 60.f;
|
||||
float kd = 1.5f;
|
||||
float dq = 0.f;
|
||||
float tau_ff = 0.f;
|
||||
|
||||
float control_dt = 0.02f;
|
||||
float max_joint_velocity = 0.5f;
|
||||
|
||||
float max_joint_delta = max_joint_velocity * control_dt;
|
||||
auto sleep_time =
|
||||
std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
|
||||
|
||||
std::array<float, 29> init_pos{-0.1, 0, 0, 0.3, -0.2, 0,
|
||||
-0.1, 0, 0, 0.3, -0.2, 0,
|
||||
0, 0, 0,
|
||||
0.2, 0.2, 0, 0.9, 0, 0, 0,
|
||||
0.2, -0.2, 0, 0.9, 0, 0, 0};
|
||||
|
||||
std::array<float, 29> target_pos = {-1.0, 0, 0, 1.5, 0.2, 0,
|
||||
-1.0, 0, 0, 1.5, 0.2, 0,
|
||||
0, 0, 0,
|
||||
0, 0.2, 0, 0, 0, 0, 0,
|
||||
0, -0.2, 0, 0, 0, 0, 0};
|
||||
|
||||
// wait for init
|
||||
std::cout << "Press ENTER to init user control ...";
|
||||
std::cin.get();
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(j).q(0);
|
||||
msg.motor_cmd().at(j).dq(0);
|
||||
msg.motor_cmd().at(j).kp(0);
|
||||
msg.motor_cmd().at(j).kd(kd);
|
||||
msg.motor_cmd().at(j).tau(0);
|
||||
}
|
||||
user_ctrl_publisher->Write(msg);
|
||||
|
||||
client.SwitchToUserCtrl();
|
||||
|
||||
// get current joint position
|
||||
std::array<float, 29> current_jpos{};
|
||||
std::cout<<"Current joint position: ";
|
||||
for (int i = 0; i < init_pos.size(); ++i) {
|
||||
current_jpos.at(i) = state_msg.motor_state().at(i).q();
|
||||
std::cout << current_jpos.at(i) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// set init pos
|
||||
std::cout << "Initailizing motors ...";
|
||||
float init_time = 2.0f;
|
||||
int init_time_steps = static_cast<int>(init_time / control_dt);
|
||||
|
||||
for (int i = 0; i < init_time_steps; ++i) {
|
||||
|
||||
float phase = 1.0 * i / init_time_steps;
|
||||
std::cout << "Phase: " << phase << std::endl;
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(j).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
|
||||
msg.motor_cmd().at(j).dq(dq);
|
||||
msg.motor_cmd().at(j).kp(kp);
|
||||
msg.motor_cmd().at(j).kd(kd);
|
||||
msg.motor_cmd().at(j).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
user_ctrl_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
// wait for control
|
||||
std::cout << "Press ENTER to start user control ..." << std::endl;
|
||||
std::cin.get();
|
||||
|
||||
// start control
|
||||
std::cout << "Start user control!" << std::endl;
|
||||
float period = 5.f;
|
||||
int num_time_steps = static_cast<int>(period / control_dt);
|
||||
|
||||
std::array<float, 29> current_jpos_des{};
|
||||
for (int i = 0; i < init_pos.size(); ++i) {
|
||||
current_jpos_des.at(i) = state_msg.motor_state().at(i).q();
|
||||
}
|
||||
|
||||
// lift arms up
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(target_pos.at(j) - current_jpos_des.at(j),
|
||||
-max_joint_delta, max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(j).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(j).dq(dq);
|
||||
msg.motor_cmd().at(j).kp(kp);
|
||||
msg.motor_cmd().at(j).kd(kd);
|
||||
msg.motor_cmd().at(j).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
user_ctrl_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// put arms down
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
|
||||
max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(j).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(j).dq(dq);
|
||||
msg.motor_cmd().at(j).kp(kp);
|
||||
msg.motor_cmd().at(j).kd(kd);
|
||||
msg.motor_cmd().at(j).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
user_ctrl_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// stop control
|
||||
std::cout << "Stoping user control ...";
|
||||
client.SwitchToInternalCtrl(unitree::robot::g1::InternalFsmMode::LAST);
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
11478
unitree_sdk2/example/g1/low_level/behavior_lib/motion.seq
Normal file
11478
unitree_sdk2/example/g1/low_level/behavior_lib/motion.seq
Normal file
File diff suppressed because it is too large
Load Diff
371
unitree_sdk2/example/g1/low_level/g1_ankle_swing_example.cpp
Normal file
371
unitree_sdk2/example/g1/low_level/g1_ankle_swing_example.cpp
Normal file
@@ -0,0 +1,371 @@
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <shared_mutex>
|
||||
|
||||
#include "gamepad.hpp"
|
||||
|
||||
// DDS
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
// IDL
|
||||
#include <unitree/idl/hg/IMUState_.hpp>
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
|
||||
|
||||
static const std::string HG_CMD_TOPIC = "rt/lowcmd";
|
||||
static const std::string HG_IMU_TORSO = "rt/secondary_imu";
|
||||
static const std::string HG_STATE_TOPIC = "rt/lowstate";
|
||||
|
||||
using namespace unitree::common;
|
||||
using namespace unitree::robot;
|
||||
using namespace unitree_hg::msg::dds_;
|
||||
|
||||
template <typename T>
|
||||
class DataBuffer {
|
||||
public:
|
||||
void SetData(const T &newData) {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = std::make_shared<T>(newData);
|
||||
}
|
||||
|
||||
std::shared_ptr<const T> GetData() {
|
||||
std::shared_lock<std::shared_mutex> lock(mutex);
|
||||
return data ? data : nullptr;
|
||||
}
|
||||
|
||||
void Clear() {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = nullptr;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<T> data;
|
||||
std::shared_mutex mutex;
|
||||
};
|
||||
|
||||
const int G1_NUM_MOTOR = 29;
|
||||
struct ImuState {
|
||||
std::array<float, 3> rpy = {};
|
||||
std::array<float, 3> omega = {};
|
||||
};
|
||||
struct MotorCommand {
|
||||
std::array<float, G1_NUM_MOTOR> q_target = {};
|
||||
std::array<float, G1_NUM_MOTOR> dq_target = {};
|
||||
std::array<float, G1_NUM_MOTOR> kp = {};
|
||||
std::array<float, G1_NUM_MOTOR> kd = {};
|
||||
std::array<float, G1_NUM_MOTOR> tau_ff = {};
|
||||
};
|
||||
struct MotorState {
|
||||
std::array<float, G1_NUM_MOTOR> q = {};
|
||||
std::array<float, G1_NUM_MOTOR> dq = {};
|
||||
};
|
||||
|
||||
// Stiffness for all G1 Joints
|
||||
std::array<float, G1_NUM_MOTOR> Kp{
|
||||
60, 60, 60, 100, 40, 40, // legs
|
||||
60, 60, 60, 100, 40, 40, // legs
|
||||
60, 40, 40, // waist
|
||||
40, 40, 40, 40, 40, 40, 40, // arms
|
||||
40, 40, 40, 40, 40, 40, 40 // arms
|
||||
};
|
||||
|
||||
// Damping for all G1 Joints
|
||||
std::array<float, G1_NUM_MOTOR> Kd{
|
||||
1, 1, 1, 2, 1, 1, // legs
|
||||
1, 1, 1, 2, 1, 1, // legs
|
||||
1, 1, 1, // waist
|
||||
1, 1, 1, 1, 1, 1, 1, // arms
|
||||
1, 1, 1, 1, 1, 1, 1 // arms
|
||||
};
|
||||
|
||||
enum class Mode {
|
||||
PR = 0, // Series Control for Ptich/Roll Joints
|
||||
AB = 1 // Parallel Control for A/B Joints
|
||||
};
|
||||
|
||||
enum G1JointIndex {
|
||||
LeftHipPitch = 0,
|
||||
LeftHipRoll = 1,
|
||||
LeftHipYaw = 2,
|
||||
LeftKnee = 3,
|
||||
LeftAnklePitch = 4,
|
||||
LeftAnkleB = 4,
|
||||
LeftAnkleRoll = 5,
|
||||
LeftAnkleA = 5,
|
||||
RightHipPitch = 6,
|
||||
RightHipRoll = 7,
|
||||
RightHipYaw = 8,
|
||||
RightKnee = 9,
|
||||
RightAnklePitch = 10,
|
||||
RightAnkleB = 10,
|
||||
RightAnkleRoll = 11,
|
||||
RightAnkleA = 11,
|
||||
WaistYaw = 12,
|
||||
WaistRoll = 13, // NOTE INVALID for g1 23dof/29dof with waist locked
|
||||
WaistA = 13, // NOTE INVALID for g1 23dof/29dof with waist locked
|
||||
WaistPitch = 14, // NOTE INVALID for g1 23dof/29dof with waist locked
|
||||
WaistB = 14, // NOTE INVALID for g1 23dof/29dof with waist locked
|
||||
LeftShoulderPitch = 15,
|
||||
LeftShoulderRoll = 16,
|
||||
LeftShoulderYaw = 17,
|
||||
LeftElbow = 18,
|
||||
LeftWristRoll = 19,
|
||||
LeftWristPitch = 20, // NOTE INVALID for g1 23dof
|
||||
LeftWristYaw = 21, // NOTE INVALID for g1 23dof
|
||||
RightShoulderPitch = 22,
|
||||
RightShoulderRoll = 23,
|
||||
RightShoulderYaw = 24,
|
||||
RightElbow = 25,
|
||||
RightWristRoll = 26,
|
||||
RightWristPitch = 27, // NOTE INVALID for g1 23dof
|
||||
RightWristYaw = 28 // NOTE INVALID for g1 23dof
|
||||
};
|
||||
|
||||
inline uint32_t Crc32Core(uint32_t *ptr, uint32_t len) {
|
||||
uint32_t xbit = 0;
|
||||
uint32_t data = 0;
|
||||
uint32_t CRC32 = 0xFFFFFFFF;
|
||||
const uint32_t dwPolynomial = 0x04c11db7;
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
xbit = 1 << 31;
|
||||
data = ptr[i];
|
||||
for (uint32_t bits = 0; bits < 32; bits++) {
|
||||
if (CRC32 & 0x80000000) {
|
||||
CRC32 <<= 1;
|
||||
CRC32 ^= dwPolynomial;
|
||||
} else
|
||||
CRC32 <<= 1;
|
||||
if (data & xbit) CRC32 ^= dwPolynomial;
|
||||
|
||||
xbit >>= 1;
|
||||
}
|
||||
}
|
||||
return CRC32;
|
||||
};
|
||||
|
||||
class G1Example {
|
||||
private:
|
||||
double time_;
|
||||
double control_dt_; // [2ms]
|
||||
double duration_; // [3 s]
|
||||
int counter_;
|
||||
Mode mode_pr_;
|
||||
uint8_t mode_machine_;
|
||||
|
||||
Gamepad gamepad_;
|
||||
REMOTE_DATA_RX rx_;
|
||||
|
||||
DataBuffer<MotorState> motor_state_buffer_;
|
||||
DataBuffer<MotorCommand> motor_command_buffer_;
|
||||
DataBuffer<ImuState> imu_state_buffer_;
|
||||
|
||||
ChannelPublisherPtr<LowCmd_> lowcmd_publisher_;
|
||||
ChannelSubscriberPtr<LowState_> lowstate_subscriber_;
|
||||
ChannelSubscriberPtr<IMUState_> imutorso_subscriber_;
|
||||
ThreadPtr command_writer_ptr_, control_thread_ptr_;
|
||||
|
||||
std::shared_ptr<unitree::robot::b2::MotionSwitcherClient> msc_;
|
||||
|
||||
public:
|
||||
G1Example(std::string networkInterface)
|
||||
: time_(0.0),
|
||||
control_dt_(0.002),
|
||||
duration_(3.0),
|
||||
counter_(0),
|
||||
mode_pr_(Mode::PR),
|
||||
mode_machine_(0) {
|
||||
ChannelFactory::Instance()->Init(0, networkInterface);
|
||||
|
||||
// try to shutdown motion control-related service
|
||||
msc_ = std::make_shared<unitree::robot::b2::MotionSwitcherClient>();
|
||||
msc_->SetTimeout(5.0f);
|
||||
msc_->Init();
|
||||
std::string form, name;
|
||||
while (msc_->CheckMode(form, name), !name.empty()) {
|
||||
if (msc_->ReleaseMode())
|
||||
std::cout << "Failed to switch to Release Mode\n";
|
||||
sleep(5);
|
||||
}
|
||||
|
||||
// create publisher
|
||||
lowcmd_publisher_.reset(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
|
||||
lowcmd_publisher_->InitChannel();
|
||||
// create subscriber
|
||||
lowstate_subscriber_.reset(new ChannelSubscriber<LowState_>(HG_STATE_TOPIC));
|
||||
lowstate_subscriber_->InitChannel(std::bind(&G1Example::LowStateHandler, this, std::placeholders::_1), 1);
|
||||
imutorso_subscriber_.reset(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
|
||||
imutorso_subscriber_->InitChannel(std::bind(&G1Example::imuTorsoHandler, this, std::placeholders::_1), 1);
|
||||
// create threads
|
||||
command_writer_ptr_ = CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000, &G1Example::LowCommandWriter, this);
|
||||
control_thread_ptr_ = CreateRecurrentThreadEx("control", UT_CPU_ID_NONE, 2000, &G1Example::Control, this);
|
||||
}
|
||||
|
||||
void imuTorsoHandler(const void *message) {
|
||||
IMUState_ imu_torso = *(const IMUState_ *)message;
|
||||
auto &rpy = imu_torso.rpy();
|
||||
if (counter_ % 500 == 0)
|
||||
printf("IMU.torso.rpy: %.2f %.2f %.2f\n", rpy[0], rpy[1], rpy[2]);
|
||||
}
|
||||
|
||||
void LowStateHandler(const void *message) {
|
||||
LowState_ low_state = *(const LowState_ *)message;
|
||||
if (low_state.crc() != Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) {
|
||||
std::cout << "[ERROR] CRC Error" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// get motor state
|
||||
MotorState ms_tmp;
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
|
||||
ms_tmp.q.at(i) = low_state.motor_state()[i].q();
|
||||
ms_tmp.dq.at(i) = low_state.motor_state()[i].dq();
|
||||
if (low_state.motor_state()[i].motorstate() && i <= RightAnkleRoll)
|
||||
std::cout << "[ERROR] motor " << i << " with code " << low_state.motor_state()[i].motorstate() << "\n";
|
||||
}
|
||||
motor_state_buffer_.SetData(ms_tmp);
|
||||
|
||||
// get imu state
|
||||
ImuState imu_tmp;
|
||||
imu_tmp.omega = low_state.imu_state().gyroscope();
|
||||
imu_tmp.rpy = low_state.imu_state().rpy();
|
||||
imu_state_buffer_.SetData(imu_tmp);
|
||||
|
||||
// update gamepad
|
||||
memcpy(rx_.buff, &low_state.wireless_remote()[0], 40);
|
||||
gamepad_.update(rx_.RF_RX);
|
||||
|
||||
// update mode machine
|
||||
if (mode_machine_ != low_state.mode_machine()) {
|
||||
if (mode_machine_ == 0) std::cout << "G1 type: " << unsigned(low_state.mode_machine()) << std::endl;
|
||||
mode_machine_ = low_state.mode_machine();
|
||||
}
|
||||
|
||||
// report robot status every second
|
||||
if (++counter_ % 500 == 0) {
|
||||
counter_ = 0;
|
||||
// IMU
|
||||
auto &rpy = low_state.imu_state().rpy();
|
||||
printf("IMU.pelvis.rpy: %.2f %.2f %.2f\n", rpy[0], rpy[1], rpy[2]);
|
||||
|
||||
// RC
|
||||
printf("gamepad_.A.pressed: %d\n", static_cast<int>(gamepad_.A.pressed));
|
||||
printf("gamepad_.B.pressed: %d\n", static_cast<int>(gamepad_.B.pressed));
|
||||
printf("gamepad_.X.pressed: %d\n", static_cast<int>(gamepad_.X.pressed));
|
||||
printf("gamepad_.Y.pressed: %d\n", static_cast<int>(gamepad_.Y.pressed));
|
||||
|
||||
// Motor
|
||||
auto &ms = low_state.motor_state();
|
||||
printf("All %d Motors:", G1_NUM_MOTOR);
|
||||
printf("\nmode: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%u,", ms[i].mode());
|
||||
printf("\npos: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%.2f,", ms[i].q());
|
||||
printf("\nvel: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%.2f,", ms[i].dq());
|
||||
printf("\ntau_est: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%.2f,", ms[i].tau_est());
|
||||
printf("\ntemperature: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%d,%d;", ms[i].temperature()[0], ms[i].temperature()[1]);
|
||||
printf("\nvol: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%.2f,", ms[i].vol());
|
||||
printf("\nsensor: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%u,%u;", ms[i].sensor()[0], ms[i].sensor()[1]);
|
||||
printf("\nmotorstate: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%u,", ms[i].motorstate());
|
||||
printf("\nreserve: ");
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%u,%u,%u,%u;", ms[i].reserve()[0], ms[i].reserve()[1], ms[i].reserve()[2], ms[i].reserve()[3]);
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
void LowCommandWriter() {
|
||||
LowCmd_ dds_low_command;
|
||||
dds_low_command.mode_pr() = static_cast<uint8_t>(mode_pr_);
|
||||
dds_low_command.mode_machine() = mode_machine_;
|
||||
|
||||
const std::shared_ptr<const MotorCommand> mc = motor_command_buffer_.GetData();
|
||||
if (mc) {
|
||||
for (size_t i = 0; i < G1_NUM_MOTOR; i++) {
|
||||
dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable
|
||||
dds_low_command.motor_cmd().at(i).tau() = mc->tau_ff.at(i);
|
||||
dds_low_command.motor_cmd().at(i).q() = mc->q_target.at(i);
|
||||
dds_low_command.motor_cmd().at(i).dq() = mc->dq_target.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kp() = mc->kp.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kd() = mc->kd.at(i);
|
||||
}
|
||||
|
||||
dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command, (sizeof(dds_low_command) >> 2) - 1);
|
||||
lowcmd_publisher_->Write(dds_low_command);
|
||||
}
|
||||
}
|
||||
|
||||
void Control() {
|
||||
MotorCommand motor_command_tmp;
|
||||
const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
|
||||
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
|
||||
motor_command_tmp.tau_ff.at(i) = 0.0;
|
||||
motor_command_tmp.q_target.at(i) = 0.0;
|
||||
motor_command_tmp.dq_target.at(i) = 0.0;
|
||||
motor_command_tmp.kp.at(i) = Kp[i];
|
||||
motor_command_tmp.kd.at(i) = Kd[i];
|
||||
}
|
||||
|
||||
if (ms) {
|
||||
time_ += control_dt_;
|
||||
if (time_ < duration_) {
|
||||
// [Stage 1]: set robot to zero posture
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
|
||||
double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
|
||||
motor_command_tmp.q_target.at(i) = (1.0 - ratio) * ms->q.at(i);
|
||||
}
|
||||
} else if (time_ < duration_ * 2) {
|
||||
// [Stage 2]: swing ankle using PR mode
|
||||
mode_pr_ = Mode::PR;
|
||||
double max_P = M_PI * 30.0 / 180.0;
|
||||
double max_R = M_PI * 10.0 / 180.0;
|
||||
double t = time_ - duration_;
|
||||
double L_P_des = max_P * std::sin(2.0 * M_PI * t);
|
||||
double L_R_des = max_R * std::sin(2.0 * M_PI * t);
|
||||
double R_P_des = max_P * std::sin(2.0 * M_PI * t);
|
||||
double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
|
||||
|
||||
motor_command_tmp.q_target.at(LeftAnklePitch) = L_P_des;
|
||||
motor_command_tmp.q_target.at(LeftAnkleRoll) = L_R_des;
|
||||
motor_command_tmp.q_target.at(RightAnklePitch) = R_P_des;
|
||||
motor_command_tmp.q_target.at(RightAnkleRoll) = R_R_des;
|
||||
} else {
|
||||
// [Stage 3]: swing ankle using AB mode
|
||||
mode_pr_ = Mode::AB;
|
||||
double max_A = M_PI * 30.0 / 180.0;
|
||||
double max_B = M_PI * 10.0 / 180.0;
|
||||
double t = time_ - duration_ * 2;
|
||||
double L_A_des = +max_A * std::sin(M_PI * t);
|
||||
double L_B_des = +max_B * std::sin(M_PI * t + M_PI);
|
||||
double R_A_des = -max_A * std::sin(M_PI * t);
|
||||
double R_B_des = -max_B * std::sin(M_PI * t + M_PI);
|
||||
|
||||
motor_command_tmp.q_target.at(LeftAnkleA) = L_A_des;
|
||||
motor_command_tmp.q_target.at(LeftAnkleB) = L_B_des;
|
||||
motor_command_tmp.q_target.at(RightAnkleA) = R_A_des;
|
||||
motor_command_tmp.q_target.at(RightAnkleB) = R_B_des;
|
||||
}
|
||||
|
||||
motor_command_buffer_.SetData(motor_command_tmp);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: g1_ankle_swing_example network_interface" << std::endl;
|
||||
exit(0);
|
||||
}
|
||||
std::string networkInterface = argv[1];
|
||||
G1Example custom(networkInterface);
|
||||
while (true) sleep(10);
|
||||
return 0;
|
||||
}
|
||||
407
unitree_sdk2/example/g1/low_level/g1_dual_arm_example.cpp
Normal file
407
unitree_sdk2/example/g1/low_level/g1_dual_arm_example.cpp
Normal file
@@ -0,0 +1,407 @@
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <shared_mutex>
|
||||
|
||||
// DDS
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
// IDL
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
|
||||
#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
|
||||
using namespace unitree::robot::b2;
|
||||
|
||||
static const std::string HG_CMD_TOPIC = "rt/lowcmd";
|
||||
static const std::string HG_STATE_TOPIC = "rt/lowstate";
|
||||
|
||||
using namespace unitree::common;
|
||||
using namespace unitree::robot;
|
||||
|
||||
const int G1_NUM_MOTOR = 29;
|
||||
|
||||
template <typename T>
|
||||
class DataBuffer {
|
||||
public:
|
||||
void SetData(const T &newData) {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = std::make_shared<T>(newData);
|
||||
}
|
||||
|
||||
std::shared_ptr<const T> GetData() {
|
||||
std::shared_lock<std::shared_mutex> lock(mutex);
|
||||
return data ? data : nullptr;
|
||||
}
|
||||
|
||||
void Clear() {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = nullptr;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<T> data;
|
||||
std::shared_mutex mutex;
|
||||
};
|
||||
|
||||
struct ImuState {
|
||||
std::array<float, 3> rpy = {};
|
||||
std::array<float, 3> omega = {};
|
||||
};
|
||||
|
||||
struct MotorCommand {
|
||||
std::array<float, G1_NUM_MOTOR> q_target = {};
|
||||
std::array<float, G1_NUM_MOTOR> dq_target = {};
|
||||
std::array<float, G1_NUM_MOTOR> kp = {};
|
||||
std::array<float, G1_NUM_MOTOR> kd = {};
|
||||
std::array<float, G1_NUM_MOTOR> tau_ff = {};
|
||||
};
|
||||
|
||||
struct MotorState {
|
||||
std::array<float, G1_NUM_MOTOR> q = {};
|
||||
std::array<float, G1_NUM_MOTOR> dq = {};
|
||||
};
|
||||
|
||||
enum MotorType { GearboxS = 0, GearboxM = 1, GearboxL = 2 };
|
||||
|
||||
std::array<MotorType, G1_NUM_MOTOR> G1MotorType{
|
||||
// clang-format off
|
||||
// legs
|
||||
GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,
|
||||
GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,
|
||||
// waist
|
||||
GearboxM, GearboxS, GearboxS,
|
||||
// arms
|
||||
GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS,
|
||||
GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS
|
||||
// clang-format on
|
||||
};
|
||||
|
||||
enum PRorAB { PR = 0, AB = 1 };
|
||||
|
||||
enum G1JointValidIndex {
|
||||
LeftShoulderPitch = 15,
|
||||
LeftShoulderRoll = 16,
|
||||
LeftShoulderYaw = 17,
|
||||
LeftElbow = 18,
|
||||
LeftWristRoll = 19,
|
||||
LeftWristPitch = 20,
|
||||
LeftWristYaw = 21,
|
||||
RightShoulderPitch = 22,
|
||||
RightShoulderRoll = 23,
|
||||
RightShoulderYaw = 24,
|
||||
RightElbow = 25,
|
||||
RightWristRoll = 26,
|
||||
RightWristPitch = 27,
|
||||
RightWristYaw = 28
|
||||
};
|
||||
|
||||
inline uint32_t Crc32Core(uint32_t *ptr, uint32_t len) {
|
||||
uint32_t xbit = 0;
|
||||
uint32_t data = 0;
|
||||
uint32_t CRC32 = 0xFFFFFFFF;
|
||||
const uint32_t dwPolynomial = 0x04c11db7;
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
xbit = 1 << 31;
|
||||
data = ptr[i];
|
||||
for (uint32_t bits = 0; bits < 32; bits++) {
|
||||
if (CRC32 & 0x80000000) {
|
||||
CRC32 <<= 1;
|
||||
CRC32 ^= dwPolynomial;
|
||||
} else
|
||||
CRC32 <<= 1;
|
||||
if (data & xbit) CRC32 ^= dwPolynomial;
|
||||
|
||||
xbit >>= 1;
|
||||
}
|
||||
}
|
||||
return CRC32;
|
||||
};
|
||||
|
||||
float GetMotorKp(MotorType type) {
|
||||
switch (type) {
|
||||
case GearboxS:
|
||||
return 40;
|
||||
case GearboxM:
|
||||
return 40;
|
||||
case GearboxL:
|
||||
return 100;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
float GetMotorKd(MotorType type) {
|
||||
switch (type) {
|
||||
case GearboxS:
|
||||
return 1;
|
||||
case GearboxM:
|
||||
return 1;
|
||||
case GearboxL:
|
||||
return 1;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
class G1Example {
|
||||
private:
|
||||
double time_;
|
||||
double control_dt_; // [2ms]
|
||||
double duration_; // [3 s]
|
||||
PRorAB mode_;
|
||||
uint8_t mode_machine_;
|
||||
std::vector<std::vector<double>> frames_data_;
|
||||
|
||||
DataBuffer<MotorState> motor_state_buffer_;
|
||||
DataBuffer<MotorCommand> motor_command_buffer_;
|
||||
DataBuffer<ImuState> imu_state_buffer_;
|
||||
|
||||
ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_> lowcmd_publisher_;
|
||||
ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> lowstate_subscriber_;
|
||||
ThreadPtr command_writer_ptr_, control_thread_ptr_;
|
||||
|
||||
std::shared_ptr<MotionSwitcherClient> msc;
|
||||
|
||||
public:
|
||||
G1Example(std::string networkInterface)
|
||||
: time_(0.0),
|
||||
control_dt_(0.002),
|
||||
duration_(3.0),
|
||||
mode_(PR),
|
||||
mode_machine_(0) {
|
||||
ChannelFactory::Instance()->Init(0, networkInterface);
|
||||
|
||||
msc.reset(new MotionSwitcherClient());
|
||||
msc->SetTimeout(5.0F);
|
||||
msc->Init();
|
||||
|
||||
/*Shut down motion control-related service*/
|
||||
while(queryMotionStatus())
|
||||
{
|
||||
std::cout << "Try to deactivate the motion control-related service." << std::endl;
|
||||
int32_t ret = msc->ReleaseMode();
|
||||
if (ret == 0) {
|
||||
std::cout << "ReleaseMode succeeded." << std::endl;
|
||||
} else {
|
||||
std::cout << "ReleaseMode failed. Error code: " << ret << std::endl;
|
||||
}
|
||||
sleep(5);
|
||||
}
|
||||
|
||||
loadBehaviorLibrary("motion");
|
||||
|
||||
// create publisher
|
||||
lowcmd_publisher_.reset(
|
||||
new ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(HG_CMD_TOPIC));
|
||||
lowcmd_publisher_->InitChannel();
|
||||
|
||||
// create subscriber
|
||||
lowstate_subscriber_.reset(
|
||||
new ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
|
||||
HG_STATE_TOPIC));
|
||||
lowstate_subscriber_->InitChannel(
|
||||
std::bind(&G1Example::LowStateHandler, this, std::placeholders::_1), 1);
|
||||
|
||||
// create threads
|
||||
command_writer_ptr_ =
|
||||
CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000,
|
||||
&G1Example::LowCommandWriter, this);
|
||||
control_thread_ptr_ = CreateRecurrentThreadEx(
|
||||
"control", UT_CPU_ID_NONE, 2000, &G1Example::Control, this);
|
||||
}
|
||||
|
||||
void loadBehaviorLibrary(std::string behavior_name) {
|
||||
std::string resource_dir = BLIB_DIR;
|
||||
YAML::Node motion = YAML::LoadFile(resource_dir + behavior_name + ".seq");
|
||||
|
||||
std::string content = motion["components"][1]["content"].as<std::string>();
|
||||
int num_parts = motion["components"][1]["num_parts"].as<int>();
|
||||
std::cout << "BehaviorName: " << behavior_name + ".seq\n";
|
||||
std::cout << content << " with " << num_parts << "\n";
|
||||
|
||||
auto frames = motion["components"][1]["frames"];
|
||||
|
||||
for (const auto &frame : frames) {
|
||||
std::vector<double> frame_data;
|
||||
for (const auto &element : frame) {
|
||||
frame_data.push_back(element.as<double>());
|
||||
}
|
||||
frames_data_.push_back(frame_data);
|
||||
}
|
||||
|
||||
std::cout << frames_data_.size() << " knots with " << frames_data_[0].size()
|
||||
<< " DOF\n";
|
||||
}
|
||||
|
||||
void ReportRPY() {
|
||||
const std::shared_ptr<const ImuState> imu_tmp_ptr =
|
||||
imu_state_buffer_.GetData();
|
||||
if (imu_tmp_ptr) {
|
||||
std::cout << "rpy: [" << imu_tmp_ptr->rpy.at(0) << ", "
|
||||
<< imu_tmp_ptr->rpy.at(1) << ", " << imu_tmp_ptr->rpy.at(2)
|
||||
<< "]" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void LowStateHandler(const void *message) {
|
||||
unitree_hg::msg::dds_::LowState_ low_state =
|
||||
*(const unitree_hg::msg::dds_::LowState_ *)message;
|
||||
|
||||
if (low_state.crc() !=
|
||||
Crc32Core((uint32_t *)&low_state,
|
||||
(sizeof(unitree_hg::msg::dds_::LowState_) >> 2) - 1)) {
|
||||
std::cout << "low_state CRC Error" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// get motor state
|
||||
MotorState ms_tmp;
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
|
||||
ms_tmp.q.at(i) = low_state.motor_state()[i].q();
|
||||
ms_tmp.dq.at(i) = low_state.motor_state()[i].dq();
|
||||
}
|
||||
motor_state_buffer_.SetData(ms_tmp);
|
||||
|
||||
// get imu state
|
||||
ImuState imu_tmp;
|
||||
imu_tmp.omega = low_state.imu_state().gyroscope();
|
||||
imu_tmp.rpy = low_state.imu_state().rpy();
|
||||
imu_state_buffer_.SetData(imu_tmp);
|
||||
|
||||
// update mode machine
|
||||
if (mode_machine_ != low_state.mode_machine()) {
|
||||
if (mode_machine_ == 0)
|
||||
std::cout << "G1 type: " << unsigned(low_state.mode_machine())
|
||||
<< std::endl;
|
||||
mode_machine_ = low_state.mode_machine();
|
||||
}
|
||||
}
|
||||
|
||||
void LowCommandWriter() {
|
||||
unitree_hg::msg::dds_::LowCmd_ dds_low_command;
|
||||
dds_low_command.mode_pr() = mode_;
|
||||
dds_low_command.mode_machine() = mode_machine_;
|
||||
|
||||
const std::shared_ptr<const MotorCommand> mc =
|
||||
motor_command_buffer_.GetData();
|
||||
if (mc) {
|
||||
for (size_t i = 0; i < G1_NUM_MOTOR; i++) {
|
||||
dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable
|
||||
dds_low_command.motor_cmd().at(i).tau() = mc->tau_ff.at(i);
|
||||
dds_low_command.motor_cmd().at(i).q() = mc->q_target.at(i);
|
||||
dds_low_command.motor_cmd().at(i).dq() = mc->dq_target.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kp() = mc->kp.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kd() = mc->kd.at(i);
|
||||
}
|
||||
|
||||
dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
|
||||
(sizeof(dds_low_command) >> 2) - 1);
|
||||
lowcmd_publisher_->Write(dds_low_command);
|
||||
}
|
||||
}
|
||||
|
||||
void Control() {
|
||||
MotorCommand motor_command_tmp;
|
||||
const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
|
||||
|
||||
if (ms) {
|
||||
time_ += control_dt_;
|
||||
if (time_ < duration_) {
|
||||
// [Stage 1]: set robot to zero posture
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
|
||||
double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
|
||||
|
||||
double q_des = 0;
|
||||
motor_command_tmp.tau_ff.at(i) = 0.0;
|
||||
motor_command_tmp.q_target.at(i) =
|
||||
(q_des - ms->q.at(i)) * ratio + ms->q.at(i);
|
||||
motor_command_tmp.dq_target.at(i) = 0.0;
|
||||
motor_command_tmp.kp.at(i) = GetMotorKp(G1MotorType[i]);
|
||||
motor_command_tmp.kd.at(i) = GetMotorKd(G1MotorType[i]);
|
||||
}
|
||||
} else {
|
||||
// [Stage 2]: tracking the offline trajectory
|
||||
size_t frame_index = (size_t)((time_ - duration_) / control_dt_);
|
||||
if (frame_index >= frames_data_.size()) {
|
||||
frame_index = frames_data_.size() - 1;
|
||||
time_ = 0.0; // RESET
|
||||
}
|
||||
|
||||
if (frame_index % 100 == 0)
|
||||
std::cout << "Frame Index: " << frame_index << std::endl;
|
||||
|
||||
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
|
||||
size_t index_in_frame = i - LeftShoulderPitch;
|
||||
motor_command_tmp.q_target.at(i) =
|
||||
(i >= LeftShoulderPitch)
|
||||
? frames_data_[frame_index][index_in_frame]
|
||||
: 0.0;
|
||||
motor_command_tmp.dq_target.at(i) = 0.0;
|
||||
motor_command_tmp.tau_ff.at(i) = 0.0;
|
||||
motor_command_tmp.kp.at(i) = GetMotorKp(G1MotorType[i]);
|
||||
motor_command_tmp.kd.at(i) = GetMotorKd(G1MotorType[i]);
|
||||
}
|
||||
}
|
||||
|
||||
motor_command_buffer_.SetData(motor_command_tmp);
|
||||
}
|
||||
}
|
||||
|
||||
std::string queryServiceName(std::string form,std::string name)
|
||||
{
|
||||
if(form == "0")
|
||||
{
|
||||
if(name == "normal" ) return "sport_mode";
|
||||
if(name == "ai" ) return "ai_sport";
|
||||
if(name == "advanced" ) return "advanced_sport";
|
||||
}
|
||||
else
|
||||
{
|
||||
if(name == "ai-w" ) return "wheeled_sport(go2W)";
|
||||
if(name == "normal-w" ) return "wheeled_sport(b2W)";
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
int queryMotionStatus()
|
||||
{
|
||||
std::string robotForm,motionName;
|
||||
int motionStatus;
|
||||
int32_t ret = msc->CheckMode(robotForm,motionName);
|
||||
if (ret == 0) {
|
||||
std::cout << "CheckMode succeeded." << std::endl;
|
||||
} else {
|
||||
std::cout << "CheckMode failed. Error code: " << ret << std::endl;
|
||||
}
|
||||
if(motionName.empty())
|
||||
{
|
||||
std::cout << "The motion control-related service is deactivated." << std::endl;
|
||||
motionStatus = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::string serviceName = queryServiceName(robotForm,motionName);
|
||||
std::cout << "Service: "<< serviceName<< " is activate" << std::endl;
|
||||
motionStatus = 1;
|
||||
}
|
||||
return motionStatus;
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: g1_dual_arm_example network_interface_name"
|
||||
<< std::endl;
|
||||
exit(0);
|
||||
}
|
||||
std::string networkInterface = argv[1];
|
||||
G1Example custom(networkInterface);
|
||||
|
||||
while (true) sleep(10);
|
||||
|
||||
return 0;
|
||||
}
|
||||
128
unitree_sdk2/example/g1/low_level/gamepad.hpp
Normal file
128
unitree_sdk2/example/g1/low_level/gamepad.hpp
Normal file
@@ -0,0 +1,128 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace unitree::common
|
||||
{
|
||||
|
||||
// bytecode mapping for raw joystick data
|
||||
// 16b
|
||||
typedef union
|
||||
{
|
||||
struct
|
||||
{
|
||||
uint8_t R1 : 1;
|
||||
uint8_t L1 : 1;
|
||||
uint8_t start : 1;
|
||||
uint8_t select : 1;
|
||||
uint8_t R2 : 1;
|
||||
uint8_t L2 : 1;
|
||||
uint8_t F1 : 1;
|
||||
uint8_t F2 : 1;
|
||||
uint8_t A : 1;
|
||||
uint8_t B : 1;
|
||||
uint8_t X : 1;
|
||||
uint8_t Y : 1;
|
||||
uint8_t up : 1;
|
||||
uint8_t right : 1;
|
||||
uint8_t down : 1;
|
||||
uint8_t left : 1;
|
||||
} components;
|
||||
uint16_t value;
|
||||
} xKeySwitchUnion;
|
||||
|
||||
// 40 Byte (now used 24B)
|
||||
typedef struct
|
||||
{
|
||||
uint8_t head[2];
|
||||
xKeySwitchUnion btn;
|
||||
float lx;
|
||||
float rx;
|
||||
float ry;
|
||||
float L2;
|
||||
float ly;
|
||||
|
||||
uint8_t idle[16];
|
||||
} xRockerBtnDataStruct;
|
||||
|
||||
typedef union
|
||||
{
|
||||
xRockerBtnDataStruct RF_RX;
|
||||
uint8_t buff[40];
|
||||
} REMOTE_DATA_RX;
|
||||
|
||||
class Button
|
||||
{
|
||||
public:
|
||||
Button() {}
|
||||
|
||||
void update(bool state)
|
||||
{
|
||||
on_press = state ? state != pressed : false;
|
||||
on_release = state ? false : state != pressed;
|
||||
pressed = state;
|
||||
}
|
||||
|
||||
bool pressed = false;
|
||||
bool on_press = false;
|
||||
bool on_release = false;
|
||||
};
|
||||
|
||||
class Gamepad
|
||||
{
|
||||
public:
|
||||
Gamepad() {}
|
||||
|
||||
void update(xRockerBtnDataStruct &key_data)
|
||||
{
|
||||
lx = lx * (1 - smooth) + (std::fabs(key_data.lx) < dead_zone ? 0.0 : key_data.lx) * smooth;
|
||||
rx = rx * (1 - smooth) + (std::fabs(key_data.rx) < dead_zone ? 0.0 : key_data.rx) * smooth;
|
||||
ry = ry * (1 - smooth) + (std::fabs(key_data.ry) < dead_zone ? 0.0 : key_data.ry) * smooth;
|
||||
l2 = l2 * (1 - smooth) + (std::fabs(key_data.L2) < dead_zone ? 0.0 : key_data.L2) * smooth;
|
||||
ly = ly * (1 - smooth) + (std::fabs(key_data.ly) < dead_zone ? 0.0 : key_data.ly) * smooth;
|
||||
|
||||
R1.update(key_data.btn.components.R1);
|
||||
L1.update(key_data.btn.components.L1);
|
||||
start.update(key_data.btn.components.start);
|
||||
select.update(key_data.btn.components.select);
|
||||
R2.update(key_data.btn.components.R2);
|
||||
L2.update(key_data.btn.components.L2);
|
||||
F1.update(key_data.btn.components.F1);
|
||||
F2.update(key_data.btn.components.F2);
|
||||
A.update(key_data.btn.components.A);
|
||||
B.update(key_data.btn.components.B);
|
||||
X.update(key_data.btn.components.X);
|
||||
Y.update(key_data.btn.components.Y);
|
||||
up.update(key_data.btn.components.up);
|
||||
right.update(key_data.btn.components.right);
|
||||
down.update(key_data.btn.components.down);
|
||||
left.update(key_data.btn.components.left);
|
||||
}
|
||||
|
||||
float lx = 0.;
|
||||
float rx = 0.;
|
||||
float ry = 0.;
|
||||
float l2 = 0.;
|
||||
float ly = 0.;
|
||||
|
||||
float smooth = 0.03;
|
||||
float dead_zone = 0.01;
|
||||
|
||||
Button R1;
|
||||
Button L1;
|
||||
Button start;
|
||||
Button select;
|
||||
Button R2;
|
||||
Button L2;
|
||||
Button F1;
|
||||
Button F2;
|
||||
Button A;
|
||||
Button B;
|
||||
Button X;
|
||||
Button Y;
|
||||
Button up;
|
||||
Button right;
|
||||
Button down;
|
||||
Button left;
|
||||
};
|
||||
} // namespace unitree::common
|
||||
46
unitree_sdk2/example/g1/low_level/terminations.cpp
Normal file
46
unitree_sdk2/example/g1/low_level/terminations.cpp
Normal file
@@ -0,0 +1,46 @@
|
||||
#include <unitree/robot/g1/common/terminations.hpp>
|
||||
#include <boost/program_options.hpp>
|
||||
#include <thread>
|
||||
|
||||
namespace po = boost::program_options;
|
||||
|
||||
using namespace unitree::robot;
|
||||
using namespace unitree_hg::msg::dds_;
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
// Parse command line arguments
|
||||
po::options_description desc("Unitree G1 termination functions testing.");
|
||||
desc.add_options()
|
||||
("network,n", po::value<std::string>()->default_value(""), "dds network interface")
|
||||
;
|
||||
po::variables_map vm;
|
||||
po::store(po::parse_command_line(argc, argv, desc), vm);
|
||||
po::notify(vm);
|
||||
std::cout << desc << std::endl;
|
||||
|
||||
// DDS Init
|
||||
ChannelFactory::Instance()->Init(0, vm["network"].as<std::string>());
|
||||
|
||||
auto lowstate_subscriber = std::make_shared<ChannelSubscriber<LowState_>>("rt/lowstate");
|
||||
LowState_ lowstate;
|
||||
lowstate_subscriber->InitChannel([&lowstate](const void* message) {
|
||||
lowstate = *(const LowState_*)message;
|
||||
});
|
||||
|
||||
std::cout << "Checking terminations..." << std::endl;
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (g1::bad_orientation(lowstate, 1.0f)) { // Tip the robot over to test bad orientation
|
||||
std::cout << "Bad orientation detected!" << std::endl;
|
||||
}
|
||||
if (g1::lost_connection(lowstate_subscriber, 1000)) { // Unplug the network cable to test lost connection
|
||||
std::cout << "Lost connection!" << std::endl;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user