def set_camera_pos_quat(self, pos, quat):
"""Set renderer position and quaternion.
Args:
pos ([list]): pos array of the camera.
quat ([list]): quat array of the camera.
"""
# to wxyz
mat = quat2rotmat([quat[-1], quat[0], quat[1], quat[2]])[:3, :3]
mat = mat.dot(np.array([0, 0, 1]))
self.renderer.set_camera(pos, [0, 0, 1], [0, 0, 1])
def _update_position(instance, env):
"""
Update position for an object or a robot in renderer.
:param instance: Instance in the renderer
"""
if isinstance(instance, Instance):
if instance.parent_body != 'worldbody':
pos_body_in_world = env.sim.data.get_body_xpos(
instance.parent_body)
rot_body_in_world = env.sim.data.get_body_xmat(
instance.parent_body).reshape((3, 3))
pose_body_in_world = T.make_pose(pos_body_in_world,
rot_body_in_world)
pose_geom_in_world = pose_body_in_world
pos, orn = T.mat2pose(pose_geom_in_world) #xyzw
else:
pos = [0, 0, 0]
orn = [0, 0, 0, 1] #xyzw
instance.set_position(pos)
instance.set_rotation(quat2rotmat(xyzw2wxyz(orn)))
def _get_camera_pose(self, camera_name):
"""
Get position and orientation of the camera given the name
Args:
camera_name (string): name of the camera
"""
for instance in self.renderer.instances:
if isinstance(instance, Robot):
for cam in instance.robot.cameras:
if cam.camera_name == camera_name:
camera_pose = cam.get_pose()
camera_pos = camera_pose[:3]
camera_ori = camera_pose[3:]
camera_ori_mat = quat2rotmat([
camera_ori[-1], camera_ori[0], camera_ori[1],
camera_ori[2]
])[:3, :3]
# Mujoco camera points in -z
camera_view_dir = camera_ori_mat.dot(
np.array([0, 0, -1]))
return camera_pos, camera_view_dir, cam.fov
raise Exception(f"Camera {self.env.render_camera} not present")