🛠️PND Adam 部署 SDK
使用 PND Adam 部署 SDK 有两种方式:从源代码构建或使用 Docker。
🏗️从源代码构建
pnd_adam_deploy_public 是为 PND 人形机器人开发的 SDK,包含机器人的仿真到真实(sim2real)/仿真到仿真(sim2sim)实验程序。
本文介绍如何使用 pnd_adam_deploy_public 进行 PND 人形机器人的快速开发与应用。
环境要求
- 推荐使用 Ubuntu 20.04 进行开发,目前暂不支持 Mac 和 Windows 系统开发。
- 若要在真实机器人上运行,请先将用户计算机与机器人 NUC 连接到同一网络。
通过脚本安装依赖
- 我们在代码中提供了安装依赖的脚本
- “代码中的
install.sh
脚本将构建 RBDL、下载 LibTorch,并提供编译 MuJoCo 的选项。” - 在运行 install.sh 之前,请先执行以下命令:
sudo apt update
sudo apt upgrade
sudo apt install wget libeigen3-dev libglfw3-dev libxinerama-dev libxcursor-dev libxi-dev libssl-dev libx11-dev
- 获取代码并运行 install.sh(x.x.x 表示版本号):
##进入路径
cd ~/Docments
##获取代码
git clone https://github.com/pndbotics/pnd_adam_deploy_public
cd pnd_adam_deploy_public
##运行脚本
sh install.sh
手动安装依赖
🐢如果需要使用 ROS2 构建
构建 robotPublisher
构建
要构建代码:
- 这将在项目中创建 build、bin 和 log 文件夹,并将所需文件安装到 bin 文件夹中。
cd robotPublisher
source install/setup.bash
cd ..
sh build.sh adam_lite|adam_inspire|adam_standard real|mujoco ros2
运行前准备
#打开另一个终端
#如果运行 Mujoco
ros2 run joy joy_node
#如果在真实机器人上运行
sudo su
source /opt/ros/humble/setup.bash
ros2 run joy joy_node
运行 Python 控制器
#如果运行 Mujoco
cd robotPublisher
source install/setup.bash
cd ../example/python/
python Controller.py
#如果在真实机器人上运行
sudo su
source /opt/ros/humble/setup.bash
cd robotPublisher
source install/setup.bash
cd ../example/python/
python Controller.py
运行 ros2 代码
#如果运行 Mujoco
cd robotPublisher
source install/setup.bash
cd ../build_adam_lite_mujoco
./pnd_adam_deploy_public
#如果在真实机器人上运行
sudo su
source /opt/ros/humble/setup.bash
cd robotPublisher
source install/setup.bash
sh run.sh
如果运行正常,您将看到三个终端窗口运行三个 ros2 节点
如果需要清理构建结果:
- 这将删除 build 和 bin 文件夹中的所有内容。如果只需正常重新构建,无需执行此操作,直接再次运行
sh build.sh adam_lite real
即可。
🤖构建并运行 Mujoco
- urdf2mujoco
- 在使用 Mujoco 进行仿真验证之前,需要注释掉 urdf 中 floating_joint 的所有轴,然后转换为 xml。提供的脚本文件位于
mujoco/urdf2mujocoXml.py
。
通过脚本构建
在 Mujoco 中运行仿真
- 运行测试
- 机器人操作流程可在机器人操作说明文档中找到。
- 进入构建路径并运行:
进入 Mujoco 仿真环境后,先按 xbox 手柄上的 A
键,再按 X
键。然后点击 Reset
几次或按键盘上的 Backspace 键,最后按 xbox 手柄上的 Y
键让 Adam 行走。
构建并运行真实机器人
当终端显示“FSM start!”信息后,即可开始控制。 按按钮 A 使机器人所有关节回到半蹲零位;按按钮 X 使机器人运行 MLP 或 DEMO;按按钮 B 使所有关节停在当前位置;同时按下 LT 和 RT 键禁用关节并退出控制程序;同时按下 LB 和 RB 键开启和关闭关节电源。
🗃️文件说明
- example/src/main.cpp
- mujoco
- adam_lite
- urdf2mujocoXml.py
- python_scripts
- source/abs.json
- example.pt
- check_abs.py
- read_abs.py
- set_zero.py
- transtojit.py
- robot_interface
- config/joint_pd_config.json
- include/pnd_algorithm.h
- lib/libxxx.so
- src
- mujoco
- pnd-cpp-sdk
- vnIMU
- basic_function.h
- fsm_state_impl.h
- joystick.h
- pconfig.hpp
- pdata_handler.hpp
- putil.h
- real_robot.h
- robot_common.hpp
- robot_handler.h
- third_party
main.cpp
程序入口 主循环
// 2.5 ms 定时循环
while (true) {
total_time = timer.currentTime() - start_time; // 总时间
start_time = timer.currentTime();
framework.getState(time_fsm, robot_data); // 从机器人获取状态(位置、速度、电流)
get_state_time = timer.currentTime() - start_time; // 获取状态执行时间
framework.runFSM(); // 运行有限状态机(计算时间不能超过 1.5ms)
fsm_time = timer.currentTime() - start_time - get_state_time; // 有限状态机执行时间
framework.setCommand(robot_data); // 向关节发送命令
cmd_time = timer.currentTime() - start_time - fsm_time - get_state_time; // 发送命令执行时间
time_fsm += kDt;
#ifdef DATALOG
// 写入数据
DataHandler::getInstance().cacheData(robot_data, time_fsm, get_state_time, fsm_time, cmd_time, start_time,
total_time, timer, framework.getCurrentState());
DataHandler::getInstance().writeData(robot_data);
#endif
if (framework.disableJoints || robot_data.error_state_) {
break;
}
abs.json
机器人执行器末端绝对编码器(abs)角度输出文件。
{
"10.10.10.100": {
"angle": 100.063,
"radian": 1.74644
},
"10.10.10.101": {
"angle": 141.24,
"radian": 2.46549
},
"10.10.10.102": {
"angle": 210.894,
"radian": 3.68079
},
"10.10.10.60": {
"angle": 322.69,
"radian": 5.63201
},
"10.10.10.61": {
"angle": 268.989,
"radian": 4.69436
},
"10.10.10.62": {
"angle": 120.52,
"radian": 2.10424
},
"10.10.10.63": {
"angle": 269.275,
"radian": 4.69973
},
"10.10.10.64": {
"angle": 300.784,
"radian": 5.25043
},
"10.10.10.65": {
"angle": 238.953,
"radian": 4.17051
},
"10.10.10.80": {
"angle": 154.731,
"radian": 2.70019
},
"10.10.10.81": {
"angle": 113.796,
"radian": 1.98612
},
"10.10.10.82": {
"angle": 7.16309,
"radian": 0.124636
},
"10.10.10.83": {
"angle": 275.449,
"radian": 4.80826
},
"10.10.10.84": {
"angle": 355.298,
"radian": 6.20112
},
"10.10.10.85": {
"angle": 213.684,
"radian": 3.72949
}
}
read_abs.py
读取 abs 数据,写入 source/abs.json
check_abs.py
检查 abs 数据是否读取成功。
set_zero.py
设置机器人零位脚本。
transtojit.py
将 RL 的 pt 文件转换为 C++ 可调用的 pt 文件。
$ python transtojit.py -s adam_stand_10000.pt -t adam_stand.pt
path:
stand_pndone_25000.pt
{'model_state_dict': OrderedDict([('std', tensor([1.6929e-01, 9.5175e-02, 6.9768e-02, 1.0085e-01, 8.1877e-01, 7.6450e+01,
1.6929e-01, 9.5175e-02, 6.9940e-02, 1.0085e-01, 8.2536e-01, 7.6404e+01,
1.2998e-01, 6.7588e-02, 1.0899e-01, 2.9934e-01, 4.4219e-01, 2.5290e-01,
2.8740e-01, 2.9462e-01, 4.4770e-01, 2.4966e-01, 2.8390e-01])), ('actor.0.weight', tensor([[-0.0170, -0.0221, -0.0417, ..., -0.1688, 0.0079, 2.1972],
[ 0.0246, 0.0244, 0.1711, ..., -0.2499, -0.3722, -0.2657],
[ 0.0785, 0.0296, -0.0664, ..., 0.2183, -0.1903, 0.7271],
...,]))])}
......
init trained PPO model
time:0.0010631084442138672
......
tensor([ 1.5421e-01, 9.7481e-03, 1.1233e-03, 4.6039e-02, 4.0215e-02,
1.7328e+01, -5.4237e-02, -1.7513e-02, 1.9551e-02, 3.8487e-02,
2.2185e-01, -1.6806e+01, 4.2187e-02, 1.8758e-01, 8.5998e-03,
9.3276e-02, 2.0171e-01, 7.7595e-02, -9.9556e-02, -4.6954e-02,
-1.7874e-01, 4.6878e-02, 2.3827e-02])
joint_pd_config.json
关节 PD 参数和零位配置。
pnd_algorithm.h
机器人算法接口。
struct RobotData {
// 向量说明:
// [0]:浮动基座 x
// [1]:浮动基座 y
// [2]:浮动基座 z
// [3]:浮动基座 横滚角
// [4]:浮动基座 俯仰角
// [5]:浮动基座 偏航角
// [6-28]:参考 RobotInterfaceImpl.cpp:第20-46行
// 实际位置
Eigen::VectorXd q_a_ = Eigen::VectorXd::Zero(kRobotDataSize);
// 实际速度
Eigen::VectorXd q_dot_a_ = Eigen::VectorXd::Zero(kRobotDataSize);
// 实际扭矩
Eigen::VectorXd tau_a_ = Eigen::VectorXd::Zero(kRobotDataSize);
// 期望位置
Eigen::VectorXd q_d_ = Eigen::VectorXd::Zero(kRobotDataSize);
// 期望速度
Eigen::VectorXd q_dot_d_ = Eigen::VectorXd::Zero(kRobotDataSize);
// 期望扭矩
Eigen::VectorXd tau_d_ = Eigen::VectorXd::Zero(kRobotDataSize);
// 向量说明:yaw pitch roll gyro_x gyro_y gyro_z acc_x acc_y acc_z
Eigen::VectorXd imu_data_ = Eigen::VectorXd::Zero(9);
bool pos_mode_ = true;
bool error_state_ = false;
#ifdef DATALOG
Eigen::VectorXd dataL = Eigen::VectorXd::Zero(350);
#endif
};
pnd-cpp-sdk
vmIMU
IMU 相关代码
basic_function.h
基础函数。
void copy_to_inputs(torch::Tensor &input_data, Eigen::VectorXd inputs_mlp, int num);
void FifthPoly(Eigen::VectorXd p0, Eigen::VectorXd p0_dot, Eigen::VectorXd p0_dotdot, // 起点状态
Eigen::VectorXd p1, Eigen::VectorXd p1_dot, Eigen::VectorXd p1_dotdot, // 终点状态
double totalTime, // 总时间
double currentTime, // 当前时间,从 0 到 totalTime
Eigen::VectorXd &pd, Eigen::VectorXd &pd_dot, Eigen::VectorXd &pd_dotdot);// 输出命令
fsm_state_impl.h
每个状态机均在此文件中定义。
class StateZero : public FSMState {
public:
explicit StateZero(RobotData *robot_data);
void onEnter() override;
void run() override;
FSMStateName checkTransition() override;
void onExit() override;
private:
double total_time = 2.0;
bool zero_finish_flag = false;
Eigen::VectorXd init_joint_pos = Eigen::VectorXd::Zero(kRobotDof);
};
class StateMLP : public FSMState {
public:
explicit StateMLP(RobotData *robot_data);
~StateMLP() override;
void onEnter() override;
void run() override;
FSMStateName checkTransition() override;
void onExit() override;
private:
Eigen::VectorXd input_data_mlp;
Eigen::VectorXd output_data_mlp = Eigen::VectorXd::Zero(kRobotDof);
float *input_array;
};
class StateStop : public FSMState {
public:
explicit StateStop(RobotData *robot_data);
void onEnter() override;
void run() override;
FSMStateName checkTransition() override;
void onExit() override;
private:
Eigen::VectorXd init_joint_pos = Eigen::VectorXd::Zero(kRobotDof);
};
joystick.h
定义手柄类,实现手柄数据采集并转换为切换状态机指令。
pconfig.hpp
定义 Adam 关节和关节绝对编码器配置类。
此结构不可自定义,需从 PConfig 获取。
struct PCfg_ {
// 关节配置
std::string ip;
std::string name;
int joint_dir;
double joint_gear_ratio;
double c_t_scale;
double zero_pos;
double default_dof_pos;
double kd_scale;
// 绝对编码器配置
float abs_pos_gear_ratio;
float abs_pos_zero;
float abs_pos_dir;
PCfg_(std::string ip, std::string name, int joint_dir, double joint_gear_ratio, double c_t_scale, double zero_pos,
double default_dof_pos, double kd_scale, float abs_pos_gear_ratio, float abs_pos_zero, float abs_pos_dir)
: ip(ip),
name(name),
joint_dir(joint_dir),
joint_gear_ratio(joint_gear_ratio),
c_t_scale(c_t_scale),
zero_pos(zero_pos),
default_dof_pos(default_dof_pos),
kd_scale(kd_scale),
abs_pos_gear_ratio(abs_pos_gear_ratio),
abs_pos_zero(abs_pos_zero),
abs_pos_dir(abs_pos_dir) {}
};
addCfg("10.10.10.70", "hipPitch_Left", -1, 7, 0.227, -0.41, -0.586, 1.66, 0, 0, 0);
addCfg("10.10.10.71", "hipRoll_Left", -1, 31, 0.136, -0.04, -0.085, 20.0, 0, 0, 0);
addCfg("10.10.10.72", "hipYaw_Left", 1, 51, 0.074, -0.23, -0.322, 30.1, 0, 0, 0);
addCfg("10.10.10.73", "kneePitch_Left", 1, 7, 0.227, 0.81, 1.288, 1.66, 0, 0, 0);
addCfg("10.10.10.74", "anklePitch_Left", -1, 30, 0.0592, -0.47, -0.789, 16.5, 0, 0, 0);
addCfg("10.10.10.75", "ankleRoll_Left", 1, 30, 0.0592, 0.0, 0.002, 16.5, 0, 0, 0);
addCfg("10.10.10.50", "hipPitch_Right", 1, 7, 0.227, -0.41, -0.586, 1.66, 0, 0, 0);
addCfg("10.10.10.51", "hipRoll_Right", -1, 31, 0.136, 0.04, 0.085, 20.0, 0, 0, 0);
addCfg("10.10.10.52", "hipYaw_Right", 1, 51, 0.074, 0.23, 0.322, 30.1, 0, 0, 0);
addCfg("10.10.10.53", "kneePitch_Right", -1, 7, 0.227, 0.81, 1.288, 1.66, 0, 0, 0);
addCfg("10.10.10.54", "anklePitch_Right", 1, 30, 0.0592, -0.47, -0.789, 16.5, 0, 0, 0);
addCfg("10.10.10.55", "ankleRoll_Right", -1, 30, 0.0592, 0.0, 0.002, 16.5, 0, 0, 0);
addCfg("10.10.10.90", "waistRoll", 1, 51, 0.074, 0.0, 0.0, 30.1, 0, 0, 0);
addCfg("10.10.10.91", "waistPitch", -1, 51, 0.074, 0.0, 0.0, 30.1, 0, 0, 0);
addCfg("10.10.10.92", "waistYaw", -1, 51, 0.074, 0.0, 0.0, 30.1, 0, 0, 0);
addCfg("10.10.10.10", "shoulderPitch_Left", -1, 51, 0.0592, 0.0, 0.0, 8.25, 0, 0, 0);
addCfg("10.10.10.11", "shoulderRoll_Left", 1, 51, 0.0592, 0.0, 0.0, 8.25, 0, 0, 0);
addCfg("10.10.10.12", "shoulderYaw_Left", 1, 51, 0.063, 0.0, 0.0, 19.8, 0, 0, 0);
addCfg("10.10.10.13", "elbow_Left", -1, 51, 0.063, -0.3, -0.3, 19.8, 0, 0, 0);
addCfg("10.10.10.14", "wristYaw_Left", 1, 51, 0.063, 0.0, 0.0, 19.8, 0, 0, 0);
addCfg("10.10.10.30", "shoulderPitch_Right", 1, 51, 0.0592, 0.0, 0.0, 8.25, 0, 0, 0);
addCfg("10.10.10.31", "shoulderRoll_Right", 1, 51, 0.0592, 0.0, 0.0, 8.25, 0, 0, 0);
addCfg("10.10.10.32", "shoulderYaw_Right", 1, 51, 0.063, 0.0, 0.0, 19.8, 0, 0, 0);
addCfg("10.10.10.33", "elbow_Right", 1, 51, 0.063, -0.3, -0.3, 19.8, 0, 0, 0);
addCfg("10.10.10.34", "wristYaw_Right", 1, 51, 0.063, 0.0, 0.0, 19.8, 0, 0, 0);
// 以上定义了 adam_lite 版本的 25 个关节配置,与 robot_data 中的顺序一致。
PConfig 是全局单例类,用于获取机器人的一些配置信息。
请勿修改此数据
class PConfig {
public:
static PConfig& getInst() {
static PConfig instance;
return instance;
}
······
};
pdata_handler.hpp
DataHandler 类提供异步写入数据的功能,实现机器人通信时间、循环时间、关节数据等的记录。文件写入 bin 文件夹,以时间戳命名。无编号的文件名表示最近写入的文件。
class DataHandler {
public:
static DataHandler& getInstance() {
static DataHandler instance;
return instance;
}
putil.h
定义通过关节 ID 获取和设置关节信息的函数。
void getInfoFormJointIds(const Eigen::VectorXd& joint_info, const std::vector<int>& joint_ids, Eigen::VectorXd& info);
void setInfoFromJointIds(const Eigen::VectorXd& info, const std::vector<int>& joint_ids, Eigen::VectorXd& joint_info);
real_robot.h
初始化机器人的一些信息,并调用 joint_interface 类与机器人执行器通信。
robot_common.hpp
定义一些通用基类。 状态机基类,所有定义的子状态机必须继承于此。
class FSMState {
public:
explicit FSMState(RobotData *robot_data) { robot_data_ = robot_data; };
virtual ~FSMState() = default;
// 进入状态时执行的行为
virtual void onEnter() = 0;
// 状态的正常运行行为
virtual void run() = 0;
// 管理状态特定的转换
virtual FSMStateName checkTransition() = 0;
// 退出状态时执行的行为
virtual void onExit() = 0;
FSMStateName current_state_name_;
protected:
const int freq_ = 4;
double timer_ = 0.;
RobotData *robot_data_;
};
robot_handler.h
框架类,包含主流程和状态机切换,以下是状态机切换的实现细节。
void Framework::runFSM() {
if (first_run_) {
current_state_->onEnter();
first_run_ = false;
std::cout << "FSM start!" << std::endl;
}
next_state_name_ = current_state_->checkTransition();
if (next_state_name_ != current_state_->current_state_name_) {
next_state_ = getNextState(next_state_name_);
current_state_->onExit();
current_state_ = next_state_;
current_state_->onEnter();
}
current_state_->run();
if (JsHum::getInst().disableJoints()) {
disableJoints = true;
}
}
手柄结构
手柄通过蓝牙连接时,操作按钮对应的宏与结构体;机器人控制指令已封装到 joystick.cpp 中。
#define XBOX_TYPE_BUTTON 0x01
#define XBOX_TYPE_AXIS 0x02
// 蓝牙映射
#define XBOX_BUTTON_A 0x00
#define XBOX_BUTTON_B 0x01
#define XBOX_BUTTON_X 0x03
#define XBOX_BUTTON_Y 0x04
#define XBOX_BUTTON_LB 0x06
#define XBOX_BUTTON_RB 0x07
#define XBOX_BUTTON_START 0x02
#define XBOX_BUTTON_BACK 0x05
#define XBOX_BUTTON_HOME 0x08
#define XBOX_BUTTON_LO 0x09 /* 左摇杆按钮 */
#define XBOX_BUTTON_RO 0x0a /* 右摇杆按钮 */
#define XBOX_BUTTON_ON 0x01
#define XBOX_BUTTON_OFF 0x00
#define XBOX_AXIS_LX 0x00 /* 左摇杆 X 轴 */
#define XBOX_AXIS_LY 0x01 /* 左摇杆 Y 轴 */
#define XBOX_AXIS_RX 0x02 /* 右摇杆 X 轴 */
#define XBOX_AXIS_RY 0x03 /* 右摇杆 Y 轴 */
#define XBOX_AXIS_LT 0x05
#define XBOX_AXIS_RT 0x04
#define XBOX_AXIS_XX 0x06 /* 方向键 X 轴 */
#define XBOX_AXIS_YY 0x07 /* 方向键 Y 轴 */
#define XBOX_AXIS_VAL_UP -32767
#define XBOX_AXIS_VAL_DOWN 32767
#define XBOX_AXIS_VAL_LEFT -32767
#define XBOX_AXIS_VAL_RIGHT 32767
#define XBOX_AXIS_VAL_MIN -32767
#define XBOX_AXIS_VAL_MAX 32767
#define XBOX_AXIS_VAL_MID 0x00
typedef struct xbox_map {
int time;
int a;
int b;
int x;
int y;
int lb;
int rb;
int start;
int back;
int home;
int lo;
int ro;
int lx;
int ly;
int rx;
int ry;
int lt;
int rt;
int xx;
int yy;
} xbox_map_t;
🔣Kp、Kd 参数说明
在 Isaac Gym 中
PD 控制器:
\(\tau=K_p*e_p+K_d*e_v\) (1)
\(K_p\):PD 控制器刚度
\(K_d\):PD 控制器阻尼
\(e_p\):位置误差
\(e_v\):速度误差
\(\tau\):作用于末端的扭矩
在真实机器人执行器中
PID 控制器:
\(I=P_p*P_v*e_p+P_v*e_v\) (2)
\(\tau'=g*I*K_t=g*(P_p*P_v*K_t*e_p+P_v*K_t*e_v)\) (3)
\(P_p\):位置控制环 PID 控制器比例参数
\(P_v\):速度控制环 PID 控制器比例参数
\(e_p\):位置误差
\(e_v\):速度误差
\(g\):末端与电机之间的齿轮比
\(K_t\):扭矩常数,可通过实验获取
\(I\):位置和速度控制环 PID 控制器输出的电流
\(\tau'\):作用于末端的扭矩
为实现仿真到真实(sim to real),需满足 \(\tau=\tau'\),由此可得:
\(K_p=g*P_p*P_v*K_t\) (4)
\(K_d=g*P_v*K_t\) (5)
\(P_v=K_d/(K_t*g)\) (6)
\(P_p=K_p/K_d\) (7)
版本说明
❓常见问题
FIND_PACKAGE Torch
请查看 README 中的安装步骤
错误代码
⚓使用 Docker
下载链接: pnd_adam_deploy_public_docker
安装 Docker
🧑🏫快速入门
如果您的计算机有 Nvidia GPU
- 首先安装 NVIDIA 容器工具包:
distribution=$(. /etc/os-release;echo $ID$VERSION_ID) \
&& curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \
&& curl -s -L https://nvidia.github.io/libnvidia-container/$distribution/libnvidia-container.list | \
sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
- 更新软件包列表并安装 NVIDIA 容器工具包:
- 配置 Docker 使用 NVIDIA 运行时并重启 Docker 服务:
- 运行针对 NVIDIA 的脚本:
如果您的计算机没有 GPU
- 运行 Docker 脚本: