diff --git a/examples/example_collision_along_path.py b/examples/example_collision_along_path.py index f65888c..801970b 100644 --- a/examples/example_collision_along_path.py +++ b/examples/example_collision_along_path.py @@ -3,10 +3,10 @@ from pinocchio.visualize import MeshcatVisualizer import meshcat.geometry as mg import numpy as np -from os.path import dirname, join, abspath import time from pyroboplan.core.utils import extract_cartesian_poses +from pyroboplan.models.panda import load_models, add_self_collisions from pyroboplan.planning.utils import discretize_joint_space_path from pyroboplan.visualization.meshcat_utils import visualize_frames @@ -31,33 +31,12 @@ def prepare_collision_scene(collision_model): obstacle_1 = pinocchio.GeometryObject( "obstacle_1", 0, - hppfcl.Box(0.25, 0.25, 0.25), + hppfcl.Box(0.3, 0.3, 0.3), pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])), ) obstacle_1.meshColor = np.array([0.0, 1.0, 0.0, 0.2]) collision_model.addGeometryObject(obstacle_1) - -if __name__ == "__main__": - # Create models and data - pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "models") - package_dir = join(pinocchio_model_dir, "panda_description") - urdf_filename = join(package_dir, "urdf", "panda.urdf") - - model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( - urdf_filename, package_dirs=pinocchio_model_dir - ) - data = model.createData() - - prepare_collision_scene(collision_model) - - # Initialize visualizer - viz = MeshcatVisualizer(model, collision_model, visual_model, data=data) - viz.initViewer(open=True) - viz.loadViewerModel() - viz.displayCollisions(True) - viz.displayVisuals(False) - # Define the active collision pairs collision_names = [ cobj.name for cobj in collision_model.geometryObjects if "panda" in cobj.name @@ -71,7 +50,23 @@ def prepare_collision_scene(collision_model): collision_model.getGeometryId(obstacle_name), ) ) - collision_data = pinocchio.GeometryData(collision_model) + + +if __name__ == "__main__": + # Create models and data + model, collision_model, visual_model = load_models() + add_self_collisions(collision_model) + prepare_collision_scene(collision_model) + + data = model.createData() + collision_data = collision_model.createData() + + # Initialize visualizer + viz = MeshcatVisualizer(model, collision_model, visual_model, data=data) + viz.initViewer(open=True) + viz.loadViewerModel() + viz.displayCollisions(True) + viz.displayVisuals(False) # Define a joint space path q_start = pinocchio.randomConfiguration(model) diff --git a/examples/example_differential_ik.py b/examples/example_differential_ik.py index d21714f..122bb2c 100644 --- a/examples/example_differential_ik.py +++ b/examples/example_differential_ik.py @@ -1,72 +1,24 @@ -import hppfcl -import pinocchio from pinocchio.visualize import MeshcatVisualizer import numpy as np -from os.path import dirname, join, abspath from pyroboplan.core.utils import get_random_state, get_random_transform from pyroboplan.ik.differential_ik import DifferentialIk, DifferentialIkOptions from pyroboplan.ik.nullspace_components import joint_limit_nullspace_component - - -def prepare_scene(visual_model, collision_model): - """Helper function to create a collision scene for this example.""" - - # Add collision objects - obstacle_sphere_1 = pinocchio.GeometryObject( - "obstacle_sphere_1", - 0, - hppfcl.Sphere(0.2), - pinocchio.SE3(np.eye(3), np.array([0.0, 0.1, 1.1])), - ) - obstacle_sphere_1.meshColor = np.array([0.0, 1.0, 0.0, 0.5]) - visual_model.addGeometryObject(obstacle_sphere_1) - collision_model.addGeometryObject(obstacle_sphere_1) - - obstacle_sphere_2 = pinocchio.GeometryObject( - "obstacle_sphere_2", - 0, - hppfcl.Sphere(0.25), - pinocchio.SE3(np.eye(3), np.array([0.5, 0.5, 0.5])), - ) - obstacle_sphere_2.meshColor = np.array([1.0, 1.0, 0.0, 0.5]) - visual_model.addGeometryObject(obstacle_sphere_2) - collision_model.addGeometryObject(obstacle_sphere_2) - - obstacle_box_1 = pinocchio.GeometryObject( - "obstacle_box_1", - 0, - hppfcl.Box(0.25, 0.25, 0.25), - pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])), - ) - obstacle_box_1.meshColor = np.array([1.0, 0.0, 0.0, 0.5]) - visual_model.addGeometryObject(obstacle_box_1) - collision_model.addGeometryObject(obstacle_box_1) - - obstacle_box_2 = pinocchio.GeometryObject( - "obstacle_box_2", - 0, - hppfcl.Box(0.33, 0.33, 0.33), - pinocchio.SE3(np.eye(3), np.array([-0.5, -0.5, 0.75])), - ) - obstacle_box_2.meshColor = np.array([0.0, 0.0, 1.0, 0.5]) - visual_model.addGeometryObject(obstacle_box_2) - collision_model.addGeometryObject(obstacle_box_2) +from pyroboplan.models.panda import ( + load_models, + add_self_collisions, + add_object_collisions, +) if __name__ == "__main__": # Create models and data - pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "models") - package_dir = join(pinocchio_model_dir, "panda_description") - urdf_filename = join(package_dir, "urdf", "panda.urdf") + model, collision_model, visual_model = load_models() + add_self_collisions(collision_model) + add_object_collisions(collision_model, visual_model) - model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( - urdf_filename, package_dirs=pinocchio_model_dir - ) data = model.createData() - - # Comment out to exclude collision objects. - prepare_scene(visual_model, collision_model) + collision_data = collision_model.createData() # Initialize visualizer viz = MeshcatVisualizer(model, collision_model, visual_model, data=data) @@ -74,26 +26,6 @@ def prepare_scene(visual_model, collision_model): viz.loadViewerModel() np.set_printoptions(precision=3) - # Define the active collision pairs - collision_names = [ - cobj.name for cobj in collision_model.geometryObjects if "panda" in cobj.name - ] - obstacle_names = [ - "obstacle_box_1", - "obstacle_box_2", - "obstacle_sphere_1", - "obstacle_sphere_2", - ] - for obstacle_name in obstacle_names: - for collision_name in collision_names: - collision_model.addCollisionPair( - pinocchio.CollisionPair( - collision_model.getGeometryId(collision_name), - collision_model.getGeometryId(obstacle_name), - ) - ) - collision_data = pinocchio.GeometryData(collision_model) - # Set up the IK solver ik = DifferentialIk( model, data=data, collision_model=collision_model, visualizer=viz diff --git a/examples/example_rrt.py b/examples/example_rrt.py index 9821aca..0f1be33 100644 --- a/examples/example_rrt.py +++ b/examples/example_rrt.py @@ -1,141 +1,30 @@ -import hppfcl -import pinocchio from pinocchio.visualize import MeshcatVisualizer -import numpy as np -from os.path import dirname, join, abspath import time from pyroboplan.core.utils import get_random_collision_free_state +from pyroboplan.models.panda import ( + load_models, + add_self_collisions, + add_object_collisions, +) from pyroboplan.planning.rrt import RRTPlanner, RRTPlannerOptions from pyroboplan.planning.utils import discretize_joint_space_path -def prepare_scene(visual_model, collision_model): - """Helper function to create a collision scene for this example.""" - - # Add self collisions - self_collision_pair_names = [ - ("panda_link0_0", "panda_link2_0"), - ("panda_link0_0", "panda_link3_0"), - ("panda_link0_0", "panda_link4_0"), - ("panda_link0_0", "panda_link5_0"), - ("panda_link0_0", "panda_link6_0"), - ("panda_link0_0", "panda_link7_0"), - ("panda_link1_0", "panda_link3_0"), - ("panda_link1_0", "panda_link4_0"), - ("panda_link1_0", "panda_link5_0"), - ("panda_link1_0", "panda_link6_0"), - ("panda_link1_0", "panda_link7_0"), - ("panda_link2_0", "panda_link4_0"), - ("panda_link2_0", "panda_link5_0"), - ("panda_link2_0", "panda_link6_0"), - ("panda_link2_0", "panda_link7_0"), - ("panda_link3_0", "panda_link5_0"), - ("panda_link3_0", "panda_link6_0"), - ("panda_link3_0", "panda_link7_0"), - ("panda_link4_0", "panda_link6_0"), - ("panda_link4_0", "panda_link7_0"), - ("panda_link5_0", "panda_link7_0"), - ] - for pair in self_collision_pair_names: - collision_model.addCollisionPair( - pinocchio.CollisionPair( - collision_model.getGeometryId(pair[0]), - collision_model.getGeometryId(pair[1]), - ) - ) - - # Add collision objects - ground_plane = pinocchio.GeometryObject( - "ground_plane", - 0, - hppfcl.Box(2.0, 2.0, 0.01), - pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, -0.006])), - ) - ground_plane.meshColor = np.array([0.5, 0.5, 0.5, 0.5]) - visual_model.addGeometryObject(ground_plane) - collision_model.addGeometryObject(ground_plane) - - obstacle_sphere_1 = pinocchio.GeometryObject( - "obstacle_sphere_1", - 0, - hppfcl.Sphere(0.2), - pinocchio.SE3(np.eye(3), np.array([0.0, 0.1, 1.1])), - ) - obstacle_sphere_1.meshColor = np.array([0.0, 1.0, 0.0, 0.5]) - visual_model.addGeometryObject(obstacle_sphere_1) - collision_model.addGeometryObject(obstacle_sphere_1) - - obstacle_sphere_2 = pinocchio.GeometryObject( - "obstacle_sphere_2", - 0, - hppfcl.Sphere(0.25), - pinocchio.SE3(np.eye(3), np.array([0.5, 0.5, 0.5])), - ) - obstacle_sphere_2.meshColor = np.array([1.0, 1.0, 0.0, 0.5]) - visual_model.addGeometryObject(obstacle_sphere_2) - collision_model.addGeometryObject(obstacle_sphere_2) - - obstacle_box_1 = pinocchio.GeometryObject( - "obstacle_box_1", - 0, - hppfcl.Box(0.25, 0.25, 0.25), - pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])), - ) - obstacle_box_1.meshColor = np.array([1.0, 0.0, 0.0, 0.5]) - visual_model.addGeometryObject(obstacle_box_1) - collision_model.addGeometryObject(obstacle_box_1) - - obstacle_box_2 = pinocchio.GeometryObject( - "obstacle_box_2", - 0, - hppfcl.Box(0.33, 0.33, 0.33), - pinocchio.SE3(np.eye(3), np.array([-0.5, -0.5, 0.75])), - ) - obstacle_box_2.meshColor = np.array([0.0, 0.0, 1.0, 0.5]) - visual_model.addGeometryObject(obstacle_box_2) - collision_model.addGeometryObject(obstacle_box_2) - - if __name__ == "__main__": # Create models and data - pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "models") - package_dir = join(pinocchio_model_dir, "panda_description") - urdf_filename = join(package_dir, "urdf", "panda.urdf") + model, collision_model, visual_model = load_models() + add_self_collisions(collision_model) + add_object_collisions(collision_model, visual_model) - model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( - urdf_filename, package_dirs=pinocchio_model_dir - ) data = model.createData() - - prepare_scene(visual_model, collision_model) + collision_data = collision_model.createData() # Initialize visualizer viz = MeshcatVisualizer(model, collision_model, visual_model, data=data) viz.initViewer(open=True) viz.loadViewerModel() - # Define the active collision pairs - collision_names = [ - cobj.name for cobj in collision_model.geometryObjects if "panda" in cobj.name - ] - obstacle_names = [ - "ground_plane", - "obstacle_box_1", - "obstacle_box_2", - "obstacle_sphere_1", - "obstacle_sphere_2", - ] - for obstacle_name in obstacle_names: - for collision_name in collision_names: - collision_model.addCollisionPair( - pinocchio.CollisionPair( - collision_model.getGeometryId(collision_name), - collision_model.getGeometryId(obstacle_name), - ) - ) - collision_data = pinocchio.GeometryData(collision_model) - # Define the start and end configurations q_start = get_random_collision_free_state(model, collision_model) q_end = get_random_collision_free_state(model, collision_model) diff --git a/src/pyroboplan/models/__init__.py b/src/pyroboplan/models/__init__.py new file mode 100644 index 0000000..4c80dc4 --- /dev/null +++ b/src/pyroboplan/models/__init__.py @@ -0,0 +1 @@ +""" Contains example robot models. """ diff --git a/src/pyroboplan/models/panda.py b/src/pyroboplan/models/panda.py new file mode 100644 index 0000000..d0b4252 --- /dev/null +++ b/src/pyroboplan/models/panda.py @@ -0,0 +1,148 @@ +""" Utilities to load example Franka Emika Panda model. """ + +import hppfcl +import numpy as np +import os +import pinocchio + +from .utils import get_example_models_folder + + +def load_models(): + """ + Gets the example Panda models. + + Returns + ------- + tuple[`pinocchio.Model`] + A 3-tuple containing the model, collision geometry model, and visual geometry model. + """ + models_folder = get_example_models_folder() + package_dir = os.path.join(models_folder, "panda_description") + urdf_filename = os.path.join(package_dir, "urdf", "panda.urdf") + + return pinocchio.buildModelsFromUrdf(urdf_filename, package_dirs=models_folder) + + +def add_self_collisions(collision_model): + """ + Adds link self-collisions to the Panda collision model. + + Parameters + ---------- + collision_model : `pinocchio.Model` + The Panda collision geometry model. + """ + self_collision_pair_names = [ + ("panda_link0_0", "panda_link2_0"), + ("panda_link0_0", "panda_link3_0"), + ("panda_link0_0", "panda_link4_0"), + ("panda_link0_0", "panda_link5_0"), + ("panda_link0_0", "panda_link6_0"), + ("panda_link0_0", "panda_link7_0"), + ("panda_link1_0", "panda_link3_0"), + ("panda_link1_0", "panda_link4_0"), + ("panda_link1_0", "panda_link5_0"), + ("panda_link1_0", "panda_link6_0"), + ("panda_link1_0", "panda_link7_0"), + ("panda_link2_0", "panda_link4_0"), + ("panda_link2_0", "panda_link5_0"), + ("panda_link2_0", "panda_link6_0"), + ("panda_link2_0", "panda_link7_0"), + ("panda_link3_0", "panda_link5_0"), + ("panda_link3_0", "panda_link6_0"), + ("panda_link3_0", "panda_link7_0"), + ("panda_link4_0", "panda_link6_0"), + ("panda_link4_0", "panda_link7_0"), + ("panda_link5_0", "panda_link7_0"), + ] + for pair in self_collision_pair_names: + collision_model.addCollisionPair( + pinocchio.CollisionPair( + collision_model.getGeometryId(pair[0]), + collision_model.getGeometryId(pair[1]), + ) + ) + + +def add_object_collisions(collision_model, visual_model): + """ + Adds link self-collisions to the Panda collision model. + + Parameters + ---------- + collision_model : `pinocchio.Model` + The Panda collision geometry model. + visual_model : `pinocchio.Model` + The Panda visual geometry model. + """ + # Add the collision objects + ground_plane = pinocchio.GeometryObject( + "ground_plane", + 0, + hppfcl.Box(2.0, 2.0, 0.01), + pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, -0.006])), + ) + ground_plane.meshColor = np.array([0.5, 0.5, 0.5, 0.5]) + visual_model.addGeometryObject(ground_plane) + collision_model.addGeometryObject(ground_plane) + + obstacle_sphere_1 = pinocchio.GeometryObject( + "obstacle_sphere_1", + 0, + hppfcl.Sphere(0.2), + pinocchio.SE3(np.eye(3), np.array([0.0, 0.1, 1.1])), + ) + obstacle_sphere_1.meshColor = np.array([0.0, 1.0, 0.0, 0.5]) + visual_model.addGeometryObject(obstacle_sphere_1) + collision_model.addGeometryObject(obstacle_sphere_1) + + obstacle_sphere_2 = pinocchio.GeometryObject( + "obstacle_sphere_2", + 0, + hppfcl.Sphere(0.25), + pinocchio.SE3(np.eye(3), np.array([0.5, 0.5, 0.5])), + ) + obstacle_sphere_2.meshColor = np.array([1.0, 1.0, 0.0, 0.5]) + visual_model.addGeometryObject(obstacle_sphere_2) + collision_model.addGeometryObject(obstacle_sphere_2) + + obstacle_box_1 = pinocchio.GeometryObject( + "obstacle_box_1", + 0, + hppfcl.Box(0.25, 0.25, 0.25), + pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])), + ) + obstacle_box_1.meshColor = np.array([1.0, 0.0, 0.0, 0.5]) + visual_model.addGeometryObject(obstacle_box_1) + collision_model.addGeometryObject(obstacle_box_1) + + obstacle_box_2 = pinocchio.GeometryObject( + "obstacle_box_2", + 0, + hppfcl.Box(0.33, 0.33, 0.33), + pinocchio.SE3(np.eye(3), np.array([-0.5, -0.5, 0.75])), + ) + obstacle_box_2.meshColor = np.array([0.0, 0.0, 1.0, 0.5]) + visual_model.addGeometryObject(obstacle_box_2) + collision_model.addGeometryObject(obstacle_box_2) + + # Define the active collision pairs between the robot and obstacle links. + collision_names = [ + cobj.name for cobj in collision_model.geometryObjects if "panda" in cobj.name + ] + obstacle_names = [ + "ground_plane", + "obstacle_box_1", + "obstacle_box_2", + "obstacle_sphere_1", + "obstacle_sphere_2", + ] + for obstacle_name in obstacle_names: + for collision_name in collision_names: + collision_model.addCollisionPair( + pinocchio.CollisionPair( + collision_model.getGeometryId(collision_name), + collision_model.getGeometryId(obstacle_name), + ) + ) diff --git a/models/panda_description/LICENSE b/src/pyroboplan/models/panda_description/LICENSE similarity index 100% rename from models/panda_description/LICENSE rename to src/pyroboplan/models/panda_description/LICENSE diff --git a/models/panda_description/README.md b/src/pyroboplan/models/panda_description/README.md similarity index 100% rename from models/panda_description/README.md rename to src/pyroboplan/models/panda_description/README.md diff --git a/models/panda_description/meshes/collision/finger.stl b/src/pyroboplan/models/panda_description/meshes/collision/finger.stl similarity index 100% rename from models/panda_description/meshes/collision/finger.stl rename to src/pyroboplan/models/panda_description/meshes/collision/finger.stl diff --git a/models/panda_description/meshes/collision/hand.stl b/src/pyroboplan/models/panda_description/meshes/collision/hand.stl similarity index 100% rename from models/panda_description/meshes/collision/hand.stl rename to src/pyroboplan/models/panda_description/meshes/collision/hand.stl diff --git a/models/panda_description/meshes/collision/link0.stl b/src/pyroboplan/models/panda_description/meshes/collision/link0.stl similarity index 100% rename from models/panda_description/meshes/collision/link0.stl rename to src/pyroboplan/models/panda_description/meshes/collision/link0.stl diff --git a/models/panda_description/meshes/collision/link1.stl b/src/pyroboplan/models/panda_description/meshes/collision/link1.stl similarity index 100% rename from models/panda_description/meshes/collision/link1.stl rename to src/pyroboplan/models/panda_description/meshes/collision/link1.stl diff --git a/models/panda_description/meshes/collision/link2.stl b/src/pyroboplan/models/panda_description/meshes/collision/link2.stl similarity index 100% rename from models/panda_description/meshes/collision/link2.stl rename to src/pyroboplan/models/panda_description/meshes/collision/link2.stl diff --git a/models/panda_description/meshes/collision/link3.stl b/src/pyroboplan/models/panda_description/meshes/collision/link3.stl similarity index 100% rename from models/panda_description/meshes/collision/link3.stl rename to src/pyroboplan/models/panda_description/meshes/collision/link3.stl diff --git a/models/panda_description/meshes/collision/link4.stl b/src/pyroboplan/models/panda_description/meshes/collision/link4.stl similarity index 100% rename from models/panda_description/meshes/collision/link4.stl rename to src/pyroboplan/models/panda_description/meshes/collision/link4.stl diff --git a/models/panda_description/meshes/collision/link5.stl b/src/pyroboplan/models/panda_description/meshes/collision/link5.stl similarity index 100% rename from models/panda_description/meshes/collision/link5.stl rename to src/pyroboplan/models/panda_description/meshes/collision/link5.stl diff --git a/models/panda_description/meshes/collision/link6.stl b/src/pyroboplan/models/panda_description/meshes/collision/link6.stl similarity index 100% rename from models/panda_description/meshes/collision/link6.stl rename to src/pyroboplan/models/panda_description/meshes/collision/link6.stl diff --git a/models/panda_description/meshes/collision/link7.stl b/src/pyroboplan/models/panda_description/meshes/collision/link7.stl similarity index 100% rename from models/panda_description/meshes/collision/link7.stl rename to src/pyroboplan/models/panda_description/meshes/collision/link7.stl diff --git a/models/panda_description/meshes/visual/finger.dae b/src/pyroboplan/models/panda_description/meshes/visual/finger.dae similarity index 100% rename from models/panda_description/meshes/visual/finger.dae rename to src/pyroboplan/models/panda_description/meshes/visual/finger.dae diff --git a/models/panda_description/meshes/visual/hand.dae b/src/pyroboplan/models/panda_description/meshes/visual/hand.dae similarity index 100% rename from models/panda_description/meshes/visual/hand.dae rename to src/pyroboplan/models/panda_description/meshes/visual/hand.dae diff --git a/models/panda_description/meshes/visual/link0.dae b/src/pyroboplan/models/panda_description/meshes/visual/link0.dae similarity index 100% rename from models/panda_description/meshes/visual/link0.dae rename to src/pyroboplan/models/panda_description/meshes/visual/link0.dae diff --git a/models/panda_description/meshes/visual/link1.dae b/src/pyroboplan/models/panda_description/meshes/visual/link1.dae similarity index 100% rename from models/panda_description/meshes/visual/link1.dae rename to src/pyroboplan/models/panda_description/meshes/visual/link1.dae diff --git a/models/panda_description/meshes/visual/link2.dae b/src/pyroboplan/models/panda_description/meshes/visual/link2.dae similarity index 100% rename from models/panda_description/meshes/visual/link2.dae rename to src/pyroboplan/models/panda_description/meshes/visual/link2.dae diff --git a/models/panda_description/meshes/visual/link3.dae b/src/pyroboplan/models/panda_description/meshes/visual/link3.dae similarity index 100% rename from models/panda_description/meshes/visual/link3.dae rename to src/pyroboplan/models/panda_description/meshes/visual/link3.dae diff --git a/models/panda_description/meshes/visual/link4.dae b/src/pyroboplan/models/panda_description/meshes/visual/link4.dae similarity index 100% rename from models/panda_description/meshes/visual/link4.dae rename to src/pyroboplan/models/panda_description/meshes/visual/link4.dae diff --git a/models/panda_description/meshes/visual/link5.dae b/src/pyroboplan/models/panda_description/meshes/visual/link5.dae similarity index 100% rename from models/panda_description/meshes/visual/link5.dae rename to src/pyroboplan/models/panda_description/meshes/visual/link5.dae diff --git a/models/panda_description/meshes/visual/link6.dae b/src/pyroboplan/models/panda_description/meshes/visual/link6.dae similarity index 100% rename from models/panda_description/meshes/visual/link6.dae rename to src/pyroboplan/models/panda_description/meshes/visual/link6.dae diff --git a/models/panda_description/meshes/visual/link7.dae b/src/pyroboplan/models/panda_description/meshes/visual/link7.dae similarity index 100% rename from models/panda_description/meshes/visual/link7.dae rename to src/pyroboplan/models/panda_description/meshes/visual/link7.dae diff --git a/models/panda_description/urdf/panda.urdf b/src/pyroboplan/models/panda_description/urdf/panda.urdf similarity index 100% rename from models/panda_description/urdf/panda.urdf rename to src/pyroboplan/models/panda_description/urdf/panda.urdf diff --git a/src/pyroboplan/models/utils.py b/src/pyroboplan/models/utils.py new file mode 100644 index 0000000..2bb9c8b --- /dev/null +++ b/src/pyroboplan/models/utils.py @@ -0,0 +1,16 @@ +""" Utilities for model loading. """ + +import importlib.resources +import pyroboplan + + +def get_example_models_folder(): + """ + Returns the full path the example models folder. + + Returns + ------- + str + The path to the `pyroboplan` example models folder. + """ + return importlib.resources.path(pyroboplan, "models").as_posix() diff --git a/test/core/test_utils.py b/test/core/test_utils.py index 38de5dd..ec61503 100644 --- a/test/core/test_utils.py +++ b/test/core/test_utils.py @@ -1,4 +1,3 @@ -from os.path import dirname, join, abspath import numpy as np import pinocchio import pytest @@ -12,54 +11,13 @@ get_random_state, get_random_transform, ) +from pyroboplan.models.panda import load_models, add_self_collisions # Use a fixed seed for random number generation in tests. np.random.seed(1234) -def load_panda_models(): - pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "..", "models") - package_dir = join(pinocchio_model_dir, "panda_description") - urdf_filename = join(package_dir, "urdf", "panda.urdf") - return pinocchio.buildModelsFromUrdf( - urdf_filename, package_dirs=pinocchio_model_dir - ) - - -def activate_panda_self_collision_pairs(collision_model): - collision_pair_names = [ - ("panda_link0_0", "panda_link2_0"), - ("panda_link0_0", "panda_link3_0"), - ("panda_link0_0", "panda_link4_0"), - ("panda_link0_0", "panda_link5_0"), - ("panda_link0_0", "panda_link6_0"), - ("panda_link0_0", "panda_link7_0"), - ("panda_link1_0", "panda_link3_0"), - ("panda_link1_0", "panda_link4_0"), - ("panda_link1_0", "panda_link5_0"), - ("panda_link1_0", "panda_link6_0"), - ("panda_link1_0", "panda_link7_0"), - ("panda_link2_0", "panda_link4_0"), - ("panda_link2_0", "panda_link5_0"), - ("panda_link2_0", "panda_link6_0"), - ("panda_link2_0", "panda_link7_0"), - ("panda_link3_0", "panda_link5_0"), - ("panda_link3_0", "panda_link6_0"), - ("panda_link3_0", "panda_link7_0"), - ("panda_link4_0", "panda_link6_0"), - ("panda_link4_0", "panda_link7_0"), - ("panda_link5_0", "panda_link7_0"), - ] - for pair in collision_pair_names: - collision_model.addCollisionPair( - pinocchio.CollisionPair( - collision_model.getGeometryId(pair[0]), - collision_model.getGeometryId(pair[1]), - ) - ) - - def test_configuration_distance(): q_start = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) q_end = np.array([0.3, 0.0, -0.4, 0.0, 0.0]) @@ -67,7 +25,7 @@ def test_configuration_distance(): def test_get_random_state(): - model, _, _ = load_panda_models() + model, _, _ = load_models() for _ in range(10): state = get_random_state(model) assert np.all(state >= model.lowerPositionLimit) @@ -75,7 +33,7 @@ def test_get_random_state(): def test_get_random_state_with_scalar_padding(): - model, _, _ = load_panda_models() + model, _, _ = load_models() padding = 0.01 for _ in range(10): state = get_random_state(model, padding=padding) @@ -84,7 +42,7 @@ def test_get_random_state_with_scalar_padding(): def test_get_random_state_with_vector_padding(): - model, _, _ = load_panda_models() + model, _, _ = load_models() padding = [0.1, 0.2, 0.1, 0.2, 0.1, 0.2, 0.1, 0.01, 0.01] for _ in range(10): state = get_random_state(model, padding=padding) @@ -93,13 +51,13 @@ def test_get_random_state_with_vector_padding(): def test_get_random_transform(): - model, _, _ = load_panda_models() + model, _, _ = load_models() tform = get_random_transform(model, "panda_hand") assert isinstance(tform, pinocchio.SE3) def test_within_limits(): - model, _, _ = load_panda_models() + model, _, _ = load_models() assert check_within_limits(model, get_random_state(model)) assert not check_within_limits(model, model.upperPositionLimit + 0.2) @@ -107,7 +65,7 @@ def test_within_limits(): def test_extract_cartesian_poses(): - model, _, _ = load_panda_models() + model, _, _ = load_models() q_vec = [get_random_state(model) for _ in range(5)] tforms = extract_cartesian_poses(model, "panda_hand", q_vec) @@ -118,8 +76,8 @@ def test_extract_cartesian_poses(): def test_check_collisions_at_state(): - model, collision_model, _ = load_panda_models() - activate_panda_self_collision_pairs(collision_model) + model, collision_model, _ = load_models() + add_self_collisions(collision_model) collision_state = np.array( [ @@ -154,8 +112,8 @@ def test_check_collisions_at_state(): def test_check_collisions_along_path(): - model, collision_model, _ = load_panda_models() - activate_panda_self_collision_pairs(collision_model) + model, collision_model, _ = load_models() + add_self_collisions(collision_model) collision_state = np.array( [ diff --git a/test/ik/test_differential_ik.py b/test/ik/test_differential_ik.py index 3f8174f..edc4f85 100644 --- a/test/ik/test_differential_ik.py +++ b/test/ik/test_differential_ik.py @@ -1,55 +1,13 @@ import copy -from os.path import dirname, join, abspath import numpy as np import pinocchio from pyroboplan.ik.differential_ik import DifferentialIk, DifferentialIkOptions - - -def load_panda_models(): - pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "..", "models") - package_dir = join(pinocchio_model_dir, "panda_description") - urdf_filename = join(package_dir, "urdf", "panda.urdf") - return pinocchio.buildModelsFromUrdf( - urdf_filename, package_dirs=pinocchio_model_dir - ) - - -def activate_panda_self_collision_pairs(collision_model): - collision_pair_names = [ - ("panda_link0_0", "panda_link2_0"), - ("panda_link0_0", "panda_link3_0"), - ("panda_link0_0", "panda_link4_0"), - ("panda_link0_0", "panda_link5_0"), - ("panda_link0_0", "panda_link6_0"), - ("panda_link0_0", "panda_link7_0"), - ("panda_link1_0", "panda_link3_0"), - ("panda_link1_0", "panda_link4_0"), - ("panda_link1_0", "panda_link5_0"), - ("panda_link1_0", "panda_link6_0"), - ("panda_link1_0", "panda_link7_0"), - ("panda_link2_0", "panda_link4_0"), - ("panda_link2_0", "panda_link5_0"), - ("panda_link2_0", "panda_link6_0"), - ("panda_link2_0", "panda_link7_0"), - ("panda_link3_0", "panda_link5_0"), - ("panda_link3_0", "panda_link6_0"), - ("panda_link3_0", "panda_link7_0"), - ("panda_link4_0", "panda_link6_0"), - ("panda_link4_0", "panda_link7_0"), - ("panda_link5_0", "panda_link7_0"), - ] - for pair in collision_pair_names: - collision_model.addCollisionPair( - pinocchio.CollisionPair( - collision_model.getGeometryId(pair[0]), - collision_model.getGeometryId(pair[1]), - ) - ) +from pyroboplan.models.panda import load_models, add_self_collisions def test_ik_solve_trivial_ik(): - model, _, _ = load_panda_models() + model, _, _ = load_models() data = model.createData() target_frame = "panda_hand" @@ -77,7 +35,7 @@ def test_ik_solve_trivial_ik(): def test_ik_solve_ik(): - model, _, _ = load_panda_models() + model, _, _ = load_models() data = model.createData() target_frame = "panda_hand" @@ -115,7 +73,7 @@ def test_ik_solve_ik(): def test_ik_solve_impossible_ik(): - model, _, _ = load_panda_models() + model, _, _ = load_models() target_frame = "panda_hand" # Target is unreachable by the panda @@ -138,8 +96,8 @@ def test_ik_solve_impossible_ik(): def test_ik_in_collision(): - model, collision_model, _ = load_panda_models() - activate_panda_self_collision_pairs(collision_model) + model, collision_model, _ = load_models() + add_self_collisions(collision_model) target_frame = "panda_hand" # Target is reachable, but in self-collision.