Merge pull request #333 from compas-dev/ik_analytical_addon

Ik analytical addon

Author: gonzalocasas

CHANGELOG.rst

@@ -18,6 +18,8 @@ Unreleased
 
 **Fixed**
 
+* Consider ``AttachedCollisionMesh`` in ``AnalyticalInverseKinematics``
+
 **Deprecated**
 
 **Removed**

docs/examples/06_backends_kinematics/files/02_iter_ik_pybullet.py

@@ -1,4 +1,5 @@
 from compas.geometry import Frame
+from compas.robots import LocalPackageMeshLoader
 import compas_fab
 from compas_fab.backends.kinematics import AnalyticalInverseKinematics
 from compas_fab.backends import PyBulletClient
@@ -13,7 +14,8 @@
 with PyBulletClient(connection_type='direct') as client:
 
     # Load UR5
-    robot = client.load_robot(urdf_filename)
+    loader = LocalPackageMeshLoader(compas_fab.get('universal_robot'), 'ur_description')
+    robot = client.load_robot(urdf_filename, [loader])
     client.load_semantics(robot, srdf_filename)
 
     ik = AnalyticalInverseKinematics(client)

docs/examples/06_backends_kinematics/files/03_iter_ik_analytic_pybullet.py

@@ -1,12 +1,14 @@
 import compas_fab
 from compas.geometry import Frame
+from compas.robots import LocalPackageMeshLoader
 from compas_fab.backends.kinematics.client import AnalyticalPyBulletClient
 
 urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur5.urdf')
 srdf_filename = compas_fab.get('universal_robot/ur5_moveit_config/config/ur5.srdf')
 
 with AnalyticalPyBulletClient(connection_type='direct') as client:
-    robot = client.load_robot(urdf_filename)
+    loader = LocalPackageMeshLoader(compas_fab.get('universal_robot'), 'ur_description')
+    robot = client.load_robot(urdf_filename, [loader])
     client.load_semantics(robot, srdf_filename)
 
     frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))

docs/examples/06_backends_kinematics/files/04_cartesian_path_analytic_pybullet.py

@@ -1,5 +1,6 @@
 import matplotlib.pyplot as plt
 from compas.geometry import Frame
+from compas.robots import LocalPackageMeshLoader
 import compas_fab
 from compas_fab.backends.kinematics.client import AnalyticalPyBulletClient
 
@@ -13,7 +14,8 @@
               Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296))]
 
 with AnalyticalPyBulletClient(connection_type='direct') as client:
-    robot = client.load_robot(urdf_filename)
+    loader = LocalPackageMeshLoader(compas_fab.get('universal_robot'), 'ur_description')
+    robot = client.load_robot(urdf_filename, [loader])
     client.load_semantics(robot, srdf_filename)
 
     options = {"solver": "ur5", "check_collision": True}

src/compas_fab/backends/__init__.py

@@ -39,6 +39,19 @@
     PyBulletPlanner
     PyBulletError
 
+
+Analytical Kinematics
+---------------------
+
+.. autosummary::
+    :toctree: generated/
+    :nosignatures:
+
+    AnalyticalPyBulletClient
+    AnalyticalInverseKinematics
+    AnalyticalPlanCartesianMotion
+
+
 Long-running tasks
 ------------------
 
@@ -116,6 +129,15 @@
     from .pybullet.planner import (
         PyBulletPlanner,
     )
+    from .kinematics.client import (
+        AnalyticalPyBulletClient,
+    )
+    from .kinematics import (
+        AnalyticalInverseKinematics,
+    )
+    from .kinematics.client import (
+        AnalyticalPlanCartesianMotion,
+    )
 
 __all__ = [
       'BackendError',
@@ -138,4 +160,8 @@
         'PyBulletClient',
         'PyBulletError',
         'PyBulletPlanner',
+        'AnalyticalPyBulletClient',
+        'AnalyticalInverseKinematics',
+        'AnalyticalPlanCartesianMotion',
+
     ]

src/compas_fab/backends/kinematics/__init__.py

@@ -1,2 +1,23 @@
+"""
+*******************************************************************************
+compas_fab.backends.kinematics
+*******************************************************************************
+
+.. module:: compas_fab.backends.kinematics
+
+.. autosummary::
+    :toctree: generated/
+
+    AnalyticalInverseKinematics
+    AnalyticalPlanCartesianMotion
+    InverseKinematicsError
+    CartesianMotionError
+
+"""
+
+from __future__ import absolute_import
 from .analytical_inverse_kinematics import AnalyticalInverseKinematics  # noqa: F401
 from .analytical_plan_cartesian_motion import AnalyticalPlanCartesianMotion  # noqa: F401
+from .exceptions import *                 # noqa: F401,F403
+
+__all__ = [name for name in dir() if not name.startswith('_')]

src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py

@@ -76,6 +76,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N
         ------
         ValueError : If the solver to solve the kinematics has not been passed.
         """
+
         options = options or {}
         solver = options.get('solver')
         if solver:
@@ -90,11 +91,21 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N
         frame_RCF = base_frame.to_local_coordinates(frame_WCF)
 
         # calculate inverse with 8 solutions
-        solutions = self.planner.inverse(frame_RCF)
+        try:
+            solutions = self.planner.inverse(frame_RCF)
+        except ValueError:
+            raise InverseKinematicsError()
         configurations = joint_angles_to_configurations(robot, solutions, group=group)
 
         # check collisions for all configurations (>> sets those to `None` that are not working)
         if options.get("check_collision", False) is True:
+            acms = options.get('attached_collision_meshes', [])
+            for acm in acms:
+                cached_robot_model = self.client.get_cached_robot(robot)
+                if not cached_robot_model.get_link_by_name(acm.collision_mesh.id):
+                    self.client.add_attached_collision_mesh(acm, options={'robot': robot})
+                    for touch_link in acm.touch_links:
+                        self.client.disabled_collisions.add((touch_link, acm.collision_mesh.id))
             for i, config in enumerate(configurations):
                 try:
                     self.client.check_collisions(robot, config)
@@ -105,7 +116,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N
         configurations = try_to_fit_configurations_between_bounds(robot, configurations, group=group)
 
         if not any(configurations):
-            raise InverseKinematicsError("No solutions found.")
+            raise InverseKinematicsError()
 
         for config in configurations:
             if config: