def __enter__(self):
from pybullet_planning.interfaces.robots.joint import get_joint_velocities, get_joint_positions
org_obj_pos, org_obj_ori, org_obj_vel = get_body_state(self.cube_id)
self.org_obj_pos = org_obj_pos
self.org_obj_ori = org_obj_ori
self.org_obj_vel = org_obj_vel
self.org_joint_pos = get_joint_positions(self.finger_id,
self.finger_links)
self.org_joint_vel = get_joint_velocities(self.finger_id,
self.finger_links)
def __exit__(self, type, value, traceback):
from pybullet_planning.interfaces.robots.joint import get_joint_velocities, get_joint_positions
obj_pos, obj_ori, obj_vel = get_body_state(self.cube_id)
np.testing.assert_array_almost_equal(self.org_obj_pos, obj_pos)
np.testing.assert_array_almost_equal(self.org_obj_ori, obj_ori)
np.testing.assert_array_almost_equal(self.org_obj_vel[0], obj_vel[0])
np.testing.assert_array_almost_equal(self.org_obj_vel[1], obj_vel[1])
joint_pos = get_joint_positions(self.finger_id, self.finger_links)
joint_vel = get_joint_velocities(self.finger_id, self.finger_links)
np.testing.assert_array_almost_equal(self.org_joint_pos, joint_pos)
np.testing.assert_array_almost_equal(self.org_joint_vel, joint_vel)