fix bug

src/rl_sar/CMakeLists.txt

@@ -37,6 +37,9 @@ catkin_package(
   rospy
 )
 
+link_directories(/usr/local/lib)
+include_directories(${YAML_CPP_INCLUDE_DIR})
+
 # Unitree A1
 include_directories(library/unitree_legged_sdk_3.2/include)
 link_directories(library/unitree_legged_sdk_3.2/lib)

src/rl_sar/include/rl_real_go2.hpp

@@ -8,6 +8,8 @@
 #include <unitree/robot/channel/channel_subscriber.hpp>
 #include <unitree/idl/go2/LowState_.hpp>
 #include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/idl/go2/WirelessController_.hpp>
+#include <unitree/robot/client/client.hpp>
 #include <unitree/common/time/time_tool.hpp>
 #include <unitree/common/thread/thread.hpp>
 #include <unitree/robot/go2/robot_state/robot_state_client.hpp>
@@ -22,9 +24,37 @@ using namespace unitree::robot;
 using namespace unitree::robot::go2;
 #define TOPIC_LOWCMD "rt/lowcmd"
 #define TOPIC_LOWSTATE "rt/lowstate"
+#define TOPIC_JOYSTICK "rt/wirelesscontroller"
 constexpr double PosStopF = (2.146E+9f);
 constexpr double VelStopF = (16000.0f);
 
+// 遥控器键值联合体
+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;
+
+
+
 class RL_Real : public RL
 {
 public:
@@ -59,16 +89,18 @@ private:
     void InitRobotStateClient();
     int QueryServiceStatus(const std::string &serviceName);
     void ActivateService(const std::string &serviceName, int activate);
-    void LowCmdwriteHandler();
     uint32_t Crc32Core(uint32_t *ptr, uint32_t len);
     void InitLowCmd();
     void LowStateMessageHandler(const void *messages);
+    void JoystickHandler(const void *message);
     RobotStateClient rsc;
     unitree_go::msg::dds_::LowCmd_ unitree_low_command{}; // default init
     unitree_go::msg::dds_::LowState_ unitree_low_state{}; // default init
+    unitree_go::msg::dds_::WirelessController_ joystick{};
     ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher; // publisher
     ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; //subscriber
-    // xRockerBtnDataStruct unitree_joy; // TODO-devel-go2 go2键盘接口
+    ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber; 
+    xKeySwitchUnion unitree_joy; 
 
     // others
     int motiontime = 0;

src/rl_sar/models/go2_isaacgym/config.yaml

@@ -17,14 +17,14 @@ go2_isaacgym:
                        100, 100, 100,
                        100, 100, 100,
                        100, 100, 100]
-  rl_kp: [30, 30, 30,
+  rl_kp: [40, 40, 40,
-          30, 30, 30,
+          40, 40, 40,
-          30, 30, 30,
+          40, 40, 40,
-          30, 30, 30]
+          40, 40, 40]
-  rl_kd: [0.75, 0.75, 0.75,
+  rl_kd: [1, 1, 1,
-          0.75, 0.75, 0.75,
+          1, 1, 1,
-          0.75, 0.75, 0.75,
+          1, 1, 1,
-          0.75, 0.75, 0.75]
+          1, 1, 1]
   fixed_kp: [60, 60, 60,
              60, 60, 60,
              60, 60, 60,

src/rl_sar/src/rl_real_go2.cpp

@@ -5,6 +5,8 @@
 
 RL_Real::RL_Real(const std::string &network_interface)
 {
+    std::cout<<"000000"<<std::endl;
+    std::cout<<"000000"<<std::endl;
     // read params from yaml
     this->robot_name = "go2_isaacgym";
     this->ReadYaml(this->robot_name);
@@ -16,7 +18,7 @@ RL_Real::RL_Real(const std::string &network_interface)
             observation = "ang_vel_body";
         }
     }
-
+    std::cout<<"000000"<<std::endl;
     // init robot
     ChannelFactory::Instance()->Init(0, network_interface);
     this->InitRobotStateClient();
