lx/Go2_navigation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始化项目，包含fast_lio_ws和unitree_sdk2

Browse Source
This commit is contained in:
龙正扬应用学生后端
2026-10-12 15:01:52 +08:00
commit 8e16ecceba
1490 changed files with 232338 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
unitree_sdk2/example/r1/CMakeLists.txt Normal file
View File

@@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 3.10)
add_executable(r1_loco_client high_level/r1_loco_client_example.cpp)
target_link_libraries(r1_loco_client unitree_sdk2)
add_executable(r1_ankle_swing_example low_level/r1_ankle_swing_example.cpp)
target_link_libraries(r1_ankle_swing_example unitree_sdk2)
add_executable(r1_audio_client_example audio/r1_audio_client_example.cpp)
target_link_libraries(r1_audio_client_example unitree_sdk2)
add_executable(r1A_wrist_swing_example low_level/r1A_wrist_swing_example.cpp)
target_link_libraries(r1A_wrist_swing_example unitree_sdk2)
add_executable(r1_arm_sdk_dds_example high_level/r1_arm_sdk_dds_example.cpp)
target_link_libraries(r1_arm_sdk_dds_example unitree_sdk2 fmt)

175
unitree_sdk2/example/r1/audio/r1_audio_client_example.cpp Normal file
View 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/r1/audio/audio_client.hpp>
#include "wav.hpp"
#define AUDIO_FILE_PATH "../example/r1/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::r1::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/r1/audio/test.wav Normal file
View File

Binary file not shown.

232
unitree_sdk2/example/r1/audio/wav.hpp Normal file
View 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

149
unitree_sdk2/example/r1/high_level/r1_arm_sdk_dds_example.cpp Normal file
View File

