Created Go2 pybullet simulation env & Go2Interface

binding/pycontroller/CMakeLists.txt

@@ -1,3 +1,4 @@
 add_subdirectory(draco_controller)
+add_subdirectory(go2_controller)
 
 pybind11_add_module(interrupt_py interrupt_pybind.cpp)

binding/pycontroller/go2_controller/CMakeLists.txt

@@ -0,0 +1,2 @@
+pybind11_add_module(go2_interface_py go2_interface_pybind_pin.cpp)
+target_link_libraries(go2_interface_py PUBLIC rpc-go2-controller)

binding/pycontroller/go2_controller/go2_interface_pybind_pin.cpp

@@ -0,0 +1,57 @@
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+
+#include "controller/go2_controller/go2_interface.hpp"
+#include "controller/interrupt_handler.hpp"
+
+class PyInterface : public Interface {
+public:
+  using Interface::Interface;
+  void GetCommand(void *sensor_data, void *command_data) override {
+    PYBIND11_OVERRIDE_PURE(void, Interface, GetCommand, sensor_data,
+                           command_data);
+  }
+};
+
+namespace py = pybind11;
+
+PYBIND11_MODULE(go2_interface_py, m) {
+  // py::module::import("interrupt_py");
+
+  py::class_<Interface, PyInterface>(m, "Interface")
+      .def(py::init<>())
+      .def("GetCommand", &Interface::GetCommand);
+
+  py::class_<Go2Interface, Interface>(m, "Go2Interface").def(py::init<>());
+  //.def_readwrite("interrupt_", &Go2Interface::interrupt_handler_);
+
+  py::class_<Go2SensorData>(m, "Go2SensorData")
+      .def(py::init<>())
+      .def_readwrite("imu_frame_quat_", &Go2SensorData::imu_frame_quat_)
+      .def_readwrite("imu_ang_vel_", &Go2SensorData::imu_ang_vel_)
+      .def_readwrite("joint_pos_", &Go2SensorData::joint_pos_)
+      .def_readwrite("joint_vel_", &Go2SensorData::joint_vel_)
+      .def_readwrite("b_FL_foot_contact_", &Go2SensorData::b_FL_foot_contact_)
+      .def_readwrite("b_FR_foot_contact_", &Go2SensorData::b_FR_foot_contact_)
+      .def_readwrite("b_RL_foot_contact_", &Go2SensorData::b_RL_foot_contact_)
+      .def_readwrite("b_RR_foot_contact_", &Go2SensorData::b_RR_foot_contact_)
+      .def_readwrite("FL_normal_force_", &Go2SensorData::FL_normal_force_)
+      .def_readwrite("FR_normal_force_", &Go2SensorData::FR_normal_force_)
+      .def_readwrite("RL_normal_force_", &Go2SensorData::RL_normal_force_)
+      .def_readwrite("RR_normal_force_", &Go2SensorData::RR_normal_force_)
+      .def_readwrite("imu_dvel_", &Go2SensorData::imu_dvel_)
+      .def_readwrite("imu_lin_acc_", &Go2SensorData::imu_lin_acc_)
+
+      // Debug
+      .def_readwrite("base_joint_pos_", &Go2SensorData::base_joint_pos_)
+      .def_readwrite("base_joint_quat_", &Go2SensorData::base_joint_quat_)
+      .def_readwrite("base_joint_lin_vel_", &Go2SensorData::base_joint_lin_vel_)
+      .def_readwrite("base_joint_ang_vel_",
+                     &Go2SensorData::base_joint_ang_vel_);
+
+  py::class_<Go2Command>(m, "Go2Command")
+      .def(py::init<>())
+      .def_readwrite("joint_pos_cmd_", &Go2Command::joint_pos_cmd_)
+      .def_readwrite("joint_vel_cmd_", &Go2Command::joint_vel_cmd_)
+      .def_readwrite("joint_trq_cmd_", &Go2Command::joint_trq_cmd_);
+}

config/go2/sim/pybullet/wbic/pnc.yaml

