File size: 3,107 Bytes
393d3de |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 |
# # 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()
|