diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index f7ffed2..3f16748 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -40,7 +40,7 @@ jobs: pytest-coverage-path: ./test/results/pytest-coverage.txt junitxml-path: ./test/results/test_results.xml pytest-xml-coverage-path: ./test/results/test_results_coverage.xml - coverage-path-prefix: src/pyroboplan/ + coverage-path-prefix: src/ # This will break on PRs from forks if: ${{ !github.event.pull_request.head.repo.fork }} - name: Create coverage badge diff --git a/examples/example_differential_ik.py b/examples/example_differential_ik.py index a054dc6..d21714f 100644 --- a/examples/example_differential_ik.py +++ b/examples/example_differential_ik.py @@ -1,3 +1,4 @@ +import hppfcl import pinocchio from pinocchio.visualize import MeshcatVisualizer import numpy as np @@ -7,6 +8,52 @@ 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) + + if __name__ == "__main__": # Create models and data pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "models") @@ -18,14 +65,39 @@ ) data = model.createData() + # Comment out to exclude collision objects. + prepare_scene(visual_model, collision_model) + # Initialize visualizer viz = MeshcatVisualizer(model, collision_model, visual_model, data=data) viz.initViewer(open=True) 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, visualizer=viz, verbose=True) + ik = DifferentialIk( + model, data=data, collision_model=collision_model, visualizer=viz + ) target_frame = "panda_hand" options = DifferentialIkOptions() nullspace_components = [ @@ -42,5 +114,6 @@ init_state=init_state, options=options, nullspace_components=[], + verbose=True, ) print(f"Solution configuration:\n{q_sol}\n") diff --git a/src/pyroboplan/ik/differential_ik.py b/src/pyroboplan/ik/differential_ik.py index 64c5b43..d2f4bfe 100644 --- a/src/pyroboplan/ik/differential_ik.py +++ b/src/pyroboplan/ik/differential_ik.py @@ -4,7 +4,11 @@ import pinocchio import time -from ..core.utils import check_within_limits, get_random_state +from ..core.utils import ( + check_collisions_at_state, + check_within_limits, + get_random_state, +) from ..visualization.meshcat_utils import visualize_frame VIZ_INITIAL_RENDER_TIME = 0.5 @@ -57,7 +61,7 @@ class DifferentialIk: * https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf """ - def __init__(self, model, data=None, visualizer=None, verbose=False): + def __init__(self, model, collision_model=None, data=None, visualizer=None): """ Creates an instance of a DifferentialIk solver. @@ -65,19 +69,21 @@ def __init__(self, model, data=None, visualizer=None, verbose=False): ---------- model : `pinocchio.Model` The model to use for this solver. + collision_model : `pinocchio.Model`, optional + The model to use for collision checking. If None, no collision checking takes place. data : `pinocchio.Data`, optional The model data to use for this solver. If None, data is created automatically. visualizer : `pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer`, optional The visualizer to use for this solver. - verbose : bool, optional - If True, prints additional information to the console. """ self.model = model if not data: data = model.createData() self.data = data + + self.collision_model = collision_model + self.visualizer = visualizer - self.verbose = verbose def solve( self, @@ -86,6 +92,7 @@ def solve( init_state=None, options=DifferentialIkOptions(), nullspace_components=[], + verbose=False, ): """ Solves an IK query. @@ -103,6 +110,8 @@ def solve( nullspace_components : list[function], optional An optional list of nullspace components to use when solving. These components must take the form `lambda model, q: component(model, q, )`. + verbose : bool, optional + If True, prints additional information to the console. Returns ------- @@ -142,14 +151,29 @@ def solve( np.linalg.norm(error[:3]) < options.max_translation_error and np.linalg.norm(error[3:]) < options.max_rotation_error ): - # Wrap to the range -/+ pi and check joint limits + # Wrap to the range -/+ pi, and then check joint limits and collision. q_cur = (q_cur + np.pi) % (2 * np.pi) - np.pi if check_within_limits(self.model, q_cur): - if self.verbose: - print("Solved and within joint limits!") - solved = True + if self.collision_model is not None: + if check_collisions_at_state( + self.model, self.collision_model, q_cur + ): + if verbose: + print( + "Solved and within joint limits, but in collision." + ) + else: + solved = True + if verbose: + print( + "Solved, within joint limits, and collision-free!" + ) + else: + solved = True + if verbose: + print("Solved and within joint limits!") else: - if self.verbose: + if verbose: print("Solved, but outside joint limits.") break @@ -194,13 +218,13 @@ def solve( # Check results at the end of this try if solved: - if self.verbose: + if verbose: print(f"Solved in {n_tries+1} tries.") break else: q_cur = get_random_state(self.model) n_tries += 1 - if self.verbose: + if verbose: print(f"Retry {n_tries}") # Check final results diff --git a/test/ik/test_differential_ik.py b/test/ik/test_differential_ik.py index 195949f..3f8174f 100644 --- a/test/ik/test_differential_ik.py +++ b/test/ik/test_differential_ik.py @@ -15,6 +15,39 @@ def load_panda_models(): ) +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_ik_solve_trivial_ik(): model, _, _ = load_panda_models() data = model.createData() @@ -29,7 +62,7 @@ def test_ik_solve_trivial_ik(): target_tform = data.oMf[target_frame_id] # Solve - ik = DifferentialIk(model, data=None, visualizer=None, verbose=False) + ik = DifferentialIk(model, data=None, visualizer=None) options = DifferentialIkOptions() q_sol = ik.solve( target_frame, @@ -59,7 +92,7 @@ def test_ik_solve_ik(): target_tform.translation[2] = target_tform.translation[2] + offset # Solve it - ik = DifferentialIk(model, data=None, visualizer=None, verbose=False) + ik = DifferentialIk(model, data=None, visualizer=None) options = DifferentialIkOptions() q_sol = ik.solve( target_frame, @@ -91,7 +124,7 @@ def test_ik_solve_impossible_ik(): target_tform = pinocchio.SE3(R, T) # Solve - ik = DifferentialIk(model, data=None, visualizer=None, verbose=False) + ik = DifferentialIk(model, data=None, visualizer=None) options = DifferentialIkOptions() q_sol = ik.solve( target_frame, @@ -102,3 +135,36 @@ def test_ik_solve_impossible_ik(): ) assert q_sol is None, "Solution should be impossible!" + + +def test_ik_in_collision(): + model, collision_model, _ = load_panda_models() + activate_panda_self_collision_pairs(collision_model) + target_frame = "panda_hand" + + # Target is reachable, but in self-collision. + R = np.array( + [ + [0.206636, -0.430153, 0.878789], + [-0.978337, -0.10235, 0.179946], + [0.0125395, -0.896935, -0.441984], + ], + ) + T = np.array([0.155525, 0.0529695, 0.0259166]) + target_tform = pinocchio.SE3(R, T) + + # Solve + ik = DifferentialIk( + model, collision_model=collision_model, data=None, visualizer=None + ) + options = DifferentialIkOptions() + q_sol = ik.solve( + target_frame, + target_tform, + init_state=None, + options=options, + nullspace_components=[], + verbose=False, + ) + + assert q_sol is None, "Solution should be in self-collision!"