diff --git a/pyrep/backend/bridge.py b/pyrep/backend/bridge.py index 9d186a6..4d91685 100644 --- a/pyrep/backend/bridge.py +++ b/pyrep/backend/bridge.py @@ -1,11 +1,26 @@ import ctypes +import sys def load(): + # add coppeliaSim's pythondir to sys.path: + from pyrep.backend.lib import ( + simGetStringParam, + simReleaseBuffer, + sim_stringparam_pythondir, + ) + + pythonDirPtr = simGetStringParam(sim_stringparam_pythondir) + pythonDir = ctypes.string_at(pythonDirPtr).decode("utf-8") + simReleaseBuffer(pythonDirPtr) + if pythonDir not in sys.path: + sys.path.append(pythonDir) + + # load lua functions for call(), getObject(), etc...: call("require", ("scriptClientBridge",)) -def call(func, args): +def call(func, args, typeHints=None): from pyrep.backend.lib import ( simCreateStack, simCallScriptFunctionEx, diff --git a/pyrep/backend/callback.py b/pyrep/backend/callback.py deleted file mode 100644 index c18a5bb..0000000 --- a/pyrep/backend/callback.py +++ /dev/null @@ -1,20 +0,0 @@ -def callback(f): - def wrapper(stackHandle): - import pyrep.backend.stack as stack - - try: - inArgs = stack.read(stackHandle) - outArgs = f(*inArgs) - if outArgs is None: - outArgs = () - elif not isinstance(outArgs, tuple): - outArgs = (outArgs,) - stack.write(stackHandle, outArgs) - return 1 - except Exception: - import traceback - - traceback.print_exc() - return 0 - - return wrapper diff --git a/pyrep/backend/lib.py b/pyrep/backend/lib.py index a4497d6..51352b7 100644 --- a/pyrep/backend/lib.py +++ b/pyrep/backend/lib.py @@ -20,19 +20,26 @@ if "COPPELIASIM_ROOT" not in os.environ: raise PyRepError("COPPELIASIM_ROOT not defined. See installation instructions.") coppeliasim_root = os.environ["COPPELIASIM_ROOT"] -coppeliasim_library = os.path.join(coppeliasim_root, "libcoppeliaSim.so") + +coppeliasim_library = "" +plat = platform.system() +if plat == "Windows": + raise NotImplementedError() + # coppeliasim_library /= f'{defaultLibNameBase}.dll' +elif plat == "Linux": + coppeliasim_library = os.path.join(coppeliasim_root, "libcoppeliaSim.so") +elif plat == "Darwin": + raise NotImplementedError() + # coppeliasim_library /= f'../MacOS/lib{defaultLibNameBase}.dylib' if not os.path.isfile(coppeliasim_library): raise PyRepError( "COPPELIASIM_ROOT was not a correct path. " "See installation instructions" ) -appDir = os.path.dirname(coppeliasim_library) -os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = appDir - +os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = coppeliasim_root -plat = platform.system() if plat == "Darwin": - fd = os.path.normpath(appDir + "/../Frameworks") + fd = os.path.normpath(coppeliasim_root + "/../Frameworks") os.environ["DYLD_LIBRARY_PATH"] = fd + ":" + os.environ.get("DYLD_LIBRARY_PATH", "") print( f"If next step fails, do: export DYLD_LIBRARY_PATH={fd}:" @@ -122,6 +129,8 @@ coppeliaSimLib.simPushDoubleTableOntoStack.restype = c_int coppeliaSimLib.simDebugStack.argtypes = [c_int, c_int] coppeliaSimLib.simDebugStack.restype = c_int +coppeliaSimLib.simGetStringParam.argtypes = [c_int] +coppeliaSimLib.simGetStringParam.restype = c_void_p __all__ = [] @@ -167,6 +176,8 @@ const.sim_stringparam_dlgverbosity = 123 const.sim_stringparam_startupscriptstring = 125 +const.sim_stringparam_pythondir = 137 + for name in dir(const): if name.startswith("sim"): f = getattr(const, name) diff --git a/pyrep/backend/stack.py b/pyrep/backend/stack.py index 3b078c7..ee15acb 100644 --- a/pyrep/backend/stack.py +++ b/pyrep/backend/stack.py @@ -71,7 +71,7 @@ def read_double(stackHandle): raise RuntimeError("expected double") -def read_string(stackHandle): +def read_string(stackHandle, encoding=None): from pyrep.backend.lib import ( simGetStackStringValue, simReleaseBuffer, @@ -80,9 +80,13 @@ def read_string(stackHandle): string_size = ctypes.c_int() string_ptr = simGetStackStringValue(stackHandle, ctypes.byref(string_size)) - string_value = ctypes.string_at(string_ptr, string_size.value) + value = ctypes.string_at(string_ptr, string_size.value) simPopStackItem(stackHandle, 1) - value = string_value.decode("utf-8") + if encoding: + try: + value = value.decode(encoding) + except UnicodeDecodeError: + pass simReleaseBuffer(string_ptr) return value @@ -119,7 +123,7 @@ def read_list(stackHandle): simMoveStackItemToTop, ) - vals = list() + lst = list() oldsz = simGetStackSize(stackHandle) simUnfoldStackTable(stackHandle) n = (simGetStackSize(stackHandle) - oldsz + 1) // 2 @@ -127,11 +131,11 @@ def read_list(stackHandle): simMoveStackItemToTop(stackHandle, oldsz - 1) read_value(stackHandle) simMoveStackItemToTop(stackHandle, oldsz - 1) - vals.append(read_value(stackHandle)) - return vals + lst.append(read_value(stackHandle)) + return lst -def read_table(stackHandle): +def read_table(stackHandle, typeHint=None): from pyrep.backend.lib import ( simGetStackTableInfo, sim_stack_table_map, @@ -139,13 +143,13 @@ def read_table(stackHandle): ) sz = simGetStackTableInfo(stackHandle, 0) - if sz >= 0: + if typeHint == "list" or sz >= 0: return read_list(stackHandle) - elif sz == sim_stack_table_map or sz == sim_stack_table_empty: + elif typeHint == "dict" or sz in (sim_stack_table_map, sim_stack_table_empty): return read_dict(stackHandle) -def read_value(stackHandle): +def read_value(stackHandle, typeHint=None): from pyrep.backend.lib import ( simGetStackItemType, sim_stackitem_null, @@ -156,25 +160,38 @@ def read_value(stackHandle): sim_stackitem_integer, ) - item_type = simGetStackItemType(stackHandle, -1) - if item_type == sim_stackitem_null: - value = read_null(stackHandle) - elif item_type == sim_stackitem_double: - value = read_double(stackHandle) - elif item_type == sim_stackitem_bool: - value = read_bool(stackHandle) - elif item_type == sim_stackitem_string: - value = read_string(stackHandle) - elif item_type == sim_stackitem_table: - value = read_table(stackHandle) - elif item_type == sim_stackitem_integer: - value = read_long(stackHandle) - else: - raise RuntimeError(f"unexpected stack item type: {item_type}") - return value - - -def read(stackHandle): + match typeHint: + case "null": + return read_null(stackHandle) + case "float" | "double": + return read_double(stackHandle) + case "bool": + return read_bool(stackHandle) + case "string": + return read_string(stackHandle, encoding="utf-8") + case "buffer": + return read_string(stackHandle, encoding=None) + case "table" | "list" | "dict": + return read_table(stackHandle, typeHint) + case "int" | "long": + return read_long(stackHandle) + itemType = simGetStackItemType(stackHandle, -1) + if itemType == sim_stackitem_null: + return read_null(stackHandle) + if itemType == sim_stackitem_double: + return read_double(stackHandle) + if itemType == sim_stackitem_bool: + return read_bool(stackHandle) + if itemType == sim_stackitem_string: + return read_string(stackHandle, encoding="utf-8") + if itemType == sim_stackitem_table: + return read_table(stackHandle, typeHint) + if itemType == sim_stackitem_integer: + return read_long(stackHandle) + raise RuntimeError(f"unexpected stack item type: {itemType} ({typeHint=})") + + +def read(stackHandle, typeHints=None): from pyrep.backend.lib import ( simGetStackSize, simMoveStackItemToTop, @@ -185,7 +202,11 @@ def read(stackHandle): tuple_data = [] for i in range(stack_size): simMoveStackItemToTop(stackHandle, 0) - tuple_data.append(read_value(stackHandle)) + if typeHints and len(typeHints) > i: + value = read_value(stackHandle, typeHints[i]) + else: + value = read_value(stackHandle) + tuple_data.append(value) simPopStackItem(stackHandle, 0) # clear all return tuple(tuple_data) @@ -222,12 +243,22 @@ def write_int(stackHandle, value): simPushInt32OntoStack(stackHandle, value) -def write_string(stackHandle, value): +def write_long(stackHandle, value): + from pyrep.backend.lib import ( + simPushInt64OntoStack, + ) + + simPushInt64OntoStack(stackHandle, value) + + +def write_string(stackHandle, value, encoding="utf-8"): from pyrep.backend.lib import ( simPushStringOntoStack, ) - simPushStringOntoStack(stackHandle, value.encode("utf-8"), len(value)) + if encoding: + value = value.encode(encoding) + simPushStringOntoStack(stackHandle, value, len(value)) def write_dict(stackHandle, value): @@ -256,28 +287,49 @@ def write_list(stackHandle, value): simInsertDataIntoStackTable(stackHandle) -def write_value(stackHandle, value): +def write_value(stackHandle, value, typeHint=None): + match typeHint: + case "null": + return write_null(stackHandle, value) + case "float" | "double": + return write_double(stackHandle, value) + case "bool": + return write_bool(stackHandle, value) + case "int" | "long": + return write_long(stackHandle, value) + case "buffer": + return write_string(stackHandle, value, encoding=None) + case "string": + return write_string(stackHandle, value, encoding="utf-8") + case "dict": + return write_dict(stackHandle, value) + case "list": + return write_list(stackHandle, value) if value is None: - write_null(stackHandle, value) + return write_null(stackHandle, value) elif isinstance(value, float): - write_double(stackHandle, value) + return write_double(stackHandle, value) elif isinstance(value, bool): - write_bool(stackHandle, value) + return write_bool(stackHandle, value) elif isinstance(value, int): - write_int(stackHandle, value) + return write_long(stackHandle, value) + elif isinstance(value, bytes): + return write_string(stackHandle, value, encoding=None) elif isinstance(value, str): - write_string(stackHandle, value) + return write_string(stackHandle, value, encoding="utf-8") elif isinstance(value, dict): - write_dict(stackHandle, value) + return write_dict(stackHandle, value) elif isinstance(value, list): - write_list(stackHandle, value) - else: - raise RuntimeError(f"unexpected type: {type(value)}") + return write_list(stackHandle, value) + raise RuntimeError(f"unexpected type: {type(value)} ({typeHint=})") -def write(stackHandle, tuple_data): - for item in tuple_data: - write_value(stackHandle, item) +def write(stackHandle, tuple_data, typeHints=None): + for i, value in enumerate(tuple_data): + if typeHints and len(typeHints) > i: + write_value(stackHandle, value, typeHints[i]) + else: + write_value(stackHandle, value) def debug(stackHandle, info=None): @@ -293,3 +345,44 @@ def debug(stackHandle, info=None): for i in range(simGetStackSize(stackHandle)): simDebugStack(stackHandle, i) print("#" * 70) + + +def callback(f): + def wrapper(stackHandle): + from typing import get_args + + inTypes = tuple( + [ + arg_type.__name__ + for arg, arg_type in f.__annotations__.items() + if arg != "return" + ] + ) + + if return_annotation := f.__annotations__.get("return"): + origin = getattr(return_annotation, "__origin__", None) + if origin in (tuple, list): # Handling built-in tuple and list + outTypes = tuple([t.__name__ for t in get_args(return_annotation)]) + elif origin: # Handling other generic types like Tuple, List from typing + outTypes = (origin.__name__,) + else: + outTypes = (return_annotation.__name__,) + else: + outTypes = () + + try: + inArgs = read(stackHandle, inTypes) + outArgs = f(*inArgs) + if outArgs is None: + outArgs = () + elif not isinstance(outArgs, tuple): + outArgs = (outArgs,) + write(stackHandle, outArgs, outTypes) + return 1 + except Exception: + import traceback + + traceback.print_exc() + return 0 + + return wrapper diff --git a/pyrep/const.py b/pyrep/const.py index ce2d7c5..e087c1d 100644 --- a/pyrep/const.py +++ b/pyrep/const.py @@ -17,7 +17,6 @@ class ObjectType(Enum): PROXIMITY_SENSOR = sim.sim_object_proximitysensor_type GRAPH = sim.sim_object_graph_type CAMERA = sim.sim_object_camera_type - PATH = sim.sim_object_dummy_type VISION_SENSOR = sim.sim_object_visionsensor_type VOLUME = sim.sim_object_volume_type MILl = sim.sim_object_mill_type diff --git a/pyrep/objects/__init__.py b/pyrep/objects/__init__.py index e69de29..ded1d74 100644 --- a/pyrep/objects/__init__.py +++ b/pyrep/objects/__init__.py @@ -0,0 +1,26 @@ +from pyrep.objects.dummy import Dummy +from pyrep.objects.cartesian_path import CartesianPath + +from pyrep.objects.force_sensor import ForceSensor +from pyrep.objects.joint import Joint +from pyrep.objects.light import Light +from pyrep.objects.object import Object +from pyrep.objects.octree import Octree + +from pyrep.objects.proximity_sensor import ProximitySensor + +from pyrep.objects.shape import Shape +from pyrep.objects.vision_sensor import VisionSensor + +__all__ = [ + Dummy, + CartesianPath, + ForceSensor, + Joint, + Light, + Object, + Octree, + ProximitySensor, + Shape, + VisionSensor, +] diff --git a/pyrep/objects/cartesian_path.py b/pyrep/objects/cartesian_path.py index d3536d5..83a0de0 100644 --- a/pyrep/objects/cartesian_path.py +++ b/pyrep/objects/cartesian_path.py @@ -4,7 +4,7 @@ from pyrep.backend import sim_const as simc from pyrep.backend.sim import SimBackend -from pyrep.objects.object import Object, object_type_to_class +from pyrep.objects.object import Object from pyrep.const import ObjectType @@ -83,7 +83,7 @@ def create( return CartesianPath(handle) def _get_requested_type(self) -> ObjectType: - return ObjectType.PATH + return ObjectType.DUMMY def get_pose_on_path( self, relative_distance: float @@ -114,7 +114,7 @@ def get_pose_on_path( [2, 2, 2, 2], ) # TODO: Hack for now; convert quat -> euler using library - from pyrep.objects import Dummy + from pyrep.objects.dummy import Dummy d = Dummy.create() d.set_position(pos, self) @@ -124,6 +124,3 @@ def get_pose_on_path( ori = d.get_orientation() d.remove() return pos, ori - - -object_type_to_class[ObjectType.PATH] = CartesianPath diff --git a/pyrep/objects/light.py b/pyrep/objects/light.py index 88bd6a8..08805ef 100644 --- a/pyrep/objects/light.py +++ b/pyrep/objects/light.py @@ -48,7 +48,7 @@ def get_diffuse(self): def set_diffuse(self, diffuse): """Set the diffuse colors of the light.""" self._sim_api.setLightParameters( - self._handle, self.is_on(), None, list(diffuse), None + self._handle, self.is_on(), None, list(diffuse), list(self.get_specular()) ) def get_specular(self): diff --git a/pyrep/objects/object.py b/pyrep/objects/object.py index c49323f..1952b28 100644 --- a/pyrep/objects/object.py +++ b/pyrep/objects/object.py @@ -62,13 +62,16 @@ def exists(name: str) -> bool: return handle >= 0 @staticmethod - def get_object_type(name: str) -> ObjectType: + def get_object_type(name_or_handle: Union[int, str]) -> ObjectType: """Gets the type of the object. :return: Type of the object. """ sim_api = SimBackend().sim_api - return ObjectType(sim_api.getObjectType(sim_api.getObjectHandle(name))) + handle = name_or_handle + if not isinstance(name_or_handle, int): + handle = sim_api.getObjectHandle(name_or_handle) + return ObjectType(sim_api.getObjectType(handle)) @staticmethod def get_object_name(name_or_handle: Union[str, int]) -> str: @@ -84,15 +87,14 @@ def get_object_name(name_or_handle: Union[str, int]) -> str: return name @staticmethod - def get_object(name_or_handle: str) -> "Object": + def get_object(name_or_handle: Union[int, str]) -> "Object": """Gets object retrieved by name. :return: The object. """ - name = Object.get_object_name(name_or_handle) - object_type = Object.get_object_type(name) + object_type = Object.get_object_type(name_or_handle) cls = object_type_to_class[object_type] - return cls(name) + return cls(name_or_handle) def _get_requested_type(self) -> ObjectType: """Used for internally checking assumptions user made about object type. @@ -520,7 +522,21 @@ def get_extension_string(self) -> str: :return: The extension string. """ - return self._sim_api.getExtensionString(self._handle, -1, "") + val = self._sim_api.getExtensionString(self._handle, -1, "") + return "" if val is None else val + + def get_configuration_tree(self) -> bytes: + """Retrieves configuration information for a hierarchy tree. + + Configuration includes object relative positions/orientations, + joint/path values. Calling :py:meth:`PyRep.set_configuration_tree` at a + later time, will restore the object configuration + (use this function to temporarily save object + positions/orientations/joint/path values). + + :return: The configuration tree. + """ + return self._sim_api.getConfigurationTree(self._handle) def rotate(self, rotation: List[float]) -> None: """Rotates a transformation matrix. diff --git a/pyrep/objects/octree.py b/pyrep/objects/octree.py index 31ffcd0..cc1f999 100644 --- a/pyrep/objects/octree.py +++ b/pyrep/objects/octree.py @@ -128,10 +128,7 @@ def check_point_occupancy(self, points: List[float], options: int = 0) -> bool: def clear_voxels(self) -> None: """Clears all voxels from the octree.""" - # self._sim_api.removeVoxelsFromOctree(self._handle, 0, None) - raise NotImplementedError( - "removeVoxelsFromOctree seems to have bug on coppeliasim side." - ) + self._sim_api.removeVoxelsFromOctree(self._handle, 0, self.get_voxels()) object_type_to_class[ObjectType.OCTREE] = Octree diff --git a/pyrep/objects/vision_sensor.py b/pyrep/objects/vision_sensor.py index ff5a79a..b085a17 100644 --- a/pyrep/objects/vision_sensor.py +++ b/pyrep/objects/vision_sensor.py @@ -132,6 +132,8 @@ def capture_rgb(self) -> np.ndarray: :return: A numpy array of size (width, height, 3) """ enc_img, resolution = self._sim_api.getVisionSensorImg(self._handle) + if isinstance(enc_img, str): + enc_img = enc_img.encode() img = np.frombuffer(enc_img, dtype=np.uint8).reshape( resolution[1], resolution[0], 3 ) diff --git a/pyrep/pyrep.py b/pyrep/pyrep.py index 873178a..5683ea0 100644 --- a/pyrep/pyrep.py +++ b/pyrep/pyrep.py @@ -286,3 +286,17 @@ def get_collection_handle_by_name(self, collection_name: str) -> int: :return: An integer handle for the collection. """ return self._sim_api.getCollectionHandle(collection_name) + + def set_configuration_tree(self, config_tree: bytes) -> None: + """Restores configuration information previously retrieved. + + Configuration information (object relative positions/orientations, + joint/path values) can be retrieved with + :py:meth:`Object.get_configuration_tree`. Dynamically simulated + objects will implicitly be reset before the command is applied + (i.e. similar to calling :py:meth:`Object.reset_dynamic_object` just + before). + + :param config_tree: The configuration tree to restore. + """ + self._sim_api.setConfigurationTree(config_tree) diff --git a/pyrep/robots/arms/arm.py b/pyrep/robots/arms/arm.py index 20624b0..f441b59 100644 --- a/pyrep/robots/arms/arm.py +++ b/pyrep/robots/arms/arm.py @@ -1,7 +1,7 @@ from ctypes import CFUNCTYPE, c_int from pyrep.backend.sim import SimBackend from pyrep.backend import sim_const as simc -from pyrep.objects import Object +from pyrep.objects.object import Object from pyrep.objects.dummy import Dummy from pyrep.robots.configuration_paths.arm_configuration_path import ArmConfigurationPath from pyrep.robots.robot_component import RobotComponent @@ -10,9 +10,7 @@ from pyrep.const import ConfigurationPathAlgorithms as Algos from typing import List, Union import numpy as np - - -from pyrep.backend.callback import callback +from pyrep.backend.stack import callback CALLBACK_NAME = "ccallback0" @@ -20,9 +18,13 @@ @callback def collision_check_callback(state, data): - arm = Arm(*data) + count, name, num_joints, base_name = data + joint_names = ["%s_joint%d" % (name, i + 1) for i in range(num_joints)] + arm = RobotComponent( + count, name, joint_names, None if len(base_name) == 0 else base_name + ) arm.set_joint_positions(state) - return not arm.check_arm_collision() + return not arm.check_collision() class Arm(RobotComponent): @@ -52,18 +54,11 @@ def __init__( self._ik_target = Dummy("%s_target%s" % (name, suffix)) self._ik_tip = Dummy("%s_tip%s" % (name, suffix)) - self._collision_collection_handle = self._sim_api.createCollection(0) - self._sim_api.addItemToCollection( - self._collision_collection_handle, - simc.sim_handle_tree, - self._joint_handles[0], - 0, - ) self._sim_ik_api = SimBackend().sim_ik_api self._sim_ompl_api = SimBackend().sim_ompl_api self._ik_env_handle = self._sim_ik_api.createEnvironment() self._ik_group_handle = self._sim_ik_api.createGroup(self._ik_env_handle) - self.set_ik_group_properties("pseudo_inverse", 6, 0.1) + self.set_ik_group_properties("pseudo_inverse", 6, 0.02) base_handle = self.get_handle() target_handle = self._ik_target.get_handle() @@ -95,7 +90,12 @@ def __init__( # Not used, but needs to be kept in scope, or will be garbage collected self._collision_callback = CFUNCTYPE(c_int, c_int)(collision_check_callback) SimBackend().lib.simRegCallback(0, self._collision_callback) - self._coll_callback_args = [count, name, num_joints, base_name] + self._coll_callback_args = [ + count, + name, + num_joints, + base_name if base_name is not None else "", + ] def set_ik_element_properties( self, @@ -150,8 +150,8 @@ def solve_ik_via_sampling( ignore_collisions: bool = False, trials: int = 300, max_configs: int = 1, - distance_threshold: float = 0.65, - max_time_ms: float = 100, + distance_threshold: float = 0.25, + max_time_ms: float = 500, relative_to: Object = None, ) -> np.ndarray: """Solves an IK group and returns the calculated joint values. @@ -212,6 +212,7 @@ def solve_ik_via_sampling( self._ik_target.get_pose().tolist(), self._sim_ik_api.handle_world, ) + initial_config = self.get_joint_positions() for i in range(trials): config = self._sim_ik_api.findConfig( self._ik_env_handle, @@ -223,6 +224,8 @@ def solve_ik_via_sampling( validation_callback, self._coll_callback_args, ) + if not np.allclose(self.get_tip().get_position(), position, atol=0.01): + raise ConfigurationError("Found a config, but tip was not on target.") if config is None: continue # TODO: Look to get alternative config @@ -233,6 +236,7 @@ def solve_ik_via_sampling( if len(valid_joint_positions) >= max_configs: break + self.set_joint_positions(initial_config) self._ik_target.set_pose(prev_pose) if len(valid_joint_positions) == 0: raise ConfigurationError( @@ -411,10 +415,10 @@ def get_nonlinear_path( ignore_collisions=False, trials=300, max_configs=1, - distance_threshold: float = 0.65, - max_time_ms: int = 100, + distance_threshold: float = 0.25, + max_time_ms: int = 500, trials_per_goal=1, - algorithm=Algos.SBL, + algorithm=Algos.RRTConnect, relative_to: Object = None, ) -> ArmConfigurationPath: """Gets a non-linear (planned) configuration path given a target pose. @@ -505,9 +509,9 @@ def get_path( trials=300, max_configs=1, distance_threshold: float = 0.65, - max_time_ms: int = 100, + max_time_ms: int = 500, trials_per_goal=1, - algorithm=Algos.SBL, + algorithm=Algos.RRTConnect, relative_to: Object = None, ) -> ArmConfigurationPath: """Tries to get a linear path, failing that tries a non-linear path. @@ -594,8 +598,4 @@ def check_arm_collision(self, obj: "Object" = None) -> bool: must be marked as collidable! :return: If the object is colliding. """ - handle = simc.sim_handle_all if obj is None else obj.get_handle() - result, colliding_handles = self._sim_api.checkCollision( - self._collision_collection_handle, handle - ) - return result == 1 + return self.check_collision(obj) diff --git a/pyrep/robots/configuration_paths/arm_configuration_path.py b/pyrep/robots/configuration_paths/arm_configuration_path.py index f98ad37..9ba7eb4 100644 --- a/pyrep/robots/configuration_paths/arm_configuration_path.py +++ b/pyrep/robots/configuration_paths/arm_configuration_path.py @@ -4,12 +4,6 @@ import numpy as np from typing import List, Optional, Union -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - # Circular import issue - from pyrep.robots.arms.arm import Arm - class ArmConfigurationPath(ConfigurationPath): """A path expressed in joint configuration space. @@ -23,7 +17,9 @@ class ArmConfigurationPath(ConfigurationPath): control systems. """ - def __init__(self, arm: Arm, path_points: Union[List[float], np.ndarray]): + def __init__( + self, arm: "Arm", path_points: Union[List[float], np.ndarray] # noqa: F821 + ): self._arm = arm self._path_points = np.asarray(path_points) self._rml_handle: Optional[int] = None @@ -89,8 +85,11 @@ def visualize(self) -> None: self._arm.set_joint_positions(self._path_points[0 : len(self._arm.joints)]) prev_point = list(tip.get_position()) + sub_sample = 10 for i in range( - len(self._arm.joints), len(self._path_points), len(self._arm.joints) + len(self._arm.joints), + len(self._path_points), + len(self._arm.joints) * sub_sample, ): points = self._path_points[i : i + len(self._arm.joints)] self._arm.set_joint_positions(points) diff --git a/pyrep/robots/end_effectors/robotiq85_gripper.py b/pyrep/robots/end_effectors/robotiq85_gripper.py index 42da5dc..592531c 100644 --- a/pyrep/robots/end_effectors/robotiq85_gripper.py +++ b/pyrep/robots/end_effectors/robotiq85_gripper.py @@ -1,6 +1,6 @@ from typing import List -from pyrep.objects import Joint +from pyrep.objects.joint import Joint from pyrep.robots.end_effectors.gripper import Gripper import numpy as np diff --git a/pyrep/robots/robot_component.py b/pyrep/robots/robot_component.py index 690ff66..e4bcba2 100644 --- a/pyrep/robots/robot_component.py +++ b/pyrep/robots/robot_component.py @@ -22,6 +22,14 @@ def __init__( self.joints = [Joint(jname + suffix) for jname in joint_names] self._joint_handles = [j.get_handle() for j in self.joints] + self._collision_collection_handle = self._sim_api.createCollection(0) + self._sim_api.addItemToCollection( + self._collision_collection_handle, + simc.sim_handle_tree, + self._joint_handles[0], + 0, + ) + def copy(self) -> "RobotComponent": """Copy and pastes the arm in the scene. @@ -282,3 +290,17 @@ def _assert_len(self, inputs: list) -> None: "Tried to set values for %d joints, but joint group consists " "of %d joints." % (len(inputs), len(self.joints)) ) + + def check_collision(self, obj: "Object" = None) -> bool: + """Checks whether two entities are colliding. + + :param obj: The other collidable object to check collision against, + or None to check against all collidable objects. Note that objects + must be marked as collidable! + :return: If the object is colliding. + """ + handle = simc.sim_handle_all if obj is None else obj.get_handle() + result, colliding_handles = self._sim_api.checkCollision( + self._collision_collection_handle, handle + ) + return result == 1 diff --git a/tests/test_objects.py b/tests/test_objects.py index 36b3d43..adbbe99 100644 --- a/tests/test_objects.py +++ b/tests/test_objects.py @@ -226,9 +226,13 @@ def test_get_objects_in_tree(self): def test_get_extention_string(self): self.assertEqual(self.dynamic_cube.get_extension_string(), "test") - def test_get_configuration_tree(self): - config = self.dynamic_cube.get_configuration_tree() + def test_set_configuration_tree(self): + dynamic_cube = Shape("dynamic_cube") + dynamic_cube.get_position() + config = dynamic_cube.get_configuration_tree() self.assertIsNotNone(config) + [self.pyrep.step() for _ in range(10)] + self.pyrep.set_configuration_tree(config) def test_rotate(self): self.dynamic_cube.rotate([0.02, 0.04, 0.06]) diff --git a/tests/test_octrees.py b/tests/test_octrees.py index 4b3887b..7736f05 100644 --- a/tests/test_octrees.py +++ b/tests/test_octrees.py @@ -1,4 +1,3 @@ -import unittest from tests.core import TestCore from pyrep.objects.shape import Shape from pyrep.objects.octree import Octree @@ -30,7 +29,6 @@ def test_octree_insert_and_subtract_object(self): voxels = self.octree.get_voxels() self.assertTrue(len(voxels) // 3 == 0) - @unittest.skip("Removing voxels in coppeliasim not working.") def test_octree_insert_and_clear(self): self.octree.insert_object(self.shape) voxels = self.octree.get_voxels() diff --git a/tests/test_suction_cups.py b/tests/test_suction_cups.py index b36dd4f..4c83f76 100644 --- a/tests/test_suction_cups.py +++ b/tests/test_suction_cups.py @@ -4,13 +4,11 @@ from pyrep.objects.shape import Shape from os import path -from pyrep.robots.end_effectors.dobot_suction_cup import DobotSuctionCup from pyrep.robots.end_effectors.baxter_suction_cup import BaxterSuctionCup ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), "assets") SUCTION_CUPS = [ - ("DobotSuctionCup", DobotSuctionCup), ("BaxterSuctionCup", BaxterSuctionCup), ]