@@ -0,0 +1,149 @@
#include "unitree/dds_wrapper/robots/g1/g1.h"
#include "unitree/dds_wrapper/robots/r1/r1.h"
// High-level arm controller for the R1 robot.
// It subscribes to the robot's low-level state (joint feedback) and publishes
// arm commands over the "rt/arm_sdk" DDS topic, providing convenience helpers
// for enabling/disabling the arms and moving the joints to target positions.
class ArmController
{
public:
ArmController()
{
// Subscribe to the low-level state so we can read the current joint positions.
lowstate_ = std::make_shared<unitree::robot::g1::subscription::LowState>();
std::cout << "Waiting for connection to lowstate..." << std::endl;
lowstate_->wait_for_connection();
// Create the arm-sdk publisher that sends joint commands to the robot.
armsdk_ = std::make_unique<unitree::robot::r1::publisher::ArmSdk>();
std::cout << "Connected to robot!" << std::endl;
}
// Take control of the arms: ramp the SDK weight up to 1.0 and initialize each
// joint command to its current measured position so the arms hold still on start.
void enable()
{
// Already enabled (weight near 1.0), nothing to do.
if(armsdk_->weight() > 0.5) return;
// weight == 1.0 hands full control of the arms to this SDK.
armsdk_->weight(1.0);
for(int i(0); i<armsdk_->JOINTS.size(); ++i)
{
int JOINT = static_cast<int>(armsdk_->JOINTS[i]);
// Seed the target with the current position to avoid a sudden jump.
armsdk_->msg_.motor_cmd().at(JOINT).q(lowstate_->msg_.motor_state().at(JOINT).q());
// Position/velocity control gains for this joint.
armsdk_->msg_.motor_cmd().at(JOINT).kp(KP[i]);
armsdk_->msg_.motor_cmd().at(JOINT).kd(KD[i]);
// No desired velocity or feed-forward torque.
armsdk_->msg_.motor_cmd().at(JOINT).dq(0);
armsdk_->msg_.motor_cmd().at(JOINT).tau(0);
}
}
// Gradually release control of the arms by ramping the SDK weight from 1.0
// down to 0.0 over `duration` seconds, then let the robot regain control.
void release(float duration = 1.0f)
{
// Already released (weight near 0.0), nothing to do.
if(armsdk_->weight() < 0.5) return;
auto t0 = std::chrono::high_resolution_clock::now();
while(true)
{
// Elapsed time since the release started.
float elasped_time = std::chrono::duration<float>(std::chrono::high_resolution_clock::now() - t0).count();
if(elasped_time > duration) break;
// Linearly interpolate the weight from 1.0 down to 0.0.
float _i = (duration - elasped_time) / duration;
armsdk_->weight(std::clamp(_i, 0.f, 1.f));
armsdk_->unlockAndPublish();
// Publish at ~100 Hz.
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
// Write the desired joint positions (radians) into the command message.
// `q_des` must be ordered to match armsdk_->JOINTS.
void set_q(Eigen::VectorXf q_des)
{
for(int i(0); i<armsdk_->JOINTS.size(); ++i)
{
int JOINT = static_cast<int>(armsdk_->JOINTS[i]);
armsdk_->msg_.motor_cmd().at(JOINT).q(q_des[i]);
}
}
// Move the joints from their current positions to `q_target` (radians) using
// linear interpolation. `max_vel` (rad/s) caps the speed of the fastest joint,
// which determines the overall motion duration.
void movej(Eigen::VectorXf q_target, float max_vel = 1.0)
{
// Read the current joint positions from the low-level state feedback.
Eigen::VectorXf q_current(armsdk_->JOINTS.size());
for(int i(0); i<armsdk_->JOINTS.size(); ++i)
{
int JOINT = static_cast<int>(armsdk_->JOINTS[i]);
q_current[i] = lowstate_->msg_.motor_state().at(JOINT).q();
}
// Duration is set by the joint with the largest travel divided by max_vel.
float duration = (q_target - q_current).cwiseAbs().maxCoeff() / max_vel;
for(float t(0); t < duration; t += 0.01)
{
// Interpolation ratio in [0, 1].
float _i = t / duration;
Eigen::VectorXf q_des = q_current + (q_target - q_current) * _i;
set_q(q_des);
armsdk_->unlockAndPublish();
// Send commands at ~100 Hz.
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
std::shared_ptr<unitree::robot::g1::subscription::LowState> lowstate_; // Low-level joint state feedback
std::unique_ptr<unitree::robot::r1::publisher::ArmSdk> armsdk_; // Arm command publisher
private:
// Proportional (stiffness) gains, one per joint in armsdk_->JOINTS order.
std::vector<float> KP = {
50.0, 50.0, 40., 40., 30.,
50.0, 50.0, 40., 40., 30.,
50., 15., 15.
};
// Derivative (damping) gains, one per joint in armsdk_->JOINTS order.
std::vector<float> KD = {
2.0, 2.0, 2., 2., 2.,
2.0, 2.0, 2., 2., 2.,
3., 1., 1.
};
};
int main(int argc, char const *argv[])
{
// The network interface used for DDS communication must be provided.
if (argc < 2) {
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
exit(-1);
}
// Initialize the DDS channel factory on the given network interface.
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
auto arm = std::make_unique<ArmController>();
// Take control of the arms.
arm->enable();
// Target joint positions (radians), ordered to match ArmSdk::JOINTS:
// left arm (5), right arm (5), waist yaw, head pitch, head yaw.
Eigen::VectorXf q_target(arm->armsdk_->JOINTS.size());
q_target << 0.0, 1.57, 0.0, 1.57, 0.0,
0.0, -1.57, 0.0, -1.57, 0.0,
0.0, 0.0, 1.0;
// Move to the target pose, then smoothly release control back to the robot.
arm->movej(q_target, 1.0);
arm->release(1.0);
}

169
unitree_sdk2/example/r1/high_level/r1_loco_client_example.cpp Normal file
View File

@@ -0,0 +1,169 @@
#include <chrono>
#include <iostream>
#include <thread>
#include "unitree/robot/r1/loco/r1_loco_api.hpp"
#include "unitree/robot/r1/loco/r1_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}});
}
}
}
std::string network_interface = args["network_interface"];
unitree::robot::ChannelFactory::Instance()->Init(0, network_interface);
unitree::robot::r1::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 == "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_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 == "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_speed_mode") {
int param = std::stoi(arg_pair.second);
client.SetSpeedMode(param);
std::cout << "set speed mode to " << arg_pair.second << std::endl;
}
std::cout << "Done!" << std::endl;
}
return 0;
}

128
unitree_sdk2/example/r1/low_level/gamepad.hpp Normal file
View 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

336
unitree_sdk2/example/r1/low_level/r1A_wrist_swing_example.cpp Normal file
View File

@@ -0,0 +1,336 @@
#include <cmath>
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <array>
#include <algorithm>
#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 R1Z_NUM_MOTOR = 12;
struct ImuState {
std::array<float, 3> rpy = {};
std::array<float, 3> omega = {};
};
struct MotorCommand {
std::array<float, R1Z_NUM_MOTOR> q_target = {};
std::array<float, R1Z_NUM_MOTOR> dq_target = {};
std::array<float, R1Z_NUM_MOTOR> kp = {};
std::array<float, R1Z_NUM_MOTOR> kd = {};
std::array<float, R1Z_NUM_MOTOR> tau_ff = {};
};
struct MotorState {
std::array<float, R1Z_NUM_MOTOR> q = {};
std::array<float, R1Z_NUM_MOTOR> dq = {};
};
// Stiffness for all R1Z joints
std::array<float, R1Z_NUM_MOTOR> Kp{
100, 100, 100, 100, 50, // left arm
100, 100, 100, 100, 50, // right arm
50, 10 // head
};
// Damping for all R1Z joints
std::array<float, R1Z_NUM_MOTOR> Kd{
2, 2, 2, 2, 2, // left arm
2, 2, 2, 2, 2, // right arm
2, 0.1 // head
};
enum R1JointIndex {
LeftShoulderPitch = 0,
LeftShoulderRoll = 1,
LeftShoulderYaw = 2,
LeftElbow = 3,
LeftWristRoll = 4,
RightShoulderPitch = 5,
RightShoulderRoll = 6,
RightShoulderYaw = 7,
RightElbow = 8,
RightWristRoll = 9,
HEAD_PITCH = 10,
HEAD_YAW = 11,
};
std::array<int, R1Z_NUM_MOTOR> joint_idx_in_idl{
15, 16, 17, 18, 19,
22, 23, 24, 25, 26,
29, 30
};
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 R1Example {
private:
double time_;
double control_dt_; // [2ms]
double duration_; // [2 s]
int counter_;
uint8_t 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:
R1Example(std::string networkInterface)
: time_(0.0),
control_dt_(0.002),
duration_(2.0),
counter_(0),
mode_pr_(0),
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(&R1Example::LowStateHandler, this, std::placeholders::_1), 1);
imutorso_subscriber_.reset(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
imutorso_subscriber_->InitChannel(std::bind(&R1Example::imuTorsoHandler, this, std::placeholders::_1), 1);
// create threads
command_writer_ptr_ = CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000, &R1Example::LowCommandWriter, this);
control_thread_ptr_ = CreateRecurrentThreadEx("control", UT_CPU_ID_NONE, 2000, &R1Example::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 < R1Z_NUM_MOTOR; ++i) {
int idl_index = joint_idx_in_idl[i];
ms_tmp.q.at(i) = low_state.motor_state()[idl_index].q();
ms_tmp.dq.at(i) = low_state.motor_state()[idl_index].dq();
if (low_state.motor_state()[idl_index].motorstate())
std::cout << "[ERROR] motor " << idl_index << " with code " << low_state.motor_state()[idl_index].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 << "R1 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:", R1Z_NUM_MOTOR);
printf("\nmode: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%u,", ms[joint_idx_in_idl[i]].mode());
printf("\npos: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%.2f,", ms[joint_idx_in_idl[i]].q());
printf("\nvel: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%.2f,", ms[joint_idx_in_idl[i]].dq());
printf("\ntau_est: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%.2f,", ms[joint_idx_in_idl[i]].tau_est());
printf("\ntemperature: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%d,%d;", ms[joint_idx_in_idl[i]].temperature()[0], ms[joint_idx_in_idl[i]].temperature()[1]);
printf("\nvol: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%.2f,", ms[joint_idx_in_idl[i]].vol());
printf("\nsensor: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%u,%u;", ms[joint_idx_in_idl[i]].sensor()[0], ms[joint_idx_in_idl[i]].sensor()[1]);
printf("\nmotorstate: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%u,", ms[joint_idx_in_idl[i]].motorstate());
printf("\nreserve: ");
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) printf("%u,%u,%u,%u;", ms[joint_idx_in_idl[i]].reserve()[0], ms[joint_idx_in_idl[i]].reserve()[1], ms[joint_idx_in_idl[i]].reserve()[2], ms[joint_idx_in_idl[i]].reserve()[3]);
printf("\n");
}
}
void LowCommandWriter() {
LowCmd_ dds_low_command;
dds_low_command.mode_pr() = mode_pr_;
dds_low_command.mode_machine() = mode_machine_;
const std::shared_ptr<const MotorCommand> mc = motor_command_buffer_.GetData();
if (mc) {
for (int i = 0; i < R1Z_NUM_MOTOR; i++) {
int idl_index = joint_idx_in_idl[i];
dds_low_command.motor_cmd().at(idl_index).mode() = 1; // 1:Enable, 0:Disable
dds_low_command.motor_cmd().at(idl_index).tau() = mc->tau_ff.at(i);
dds_low_command.motor_cmd().at(idl_index).q() = mc->q_target.at(i);
dds_low_command.motor_cmd().at(idl_index).dq() = mc->dq_target.at(i);
dds_low_command.motor_cmd().at(idl_index).kp() = mc->kp.at(i);
dds_low_command.motor_cmd().at(idl_index).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 < R1Z_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 r1z arms and head to zero posture
double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
for (int i = 0; i < R1Z_NUM_MOTOR; ++i) {
motor_command_tmp.q_target.at(i) = (1.0 - ratio) * ms->q.at(i);
}
} else {
const double t = std::fmod(time_ - duration_, duration_ * 2.0);
mode_pr_ = 0;
if (t < duration_) {
// [Stage 2]: swing wrist roll joints for 2 s
double max_wrist_roll = M_PI * 30.0 / 180.0;
double wrist_roll_des = max_wrist_roll * std::sin(2.0 * M_PI * t / duration_);
motor_command_tmp.q_target.at(LeftWristRoll) = wrist_roll_des;
motor_command_tmp.q_target.at(RightWristRoll) = -wrist_roll_des;
} else {
// [Stage 3]: swing head pitch and yaw joints for 2 s
double head_t = t - duration_;
double max_head_yaw = M_PI * 30.0 / 180.0;
motor_command_tmp.q_target.at(HEAD_YAW) = max_head_yaw * std::sin(2.0 * M_PI * head_t / duration_);
}
}
motor_command_buffer_.SetData(motor_command_tmp);
}
}
};
int main(int argc, char const *argv[]) {
if (argc < 2) {
std::cout << "Usage: r1A_wrist_swing_example network_interface" << std::endl;
exit(0);
}
std::string networkInterface = argv[1];
R1Example custom(networkInterface);
while (true) sleep(10);
return 0;
}

