def _rotate_local(
scene_node: SceneNode, theta: float, axis: int, constraint: Optional[float] = None
):
if constraint is not None:
rotation = scene_node.rotation
if abs(float(rotation.angle())) > 0:
ref_vector = mn.Vector3()
ref_vector[axis] = 1
if mn.math.angle(ref_vector, rotation.axis().normalized()) > mn.Rad(1e-3):
raise RuntimeError(
"Constrained look only works for a singular look action type"
)
look_vector = rotation.transform_vector(FRONT)
if axis == 0:
look_angle = mn.Rad(np.arctan2(look_vector[1], -look_vector[2]))
elif axis == 1:
look_angle = -mn.Rad(np.arctan2(look_vector[0], -look_vector[2]))
new_angle = look_angle + mn.Deg(theta)
constraint = mn.Deg(constraint)
if new_angle > constraint:
theta = constraint - look_angle
elif new_angle < -constraint:
theta = -constraint - look_angle
_rotate_local_fns[axis](scene_node, mn.Deg(theta))
scene_node.rotation = scene_node.rotation.normalized()
def _noisy_action_impl(
scene_node: hsim.SceneNode,
translate_amount: float,
rotate_amount: float,
multiplier: float,
model: MotionNoiseModel,
):
# Perform the action in the coordinate system of the node
transform = scene_node.transformation
move_ax = -transform[_Z_AXIS].xyz
perp_ax = transform[_X_AXIS].xyz
# + EPS to make sure 0 is positive. We multiply the mean by the sign of the translation
# as otherwise forward would overshoot on average and backward would undershoot, while
# both should overshoot
translation_noise = multiplier * np.random.multivariate_normal(
np.sign(translate_amount + 1e-8) * model.linear.mean, model.linear.cov)
scene_node.translate_local(move_ax *
(translate_amount + translation_noise[0]) +
perp_ax * translation_noise[1])
# Same deal with rotation about + EPS and why we multiply by the sign
rot_noise = multiplier * np.random.multivariate_normal(
np.sign(rotate_amount + 1e-8) * model.rotation.mean,
model.rotation.cov)
scene_node.rotate_y_local(mn.Deg(rotate_amount) + mn.Rad(rot_noise[0]))
scene_node.rotation = scene_node.rotation.normalized()
def test_constrainted(
scene_graph, control_name, control_axis, actuation_amount, actuation_constraint
):
initial_look_angle = mn.Deg(
np.random.uniform(-actuation_constraint, actuation_constraint)
)
rotation_vector = mn.Vector3()
rotation_vector[control_axis] = 1
initial_rotation = mn.Quaternion.rotation(
mn.Rad(initial_look_angle), rotation_vector
)
node = scene_graph.get_root_node().create_child()
node.rotation = initial_rotation
spec = habitat_sim.agent.controls.ActuationSpec(
actuation_amount, actuation_constraint
)
habitat_sim.registry.get_move_fn(control_name)(node, spec)
expected_angle = initial_look_angle + mn.Deg(
-actuation_amount
if control_name in {"look_down", "look_right"}
else actuation_amount
)
if expected_angle > mn.Deg(actuation_constraint):
expected_angle = mn.Deg(actuation_constraint)
elif expected_angle < mn.Deg(-actuation_constraint):
expected_angle = mn.Deg(-actuation_constraint)
final_rotation = node.rotation
look_vector = final_rotation.transform_vector(habitat_sim.geo.FRONT)
if control_axis == 0:
look_angle = mn.Deg(mn.Rad(np.arctan2(look_vector[1], -look_vector[2])))
elif control_axis == 1:
look_angle = -mn.Deg(mn.Rad(np.arctan2(look_vector[0], -look_vector[2])))
assert np.abs(float(expected_angle - look_angle)) < 1e-1
def _noisy_action_impl(
scene_node: hsim.SceneNode,
translate_amount: float,
rotate_amount: float,
multiplier: float,
model: MotionNoiseModel,
motion_type: str,
):
# Perform the action in the coordinate system of the node
transform = scene_node.transformation
move_ax = -transform[_Z_AXIS].xyz
perp_ax = transform[_X_AXIS].xyz
if motion_type == "rotational":
translation_noise = multiplier * model.linear.sample()
else:
# The robot will always move a little bit. This has to be defined based on the intended actuation
# as otherwise small rotation amounts would be invalid. However, pretty quickly, we'll
# get to the truncation of 3 sigma
trunc = [(-0.95 * np.abs(translate_amount), None), None]
translation_noise = multiplier * model.linear.sample(trunc)
# + EPS to make sure 0 is positive. We multiply by the sign of the translation
# as otherwise forward would overshoot on average and backward would undershoot, while
# both should overshoot
translation_noise *= np.sign(translate_amount + 1e-8)
scene_node.translate_local(
move_ax * (translate_amount + translation_noise[0])
+ perp_ax * translation_noise[1]
)
if motion_type == "linear":
rot_noise = multiplier * model.rotation.sample()
else:
# The robot will always turn a little bit. This has to be defined based on the intended actuation
# as otherwise small rotation amounts would be invalid. However, pretty quickly, we'll
# get to the truncation of 3 sigma
trunc = [(-0.95 * np.abs(np.deg2rad(rotate_amount)), None)]
rot_noise = multiplier * model.rotation.sample(trunc)
# Same deal with rotation about + EPS and why we multiply by the sign
rot_noise *= np.sign(rotate_amount + 1e-8)
scene_node.rotate_y_local(mn.Deg(rotate_amount) + mn.Rad(rot_noise))
scene_node.rotation = scene_node.rotation.normalized()
def place_robot_from_agent(
sim: habitat_sim.Simulator,
robot: habitat_sim._ext.habitat_sim_bindings.ManagedBulletArticulatedObject,
local_base_pos: list,
orientation_vector: list,
angle_correction: float,
) -> None:
"""Moves robot to reasonable transformation relative to agent."""
local_base_pos = np.array(local_base_pos)
# place the robot root state relative to the agent
agent_transform = sim.agents[0].scene_node.transformation_matrix()
base_transform = mn.Matrix4.rotation(
mn.Rad(angle_correction), mn.Vector3(*orientation_vector)
)
base_transform.translation = agent_transform.transform_point(local_base_pos)
robot.transformation = base_transform
agent_state = sim.agents[0].state
agent_state.position = np.array([-1.97496, 0.072447, -2.0894])
agent_state.rotation = ut.quat_from_coeffs([0, -1, 0, 0])
sim.agents[0].set_state(agent_state)
# load the target objects
cheezit_handle = obj_attr_mgr.get_template_handles("cheezit")[0]
# create range from center and half-extent
target_zone = mn.Range3D.from_center(
mn.Vector3(-2.07496, 1.07245, -0.2894), mn.Vector3(0.5, 0.05, 0.1)
)
num_targets = 9 # @param{type:"integer"}
for _target in range(num_targets):
obj_id = sim.add_object_by_handle(cheezit_handle)
# rotate boxes off of their sides
rotate = mn.Quaternion.rotation(mn.Rad(-mn.math.pi_half), mn.Vector3(1.0, 0, 0))
sim.set_rotation(rotate, obj_id)
# sample state from the target zone
if not sample_object_state(sim, obj_id, False, True, 100, target_zone):
sim.remove_object(obj_id)
show_target_zone = False # @param{type:"boolean"}
if show_target_zone:
# Get and modify the wire cube template from the range
cube_handle = obj_attr_mgr.get_template_handles("cubeWireframe")[0]
cube_template_cpy = obj_attr_mgr.get_template_by_handle(cube_handle)
cube_template_cpy.scale = target_zone.size()
cube_template_cpy.is_collidable = False
# Register the modified template under a new name.
obj_attr_mgr.register_template(cube_template_cpy, "target_zone")
# setup agent state manually to face the bar
agent_state = sim.agents[0].state
agent_state.position = np.array([-1.97496, 0.072447, -2.0894])
agent_state.rotation = ut.quat_from_coeffs([0, -1, 0, 0])
sim.agents[0].set_state(agent_state)
# load the target objects
cheezit_handle = obj_attr_mgr.get_template_handles("cheezit")[0]
# create range from center and half-extent
target_zone = mn.Range3D.from_center(mn.Vector3(-2.07496, 1.07245, -0.2894),
mn.Vector3(0.5, 0.05, 0.1))
num_targets = 9 # @param{type:"integer"}
for target in range(num_targets):
obj_id = sim.add_object_by_handle(cheezit_handle)
# rotate boxes off of their sides
rotate = mn.Quaternion.rotation(mn.Rad(-mn.math.pi_half),
mn.Vector3(1.0, 0, 0))
sim.set_rotation(rotate, obj_id)
# sample state from the target zone
if not sample_object_state(sim, obj_id, False, True, 100, target_zone):
sim.remove_object(obj_id)
show_target_zone = False # @param{type:"boolean"}
if show_target_zone:
# Get and modify the wire cube template from the range
cube_handle = obj_attr_mgr.get_template_handles("cubeWireframe")[0]
cube_template_cpy = obj_attr_mgr.get_template_by_handle(cube_handle)
cube_template_cpy.scale = target_zone.size()
cube_template_cpy.is_collidable = False
# Register the modified template under a new name.
obj_attr_mgr.register_template(cube_template_cpy, "target_zone")
def test_velocity_control():
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings["scene"] = "NONE"
cfg_settings["enable_physics"] = True
hab_cfg = examples.settings.make_cfg(cfg_settings)
with habitat_sim.Simulator(hab_cfg) as sim:
sim.set_gravity(np.array([0, 0, 0.0]))
obj_mgr = sim.get_object_template_manager()
template_path = osp.abspath("data/test_assets/objects/nested_box")
template_ids = obj_mgr.load_object_configs(template_path)
object_template = obj_mgr.get_template_by_ID(template_ids[0])
object_template.linear_damping = 0.0
object_template.angular_damping = 0.0
obj_mgr.register_template(object_template)
obj_handle = obj_mgr.get_template_handle_by_ID(template_ids[0])
for iteration in range(2):
sim.reset()
object_id = sim.add_object_by_handle(obj_handle)
vel_control = sim.get_object_velocity_control(object_id)
if iteration == 0:
if (sim.get_object_motion_type(object_id) !=
habitat_sim.physics.MotionType.DYNAMIC):
# Non-dynamic simulator in use. Skip 1st pass.
sim.remove_object(object_id)
continue
elif iteration == 1:
# test KINEMATIC
sim.set_object_motion_type(
habitat_sim.physics.MotionType.KINEMATIC, object_id)
# test global velocities
vel_control.linear_velocity = np.array([1.0, 0, 0])
vel_control.angular_velocity = np.array([0, 1.0, 0])
vel_control.controlling_lin_vel = True
vel_control.controlling_ang_vel = True
while sim.get_world_time() < 1.0:
# NOTE: stepping close to default timestep to get near-constant velocity control of DYNAMIC bodies.
sim.step_physics(0.00416)
ground_truth_pos = sim.get_world_time(
) * vel_control.linear_velocity
assert np.allclose(sim.get_translation(object_id),
ground_truth_pos,
atol=0.01)
ground_truth_q = mn.Quaternion([[0, 0.480551, 0], 0.876967])
angle_error = mn.math.angle(ground_truth_q,
sim.get_rotation(object_id))
assert angle_error < mn.Rad(0.005)
sim.reset()
# test local velocities (turn in a half circle)
vel_control.lin_vel_is_local = True
vel_control.ang_vel_is_local = True
vel_control.linear_velocity = np.array([0, 0, -math.pi])
vel_control.angular_velocity = np.array([math.pi * 2.0, 0, 0])
sim.set_translation(np.array([0, 0, 0.0]), object_id)
sim.set_rotation(mn.Quaternion(), object_id)
while sim.get_world_time() < 0.5:
# NOTE: stepping close to default timestep to get near-constant velocity control of DYNAMIC bodies.
sim.step_physics(0.008)
print(sim.get_world_time())
# NOTE: explicit integration, so expect some error
ground_truth_q = mn.Quaternion([[1.0, 0, 0], 0])
print(sim.get_translation(object_id))
assert np.allclose(sim.get_translation(object_id),
np.array([0, 1.0, 0.0]),
atol=0.07)
angle_error = mn.math.angle(ground_truth_q,
sim.get_rotation(object_id))
assert angle_error < mn.Rad(0.05)
sim.remove_object(object_id)
remove_all_objects(sim)
observations = []
capsule_template = prim_templates_mgr.get_default_capsule_template(is_wireframe = False)
capsule_template_handle = capsule_template.handle
id_1 = sim.add_object_by_handle(capsule_template_handle)
sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)
sim.set_translation(np.array([0.8, 0, 0.5]), id_1)
start_time = sim.get_world_time()
dt = 2
while sim.get_world_time() < start_time + dt:
# manually control the object's kinematic state
sim.set_translation(sim.get_translation(id_1) + np.array([0, 0, 0.01]), id_1)
sim.set_rotation(
mn.Quaternion.rotation(mn.Rad(0.025), np.array([0, 0, -1.0]))
* sim.get_rotation(id_1),
id_1,
)
sim.step_physics(1.0 / 60.0)
observations.append(sim.get_sensor_observations())
if make_video:
vut.make_video(
observations,
"semantic_camera_1stperson",
"semantic",
output_path + "3_kinematic_update_z",
open_vid=show_video,
)
# %%
observations = []
obj_templates_mgr.load_configs(str(os.path.join(data_path, "objects")))
# search for an object template by key sub-string
cheezit_template_handle = obj_templates_mgr.get_template_handles(
"data/objects/cheezit"
)[0]
box_positions = [
np.array([2.39, -0.37, 0]),
np.array([2.39, -0.64, 0]),
np.array([2.39, -0.91, 0]),
np.array([2.39, -0.64, -0.22]),
np.array([2.39, -0.64, 0.22]),
]
box_orientation = mn.Quaternion.rotation(
mn.Rad(math.pi / 2.0), np.array([-1.0, 0, 0])
)
# instance and place the boxes
box_ids = []
for b in range(5):
box_ids.append(sim.add_object_by_handle(cheezit_template_handle))
sim.set_translation(box_positions[b], box_ids[b])
sim.set_rotation(box_orientation, box_ids[b])
# get the object's initialization attributes (all boxes initialized with same mass)
object_init_template = sim.get_object_initialization_template(box_ids[0])
# anti-gravity force f=m(-g)
anti_grav_force = -1.0 * sim.get_gravity() * object_init_template.mass
# throw a sphere at the boxes from the agent position
sphere_template = obj_templates_mgr.get_template_by_ID(sphere_template_id)
# [kinematic_update]
observations = []
clamp_template_handle = obj_templates_mgr.get_template_handles(
"data/objects/largeclamp")[0]
clamp_obj = rigid_obj_mgr.add_object_by_handle(clamp_template_handle)
clamp_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC
clamp_obj.translation = [0.8, 0.0, 0.5]
start_time = sim.get_world_time()
dt = 1.0
while sim.get_world_time() < start_time + dt:
# manually control the object's kinematic state
clamp_obj.translation = clamp_obj.translation + [0.0, 0.0, 0.01]
clamp_obj.rotation = (
mn.Quaternion.rotation(mn.Rad(0.05), [-1.0, 0.0, 0.0]) *
clamp_obj.rotation)
sim.step_physics(1.0 / 60.0)
observations.append(sim.get_sensor_observations())
if make_video:
vut.make_video(
observations,
"rgba_camera_1stperson",
"color",
output_path + "kinematic_update",
open_vid=show_video,
)
# [/kinematic_update]
# %%
build_widget_ui(sim.metadata_mediator)
# %% [markdown]
# ## Load the Select Scene and Simulate!
# This cell will load the scene selected above, simulate, and produce a visualization.
# %%
global selected_scene
if sim_settings["scene"] != selected_scene:
sim_settings["scene"] = selected_scene
make_simulator_from_settings(sim_settings)
observations = []
start_time = sim.get_world_time()
while sim.get_world_time() < start_time + 4.0:
sim.agents[0].scene_node.rotate(mn.Rad(mn.math.pi_half / 60.0),
mn.Vector3(0, 1, 0))
sim.step_physics(1.0 / 60.0)
if make_video:
observations.append(sim.get_sensor_observations())
# video rendering of carousel view
video_prefix = "ReplicaCAD_scene_view"
if make_video:
vut.make_video(
observations,
"color_sensor_1st_person",
"color",
output_path + video_prefix,
open_vid=show_video,
video_dims=[1280, 720],
def base_rot(self, rotation_y_rad: float):
self.sim_obj.rotation = mn.Quaternion.rotation(
mn.Rad(rotation_y_rad), mn.Vector3(0, 1, 0)
)
def base_transformation(self):
add_rot = mn.Matrix4.rotation(mn.Rad(-np.pi / 2), mn.Vector3(1.0, 0, 0))
return self.sim_obj.transformation @ add_rot
sphere_template_handle = sphere_template.handle
#add 5 cubes
cheezit_template = prim_templates_mgr.get_default_cylinder_template(is_wireframe = False)
cheezit_template_handle = cheezit_template.handle
cheezit_template.half_length = 2.5
box_positions = [
np.array([2.39, -0.37, 0]),
np.array([2.39, -0.64, 0]),
np.array([2.39, -0.91, 0]),
np.array([2.39, -0.64, -0.22]),
np.array([2.39, -0.64, 0.22]),
]
box_orientation = mn.Quaternion.rotation(
mn.Rad(math.pi / 2.0), np.array([-1.0, 0, 0]))
print("box_orientation: ",box_orientation)
# instance and place the boxes
box_ids = []
for b in range(5):
box_ids.append(sim.add_object_by_handle(cheezit_template_handle))
sim.set_translation(box_positions[b], box_ids[b])
sim.set_rotation(box_orientation, box_ids[b])
# get the object's initialization attributes (all boxes initialized with same mass)
object_init_template = sim.get_object_initialization_template(box_ids[0])
# anti-gravity force f=m(-g)
anti_grav_force = -1.0 * sim.get_gravity() * object_init_template.mass
# throw a sphere at the boxes from the agent position
sphere_template_id = sphere_template.ID
def sample_object_state(
sim, object_id, from_navmesh=True, maintain_object_up=True, max_tries=100, bb=None
):
# check that the object is not STATIC
if sim.get_object_motion_type(object_id) is habitat_sim.physics.MotionType.STATIC:
print("sample_object_state : Object is STATIC, aborting.")
if from_navmesh:
if not sim.pathfinder.is_loaded:
print("sample_object_state : No pathfinder, aborting.")
return False
elif not bb:
print(
"sample_object_state : from_navmesh not specified and no bounding box provided, aborting."
)
return False
tries = 0
valid_placement = False
# Note: following assumes sim was not reconfigured without close
scene_collision_margin = stage_attr_mgr.get_template_by_ID(0).margin
while not valid_placement and tries < max_tries:
tries += 1
# initialize sample location to random point in scene bounding box
sample_location = np.array([0, 0, 0])
if from_navmesh:
# query random navigable point
sample_location = sim.pathfinder.get_random_navigable_point()
else:
sample_location = np.random.uniform(bb.min, bb.max)
# set the test state
sim.set_translation(sample_location, object_id)
if maintain_object_up:
# random rotation only on the Y axis
y_rotation = mn.Quaternion.rotation(
mn.Rad(random.random() * 2 * math.pi), mn.Vector3(0, 1.0, 0)
)
sim.set_rotation(y_rotation * sim.get_rotation(object_id), object_id)
else:
# unconstrained random rotation
sim.set_rotation(ut.random_quaternion(), object_id)
# raise object such that lowest bounding box corner is above the navmesh sample point.
if from_navmesh:
obj_node = sim.get_object_scene_node(object_id)
xform_bb = habitat_sim.geo.get_transformed_bb(
obj_node.cumulative_bb, obj_node.transformation
)
# also account for collision margin of the scene
y_translation = mn.Vector3(
0, xform_bb.size_y() / 2.0 + scene_collision_margin, 0
)
sim.set_translation(
y_translation + sim.get_translation(object_id), object_id
)
# test for penetration with the environment
if not sim.contact_test(object_id):
valid_placement = True
if not valid_placement:
return False
return True
try_to_make_valid = True # @param {type:"boolean"}
if set_agent_state:
pos_x = 0 # @param {type:"number"}
pos_y = 0 # @param {type:"number"}
pos_z = 0.0 # @param {type:"number"}
# @markdown Y axis rotation (radians):
orientation = 1.56 # @param {type:"number"}
agent_state.position = np.array([pos_x, pos_y, pos_z])
if try_to_make_valid:
snapped_point = np.array(sim.pathfinder.snap_point(agent_state.position))
if not np.isnan(np.sum(snapped_point)):
print("Successfully snapped point to: " + str(snapped_point))
agent_state.position = snapped_point
if set_agent_state or set_random_valid_state:
agent_state.rotation = utils.quat_from_magnum(
mn.Quaternion.rotation(-mn.Rad(orientation), mn.Vector3(0, 1.0, 0))
)
sim.agents[0].set_state(agent_state)
agent_state = sim.agents[0].get_state()
print("Agent state: " + str(agent_state))
print(" position = " + str(agent_state.position))
print(" rotation = " + str(agent_state.rotation))
print(" orientation (about Y) = " + str(orientation))
observations = sim.get_sensor_observations()
rgb = observations["color_sensor"]
semantic = observations["semantic_sensor"]
depth = observations["depth_sensor"]
if display:
def main(make_video=True, show_video=True):
if make_video:
if not os.path.exists(output_path):
os.mkdir(output_path)
# [initialize]
# create the simulator
cfg = make_configuration()
sim = habitat_sim.Simulator(cfg)
agent_transform = place_agent(sim)
# [/initialize]
# [basics]
# load some object templates from configuration files
sphere_template_id = sim.load_object_configs(
str(os.path.join(data_path, "test_assets/objects/sphere")))[0]
# add an object to the scene
id_1 = sim.add_object(sphere_template_id)
sim.set_translation(np.array([2.50, 0, 0.2]), id_1)
# simulate
observations = simulate(sim, dt=1.5, get_frames=make_video)
if make_video:
make_video_cv2(observations, prefix="sim_basics", open_vid=show_video)
# [/basics]
remove_all_objects(sim)
# [dynamic_control]
observations = []
# search for an object template by key sub-string
cheezit_template_handle = sim.get_template_handles(
"data/objects/cheezit")[0]
box_positions = [
np.array([2.39, -0.37, 0]),
np.array([2.39, -0.64, 0]),
np.array([2.39, -0.91, 0]),
np.array([2.39, -0.64, -0.22]),
np.array([2.39, -0.64, 0.22]),
]
box_orientation = mn.Quaternion.rotation(mn.Rad(math.pi / 2.0),
np.array([-1.0, 0, 0]))
# instance and place the boxes
box_ids = []
for b in range(5):
box_ids.append(sim.add_object_by_handle(cheezit_template_handle))
sim.set_translation(box_positions[b], box_ids[b])
sim.set_rotation(box_orientation, box_ids[b])
# get the object's initialization attributes (all boxes initialized with same mass)
object_init_template = sim.get_object_initialization_template(box_ids[0])
# anti-gravity force f=m(-g)
anti_grav_force = -1.0 * sim.get_gravity() * object_init_template.get_mass(
)
# throw a sphere at the boxes from the agent position
sphere_template = sim.get_object_template(sphere_template_id)
sphere_template.set_scale(np.array([0.5, 0.5, 0.5]))
sphere_id = sim.add_object(sphere_template_id)
sim.set_translation(
sim.agents[0].get_state().position + np.array([0, 1.0, 0]), sphere_id)
# get the vector from the sphere to a box
target_direction = sim.get_translation(
box_ids[0]) - sim.get_translation(sphere_id)
# apply an initial velocity for one step
sim.set_linear_velocity(target_direction * 5, sphere_id)
sim.set_angular_velocity(np.array([0, -1.0, 0]), sphere_id)
start_time = sim.get_world_time()
dt = 3.0
while sim.get_world_time() < start_time + dt:
# set forces/torques before stepping the world
for box_id in box_ids:
sim.apply_force(anti_grav_force, np.array([0, 0.0, 0]), box_id)
sim.apply_torque(np.array([0, 0.01, 0]), box_id)
sim.step_physics(1.0 / 60.0)
observations.append(sim.get_sensor_observations())
if make_video:
make_video_cv2(observations,
prefix="dynamic_control",
open_vid=show_video)
# [/dynamic_control]
remove_all_objects(sim)
# [kinematic_interactions]
chefcan_template_handle = sim.get_template_handles(
"data/objects/chefcan")[0]
id_1 = sim.add_object_by_handle(chefcan_template_handle)
sim.set_translation(np.array([2.4, -0.64, 0]), id_1)
# set one object to kinematic
sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)
# drop some dynamic objects
id_2 = sim.add_object_by_handle(chefcan_template_handle)
sim.set_translation(np.array([2.4, -0.64, 0.28]), id_2)
id_3 = sim.add_object_by_handle(chefcan_template_handle)
sim.set_translation(np.array([2.4, -0.64, -0.28]), id_3)
id_4 = sim.add_object_by_handle(chefcan_template_handle)
sim.set_translation(np.array([2.4, -0.3, 0]), id_4)
# simulate
observations = simulate(sim, dt=1.5, get_frames=True)
if make_video:
make_video_cv2(observations,
prefix="kinematic_interactions",
open_vid=show_video)
# [/kinematic_interactions]
remove_all_objects(sim)
# [kinematic_update]
observations = []
clamp_template_handle = sim.get_template_handles(
"data/objects/largeclamp")[0]
id_1 = sim.add_object_by_handle(clamp_template_handle)
sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)
sim.set_translation(np.array([0.8, 0, 0.5]), id_1)
start_time = sim.get_world_time()
dt = 1.0
while sim.get_world_time() < start_time + dt:
# manually control the object's kinematic state
sim.set_translation(
sim.get_translation(id_1) + np.array([0, 0, 0.01]), id_1)
sim.set_rotation(
mn.Quaternion.rotation(mn.Rad(0.05), np.array([-1.0, 0, 0])) *
sim.get_rotation(id_1),
id_1,
)
sim.step_physics(1.0 / 60.0)
observations.append(sim.get_sensor_observations())
if make_video:
make_video_cv2(observations,
prefix="kinematic_update",
open_vid=show_video)
# [/kinematic_update]
# [velocity_control]
# get object VelocityControl structure and setup control
vel_control = sim.get_object_velocity_control(id_1)
vel_control.linear_velocity = np.array([0, 0, -1.0])
vel_control.angular_velocity = np.array([4.0, 0, 0])
vel_control.controlling_lin_vel = True
vel_control.controlling_ang_vel = True
observations = simulate(sim, dt=1.0, get_frames=True)
# reverse linear direction
vel_control.linear_velocity = np.array([0, 0, 1.0])
observations += simulate(sim, dt=1.0, get_frames=True)
make_video_cv2(observations, prefix="velocity_control", open_vid=True)
# [/velocity_control]
# [local_velocity_control]
vel_control.linear_velocity = np.array([0, 0, 2.3])
vel_control.angular_velocity = np.array([-4.3, 0.0, 0])
vel_control.lin_vel_is_local = True
vel_control.ang_vel_is_local = True
observations = simulate(sim, dt=1.5, get_frames=True)
# video rendering
make_video_cv2(observations,
prefix="local_velocity_control",
open_vid=True)
# [/local_velocity_control]
# [embodied_agent]
# load the lobot_merged asset
locobot_template_id = sim.load_object_configs(
str(os.path.join(data_path, "objects/locobot_merged")))[0]
# add robot object to the scene with the agent/camera SceneNode attached
id_1 = sim.add_object(locobot_template_id, sim.agents[0].scene_node)
sim.set_translation(np.array([1.75, -1.02, 0.4]), id_1)
vel_control = sim.get_object_velocity_control(id_1)
vel_control.linear_velocity = np.array([0, 0, -1.0])
vel_control.angular_velocity = np.array([0.0, 2.0, 0])
# simulate robot dropping into place
observations = simulate(sim, dt=1.5, get_frames=make_video)
vel_control.controlling_lin_vel = True
vel_control.controlling_ang_vel = True
vel_control.lin_vel_is_local = True
vel_control.ang_vel_is_local = True
# simulate forward and turn
observations += simulate(sim, dt=1.0, get_frames=make_video)
vel_control.controlling_lin_vel = False
vel_control.angular_velocity = np.array([0.0, 1.0, 0])
# simulate turn only
observations += simulate(sim, dt=1.5, get_frames=make_video)
vel_control.angular_velocity = np.array([0.0, 0.0, 0])
vel_control.controlling_lin_vel = True
vel_control.controlling_ang_vel = True
# simulate forward only with damped angular velocity (reset angular velocity to 0 after each step)
observations += simulate(sim, dt=1.0, get_frames=make_video)
vel_control.angular_velocity = np.array([0.0, -1.25, 0])
# simulate forward and turn
observations += simulate(sim, dt=2.0, get_frames=make_video)
vel_control.controlling_ang_vel = False
vel_control.controlling_lin_vel = False
# simulate settling
observations += simulate(sim, dt=3.0, get_frames=make_video)
# video rendering with embedded 1st person view
make_video_cv2(observations,
prefix="robot_control",
open_vid=True,
multi_obs=True)
# %%
# [dynamic_control]
observations = []
obj_templates_mgr.load_configs(str(os.path.join(data_path, "objects")))
# search for an object template by key sub-string
cheezit_template_handle = obj_templates_mgr.get_template_handles(
"data/objects/cheezit")[0]
box_positions = [
np.array([2.39, -0.37, 0]),
np.array([2.39, -0.64, 0]),
np.array([2.39, -0.91, 0]),
np.array([2.39, -0.64, -0.22]),
np.array([2.39, -0.64, 0.22]),
]
box_orientation = mn.Quaternion.rotation(mn.Rad(math.pi / 2.0),
np.array([-1.0, 0, 0]))
# instance and place the boxes
box_ids = []
for b in range(5):
box_ids.append(sim.add_object_by_handle(cheezit_template_handle))
sim.set_translation(box_positions[b], box_ids[b])
sim.set_rotation(box_orientation, box_ids[b])
# get the object's initialization attributes (all boxes initialized with same mass)
object_init_template = sim.get_object_initialization_template(box_ids[0])
# anti-gravity force f=m(-g)
anti_grav_force = -1.0 * sim.get_gravity() * object_init_template.mass
# throw a sphere at the boxes from the agent position
sphere_template = obj_templates_mgr.get_template_by_ID(sphere_template_id)
def main(make_video=True, show_video=True):
if make_video:
if not os.path.exists(output_path):
os.mkdir(output_path)
# [initialize]
# create the simulator
cfg = make_configuration()
sim = habitat_sim.Simulator(cfg)
agent_transform = place_agent(sim)
# [/initialize]
# [basics]
# load some object templates from configuration files
sphere_template_id = sim.load_object_configs(
str(os.path.join(data_path, "test_assets/objects/sphere")))[0]
# add an object to the scene
id_1 = sim.add_object(sphere_template_id)
sim.set_translation(np.array([2.50, 0, 0.2]), id_1)
# simulate
observations = simulate(sim, dt=1.5, get_frames=make_video)
if make_video:
make_video_cv2(observations, prefix="sim_basics", open_vid=show_video)
# [/basics]
remove_all_objects(sim)
# [dynamic_control]
observations = []
# search for an object template by key sub-string
cheezit_template_handle = sim.get_template_handles(
"data/objects/cheezit")[0]
box_positions = [
np.array([2.39, -0.37, 0]),
np.array([2.39, -0.64, 0]),
np.array([2.39, -0.91, 0]),
np.array([2.39, -0.64, -0.22]),
np.array([2.39, -0.64, 0.22]),
]
box_orientation = mn.Quaternion.rotation(mn.Rad(math.pi / 2.0),
np.array([-1.0, 0, 0]))
# instance and place the boxes
box_ids = []
for b in range(5):
box_ids.append(sim.add_object_by_handle(cheezit_template_handle))
sim.set_translation(box_positions[b], box_ids[b])
sim.set_rotation(box_orientation, box_ids[b])
# get the object's initialization attributes (all boxes initialized with same mass)
object_init_template = sim.get_object_initialization_template(box_ids[0])
# anti-gravity force f=m(-g)
anti_grav_force = -1.0 * sim.get_gravity() * object_init_template.get_mass(
)
# throw a sphere at the boxes from the agent position
sphere_template = sim.get_object_template(sphere_template_id)
sphere_template.set_scale(np.array([0.5, 0.5, 0.5]))
sphere_id = sim.add_object(sphere_template_id)
sim.set_translation(
sim.agents[0].get_state().position + np.array([0, 1.0, 0]), sphere_id)
# get the vector from the sphere to a box
target_direction = sim.get_translation(
box_ids[0]) - sim.get_translation(sphere_id)
# apply an initial velocity for one step
sim.set_linear_velocity(target_direction * 5, sphere_id)
sim.set_angular_velocity(np.array([0, -1.0, 0]), sphere_id)
start_time = sim.get_world_time()
dt = 3.0
while sim.get_world_time() < start_time + dt:
# set forces/torques before stepping the world
for box_id in box_ids:
sim.apply_force(anti_grav_force, np.array([0, 0.0, 0]), box_id)
sim.apply_torque(np.array([0, 0.01, 0]), box_id)
sim.step_physics(1.0 / 60.0)
observations.append(sim.get_sensor_observations())
if make_video:
make_video_cv2(observations,
prefix="dynamic_control",
open_vid=show_video)
# [/dynamic_control]
remove_all_objects(sim)
# [kinematic_interactions]
chefcan_template_handle = sim.get_template_handles(
"data/objects/chefcan")[0]
id_1 = sim.add_object_by_handle(chefcan_template_handle)
sim.set_translation(np.array([2.4, -0.64, 0]), id_1)
# set one object to kinematic
sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)
# drop some dynamic objects
id_2 = sim.add_object_by_handle(chefcan_template_handle)
sim.set_translation(np.array([2.4, -0.64, 0.28]), id_2)
id_3 = sim.add_object_by_handle(chefcan_template_handle)
sim.set_translation(np.array([2.4, -0.64, -0.28]), id_3)
id_4 = sim.add_object_by_handle(chefcan_template_handle)
sim.set_translation(np.array([2.4, -0.3, 0]), id_4)
# simulate
observations = simulate(sim, dt=1.5, get_frames=True)
if make_video:
make_video_cv2(observations,
prefix="kinematic_interactions",
open_vid=show_video)
# [/kinematic_interactions]
remove_all_objects(sim)
# [kinematic_update]
observations = []
clamp_template_handle = sim.get_template_handles(
"data/objects/largeclamp")[0]
id_1 = sim.add_object_by_handle(clamp_template_handle)
sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)
sim.set_translation(np.array([0.8, 0, 0.5]), id_1)
start_time = sim.get_world_time()
dt = 1.0
while sim.get_world_time() < start_time + dt:
# manually control the object's kinematic state
sim.set_translation(
sim.get_translation(id_1) + np.array([0, 0, 0.01]), id_1)
sim.set_rotation(
mn.Quaternion.rotation(mn.Rad(0.05), np.array([-1.0, 0, 0])) *
sim.get_rotation(id_1),
id_1,
)
sim.step_physics(1.0 / 60.0)
observations.append(sim.get_sensor_observations())
if make_video:
make_video_cv2(observations,
prefix="kinematic_update",
open_vid=show_video)
# [/kinematic_update]
# [velocity_control]
# get object VelocityControl structure and setup control
vel_control = sim.get_object_velocity_control(id_1)
vel_control.linear_velocity = np.array([0, 0, -1.0])
vel_control.angular_velocity = np.array([4.0, 0, 0])
vel_control.controlling_lin_vel = True
vel_control.controlling_ang_vel = True
observations = simulate(sim, dt=1.0, get_frames=True)
# reverse linear direction
vel_control.linear_velocity = np.array([0, 0, 1.0])
observations += simulate(sim, dt=1.0, get_frames=True)
if make_video:
make_video_cv2(observations, prefix="velocity_control", open_vid=True)
# [/velocity_control]
# [local_velocity_control]
vel_control.linear_velocity = np.array([0, 0, 2.3])
vel_control.angular_velocity = np.array([-4.3, 0.0, 0])
vel_control.lin_vel_is_local = True
vel_control.ang_vel_is_local = True
observations = simulate(sim, dt=1.5, get_frames=True)
# video rendering
if make_video:
make_video_cv2(observations,
prefix="local_velocity_control",
open_vid=True)
# [/local_velocity_control]
# [embodied_agent]
# load the lobot_merged asset
locobot_template_id = sim.load_object_configs(
str(os.path.join(data_path, "objects/locobot_merged")))[0]
# add robot object to the scene with the agent/camera SceneNode attached
id_1 = sim.add_object(locobot_template_id, sim.agents[0].scene_node)
sim.set_translation(np.array([1.75, -1.02, 0.4]), id_1)
vel_control = sim.get_object_velocity_control(id_1)
vel_control.linear_velocity = np.array([0, 0, -1.0])
vel_control.angular_velocity = np.array([0.0, 2.0, 0])
# simulate robot dropping into place
observations = simulate(sim, dt=1.5, get_frames=make_video)
vel_control.controlling_lin_vel = True
vel_control.controlling_ang_vel = True
vel_control.lin_vel_is_local = True
vel_control.ang_vel_is_local = True
# simulate forward and turn
observations += simulate(sim, dt=1.0, get_frames=make_video)
vel_control.controlling_lin_vel = False
vel_control.angular_velocity = np.array([0.0, 1.0, 0])
# simulate turn only
observations += simulate(sim, dt=1.5, get_frames=make_video)
vel_control.angular_velocity = np.array([0.0, 0.0, 0])
vel_control.controlling_lin_vel = True
vel_control.controlling_ang_vel = True
# simulate forward only with damped angular velocity (reset angular velocity to 0 after each step)
observations += simulate(sim, dt=1.0, get_frames=make_video)
vel_control.angular_velocity = np.array([0.0, -1.25, 0])
# simulate forward and turn
observations += simulate(sim, dt=2.0, get_frames=make_video)
vel_control.controlling_ang_vel = False
vel_control.controlling_lin_vel = False
# simulate settling
observations += simulate(sim, dt=3.0, get_frames=make_video)
# remove the agent's body while preserving the SceneNode
sim.remove_object(id_1, False)
# video rendering with embedded 1st person view
if make_video:
make_video_cv2(observations,
prefix="robot_control",
open_vid=True,
multi_obs=True)
# [/embodied_agent]
# [embodied_agent_navmesh]
# load the lobot_merged asset
locobot_template_id = sim.load_object_configs(
str(os.path.join(data_path, "objects/locobot_merged")))[0]
# add robot object to the scene with the agent/camera SceneNode attached
id_1 = sim.add_object(locobot_template_id, sim.agents[0].scene_node)
initial_rotation = sim.get_rotation(id_1)
# set the agent's body to kinematic since we will be updating position manually
sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)
# create and configure a new VelocityControl structure
# Note: this is NOT the object's VelocityControl, so it will not be consumed automatically in sim.step_physics
vel_control = habitat_sim.physics.VelocityControl()
vel_control.controlling_lin_vel = True
vel_control.lin_vel_is_local = True
vel_control.controlling_ang_vel = True
vel_control.ang_vel_is_local = True
vel_control.linear_velocity = np.array([0, 0, -1.0])
# try 2 variations of the control experiment
for iteration in range(2):
# reset observations and robot state
observations = []
sim.set_translation(np.array([1.75, -1.02, 0.4]), id_1)
sim.set_rotation(initial_rotation, id_1)
vel_control.angular_velocity = np.array([0.0, 0, 0])
video_prefix = "robot_control_sliding"
# turn sliding off for the 2nd pass
if iteration == 1:
sim.config.sim_cfg.allow_sliding = False
video_prefix = "robot_control_no_sliding"
# manually control the object's kinematic state via velocity integration
start_time = sim.get_world_time()
last_velocity_set = 0
dt = 6.0
time_step = 1.0 / 60.0
while sim.get_world_time() < start_time + dt:
previous_rigid_state = sim.get_rigid_state(id_1)
# manually integrate the rigid state
target_rigid_state = vel_control.integrate_transform(
time_step, previous_rigid_state)
# snap rigid state to navmesh and set state to object/agent
end_pos = sim.step_filter(previous_rigid_state.translation,
target_rigid_state.translation)
sim.set_translation(end_pos, id_1)
sim.set_rotation(target_rigid_state.rotation, id_1)
# Check if a collision occured
dist_moved_before_filter = (
target_rigid_state.translation -
previous_rigid_state.translation).dot()
dist_moved_after_filter = (end_pos -
previous_rigid_state.translation).dot()
# NB: There are some cases where ||filter_end - end_pos|| > 0 when a
# collision _didn't_ happen. One such case is going up stairs. Instead,
# we check to see if the the amount moved after the application of the filter
# is _less_ than the amount moved before the application of the filter
EPS = 1e-5
collided = (dist_moved_after_filter +
EPS) < dist_moved_before_filter
# run any dynamics simulation
sim.step_physics(time_step)
# render observation
observations.append(sim.get_sensor_observations())
# randomize angular velocity
last_velocity_set += time_step
if last_velocity_set >= 1.0:
vel_control.angular_velocity = np.array(
[0, (random.random() - 0.5) * 2.0, 0])
last_velocity_set = 0
# video rendering with embedded 1st person view
if make_video:
make_video_cv2(observations,
prefix=video_prefix,
open_vid=True,
multi_obs=True)
