Skip to content

🛠️PND Adam Deploy SDK

There are two ways to use PND Adam Deploy SDK: Build from source or use the docker.

⚓Use the docker

Download link:

pnd_adam_deploy_public_docker

Install docker

sudo apt-get update
sudo apt-get install docker docker.io docker-buildx

🧑‍🏫Getting started

If your computer has Nvidia_GPU

  • install NVIDIA Container Toolkit first:
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
  • Update the package list and install the NVIDIA Container Toolkit:
sudo apt update
sudo apt install -y nvidia-container-toolkit
  • Configure Docker to use the NVIDIA runtime and restart docker server:
sudo nvidia-ctk runtime configure --runtime=docker

sudo systemctl restart docker
  • run the scripts for nvidia
sudo sh runDockerNvidia.sh

If your computer has no GPU

  • run the script for Docker
docker build -t pndbotics:v121 .
sh runDocker.sh

After you are in docker env

cd pnd_adam_deploy_public-1.2.1
sh install.sh

For Mujoco

sh build.sh adam_lite mujoco
sh run_mujoco.sh adam_lite

Exit docker

#ctrl+d to exit the docker env

Run the docker container

docker ps -a #find the docker id
docker start ${docker id}
sudo docker exec -it ${docker id} bash

🏗️Build from source

pnd_adam_deploy_public It is the SDK developed for PND humanoid robot, and it is contain the sim2real/sim2sim experiment program of the robot.

This article describes how to use pnd_adam_deploy_public for rapid development and application of PND humanoid robots.

Environment

  • Ubuntu 20.04 is recommended for development. Mac and Windows are not currently supported for development.
  • If you want to run it on real robot please Connect the user computer to the same network as the robot NUC first.

Install Dependencies by Script

  • we offer script for install the Dependencies in code
  • "The install.sh script in code will build RBDL, download LibTorch, and provide an option to compile MuJoCo."
  • Before running install.sh, execute the following commands first
  • sudo apt update
  • sudo apt upgrade
  • sudo apt install wget libeigen3-dev libglfw3-dev libxinerama-dev libxcursor-dev libxi-dev libssl-dev libx11-dev
  • Get code then run the install.sh
  • x.x.x indicates the version number
##Enter path
cd ~/Docments

##get code
wget https://pndwiki.oss-cn-beijing.aliyuncs.com/sdk_adlskjfas412838_sakjfhrjdsaljf_skfj3jskdjfd32-38439/pnd_adam_deploy_public-1.4.0.tar.gz
tar -xzf pnd_adam_deploy_public-1.2.2.tar.gz
cd pnd_adam_deploy_public

## run scripts
sh install.sh

Manual install Dependencies

  • Eigen
  • rbdl
  • libtorch
  • (You need to download and unzip libtorch and place it in the pnd_adam_deploy_public floder).

🐢If you want to build with ROS2

#build the ros2 publisher
  cd robotPublisher
  colcon build
  source install/setup.bash
  cd ../example/python
  python Controller.py

Build

To build the code:

  • which will create folder build and folder bin in your project and install the required files into folder bin.
cd robotPublisher
source install/setup.bash
cd ..
sh build.sh adam_lite|adam_inspire|adam_standard real|mujoco ros2

Before Run

#open one more terminal
ros2 run joy joy_node

Run ros2 code

##in mujoco
cd build_adam_lite_mujoco
./pnd_adam_deploy_public

##in real
sh run.sh

If you need to clean your build results:

  • which will delete all contents in folder build and bin. If you just want to normal rebuild, you do not need to do this, but simply run sh build.sh adam_lite real again.
sh cleanrobot.sh

🤖Build & Run Mujoco

  • urdf2mujoco
  • Before using Mujoco for simulation verification, all axes in the floating_joint in your urdf need to be commented out and then converted to xml. The provided script file is located at mujoco/urdf2mujocoXml.py

build by script

sh build.sh adam_lite mujoco

Run simulation in Mujoco

  • Run the test
  • The robot operation process can be found in the robot operation instruction document.
  • Enter the build path then run :
cd build_adam_lite_mujoco
./pnd_adam_deploy_public

