Tests for PyBulletClient and PyBulletPlanner

CHANGELOG.md

@@ -9,6 +9,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+* Added unit test for `PyBulletClient` and `PyBulletPlanner` backend features, including Ik and FK agreement tests.
+* Added `PyBulletClient.load_existing_robot` to load from an existing Robot, such as those in RobotLibrary.
 * Added `compas_fab.robots.Waypoints` class to represent a sequence of targets. It has two child classes: `FrameWaypoints` and `PointAxisWaypoints`.
 * Added `compas_fab.robots.Target` class to represent a motion planning target.
 * Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget`
@@ -17,6 +19,14 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+* Fixed error in `PyBulletForwardKinematics.forward_kinematics` where function would crash if `options` was not passed.
+* Fixed error in `PyBulletInverseKinematics._accurate_inverse_kinematics` where threshold was not squared for comparison.
+* Renamed `PybulletClient.get_cached_robot` to `PybulletClient.get_cached_robot_model` to avoid confusion between the `RobotModel` and `Robot` class.
+* Renamed `PybulletClient.ensure_cached_robot` to `PybulletClient.ensure_cached_robot_model`.
+* Renamed `PybulletClient.ensure_cached_robot_geometry` to `PybulletClient.ensure_cached_robot_model_geometry`.
+* Renamed `PybulletClient.cache_robot` to `PybulletClient.cache_robot_model`.
+* Backend planners now use multi-inherence instead of `__call__` to include the backend functions. This allows for better generated documentation.
+* `Robot.plan_cartesian_motion()` now accepts `Waypoints` as target. Implementation for `FrameWaypoints` is supported with same functionality as before. Simply wrap `Frame` objects using `FrameWaypoints(frames)`.
 * Changed `BoundingVolume`, `Constraint`, `JointConstraint`, `OrientationConstraint`, `PositionConstraint` to inherit from `compas.data.Data` class.
 * Change the signature of `plan_motion()` to use `target` (`Target` class) instead of `goal_constraints`. Only one target is accepted. Users who wish to compose their own constraint sets can still use `ConstraintSetTarget`.
 * Moved `Robot.orientation_constraint_from_frame()` to `OrientationConstraint.from_frame()`, as constraints are no longer intended for users to use directly.

docs/developer/backends.rst

@@ -82,3 +82,15 @@ Backend interfaces
 ==================
 
 .. automodule:: compas_fab.backends.interfaces
+
+Implemented backend features
+============================
+
+The following backend features are implemented for the ROS backend:
+
+.. automodule:: compas_fab.backends.ros.backend_features
+
+The following backend features are implemented for the PyBullet backend:
+
+.. automodule:: compas_fab.backends.pybullet.backend_features
+

docs/examples/02_description_models/03_targets.rst

@@ -1,16 +1,15 @@
 .. _targets:
 
 *******************************************************************************
-Targets
+Targets and Waypoints
 *******************************************************************************
 
 -----------------------
-Single Targets (Static)
+Targets (Single Goal)
 -----------------------
 
 Target classes are used to describe the goal condition (i.e. end condition) of a robot
-for motion planning. They can be used for both Free Motion Planning (FMP) and Cartesian
-Motion Planning (CMP).
+for motion planning. They can be used for Free-space Motion Planning with :meth:`compas_fab.robots.Robot.plan_motion`.
 
 The :class:`compas_fab.robots.FrameTarget` is the most common target for motion planning.
 It defines the complete pose of the end-effector (or the robot flange, if no tool is attached).
@@ -38,3 +37,29 @@ The :class:`compas_fab.robots.ConstraintSetTarget` class is used to specify a li
 constraints as a planning target. This is intended for advanced users who want to create custom
 combination of constraints. See :class:`compas_fab.robots.Constraint` for available
 constraints. At the moment, only the ROS MoveIt planning backend supports this target type.
+
+.. _waypoints:
+
+------------------------------------------
+Waypoints (Multiple Points / Segments)
+------------------------------------------
+
+Waypoints classes are used to describe a sequence of
+waypoints that the robot should pass through in a planned motion. They are similar to Targets classes
+but contain a list of targets instead of a single target, which is useful for tasks such as
+drawing, welding or 3D printing.
+They can be used for Cartesian Motion Planning with :meth:`compas_fab.robots.Robot.plan_cartesian_motion`.
+
+The :class:`compas_fab.robots.FrameWaypoints` is the most common waypoint for Cartesian motion planning.
+It defines a list of complete pose for the end-effector (or the robot flange, if no tool is attached).
+It is created by a list of :class:`compas.geometry.Frame` objects or alternatively from a list of
+:class:`compas.geometry.Transformation` objects.
+
+The :class:`compas_fab.robots.PointAxisWaypoints` class is used for specifying a list of waypoints based on
+the Point-Axis concept used in the :class:`compas_fab.robots.PointAxisTarget`. Compared to
+:class:`~compas_fab.robots.FrameWaypoints`, this class allows for specifying targets where the rotation
+around the Z-axis is not fixed. This is useful for example when the robot is using a cylindrical tool
+to perform a task, for example 3D printing, welding or drilling. The freely rotating axis is defined relative
+to the Z-axis of the tool coordinate frame (TCF). Note that the orientation of the tool
+at the end of the motion is not determined until after the motion is planned.
+

docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py

@@ -1,25 +1,25 @@
 from compas.geometry import Frame
 
 from compas_fab.backends import RosClient
+from compas_fab.robots import FrameWaypoints
 
 with RosClient() as client:
     robot = client.load_robot()
-    assert robot.name == 'ur5_robot'
+    assert robot.name == "ur5_robot"
 
     frames = []
     frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]))
     frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0]))
+    waypoints = FrameWaypoints(frames)
 
     start_configuration = robot.zero_configuration()
     start_configuration.joint_values = (-0.042, 0.033, -2.174, 5.282, -1.528, 0.000)
     options = {
-        'max_step': 0.01,
-        'avoid_collisions': True,
+        "max_step": 0.01,
+        "avoid_collisions": True,
     }
 
-    trajectory = robot.plan_cartesian_motion(frames,
-                                             start_configuration,
-                                             options=options)
+    trajectory = robot.plan_cartesian_motion(waypoints, start_configuration, options=options)
 
     print("Computed cartesian path with %d configurations, " % len(trajectory.points))
     print("following %d%% of requested trajectory." % (trajectory.fraction * 100))

docs/examples/03_backends_ros/files/gh_plan_cartesian_motion.py

@@ -22,11 +22,12 @@
         :class:`compas_fab.robots.JointTrajectory`
             The calculated trajectory.
 """
+
 from __future__ import print_function
 import scriptcontext as sc
 
 from compas.geometry import Frame
-
+from compas_fab.robots import FrameWaypoints
 
 guid = str(ghenv.Component.InstanceGuid)
 response_key = "response_" + guid
@@ -38,14 +39,14 @@
 if robot and robot.client and start_configuration and compute:
     if robot.client.is_connected:
         options = {
-            'max_step': float(max_step),
-            'avoid_collisions': bool(avoid_collisions),
-            'attached_collision_meshes': list(attached_colllision_meshes),
+            "max_step": float(max_step),
+            "avoid_collisions": bool(avoid_collisions),
+            "attached_collision_meshes": list(attached_colllision_meshes),
         }
-        sc.sticky[response_key] = robot.plan_cartesian_motion(frames,
-                                                              start_configuration=start_configuration,
-                                                              group=group,
-                                                              options=options)
+        waypoints = FrameWaypoints(frames)
+        sc.sticky[response_key] = robot.plan_cartesian_motion(
+            waypoints, start_configuration=start_configuration, group=group, options=options
+        )
     else:
         print("Robot client is not connected")
 

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

@@ -2,6 +2,7 @@
 from compas.geometry import Frame
 
 from compas_fab.backends import AnalyticalPyBulletClient
+from compas_fab.robots import FrameWaypoints
 
 frames_WCF = [
     Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)),
@@ -16,7 +17,8 @@
 
     options = {"solver": "ur5", "check_collision": True}
     start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1]
-    trajectory = robot.plan_cartesian_motion(frames_WCF, start_configuration=start_configuration, options=options)
+    waypoints = FrameWaypoints(frames_WCF)
+    trajectory = robot.plan_cartesian_motion(waypoints, start_configuration=start_configuration, options=options)
     assert trajectory.fraction == 1.0
 
     j = [c.joint_values for c in trajectory.points]

src/compas_fab/backends/__init__.py

@@ -149,6 +149,7 @@
 )
 
 if not compas.IPY:
+    # PyBullet do not work in IronPython
     from .pybullet import (
         PyBulletClient,
         CollisionError,

src/compas_fab/backends/interfaces/__init__.py

@@ -21,6 +21,7 @@
     :toctree: generated/
     :nosignatures:
 
+    BackendFeature
     ForwardKinematics
     InverseKinematics
     PlanMotion
@@ -46,6 +47,7 @@
 from .backend_features import AddAttachedCollisionMesh
 from .backend_features import AddCollisionMesh
 from .backend_features import AppendCollisionMesh
+from .backend_features import BackendFeature
 from .backend_features import ForwardKinematics
 from .backend_features import GetPlanningScene
 from .backend_features import InverseKinematics
@@ -58,17 +60,18 @@
 from .client import PlannerInterface
 
 __all__ = [
+    "AddAttachedCollisionMesh",
+    "AddCollisionMesh",
+    "AppendCollisionMesh",
+    "BackendFeature",
+    "ClientInterface",
     "ForwardKinematics",
+    "GetPlanningScene",
     "InverseKinematics",
-    "PlanMotion",
     "PlanCartesianMotion",
-    "GetPlanningScene",
-    "AddCollisionMesh",
-    "AppendCollisionMesh",
+    "PlanMotion",
+    "PlannerInterface",
     "RemoveCollisionMesh",
-    "AddAttachedCollisionMesh",
     "RemoveAttachedCollisionMesh",
     "ResetPlanningScene",
-    "ClientInterface",
-    "PlannerInterface",
 ]