From 6b69f9a8b4ad48e08dcefd40544dcdbc67dfec03 Mon Sep 17 00:00:00 2001 From: mahdinobar Date: Fri, 5 Apr 2024 21:28:24 +0200 Subject: [PATCH] reset and step env corrected, #2 --- spinup/backup_gym/gym/envs/classic_control/tworr.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/spinup/backup_gym/gym/envs/classic_control/tworr.py b/spinup/backup_gym/gym/envs/classic_control/tworr.py index 63dfd7139..39deafa68 100644 --- a/spinup/backup_gym/gym/envs/classic_control/tworr.py +++ b/spinup/backup_gym/gym/envs/classic_control/tworr.py @@ -246,7 +246,9 @@ def reset(self): e0 = rd_t - r_hat_t self.e = e0.reshape(1, 2) dqc_t, e=self.q_command(r_ee=r_hat_t, v_ee=v_hat_t, Jpinv=Jpinv_t, rd=rd_t, vd=vd_t, e=self.e, dt=self.dt) - qc_t = dqc_t*self.dt+self.q[-1,:] #TODO check initialization of q and dq + # Attention: for simple observer ASSUME q(-1)=q(0) and dq(-1)=dq(0) + + qc_t = dqc_t*self.dt+self.q[-1,:] ddqc_t = (dqc_t-self.dq[-1,:])/self.dt self.qc = qc_t.reshape(1, 2) self.dqc = dqc_t.reshape(1, 2) @@ -264,10 +266,10 @@ def reset(self): self.state = [r_hat_t[0]-rd_t[0], r_hat_t[1]-rd_t[1], - q_t[0], - q_t[1], - dq_t[0], - dq_t[1], + q_FD[0], + q_FD[2], + q_FD[1], + q_FD[2], tau1_hat, tau2_hat, dqc_t[0],