@@ -0,0 +1,149 @@
+env: pybullet
+wbc_type: wbic
+servo_dt: 0.00125
+ip_address: "tcp://127.0.0.1:5557"
+#data_save_freq: 1
+data_save_freq: 50
+stance_foot: 1 # 0: left_foot 1: right_foot
+
+state_estimator:
+  b_cheater_mode: true ## true: use simulation groundtruth states
+  state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf 
+  b_use_marg_filter: false   # only used when kf is set to true
+
+  foot_reference_frame: 1 #0: left foot, 1: right foot
+
+  sigma_base_vel: [0.0, 0.0, 0.0]
+  sigma_base_acc: [0.5, 0.5, 0.5]
+  sigma_pos_lfoot: [0.001, 0.001, 0.001]
+  sigma_pos_rfoot: [0.001, 0.001, 0.001]
+  sigma_vel_lfoot: [0.005, 0.005, 0.002]
+  sigma_vel_rfoot: [0.005, 0.005, 0.002]
+  imu_accel_bias: [0.0, 0.0, 0.0]
+  base_accel_time_constant: 0.02
+  base_accel_limits: [1., 1., 1.]
+  base_com_vel_time_constant: 1.0
+  num_data_base_accel: [10, 10, 10]
+  num_data_ang_vel: [5, 5, 5]
+  ang_vel_time_constant: 0.01
+
+  ## for simulation please use 0
+  com_vel_filter_type: 0 ## 0: moving average // 1: exponential smoother // 2: low pass filter
+
+  ## IF 0: simple moving average filter parameter
+  num_data_com_vel: [10, 10, 10]
+
+  ## IF 1: exponential smoother filter parameter
+  #com_vel_time_constant: 0.1 
+  com_vel_time_constant: 0.01
+  com_vel_err_limit: [1., 1., 1.]
+
+  ## IF 2: low-pass filter parameter
+  cut_off_period: 0.01
+
+
+controller:
+  b_smoothing_command: true 
+  smoothing_command_duration: 0.01 ## this will filter the control command at the beginning
+  b_use_modified_swing_foot_jac: true
+  b_use_filtered_torque: false
+  alpha_cmd: 0.5
+
+wbc:
+  task:
+    com_xy_task:
+      com_feedback_source: 0 # 0: com feedback / 1: icp feedback
+      icp_integrator_type: 0 # 0: exponential smoother / 1: leaky integrator
+
+      kp : [500., 500.]
+      kd : [60, 60]
+
+      kp_ik : [1., 1.]
+
+    com_z_task:
+      com_height_target_source: 0 # 0: com height / 1: base height
+      com_kp : [1000.]
+      com_kd : [100.]
+
+      base_kp : [1000.]
+      base_kd : [85.]
+
+      kp_ik : [1.0]
+
+    cam_task:
+      kd : [0., 0., 0.]
+
+    wbo_task:
+      kp : [0.0, 0.0, 1.0]
+      kd : [0.0, 0.0, 2.0]
+      kp_ik : [0., 0., 1]
+
+    torso_ori_task:
+      kp : [500., 500., 300.] ## issues when yaw ~ 90 degree
+      kd : [50., 50., 30.]
+      kp_ik : [1.0, 1.0, 1.0]
+
+    foot_pos_task:
+      kp : [1000., 1500., 2500.]
+      kd : [50, 70, 100]
+      kp_ik : [1., 1., 1.]
+
+    foot_ori_task:
+      kp : [1000., 1000., 2500.]
+      kd : [100, 100, 250]
+      kp_ik : [1., 1., 1.]
+
+    foot_rf_task:
+      weight : [0., 0., 0., 0., 0., 0.] ## trq, force order
+
+  contact:
+    max_rf_z: 600.
+
+    mu: 0.5
+    foot_half_length: 0.08
+    foot_half_width: 0.04 ## 0.04
+    kp: [0, 0, 0, 0, 0, 0] ## ori, pos order 
+    kd: [0, 0, 0, 0, 0, 0]
+
+  qp:
+    W_delta_qddot: 1e5 #1e5
+
+    ## for DCM Walking
+    ########################################################################
+    W_delta_rf_left_foot_in_contact: [1, 1, 1e-4, 1e-3, 1e-3, 1e-4]
+    W_delta_rf_right_foot_in_contact: [1, 1, 1e-4, 1e-3, 1e-3, 1e-4]
+
+    W_xc_ddot_in_contact: [0, 0, 0, 0, 0, 0]
+    W_xc_ddot_in_swing: 0
+
+    ## for MPC walking
+    ########################################################################
+    W_delta_rf_left_foot_in_contact_mpc: [1e5, 1e5, 1e5, 1e5, 1e5, 1e5]
+    W_delta_rf_right_foot_in_contact_mpc: [1e5, 1e5, 1e5, 1e5, 1e5, 1e5]
+
+    W_delta_rf_left_foot_in_swing: [5, 5, 5, 5e-3, 5e-3, 5e-4]
+    W_delta_rf_right_foot_in_swing: [5, 5, 5, 5e-3, 5e-3, 5e-4]
+
+    W_force_rate_of_change_left_foot: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    W_force_rate_of_change_right_foot: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
+state_machine:
+    initialize:
+      init_duration: 5.
+      target_joint_pos: [-1.434e-4, -2.827e-3, -5.650e-1, 5.650e-1, 5.650e-1, -5.650e-1, 2.827e-3, 5.574e-8, 0.523, 3.1517e-8, -1.57, 1.555e-7, -4.075e-8, -5.106e-6, 3.007e-6, -2.977e-3, -5.650e-1, 5.650e-1, 5.650e-1, -5.650e-1, -2.9767e-3, -2.347e-8, -0.523, 4.1193e-8, -1.57, -6.215e-7, -3.4377e-7] #pinocchio robotsystem joint order
+      b_only_joint_pos_control: false
+      wait_time: 2.
+
+    stand_up:
+      standup_duration: 1.
+      target_com_height: 0.7
+      target_base_height: 0.93
+
+      rf_z_max_interp_duration: 0.1
+
+    single_support_swing:
+      swing_height: 0.10
+
+    com_swaying:
+      amplitude: [0., 0.05, 0.] # [m]
+      frequency: [0., 0.50, 0.] # [hz]

