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_state(self, state: AgentState, reset_sensors: bool = True):
r"""Sets the agents state
Args:
state (AgentState): The state to set the agent to
reset_sensors (bool): Whether or not to reset the sensors to their default intrinsic/extrinsic parameters
before setting their extrinsic state
"""
habitat_sim.errors.assert_obj_valid(self.body)
if isinstance(state.rotation, list):
state.rotation = utils.quat_from_coeffs(state.rotation)
self.body.reset_transformation()
self.body.translate(state.position)
self.body.set_rotation(utils.quat_to_coeffs(state.rotation))
if reset_sensors:
for _, v in self.sensors.items():
v.set_transformation_from_spec()
for k, v in state.sensor_states.items():
assert k in self.sensors
if isinstance(v.rotation, list):
v.rotation = utils.quat_from_coeffs(v.rotation)
s = self.sensors[k]
s.reset_transformation()
s.translate(
utils.quat_rotate_vector(state.rotation.inverse(),
v.position - state.position))
s.set_rotation(
utils.quat_to_coeffs(state.rotation.inverse() * v.rotation))
def get_state(self):
# Returns (x, y, yaw)
cur_state = self.get_full_state()
true_position = cur_state.position - self.init_state.position
true_position = habUtils.quat_rotate_vector(
self.init_state.rotation.inverse(), true_position)
true_position = habUtils.quat_rotate_vector(
self._fix_transform().inverse(), true_position)
true_rotation = self.init_state.rotation.inverse() * cur_state.rotation
quat_list = [
true_rotation.x, true_rotation.y, true_rotation.z, true_rotation.w
]
(r, yaw, p) = euler_from_quaternion(quat_list)
return (true_position[0], true_position[1], yaw)
def get_state(self, state_type="odom"):
# Returns (x, y, yaw)
assert state_type == "odom", "Error: Only Odom state is availalabe"
cur_state = self.get_full_state()
true_position = cur_state.position - self.init_state.position
true_position = habUtils.quat_rotate_vector(
self.init_state.rotation.inverse(), true_position)
true_position = habUtils.quat_rotate_vector(
self._fix_transform().inverse(), true_position)
true_rotation = self.init_state.rotation.inverse() * cur_state.rotation
quat_list = [
true_rotation.x, true_rotation.y, true_rotation.z, true_rotation.w
]
(r, yaw, p) = euler_from_quaternion(quat_list)
return (true_position[0], true_position[1], yaw)