diff --git a/src/unitree_rl/lib/model.cpp b/src/unitree_rl/lib/model.cpp index 1578f08..51aac75 100644 --- a/src/unitree_rl/lib/model.cpp +++ b/src/unitree_rl/lib/model.cpp @@ -13,8 +13,14 @@ torch::Tensor Model::quat_rotate_inverse(torch::Tensor q, torch::Tensor v) torch::Tensor Model::compute_torques(torch::Tensor actions) { - actions *= this->params.action_scale; - torch::Tensor torques = this->params.p_gains * (actions + this->params.default_dof_pos - this->obs.dof_pos) - this->params.d_gains * this->obs.dof_vel; + torch::Tensor actions_scaled = actions * this->params.action_scale; + int indices[] = {0, 3, 6, 9}; + for (int i : indices) + { + actions_scaled[0][i] *= this->params.hip_scale_reduction; + } + + torch::Tensor torques = this->params.p_gains * (actions_scaled + this->params.default_dof_pos - this->obs.dof_pos) - this->params.d_gains * this->obs.dof_vel; torch::Tensor clamped = torch::clamp(torques, -(this->params.torque_limits), this->params.torque_limits); return clamped; } diff --git a/src/unitree_rl/lib/model.hpp b/src/unitree_rl/lib/model.hpp index 63606f5..791e7c6 100644 --- a/src/unitree_rl/lib/model.hpp +++ b/src/unitree_rl/lib/model.hpp @@ -10,6 +10,7 @@ struct ModelParams { float damping; float stiffness; float action_scale; + float hip_scale_reduction; float num_of_dofs; float lin_vel_scale; float ang_vel_scale; diff --git a/src/unitree_rl/src/unitree_rl.cpp b/src/unitree_rl/src/unitree_rl.cpp index 6e67d09..b69b5af 100644 --- a/src/unitree_rl/src/unitree_rl.cpp +++ b/src/unitree_rl/src/unitree_rl.cpp @@ -43,6 +43,7 @@ Unitree_RL::Unitree_RL() 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; + this->params.hip_scale_reduction = 0.5; this->params.num_of_dofs = 12; this->params.lin_vel_scale = 2.0; this->params.ang_vel_scale = 0.25;