@@ -33,8 +35,10 @@ RL_Real::RL_Real(const std::string &network_interface)
     // create subscriber
     lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
     lowstate_subscriber->InitChannel(std::bind(&RL_Real::LowStateMessageHandler, this, std::placeholders::_1), 1);
-    // loop publishing thread TODO-devel-go2 why?
+
-    // lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &RL_Real::LowCmdwriteHandler, this);
+    joystick_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>(TOPIC_JOYSTICK));
+    joystick_subscriber->InitChannel(std::bind(&RL_Real::JoystickHandler, this, std::placeholders::_1), 1);
+
 
     // init rl
     torch::autograd::GradMode::set_enabled(false);
@@ -86,21 +90,18 @@ RL_Real::~RL_Real()
 
 void RL_Real::GetState(RobotState<double> *state)
 {
-    // TODO-devel-go2 go2键盘接口
+    if ((int)this->unitree_joy.components.R2 == 1)
-    // memcpy(&this->unitree_joy, &this->unitree_low_state.wireless_remote()[0], 40);
+    {
-
+        this->control.control_state = STATE_POS_GETUP;
-    // if ((int)this->unitree_joy.btn.components.R2 == 1)
+    }
-    // {
+    else if ((int)this->unitree_joy.components.R1 == 1)
-    //     this->control.control_state = STATE_POS_GETUP;
+    {
-    // }
+        this->control.control_state = STATE_RL_INIT;
-    // else if ((int)this->unitree_joy.btn.components.R1 == 1)
+    }
-    // {
+    else if ((int)this->unitree_joy.components.L2 == 1)
-    //     this->control.control_state = STATE_RL_INIT;
+    {
-    // }
+        this->control.control_state = STATE_POS_GETDOWN;
-    // else if ((int)this->unitree_joy.btn.components.L2 == 1)
+    }
-    // {
-    //     this->control.control_state = STATE_POS_GETDOWN;
-    // }
 
     if (this->params.framework == "isaacgym")
     {
@@ -141,7 +142,6 @@ void RL_Real::SetCommand(const RobotCommand<double> *command)
         this->unitree_low_command.motor_cmd()[i].tau() = command->motor_command.tau[command_mapping[i]];
     }
 
-    // 暂时不发 TODO-devel-go2 Why?
     this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
     lowcmd_publisher->Write(unitree_low_command);
 }
@@ -160,8 +160,8 @@ void RL_Real::RunModel()
     if (this->running_state == STATE_RL_RUNNING)
     {
         this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0);
-        // this->obs.commands = torch::tensor({{this->unitree_joy.ly, -this->unitree_joy.rx, -this->unitree_joy.lx}});
+        this->obs.commands = torch::tensor({{this->joystick.ly(), -this->joystick.rx(), -this->joystick.lx()}});
-        this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}});
+        //this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}});
         this->obs.base_quat = torch::tensor(this->robot_state.imu.quaternion).unsqueeze(0);
         this->obs.dof_pos = torch::tensor(this->robot_state.motor_state.q).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
         this->obs.dof_vel = torch::tensor(this->robot_state.motor_state.dq).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
@@ -330,12 +330,13 @@ void RL_Real::LowStateMessageHandler(const void *message)
     unitree_low_state = *(unitree_go::msg::dds_::LowState_ *)message;
 }
 
-void RL_Real::LowCmdwriteHandler()
+void RL_Real::JoystickHandler(const void *message)
 {
-    // this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
+    joystick = *(unitree_go::msg::dds_::WirelessController_ *)message;
-    // this->lowcmd_publisher->Write(unitree_low_command);
+    unitree_joy.value = joystick.keys();
 }
 
+
 void signalHandler(int signum)
 {
     exit(0);
@@ -343,6 +344,7 @@ void signalHandler(int signum)
 
 int main(int argc, char **argv)
 {
+    std::cout<<"000000"<<std::endl;
     signal(SIGINT, signalHandler);
 
     if (argc < 2)
@@ -350,8 +352,8 @@ int main(int argc, char **argv)
         std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
         exit(-1);
     }
-
+    std::cout<<argv[1]<<std::endl;
-    RL_Real rl_sar(argv[1]);
+    RL_Real rl_sar(argv[0]);
 
     while (1)
     {