diff --git a/examples/differential_ik_whole_body.py b/examples/differential_ik_whole_body.py index 218d3f5..66b235f 100644 --- a/examples/differential_ik_whole_body.py +++ b/examples/differential_ik_whole_body.py @@ -18,16 +18,16 @@ joint_limit_nullspace_component, collision_avoidance_nullspace_component, ) -from pyroboplan.models.ur5 import ( - load_ur5_on_base_models, - add_ur5_on_base_self_collisions, -) +from pyroboplan.models.ur10 import ( + load_ur10_on_base_models, + add_ur10_on_base_self_collisions, +) if __name__ == "__main__": # Create models and data - model, collision_model, visual_model = load_ur5_on_base_models() - add_ur5_on_base_self_collisions(model, collision_model) + model, collision_model, visual_model = load_ur10_on_base_models() + add_ur10_on_base_self_collisions(model, collision_model) data = model.createData() collision_data = collision_model.createData() diff --git a/src/pyroboplan/models/ur10.py b/src/pyroboplan/models/ur10.py new file mode 100644 index 0000000..08d8d7e --- /dev/null +++ b/src/pyroboplan/models/ur10.py @@ -0,0 +1,47 @@ +""" Utilities to load example Universal Robots UR10 robot models. """ + +import os +import pinocchio + +from .utils import get_example_models_folder + + +def load_ur10_on_base_models(): + """ + Gets the example UR10 on base 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, "ur_description") + urdf_filename = os.path.join(package_dir, "urdf", "ur10_on_base.urdf") + + return pinocchio.buildModelsFromUrdf(urdf_filename, package_dirs=models_folder) + + +def add_ur10_on_base_self_collisions(model, collision_model, srdf_filename=None): + """ + Adds link self-collisions to the UR10 collision model. + + This uses an SRDF file to remove any excluded collision pairs. + + Parameters + ---------- + model : `pinocchio.Model` + The Panda model. + collision_model : `pinocchio.Model` + The Panda collision geometry model. + srdf_filename : str, optional + Path to the SRDF file describing the excluded collision pairs. + If not specified, uses a default file included with the Panda model. + """ + if srdf_filename is None: + models_folder = get_example_models_folder() + package_dir = os.path.join(models_folder, "ur_description") + srdf_filename = os.path.join(package_dir, "srdf", "ur10_on_base.srdf") + + collision_model.addAllCollisionPairs() + pinocchio.removeCollisionPairs(model, collision_model, srdf_filename)