After entering the Mujoco simulation environment, press A first and then X on xbox. Then click Reset several times or press the Backspace key on the keyboard, then press Y on xbox to let adam walk.

mujocoSim

Build & Run Real Robot

sh build.sh adam_lite|adam_inspire|adam_standard real
# before v1.1.0 sh (build_adam_lite.sh | build_adam_standard.sh | build_adam_inspire.sh) # before v0.4.0 sh buildrobot.sh
sh run.sh

When the terminal displays 'FSM start!' After the information can start to control. Press button A to make all joint of the robot return to the semi-squat zero position; Press button X to make the robot run MLP or DEMO; Press button B to make all joints stop at the current position; Press LT and RT keys at the same time disables the joint and exits the control program; Press LB and RB at the sime time to power on and off the joint.

🗃️File Description

main.cpp

Program entry Main cycle

  // 2.5 ms timing loop
  while (true) {
    total_time = timer.currentTime() - start_time;  // total time
    start_time = timer.currentTime();

    framework.getState(time_fsm, robot_data);  // get state (position, velocity, current) from robot

    get_state_time = timer.currentTime() - start_time;  // get state execution time

    framework.runFSM();  // run fsm (The calculation time cannot exceed 1.5ms)

    fsm_time = timer.currentTime() - start_time - get_state_time;  // Finite state machine execution time

    framework.setCommand(robot_data);  // send commands to joints

    cmd_time = timer.currentTime() - start_time - fsm_time - get_state_time;  // send command execution time

    time_fsm += kDt;

#ifdef DATALOG
    // write data
    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

Robot actuator end absolute encoder (abs) Angle output file.

{
    "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
    }
}

example.pt

RL policy net

read_abs.py

Read abs, write to source/abs.json

$ python3 read_abs.py 
read abs complete!

check_abs.py

Check abs whether the data is read successfully.

$ python3 check_abs.py 
True

set_zero.py

Set robot zero position script.

transtojit.py

Convert rl pt files to c++ callable pt files.

$ 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

Joint pd and zero profile.

pnd_algorithm.h

Robot algorithm interface.

struct RobotData {
  // vector description:
  // [0]:floating base x
  // [1]:floating base y
  // [2]:floating base z
  // [3]:floating base roll
  // [4]:floating base pitch
  // [5]:floating base yaw
  // [6-28]:ref to RobotInterfaceImpl.cpp:line20-46

  // actual position
  Eigen::VectorXd q_a_ = Eigen::VectorXd::Zero(kRobotDataSize);
  // actual velocity
  Eigen::VectorXd q_dot_a_ = Eigen::VectorXd::Zero(kRobotDataSize);
  // actual torque
  Eigen::VectorXd tau_a_ = Eigen::VectorXd::Zero(kRobotDataSize);

  // desired position
  Eigen::VectorXd q_d_ = Eigen::VectorXd::Zero(kRobotDataSize);
  // desired velocity
  Eigen::VectorXd q_dot_d_ = Eigen::VectorXd::Zero(kRobotDataSize);
  // desired torque
  Eigen::VectorXd tau_d_ = Eigen::VectorXd::Zero(kRobotDataSize);

  // vector description: 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

pnd-cpp-sdk

vmIMU

imu code

basic_function.h

Base functions.

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,     // start point states
               Eigen::VectorXd p1, Eigen::VectorXd p1_dot, Eigen::VectorXd p1_dotdot,     // end point states
               double totalTime,       // total time
               double currentTime,     //current time,from 0 to total time
               Eigen::VectorXd &pd, Eigen::VectorXd &pd_dot, Eigen::VectorXd &pd_dotdot);// output command

fsm_state_impl.h

Each state machine is defined in this file.

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

Defined Joystick classes, this class realizes the acquisition of handle data and turns it into a switching state machine instruction.

pconfig.hpp

Defined Adam joint and joint absolute encoder configuration classes.

This structure cannot be customized, obtain it from PConfig.

struct PCfg_ {
  // joint config
  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;

  // absolute encoder config
  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);
// The configuration of the adam_lite version's 25 joints is defined above, matching the order in robot_data.

PConfig is a global singleton class that is used to get some configuration of the robot.

Do not modify this data

