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

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;
}