From 740701ed7c4340797f26d09adef91993884648b3 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Fri, 19 Jul 2019 16:57:04 +0200 Subject: [PATCH] fix joint trigo state --- pyrobolearn/states/robot_states/joint_states.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/pyrobolearn/states/robot_states/joint_states.py b/pyrobolearn/states/robot_states/joint_states.py index 2cb0cbc..61893e9 100644 --- a/pyrobolearn/states/robot_states/joint_states.py +++ b/pyrobolearn/states/robot_states/joint_states.py @@ -163,7 +163,7 @@ class JointTrigonometricPositionState(JointState): super(JointTrigonometricPositionState, self).__init__(robot, joint_ids, window_size=window_size, axis=axis, ticks=ticks) - high = np.ones(len(self.joints)) + high = np.ones(2 * len(self.joints)) self._space = spaces.Box(low=-high, high=high, dtype=np.float32) def _read(self): @@ -205,6 +205,18 @@ class JointVelocityState(JointState): """Read the next joint velocity state.""" self.data = self.robot.get_joint_velocities(self.joints) + def _reset(self): + """Reset the state.""" + # reset counter + self.cnt = 0. + + # reset the robot joint position based on the data + if len(self.data) > 0: + self.robot.reset_joint_states(dq=self.data[0], joint_ids=self.joints) + + # read the next data + self._read() + class JointForceTorqueState(JointState): r"""Joint Force Torque State