class PConfig {
 public:
  static PConfig& getInst() {
    static PConfig instance;
    return instance;
  }
  ······
};

pdata_handler.hpp

The DataHandler class provides the function of asynchronously writing data, which implements the recording of robot communication time, cycle time, joint data, etc. The file is written to the bin folder and named with a timestamp. The file name without a number indicates that the file was recently written.

class DataHandler {
 public:
  static DataHandler& getInstance() {
    static DataHandler instance;
    return instance;
  }

putil.h

Defines functions to get and set joint information, via joint 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

Initialize some information for the robot, and call the joint_interface class to communicate with the robot actuator.

robot_common.hpp

Some common base classes are defined. State machine base class, form which all defined substate machines must inherit.

class FSMState {
 public:
  explicit FSMState(RobotData *robot_data) { robot_data_ = robot_data; };
  virtual ~FSMState() = default;

  // Behavior to be carried out when entering a state
  virtual void onEnter() = 0;
  // Run the normal behavior for the state
  virtual void run() = 0;
  // Manages state specific transitions
  virtual FSMStateName checkTransition() = 0;
  // Behavior to be carried out when exiting a state
  virtual void onExit() = 0;

  FSMStateName current_state_name_;

 protected:
  const int freq_ = 4;
  double timer_ = 0.;

  RobotData *robot_data_;
};

robot_handler.h

Framework class, including the main process and state machine switching, the following is the state machine switching implementation details.

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 Structure

When the joystick is connected via Bluetooth, the macros and structures corresponding to the operation buttons are as follows; the robot control commands are encapsulated in joystick.cpp.

#define XBOX_TYPE_BUTTON 0x01
#define XBOX_TYPE_AXIS 0x02

// bluetooth map
#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 /* Left rocker button */
#define XBOX_BUTTON_RO 0x0a /* Right rocker button */

#define XBOX_BUTTON_ON 0x01
#define XBOX_BUTTON_OFF 0x00

#define XBOX_AXIS_LX 0x00 /* Left rocker X axis */
#define XBOX_AXIS_LY 0x01 /* Left rocker Y axis */
#define XBOX_AXIS_RX 0x02 /* Right rocker X axis */
#define XBOX_AXIS_RY 0x03 /* Right rocker Y axis */
#define XBOX_AXIS_LT 0x05
#define XBOX_AXIS_RT 0x04
#define XBOX_AXIS_XX 0x06 /* Arrow key X axis */
#define XBOX_AXIS_YY 0x07 /* Arrow key Y axis */

#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 Paramerter Explanation

In Isaac Gym

The PD controller:

\(\tau=K_p*e_p+K_d*e_v\) (1)

\(K_p\) : stiffness of PD controller

\(K_d\) : damping of PD controller

\(e_p\) : position error

\(e_v\) : velocity error

\(\tau\) : the torque worked on end

In Real Robot Actuator

The PID controller: \(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\) : proportional parameter of position control loop PID controller

\(P_v\) : proportional parameter of velocity control loop PID controller

\(e_p\) : position error

\(e_v\) : velocity error

\(g\) : the gear ratio between end and motor

\(K_t\) : torque constant,this can be get from experiment

\(I\) : the current output of position and velocity control loop PID controller

\(\tau'\) : the torque worked on end

For sim to real purpose, we need \(\tau=\tau'\), then we can get:

\(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)

Release Notes

v1.4.0

Fixed:

  • Support new abs.
  • Add ankle test demo.

v1.3.1

Fixed:

  • Fix angle torque for s2p.

v1.3.0

Feature:

  • Add ros2-humble support.

v1.2.2

Feature:

  • Add install.sh installation script.

v1.2.1

Fixed:

  • Fix ankle Series to parallel.

v1.2.0

Feature:

  • Added a runnable reinforcement learning model

v1.1.0

Feature:

  • Supports all adam types to use abs goto zero

v1.0.0

Feature:

  • Clip robot joints desired positions
  • Create FSM using factory pattern

v0.6.0

Feature:

  • Support Adam Inspire version

v0.5.0

Feature:

  • Support Adam Standard version

v0.4.0

  • base version

❓FAQ

FIND_PACKAGE Torch

See the installation steps in readme

error code

pnd-cpp-sdk error-code