fix cyberdog model

src/rl_sar/include/rl_real_cyberdog.hpp

@@ -79,6 +79,14 @@ private:
 
     int dof_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
     int hip_scale_reduction_indices[4] = {0, 3, 6, 9};
+    float Kp[13] = {20, 40, 40, //FL
+                    20, 40, 40, //FR
+                    20, 40, 40, //RL
+                    20, 40, 40};//RR
+    float Kd[13] = {1.0, 1.5, 1.5, 
+                    1.0, 1.5, 1.5, 
+                    1.0, 1.5, 1.5, 
+                    1.0, 1.5, 1.5};
 
     std::chrono::high_resolution_clock::time_point start_time;
 

src/rl_sar/scripts/init_cyberdog.sh

@@ -1,12 +1,20 @@
 #!/bin/bash
-set -e
+set +e
 set -u
 
-# 获取IP地址对应的网络设备
+HOMENAME=$(basename $HOME)
+
 network_device=$(ifconfig | grep -B 1 192.168.55.100 | grep "flags" | cut -d ':' -f1)
-
-# 配置网络设备为多播
 sudo ifconfig $network_device multicast
-
-# 添加路由表
 sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev $network_device
+
+sudo sed -i 's/#\s*StrictHostKeyChecking/StrictHostKeyChecking/' /etc/ssh/ssh_config
+if grep -q "StrictHostKeyChecking" /etc/ssh/ssh_config; then
+    sudo sed -i 's/StrictHostKeyChecking.*/StrictHostKeyChecking no/' /etc/ssh/ssh_config
+else
+    echo "StrictHostKeyChecking no" | sudo tee -a /etc/ssh/ssh_config
+fi
+
+ssh-keygen -f "/home/${HOMENAME}/.ssh/known_hosts" -R "192.168.55.233"
+
+bash kill_cyberdog.sh

src/rl_sar/src/rl_real_cyberdog.cpp

@@ -20,8 +20,8 @@ RL_Real::RL_Real() : CustomInterface(500)
     this->params.num_observations = 45;
     this->params.clip_obs = 100.0;
     this->params.clip_actions = 100.0;
-    this->params.damping = 1.0;
+    this->params.damping = 0.5;
-    this->params.stiffness = 30;
+    this->params.stiffness = 20;
     this->params.d_gains = torch::ones(12) * this->params.damping;
     this->params.p_gains = torch::ones(12) * this->params.stiffness;
     this->params.action_scale = 0.25;
@@ -167,8 +167,10 @@ void RL_Real::RobotControl()
             // cyberdogCmd.q_des[i] = 0;
             cyberdogCmd.q_des[i] = output_dof_pos[0][dof_mapping[i]].item<double>();
             cyberdogCmd.qd_des[i] = 0;
-            cyberdogCmd.kp_des[i] = params.stiffness;
+            // cyberdogCmd.kp_des[i] = params.stiffness;
-            cyberdogCmd.kd_des[i] = params.damping;
+            // cyberdogCmd.kd_des[i] = params.damping;
+            cyberdogCmd.kp_des[i] = Kp[dof_mapping[i]];
+            cyberdogCmd.kd_des[i] = Kd[dof_mapping[i]];
             // cyberdogCmd.tau_des[i] = output_torques[0][dof_mapping[i]].item<double>();
             cyberdogCmd.tau_des[i] = 0;
         }
@@ -243,46 +245,20 @@ void RL_Real::run_keyboard()
             c = fgetc(stdin);
             switch(c)
             {
-                case '0':
+                case '0': keyboard.robot_state = STATE_POS_GETUP; break;
-                    keyboard.robot_state = STATE_POS_GETUP;
+                case 'p': keyboard.robot_state = STATE_RL_INIT; break;
-                    break;
+                case '1': keyboard.robot_state = STATE_POS_GETDOWN; break;
-                case 'p':
+                case 'q': break;
-                    keyboard.robot_state = STATE_RL_INIT;
+                case 'w': keyboard.x += 0.1; break;
-                    break;
+                case 's': keyboard.x -= 0.1; break;
-                case '1':
+                case 'a': keyboard.yaw += 0.1; break;
-                    keyboard.robot_state = STATE_POS_GETDOWN;
+                case 'd': keyboard.yaw -= 0.1; break;
-                    break;
+                case 'i': break;
-                case 'q':
+                case 'k': break;
-                    break;
+                case 'j': keyboard.y += 0.1; break;
-                case 'w':
+                case 'l': keyboard.y -= 0.1; break;
-                    keyboard.x += 0.1;
+                case ' ': keyboard.x = 0; keyboard.y = 0; keyboard.yaw = 0; break;
-                    break;
+                default: break;
-                case 's':
-                    keyboard.x -= 0.1;
-                    break;
-                case 'a':
-                    keyboard.yaw += 0.1;
-                    break;
-                case 'd':
-                    keyboard.yaw -= 0.1;
-                    break;
-                case 'i':
-                    break;
-                case 'k':
-                    break;
-                case 'j':
-                    keyboard.y += 0.1;
-                    break;
-                case 'l':
-                    keyboard.y -= 0.1;
-                    break;
-                case ' ':
-                    keyboard.x = 0;
-                    keyboard.y = 0;
-                    keyboard.yaw = 0;
-                    break;
-                default:
-                    break;
             }
         }
         usleep(10000);

