Reacher.calc_state() doesn't work as expected

[ven-kyoshiro]

State space is overlapping and not natural

bullet3/examples/pybullet/gym/pybullet_envs/robot_manipulators.py

Line 29 in c4300dc

theta, self.theta_dot = self.central_joint.current_relative_position()

I think this row should use

self.central_joint.current_position()

Because the domain of (sin,cos) is overlapped.
For example,

env = ReacherBulletEnv()
env.reset()

# for all a
a = 0.1
env.robot.central_joint.reset_current_position(a, 0)
s1 = env.robot.calc_state()

env.robot.central_joint.reset_current_position(a+np.pi, 0)
s2 = env.robot.calc_state()
s1[4:6] ==s2[4:6]
# array([ True,  True])

[manuelbre]

I came here to post about the same topic and found this open issue.

I agree with the statement that this can be solved by using self.central_joint.current_position() .
In addition, I believe that it would make sense to also use self.central_joint.current_position() for gamma although I believe that it is correct as is, but using the self.central_joint.current_position() makes the state interpretable without having to search for the upper and lower limit of the elbow joint.

I would propose to change

bullet3/examples/pybullet/gym/pybullet_envs/robot_manipulators.py

Lines 29 to 44 in c4300dc

	theta, self.theta_dot = self.central_joint.current_relative_position()
	self.gamma, self.gamma_dot = self.elbow_joint.current_relative_position()
	target_x, _ = self.jdict["target_x"].current_position()
	target_y, _ = self.jdict["target_y"].current_position()
	self.to_target_vec = np.array(self.fingertip.pose().xyz()) - np.array(self.target.pose().xyz())
	return np.array([
	target_x,
	target_y,
	self.to_target_vec[0],
	self.to_target_vec[1],
	np.cos(theta),
	np.sin(theta),
	self.theta_dot,
	self.gamma,
	self.gamma_dot,
	])

to:

...
theta, self.theta_dot = self.central_joint.current_position()
self.gamma, self.gamma_dot = self.elbow_joint.current_position()
...

Connected to this issue is that it would also make sense to use cos(gamma), sin(gamma) instead of simply using gamma as the state. Let me know if I should post a second issue about this.