From 87a7da6cf077454af2b02ca57723149ca4e11f2c Mon Sep 17 00:00:00 2001 From: Abhishek Joshi Date: Wed, 10 Apr 2024 12:00:05 -0500 Subject: [PATCH] updating capsule to consist of hemispheres instead of spheres --- python/mujoco/usd/demo.py | 2 +- python/mujoco/usd/shapes.py | 76 +++++++++++++++++++++++++++++-------- python/mujoco/usd/test.py | 43 +++++++++++++++++++++ 3 files changed, 104 insertions(+), 17 deletions(-) create mode 100644 python/mujoco/usd/test.py diff --git a/python/mujoco/usd/demo.py b/python/mujoco/usd/demo.py index 19b4c358..86346530 100644 --- a/python/mujoco/usd/demo.py +++ b/python/mujoco/usd/demo.py @@ -6,7 +6,7 @@ import exporter if __name__ == "__main__": # load a model to mujoco - m = mujoco.MjModel.from_xml_path("/Users/abhishek/Documents/research/mujoco/model/car/car.xml") + m = mujoco.MjModel.from_xml_path("/Users/abhishek/Documents/research/mujoco/model/humanoid/humanoid.xml") d = mujoco.MjData(m) # create an instance of the USDExporter diff --git a/python/mujoco/usd/shapes.py b/python/mujoco/usd/shapes.py index 2959e7da..0cd6842e 100644 --- a/python/mujoco/usd/shapes.py +++ b/python/mujoco/usd/shapes.py @@ -4,6 +4,30 @@ import pprint import numpy as np import open3d as o3d +def create_hemisphere( + radius: float, + resolution: int = 20, + theta_steps: int = 50, + phi_steps: int = 50 +): + + points = [] + for i in range(phi_steps + 1): + phi = np.pi / 2 * i / phi_steps + for j in range(theta_steps + 1): + theta = 2 * np.pi * j / theta_steps + x = radius * np.sin(phi) * np.cos(theta) + y = radius * np.sin(phi) * np.sin(theta) + z = radius * np.cos(phi) + points.append([x, y, z]) + + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + + mesh = pcd.compute_convex_hull()[0] + + return mesh + def mesh_config_generator( name: str, geom_type: mujoco.mjtGeom, @@ -29,19 +53,22 @@ def mesh_config_generator( } elif geom_type == mujoco.mjtGeom.mjGEOM_CAPSULE: cylinder = mesh_config_generator(name, mujoco.mjtGeom.mjGEOM_CYLINDER, size) - left_sphere = mesh_config_generator(name, mujoco.mjtGeom.mjGEOM_SPHERE, size) - right_sphere = copy.deepcopy(left_sphere) - left_sphere["sphere"]["transform"] = { - "translate": (0, 0, -size[2]) - } - right_sphere["sphere"]["transform"] = { - "translate": (0, 0, size[2]) - } return { "name": name, "cylinder": cylinder["cylinder"], - "left_sphere": left_sphere["sphere"], - "right_sphere": right_sphere["sphere"], + "left_hemisphere": { + "radius": size[0], + "transform": { + "translate": (0, 0, -size[2]), + "rotate": (np.pi, 0, 0) + } + }, + "right_hemisphere": { + "radius": size[0], + "transform": { + "translate": (0, 0, size[2]) + } + }, } elif geom_type == mujoco.mjtGeom.mjGEOM_ELLIPSOID: sphere = mesh_config_generator(name, mujoco.mjtGeom.mjGEOM_SPHERE, [1.0]) @@ -94,6 +121,10 @@ def mesh_generator( create_uv_map=True, map_texture_to_each_face=True, ) + elif "hemisphere" in shape: + prim_mesh = create_hemisphere( + radius=mesh_config[shape]["radius"] + ) elif "sphere" in shape: prim_mesh = o3d.geometry.TriangleMesh.create_sphere( radius=mesh_config[shape]["radius"], @@ -107,12 +138,25 @@ def mesh_generator( ) if "transform" in config: - for transform, val in config["transform"].items(): - if transform == "translate": - prim_mesh.translate(val) - if transform == "scale": - prim_mesh.vertices = o3d.utility.Vector3dVector( - np.asarray(prim_mesh.vertices) * np.array(val)) + + if "rotate" in config["transform"]: + R = mesh.get_rotation_matrix_from_xyz(config["transform"]["rotate"]) + prim_mesh.rotate(R, center=(0, 0, 0)) + if "scale" in config["transform"]: + prim_mesh.vertices = o3d.utility.Vector3dVector( + np.asarray(prim_mesh.vertices) * np.array(config["transform"]["scale"])) + if "translate" in config["transform"]: + prim_mesh.translate(config["transform"]["translate"]) + + # for transform, val in config["transform"].items(): + # if transform == "translate": + # prim_mesh.translate(val) + # elif transform == "scale": + # prim_mesh.vertices = o3d.utility.Vector3dVector( + # np.asarray(prim_mesh.vertices) * np.array(val)) + # elif transform == "rotate": + # R = mesh.get_rotation_matrix_from_xyz(val) + # prim_mesh.rotate(R, center=(0, 0, 0)) if not mesh: mesh = prim_mesh diff --git a/python/mujoco/usd/test.py b/python/mujoco/usd/test.py new file mode 100644 index 00000000..0c189671 --- /dev/null +++ b/python/mujoco/usd/test.py @@ -0,0 +1,43 @@ +import open3d as o3d +import numpy as np + +import open3d as o3d +import numpy as np + +# Define parameters +radius = 1.0 # Radius of the hemisphere +resolution = 20 # Number of points per circle +theta_steps = 20 # Number of vertical steps (slices) +phi_steps = 20 # Number of horizontal steps + +# Generate points for the hemisphere +points = [] +for i in range(phi_steps + 1): + phi = np.pi / 2 * i / phi_steps + for j in range(theta_steps + 1): + theta = 2 * np.pi * j / theta_steps + x = radius * np.sin(phi) * np.cos(theta) + y = radius * np.sin(phi) * np.sin(theta) + z = radius * np.cos(phi) + points.append([x, y, z]) + +# Create Open3D point cloud +pcd = o3d.geometry.PointCloud() +pcd.points = o3d.utility.Vector3dVector(points) + +# Convert point cloud to mesh - alpha +# mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd, alpha=10) + +# Convert point cloud to mesh - ball pivoting +# radii = [0.005, 0.01, 0.02, 0.04] +# pcd.estimate_normals( +# search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=1, max_nn=30)) +# mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting( +# pcd, o3d.utility.DoubleVector(radii) +# ) + +# Visualize the mesh +o3d.visualization.draw_geometries([mesh]) + +# Visualize the point cloud +# o3d.visualization.draw_geometries([pcd]) \ No newline at end of file