src/rl_sar/src/rl_sim.cpp

@@ -42,10 +42,10 @@ RL_Sim::RL_Sim()
                                                  20.0, 55.0, 55.0}});
 
     //                                              hip,    thigh,   calf
-    this->params.default_dof_pos = torch::tensor({{0.1000, 0.8000, -1.5000,   // front left
+    this->params.default_dof_pos = torch::tensor({{ 0.1000, 0.8000, -1.5000,   // FL
-                                                  -0.1000, 0.8000, -1.5000,   // front right
+                                                   -0.1000, 0.8000, -1.5000,   // FR
-                                                   0.1000, 1.0000, -1.5000,   // rear  left
+                                                    0.1000, 1.0000, -1.5000,   // RR
-                                                  -0.1000, 1.0000, -1.5000}});// rear  right
+                                                   -0.1000, 1.0000, -1.5000}});// RL
 
     this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
 
@@ -156,7 +156,7 @@ void RL_Sim::RunModel()
     //     joint_velocities[7], joint_velocities[8], joint_velocities[6],
     //     joint_velocities[10], joint_velocities[11], joint_velocities[9]);
 
-    this->obs.lin_vel = torch::tensor({{vel.linear.x, vel.linear.y, vel.linear.z}});
+    // this->obs.lin_vel = torch::tensor({{vel.linear.x, vel.linear.y, vel.linear.z}});
     this->obs.ang_vel = torch::tensor({{vel.angular.x, vel.angular.y, vel.angular.z}});
     this->obs.commands = torch::tensor({{cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z}});
     this->obs.base_quat = torch::tensor({{pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w}});