config/go2/sim/pybullet/wbic/pybullet_params.py

@@ -0,0 +1,82 @@
+import numpy as np
+
+
+class Config(object):
+    CONTROLLER_DT = 0.001
+    N_SUBSTEP = 1
+
+    INITIAL_BASE_JOINT_POS = [0, 0, 0.35]
+    INITIAL_BASE_JOINT_QUAT = [0, 0, 0, 1]
+
+    PRINT_ROBOT_INFO = True
+
+    MEASURE_COMPUTATION_TIME = False
+
+    VIDEO_RECORD = False
+    RECORD_FREQ = 50
+    RENDER_WIDTH = 1920
+    RENDER_HEIGHT = 1080
+
+    ##TODO:
+    USE_MESHCAT = False
+
+
+class Go2LinkIdx(object):
+    base = -1
+    Head_upper = 0
+    Head_lower = 1
+    FL_hip = 2
+    FL_thigh = 3
+    FL_calf = 4
+    FL_calflower = 5
+    FL_calflower1 = 6
+    FL_foot = 7
+    FR_hip = 8
+    FR_thigh = 9
+    FR_calf = 10
+    FR_calflower = 11
+    FR_calflower1 = 12
+    FR_foot = 13
+    RL_hip = 14
+    RL_thigh = 15
+    RL_calf = 16
+    RL_calflower = 17
+    RL_calflower1 = 18
+    RL_foot = 19
+    RR_hip = 20
+    RR_thigh = 21
+    RR_calf = 22
+    RR_calflower = 23
+    RR_calflower1 = 24
+    RR_foot = 25
+    imu = 26
+    radar = 27
+
+
+class Go2JointIdx(object):
+    FL_hip_joint = 2
+    FL_thigh_joint = 3
+    FL_calf_joint = 4
+    FR_hip_joint = 8
+    FR_thigh_joint = 9
+    FR_calf_joint = 10
+    RL_hip_joint = 14
+    RL_thigh_joint = 15
+    RL_calf_joint = 16
+    RR_hip_joint = 20
+    RR_thigh_joint = 21
+    RR_calf_joint = 22
+
+
+class JointGains(object):
+    # kp = 5. * np.ones(27)
+    # kd = 0. * np.ones(27)
+    kp = np.array([
+        10, 10, 10, 10, 10, 10, 10, 5, 5, 5, 5, 5, 5, 5, 10, 10, 10, 10, 10,
+        10, 10, 5, 5, 5, 5, 5, 5
+    ])
+    kd = np.array([
+        0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.001, 0.001, 0.001, 0.001,
+        0.001, 0.001, 0.01, 0.01, 0.001, 0.01, 0.01, 0.01, 0.01, 0.01, 0.001,
+        0.001, 0.001, 0.001, 0.001, 0.001
+    ])

