From 5ec8fa456963e6693604715fcdb837e1c08b6885 Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Mon, 3 Mar 2025 21:54:54 +0800 Subject: [PATCH] scale --- src/rl_sar/library/core/rl_sdk/rl_sdk.cpp | 22 ++++++------- src/rl_sar/library/core/rl_sdk/rl_sdk.hpp | 5 +-- src/rl_sar/models/a1/legged_gym/config.yaml | 8 ++--- src/rl_sar/models/a1/robot_lab/config.yaml | 8 ++--- src/rl_sar/models/b2/robot_lab/config.yaml | 8 ++--- src/rl_sar/models/b2w/robot_lab/config.yaml | 9 +++--- src/rl_sar/models/go2/himloco/config.yaml | 8 ++--- src/rl_sar/models/go2/robot_lab/config.yaml | 8 ++--- src/rl_sar/models/go2w/robot_lab/config.yaml | 9 +++--- .../models/gr1t1/legged_gym/config.yaml | 6 ++-- .../models/gr1t2/legged_gym/config.yaml | 6 ++-- src/rl_sar/models/l4w4/legged_gym/config.yaml | 8 ++--- src/rl_sar/scripts/rl_sdk.py | 31 ++++++++++++++----- src/rl_sar/scripts/rl_sim.py | 16 +++------- src/rl_sar/src/rl_real_a1.cpp | 10 +----- src/rl_sar/src/rl_real_go2.cpp | 10 +----- src/rl_sar/src/rl_real_l4w4.cpp | 10 +----- src/rl_sar/src/rl_sim.cpp | 10 +----- 18 files changed, 80 insertions(+), 112 deletions(-) diff --git a/src/rl_sar/library/core/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/core/rl_sdk/rl_sdk.cpp index 4395cf8..0793677 100644 --- a/src/rl_sar/library/core/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/core/rl_sdk/rl_sdk.cpp @@ -102,17 +102,18 @@ void RL::InitControl() void RL::ComputeOutput(const torch::Tensor &actions, torch::Tensor &output_dof_pos, torch::Tensor &output_dof_vel, torch::Tensor &output_dof_tau) { - torch::Tensor joint_actions_scaled = actions * this->params.action_scale; - torch::Tensor wheel_actions_scaled = torch::zeros({1, this->params.num_of_dofs}); + torch::Tensor actions_scaled = actions * this->params.action_scale; + torch::Tensor pos_actions_scaled = actions_scaled; + torch::Tensor vel_actions_scaled = torch::zeros_like(actions); for (int i : this->params.wheel_indices) { - joint_actions_scaled[0][i] = 0.0; - wheel_actions_scaled[0][i] = actions[0][i] * this->params.action_scale_wheel; + pos_actions_scaled[0][i] = 0.0; + vel_actions_scaled[0][i] = actions[0][i]; } - torch::Tensor actions_scaled = joint_actions_scaled + wheel_actions_scaled; - output_dof_pos = joint_actions_scaled + this->params.default_dof_pos; - output_dof_vel = wheel_actions_scaled; - output_dof_tau = this->params.rl_kp * (actions_scaled + this->params.default_dof_pos - this->obs.dof_pos) - this->params.rl_kd * this->obs.dof_vel; + torch::Tensor all_actions_scaled = pos_actions_scaled + vel_actions_scaled; + output_dof_pos = pos_actions_scaled + this->params.default_dof_pos; + output_dof_vel = vel_actions_scaled; + output_dof_tau = this->params.rl_kp * (all_actions_scaled + this->params.default_dof_pos - this->obs.dof_pos) - this->params.rl_kd * this->obs.dof_vel; output_dof_tau = torch::clamp(output_dof_tau, -(this->params.torque_limits), this->params.torque_limits); } @@ -498,10 +499,7 @@ void RL::ReadYaml(std::string robot_path) this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml(config["clip_actions_upper"])).view({1, -1}); this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml(config["clip_actions_lower"])).view({1, -1}); } - this->params.action_scale = config["action_scale"].as(); - this->params.hip_scale_reduction = config["hip_scale_reduction"].as(); - this->params.hip_scale_reduction_indices = ReadVectorFromYaml(config["hip_scale_reduction_indices"]); - this->params.action_scale_wheel = config["action_scale_wheel"].as(); + this->params.action_scale = torch::tensor(ReadVectorFromYaml(config["action_scale"])).view({1, -1}); this->params.wheel_indices = ReadVectorFromYaml(config["wheel_indices"]); this->params.num_of_dofs = config["num_of_dofs"].as(); this->params.lin_vel_scale = config["lin_vel_scale"].as(); diff --git a/src/rl_sar/library/core/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/core/rl_sdk/rl_sdk.hpp index 5dabf1d..5153b43 100644 --- a/src/rl_sar/library/core/rl_sdk/rl_sdk.hpp +++ b/src/rl_sar/library/core/rl_sdk/rl_sdk.hpp @@ -87,10 +87,7 @@ struct ModelParams std::vector observations_history; double damping; double stiffness; - double action_scale; - double hip_scale_reduction; - std::vector hip_scale_reduction_indices; - double action_scale_wheel; + torch::Tensor action_scale; std::vector wheel_indices; int num_of_dofs; double lin_vel_scale; diff --git a/src/rl_sar/models/a1/legged_gym/config.yaml b/src/rl_sar/models/a1/legged_gym/config.yaml index 62fa06d..d26b700 100644 --- a/src/rl_sar/models/a1/legged_gym/config.yaml +++ b/src/rl_sar/models/a1/legged_gym/config.yaml @@ -34,11 +34,11 @@ a1/legged_gym: 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0] - hip_scale_reduction: 0.5 - hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 - action_scale: 0.25 - action_scale_wheel: 0.0 + action_scale: [0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25] wheel_indices: [] lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/models/a1/robot_lab/config.yaml b/src/rl_sar/models/a1/robot_lab/config.yaml index 67b406b..e705b00 100644 --- a/src/rl_sar/models/a1/robot_lab/config.yaml +++ b/src/rl_sar/models/a1/robot_lab/config.yaml @@ -34,11 +34,11 @@ a1/robot_lab: 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] num_of_dofs: 12 - action_scale: 0.25 - action_scale_wheel: 0.0 + action_scale: [0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25] wheel_indices: [] lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/models/b2/robot_lab/config.yaml b/src/rl_sar/models/b2/robot_lab/config.yaml index 37b8b39..221dcdc 100644 --- a/src/rl_sar/models/b2/robot_lab/config.yaml +++ b/src/rl_sar/models/b2/robot_lab/config.yaml @@ -34,11 +34,11 @@ b2/robot_lab: 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] num_of_dofs: 12 - action_scale: 0.25 - action_scale_wheel: 0.0 + action_scale: [0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25] wheel_indices: [] lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/models/b2w/robot_lab/config.yaml b/src/rl_sar/models/b2w/robot_lab/config.yaml index 318e5b5..8caaf7a 100644 --- a/src/rl_sar/models/b2w/robot_lab/config.yaml +++ b/src/rl_sar/models/b2w/robot_lab/config.yaml @@ -37,11 +37,12 @@ b2w/robot_lab: 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 1.0, 1.0, 1.0, 1.0] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] num_of_dofs: 16 - action_scale: 0.25 - action_scale_wheel: 5.0 + action_scale: [0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 5.0, 5.0, 5.0, 5.0] wheel_indices: [12, 13, 14, 15] lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/models/go2/himloco/config.yaml b/src/rl_sar/models/go2/himloco/config.yaml index 99e56d4..b10a0d5 100644 --- a/src/rl_sar/models/go2/himloco/config.yaml +++ b/src/rl_sar/models/go2/himloco/config.yaml @@ -34,11 +34,11 @@ go2/himloco: 5, 5, 5, 5, 5, 5, 5, 5, 5] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 - action_scale: 0.25 - action_scale_wheel: 0.0 + action_scale: [0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25] wheel_indices: [] lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/models/go2/robot_lab/config.yaml b/src/rl_sar/models/go2/robot_lab/config.yaml index a1c0dff..0d24dc8 100644 --- a/src/rl_sar/models/go2/robot_lab/config.yaml +++ b/src/rl_sar/models/go2/robot_lab/config.yaml @@ -34,11 +34,11 @@ go2/robot_lab: 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] num_of_dofs: 12 - action_scale: 0.25 - action_scale_wheel: 0.0 + action_scale: [0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25] wheel_indices: [] lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/models/go2w/robot_lab/config.yaml b/src/rl_sar/models/go2w/robot_lab/config.yaml index 379b71d..6cb1a64 100644 --- a/src/rl_sar/models/go2w/robot_lab/config.yaml +++ b/src/rl_sar/models/go2w/robot_lab/config.yaml @@ -37,11 +37,12 @@ go2w/robot_lab: 3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 0.5, 0.5, 0.5, 0.5] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] num_of_dofs: 16 - action_scale: 0.25 - action_scale_wheel: 5.0 + action_scale: [0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 5.0, 5.0, 5.0, 5.0] wheel_indices: [12, 13, 14, 15] lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/models/gr1t1/legged_gym/config.yaml b/src/rl_sar/models/gr1t1/legged_gym/config.yaml index 7ef7d6f..0df125f 100644 --- a/src/rl_sar/models/gr1t1/legged_gym/config.yaml +++ b/src/rl_sar/models/gr1t1/legged_gym/config.yaml @@ -22,11 +22,9 @@ gr1t1/legged_gym: 57.0, 43.0, 114.0, 114.0, 15.3] fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, 5.7, 4.3, 11.4, 11.4, 1.5] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] num_of_dofs: 10 - action_scale: 1.0 - action_scale_wheel: 0.0 + action_scale: [1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0] wheel_indices: [] lin_vel_scale: 1.0 ang_vel_scale: 1.0 diff --git a/src/rl_sar/models/gr1t2/legged_gym/config.yaml b/src/rl_sar/models/gr1t2/legged_gym/config.yaml index 3add096..635c2b3 100644 --- a/src/rl_sar/models/gr1t2/legged_gym/config.yaml +++ b/src/rl_sar/models/gr1t2/legged_gym/config.yaml @@ -22,11 +22,9 @@ gr1t2/legged_gym: 57.0, 43.0, 114.0, 114.0, 15.3] fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, 5.7, 4.3, 11.4, 11.4, 1.5] - hip_scale_reduction: 1.0 - hip_scale_reduction_indices: [] num_of_dofs: 10 - action_scale: 1.0 - action_scale_wheel: 0.0 + action_scale: [1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0] wheel_indices: [] lin_vel_scale: 1.0 ang_vel_scale: 1.0 diff --git a/src/rl_sar/models/l4w4/legged_gym/config.yaml b/src/rl_sar/models/l4w4/legged_gym/config.yaml index c7c65d9..40e7201 100644 --- a/src/rl_sar/models/l4w4/legged_gym/config.yaml +++ b/src/rl_sar/models/l4w4/legged_gym/config.yaml @@ -36,11 +36,11 @@ l4w4/legged_gym: 3.0, 3.0, 3.0, 0.5, 3.0, 3.0, 3.0, 0.5, 3.0, 3.0, 3.0, 0.5] - hip_scale_reduction: 0.5 - hip_scale_reduction_indices: [0, 4, 8, 12] num_of_dofs: 16 - action_scale: 0.5 - action_scale_wheel: 8.0 + action_scale: [0.25, 0.5, 0.5, 8.0, + 0.25, 0.5, 0.5, 8.0, + 0.25, 0.5, 0.5, 8.0, + 0.25, 0.5, 0.5, 8.0] wheel_indices: [3, 7, 11, 15] lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/scripts/rl_sdk.py b/src/rl_sar/scripts/rl_sdk.py index 2f8a542..00f05a5 100644 --- a/src/rl_sar/scripts/rl_sdk.py +++ b/src/rl_sar/scripts/rl_sdk.py @@ -75,16 +75,15 @@ class ModelParams: self.damping = None self.stiffness = None self.action_scale = None - self.hip_scale_reduction = None - self.hip_scale_reduction_indices = None - self.clip_actions_upper = None - self.clip_actions_lower = None + self.wheel_indices = None self.num_of_dofs = None self.lin_vel_scale = None self.ang_vel_scale = None self.dof_pos_scale = None self.dof_vel_scale = None self.clip_obs = None + self.clip_actions_upper = None + self.clip_actions_lower = None self.torque_limits = None self.rl_kd = None self.rl_kp = None @@ -161,7 +160,10 @@ class RL: elif observation == "commands": obs_list.append(self.obs.commands * self.params.commands_scale) elif observation == "dof_pos": - obs_list.append((self.obs.dof_pos - self.params.default_dof_pos) * self.params.dof_pos_scale) + dof_pos_rel = self.obs.dof_pos - self.params.default_dof_pos + for i in self.params.wheel_indices: + dof_pos_rel[0, i] = 0.0 + obs_list.append(dof_pos_rel * self.params.dof_pos_scale) elif observation == "dof_vel": obs_list.append(self.obs.dof_vel * self.params.dof_vel_scale) elif observation == "actions": @@ -199,6 +201,20 @@ class RL: actions_scaled = actions * self.params.action_scale return actions_scaled + self.params.default_dof_pos + def ComputeOutput(self, actions): + actions_scaled = actions * self.params.action_scale + pos_actions_scaled = actions_scaled.clone() + vel_actions_scaled = torch.zeros_like(actions) + for i in self.params.wheel_indices: + pos_actions_scaled[0][i] = 0.0 + vel_actions_scaled[0][i] = actions[0][i] + all_actions_scaled = pos_actions_scaled + vel_actions_scaled + output_dof_pos = pos_actions_scaled + self.params.default_dof_pos + output_dof_vel = vel_actions_scaled + output_dof_tau = self.params.rl_kp * (all_actions_scaled + self.params.default_dof_pos - self.obs.dof_pos) - self.params.rl_kd * self.obs.dof_vel + output_dof_tau = torch.clamp(output_dof_tau, -(self.params.torque_limits), self.params.torque_limits) + return output_dof_pos, output_dof_vel, output_dof_tau + def QuatRotateInverse(self, q, v, framework): if framework == "isaacsim": q_w = q[:, 0] @@ -384,15 +400,14 @@ class RL: self.params.observations = config["observations"] self.params.observations_history = config["observations_history"] self.params.clip_obs = config["clip_obs"] - self.params.action_scale = config["action_scale"] - self.params.hip_scale_reduction = config["hip_scale_reduction"] - self.params.hip_scale_reduction_indices = config["hip_scale_reduction_indices"] if config["clip_actions_lower"] is None and config["clip_actions_upper"] is None: self.params.clip_actions_upper = None self.params.clip_actions_lower = None else: self.params.clip_actions_upper = torch.tensor(config["clip_actions_upper"]).view(1, -1) self.params.clip_actions_lower = torch.tensor(config["clip_actions_lower"]).view(1, -1) + self.params.action_scale = torch.tensor(config["action_scale"]).view(1, -1) + self.params.wheel_indices = config["wheel_indices"] self.params.num_of_dofs = config["num_of_dofs"] self.params.lin_vel_scale = config["lin_vel_scale"] self.params.ang_vel_scale = config["ang_vel_scale"] diff --git a/src/rl_sar/scripts/rl_sim.py b/src/rl_sar/scripts/rl_sim.py index 6ad6cd1..0570cc4 100644 --- a/src/rl_sar/scripts/rl_sim.py +++ b/src/rl_sar/scripts/rl_sim.py @@ -47,6 +47,7 @@ class RL_Sim(RL): # init torch.set_grad_enabled(False) + torch.set_num_threads(4) self.joint_publishers_commands = [MotorCommand() for _ in range(self.params.num_of_dofs)] self.InitObservations() self.InitOutputs() @@ -189,19 +190,10 @@ class RL_Sim(RL): self.obs.dof_pos = torch.tensor(self.robot_state.motor_state.q).narrow(0, 0, self.params.num_of_dofs).unsqueeze(0) self.obs.dof_vel = torch.tensor(self.robot_state.motor_state.dq).narrow(0, 0, self.params.num_of_dofs).unsqueeze(0) - clamped_actions = self.Forward() + self.obs.actions = self.Forward() + self.output_dof_pos, self.output_dof_vel, self.output_dof_tau = self.ComputeOutput(self.obs.actions) - for i in self.params.hip_scale_reduction_indices: - clamped_actions[0][i] *= self.params.hip_scale_reduction - - self.obs.actions = clamped_actions - - origin_output_dof_tau = self.ComputeTorques(self.obs.actions) - - # self.TorqueProtect(origin_output_dof_tau) - - self.output_dof_tau = torch.clamp(origin_output_dof_tau, -(self.params.torque_limits), self.params.torque_limits) - self.output_dof_pos = self.ComputePosition(self.obs.actions) + # self.TorqueProtect(self.output_dof_pos) if CSV_LOGGER: tau_est = torch.zeros((1, self.params.num_of_dofs)) diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 67dfee7..db13f7d 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -164,15 +164,7 @@ void RL_Real::RunModel() 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); - torch::Tensor clamped_actions = this->Forward(); - - this->obs.actions = clamped_actions; - - for (int i : this->params.hip_scale_reduction_indices) - { - clamped_actions[0][i] *= this->params.hip_scale_reduction; - } - + this->obs.actions = this->Forward(); this->ComputeOutput(this->obs.actions, this->output_dof_pos, this->output_dof_vel, this->output_dof_tau); if (this->output_dof_pos.defined() && this->output_dof_pos.numel() > 0) diff --git a/src/rl_sar/src/rl_real_go2.cpp b/src/rl_sar/src/rl_real_go2.cpp index 34259e9..e04dd15 100644 --- a/src/rl_sar/src/rl_real_go2.cpp +++ b/src/rl_sar/src/rl_real_go2.cpp @@ -170,15 +170,7 @@ void RL_Real::RunModel() 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); - torch::Tensor clamped_actions = this->Forward(); - - this->obs.actions = clamped_actions; - - for (int i : this->params.hip_scale_reduction_indices) - { - clamped_actions[0][i] *= this->params.hip_scale_reduction; - } - + this->obs.actions = this->Forward(); this->ComputeOutput(this->obs.actions, this->output_dof_pos, this->output_dof_vel, this->output_dof_tau); if (this->output_dof_pos.defined() && this->output_dof_pos.numel() > 0) diff --git a/src/rl_sar/src/rl_real_l4w4.cpp b/src/rl_sar/src/rl_real_l4w4.cpp index 8f451b5..6563f1b 100644 --- a/src/rl_sar/src/rl_real_l4w4.cpp +++ b/src/rl_sar/src/rl_real_l4w4.cpp @@ -186,15 +186,7 @@ void RL_Real::RunModel() 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); - torch::Tensor clamped_actions = this->Forward(); - - this->obs.actions = clamped_actions; - - for (int i : this->params.hip_scale_reduction_indices) - { - clamped_actions[0][i] *= this->params.hip_scale_reduction; - } - + this->obs.actions = this->Forward(); this->ComputeOutput(this->obs.actions, this->output_dof_pos, this->output_dof_vel, this->output_dof_tau); if (this->output_dof_pos.defined() && this->output_dof_pos.numel() > 0) diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 19b377e..7e564a1 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -270,15 +270,7 @@ void RL_Sim::RunModel() 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); - torch::Tensor clamped_actions = this->Forward(); - - this->obs.actions = clamped_actions; - - for (int i : this->params.hip_scale_reduction_indices) - { - clamped_actions[0][i] *= this->params.hip_scale_reduction; - } - + this->obs.actions = this->Forward(); this->ComputeOutput(this->obs.actions, this->output_dof_pos, this->output_dof_vel, this->output_dof_tau); if (this->output_dof_pos.defined() && this->output_dof_pos.numel() > 0)