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")