378
unitree_sdk2/example/r1/low_level/r1_ankle_swing_example.cpp Normal file
View File

@@ -0,0 +1,378 @@
#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 R1_NUM_MOTOR = 26;
struct ImuState {
std::array<float, 3> rpy = {};
std::array<float, 3> omega = {};
};
struct MotorCommand {
std::array<float, R1_NUM_MOTOR> q_target = {};
std::array<float, R1_NUM_MOTOR> dq_target = {};
std::array<float, R1_NUM_MOTOR> kp = {};
std::array<float, R1_NUM_MOTOR> kd = {};
std::array<float, R1_NUM_MOTOR> tau_ff = {};
};
struct MotorState {
std::array<float, R1_NUM_MOTOR> q = {};
std::array<float, R1_NUM_MOTOR> dq = {};
};
// Stiffness for all R1 Joints
std::array<float, R1_NUM_MOTOR> Kp{
200, 200, 200, 200, 200, 200, // legs
200, 200, 200, 200, 200, 200, // legs
300, 300, // waist
100, 100, 100, 100, 50, // arms
100, 100, 100, 100, 50, // arms
50, 10 // head
};
// Damping for all R1 Joints
std::array<float, R1_NUM_MOTOR> Kd{
3, 3, 3, 3, 3, 3, // legs
3, 3, 3, 3, 3, 3, // legs
5, 5, // waist
2, 2, 2, 2, 2, // arms
2, 2, 2, 2, 2, // arms
2, 0.1 // head
};
std::array<float, R1_NUM_MOTOR> joint_idx_in_idl{
0, 1, 2, 3, 4, 5,
6, 7, 8, 9, 10, 11,
12, 13,
15, 16, 17, 18, 19,
22, 23, 24, 25, 26,
29, 30
};
enum class Mode {
PR = 0, // Series Control for Ptich/Roll Joints
AB = 1 // Parallel Control for A/B Joints
};
enum R1JointIndex {
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,
WaistRoll = 12,
WaistYaw = 13,
LeftShoulderPitch = 14,
LeftShoulderRoll = 15,
LeftShoulderYaw = 16,
LeftElbow = 17,
LeftWristRoll = 18,
RightShoulderPitch = 19,
RightShoulderRoll = 20,
RightShoulderYaw = 21,
RightElbow = 22,
RightWristRoll = 23,
HEAD_PITCH = 24,
HEAD_YAW = 25,
};
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 R1Example {
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:
R1Example(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(&R1Example::LowStateHandler, this, std::placeholders::_1), 1);
imutorso_subscriber_.reset(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
imutorso_subscriber_->InitChannel(std::bind(&R1Example::imuTorsoHandler, this, std::placeholders::_1), 1);
// create threads
command_writer_ptr_ = CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000, &R1Example::LowCommandWriter, this);
control_thread_ptr_ = CreateRecurrentThreadEx("control", UT_CPU_ID_NONE, 2000, &R1Example::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 < R1_NUM_MOTOR; ++i) {
ms_tmp.q.at(i) = low_state.motor_state()[joint_idx_in_idl[i]].q();
ms_tmp.dq.at(i) = low_state.motor_state()[joint_idx_in_idl[i]].dq();
if (low_state.motor_state()[joint_idx_in_idl[i]].motorstate() && joint_idx_in_idl[i] <= RightAnkleRoll)
std::cout << "[ERROR] motor " << joint_idx_in_idl[i] << " with code " << low_state.motor_state()[joint_idx_in_idl[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 << "R1 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:", R1_NUM_MOTOR);
printf("\nmode: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%u,", ms[joint_idx_in_idl[i]].mode());
printf("\npos: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%.2f,", ms[joint_idx_in_idl[i]].q());
printf("\nvel: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%.2f,", ms[joint_idx_in_idl[i]].dq());
printf("\ntau_est: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%.2f,", ms[joint_idx_in_idl[i]].tau_est());
printf("\ntemperature: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%d,%d;", ms[joint_idx_in_idl[i]].temperature()[0], ms[joint_idx_in_idl[i]].temperature()[1]);
printf("\nvol: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%.2f,", ms[joint_idx_in_idl[i]].vol());
printf("\nsensor: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%u,%u;", ms[joint_idx_in_idl[i]].sensor()[0], ms[joint_idx_in_idl[i]].sensor()[1]);
printf("\nmotorstate: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%u,", ms[joint_idx_in_idl[i]].motorstate());
printf("\nreserve: ");
for (int i = 0; i < R1_NUM_MOTOR; ++i) printf("%u,%u,%u,%u;", ms[joint_idx_in_idl[i]].reserve()[0], ms[joint_idx_in_idl[i]].reserve()[1], ms[joint_idx_in_idl[i]].reserve()[2], ms[joint_idx_in_idl[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 < R1_NUM_MOTOR; i++) {
dds_low_command.motor_cmd().at(joint_idx_in_idl[i]).mode() = 1; // 1:Enable, 0:Disable
dds_low_command.motor_cmd().at(joint_idx_in_idl[i]).tau() = mc->tau_ff.at(i);
dds_low_command.motor_cmd().at(joint_idx_in_idl[i]).q() = mc->q_target.at(i);
dds_low_command.motor_cmd().at(joint_idx_in_idl[i]).dq() = mc->dq_target.at(i);
dds_low_command.motor_cmd().at(joint_idx_in_idl[i]).kp() = mc->kp.at(i);
dds_low_command.motor_cmd().at(joint_idx_in_idl[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 < R1_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 < R1_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 * 30.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: r1_ankle_swing_example network_interface" << std::endl;
exit(0);
}
std::string networkInterface = argv[1];
R1Example custom(networkInterface);
while (true) sleep(10);
return 0;
}