mirror of https://github.com/fan-ziqi/rl_sar.git
fix cyberdog model
This commit is contained in:
parent
9f1e3f29ca
commit
067baf430b
|
@ -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;
|
||||
|
||||
|
|
|
@ -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
|
|
@ -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.stiffness = 30;
|
||||
this->params.damping = 0.5;
|
||||
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.kd_des[i] = params.damping;
|
||||
// cyberdogCmd.kp_des[i] = params.stiffness;
|
||||
// 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':
|
||||
keyboard.robot_state = STATE_POS_GETUP;
|
||||
break;
|
||||
case 'p':
|
||||
keyboard.robot_state = STATE_RL_INIT;
|
||||
break;
|
||||
case '1':
|
||||
keyboard.robot_state = STATE_POS_GETDOWN;
|
||||
break;
|
||||
case 'q':
|
||||
break;
|
||||
case 'w':
|
||||
keyboard.x += 0.1;
|
||||
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;
|
||||
case '0': keyboard.robot_state = STATE_POS_GETUP; break;
|
||||
case 'p': keyboard.robot_state = STATE_RL_INIT; break;
|
||||
case '1': keyboard.robot_state = STATE_POS_GETDOWN; break;
|
||||
case 'q': break;
|
||||
case 'w': keyboard.x += 0.1; 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);
|
||||
|
|
|
@ -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
|
||||
-0.1000, 0.8000, -1.5000, // front right
|
||||
0.1000, 1.0000, -1.5000, // rear left
|
||||
-0.1000, 1.0000, -1.5000}});// rear right
|
||||
this->params.default_dof_pos = torch::tensor({{ 0.1000, 0.8000, -1.5000, // FL
|
||||
-0.1000, 0.8000, -1.5000, // FR
|
||||
0.1000, 1.0000, -1.5000, // RR
|
||||
-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,
|
||||
(this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel)) * this->params.ang_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.gravity_vec),
|
||||
this->obs.commands * this->params.commands_scale,
|
||||
(this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale,
|
||||
|
|
|
@ -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', ]
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -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,,,,
|
|
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue