diff --git a/CHANGELOG.md b/CHANGELOG.md index 9c304f4c08..67bca16a77 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,8 +17,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed -* Backend planners 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)`. + +* 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_carteisan_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. diff --git a/src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py b/src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py index 3990c0e7ea..59c6075020 100644 --- a/src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py +++ b/src/compas_fab/backends/kinematics/analytical_inverse_kinematics.py @@ -103,7 +103,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N 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) + cached_robot_model = self.client.get_cached_robot_model(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: diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py index 87fe42d2df..ee14a2815b 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_add_attached_collision_mesh.py @@ -60,7 +60,7 @@ def add_attached_collision_mesh(self, attached_collision_mesh, options=None): ``None`` """ robot = options["robot"] - self.client.ensure_cached_robot_geometry(robot) + self.client.ensure_cached_robot_model_geometry(robot) mass = options.get("mass", 1.0) concavity = options.get("concavity", False) @@ -68,7 +68,7 @@ def add_attached_collision_mesh(self, attached_collision_mesh, options=None): inertial_origin = options.get("inertial_origin", Frame.worldXY()) collision_origin = options.get("collision_origin", Frame.worldXY()) - cached_robot_model = self.client.get_cached_robot(robot) + cached_robot_model = self.client.get_cached_robot_model(robot) # add link mesh = attached_collision_mesh.collision_mesh.mesh diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py index dab8262272..730400591e 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py @@ -37,9 +37,9 @@ def forward_kinematics(self, robot, configuration, group=None, options=None): The frame in the world's coordinate system (WCF). """ link_name = options.get("link") or robot.get_end_effector_link_name(group) - cached_robot = self.client.get_cached_robot(robot) - body_id = self.client.get_uid(cached_robot) - link_id = self.client._get_link_id_by_name(link_name, cached_robot) + cached_robot_model = self.client.get_cached_robot_model(robot) + body_id = self.client.get_uid(cached_robot_model) + link_id = self.client._get_link_id_by_name(link_name, cached_robot_model) self.client.set_robot_configuration(robot, configuration, group) frame = self.client._get_link_frame(link_id, body_id) if options.get("check_collision"): diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py index a3586e95fd..27b9d2445c 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py @@ -74,12 +74,12 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N high_accuracy = options.get("high_accuracy", True) max_results = options.get("max_results", 100) link_name = options.get("link_name") or robot.get_end_effector_link_name(group) - cached_robot = self.client.get_cached_robot(robot) - body_id = self.client.get_uid(cached_robot) - link_id = self.client._get_link_id_by_name(link_name, cached_robot) + cached_robot_model = self.client.get_cached_robot_model(robot) + body_id = self.client.get_uid(cached_robot_model) + link_id = self.client._get_link_id_by_name(link_name, cached_robot_model) point, orientation = pose_from_frame(frame_WCF) - joints = cached_robot.get_configurable_joints() + joints = cached_robot_model.get_configurable_joints() joints.sort(key=lambda j: j.attr["pybullet"]["id"]) joint_names = [joint.name for joint in joints] diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py index 8497331ff9..698b5192cd 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_remove_attached_collision_mesh.py @@ -34,9 +34,9 @@ def remove_attached_collision_mesh(self, id, options=None): ``None`` """ robot = options["robot"] - self.client.ensure_cached_robot_geometry(robot) + self.client.ensure_cached_robot_model_geometry(robot) - cached_robot_model = self.client.get_cached_robot(robot) + cached_robot_model = self.client.get_cached_robot_model(robot) # remove link and fixed joint cached_robot_model.remove_link(id) diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index 6f382f2259..889c4f36a0 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -32,6 +32,11 @@ pybullet = LazyLoader("pybullet", globals(), "pybullet") +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import list __all__ = [ "PyBulletClient", @@ -123,7 +128,7 @@ def is_connected(self): class PyBulletClient(PyBulletBase, ClientInterface): """Interface to use pybullet as backend. - :class:`compasfab.backends.PyBulletClient` is a context manager type, so it's best + :class:`compas_fab.backends.PyBulletClient` is a context manager type, so it's best used in combination with the ``with`` statement to ensure resource deallocation. @@ -199,9 +204,9 @@ def load_ur5(self, load_geometry=False, concavity=False): robot.attributes["pybullet"] = {} if load_geometry: - self.cache_robot(robot, concavity) + self.cache_robot_model(robot, concavity) else: - robot.attributes["pybullet"]["cached_robot"] = robot.model + robot.attributes["pybullet"]["cached_robot_model"] = robot.model robot.attributes["pybullet"]["cached_robot_filepath"] = compas_fab.get( "robot_library/ur5_robot/urdf/robot_description.urdf" ) @@ -244,9 +249,9 @@ def load_robot(self, urdf_file, resource_loaders=None, concavity=False, precisio robot.attributes["pybullet"] = {} if resource_loaders: robot_model.load_geometry(*resource_loaders, precision=precision) - self.cache_robot(robot, concavity) + self.cache_robot_model(robot, concavity) else: - robot.attributes["pybullet"]["cached_robot"] = robot.model + robot.attributes["pybullet"]["cached_robot_model"] = robot.model robot.attributes["pybullet"]["cached_robot_filepath"] = urdf_file urdf_fp = robot.attributes["pybullet"]["cached_robot_filepath"] @@ -265,20 +270,20 @@ def load_semantics(self, robot, srdf_filename): srdf_filename : :obj:`str` or file object Absolute file path to the srdf file name. """ - cached_robot = self.get_cached_robot(robot) - robot.semantics = RobotSemantics.from_srdf_file(srdf_filename, cached_robot) + cached_robot_model = self.get_cached_robot_model(robot) + robot.semantics = RobotSemantics.from_srdf_file(srdf_filename, cached_robot_model) self.disabled_collisions = robot.semantics.disabled_collisions def _load_robot_to_pybullet(self, urdf_file, robot): - cached_robot = self.get_cached_robot(robot) + cached_robot_model = self.get_cached_robot_model(robot) with redirect_stdout(enabled=not self.verbose): pybullet_uid = pybullet.loadURDF( urdf_file, useFixedBase=True, physicsClientId=self.client_id, flags=pybullet.URDF_USE_SELF_COLLISION ) - cached_robot.attr["uid"] = pybullet_uid + cached_robot_model.attr["uid"] = pybullet_uid - self._add_ids_to_robot_joints(cached_robot) - self._add_ids_to_robot_links(cached_robot) + self._add_ids_to_robot_joints(cached_robot_model) + self._add_ids_to_robot_links(cached_robot_model) def reload_from_cache(self, robot): """Reloads the PyBullet server with the robot's cached model. @@ -290,7 +295,7 @@ def reload_from_cache(self, robot): """ current_configuration = self.get_robot_configuration(robot) - cached_robot_model = self.get_cached_robot(robot) + cached_robot_model = self.get_cached_robot_model(robot) cached_robot_filepath = self.get_cached_robot_filepath(robot) robot_uid = self.get_uid(cached_robot_model) pybullet.removeBody(robot_uid, physicsClientId=self.client_id) @@ -303,7 +308,7 @@ def reload_from_cache(self, robot): self.set_robot_configuration(robot, current_configuration) self.step_simulation() - def cache_robot(self, robot, concavity=False): + def cache_robot_model(self, robot, concavity=False): """Saves an editable copy of the robot's model and its meshes for shadowing the state of the robot on the PyBullet server. @@ -345,27 +350,28 @@ def cache_robot(self, robot, concavity=False): mesh.attrib["filename"] = address_dict[filename] # write urdf - cached_robot_file_name = str(robot.model.guid) + ".urdf" - cached_robot_filepath = os.path.join(self._cache_dir.name, cached_robot_file_name) + cached_robot_model_file_name = str(robot.model.guid) + ".urdf" + cached_robot_filepath = os.path.join(self._cache_dir.name, cached_robot_model_file_name) urdf.to_file(cached_robot_filepath, prettify=True) - cached_robot = RobotModel.from_urdf_file(cached_robot_filepath) - robot.attributes["pybullet"]["cached_robot"] = cached_robot + cached_robot_model = RobotModel.from_urdf_file(cached_robot_filepath) + robot.attributes["pybullet"]["cached_robot_model"] = cached_robot_model robot.attributes["pybullet"]["cached_robot_filepath"] = cached_robot_filepath robot.attributes["pybullet"]["robot_geometry_cached"] = True @staticmethod - def ensure_cached_robot(robot): + def ensure_cached_robot_model(robot): """Checks if a :class:`compas_fab.robots.Robot` has been cached for use with PyBullet.""" - if not robot.attributes["pybullet"]["cached_robot"]: + if not robot.attributes["pybullet"]["cached_robot_model"]: raise Exception("This method is only callable once the robot has been cached.") @staticmethod - def ensure_cached_robot_geometry(robot): + def ensure_cached_robot_model_geometry(robot): """Checks if the geometry of a :class:`compas_fab.robots.Robot` has been cached for use with PyBullet.""" if not robot.attributes["pybullet"].get("robot_geometry_cached"): raise Exception("This method is only callable once the robot with loaded geometry has been cached.") - def get_cached_robot(self, robot): + def get_cached_robot_model(self, robot): + # type: (Robot) -> RobotModel """Returns the editable copy of the robot's model for shadowing the state of the robot on the PyBullet server. @@ -384,10 +390,11 @@ def get_cached_robot(self, robot): If the robot has not been cached. """ - self.ensure_cached_robot(robot) - return robot.attributes["pybullet"]["cached_robot"] + self.ensure_cached_robot_model(robot) + return robot.attributes["pybullet"]["cached_robot_model"] def get_cached_robot_filepath(self, robot): + # type: (Robot) -> str """Returns the filepath of the editable copy of the robot's model for shadowing the state of the robot on the PyBullet server. @@ -406,16 +413,16 @@ def get_cached_robot_filepath(self, robot): If the robot has not been cached. """ - self.ensure_cached_robot(robot) + self.ensure_cached_robot_model(robot) return robot.attributes["pybullet"]["cached_robot_filepath"] - def get_uid(self, cached_robot): + def get_uid(self, cached_robot_model): """Returns the internal PyBullet id of the robot's model for shadowing the state of the robot on the PyBullet server. Parameters ---------- - cached_robot : :class:`compas_robots.RobotModel` + cached_robot_model : :class:`compas_robots.RobotModel` The robot model saved for use with PyBullet. Returns @@ -423,37 +430,37 @@ def get_uid(self, cached_robot): :obj:`int` """ - return cached_robot.attr["uid"] + return cached_robot_model.attr["uid"] - def _add_ids_to_robot_joints(self, cached_robot): - body_id = self.get_uid(cached_robot) + def _add_ids_to_robot_joints(self, cached_robot_model): + body_id = self.get_uid(cached_robot_model) joint_ids = self._get_joint_ids(body_id) for joint_id in joint_ids: joint_name = self._get_joint_name(joint_id, body_id) - joint = cached_robot.get_joint_by_name(joint_name) + joint = cached_robot_model.get_joint_by_name(joint_name) pybullet_attr = {"id": joint_id} joint.attr.setdefault("pybullet", {}).update(pybullet_attr) - def _add_ids_to_robot_links(self, cached_robot): - body_id = self.get_uid(cached_robot) + def _add_ids_to_robot_links(self, robot_model): + body_id = self.get_uid(robot_model) joint_ids = self._get_joint_ids(body_id) for link_id in joint_ids: link_name = self._get_link_name(link_id, body_id) - link = cached_robot.get_link_by_name(link_name) + link = robot_model.get_link_by_name(link_name) pybullet_attr = {"id": link_id} link.attr.setdefault("pybullet", {}).update(pybullet_attr) - def _get_joint_id_by_name(self, name, cached_robot): - return cached_robot.get_joint_by_name(name).attr["pybullet"]["id"] + def _get_joint_id_by_name(self, name, robot_model): + return robot_model.get_joint_by_name(name).attr["pybullet"]["id"] - def _get_joint_ids_by_name(self, names, cached_robot): - return tuple(self._get_joint_id_by_name(name, cached_robot) for name in names) + def _get_joint_ids_by_name(self, names, robot_model): + return tuple(self._get_joint_id_by_name(name, robot_model) for name in names) - def _get_link_id_by_name(self, name, cached_robot): - return cached_robot.get_link_by_name(name).attr["pybullet"]["id"] + def _get_link_id_by_name(self, name, robot_model): + return robot_model.get_link_by_name(name).attr["pybullet"]["id"] - def _get_link_ids_by_name(self, names, cached_robot): - return tuple(self._get_link_id_by_name(name, cached_robot) for name in names) + def _get_link_ids_by_name(self, names, robot_model): + return tuple(self._get_link_id_by_name(name, robot_model) for name in names) def filter_configurations_in_collision(self, robot, configurations): """Filters from a list of configurations those which are in collision. @@ -495,10 +502,10 @@ def check_collisions(self, robot, configuration=None): ------ :class:`compas_fab.backends.pybullet.DetectedCollision` """ - cached_robot = self.get_cached_robot(robot) - body_id = self.get_uid(cached_robot) + cached_robot_model = self.get_cached_robot_model(robot) + body_id = self.get_uid(cached_robot_model) if configuration: - joint_ids = self._get_joint_ids_by_name(configuration.joint_names, cached_robot) + joint_ids = self._get_joint_ids_by_name(configuration.joint_names, cached_robot_model) self._set_joint_positions(joint_ids, configuration.joint_values, body_id) self.check_collision_with_objects(robot) self.check_robot_self_collision(robot) @@ -518,7 +525,7 @@ def check_collision_with_objects(self, robot): """ for name, body_ids in self.collision_objects.items(): for body_id in body_ids: - self._check_collision(self.get_uid(self.get_cached_robot(robot)), "robot", body_id, name) + self._check_collision(self.get_uid(self.get_cached_robot_model(robot)), "robot", body_id, name) def check_robot_self_collision(self, robot): """Checks whether the robot and its attached collision objects with its current @@ -533,15 +540,15 @@ def check_robot_self_collision(self, robot): ------ :class:`compas_fab.backends.pybullet.DetectedCollision` """ - cached_robot = self.get_cached_robot(robot) - body_id = self.get_uid(cached_robot) - link_names = [link.name for link in cached_robot.iter_links() if link.collision] + cached_robot_model = self.get_cached_robot_model(robot) + body_id = self.get_uid(cached_robot_model) + link_names = [link.name for link in cached_robot_model.iter_links() if link.collision] # check for collisions between robot links for link_1_name, link_2_name in combinations(link_names, 2): if {link_1_name, link_2_name} in self.unordered_disabled_collisions: continue - link_1_id = self._get_link_id_by_name(link_1_name, cached_robot) - link_2_id = self._get_link_id_by_name(link_2_name, cached_robot) + link_1_id = self._get_link_id_by_name(link_1_name, cached_robot_model) + link_2_id = self._get_link_id_by_name(link_2_name, cached_robot_model) self._check_collision(body_id, link_1_name, body_id, link_2_name, link_1_id, link_2_id) def check_collision_objects_for_collision(self): @@ -603,6 +610,7 @@ def _get_num_joints(self, body_id): return pybullet.getNumJoints(body_id, physicsClientId=self.client_id) def _get_joint_ids(self, body_id): + # type: (int) -> list[int] return list(range(self._get_num_joints(body_id))) def _get_joint_name(self, joint_id, body_id): @@ -652,11 +660,11 @@ def set_robot_configuration(self, robot, configuration, group=None): The planning group used for calculation. Defaults to the robot's main planning group. """ - cached_robot = self.get_cached_robot(robot) - body_id = self.get_uid(cached_robot) + cached_robot_model = self.get_cached_robot_model(robot) + body_id = self.get_uid(cached_robot_model) default_config = robot.zero_configuration() full_configuration = robot.merge_group_with_full_configuration(configuration, default_config, group) - joint_ids = self._get_joint_ids_by_name(full_configuration.joint_names, cached_robot) + joint_ids = self._get_joint_ids_by_name(full_configuration.joint_names, cached_robot_model) self._set_joint_positions(joint_ids, full_configuration.joint_values, body_id) return full_configuration @@ -671,10 +679,10 @@ def get_robot_configuration(self, robot): ------- :class:`compas_robots.Configuration` """ - cached_robot = self.get_cached_robot(robot) - body_id = self.get_uid(cached_robot) + cached_robot_model = self.get_cached_robot_model(robot) + body_id = self.get_uid(cached_robot_model) default_config = robot.zero_configuration() - joint_ids = self._get_joint_ids_by_name(default_config.joint_names, cached_robot) + joint_ids = self._get_joint_ids_by_name(default_config.joint_names, cached_robot_model) joint_values = self._get_joint_positions(joint_ids, body_id) default_config.joint_values = joint_values return default_config