controller/CMakeLists.txt

@@ -1,4 +1,5 @@
 add_subdirectory(draco_controller)
+add_subdirectory(go2_controller)
 add_subdirectory(robot_system)
 add_subdirectory(whole_body_controller)
 add_subdirectory(filter)

controller/go2_controller/CMakeLists.txt

@@ -0,0 +1,13 @@
+file(GLOB_RECURSE sources "*.cpp" "*.c")
+file(GLOB controller_headers "*.hpp")
+
+add_library(rpc-go2-controller SHARED ${sources} ${controller_headers})
+target_link_libraries(rpc-go2-controller PUBLIC rpc-pin-robot-system
+                                           PUBLIC rpc-wbc
+                                           PUBLIC rpc-util
+                                           )
+
+
+install(TARGETS rpc-go2-controller DESTINATION "${INSTALL_LIB_DIR}")
+install(FILES ${controller_headers} DESTINATION
+    "${INSTALL_INCLUDE_DIR}/controller/go2_controller")

controller/go2_controller/go2_definition.hpp

@@ -0,0 +1,51 @@
+namespace go2_link {
+constexpr int base = 2;
+constexpr int FL_hip = 4;
+constexpr int FL_thigh = 6;
+constexpr int FL_calf = 8;
+constexpr int FL_calflower = 10;
+constexpr int FL_calflower1 = 12;
+constexpr int FL_foot = 14;
+constexpr int FR_hip = 16;
+constexpr int FR_thigh = 18;
+constexpr int FR_calf = 20;
+constexpr int FR_calflower = 22;
+constexpr int FR_calflower1 = 24;
+constexpr int FR_foot = 26;
+constexpr int Head_upper = 28;
+constexpr int Head_lower = 30;
+constexpr int RL_hip = 32;
+constexpr int RL_thigh = 34;
+constexpr int RL_calf = 36;
+constexpr int RL_calflower = 38;
+constexpr int RL_calflower1 = 40;
+constexpr int RL_foot = 42;
+constexpr int RR_hip = 44;
+constexpr int RR_thigh = 46;
+constexpr int RR_calf = 48;
+constexpr int RR_calflower = 50;
+constexpr int RR_calflower1 = 52;
+constexpr int RR_foot = 54;
+constexpr int imu = 56;
+constexpr int radar = 58;
+} // namespace go2_link
+
+namespace go2_joint {
+constexpr int FL_hip_joint = 0;
+constexpr int FL_thigh_joint = 1;
+constexpr int FL_calf_joint = 2;
+constexpr int FR_hip_joint = 3;
+constexpr int FR_thigh_joint = 4;
+constexpr int FR_calf_joint = 5;
+constexpr int RL_hip_joint = 6;
+constexpr int RL_thigh_joint = 7;
+constexpr int RL_calf_joint = 8;
+constexpr int RR_hip_joint = 9;
+constexpr int RR_thigh_joint = 10;
+constexpr int RR_calf_joint = 11;
+} // namespace go2_joint
+
+namespace go2 {
+constexpr int n_qdot = 18;
+constexpr int n_adof = 12;
+} // namespace go2