# # coding=utf-8
# # Copyright 2022 The Reach ML Authors.
# #
# # Licensed under the Apache License, Version 2.0 (the "License");
# # you may not use this file except in compliance with the License.
# # You may obtain a copy of the License at
# #
# #     http://www.apache.org/licenses/LICENSE-2.0
# #
# # Unless required by applicable law or agreed to in writing, software
# # distributed under the License is distributed on an "AS IS" BASIS,
# # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# # See the License for the specific language governing permissions and
# # limitations under the License.

# """Tests for envs.xarm_sim_robot."""
# import math

# from envs.utils import xarm_sim_robot
# from envs.utils.pose3d import Pose3d
# import numpy as np
# from scipy.spatial import transform
# import tensorflow.compat.v2 as tf
# import pybullet
# import pybullet_utils.bullet_client as bullet_client


# class XArmSimRobotTest(tf.test.TestCase):
#     def setUp(self):
#         super(XArmSimRobotTest, self).setUp()

#         # To debug we can use the SHARED_MEMORY connection.
#         # pybullet.connect(pybullet.SHARED_MEMORY)
#         connection_mode = pybullet.SHARED_MEMORY
#         connection_mode = pybullet.DIRECT
#         self._pybullet_client = bullet_client.BulletClient(connection_mode)
#         self._pybullet_client.resetSimulation()
#         self._pybullet_client.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
#         self._pybullet_client.setPhysicsEngineParameter(enableFileCaching=0)

#     def test_arm_loads(self):
#         xarm_sim_robot.XArmSimRobot(self._pybullet_client)

#     def test_arm_loads_suction(self):
#         xarm_sim_robot.XArmSimRobot(self._pybullet_client, end_effector="suction")

#     def test_forward_kinematics(self):
#         robot = xarm_sim_robot.XArmSimRobot(self._pybullet_client)

#         # Pointing down X Axis
#         robot.reset_joints([0, math.pi / 2, math.pi, 0, 0, 0])
#         x, y, _ = robot.forward_kinematics().translation

#         self.assertAlmostEqual(0.714479, x, places=3)
#         self.assertAlmostEqual(-0.0006, y, places=3)

#         # Pointing down Y Axis
#         robot.reset_joints([math.pi / 2, math.pi / 2, math.pi, 0, 0, 0])
#         x, y, _ = robot.forward_kinematics().translation

#         self.assertAlmostEqual(0.0006, x, places=3)
#         self.assertAlmostEqual(0.714479, y, places=3)

#     def test_inverse_kinematics(self):
#         robot = xarm_sim_robot.XArmSimRobot(self._pybullet_client)
#         initial_pose = robot.forward_kinematics()

#         rotation = transform.Rotation.from_rotvec([0, math.pi / 2, 0])
#         translation = np.array([0.5, 0.0, 0.10])
#         target_pose = Pose3d(rotation=rotation, translation=translation)

#         robot.reset_joints(robot.inverse_kinematics(target_pose))
#         pose = robot.forward_kinematics()

#         self.assertFalse(np.all(initial_pose.vec7 == pose.vec7))
#         np.testing.assert_almost_equal(pose.vec7, target_pose.vec7, decimal=2)


# if __name__ == "__main__":
#     tf.test.main()