From d831cc3944002870a87c80b8cc9d3d05b373e59c Mon Sep 17 00:00:00 2001 From: Kenny Erleben Date: Mon, 26 Aug 2024 20:07:30 +0200 Subject: [PATCH] Refactoring Adding Hinge joints to rigid body simulator --- rainbow/simulators/prox_rigid_bodies/api.py | 166 +++++--- .../prox_rigid_bodies/gauss_seidel_solver.py | 7 +- .../prox_rigid_bodies/procedural/__init__.py | 13 +- .../procedural/create_box_stack.py | 61 +++ .../procedural/create_cube_hinge_chain.py | 85 ++++ .../procedural/create_gear_train.py | 41 ++ .../procedural/create_scene.py | 272 +++++++++++++ .../prox_rigid_bodies/procedural/write_xml.py | 119 ++++++ rainbow/simulators/prox_rigid_bodies/types.py | 6 +- rigid_bodies_app_gui.py | 373 ++---------------- 10 files changed, 753 insertions(+), 390 deletions(-) create mode 100644 rainbow/simulators/prox_rigid_bodies/procedural/create_box_stack.py create mode 100644 rainbow/simulators/prox_rigid_bodies/procedural/create_cube_hinge_chain.py create mode 100644 rainbow/simulators/prox_rigid_bodies/procedural/create_scene.py create mode 100644 rainbow/simulators/prox_rigid_bodies/procedural/write_xml.py diff --git a/rainbow/simulators/prox_rigid_bodies/api.py b/rainbow/simulators/prox_rigid_bodies/api.py index 60202aa..30d5a5a 100644 --- a/rainbow/simulators/prox_rigid_bodies/api.py +++ b/rainbow/simulators/prox_rigid_bodies/api.py @@ -16,6 +16,7 @@ import rainbow.math.coordsys as FRAME import rainbow.simulators.prox_rigid_bodies.steppers as STEPPERS from rainbow.simulators.prox_rigid_bodies.types import * +import rainbow.math.matrix3 as M3 def generate_unique_name(name: str) -> str: @@ -77,60 +78,77 @@ def create_hinge(engine, hinge_name: str) -> None: engine.hinges[hinge_name] = hinge -def set_hinge_sockets(engine, hinge_name: str, - parent_name: str, - r_parent: np.ndarray, - q_parent: np.ndarray, - child_name: str, - r_child: np.ndarray, - q_child: np.ndarray - ) -> None: - """ - - :param engine: - :param hinge_name: - :param parent_name: - :param r_parent: - :param q_parent: - :param child_name: - :param r_child: - :param q_child: +def set_hinge( + engine: Engine, + hinge_name: str, + parent_name: str, + child_name: str, + origin: np.ndarray, + axis: np.ndarray, + mode: str = "world" +) -> None: + """ + Set hinge joint parameters. + + :param engine: The engine that contains the hinge. + :param hinge_name: The name of the hinge. + :param parent_name: The name of the parent body (link). + :param child_name: The name of the child body (link). + :param origin: The position of the origin for the joint frame. + :param axis: The joint axis of the hinge. + :param mode: This controls the coordinate space in which axis and origin are given with respect + to. + It can be either "world", "body" frame of parent body, or "model" frame of parent. """ if hinge_name in engine.hinges: hinge = engine.hinges[hinge_name] else: - raise RuntimeError("set_hinge_sockets() no such rigid body exist with that name") + raise RuntimeError("set_hinge() no such rigid body exist with that name") if parent_name in engine.bodies: - parent = engine.bodies[parent_name] + parent_link = engine.bodies[parent_name] else: - raise RuntimeError("set_hinge_sockets() no such rigid body exist with that name") + raise RuntimeError("set_hinge() no such rigid body exist with that name") if child_name in engine.bodies: - child = engine.bodies[child_name] + child_link = engine.bodies[child_name] + else: + raise RuntimeError("set_hinge() no such rigid body exist with that name") + + o_world = None + s_world = None + + if mode == "world": + o_world = np.copy(origin) + s_world = np.copy(axis) + elif mode == "body": + # Convert o and s from body frame to world frame + o_world = Q.rotate(parent_link.q, origin) + parent_link.r + s_world = Q.rotate(parent_link.q, axis) + elif mode == "model": + # Convert o and s from model frame to body frame + r_b2m = np.copy(parent_link.shape.r) + q_b2m = np.copy(parent_link.shape.q) + o_body = Q.rotate(Q.conjugate(q_b2m), origin - r_b2m) + s_body = Q.rotate(Q.conjugate(q_b2m), axis) + # Convert o and s from body frame to world frame + o_world = Q.rotate(parent_link.q, o_body) + parent_link.r + s_world = Q.rotate(parent_link.q, s_body) else: - raise RuntimeError("set_hinge_sockets() no such rigid body exist with that name") + raise ValueError(f"set_hinge() no such mode {mode} is supported") - socket_parent = FRAME.make(r_parent, q_parent) - socket_child = FRAME.make(r_child, q_child) + # Observe, we use convention that n (the local z-axis) will be the joint axis. + t, b, n = V3.make_orthonormal_vectors(s_world) + R_world = M3.make_from_cols(t, b, n) + q_world = Q.from_matrix(R_world) - # Currently, we assume that the socket joint frames live in the body frame coordinate systems - # of the rigid bodies they belong to. - # - # A socket is a coordinate mapping from joint frame space to body-space of the link. - # - # When rigging a simulation, it may be that the rigging person does not know the body - # frames. - # Instead, what is known is the model frames of the rigid bodies. - # - # R_parent_bf2mf = np.copy(parent.shape.r) - # q_parent_bf2mf = np.copy(parent.shape.q) - # - # - # Hence, we know (bf->mf) we are given (jf->mf) and we need to compute (jf->bf) - # - # Xjb = Xjm Xmb - # - hinge.set_parent_socket(parent, socket_parent) - hinge.set_parent_socket(child, socket_child) + q_parent_socket = Q.prod(Q.conjugate(parent_link.q), q_world) + q_child_socket = Q.prod(Q.conjugate(child_link.q), q_world) + r_parent_socket = Q.rotate(Q.conjugate(parent_link.q), o_world - parent_link.r) + r_child_socket = Q.rotate(Q.conjugate(child_link.q), o_world - child_link.r) + + parent_socket = FRAME.make(r_parent_socket, q_parent_socket) + child_socket = FRAME.make(r_child_socket, q_child_socket) + hinge.set_parent_socket(parent_link, parent_socket) + hinge.set_child_socket(child_link, child_socket) def create_mesh(V: np.ndarray, T: np.ndarray) -> MESH.Mesh: @@ -387,6 +405,21 @@ def set_position(engine, body_name: str, r: np.ndarray, use_model_frame=False) - body.r = np.copy(r) +def get_position(engine, body_name: str) -> np.ndarray: + """ + Retrieve the center of mass position for the rigid body. + + :param engine: The engine that stores the rigid body. + :param body_name: The name of the rigid body. + :return: The center of mass position for the given rigid body. + """ + if body_name in engine.bodies: + body = engine.bodies[body_name] + else: + raise RuntimeError("get_position() no such rigid body exist with that name") + return np.copy(body.r) + + def set_orientation(engine, body_name: str, q: np.ndarray, use_model_frame=False) -> None: """ Set orientation of rigid body. @@ -446,6 +479,21 @@ def set_orientation(engine, body_name: str, q: np.ndarray, use_model_frame=False body.q = Q.unit(q) +def get_orientation(engine, body_name: str) -> np.ndarray: + """ + Retrieve body frame orientation for the rigid body. + + :param engine: The engine that stores the rigid body. + :param body_name: The name of the rigid body. + :return: The orientation of the body frame for the given rigid body. + """ + if body_name in engine.bodies: + body = engine.bodies[body_name] + else: + raise RuntimeError("get_orientation() no such rigid body exist with that name") + return np.copy(body.q) + + def set_velocity(engine, body_name: str, v: np.ndarray) -> None: """ Set the linear velocity of a rigid body. @@ -462,6 +510,21 @@ def set_velocity(engine, body_name: str, v: np.ndarray) -> None: body.v = np.copy(v) +def get_velocity(engine, body_name: str) -> np.ndarray: + """ + Retrieve the linear velocity for the rigid body. + + :param engine: The engine that stores the rigid body. + :param body_name: The name of the rigid body. + :return: The linear velocity for the given rigid body. + """ + if body_name in engine.bodies: + body = engine.bodies[body_name] + else: + raise RuntimeError("get_velocity() no such rigid body exist with that name") + return np.copy(body.v) + + def set_spin(engine, body_name: str, w: np.ndarray) -> None: """ Set the angular velocity of a rigid body. @@ -478,6 +541,21 @@ def set_spin(engine, body_name: str, w: np.ndarray) -> None: body.w = np.copy(w) +def get_spin(engine, body_name: str) -> np.ndarray: + """ + Retrieve the angular velocity for the rigid body. + + :param engine: The engine that stores the rigid body. + :param body_name: The name of the rigid body. + :return: The angular velocity for the given rigid body. + """ + if body_name in engine.bodies: + body = engine.bodies[body_name] + else: + raise RuntimeError("get_spin() no such rigid body exist with that name") + return np.copy(body.w) + + def set_mass_properties(engine, body_name: str, density: float) -> None: """ Set the mass properties of a given rigid body. diff --git a/rainbow/simulators/prox_rigid_bodies/gauss_seidel_solver.py b/rainbow/simulators/prox_rigid_bodies/gauss_seidel_solver.py index 8c2dbf5..8f4a087 100644 --- a/rainbow/simulators/prox_rigid_bodies/gauss_seidel_solver.py +++ b/rainbow/simulators/prox_rigid_bodies/gauss_seidel_solver.py @@ -11,7 +11,12 @@ from rainbow.util.timer import Timer -def solve(engine, Problems: list, performance_data: dict[str, any], profiling_on: bool, prefix: str) -> None: +def solve( + engine, Problems: list, + performance_data: dict[str, any], + profiling_on: bool, + prefix: str +) -> None: """ :param engine: diff --git a/rainbow/simulators/prox_rigid_bodies/procedural/__init__.py b/rainbow/simulators/prox_rigid_bodies/procedural/__init__.py index a506e9c..9791699 100644 --- a/rainbow/simulators/prox_rigid_bodies/procedural/__init__.py +++ b/rainbow/simulators/prox_rigid_bodies/procedural/__init__.py @@ -19,6 +19,12 @@ from .create_temple import create_temple from .create_tower import create_tower from .create_ground import create_ground +from .create_box_stack import create_box_stack +from .create_cube_hinge_chain import create_cube_hinge_chain +from .write_xml import write_xml +from .create_scene import create_scene +from .create_scene import get_scene_names + __all__ = ["create_arch", "create_chainmail", @@ -35,5 +41,10 @@ "create_poles", "create_rock_slide", "create_sandbox", - "create_temple" + "create_temple", + "create_box_stack", + "create_cube_hinge_chain", + "write_xml", + "create_scene", + "get_scene_names" ] diff --git a/rainbow/simulators/prox_rigid_bodies/procedural/create_box_stack.py b/rainbow/simulators/prox_rigid_bodies/procedural/create_box_stack.py new file mode 100644 index 0000000..7f3d2ae --- /dev/null +++ b/rainbow/simulators/prox_rigid_bodies/procedural/create_box_stack.py @@ -0,0 +1,61 @@ +""" +This script contains code to create a sandbox scene. +""" +import numpy as np + +import rainbow.math.vector3 as V3 +import rainbow.math.quaternion as Q +import rainbow.geometry.surface_mesh as MESH +import rainbow.simulators.prox_rigid_bodies.api as API +from rainbow.simulators.prox_rigid_bodies.types import Engine + + +def create_box_stack( + engine: Engine, + box_width: float, + box_height: float, + box_depth: float, + K_boxes: int, + density: float, + material_name: str, +) -> list[str]: + """ + This function creates a box stack scene. + + :param engine: The engine that will be used to create the dry stone rigid bodies in. + :param box_width: The width of the boxes. + :param box_height: The height of the boxes. + :param box_depth: The depth of the boxes. + :param K_boxes: The number of boxes that will be stacked. + :param density: The mass density to use for all the rigid bodies. + :param material_name: The material name to use for all the rigid bodies that are created. + :return: A list with the names of all the rigid bodies that were created. + """ + + shape_name = API.generate_unique_name("box") + V, T = MESH.create_box(box_width, box_height, box_depth) + mesh = API.create_mesh(V, T) + API.create_shape(engine, shape_name, mesh) + + body_names = [] + + for k in range(K_boxes): + + body_name = API.generate_unique_name("body") + body_names.append(body_name) + API.create_rigid_body(engine, body_name) + API.connect_shape(engine, body_name, shape_name) + + height = k*box_height + box_height/2.0 + r = V3.make(0.0, height, 0.0) + + radians = k*np.pi/4 + q = Q.Ry(radians) + + API.set_position(engine, body_name, r, True) + API.set_orientation(engine, body_name, q, True) + API.set_body_type(engine, body_name, "free") + API.set_body_material(engine, body_name, material_name) + API.set_mass_properties(engine, body_name, density) + + return body_names diff --git a/rainbow/simulators/prox_rigid_bodies/procedural/create_cube_hinge_chain.py b/rainbow/simulators/prox_rigid_bodies/procedural/create_cube_hinge_chain.py new file mode 100644 index 0000000..343a13e --- /dev/null +++ b/rainbow/simulators/prox_rigid_bodies/procedural/create_cube_hinge_chain.py @@ -0,0 +1,85 @@ +""" +This script contains code to create a sandbox scene. +""" +import numpy as np + +import rainbow.math.vector3 as V3 +import rainbow.math.quaternion as Q +import rainbow.geometry.surface_mesh as MESH +import rainbow.simulators.prox_rigid_bodies.api as API +from rainbow.simulators.prox_rigid_bodies.types import Engine + + +def create_cube_hinge_chain( + engine: Engine, + cube_edge_length: float, + K_cubes: int, + density: float, + material_name: str, +) -> list[str]: + """ + This function creates a scene with a chain of cubes connected by hinge joints. + + :param engine: The engine that will be used to create the dry stone rigid bodies in. + :param cube_edge_length: The edge length of all the cubes. + :param K_cubes: The number of cubes that will be stacked. + :param density: The mass density to use for all the rigid bodies. + :param material_name: The material name to use for all the rigid bodies that are created. + :return: A list with the names of all the rigid bodies that were created. + """ + + diameter = np.sqrt(2 * cube_edge_length ** 2) + radius = diameter / 2.0 + margin = 0.1 * radius + chain_length = K_cubes * diameter + (K_cubes - 1) * margin + + shape_name = API.generate_unique_name("cube") + V, T = MESH.create_box(cube_edge_length, cube_edge_length, cube_edge_length) + mesh = API.create_mesh(V, T) + API.create_shape(engine, shape_name, mesh) + + body_names = [] + + for k in range(K_cubes): + body_name = API.generate_unique_name("link") + body_names.append(body_name) + API.create_rigid_body(engine, body_name) + API.connect_shape(engine, body_name, shape_name) + + displacement = k * (diameter + margin) + radius - chain_length / 2.0 + r = V3.make(displacement, 0.0, 0.0) + + radians = np.pi / 4 + q = Q.Rz(radians) + + API.set_position(engine, body_name, r, True) + API.set_orientation(engine, body_name, q, True) + API.set_body_type(engine, body_name, "free") + API.set_body_material(engine, body_name, material_name) + API.set_mass_properties(engine, body_name, density) + + API.set_body_type(engine, body_names[0], "fixed") + + for k in range(K_cubes - 1): + parent_name = body_names[k] + child_name = body_names[k + 1] + + hinge_name = parent_name + "_" + child_name + API.create_hinge(engine, hinge_name) + + s_world = V3.k() + r_parent = API.get_position(engine, parent_name) + r_child = API.get_position(engine, child_name) + o_world = (r_parent + r_child) / 2 + + API.set_hinge( + engine=engine, + hinge_name=hinge_name, + parent_name=parent_name, + child_name=child_name, + origin=o_world, + axis=s_world, + mode="world" + ) + + return body_names diff --git a/rainbow/simulators/prox_rigid_bodies/procedural/create_gear_train.py b/rainbow/simulators/prox_rigid_bodies/procedural/create_gear_train.py index ca78769..58e80a6 100644 --- a/rainbow/simulators/prox_rigid_bodies/procedural/create_gear_train.py +++ b/rainbow/simulators/prox_rigid_bodies/procedural/create_gear_train.py @@ -7,6 +7,7 @@ import rainbow.math.vector3 as V3 import rainbow.math.quaternion as Q import rainbow.simulators.prox_rigid_bodies.api as API +import rainbow.geometry.surface_mesh as MESH from rainbow.simulators.prox_rigid_bodies.types import Engine @@ -360,6 +361,7 @@ def create_gear_train(engine: Engine, gear_specs = [] gear_names = [] + # Create and place all the gears q_m2w = Q.Rx(-np.pi / 2) # Needed to change the z-up direction to a y-up direction. m = 1.0 # Gear module @@ -414,4 +416,43 @@ def create_gear_train(engine: Engine, API.set_position(engine, driven_gear_body_name, r_w, True) API.set_orientation(engine, driven_gear_body_name, q_w, True) + # Create a fixed object in the world + shape_name = API.generate_unique_name("ground_shape") + + V, T = MESH.create_box(200.0, 1.0, 200.0) + mesh = API.create_mesh(V, T) + API.create_shape(engine, shape_name, mesh) + + body_name = API.generate_unique_name("ground_body") + API.create_rigid_body(engine, body_name) + API.connect_shape(engine, body_name, shape_name) + + r = V3.make(0.0, -0.5 - face_width, 0.0) + q = Q.identity() + + API.set_position(engine, body_name, r, True) + API.set_orientation(engine, body_name, q, True) + + API.set_body_type(engine, body_name, "fixed") + API.set_body_material(engine, body_name, material_name) + API.set_mass_properties(engine, body_name, density) + body_names.append(body_name) + + # Create hinge-joints between gears and fixed object + parent_name = body_name + for i in range(N): + child_name = body_names[i] + hinge_name = parent_name + "_" + child_name + API.create_hinge(engine, hinge_name) + origin = API.get_position(engine, child_name) - V3.make(0.0, face_width/2.0, 0.0) + API.set_hinge( + engine=engine, + hinge_name=hinge_name, + parent_name=parent_name, + child_name=child_name, + origin= origin, + axis= V3.k(), + mode="world" + ) + return body_names diff --git a/rainbow/simulators/prox_rigid_bodies/procedural/create_scene.py b/rainbow/simulators/prox_rigid_bodies/procedural/create_scene.py new file mode 100644 index 0000000..84601c8 --- /dev/null +++ b/rainbow/simulators/prox_rigid_bodies/procedural/create_scene.py @@ -0,0 +1,272 @@ +""" +This script contains a high-level function to create different scenes. +""" +import logging + +import rainbow.simulators.prox_rigid_bodies.types as TYPES +import rainbow.simulators.prox_rigid_bodies.api as API +import rainbow.simulators.prox_rigid_bodies.procedural as PROC +import rainbow.math.quaternion as Q +import rainbow.math.vector3 as V3 + + +def get_scene_names() -> list[str]: + """ + Get the list of all scene names that can be created by the create_scene function. + + :return: List of scene names. + """ + names = [ + "pillar", + "arch", + "dome", + "tower", + "colosseum", + "pantheon", + "funnel", + "glasses", + "poles", + "temple", + "chainmail", + "gear_train", + "rock_slide", + "sandbox", + "box_stack", + "cube_hinge_chain" + ] + return names + + +def create_scene(engine: TYPES.Engine, scene_name: str) -> None: + """ + Create a scene in a rigid body simulator. + + :param engine: The engine that should contain the scene. + :param scene_name: The name of the scene is to be created. + """ + logger = logging.getLogger("main.setup_scene") + logger.info(f"Setting up: {scene_name}") + + scene_names = get_scene_names() + + if scene_name == scene_names[0]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_pillar(engine, + r=V3.zero(), + q=Q.identity(), + width=1.0, + height=5.0, + depth=1.0, + stones=3, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[1]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_arch(engine, + r=V3.zero(), + q=Q.identity(), + width=2.0, + height=3.0, + depth=0.5, + pier_stones=3, + arch_stones=5, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[2]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_dome(engine, + r=V3.zero(), + q=Q.identity(), + outer_radius=5.0, + inner_radius=4.0, + layers=4, + segments=11, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[3]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_tower(engine, + r=V3.zero(), + q=Q.identity(), + outer_radius=5.0, + inner_radius=4.0, + height=8.0, + layers=6, + segments=11, + use_cubes=False, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[4]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_colosseum(engine, + r=V3.zero(), + q=Q.identity(), + outer_radius=5.0, + inner_radius=4.0, + height=7.0, + levels=3, + arches=12, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[5]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_pantheon(engine, + r=V3.zero(), + q=Q.identity(), + outer_radius=5.0, + inner_radius=4.0, + height=8.0, + layers=4, + segments=11, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[6]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_funnel(engine, + funnel_height=4.0, + funnel_radius=4.0, + grid_width=4.0, + grid_height=8.0, + grid_depth=4.0, + I=10, + J=20, + K=10, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[7]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_glasses(engine, + glass_height=4.0, + glass_radius=2.0, + grid_width=3.0, + grid_height=3.0, + grid_depth=3.0, + I=4, + J=4, + K=4, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[8]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_poles(engine, + pole_height=2.0, + pole_radius=0.1, + I_poles=6, + K_poles=6, + grid_width=4.0, + grid_height=4.0, + grid_depth=4.0, + I_grid=4, + J_grid=4, + K_grid=4, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[9]: + PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') + PROC.create_temple(engine, + I_pillars=4, + K_pillars=7, + pillar_width=1.0, + pillar_height=3.0, + pillar_depth=1.0, + pillar_stones=3, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[10]: + PROC.create_chainmail( + engine, + major_radius=2, + minor_radius=0.5, + columns=10, + rows=10, + stretch=0.75, + density=1.0, + material_name='default' + ) + PROC.create_jack_lattice( + engine, + r=V3.make(-20, 40, -20), + q=Q.identity(), + width=40.0, + height=40.0, + depth=40.0, + I=5, + J=5, + K=5, + density=1.0, + material_name='default', + use_random_orientation=True + ) + elif scene_name == scene_names[11]: + PROC.create_gear_train( + engine, + N=14, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[12]: + PROC.create_rock_slide( + engine, + pile_width=8, + pile_height=4, + pile_depth=2, + I_rocks=20, + J_rocks=10, + K_rocks=5, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[13]: + PROC.create_sandbox( + engine, + box_width=10, + box_height=10, + box_depth=10, + I_grains=20, + J_grains=20, + K_grains=20, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[14]: + PROC.create_ground( + engine, + V3.zero(), + Q.identity(), + density=1.0, + material_name='default' + ) + PROC.create_box_stack( + engine, + box_width=1.0, + box_height=1.0, + box_depth=1.0, + K_boxes=5, + density=1.0, + material_name='default' + ) + elif scene_name == scene_names[15]: + PROC.create_cube_hinge_chain( + engine, + cube_edge_length=10.0, + K_cubes=5, + density=1.0, + material_name='default' + ) + + API.create_gravity_force(engine=engine, force_name="earth", g=9.81, up=V3.j()) + API.create_damping_force(engine=engine, force_name="air", alpha=0.01, beta=0.01) + for body in engine.bodies.values(): + API.connect_force(engine=engine, body_name=body.name, force_name="earth") + API.connect_force(engine=engine, body_name=body.name, force_name="air") + logger.info(f"Done with creating {scene_name}") diff --git a/rainbow/simulators/prox_rigid_bodies/procedural/write_xml.py b/rainbow/simulators/prox_rigid_bodies/procedural/write_xml.py new file mode 100644 index 0000000..e11ea09 --- /dev/null +++ b/rainbow/simulators/prox_rigid_bodies/procedural/write_xml.py @@ -0,0 +1,119 @@ +""" +This file contains a function to export the scene content of a rigid body engine to a file. +""" +import logging + +import numpy as np + +import rainbow.math.quaternion as Q +import rainbow.simulators.prox_rigid_bodies.types as TYPES + + +def write_xml(engine: TYPES.Engine, xml_filename: str) -> None: + """ + Write a scene to xml-file. + + :param engine: The engine containing the scene. + :param xml_filename: The filename of the output xml file. + """ + logger = logging.getLogger("rainbow.simulators.prox_rigid_bodies.procedural.write_xml") + logger.info(f"Converting scene to xml file: {xml_filename}") + + import xml.etree.ElementTree as ET + + root = ET.Element("scene") + + bodies_xml_node = ET.SubElement(root, "bodies") + for body in engine.bodies.values(): + body_xml_node = ET.SubElement(bodies_xml_node, "body") + body_xml_node.set("name", body.name) + body_xml_node.set("idx", str(body.idx)) + body_xml_node.set("shape", body.shape.name) + body_xml_node.set("pos", np.array2string(body.r)) + body_xml_node.set("rot", np.array2string(body.q)) + body_xml_node.set("vel", np.array2string(body.v)) + body_xml_node.set("spin", np.array2string(body.w)) + body_xml_node.set("mass", str(body.mass)) + body_xml_node.set("inertia", np.array2string(body.inertia)) + body_xml_node.set("is_fixed", str(body.is_fixed)) + body_xml_node.set("material", body.material) + for force in body.forces: + apply_force_xml_node = ET.SubElement(body_xml_node, "apply") + apply_force_xml_node.set("name", force.name) + + forces_xml_node = ET.SubElement(root, "forces") + for force in engine.forces.values(): + if force.force_type == "Gravity": + gravity_xml_node = ET.SubElement(forces_xml_node, "gravity") + gravity_xml_node.set("name", force.name) + # print(force.force_type) + gravity_xml_node.set("g", str(force.g)) + gravity_xml_node.set("up", np.array2string(force.up)) + if force.force_type == "Damping": + damping_xml_node = ET.SubElement(forces_xml_node, "damping") + damping_xml_node.set("name", force.name) + # print(force.force_type) + damping_xml_node.set("alpha", str(force.alpha)) + damping_xml_node.set("beta", str(force.beta)) + + shapes_xml_node = ET.SubElement(root, "shapes") + for shape in engine.shapes.values(): + shape_xml_node = ET.SubElement(shapes_xml_node, "shape") + shape_xml_node.set("name", shape.name) + vertices_xml_node = ET.SubElement(shape_xml_node, "vertices") + vertices_xml_node.set("count", str(len(shape.mesh.V))) + for idx, v in enumerate(shape.mesh.V): + vertex_xml_node = ET.SubElement(vertices_xml_node, "v") + vertex_xml_node.set("idx", str(idx)) + vertex_xml_node.set("p", np.array2string(v)) + triangles_xml_node = ET.SubElement(shape_xml_node, "triangles") + triangles_xml_node.set("count", str(len(shape.mesh.T))) + for idx, t in enumerate(shape.mesh.T): + triangle_xml_node = ET.SubElement(triangles_xml_node, "t") + triangle_xml_node.set("idx", str(idx)) + triangle_xml_node.set("labels", np.array2string(t)) + + materials_xml_node = ET.SubElement(root, "materials") + for key, interaction in engine.surfaces_interactions.storage.items(): + interaction_xml_node = ET.SubElement(materials_xml_node, "interaction") + interaction_xml_node.set("materials", key) + interaction_xml_node.set("friction", np.array2string(interaction.mu)) + interaction_xml_node.set("restitution", str(interaction.epsilon)) + + hinges_xml_node = ET.SubElement(root, "hinges") + for idx, hinge in enumerate(engine.hinges.values()): + hinge_xml_node = ET.SubElement(hinges_xml_node, "hinge") + hinge_xml_node.set("name", hinge.name) + hinge_xml_node.set("idx", str(idx)) + hinge_xml_node.set("parent", hinge.parent.name) + hinge_xml_node.set("child", hinge.child.name) + o_world = Q.rotate(hinge.parent.q, hinge.arm_p) + hinge.parent.r + s_world = Q.rotate(hinge.parent.q, hinge.axis_p) + hinge_xml_node.set("origin", np.array2string(o_world)) + hinge_xml_node.set("axis", np.array2string(s_world)) + hinge_xml_node.set("mode", "world") + + params_xml_node = ET.SubElement(root, "params") + settings = dir(engine.params) + for name in settings: + if not name.startswith("__"): # Skip built-in attributes + param_xml_node = ET.SubElement(params_xml_node, "param") + value = getattr(engine.params, name) + param_xml_node.set("name", str(name)) + param_xml_node.set("value", str(value)) + param_xml_node.set("type", type(value).__name__) + + tree = ET.ElementTree(root) + ET.indent(tree, ' ', level=0) + ET.indent(tree, ' ', level=1) + ET.indent(tree, ' ', level=2) + ET.indent(tree, ' ', level=3) + + tree.write( + xml_filename, + encoding="utf-8", + xml_declaration=True, + method="xml", + short_empty_elements=True + ) + logger.info(f"Done writing file: {xml_filename}") diff --git a/rainbow/simulators/prox_rigid_bodies/types.py b/rainbow/simulators/prox_rigid_bodies/types.py index 6a82a9e..4c24ac1 100644 --- a/rainbow/simulators/prox_rigid_bodies/types.py +++ b/rainbow/simulators/prox_rigid_bodies/types.py @@ -5,7 +5,7 @@ rigid bodies. """ from abc import ABC, abstractmethod -from typing import Tuple, Optional +from typing import Tuple, Optional, Union import numpy as np @@ -363,7 +363,7 @@ def __init__(self): Create an instance of the parameter class. """ self.time_step: float = ( - 0.001 # The desired time step size to use when taking one simulation solver step. + 0.01 # The desired time step size to use when taking one simulation solver step. ) self.max_iterations: int = 200 # Maximum number of Gauss Seidel iterations self.use_bounce: bool = False # Turning bounce on and off @@ -436,7 +436,7 @@ def __init__(self): """ self.simulator_type: str = 'rigid_body' # The simulation type for the engine self.bodies: dict[str, RigidBody] = dict() - self.forces: dict[str, ForceCalculator] = dict() + self.forces: dict[str, Union[Gravity, Damping]] = dict() self.shapes: dict[str, Shape] = dict() self.hinges: dict[str, Hinge] = dict() # All hinge joints in the current simulation self.contact_points: list[ContactPoint] = [] diff --git a/rigid_bodies_app_gui.py b/rigid_bodies_app_gui.py index 4d1e057..7192f04 100644 --- a/rigid_bodies_app_gui.py +++ b/rigid_bodies_app_gui.py @@ -9,300 +9,7 @@ import rainbow.simulators.prox_rigid_bodies.api as API import rainbow.simulators.prox_rigid_bodies.procedural as PROC -app_params = {} # Dictionary used to control parameters that affect application - - -def setup_scene(engine, scene_name: str): - logger = logging.getLogger("main.setup_scene") - logger.info(f"Setting up: {scene_name}") - - if scene_name == "pillar": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_pillar(engine, - r=V3.zero(), - q=Q.identity(), - width=1.0, - height=5.0, - depth=1.0, - stones=3, - density=1.0, - material_name='default' - ) - elif scene_name == "arch": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_arch(engine, - r=V3.zero(), - q=Q.identity(), - width=2.0, - height=3.0, - depth=0.5, - pier_stones=3, - arch_stones=5, - density=1.0, - material_name='default' - ) - elif scene_name == "dome": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_dome(engine, - r=V3.zero(), - q=Q.identity(), - outer_radius=5.0, - inner_radius=4.0, - layers=4, - segments=11, - density=1.0, - material_name='default' - ) - elif scene_name == "tower": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_tower(engine, - r=V3.zero(), - q=Q.identity(), - outer_radius=5.0, - inner_radius=4.0, - height=8.0, - layers=6, - segments=11, - use_cubes=False, - density=1.0, - material_name='default' - ) - elif scene_name == "colosseum": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_colosseum(engine, - r=V3.zero(), - q=Q.identity(), - outer_radius=5.0, - inner_radius=4.0, - height=7.0, - levels=3, - arches=12, - density=1.0, - material_name='default' - ) - elif scene_name == "pantheon": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_pantheon(engine, - r=V3.zero(), - q=Q.identity(), - outer_radius=5.0, - inner_radius=4.0, - height=8.0, - layers=4, - segments=11, - density=1.0, - material_name='default' - ) - elif scene_name == "funnel": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_funnel(engine, - funnel_height=4.0, - funnel_radius=4.0, - grid_width=4.0, - grid_height=8.0, - grid_depth=4.0, - I=10, - J=20, - K=10, - density=1.0, - material_name='default' - ) - elif scene_name == "glasses": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_glasses(engine, - glass_height=4.0, - glass_radius=2.0, - grid_width=3.0, - grid_height=3.0, - grid_depth=3.0, - I=4, - J=4, - K=4, - density=1.0, - material_name='default' - ) - elif scene_name == "poles": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_poles(engine, - pole_height=2.0, - pole_radius=0.1, - I_poles=6, - K_poles=6, - grid_width=4.0, - grid_height=4.0, - grid_depth=4.0, - I_grid=4, - J_grid=4, - K_grid=4, - density=1.0, - material_name='default' - ) - elif scene_name == "temple": - PROC.create_ground(engine, V3.zero(), Q.identity(), density=1.0, material_name='default') - PROC.create_temple(engine, - I_pillars=4, - K_pillars=7, - pillar_width=1.0, - pillar_height=3.0, - pillar_depth=1.0, - pillar_stones=3, - density=1.0, - material_name='default' - ) - elif scene_name == "chainmail": - PROC.create_chainmail( - engine, - major_radius=2, - minor_radius=0.5, - columns=10, - rows=10, - stretch=0.75, - density=1.0, - material_name='default' - ) - PROC.create_jack_lattice( - engine, - r=V3.make(-20, 40, -20), - q=Q.identity(), - width=40.0, - height=40.0, - depth=40.0, - I=5, - J=5, - K=5, - density=1.0, - material_name='default', - use_random_orientation=True - ) - elif scene_name == "gear_train": - PROC.create_gear_train( - engine, - N=14, - density=1.0, - material_name='default' - ) - elif scene_name == "rock_slide": - PROC.create_rock_slide( - engine, - pile_width=8, - pile_height=4, - pile_depth=2, - I_rocks=20, - J_rocks=10, - K_rocks=5, - density=1.0, - material_name='default' - ) - elif scene_name == "sandbox": - PROC.create_sandbox( - engine, - box_width=10, - box_height=10, - box_depth=10, - I_grains=20, - J_grains=20, - K_grains=20, - density=1.0, - material_name='default' - ) - - API.create_gravity_force(engine=engine, force_name="earth", g=9.81, up=V3.j()) - API.create_damping_force(engine=engine, force_name="air", alpha=0.01, beta=0.01) - for body in engine.bodies.values(): - API.connect_force(engine=engine, body_name=body.name, force_name="earth") - API.connect_force(engine=engine, body_name=body.name, force_name="air") - logger.info(f"Done with creating {scene_name}") - - -def export_to_xml(engine, xml_filename): - logger = logging.getLogger("main.export_to_xml") - logger.info(f"Converting scene to xml file: {xml_filename}") - - import xml.etree.ElementTree as ET - - root = ET.Element("scene") - - bodies_xml_node = ET.SubElement(root, "bodies") - for body in engine.bodies.values(): - body_xml_node = ET.SubElement(bodies_xml_node, "body") - body_xml_node.set("name", body.name) - body_xml_node.set("idx", str(body.idx)) - body_xml_node.set("shape", body.shape.name) - body_xml_node.set("pos", np.array2string(body.r)) - body_xml_node.set("rot", np.array2string(body.q)) - body_xml_node.set("vel", np.array2string(body.v)) - body_xml_node.set("spin", np.array2string(body.w)) - body_xml_node.set("mass", str(body.mass)) - body_xml_node.set("inertia", np.array2string(body.inertia)) - body_xml_node.set("is_fixed", str(body.is_fixed)) - body_xml_node.set("material", body.material) - for force in body.forces: - apply_force_xml_node = ET.SubElement(body_xml_node, "apply") - apply_force_xml_node.set("name", force.name) - - forces_xml_node = ET.SubElement(root, "forces") - for force in engine.forces.values(): - if force.force_type == "Gravity": - gravity_xml_node = ET.SubElement(forces_xml_node, "gravity") - gravity_xml_node.set("name", force.name) - # print(force.force_type) - gravity_xml_node.set("g", str(force.g)) - gravity_xml_node.set("up", np.array2string(force.up)) - if force.force_type == "Damping": - damping_xml_node = ET.SubElement(forces_xml_node, "damping") - damping_xml_node.set("name", force.name) - # print(force.force_type) - damping_xml_node.set("alpha", str(force.alpha)) - damping_xml_node.set("beta", str(force.beta)) - - shapes_xml_node = ET.SubElement(root, "shapes") - for shape in engine.shapes.values(): - shape_xml_node = ET.SubElement(shapes_xml_node, "shape") - shape_xml_node.set("name", shape.name) - vertices_xml_node = ET.SubElement(shape_xml_node, "vertices") - vertices_xml_node.set("count", str(len(shape.mesh.V))) - for idx, v in enumerate(shape.mesh.V): - vertex_xml_node = ET.SubElement(vertices_xml_node, "v") - vertex_xml_node.set("idx", str(idx)) - vertex_xml_node.set("p", np.array2string(v)) - triangles_xml_node = ET.SubElement(shape_xml_node, "triangles") - triangles_xml_node.set("count", str(len(shape.mesh.T))) - for idx, t in enumerate(shape.mesh.T): - triangle_xml_node = ET.SubElement(triangles_xml_node, "t") - triangle_xml_node.set("idx", str(idx)) - triangle_xml_node.set("labels", np.array2string(t)) - - materials_xml_node = ET.SubElement(root, "materials") - for key, interaction in engine.surfaces_interactions.storage.items(): - interaction_xml_node = ET.SubElement(materials_xml_node, "interaction") - interaction_xml_node.set("materials", key) - interaction_xml_node.set("friction", np.array2string(interaction.mu)) - interaction_xml_node.set("restitution", str(interaction.epsilon)) - - params_xml_node = ET.SubElement(root, "params") - settings = dir(engine.params) - for name in settings: - if not name.startswith("__"): # Skip built-in attributes - param_xml_node = ET.SubElement(params_xml_node, "param") - value = getattr(engine.params, name) - param_xml_node.set("name", str(name)) - param_xml_node.set("value", str(value)) - param_xml_node.set("type", type(value).__name__) - - tree = ET.ElementTree(root) - ET.indent(tree, ' ', level=0) - ET.indent(tree, ' ', level=1) - ET.indent(tree, ' ', level=2) - ET.indent(tree, ' ', level=3) - - tree.write( - xml_filename, - encoding="utf-8", - xml_declaration=True, - method="xml", - short_empty_elements=True - ) - logger.info(f"Done writing file: {xml_filename}") +app_params = {} # Dictionary used to control parameters that affect application def plotting(profiling_data): @@ -440,57 +147,56 @@ def create_gui(): global app_params - changed, app_params["simulate"] = psim.Checkbox("Simulate", app_params["simulate"]) + changed, app_params['simulate'] = psim.Checkbox('Simulate', app_params['simulate']) if changed: - logger.info(f"Application will run simulation loop = {app_params["simulate"]}") - changed, app_params["xml"] = psim.Checkbox("Save xml", app_params["xml"]) + logger.info(f"Application will run simulation loop = {app_params['simulate']}") + changed, app_params['xml'] = psim.Checkbox('Save xml', app_params['xml']) if changed: - logger.info(f"Application will save XML files = {app_params["xml"]}") - + logger.info(f"Application will save XML files = {app_params['xml']}") - changed, app_params["profiling"] = psim.Checkbox("Show profiling", app_params["profiling"]) + changed, app_params['profiling'] = psim.Checkbox('Show profiling', app_params['profiling']) if changed: - logger.info(f"Application will show profiling and plots = {app_params["profiling"]}") - changed, app_params["selected"] = psim.Combo("Scene", app_params["selected"], app_params["names"]) + logger.info(f"Application will show profiling and plots = {app_params['profiling']}") + changed, app_params['selected'] = psim.Combo('Scene', app_params['selected'], app_params['names']) if changed: - logger.info(f"Selected scene = {app_params["selected"]}") - if psim.Button("Create scene"): - scene_name = app_params["names"][app_params["selected"]] + logger.info(f"Selected scene = {app_params['selected']}") + if psim.Button('Create scene'): + scene_name = app_params['names'][app_params['selected']] logger.info(f"Creating scene = {scene_name}") engine = API.create_engine() - total_time = 0.5 + total_time = 10.0 steps = int(np.round(total_time / engine.params.time_step)) - app_params["total time"] = total_time - app_params["steps"] = steps - app_params["step"] = 0 + app_params['total time'] = total_time + app_params['steps'] = steps + app_params['step'] = 0 - setup_scene(engine=engine, scene_name=scene_name) + PROC.create_scene(engine=engine, scene_name=scene_name) - if app_params["xml"]: - scene_name = app_params["names"][app_params['selected']] - export_to_xml(engine, scene_name + ".xml") + if app_params['xml']: + scene_name = app_params['names'][app_params['selected']] + PROC.write_xml(engine, scene_name + '.xml') create_visual_geometry(engine=engine) - app_params["engine"] = engine + app_params['engine'] = engine def simulate() -> None: logger = logging.getLogger("main.simulate") - engine = app_params["engine"] + engine = app_params['engine'] if engine is None: return - if not app_params["simulate"]: + if not app_params['simulate']: return - if app_params["step"] >= app_params["steps"]: + if app_params['step'] >= app_params['steps']: return - logger.info(f"Running simulation step {app_params["step"]}") + logger.info(f"Running simulation step {app_params['step']}") for body in engine.bodies.values(): T = np.eye(4) T[:3, :3] = Q.to_matrix(body.q) @@ -499,7 +205,7 @@ def simulate() -> None: API.simulate(engine=engine, T=engine.params.time_step, profiling_on=True) - app_params["step"] += 1 + app_params['step'] += 1 logger.info(f"Completed simulation step") @@ -514,30 +220,15 @@ def main(): ps.set_up_dir('y_up') ps.init() ps.set_build_default_gui_panels(False) - ps.set_ground_plane_mode("none") + ps.set_ground_plane_mode('none') ps.look_at((0., 0., 100.), (0., 0., 0.)) - app_params["engine"] = None - app_params["simulate"] = False - app_params["xml"] = False - app_params["profiling"] = False - app_params["selected"] = 0 - app_params["names"] = [ - "pillar", - "arch", - "dome", - "tower", - "colosseum", - "pantheon", - "funnel", - "glasses", - "poles", - "temple", - "chainmail", - "gear_train", - "rock_slide", - "sandbox" - ] + app_params['engine'] = None + app_params['simulate'] = False + app_params['xml'] = False + app_params['profiling'] = False + app_params['selected'] = 0 + app_params['names'] = PROC.get_scene_names() ps.set_user_callback(callback)