def __init__(self, configs, simulator):
self.sim = simulator.sim
self.configs = configs
self.agent = self.sim.get_agent(self.configs.COMMON.SIMULATOR.DEFAULT_AGENT_ID)
# Depth Image processor
self.depth_cam = DepthImgProcessor(
subsample_pixs=1,
depth_threshold=(0, 100),
cfg_filename="realsense_habitat.yaml"
)
# Pan and tilt related vairades.
self.pan = 0.0
self.tilt = 0.0
def restart_habitat(self):
if hasattr(self, "_robot"):
del self._robot
backend_config = self.backend_config
self._robot = Robot("habitat", common_config=backend_config)
from habitat_utils import reconfigure_scene
# adds objects to the scene, doing scene-specific configurations
reconfigure_scene(self, backend_config["scene_path"], self.add_humans)
from pyrobot.locobot.camera import DepthImgProcessor
if hasattr(self, "_dip"):
del self._dip
self._dip = DepthImgProcessor(cfg_filename="realsense_habitat.yaml")
def restart_habitat(self):
if hasattr(self, "_robot"):
del self._robot
backend_config = self.backend_config
self._robot = Robot("habitat", common_config=backend_config)
# todo: a bad package seems to override python logging after the line above is run.
# So, all `logging.warn` and `logging.info` calls are failing to route
# to STDOUT/STDERR after this.
from habitat_utils import reconfigure_scene
# adds objects to the scene, doing scene-specific configurations
reconfigure_scene(self, backend_config["scene_path"])
from pyrobot.locobot.camera import DepthImgProcessor
if hasattr(self, "_dip"):
del self._dip
self._dip = DepthImgProcessor(cfg_filename="realsense_habitat.yaml")
def __init__(self, backend="locobot", backend_config=None):
if backend == "locobot":
base_config_dict = {"base_controller": "proportional"}
arm_config_dict = dict(moveit_planner="ESTkConfigDefault")
self._robot = Robot(
backend,
use_base=True,
use_arm=True,
use_camera=True,
base_config=base_config_dict,
arm_config=arm_config_dict,
)
self._dip = DepthImgProcessor()
from grasp_samplers.grasp import Grasper
elif backend == "habitat":
if backend_config["physics_config"] == "DEFAULT":
assets_path = os.path.abspath(
os.path.join(os.path.dirname(__file__),
"../test/test_assets"))
backend_config["physics_config"] = os.path.join(
assets_path, "default.phys_scene_config.json")
print("backend_config", backend_config)
self.backend_config = backend_config
# we do it this way to have the ability to restart from the client at arbitrary times
self.restart_habitat()
else:
raise RuntimeError("Unknown backend", backend)
# check skfmm, skimage in installed, its necessary for slam
self._slam = Slam(self._robot, backend)
self._slam_step_size = 25 # step size in cm
self._done = True
self.backend = backend
class LoCoBotCamera(object):
"""docstring for SimpleCamera"""
def __init__(self, configs, simulator):
self.sim = simulator.sim
self.configs = configs
self.agent = self.sim.get_agent(self.configs.COMMON.SIMULATOR.DEFAULT_AGENT_ID)
# Depth Image processor
self.depth_cam = DepthImgProcessor(
subsample_pixs=1,
depth_threshold=(0, 100),
cfg_filename="realsense_habitat.yaml"
)
# Pan and tilt related vairades.
self.pan = 0.0
self.tilt = 0.0
def get_rgb(self):
observations = self.sim.get_sensor_observations()
return observations["rgb"][:, :, 0:3]
def get_depth(self):
observations = self.sim.get_sensor_observations()
return observations["depth"]
def get_rgb_depth(self):
observations = self.sim.get_sensor_observations()
return observations["rgb"][:, :, 0:3], observations["depth"]
def get_intrinsics(self):
"""
Returns the instrinsic matrix of the camera
:return: the intrinsic matrix (shape: :math:`[3, 3]`)
:rtype: np.ndarray
"""
fx = self.configs['Camera.fx']
fy = self.configs['Camera.fy']
cx = self.configs['Camera.cx']
cy = self.configs['Camera.cy']
Itc = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
return Itc
def pix_to_3dpt(self, rs, cs, in_cam=False, initial_rotation=None):
"""
Get the 3D points of the pixels in RGB images.
:param rs: rows of interest in the RGB image.
It can be a list or 1D numpy array
which contains the row indices.
The default value is None,
which means all rows.
:param cs: columns of interest in the RGB image.
It can be a list or 1D numpy array
which contains the column indices.
The default value is None,
which means all columns.
:param in_cam: return points in camera frame,
otherwise, return points in base frame
:param initial_rotation: a known initial rotation of the camera sensor
to calibrate habitat origin. The default value
is None which means it uses the Habitat-reported
value.
:type rs: list or np.ndarray
:type cs: list or np.ndarray
:type in_cam: bool
:returns: tuple (pts, colors)
pts: point coordinates in world frame
(shape: :math:`[N, 3]`)
colors: rgb values for pts_in_cam
(shape: :math:`[N, 3]`)
:rtype: tuple(np.ndarray, np.ndarray)
"""
rgb_im, depth_im = self.get_rgb_depth()
pts_in_cam = self.depth_cam.get_pix_3dpt(depth_im=depth_im, rs=rs, cs=cs)
pts = pts_in_cam[:3, :].T
colors = rgb_im[rs, cs].reshape(-1, 3)
if in_cam:
return pts, colors
# here, points are now given in camera frame
# the thing to do next is to transform the points from camera frame into the
# global frame of pyrobot environment
# This does not translate to what habitat thinks as origin,
# because pyrobot's habitat-reference origin is `self.agent.init_state`
# So, CAMERA frame -> HABITAT frame -> PYROBOT frame
init_state = self.agent.initial_state # habitat - x,y,z
cur_state = self.agent.get_state()
cur_sensor_state = cur_state.sensor_states['rgb']
if initial_rotation is None:
initial_rotation = init_state.sensor_states['rgb'].rotation
rot_init_rotation = self._rot_matrix(initial_rotation)
ros_to_habitat_frame = np.array([[ 0.0, -1.0, 0.0],
[ 0.0, 0.0, 1.0],
[-1.0, 0.0, 0.0]])
camera_to_image_frame = np.array([[1.0, 0.0, 0.0],
[0.0, -1.0, 0.0],
[0.0, 0.0, -1.0]])
relative_position = cur_sensor_state.position - init_state.position
relative_position = rot_init_rotation.T @ relative_position
cur_rotation = self._rot_matrix(cur_sensor_state.rotation)
cur_rotation = rot_init_rotation.T @ cur_rotation
rot_habitat_origin_to_image_frame = cur_rotation @ camera_to_image_frame
# now do the final transformation and return the points
pts = np.dot(pts, rot_habitat_origin_to_image_frame.T)
pts = pts + relative_position
pts = ros_to_habitat_frame.T @ pts.T
pts = pts.T
return pts, colors
def _rot_matrix(self, habitat_quat):
quat_list = [habitat_quat.x, habitat_quat.y, habitat_quat.z, habitat_quat.w]
return prutil.quat_to_rot_mat(quat_list)
def get_current_pcd(self, in_cam=True):
"""
Return the point cloud at current time step (one frame only)
:param in_cam: return points in camera frame,
otherwise, return points in base frame
:type in_cam: bool
:returns: tuple (pts, colors)
pts: point coordinates in world frame (shape: :math:`[N, 3]`)
colors: rgb values for pts_in_cam (shape: :math:`[N, 3]`)
:rtype: tuple(np.ndarray, np.ndarray)
"""
raise NotImplementedError
@property
def state(self):
"""
Return the current pan and tilt joint angles of the robot camera.
:return:
pan_tilt: A list the form [pan angle, tilt angle]
:rtype: list
"""
return self.get_state()
def get_state(self):
"""
Return the current pan and tilt joint angles of the robot camera.
:return:
pan_tilt: A list the form [pan angle, tilt angle]
:rtype: list
"""
return [self.pan, self.tilt]
def get_pan(self):
"""
Return the current pan joint angle of the robot camera.
:return:
pan: Pan joint angle
:rtype: float
"""
return self.pan
def get_tilt(self):
"""
Return the current tilt joint angle of the robot camera.
:return:
tilt: Tilt joint angle
:rtype: float
"""
return self.tilt
def set_pan(self, pan, wait=True):
"""
Sets the pan joint angle to the specified value.
:param pan: value to be set for pan joint
:param wait: wait until the pan angle is set to
the target angle.
:type pan: float
:type wait: bool
"""
self.set_pan_tilt(pan, self.tilt)
def set_tilt(self, tilt, wait=True):
"""
Sets the tilt joint angle to the specified value.
:param tilt: value to be set for the tilt joint
:param wait: wait until the tilt angle is set to
the target angle.
:type tilt: float
:type wait: bool
"""
self.set_pan_tilt(self.pan, tilt)
def _compute_relative_pose(self, pan, tilt):
pan_link = 0.1 # length of pan link
tilt_link = 0.1 # length of tilt link
sensor_offset_tilt = np.asarray([0.0, 0.0, -1 * tilt_link])
quat_cam_to_pan = habUtils.quat_from_angle_axis(
-1 * tilt, np.asarray([1.0, 0.0, 0.0])
)
sensor_offset_pan = habUtils.quat_rotate_vector(
quat_cam_to_pan, sensor_offset_tilt
)
sensor_offset_pan += np.asarray([0.0, pan_link, 0.0])
quat_pan_to_base = habUtils.quat_from_angle_axis(
-1 * pan, np.asarray([0.0, 1.0, 0.0])
)
sensor_offset_base = habUtils.quat_rotate_vector(
quat_pan_to_base, sensor_offset_pan
)
sensor_offset_base += np.asarray([0.0, 0.5, 0.1]) # offset w.r.t base
# translation
quat = quat_cam_to_pan * quat_pan_to_base
return sensor_offset_base, quat.inverse()
def set_pan_tilt(self, pan, tilt, wait=True):
"""
Sets both the pan and tilt joint angles to the specified values.
:param pan: value to be set for pan joint
:param tilt: value to be set for the tilt joint
:param wait: wait until the pan and tilt angles are set to
the target angles.
:type pan: float
:type tilt: float
:type wait: bool
"""
self.pan = pan
self.tilt = tilt
sensor_offset, quat_base_to_sensor = self._compute_relative_pose(pan, tilt)
cur_state = self.agent.get_state() # Habitat frame
sensor_position = cur_state.position + sensor_offset
sensor_quat = cur_state.rotation * quat_base_to_sensor
cur_state.sensor_states["rgb"].position = sensor_position
cur_state.sensor_states["rgb"].rotation = sensor_quat
self.agent.set_state(cur_state)
def reset(self):
"""
This function resets the pan and tilt joints by actuating
them to their home configuration.
"""
self.set_pan_tilt(self.configs.CAMERA.RESET_PAN, self.configs.CAMERA.RESET_TILT)