@@ -182,8 +182,8 @@ void RL_Sim::RunModel()
 
 torch::Tensor RL_Sim::ComputeObservation()
 {
-    torch::Tensor obs = torch::cat({// (this->QuatRotateInverse(this->base_quat, this->lin_vel)) * this->params.lin_vel_scale,
+    torch::Tensor obs = torch::cat({// this->QuatRotateInverse(this->obs.base_quat, this->obs.lin_vel) * this->params.lin_vel_scale,
-                                    (this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel)) * this->params.ang_vel_scale,
+                                    this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel) * this->params.ang_vel_scale,
                                     this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec),
                                     this->obs.commands * this->params.commands_scale,
                                     (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale,

src/robots/cyberdog_description/config/joint_names_cyberdog_urdf_test.yaml

@@ -1 +0,0 @@
-controller_joint_names: ['joint_abad_bl', 'joint_abad_fr', 'joint_hip_fr', 'joint_knee_fr', 'joint_abad_fl', 'joint_hip_fl', 'joint_knee_fl', 'joint_abad_br', 'joint_hip_br', 'joint_knee_br', 'joint_abad_bl', 'joint_hip_bl', 'joint_knee_bl', ]

src/robots/cyberdog_description/urdf/cyberdog_description.csv

@@ -1,14 +0,0 @@
-Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
-body,0,0,0,0,0,0,7.18,0.032051,-0.00023217,0.002728,0.13707,5.6623E-05,0.14946,0,0,0,0,0,0,package://cyberdog_description/meshes/body.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/body.STL,,body^Cyberdog-1;cyber_dog_body-1,coor_body,axis_left_abad,joint_abad_bl,revolute,-0.23536,0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,,
-link_abad_fr,0,-0.053575,0,0,0,0,0.509,0.00038097,1.2523E-05,-1.1653E-05,0.00069383,5.071E-06,0.00047336,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_fr.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_fr.STL,,abad_link^Cyberdog-8,coor_leg0_abad,axis_right_abad,joint_abad_fr,revolute,0.23536,-0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,,
-link_hip_fr,0,0,-0.1,0,0,0,0.664,0.0033376,-7.1504E-07,-0.00019282,0.0026385,-9.3033E-06,0.0013093,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_fr.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_fr.STL,,hip_link^Cyberdog-6,coor_leg0_hip,axis_leg0_hip,joint_hip_fr,revolute,0,-0.10715,0,0,0,0,link_abad_fr,0,-1,0,24,12,4.363,-1.257,,,0.01,0.2,,,,
-link_knee_fr,0,0,-0.11439,0,0,0,0.114,0.0014553,-3.2376E-08,8.3885E-05,0.0021522,5.1259E-07,0.00070545,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_fr.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_fr.STL,,knee_link^Cyberdog-6,coor_leg0_knee,axis_leg0_knee,joint_knee_fr,revolute,0,0,-0.2,0,0,0,link_hip_fr,0,-1,0,24,12,-0.506,-2.478,,,0.01,0.2,,,,
-link_abad_fl,0,0.053575,0,0,0,0,0.509,0.00038097,1.2523E-05,-1.1653E-05,0.00069383,5.071E-06,0.00047336,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_fl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_fl.STL,,abad_link^Cyberdog-4,coor_leg1_abad,axis_left_abad,joint_abad_fl,revolute,0.23536,0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,,
-link_hip_fl,0,0,-0.1,0,0,0,0.664,0.0033376,-7.150375E-07,-0.00019282,0.0026385,-9.3033E-06,0.0013093,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_fl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_fl.STL,,hip_link^Cyberdog-2,coor_leg1_hip,axis_leg1_hip,joint_hip_fl,revolute,0,0.10715,0,0,0,0,link_abad_fl,0,-1,0,24,12,4.363,-1.257,,,0.01,0.2,,,,
-link_knee_fl,0,0,-0.11439,0,0,0,0.114,0.0014553,-3.2376E-08,8.3885E-05,0.0021522,5.1259E-07,0.00070545,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_fl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_fl.STL,,knee_link^Cyberdog-2,coor_leg1_knee,axis_leg1_knee,joint_knee_fl,revolute,0,0,-0.2,0,0,0,link_hip_fl,0,-1,0,24,12,-0.506,-2.478,,,0.01,0.2,,,,
-link_abad_br,0,-0.053575,0,0,0,0,0.509,0.00038097,1.2523E-05,-1.1653E-05,0.00069383,5.071E-06,0.00047336,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_br.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_br.STL,,abad_link^Cyberdog-9,coor_leg2_abad,axis_right_abad,joint_abad_br,revolute,-0.23536,-0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,,
-link_hip_br,0,0,-0.1,0,0,0,0.664,0.0033376,-7.150375E-07,-0.00019282,0.0026385,-9.3033E-06,0.0013093,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_br.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_br.STL,,hip_link^Cyberdog-7,coor_leg2_hip,axis_leg2_hip,joint_hip_br,revolute,0,-0.10715,0,0,0,0,link_abad_br,0,-1,0,24,12,3.49,-2.01,,,0.01,0.2,,,,
-link_knee_br,0,0,-0.11439,0,0,0,0.114,0.0014553,-3.2376E-08,8.3885E-05,0.0021522,5.1259E-07,0.00070545,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_br.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_br.STL,,knee_link^Cyberdog-7,coor_leg2_knee,axis_leg2_knee,joint_knee_br,revolute,0,0,-0.2,0,0,0,link_hip_br,0,-1,0,24,12,-0.506,-2.478,,,0.01,0.2,,,,
-link_abad_bl,0,0.053575,0,0,0,0,0.509,0.00038097,1.2523E-05,-1.1653E-05,0.00069383,5.071E-06,0.00047336,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_bl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_bl.STL,,abad_link^Cyberdog-5,coor_leg3_abad,axis_left_abad,joint_abad_bl,revolute,-0.23536,0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,,
-link_hip_bl,0,0,-0.1,0,0,0,0.664,0.0033376,-7.1504E-07,-0.00019282,0.0026385,-9.3033E-06,0.0013093,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_bl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_bl.STL,,hip_link^Cyberdog-3,coor_leg3_hip,axis_leg3_hip,joint_hip_bl,revolute,0,0.10715,0,0,0,0,link_abad_bl,0,-1,0,24,12,3.49,-2.01,,,0.01,0.2,,,,
-link_knee_bl,0,0,-0.11439,0,0,0,0.114,0.0014553,-3.2376E-08,8.3885E-05,0.0021522,5.1259E-07,0.00070545,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_bl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_bl.STL,,knee_link^Cyberdog-3,coor_leg3_knee,axis_leg3_knee,joint_knee_bl,revolute,0,0,-0.2,0,0,0,link_hip_bl,0,-1,0,24,12,-0.506,-2.478,,,0.01,0.2,,,,