diff --git a/.gitignore b/.gitignore index 547d7d1..50bd20a 100644 --- a/.gitignore +++ b/.gitignore @@ -99,4 +99,3 @@ ENV/ .mypy_cache/ pyrep/backend/_sim_cffi* - diff --git a/README.md b/README.md index 94a4884..2848d3f 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ PyRep has undergone a __MAJOR__ update, and is now compatible with the most rece ## Install -PyRep requires version **4.6.0** of CoppeliaSim. Download: +PyRep requires version **4.6.0** of CoppeliaSim. Download: - [Ubuntu 20.04](https://downloads.coppeliarobotics.com/V4_6_0_rev16/CoppeliaSim_Edu_V4_6_0_rev16_Ubuntu20_04.tar.xz) - [Ubuntu 22.04](https://downloads.coppeliarobotics.com/V4_6_0_rev16/CoppeliaSim_Edu_V4_6_0_rev16_Ubuntu22_04.tar.xz) @@ -97,7 +97,7 @@ from pyrep import PyRep pr = PyRep() # Launch the application with a scene file in headless mode -pr.launch('scene.ttt', headless=True) +pr.launch('scene.ttt', headless=True) pr.start() # Start the simulation # Do some stuff @@ -113,7 +113,7 @@ pr.shutdown() # Close the application from pyrep.objects.shape import Shape from pyrep.const import PrimitiveShape -object = Shape.create(type=PrimitiveShape.CYLINDER, +object = Shape.create(type=PrimitiveShape.CYLINDER, color=[r,g,b], size=[w, h, d], position=[x, y, z]) object.set_color([r, g, b]) @@ -124,7 +124,7 @@ object.set_position([x, y, z]) Robots are designed to be modular; arms are treated separately to grippers. -Use the robot ttm files defined in robots/ttms. These have been altered slightly from the original ones shipped with CoppeliaSim to allow them to be used with motional planning out of the box. +Use the robot ttm files defined in robots/ttms. These have been altered slightly from the original ones shipped with CoppeliaSim to allow them to be used with motional planning out of the box. The 'tip' of the robot may not be where you want it, so feel free to play around with this. ```python @@ -134,7 +134,7 @@ from pyrep.robots.end_effectors.panda_gripper import PandaGripper pr = PyRep() # Launch the application with a scene file that contains a robot -pr.launch('scene_with_panda.ttt') +pr.launch('scene_with_panda.ttt') pr.start() # Start the simulation arm = Panda() # Get the panda from the scene @@ -149,7 +149,7 @@ done = False while not done: done = gripper.actuate(0.5, velocity=0.04) pr.step() - + pr.stop() # Stop the simulation pr.shutdown() # Close the application ``` diff --git a/docs/source/conf.py b/docs/source/conf.py index 4cf517f..23ebac8 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -12,14 +12,15 @@ # import os import sys -sys.path.insert(0, os.path.abspath('../../')) + +sys.path.insert(0, os.path.abspath("../../")) # -- Project information ----------------------------------------------------- -project = 'PyRep' -copyright = '2019, Stephen James' -author = 'Stephen James' +project = "PyRep" +copyright = "2019, Stephen James" +author = "Stephen James" # -- General configuration --------------------------------------------------- @@ -27,16 +28,12 @@ # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. -extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.coverage', - 'sphinx.ext.napoleon' -] +extensions = ["sphinx.ext.autodoc", "sphinx.ext.coverage", "sphinx.ext.napoleon"] autodoc_mock_imports = ["pyrep.backend._sim_cffi"] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. @@ -49,14 +46,14 @@ # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] -master_doc = 'index' +master_doc = "index" # -- Extension configuration ------------------------------------------------- diff --git a/examples/example_baxter_pick_and_pass.py b/examples/example_baxter_pick_and_pass.py index b957874..ef5bfd1 100644 --- a/examples/example_baxter_pick_and_pass.py +++ b/examples/example_baxter_pick_and_pass.py @@ -12,7 +12,7 @@ from pyrep.objects.dummy import Dummy from pyrep.objects.shape import Shape -SCENE_FILE = join(dirname(abspath(__file__)), 'scene_baxter_pick_and_pass.ttt') +SCENE_FILE = join(dirname(abspath(__file__)), "scene_baxter_pick_and_pass.ttt") pr = PyRep() pr.launch(SCENE_FILE, headless=False) @@ -23,67 +23,73 @@ baxter_gripper_left = BaxterGripper(0) baxter_gripper_right = BaxterGripper(1) -cup = Shape('Cup') -waypoints = [Dummy('waypoint%d' % i) for i in range(7)] +cup = Shape("Cup") +waypoints = [Dummy("waypoint%d" % i) for i in range(7)] -print('Planning path for left arm to cup ...') -path = baxter_left.get_path(position=waypoints[0].get_position(), - quaternion=waypoints[0].get_quaternion()) +print("Planning path for left arm to cup ...") +path = baxter_left.get_path( + position=waypoints[0].get_position(), quaternion=waypoints[0].get_quaternion() +) path.visualize() # Let's see what the path looks like -print('Executing plan ...') +print("Executing plan ...") done = False while not done: done = path.step() pr.step() path.clear_visualization() -print('Planning path closer to cup ...') -path = baxter_left.get_path(position=waypoints[1].get_position(), - quaternion=waypoints[1].get_quaternion()) -print('Executing plan ...') +print("Planning path closer to cup ...") +path = baxter_left.get_path( + position=waypoints[1].get_position(), quaternion=waypoints[1].get_quaternion() +) +print("Executing plan ...") done = False while not done: done = path.step() pr.step() -print('Closing left gripper ...') +print("Closing left gripper ...") while not baxter_gripper_left.actuate(0.0, 0.4): pr.step() baxter_gripper_left.grasp(cup) -print('Planning path to lift cup ...') -path = baxter_left.get_path(position=waypoints[2].get_position(), - quaternion=waypoints[2].get_quaternion(), - ignore_collisions=True) -print('Executing plan ...') +print("Planning path to lift cup ...") +path = baxter_left.get_path( + position=waypoints[2].get_position(), + quaternion=waypoints[2].get_quaternion(), + ignore_collisions=True, +) +print("Executing plan ...") done = False while not done: done = path.step() pr.step() -print('Planning path for right arm to cup ...') -path = baxter_right.get_path(position=waypoints[3].get_position(), - quaternion=waypoints[3].get_quaternion()) -print('Executing Plan ...') +print("Planning path for right arm to cup ...") +path = baxter_right.get_path( + position=waypoints[3].get_position(), quaternion=waypoints[3].get_quaternion() +) +print("Executing Plan ...") done = False while not done: done = path.step() pr.step() -print('Planning path closer to cup ...') -path = baxter_right.get_path(position=waypoints[4].get_position(), - quaternion=waypoints[4].get_quaternion()) -print('Executing plan ...') +print("Planning path closer to cup ...") +path = baxter_right.get_path( + position=waypoints[4].get_position(), quaternion=waypoints[4].get_quaternion() +) +print("Executing plan ...") done = False while not done: done = path.step() pr.step() -print('Closing right gripper ...') +print("Closing right gripper ...") while not baxter_gripper_right.actuate(0.0, 0.4): pr.step() -print('Opening left gripper ...') +print("Opening left gripper ...") while not baxter_gripper_left.actuate(1.0, 0.4): pr.step() @@ -92,15 +98,17 @@ baxter_gripper_right.grasp(cup) pr.step() -print('Planning path for left arm to home position ...') -path_l = baxter_left.get_path(position=waypoints[5].get_position(), - quaternion=waypoints[5].get_quaternion()) +print("Planning path for left arm to home position ...") +path_l = baxter_left.get_path( + position=waypoints[5].get_position(), quaternion=waypoints[5].get_quaternion() +) -print('Planning path for right arm to home position ...') -path_r = baxter_right.get_path(position=waypoints[6].get_position(), - quaternion=waypoints[6].get_quaternion()) +print("Planning path for right arm to home position ...") +path_r = baxter_right.get_path( + position=waypoints[6].get_position(), quaternion=waypoints[6].get_quaternion() +) -print('Executing plan on both arms ...') +print("Executing plan on both arms ...") done_l = done_r = False while not done_l or not done_r: if not done_l: @@ -109,7 +117,7 @@ done_r = path_r.step() pr.step() -print('Done ...') -input('Press enter to finish ...') +print("Done ...") +input("Press enter to finish ...") pr.stop() pr.shutdown() diff --git a/examples/example_locobot_stack_cube.py b/examples/example_locobot_stack_cube.py index 3a849b5..38dba6b 100644 --- a/examples/example_locobot_stack_cube.py +++ b/examples/example_locobot_stack_cube.py @@ -12,7 +12,7 @@ from pyrep.objects.shape import Shape from pyrep.objects.dummy import Dummy -SCENE_FILE = join(dirname(abspath(__file__)), 'scene_locobot_stack_cube.ttt') +SCENE_FILE = join(dirname(abspath(__file__)), "scene_locobot_stack_cube.ttt") pr = PyRep() pr.launch(SCENE_FILE, headless=False) pr.start() @@ -35,9 +35,9 @@ def drive_to_position(position, orientation): def move_arm(position, quaternion, ignore_collisions=False): - arm_path = arm.get_path(position, - quaternion=quaternion, - ignore_collisions=ignore_collisions) + arm_path = arm.get_path( + position, quaternion=quaternion, ignore_collisions=ignore_collisions + ) arm_path.visualize() done = False while not done: @@ -46,51 +46,51 @@ def move_arm(position, quaternion, ignore_collisions=False): arm_path.clear_visualization() -cuboid = Shape('cuboid') -goal = Shape('goal') -grasp_point = Dummy('grasp_point') +cuboid = Shape("cuboid") +goal = Shape("goal") +grasp_point = Dummy("grasp_point") drive_pos = cuboid.get_position() drive_pos[1] -= 0.3 -print('Driving to cube ...') +print("Driving to cube ...") drive_to_position(drive_pos, 0) grasp_point_raised = grasp_point.get_position() grasp_point_raised[2] += 0.075 -print('Move arm above cube ...') +print("Move arm above cube ...") move_arm(grasp_point_raised, grasp_point.get_quaternion()) -print('Arm approach cube ...') +print("Arm approach cube ...") move_arm(grasp_point.get_position(), grasp_point.get_quaternion(), True) -print('Closing gripper ...') +print("Closing gripper ...") while not gripper.actuate(0.0, 0.4): pr.step() gripper.grasp(cuboid) -print('Lift cube ...') +print("Lift cube ...") move_arm(grasp_point_raised, grasp_point.get_quaternion(), True) drive_pos = goal.get_position() drive_pos[1] -= 0.35 -print('Driving to goal ...') +print("Driving to goal ...") drive_to_position(drive_pos, 0) goal_point_raised = goal.get_position() goal_point_raised[2] += 0.05 -print('Move arm above goal ...') +print("Move arm above goal ...") move_arm(goal_point_raised, grasp_point.get_quaternion()) -print('Drop cube ...') +print("Drop cube ...") gripper.release() while not gripper.actuate(1.0, 0.4): pr.step() -print('Done!') +print("Done!") pr.stop() pr.shutdown() diff --git a/examples/example_panda_end_effector_control.py b/examples/example_panda_end_effector_control.py index 52961af..89f869c 100644 --- a/examples/example_panda_end_effector_control.py +++ b/examples/example_panda_end_effector_control.py @@ -8,7 +8,7 @@ from pyrep import PyRep from pyrep.robots.arms.panda import Panda -SCENE_FILE = join(dirname(abspath(__file__)), 'scene_panda_reach_target.ttt') +SCENE_FILE = join(dirname(abspath(__file__)), "scene_panda_reach_target.ttt") DELTA = 0.01 pr = PyRep() pr.launch(SCENE_FILE, headless=False) diff --git a/examples/example_panda_ik.py b/examples/example_panda_ik.py index 384250d..d2f80a0 100644 --- a/examples/example_panda_ik.py +++ b/examples/example_panda_ik.py @@ -8,7 +8,7 @@ from pyrep.errors import IKError from pyrep.robots.arms.panda import Panda -SCENE_FILE = join(dirname(abspath(__file__)), 'scene_panda_reach_target.ttt') +SCENE_FILE = join(dirname(abspath(__file__)), "scene_panda_reach_target.ttt") pr = PyRep() pr.launch(SCENE_FILE, headless=False, responsive_ui=True) pr.start() @@ -26,13 +26,15 @@ except IKError: # So let's swap to an alternative IK method... # This returns 'max_configs' number of joint positions - input('Press key to run solve_ik_via_sampling...') - new_joint_pos = agent.solve_ik_via_sampling([x, y, z - 0.4], quaternion=q, max_time_ms=2000)[0] + input("Press key to run solve_ik_via_sampling...") + new_joint_pos = agent.solve_ik_via_sampling( + [x, y, z - 0.4], quaternion=q, max_time_ms=2000 + )[0] # Because the arm is in Forxe/Torque mode, we need to temporarily disable # dynamics in order to instantaneously move joints. agent.set_joint_positions(new_joint_pos, disable_dynamics=True) -input('Press any key to finish...') +input("Press any key to finish...") pr.stop() pr.shutdown() diff --git a/examples/example_panda_reach_target.py b/examples/example_panda_reach_target.py index 4436a60..9b7a060 100644 --- a/examples/example_panda_reach_target.py +++ b/examples/example_panda_reach_target.py @@ -14,24 +14,26 @@ import math LOOPS = 10 -SCENE_FILE = join(dirname(abspath(__file__)), 'scene_panda_reach_target.ttt') +SCENE_FILE = join(dirname(abspath(__file__)), "scene_panda_reach_target.ttt") pr = PyRep() pr.launch(SCENE_FILE, headless=False) pr.start() agent = Panda() # We could have made this target in the scene, but lets create one dynamically -target = Shape.create(type=PrimitiveShape.SPHERE, - size=[0.05, 0.05, 0.05], - color=[1.0, 0.1, 0.1], - static=True, respondable=False) +target = Shape.create( + type=PrimitiveShape.SPHERE, + size=[0.05, 0.05, 0.05], + color=[1.0, 0.1, 0.1], + static=True, + respondable=False, +) position_min, position_max = [0.8, -0.2, 1.0], [1.0, 0.2, 1.4] starting_joint_positions = agent.get_joint_positions() for i in range(LOOPS): - # Reset the arm at the start of each 'episode' agent.set_joint_positions(starting_joint_positions) @@ -41,10 +43,9 @@ # Get a path to the target (rotate so z points down) try: - path = agent.get_path( - position=pos, euler=[0, math.radians(180), 0]) - except ConfigurationPathError as e: - print('Could not find path') + path = agent.get_path(position=pos, euler=[0, math.radians(180), 0]) + except ConfigurationPathError: + print("Could not find path") continue # Step the simulation and advance the agent along the path @@ -53,7 +54,7 @@ done = path.step() pr.step() - print('Reached target %d!' % i) + print("Reached target %d!" % i) pr.stop() pr.shutdown() diff --git a/examples/example_reinforcement_learning_env.py b/examples/example_reinforcement_learning_env.py index 92ae497..aed5d1e 100644 --- a/examples/example_reinforcement_learning_env.py +++ b/examples/example_reinforcement_learning_env.py @@ -13,15 +13,13 @@ from pyrep.objects.shape import Shape import numpy as np -SCENE_FILE = join(dirname(abspath(__file__)), - 'scene_reinforcement_learning_env.ttt') +SCENE_FILE = join(dirname(abspath(__file__)), "scene_reinforcement_learning_env.ttt") POS_MIN, POS_MAX = [0.8, -0.2, 1.0], [1.0, 0.2, 1.4] EPISODES = 5 EPISODE_LENGTH = 200 class ReacherEnv(object): - def __init__(self): self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) @@ -29,15 +27,19 @@ def __init__(self): self.agent = Panda() self.agent.set_control_loop_enabled(False) self.agent.set_motor_locked_at_zero_velocity(True) - self.target = Shape('target') + self.target = Shape("target") self.agent_ee_tip = self.agent.get_tip() self.initial_joint_positions = self.agent.get_joint_positions() def _get_state(self): # Return state containing arm joint angles/velocities & target position - return np.concatenate([self.agent.get_joint_positions(), - self.agent.get_joint_velocities(), - self.target.get_position()]) + return np.concatenate( + [ + self.agent.get_joint_positions(), + self.agent.get_joint_velocities(), + self.target.get_position(), + ] + ) def reset(self): # Get a random position within a cuboid and set the target position @@ -61,7 +63,6 @@ def shutdown(self): class Agent(object): - def act(self, state): del state return list(np.random.uniform(-1.0, 1.0, size=(7,))) @@ -76,8 +77,7 @@ def learn(self, replay_buffer): replay_buffer = [] for e in range(EPISODES): - - print('Starting episode %d' % e) + print("Starting episode %d" % e) state = env.reset() for i in range(EPISODE_LENGTH): action = agent.act(state) @@ -86,5 +86,5 @@ def learn(self, replay_buffer): state = next_state agent.learn(replay_buffer) -print('Done!') +print("Done!") env.shutdown() diff --git a/examples/example_turtlebot_navigation.py b/examples/example_turtlebot_navigation.py index f575571..af0f513 100644 --- a/examples/example_turtlebot_navigation.py +++ b/examples/example_turtlebot_navigation.py @@ -11,17 +11,20 @@ import numpy as np LOOPS = 4 -SCENE_FILE = join(dirname(abspath(__file__)), 'scene_turtlebot_navigation.ttt') +SCENE_FILE = join(dirname(abspath(__file__)), "scene_turtlebot_navigation.ttt") pr = PyRep() pr.launch(SCENE_FILE, headless=False) pr.start() agent = TurtleBot() # We could have made this target in the scene, but lets create one dynamically -target = Shape.create(type=PrimitiveShape.SPHERE, - size=[0.05, 0.05, 0.05], - color=[1.0, 0.1, 0.1], - static=True, respondable=False) +target = Shape.create( + type=PrimitiveShape.SPHERE, + size=[0.05, 0.05, 0.05], + color=[1.0, 0.1, 0.1], + static=True, + respondable=False, +) position_min, position_max = [-0.5, 1, 0.1], [0.5, 1.5, 0.1] @@ -47,7 +50,7 @@ path.clear_visualization() - print('Reached target %d!' % i) + print("Reached target %d!" % i) pr.stop() pr.shutdown() diff --git a/examples/example_youbot_navigation.py b/examples/example_youbot_navigation.py index 0aac66f..063de74 100644 --- a/examples/example_youbot_navigation.py +++ b/examples/example_youbot_navigation.py @@ -11,17 +11,20 @@ import numpy as np LOOPS = 4 -SCENE_FILE = join(dirname(abspath(__file__)), 'scene_youbot_navigation.ttt') +SCENE_FILE = join(dirname(abspath(__file__)), "scene_youbot_navigation.ttt") pr = PyRep() pr.launch(SCENE_FILE, headless=False) pr.start() agent = YouBot() # We could have made this target in the scene, but lets create one dynamically -target = Shape.create(type=PrimitiveShape.SPHERE, - size=[0.05, 0.05, 0.05], - color=[1.0, 0.1, 0.1], - static=True, respondable=False) +target = Shape.create( + type=PrimitiveShape.SPHERE, + size=[0.05, 0.05, 0.05], + color=[1.0, 0.1, 0.1], + static=True, + respondable=False, +) position_min, position_max = [-0.5, 1.4, 0.1], [1.0, 0.5, 0.1] @@ -45,7 +48,7 @@ path.clear_visualization() - print('Reached target %d!' % i) + print("Reached target %d!" % i) pr.stop() pr.shutdown() diff --git a/pyrep/__init__.py b/pyrep/__init__.py index ff45d68..023fcd5 100644 --- a/pyrep/__init__.py +++ b/pyrep/__init__.py @@ -1,5 +1,7 @@ -__version__ = '4.6.0' +from pyrep.pyrep import PyRep -testing = False +__version__ = "4.6.0" + +__all__ = ["PyRep"] -from .pyrep import PyRep +testing = False diff --git a/pyrep/backend/bridge.py b/pyrep/backend/bridge.py index efc88ab..9d186a6 100644 --- a/pyrep/backend/bridge.py +++ b/pyrep/backend/bridge.py @@ -2,7 +2,7 @@ def load(): - call('require', ('scriptClientBridge',)) + call("require", ("scriptClientBridge",)) def call(func, args): @@ -13,17 +13,18 @@ def call(func, args): sim_scripttype_sandboxscript, ) import pyrep.backend.stack as stack + stackHandle = simCreateStack() stack.write(stackHandle, args) s = sim_scripttype_sandboxscript - f = ctypes.c_char_p(f'{func}@lua'.encode('ascii')) + f = ctypes.c_char_p(f"{func}@lua".encode("ascii")) r = simCallScriptFunctionEx(s, f, stackHandle) if r == -1: if False: - what = f'simCallScriptFunctionEx({s}, {func!r}, {args!r})' + what = f"simCallScriptFunctionEx({s}, {func!r}, {args!r})" else: - what = 'simCallScriptFunctionEx' - raise Exception(f'{what} returned -1') + what = "simCallScriptFunctionEx" + raise Exception(f"{what} returned -1") ret = stack.read(stackHandle) simReleaseStack(stackHandle) if len(ret) == 1: @@ -35,29 +36,35 @@ def call(func, args): def getObject(name, _info=None): ret = type(name, (), {}) if not _info: - _info = call('scriptClientBridge.info', [name]) + _info = call("scriptClientBridge.info", [name]) for k, v in _info.items(): if not isinstance(v, dict): - raise ValueError('found nondict') - if len(v) == 1 and 'func' in v: - if f'{name}.{k}' == 'sim.getScriptFunctions': - setattr(ret, k, lambda scriptHandle: - type('', (object,), { - '__getattr__': - lambda _, func: - lambda *args: - call('sim.callScriptFunction', (func, scriptHandle) + args) - })()) + raise ValueError("found nondict") + if len(v) == 1 and "func" in v: + if f"{name}.{k}" == "sim.getScriptFunctions": + setattr( + ret, + k, + lambda scriptHandle: type( + "", + (object,), + { + "__getattr__": lambda _, func: lambda *args: call( + "sim.callScriptFunction", (func, scriptHandle) + args + ) + }, + )(), + ) continue - setattr(ret, k, lambda *a, func=f'{name}.{k}': call(func, a)) - elif len(v) == 1 and 'const' in v: - setattr(ret, k, v['const']) + setattr(ret, k, lambda *a, func=f"{name}.{k}": call(func, a)) + elif len(v) == 1 and "const" in v: + setattr(ret, k, v["const"]) else: - setattr(ret, k, getObject(f'{name}.{k}', _info=v)) + setattr(ret, k, getObject(f"{name}.{k}", _info=v)) return ret def require(obj): - call('scriptClientBridge.require', [obj]) + call("scriptClientBridge.require", [obj]) o = getObject(obj) return o diff --git a/pyrep/backend/callback.py b/pyrep/backend/callback.py index c3720a8..c18a5bb 100644 --- a/pyrep/backend/callback.py +++ b/pyrep/backend/callback.py @@ -1,6 +1,7 @@ def callback(f): def wrapper(stackHandle): import pyrep.backend.stack as stack + try: inArgs = stack.read(stackHandle) outArgs = f(*inArgs) @@ -12,6 +13,8 @@ def wrapper(stackHandle): return 1 except Exception: import traceback + traceback.print_exc() return 0 - return wrapper \ No newline at end of file + + return wrapper diff --git a/pyrep/backend/lib.py b/pyrep/backend/lib.py index 7977e39..a4497d6 100644 --- a/pyrep/backend/lib.py +++ b/pyrep/backend/lib.py @@ -5,6 +5,7 @@ from ctypes import CFUNCTYPE from pyrep.errors import PyRepError +import platform c_void = None @@ -16,25 +17,27 @@ c_callbackfn_p = CFUNCTYPE(c_int, c_int) -if 'COPPELIASIM_ROOT' not in os.environ: - raise PyRepError( - 'COPPELIASIM_ROOT not defined. See installation instructions.') -coppeliasim_root = os.environ['COPPELIASIM_ROOT'] +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") if not os.path.isfile(coppeliasim_library): raise PyRepError( - 'COPPELIASIM_ROOT was not a correct path. ' - 'See installation instructions') + "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"] = appDir + -import platform plat = platform.system() -if plat == 'Darwin': - fd = os.path.normpath(appDir + '/../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}:$DYLD_LIBRARY_PATH and relaunch.') +if plat == "Darwin": + fd = os.path.normpath(appDir + "/../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}:" + "$DYLD_LIBRARY_PATH and relaunch." + ) coppeliaSimLib = cdll.LoadLibrary(coppeliasim_library) coppeliaSimLib.simRunGui.argtypes = [c_int] @@ -123,13 +126,13 @@ __all__ = [] for name in dir(coppeliaSimLib): - if name.startswith('sim'): + if name.startswith("sim"): f = getattr(coppeliaSimLib, name) if callable(f): globals()[name] = f __all__.append(name) -const = type('', (), {}) +const = type("", (), {}) const.sim_stackitem_null = 0 const.sim_stackitem_double = 1 @@ -153,7 +156,7 @@ const.sim_scripttype_customizationscript = 6 const.sim_scripttype_sandboxscript = 8 -const.sim_gui_all = 0x0ffff +const.sim_gui_all = 0x0FFFF const.sim_gui_headless = 0x10000 const.sim_stringparam_app_arg1 = 2 @@ -165,7 +168,7 @@ const.sim_stringparam_startupscriptstring = 125 for name in dir(const): - if name.startswith('sim'): + if name.startswith("sim"): f = getattr(const, name) globals()[name] = f - __all__.append(name) \ No newline at end of file + __all__.append(name) diff --git a/pyrep/backend/sim.py b/pyrep/backend/sim.py index aee9471..c1567f5 100644 --- a/pyrep/backend/sim.py +++ b/pyrep/backend/sim.py @@ -6,13 +6,17 @@ from pyrep.backend import lib from pyrep.backend import bridge -from pyrep.backend.sim_const import sim_gui_all, sim_gui_headless, \ - sim_stringparam_verbosity, sim_stringparam_statusbarverbosity +from pyrep.backend.sim_const import ( + sim_gui_all, + sim_gui_headless, + sim_stringparam_verbosity, + sim_stringparam_statusbarverbosity, +) sim_api = None -class SimBackend: +class SimBackend: _instance = None def __new__(cls): @@ -43,24 +47,28 @@ def lib(self): return lib def simInitialize(self, appDir: str, verbosity: str): - lib.simSetStringParam(sim_stringparam_verbosity, c_char_p(verbosity.encode('utf-8'))) - lib.simSetStringParam(sim_stringparam_statusbarverbosity, c_char_p(verbosity.encode('utf-8'))) - lib.simInitialize(c_char_p(appDir.encode('utf-8')), 0) + lib.simSetStringParam( + sim_stringparam_verbosity, c_char_p(verbosity.encode("utf-8")) + ) + lib.simSetStringParam( + sim_stringparam_statusbarverbosity, c_char_p(verbosity.encode("utf-8")) + ) + lib.simInitialize(c_char_p(appDir.encode("utf-8")), 0) bridge.load() # fetch CoppeliaSim API sim-namespace functions: - self._sim = bridge.require('sim') - self._sim_ik = bridge.require('simIK') - self._sim_ompl = bridge.require('simOMPL') - self._sim_vision = bridge.require('simVision') + self._sim = bridge.require("sim") + self._sim_ik = bridge.require("simIK") + self._sim_ompl = bridge.require("simOMPL") + self._sim_vision = bridge.require("simVision") v = self._sim.getInt32Param(self._sim.intparam_program_full_version) - self._coppelia_version = '.'.join(str(v // 100 ** (3 - i) % 100) for i in range(4)) + self._coppelia_version = ".".join( + str(v // 100 ** (3 - i) % 100) for i in range(4) + ) return self._sim def create_ui_thread(self, headless: bool) -> threading.Thread: options = sim_gui_headless if headless else sim_gui_all - ui_thread = threading.Thread( - target=lib.simRunGui, - args=(options,)) + ui_thread = threading.Thread(target=lib.simRunGui, args=(options,)) ui_thread.daemon = True return ui_thread diff --git a/pyrep/backend/sim_const.py b/pyrep/backend/sim_const.py index c02a6b3..ba79564 100755 --- a/pyrep/backend/sim_const.py +++ b/pyrep/backend/sim_const.py @@ -1726,4 +1726,4 @@ sim_pathproperty_invert_velocity = 16 sim_scripttype_contactcallback = 5 sim_scripttype_generalcallback = 7 -sim_scripttype_jointctrlcallback = 4 \ No newline at end of file +sim_scripttype_jointctrlcallback = 4 diff --git a/pyrep/backend/stack.py b/pyrep/backend/stack.py index 4d32beb..3b078c7 100644 --- a/pyrep/backend/stack.py +++ b/pyrep/backend/stack.py @@ -7,11 +7,12 @@ def read_null(stackHandle): simPopStackItem, sim_stackitem_null, ) + if simGetStackItemType(stackHandle, -1) == sim_stackitem_null: simPopStackItem(stackHandle, 1) return None else: - raise RuntimeError('expected nil') + raise RuntimeError("expected nil") def read_bool(stackHandle): @@ -19,12 +20,13 @@ def read_bool(stackHandle): simGetStackBoolValue, simPopStackItem, ) + value = ctypes.c_bool() if simGetStackBoolValue(stackHandle, ctypes.byref(value)) == 1: simPopStackItem(stackHandle, 1) return value.value else: - raise RuntimeError('expected bool') + raise RuntimeError("expected bool") def read_int(stackHandle): @@ -32,12 +34,13 @@ def read_int(stackHandle): simGetStackInt32Value, simPopStackItem, ) + value = ctypes.c_int() if simGetStackInt32Value(stackHandle, ctypes.byref(value)) == 1: simPopStackItem(stackHandle, 1) return value.value else: - raise RuntimeError('expected int') + raise RuntimeError("expected int") def read_long(stackHandle): @@ -45,12 +48,13 @@ def read_long(stackHandle): simGetStackInt64Value, simPopStackItem, ) + value = ctypes.c_longlong() if simGetStackInt64Value(stackHandle, ctypes.byref(value)) == 1: simPopStackItem(stackHandle, 1) return value.value else: - raise RuntimeError('expected int64') + raise RuntimeError("expected int64") def read_double(stackHandle): @@ -58,12 +62,13 @@ def read_double(stackHandle): simGetStackDoubleValue, simPopStackItem, ) + value = ctypes.c_double() if simGetStackDoubleValue(stackHandle, ctypes.byref(value)) == 1: simPopStackItem(stackHandle, 1) return value.value else: - raise RuntimeError('expected double') + raise RuntimeError("expected double") def read_string(stackHandle): @@ -72,11 +77,12 @@ def read_string(stackHandle): simReleaseBuffer, simPopStackItem, ) + string_size = ctypes.c_int() string_ptr = simGetStackStringValue(stackHandle, ctypes.byref(string_size)) string_value = ctypes.string_at(string_ptr, string_size.value) simPopStackItem(stackHandle, 1) - value = string_value.decode('utf-8') + value = string_value.decode("utf-8") simReleaseBuffer(string_ptr) return value @@ -90,10 +96,11 @@ def read_dict(stackHandle): sim_stack_table_map, sim_stack_table_empty, ) + d = dict() info = simGetStackTableInfo(stackHandle, 0) if info != sim_stack_table_map and info != sim_stack_table_empty: - raise RuntimeError('expected a map') + raise RuntimeError("expected a map") oldsz = simGetStackSize(stackHandle) simUnfoldStackTable(stackHandle) n = (simGetStackSize(stackHandle) - oldsz + 1) // 2 @@ -111,7 +118,8 @@ def read_list(stackHandle): simUnfoldStackTable, simMoveStackItemToTop, ) - l = list() + + vals = list() oldsz = simGetStackSize(stackHandle) simUnfoldStackTable(stackHandle) n = (simGetStackSize(stackHandle) - oldsz + 1) // 2 @@ -119,8 +127,8 @@ def read_list(stackHandle): simMoveStackItemToTop(stackHandle, oldsz - 1) read_value(stackHandle) simMoveStackItemToTop(stackHandle, oldsz - 1) - l.append(read_value(stackHandle)) - return l + vals.append(read_value(stackHandle)) + return vals def read_table(stackHandle): @@ -129,6 +137,7 @@ def read_table(stackHandle): sim_stack_table_map, sim_stack_table_empty, ) + sz = simGetStackTableInfo(stackHandle, 0) if sz >= 0: return read_list(stackHandle) @@ -146,6 +155,7 @@ def read_value(stackHandle): sim_stackitem_table, sim_stackitem_integer, ) + item_type = simGetStackItemType(stackHandle, -1) if item_type == sim_stackitem_null: value = read_null(stackHandle) @@ -160,7 +170,7 @@ def read_value(stackHandle): elif item_type == sim_stackitem_integer: value = read_long(stackHandle) else: - raise RuntimeError(f'unexpected stack item type: {item_type}') + raise RuntimeError(f"unexpected stack item type: {item_type}") return value @@ -170,12 +180,13 @@ def read(stackHandle): simMoveStackItemToTop, simPopStackItem, ) + stack_size = simGetStackSize(stackHandle) tuple_data = [] for i in range(stack_size): simMoveStackItemToTop(stackHandle, 0) tuple_data.append(read_value(stackHandle)) - simPopStackItem(stackHandle, 0) # clear all + simPopStackItem(stackHandle, 0) # clear all return tuple(tuple_data) @@ -183,6 +194,7 @@ def write_null(stackHandle, value): from pyrep.backend.lib import ( simPushNullOntoStack, ) + simPushNullOntoStack(stackHandle) @@ -190,6 +202,7 @@ def write_double(stackHandle, value): from pyrep.backend.lib import ( simPushDoubleOntoStack, ) + simPushDoubleOntoStack(stackHandle, value) @@ -197,6 +210,7 @@ def write_bool(stackHandle, value): from pyrep.backend.lib import ( simPushBoolOntoStack, ) + simPushBoolOntoStack(stackHandle, value) @@ -204,6 +218,7 @@ def write_int(stackHandle, value): from pyrep.backend.lib import ( simPushInt32OntoStack, ) + simPushInt32OntoStack(stackHandle, value) @@ -211,7 +226,8 @@ def write_string(stackHandle, value): from pyrep.backend.lib import ( simPushStringOntoStack, ) - simPushStringOntoStack(stackHandle, value.encode('utf-8'), len(value)) + + simPushStringOntoStack(stackHandle, value.encode("utf-8"), len(value)) def write_dict(stackHandle, value): @@ -219,6 +235,7 @@ def write_dict(stackHandle, value): simPushTableOntoStack, simInsertDataIntoStackTable, ) + simPushTableOntoStack(stackHandle) for k, v in value.items(): write_value(stackHandle, k) @@ -231,6 +248,7 @@ def write_list(stackHandle, value): simPushTableOntoStack, simInsertDataIntoStackTable, ) + simPushTableOntoStack(stackHandle) for i, v in enumerate(value): write_value(stackHandle, i + 1) @@ -254,7 +272,7 @@ def write_value(stackHandle, value): elif isinstance(value, list): write_list(stackHandle, value) else: - raise RuntimeError(f'unexpected type: {type(value)}') + raise RuntimeError(f"unexpected type: {type(value)}") def write(stackHandle, tuple_data): @@ -262,15 +280,16 @@ def write(stackHandle, tuple_data): write_value(stackHandle, item) -def debug(stackHandle, info = None): +def debug(stackHandle, info=None): from pyrep.backend.lib import ( simGetStackSize, simDebugStack, ) - info = '' if info is None else f' {info} ' + + info = "" if info is None else f" {info} " n = (70 - len(info)) // 2 m = 70 - len(info) - n - print('#' * n + info + '#' * m) + print("#" * n + info + "#" * m) for i in range(simGetStackSize(stackHandle)): simDebugStack(stackHandle, i) - print('#' * 70) \ No newline at end of file + print("#" * 70) diff --git a/pyrep/backend/utils.py b/pyrep/backend/utils.py index 4b80764..24ce4a1 100644 --- a/pyrep/backend/utils.py +++ b/pyrep/backend/utils.py @@ -45,10 +45,14 @@ def to_type(handle: int) -> Object: raise ValueError -def script_call(function_name_at_script_name: str, - script_handle_or_type: int, - ints=(), floats=(), strings=(), bytes='') -> ( - Tuple[List[int], List[float], List[str], str]): +def script_call( + function_name_at_script_name: str, + script_handle_or_type: int, + ints=(), + floats=(), + strings=(), + bytes="", +) -> Tuple[List[int], List[float], List[str], str]: """Calls a script function (from a plugin, the main client application, or from another script). This represents a callback inside of a script. @@ -66,12 +70,18 @@ def script_call(function_name_at_script_name: str, """ sim_api = SimBackend().sim_api return sim_api.extCallScriptFunction( - function_name_at_script_name, script_handle_or_type, list(ints), - list(floats), list(strings), bytes) + function_name_at_script_name, + script_handle_or_type, + list(ints), + list(floats), + list(strings), + bytes, + ) def _is_in_ipython(): import builtins + try: return getattr(builtins, "__IPYTHON__", False) except NameError: diff --git a/pyrep/const.py b/pyrep/const.py index 708fbbd..ce2d7c5 100644 --- a/pyrep/const.py +++ b/pyrep/const.py @@ -42,31 +42,31 @@ class JointMode(Enum): class ConfigurationPathAlgorithms(Enum): - BiTRRT = 'BiTRRT' - BITstar = 'BITstar' - BKPIECE1 = 'BKPIECE1' - CForest = 'CForest' - EST = 'EST' - FMT = 'FMT' - KPIECE1 = 'KPIECE1' - LazyPRM = 'LazyPRM' - LazyPRMstar = 'LazyPRMstar' - LazyRRT = 'LazyRRT' - LBKPIECE1 = 'LBKPIECE1' - LBTRRT = 'LBTRRT' - PDST = 'PDST' - PRM = 'PRM' - PRMstar = 'PRMstar' - pRRT = 'pRRT' - pSBL = 'pSBL' - RRT = 'RRT' - RRTConnect = 'RRTConnect' - RRTstar = 'RRTstar' - SBL = 'SBL' - SPARS = 'SPARS' - SPARStwo = 'SPARStwo' - STRIDE = 'STRIDE' - TRRT = 'TRRT' + BiTRRT = "BiTRRT" + BITstar = "BITstar" + BKPIECE1 = "BKPIECE1" + CForest = "CForest" + EST = "EST" + FMT = "FMT" + KPIECE1 = "KPIECE1" + LazyPRM = "LazyPRM" + LazyPRMstar = "LazyPRMstar" + LazyRRT = "LazyRRT" + LBKPIECE1 = "LBKPIECE1" + LBTRRT = "LBTRRT" + PDST = "PDST" + PRM = "PRM" + PRMstar = "PRMstar" + pRRT = "pRRT" + pSBL = "pSBL" + RRT = "RRT" + RRTConnect = "RRTConnect" + RRTstar = "RRTstar" + SBL = "SBL" + SPARS = "SPARS" + SPARStwo = "SPARStwo" + STRIDE = "STRIDE" + TRRT = "TRRT" class TextureMappingMode(Enum): @@ -93,18 +93,18 @@ class RenderMode(Enum): class Verbosity(Enum): - NONE = 'none' - ERRORS = 'errors' - WARNINGS = 'warnings' - LOAD_INFOS = 'loadinfos' - SCRIPT_ERRORS = 'scripterrors' - SCRIPT_WARNINGS = 'scriptwarnings' - SCRIPT_INFOS = 'scriptinfos' - INFOS = 'infos' - DEBUG = 'debug' - TRACE = 'trace' - TRACE_LUA = 'tracelua' - TYRACE_ALL = 'traceall' + NONE = "none" + ERRORS = "errors" + WARNINGS = "warnings" + LOAD_INFOS = "loadinfos" + SCRIPT_ERRORS = "scripterrors" + SCRIPT_WARNINGS = "scriptwarnings" + SCRIPT_INFOS = "scriptinfos" + INFOS = "infos" + DEBUG = "debug" + TRACE = "trace" + TRACE_LUA = "tracelua" + TYRACE_ALL = "traceall" PYREP_SCRIPT_TYPE = sim.sim_scripttype_addonscript diff --git a/pyrep/errors.py b/pyrep/errors.py index 81309b5..3c4f088 100644 --- a/pyrep/errors.py +++ b/pyrep/errors.py @@ -29,5 +29,6 @@ class GripperError(Exception): class IKError(Exception): pass + class CoppeliaSimError(Exception): - pass \ No newline at end of file + pass diff --git a/pyrep/misc/signals.py b/pyrep/misc/signals.py index 29bb812..691777d 100644 --- a/pyrep/misc/signals.py +++ b/pyrep/misc/signals.py @@ -1,6 +1,5 @@ from pyrep.backend.sim import SimBackend from pyrep.errors import PyRepError -from pyrep.backend import sim from typing import Any @@ -40,8 +39,9 @@ def clear(self) -> int: def _check_signal(self, value: int, type_name: str) -> None: if value == 0: - raise PyRepError('Signal %s of type %s does not exist.' % ( - self._name, type_name)) + raise PyRepError( + "Signal %s of type %s does not exist." % (self._name, type_name) + ) class IntegerSignal(Signal): diff --git a/pyrep/objects/__init__.py b/pyrep/objects/__init__.py index 6860e72..e69de29 100644 --- a/pyrep/objects/__init__.py +++ b/pyrep/objects/__init__.py @@ -1,10 +0,0 @@ -from pyrep.objects.object import Object -from pyrep.objects.cartesian_path import CartesianPath -from pyrep.objects.dummy import Dummy -from pyrep.objects.force_sensor import ForceSensor -from pyrep.objects.joint import Joint -from pyrep.objects.proximity_sensor import ProximitySensor -from pyrep.objects.shape import Shape -from pyrep.objects.vision_sensor import VisionSensor -from pyrep.objects.octree import Octree -from pyrep.objects.camera import Camera diff --git a/pyrep/objects/camera.py b/pyrep/objects/camera.py index 90536bc..659fa83 100644 --- a/pyrep/objects/camera.py +++ b/pyrep/objects/camera.py @@ -4,12 +4,11 @@ class Camera(Object): - """Cameras can be associated with views. - """ + """Cameras can be associated with views.""" def __init__(self, name_or_handle: Union[str, int]): super().__init__(name_or_handle) - + @staticmethod def create(): raise NotImplementedError("API does not provide simCreateCamera.") diff --git a/pyrep/objects/cartesian_path.py b/pyrep/objects/cartesian_path.py index 2a66cd5..d3536d5 100644 --- a/pyrep/objects/cartesian_path.py +++ b/pyrep/objects/cartesian_path.py @@ -9,17 +9,25 @@ class CartesianPath(Object): - """An object that defines a cartesian path or trajectory in space. - """ + """An object that defines a cartesian path or trajectory in space.""" @staticmethod - def create(show_line: bool = True, show_orientation: bool = True, - show_position: bool = True, closed_path: bool = False, - automatic_orientation: bool = True, flat_path: bool = False, - keep_x_up: bool = False, line_size: int = 1, - length_calculation_method: int = simc.sim_distcalcmethod_dl_if_nonzero, control_point_size: float = 0.01, - ang_to_lin_conv_coeff: float = 1., virt_dist_scale_factor: float = 1., - path_color: tuple = (0.1, 0.75, 1.), paths_points: list = []) -> 'CartesianPath': + def create( + show_line: bool = True, + show_orientation: bool = True, + show_position: bool = True, + closed_path: bool = False, + automatic_orientation: bool = True, + flat_path: bool = False, + keep_x_up: bool = False, + line_size: int = 1, + length_calculation_method: int = simc.sim_distcalcmethod_dl_if_nonzero, + control_point_size: float = 0.01, + ang_to_lin_conv_coeff: float = 1.0, + virt_dist_scale_factor: float = 1.0, + path_color: tuple = (0.1, 0.75, 1.0), + paths_points: list = [], + ) -> "CartesianPath": """Creates a cartesian path and inserts in the scene. :param show_line: Shows line in UI. @@ -53,7 +61,7 @@ def create(show_line: bool = True, show_orientation: bool = True, :return: The newly created cartesian path. """ - attributes = 16 # the path points' orientation is computed according to the orientationMode below + attributes = 16 # path points orientation computed according to orientationMode if closed_path: attributes |= 2 orientation_mode = 0 @@ -69,14 +77,17 @@ def create(show_line: bool = True, show_orientation: bool = True, if len(paths_points) == 0: # Path must have at least 2 points paths_points = np.zeros((14,)).tolist() - handle = sim_api.createPath(paths_points, attributes, subdiv, smoothness, orientation_mode, up_vector) + handle = sim_api.createPath( + paths_points, attributes, subdiv, smoothness, orientation_mode, up_vector + ) return CartesianPath(handle) def _get_requested_type(self) -> ObjectType: return ObjectType.PATH - def get_pose_on_path(self, relative_distance: float - ) -> Tuple[List[float], List[float]]: + def get_pose_on_path( + self, relative_distance: float + ) -> Tuple[List[float], List[float]]: """Retrieves the absolute interpolated pose of a point along the path. :param relative_distance: A value between 0 and 1, where 0 is the @@ -85,15 +96,26 @@ def get_pose_on_path(self, relative_distance: float orientation of the point on the path (in radians). """ sim_api = SimBackend().sim_api - path_data = sim_api.unpackDoubleTable(sim_api.readCustomDataBlock(self.get_handle(), 'PATH')) + path_data = sim_api.unpackDoubleTable( + sim_api.readCustomDataBlock(self.get_handle(), "PATH") + ) m = np.array(path_data).reshape(len(path_data) // 7, 7) path_positions = m[:, :3].flatten().tolist() path_quaternions = m[:, 3:].flatten().tolist() path_lengths, total_length = sim_api.getPathLengths(path_positions, 3) - pos = sim_api.getPathInterpolatedConfig(path_positions, path_lengths, total_length * relative_distance) - quat = sim_api.getPathInterpolatedConfig(path_quaternions, path_lengths, total_length * relative_distance, None, [2, 2, 2, 2]) + pos = sim_api.getPathInterpolatedConfig( + path_positions, path_lengths, total_length * relative_distance + ) + quat = sim_api.getPathInterpolatedConfig( + path_quaternions, + path_lengths, + total_length * relative_distance, + None, + [2, 2, 2, 2], + ) # TODO: Hack for now; convert quat -> euler using library from pyrep.objects import Dummy + d = Dummy.create() d.set_position(pos, self) d.set_quaternion(quat, self) diff --git a/pyrep/objects/dummy.py b/pyrep/objects/dummy.py index 39568c3..f9f308f 100644 --- a/pyrep/objects/dummy.py +++ b/pyrep/objects/dummy.py @@ -1,7 +1,6 @@ from pyrep.backend.sim import SimBackend from pyrep.objects.object import Object, object_type_to_class from pyrep.const import ObjectType -from pyrep.backend import sim class Dummy(Object): @@ -11,7 +10,7 @@ class Dummy(Object): """ @staticmethod - def create(size=0.01) -> 'Dummy': + def create(size=0.01) -> "Dummy": """Creates a dummy object and inserts in the scene. :param size: The size of the dummy object. diff --git a/pyrep/objects/force_sensor.py b/pyrep/objects/force_sensor.py index 13e698f..2447747 100644 --- a/pyrep/objects/force_sensor.py +++ b/pyrep/objects/force_sensor.py @@ -1,20 +1,17 @@ from typing import Tuple, List -from pyrep.backend import sim from pyrep.backend.sim import SimBackend from pyrep.objects.object import Object, object_type_to_class from pyrep.const import ObjectType class ForceSensor(Object): - """An object able to measure forces and torques that are applied to it. - """ + """An object able to measure forces and torques that are applied to it.""" def _get_requested_type(self) -> ObjectType: return ObjectType.FORCE_SENSOR @classmethod - def create(cls, sensor_size=0.01) -> 'ForceSensor': - options = 0 # force and torque threshold are disabled + def create(cls, sensor_size=0.01) -> "ForceSensor": intParams = [0, 0, 0, 0, 0] floatParams = [sensor_size, 0, 0, 0, 0] sim_api = SimBackend().sim_api diff --git a/pyrep/objects/joint.py b/pyrep/objects/joint.py index a4d64a6..d1b3e4a 100644 --- a/pyrep/objects/joint.py +++ b/pyrep/objects/joint.py @@ -38,8 +38,9 @@ def get_joint_position(self) -> float: """ return self._sim_api.getJointPosition(self._handle) - def set_joint_position(self, position: float, - disable_dynamics: bool = False) -> None: + def set_joint_position( + self, position: float, disable_dynamics: bool = False + ) -> None: """Sets the intrinsic position of the joint. :param disable_dynamics: If True, then the position can be set even @@ -149,7 +150,8 @@ def get_joint_velocity(self) -> float: on the joint-type). """ return self._sim_api.getObjectFloatParam( - self._handle, simc.sim_jointfloatparam_velocity) + self._handle, simc.sim_jointfloatparam_velocity + ) def get_joint_interval(self) -> Tuple[bool, List[float]]: """Retrieves the interval parameters of a joint. @@ -184,7 +186,8 @@ def get_joint_upper_velocity_limit(self) -> float: :return: The upper velocity limit. """ return self._sim_api.getObjectFloatParam( - self._handle, simc.sim_jointfloatparam_upper_limit) + self._handle, simc.sim_jointfloatparam_upper_limit + ) def is_control_loop_enabled(self) -> bool: """Gets whether the control loop is enable. @@ -192,7 +195,8 @@ def is_control_loop_enabled(self) -> bool: :return: True if the control loop is enabled. """ return self._sim_api.getObjectInt32Param( - self._handle, simc.sim_jointintparam_ctrl_enabled) + self._handle, simc.sim_jointintparam_ctrl_enabled + ) def set_control_loop_enabled(self, value: bool) -> None: """Sets whether the control loop is enable. @@ -200,7 +204,8 @@ def set_control_loop_enabled(self, value: bool) -> None: :param value: The new value for the control loop state. """ self._sim_api.setObjectInt32Param( - self._handle, simc.sim_jointintparam_ctrl_enabled, int(value)) + self._handle, simc.sim_jointintparam_ctrl_enabled, int(value) + ) def is_motor_enabled(self) -> bool: """Gets whether the motor is enable. @@ -208,7 +213,8 @@ def is_motor_enabled(self) -> bool: :return: True if the motor is enabled. """ return self._sim_api.getObjectInt32Param( - self._handle, simc.sim_jointintparam_motor_enabled) + self._handle, simc.sim_jointintparam_motor_enabled + ) def set_motor_enabled(self, value: bool) -> None: """Sets whether the motor is enable. @@ -216,7 +222,8 @@ def set_motor_enabled(self, value: bool) -> None: :param value: The new value for the motor state. """ self._sim_api.setObjectInt32Param( - self._handle, simc.sim_jointintparam_motor_enabled, int(value)) + self._handle, simc.sim_jointintparam_motor_enabled, int(value) + ) def is_motor_locked_at_zero_velocity(self) -> bool: """Gets if the motor is locked when target velocity is zero. @@ -227,7 +234,8 @@ def is_motor_locked_at_zero_velocity(self) -> bool: :return: If the motor will be locked at zero velocity. """ return self._sim_api.getObjectInt32Param( - self._handle, simc.sim_jointintparam_velocity_lock) + self._handle, simc.sim_jointintparam_velocity_lock + ) def set_motor_locked_at_zero_velocity(self, value: bool) -> None: """Set if the motor is locked when target velocity is zero. @@ -238,7 +246,8 @@ def set_motor_locked_at_zero_velocity(self, value: bool) -> None: :param value: If the motor should be locked at zero velocity. """ self._sim_api.setObjectInt32Param( - self._handle, simc.sim_jointintparam_velocity_lock, int(value)) + self._handle, simc.sim_jointintparam_velocity_lock, int(value) + ) def get_joint_mode(self) -> JointMode: """Retrieves the operation mode of the joint. diff --git a/pyrep/objects/light.py b/pyrep/objects/light.py index 5d89a68..88bd6a8 100644 --- a/pyrep/objects/light.py +++ b/pyrep/objects/light.py @@ -7,8 +7,7 @@ class Light(Object): - """A light. - """ + """A light.""" def __init__(self, name_or_handle: Union[str, int]): super().__init__(name_or_handle) @@ -19,23 +18,21 @@ def _get_requested_type(self) -> ObjectType: # On and Off def turn_on(self): - """ Turn the light on. - """ + """Turn the light on.""" self._sim_api.setLightParameters(self._handle, 1, None, None, None) def turn_off(self): - """ Turn the light off. - """ + """Turn the light off.""" self._sim_api.setLightParameters(self._handle, 0, None, None, None) def is_on(self): - """ Determines whether the light is on. + """Determines whether the light is on. return: Boolean """ return self._sim_api.getLightParameters(self._handle)[0] def is_off(self): - """ Determines whether the light is off. + """Determines whether the light is off. return: Boolean """ return not self._sim_api.getLightParameters(self._handle)[0] @@ -43,76 +40,125 @@ def is_off(self): # Get and Set Color def get_diffuse(self): - """ Get the diffuse colors of the light. + """Get the diffuse colors of the light. return: 3-vector np.array of diffuse colors """ return np.asarray(self._sim_api.getLightParameters(self._handle)[2]) 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) + """Set the diffuse colors of the light.""" + self._sim_api.setLightParameters( + self._handle, self.is_on(), None, list(diffuse), None + ) def get_specular(self): - """ Get the specular colors of the light. + """Get the specular colors of the light. return: 3-vector np.array of specular colors """ return np.asarray(self._sim_api.getLightParameters(self._handle)[3]) def set_specular(self, specular): - """ Set the specular colors of the light. - """ - self._sim_api.setLightParameters(self._handle, self.is_on(), None, list(self.get_diffuse()), list(specular)) + """Set the specular colors of the light.""" + self._sim_api.setLightParameters( + self._handle, self.is_on(), None, list(self.get_diffuse()), list(specular) + ) # Intensity Properties def get_intensity_properties(self): - """ Gets light intensity properties. + """Gets light intensity properties. - :return: The light intensity properties cast_shadows, spot_exponent, spot_cutoff, const_atten_factor, + :return: The light intensity properties cast_shadows, + spot_exponent, spot_cutoff, const_atten_factor, linear_atten_factor, quad_atten_factor """ - cast_shadows = self._sim_api.getObjectInt32Param(self._handle, simc.sim_lightintparam_pov_casts_shadows) - spot_exponent = self._sim_api.getObjectFloatParam(self._handle, simc.sim_lightfloatparam_spot_exponent) - spot_cutoff = self._sim_api.getObjectFloatParam(self._handle, simc.sim_lightfloatparam_spot_cutoff) - const_atten_factor = self._sim_api.getObjectFloatParam(self._handle, simc.sim_lightfloatparam_const_attenuation) - linear_atten_factor = self._sim_api.getObjectFloatParam(self._handle, simc.sim_lightfloatparam_lin_attenuation) - quad_atten_factor = self._sim_api.getObjectFloatParam(self._handle, simc.sim_lightfloatparam_quad_attenuation) - return bool(cast_shadows), spot_exponent, spot_cutoff, const_atten_factor, linear_atten_factor, quad_atten_factor - - def set_intensity_properties(self, cast_shadows=None, spot_exponent=None, spot_cutoff=None, const_atten_factor=None, - linear_atten_factor=None, quad_atten_factor=None): - """ Set light intensity properties. - - :param cast_shadows: POV-Ray light casts shadows - :param spot_exponent: light spot exponent - :param spot_cutoff: light spot cutoff - :param const_atten_factor: light constant attenuation factor, currently not supported - :param linear_atten_factor: light linear attenuation factor, currently not supported - :param quad_atten_factor: light quadratic attenuation factor, currently not supported + cast_shadows = self._sim_api.getObjectInt32Param( + self._handle, simc.sim_lightintparam_pov_casts_shadows + ) + spot_exponent = self._sim_api.getObjectFloatParam( + self._handle, simc.sim_lightfloatparam_spot_exponent + ) + spot_cutoff = self._sim_api.getObjectFloatParam( + self._handle, simc.sim_lightfloatparam_spot_cutoff + ) + const_atten_factor = self._sim_api.getObjectFloatParam( + self._handle, simc.sim_lightfloatparam_const_attenuation + ) + linear_atten_factor = self._sim_api.getObjectFloatParam( + self._handle, simc.sim_lightfloatparam_lin_attenuation + ) + quad_atten_factor = self._sim_api.getObjectFloatParam( + self._handle, simc.sim_lightfloatparam_quad_attenuation + ) + return ( + bool(cast_shadows), + spot_exponent, + spot_cutoff, + const_atten_factor, + linear_atten_factor, + quad_atten_factor, + ) + + def set_intensity_properties( + self, + cast_shadows=None, + spot_exponent=None, + spot_cutoff=None, + const_atten_factor=None, + linear_atten_factor=None, + quad_atten_factor=None, + ): + """Set light intensity properties. + + :param cast_shadows: POV-Ray light casts shadows. + :param spot_exponent: light spot exponent. + :param spot_cutoff: light spot cutoff. + :param const_atten_factor: light constant attenuation factor, + currently not supported. + :param linear_atten_factor: light linear attenuation factor, + currently not supported. + :param quad_atten_factor: light quadratic attenuation factor, + currently not supported. """ if cast_shadows is not None: self._sim_api.setObjectInt32Parameter( - self._handle, simc.sim_lightintparam_pov_casts_shadows, int(cast_shadows)) + self._handle, + simc.sim_lightintparam_pov_casts_shadows, + int(cast_shadows), + ) if spot_exponent is not None: if spot_exponent % 1 != 0: - warnings.warn('spot exponent must be an integer, rounding input of {} to {}'.format( - spot_exponent, round(spot_exponent))) + warnings.warn( + "spot exponent must be an integer, rounding input of " + f"{spot_exponent} to {round(spot_exponent)}" + ) self._sim_api.setObjectFloatParameter( - self._handle, simc.sim_lightfloatparam_spot_exponent, float(round(spot_exponent))) + self._handle, + simc.sim_lightfloatparam_spot_exponent, + float(round(spot_exponent)), + ) if spot_cutoff is not None: spot_cutoff = float(spot_cutoff) - if spot_cutoff > np.pi/2: - warnings.warn('Tried to set spot_cutoff to {}, but the maximum allowed value is pi/2,' - 'therefore setting to pi/2'.format(spot_cutoff)) + if spot_cutoff > np.pi / 2: + warnings.warn( + f"Tried to set spot_cutoff to {spot_cutoff}, but the maximum " + "allowed value is pi/2, therefore setting to pi/2" + ) self._sim_api.setObjectFloatParameter( - self._handle, simc.sim_lightfloatparam_spot_cutoff, float(spot_cutoff)) + self._handle, simc.sim_lightfloatparam_spot_cutoff, float(spot_cutoff) + ) if const_atten_factor is not None: - raise Exception('CoppeliaSim currently does not support setting attenuation factors') + raise Exception( + "CoppeliaSim currently does not support setting attenuation factors" + ) if linear_atten_factor is not None: - raise Exception('CoppeliaSim currently does not support setting attenuation factors') + raise Exception( + "CoppeliaSim currently does not support setting attenuation factors" + ) if quad_atten_factor is not None: - raise Exception('CoppeliaSim currently does not support setting attenuation factors') + raise Exception( + "CoppeliaSim currently does not support setting attenuation factors" + ) object_type_to_class[ObjectType.LIGHT] = Light diff --git a/pyrep/objects/object.py b/pyrep/objects/object.py index 4fe1e41..c49323f 100644 --- a/pyrep/objects/object.py +++ b/pyrep/objects/object.py @@ -3,8 +3,12 @@ from pyrep.backend import sim_const as simc from pyrep.backend.sim import SimBackend from pyrep.const import ObjectType -from pyrep.errors import WrongObjectTypeError, CoppeliaSimError, ObjectIsNotModelError, \ - ObjectAlreadyRemovedError +from pyrep.errors import ( + WrongObjectTypeError, + CoppeliaSimError, + ObjectIsNotModelError, + ObjectAlreadyRemovedError, +) from typing import Any, Dict, List, Tuple, Union, Optional import numpy as np @@ -25,14 +29,15 @@ def __init__(self, name_or_handle: Union[str, int]): else: try: self._handle = self._sim_api.getObjectHandle(name_or_handle) - except CoppeliaSimError as e: + except CoppeliaSimError: raise ValueError(f"Object with name '{name_or_handle}' not found!") assert_type = self._get_requested_type() actual = ObjectType(self._sim_api.getObjectType(self._handle)) if actual != assert_type: raise WrongObjectTypeError( - 'You requested object of type %s, but the actual type was ' - '%s' % (assert_type.name, actual.name)) + "You requested object of type %s, but the actual type was " + "%s" % (assert_type.name, actual.name) + ) def __eq__(self, other: object): if not isinstance(other, Object): @@ -49,11 +54,12 @@ def exists(name: str) -> bool: :return: True of the object exists. """ sim_api = SimBackend().sim_api + handle = -1 try: - sim_api.getObjectHandle(name) - except: - return False - return True + handle = sim_api.getObjectHandle(name) + except Exception: + pass + return handle >= 0 @staticmethod def get_object_type(name: str) -> ObjectType: @@ -78,7 +84,7 @@ 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: str) -> "Object": """Gets object retrieved by name. :return: The object. @@ -93,7 +99,7 @@ def _get_requested_type(self) -> ObjectType: :return: Type of the object. """ - raise NotImplementedError('Must be overridden.') + raise NotImplementedError("Must be overridden.") def get_type(self) -> ObjectType: """Gets the type of the object. @@ -124,8 +130,7 @@ def get_name(self) -> str: return self._sim_api.getObjectName(self._handle) def set_name(self, name: str) -> None: - """Sets the objects name in the scene. - """ + """Sets the objects name in the scene.""" self._sim_api.setObjectName(self._handle, name) def scale_object(self, scale_x: float, scale_y: float, scale_z: float) -> None: @@ -161,8 +166,9 @@ def get_position(self, relative_to=None) -> np.ndarray: position = self._sim_api.getObjectPosition(self._handle, relto) return np.array(position, dtype=np.float64) - def set_position(self, position: Union[list, np.ndarray], relative_to=None, - reset_dynamics=True) -> None: + def set_position( + self, position: Union[list, np.ndarray], relative_to=None, reset_dynamics=True + ) -> None: """Sets the position of this object. :param position: A list containing the x, y, z position of the object. @@ -194,8 +200,12 @@ def get_orientation(self, relative_to=None) -> np.ndarray: orientation = self._sim_api.getObjectOrientation(self._handle, relto) return np.array(orientation, dtype=np.float64) - def set_orientation(self, orientation: Union[list, np.ndarray], - relative_to=None, reset_dynamics=True) -> None: + def set_orientation( + self, + orientation: Union[list, np.ndarray], + relative_to=None, + reset_dynamics=True, + ) -> None: """Sets the orientation of this object. :param orientation: An array containing the x, y, z orientation of @@ -226,8 +236,9 @@ def get_quaternion(self, relative_to=None) -> np.ndarray: quaternion = self._sim_api.getObjectQuaternion(self._handle, relto) return np.array(quaternion, dtype=np.float64) - def set_quaternion(self, quaternion: Union[list, np.ndarray], - relative_to=None, reset_dynamics=True) -> None: + def set_quaternion( + self, quaternion: Union[list, np.ndarray], relative_to=None, reset_dynamics=True + ) -> None: """Sets the orientation of this object. If the quaternion is not normalised, it will be normalised for you. @@ -264,8 +275,9 @@ def get_pose(self, relative_to=None) -> np.ndarray: quaternion = self.get_quaternion(relative_to) return np.r_[position, quaternion] - def set_pose(self, pose: Union[list, np.ndarray], relative_to=None, - reset_dynamics=True) -> None: + def set_pose( + self, pose: Union[list, np.ndarray], relative_to=None, reset_dynamics=True + ) -> None: """Sets the position and quaternion of an object. :param pose: An array containing the (X,Y,Z,Qx,Qy,Qz,Qw) pose of @@ -290,7 +302,7 @@ def get_velocity(self) -> Tuple[np.ndarray, np.ndarray]: angular_vel = np.array(angular_vel, dtype=np.float64) return linear_vel, angular_vel - def get_parent(self) -> Union['Object', None]: + def get_parent(self) -> Union["Object", None]: """Gets the parent of this object in the scene hierarchy. :return: The parent of this object, or None if it doesn't have a parent. @@ -302,8 +314,9 @@ def get_parent(self) -> Union['Object', None]: cls = object_type_to_class.get(object_type, Object) return cls(handle) - def set_parent(self, parent_object: Union['Object', None], - keep_in_place=True) -> None: + def set_parent( + self, parent_object: Union["Object", None], keep_in_place=True + ) -> None: """Sets this objects parent object in the scene hierarchy. :param parent_object: The object that will become parent, or None if @@ -338,10 +351,11 @@ def set_matrix(self, matrix: np.ndarray, relative_to=None) -> None: :param matrix: A 4x4 transformation matrix. """ if not isinstance(matrix, np.ndarray): - raise ValueError('Expected Numpy 4x4 array.') + raise ValueError("Expected Numpy 4x4 array.") relto = -1 if relative_to is None else relative_to.get_handle() self._sim_api.setObjectMatrix( - self._handle, relto, matrix[:3, :4].reshape((12)).tolist()) + self._handle, relto, matrix[:3, :4].reshape((12)).tolist() + ) def is_collidable(self) -> bool: """Whether the object is collidable or not. @@ -357,7 +371,9 @@ def set_collidable(self, value: bool) -> None: """ self._set_property(simc.sim_objectspecialproperty_collidable, value) - def get_contact(self, contact_obj: Optional['Object']) -> Union[tuple[list, list, list], None]: + def get_contact( + self, contact_obj: Optional["Object"] + ) -> Union[tuple[list, list, list], None]: """Get the contact point and force with other object :param contact_obj: The object want to check contact info with. @@ -366,7 +382,9 @@ def get_contact(self, contact_obj: Optional['Object']) -> Union[tuple[list, list contact_index = 0 contact_obj_handle = None if contact_obj is None else contact_obj.get_handle() while True: - handles, point, force, norm_contact_point = self._sim_api.getContactInfo(simc.sim_handle_all, self.get_handle(), contact_index) + handles, point, force, norm_contact_point = self._sim_api.getContactInfo( + simc.sim_handle_all, self.get_handle(), contact_index + ) if len(handles) == 0: return None contact_index += 1 @@ -381,7 +399,9 @@ def get_contacts(self) -> list[tuple[list, list, list]]: contact_index = 0 contact_info = [] while True: - handles, point, force, norm_contact_point = self._sim_api.getContactInfo(simc.sim_handle_all, self.get_handle(), contact_index) + handles, point, force, norm_contact_point = self._sim_api.getContactInfo( + simc.sim_handle_all, self.get_handle(), contact_index + ) if len(handles) == 0: break contact_index += 1 @@ -461,7 +481,8 @@ def remove(self) -> None: self._sim_api.removeObject(self._handle) except RuntimeError as e: raise ObjectAlreadyRemovedError( - 'The object/model was already deleted.') from e + "The object/model was already deleted." + ) from e def reset_dynamic_object(self) -> None: """Dynamically resets an object that is dynamically simulated. @@ -484,21 +505,22 @@ def get_bounding_box(self) -> List[float]: :return: A list containing the min x, max x, min y, max y, min z, max z positions. """ - params = [simc.sim_objfloatparam_objbbox_min_x, - simc.sim_objfloatparam_objbbox_max_x, - simc.sim_objfloatparam_objbbox_min_y, - simc.sim_objfloatparam_objbbox_max_y, - simc.sim_objfloatparam_objbbox_min_z, - simc.sim_objfloatparam_objbbox_max_z] - return [self._sim_api.getObjectFloatParam( - self._handle, p) for p in params] + params = [ + simc.sim_objfloatparam_objbbox_min_x, + simc.sim_objfloatparam_objbbox_max_x, + simc.sim_objfloatparam_objbbox_min_y, + simc.sim_objfloatparam_objbbox_max_y, + simc.sim_objfloatparam_objbbox_min_z, + simc.sim_objfloatparam_objbbox_max_z, + ] + return [self._sim_api.getObjectFloatParam(self._handle, p) for p in params] def get_extension_string(self) -> str: """A string that describes additional environment/object properties. :return: The extension string. """ - return self._sim_api.getExtensionString(self._handle, -1, '') + return self._sim_api.getExtensionString(self._handle, -1, "") def rotate(self, rotation: List[float]) -> None: """Rotates a transformation matrix. @@ -515,7 +537,7 @@ def rotate(self, rotation: List[float]) -> None: m = self._sim_api.rotateAroundAxis(m, x_axis, axis_pos, rotation[0]) self._sim_api.setObjectMatrix(self._handle, -1, m) - def check_collision(self, obj: 'Object' = None) -> bool: + def check_collision(self, obj: "Object" = None) -> bool: """Checks whether two entities are colliding. :param obj: The other collidable object to check collision against, @@ -534,8 +556,7 @@ def is_model_collidable(self) -> bool: :raises: ObjectIsNotModel if the object is not a model. :return: If the model is collidable. """ - return self._get_model_property( - simc.sim_modelproperty_not_collidable) + return self._get_model_property(simc.sim_modelproperty_not_collidable) def set_model_collidable(self, value: bool): """Set whether the model is collidable or not. @@ -543,8 +564,7 @@ def set_model_collidable(self, value: bool): :param value: The new value of the collidable state of the model. :raises: ObjectIsNotModel if the object is not a model. """ - self._set_model_property( - simc.sim_modelproperty_not_collidable, value) + self._set_model_property(simc.sim_modelproperty_not_collidable, value) def is_model_measurable(self) -> bool: """Whether the model is measurable or not. @@ -552,8 +572,7 @@ def is_model_measurable(self) -> bool: :raises: ObjectIsNotModel if the object is not a model. :return: If the model is measurable. """ - return self._get_model_property( - simc.sim_modelproperty_not_measurable) + return self._get_model_property(simc.sim_modelproperty_not_measurable) def set_model_measurable(self, value: bool): """Set whether the model is measurable or not. @@ -561,8 +580,7 @@ def set_model_measurable(self, value: bool): :param value: The new value of the measurable state of the model. :raises: ObjectIsNotModel if the object is not a model. """ - self._set_model_property( - simc.sim_modelproperty_not_measurable, value) + self._set_model_property(simc.sim_modelproperty_not_measurable, value) def is_model_detectable(self) -> bool: """Whether the model is detectable or not. @@ -570,8 +588,7 @@ def is_model_detectable(self) -> bool: :raises: ObjectIsNotModel if the object is not a model. :return: If the model is detectable. """ - return self._get_model_property( - simc.sim_modelproperty_not_detectable) + return self._get_model_property(simc.sim_modelproperty_not_detectable) def set_model_detectable(self, value: bool): """Set whether the model is detectable or not. @@ -579,8 +596,7 @@ def set_model_detectable(self, value: bool): :param value: The new value of the detectable state of the model. :raises: ObjectIsNotModel if the object is not a model. """ - self._set_model_property( - simc.sim_modelproperty_not_detectable, value) + self._set_model_property(simc.sim_modelproperty_not_detectable, value) def is_model_renderable(self) -> bool: """Whether the model is renderable or not. @@ -588,8 +604,7 @@ def is_model_renderable(self) -> bool: :raises: ObjectIsNotModel if the object is not a model. :return: If the model is renderable. """ - return self._get_model_property( - simc.sim_modelproperty_not_renderable) + return self._get_model_property(simc.sim_modelproperty_not_renderable) def set_model_renderable(self, value: bool): """Set whether the model is renderable or not. @@ -597,8 +612,7 @@ def set_model_renderable(self, value: bool): :param value: The new value of the renderable state of the model. :raises: ObjectIsNotModel if the object is not a model. """ - self._set_model_property( - simc.sim_modelproperty_not_renderable, value) + self._set_model_property(simc.sim_modelproperty_not_renderable, value) def is_model_dynamic(self) -> bool: """Whether the model is dynamic or not. @@ -606,8 +620,7 @@ def is_model_dynamic(self) -> bool: :raises: ObjectIsNotModel if the object is not a model. :return: If the model is dynamic. """ - return self._get_model_property( - simc.sim_modelproperty_not_dynamic) + return self._get_model_property(simc.sim_modelproperty_not_dynamic) def set_model_dynamic(self, value: bool): """Set whether the model is dynamic or not. @@ -615,8 +628,7 @@ def set_model_dynamic(self, value: bool): :param value: The new value of the dynamic state of the model. :raises: ObjectIsNotModel if the object is not a model. """ - self._set_model_property( - simc.sim_modelproperty_not_dynamic, value) + self._set_model_property(simc.sim_modelproperty_not_dynamic, value) def is_model_respondable(self) -> bool: """Whether the model is respondable or not. @@ -624,8 +636,7 @@ def is_model_respondable(self) -> bool: :raises: ObjectIsNotModel if the object is not a model. :return: If the model is respondable. """ - return self._get_model_property( - simc.sim_modelproperty_not_respondable) + return self._get_model_property(simc.sim_modelproperty_not_respondable) def set_model_respondable(self, value: bool): """Set whether the model is respondable or not. @@ -633,8 +644,7 @@ def set_model_respondable(self, value: bool): :param value: The new value of the respondable state of the model. :raises: ObjectIsNotModel if the object is not a model. """ - self._set_model_property( - simc.sim_modelproperty_not_respondable, value) + self._set_model_property(simc.sim_modelproperty_not_respondable, value) def save_model(self, path: str) -> None: """Saves a model. @@ -656,34 +666,37 @@ def get_model_bounding_box(self) -> List[float]: positions. """ self._check_model() - params = [simc.sim_objfloatparam_modelbbox_min_x, - simc.sim_objfloatparam_modelbbox_max_x, - simc.sim_objfloatparam_modelbbox_min_y, - simc.sim_objfloatparam_modelbbox_max_y, - simc.sim_objfloatparam_modelbbox_min_z, - simc.sim_objfloatparam_modelbbox_max_z] - return [self._sim_api.getObjectFloatParam( - self._handle, p) for p in params] + params = [ + simc.sim_objfloatparam_modelbbox_min_x, + simc.sim_objfloatparam_modelbbox_max_x, + simc.sim_objfloatparam_modelbbox_min_y, + simc.sim_objfloatparam_modelbbox_max_y, + simc.sim_objfloatparam_modelbbox_min_z, + simc.sim_objfloatparam_modelbbox_max_z, + ] + return [self._sim_api.getObjectFloatParam(self._handle, p) for p in params] @staticmethod - def _get_objects_in_tree(root_object=None, object_type=ObjectType.ALL, - exclude_base=True, first_generation_only=False - ) -> List['Object']: + def _get_objects_in_tree( + root_object=None, + object_type=ObjectType.ALL, + exclude_base=True, + first_generation_only=False, + ) -> List["Object"]: sim_api = SimBackend().sim_api if root_object is None: root_object = simc.sim_handle_scene elif isinstance(root_object, Object): root_object = root_object.get_handle() elif not isinstance(root_object, int): - raise ValueError('root_object must be None, int or Object') + raise ValueError("root_object must be None, int or Object") options = 0 if exclude_base: options |= 1 if first_generation_only: options |= 2 - handles = sim_api.getObjectsInTree( - root_object, object_type.value, options) + handles = sim_api.getObjectsInTree(root_object, object_type.value, options) objects = [] for handle in handles: try: @@ -693,10 +706,11 @@ def _get_objects_in_tree(root_object=None, object_type=ObjectType.ALL, type = Object.get_object_type(name) warnings.warn( "Object ({}, '{}') has {}, " - 'which is not supported'.format(handle, name, type)) + "which is not supported".format(handle, name, type) + ) return objects - def get_objects_in_tree(self, *args, **kwargs) -> List['Object']: + def get_objects_in_tree(self, *args, **kwargs) -> List["Object"]: """Retrieves the objects in a given hierarchy tree. :param object_type: The object type to retrieve. @@ -708,7 +722,7 @@ def get_objects_in_tree(self, *args, **kwargs) -> List['Object']: """ return self._get_objects_in_tree(self._handle, *args, **kwargs) - def copy(self) -> 'Object': + def copy(self) -> "Object": """Copy and pastes object in the scene. The object is copied together with all its associated calculation @@ -718,31 +732,37 @@ def copy(self) -> 'Object': """ return self.__class__((self._sim_api.copyPasteObjects([self._handle], 0)[0])) - def check_distance(self, other: 'Object') -> float: + def check_distance(self, other: "Object") -> float: """Checks the minimum distance between two objects. :param other: The other object to check distance against. :return: The distance between the objects. """ result, distance_data, object_handle_pair = self._sim_api.checkDistance( - self.get_handle(), other.get_handle(), -1) + self.get_handle(), other.get_handle(), -1 + ) obj1x, obj1y, obj1z, obj2x, obj2y, obj2z, _ = distance_data - return np.linalg.norm(np.array([obj1x, obj1y, obj1z]) - np.array([obj2x, obj2y, obj2z])) + return np.linalg.norm( + np.array([obj1x, obj1y, obj1z]) - np.array([obj2x, obj2y, obj2z]) + ) def get_bullet_friction(self) -> float: """Get bullet friction parameter. :return: The friction. """ - return self._sim_api.getEngineFloatParameter(simc.sim_bullet_body_friction, - self._handle) + return self._sim_api.getEngineFloatParameter( + simc.sim_bullet_body_friction, self._handle + ) def set_bullet_friction(self, friction) -> None: """Set bullet friction parameter. :param friction: The friction to set. """ - self._sim_api.setEngineFloatParameter(simc.sim_bullet_body_friction, self._handle, friction) + self._sim_api.setEngineFloatParameter( + simc.sim_bullet_body_friction, self._handle, friction + ) def get_explicit_handling(self) -> int: """Get explicit handling flags. @@ -763,7 +783,8 @@ def set_explicit_handling(self, value: int) -> None: def _check_model(self) -> None: if not self.is_model(): raise ObjectIsNotModelError( - "Object '%s' is not a model. Use 'set_model(True)' to convert.") + "Object '%s' is not a model. Use 'set_model(True)' to convert." + ) def _get_model_property(self, prop_type: int) -> bool: current = self._sim_api.getModelProperty(self._handle) diff --git a/pyrep/objects/octree.py b/pyrep/objects/octree.py index 70b181e..31ffcd0 100644 --- a/pyrep/objects/octree.py +++ b/pyrep/objects/octree.py @@ -12,10 +12,11 @@ def __init__(self, name_or_handle: Union[str, int]): super().__init__(name_or_handle) @staticmethod - def create(voxel_size: float, point_size: Optional[float] = None, - options: int = 0) -> 'Octree': + def create( + voxel_size: float, point_size: Optional[float] = None, options: int = 0 + ) -> "Octree": """Creates an octree object and inserts in the scene. - + :param voxelSize: The resolution of octree voxels. :param options: Octree options. :param pointSize: Point representation size of voxels. @@ -30,100 +31,107 @@ def create(voxel_size: float, point_size: Optional[float] = None, def _get_requested_type(self) -> ObjectType: return ObjectType.OCTREE - def insert_voxels(self, points: List[float], color: Optional[list[float]] = None, - options: int = 0) -> None: + def insert_voxels( + self, points: List[float], color: Optional[list[float]] = None, options: int = 0 + ) -> None: """Inserts voxels into the octree. - + :param points: A list of x,y,z numbers. :param color: A list containing RGB data, or None. :param options: Voxel insertion options. """ if not isinstance(points, list): - raise ValueError( - 'Octree.insert_voxels: points parameter is not a list.') + raise ValueError("Octree.insert_voxels: points parameter is not a list.") if len(points) % 3 != 0: raise ValueError( - 'Octree.insert_voxels: points parameter length ' - 'not a multiple of 3.') + "Octree.insert_voxels: points parameter length " "not a multiple of 3." + ) if color is not None: if not isinstance(color, list): - raise ValueError( - 'Octree.insert_voxels: color parameter not a list.') + raise ValueError("Octree.insert_voxels: color parameter not a list.") elif len(color) != 3: raise ValueError( - 'Octree.insert_voxels: color parameter not an RGB list.') - self._sim_api.insertVoxelsIntoOctree(self._handle, options, points, color,None) + "Octree.insert_voxels: color parameter not an RGB list." + ) + self._sim_api.insertVoxelsIntoOctree(self._handle, options, points, color, None) return - def remove_voxels(self, points : Optional[List[float]], - options: int = 0) -> None: + def remove_voxels(self, points: Optional[List[float]], options: int = 0) -> None: """Remove voxels from the octree. - + :param points: A list of x,y,z numbers, or None to clear the octree. :param options: Voxel removal options. """ if points is not None: if not isinstance(points, list): raise ValueError( - 'Octree.insert_voxels: points parameter is not a list.') + "Octree.insert_voxels: points parameter is not a list." + ) if len(points) % 3 != 0: raise ValueError( - 'Octree.insert_voxels: points parameter length ' - 'not a multiple of 3.') + "Octree.insert_voxels: points parameter length " + "not a multiple of 3." + ) self._sim_api.removeVoxelsFromOctree(self._handle, options, points) def get_voxels(self) -> list: """Returns voxels from the octree. - + :return: List of voxel x,y,z coordinates. """ return self._sim_api.getOctreeVoxels(self._handle) - def insert_object(self, obj: Object, color: Optional[list[float]] = None, - options: int = 0) -> None: + def insert_object( + self, obj: Object, color: Optional[list[float]] = None, options: int = 0 + ) -> None: """Inserts object into the octree. - + :param obj: Object to insert. :param color: A list containing RGB data, or None. :param options: Object insertion options. """ if color is not None: if not isinstance(color, list): - raise ValueError( - 'Octree.insert_object: color parameter not a list.') + raise ValueError("Octree.insert_object: color parameter not a list.") elif len(color) != 3: raise ValueError( - 'Octree.insert_object: color parameter not an RGB list.') - self._sim_api.insertObjectIntoOctree(self._handle, obj.get_handle(), options, - color, 0) + "Octree.insert_object: color parameter not an RGB list." + ) + self._sim_api.insertObjectIntoOctree( + self._handle, obj.get_handle(), options, color, 0 + ) return def subtract_object(self, obj: Object, options: int = 0) -> None: """Subtract object from the octree. - + :param obj: Object to subtract. :param options: Object subtraction options. """ self._sim_api.subtractObjectFromOctree(self._handle, obj.get_handle(), options) return - def check_point_occupancy(self, points: List[float], - options: int = 0) -> bool: + def check_point_occupancy(self, points: List[float], options: int = 0) -> bool: if not isinstance(points, list): raise ValueError( - 'Octree.check_point_occupancy: points parameter is not a list.') + "Octree.check_point_occupancy: points parameter is not a list." + ) if len(points) % 3 != 0: raise ValueError( - 'Octree._check_point_occupancy: points parameter length ' - 'not a multiple of 3.') - res, tag, loc_low, loc_high = self._sim_api.checkOctreePointOccupancy(self._handle, options, points) + "Octree._check_point_occupancy: points parameter length " + "not a multiple of 3." + ) + res, tag, loc_low, loc_high = self._sim_api.checkOctreePointOccupancy( + self._handle, options, points + ) return bool(res) def clear_voxels(self) -> None: - """Clears all voxels from the octree. - """ + """Clears all voxels from the octree.""" # self._sim_api.removeVoxelsFromOctree(self._handle, 0, None) - raise NotImplementedError("removeVoxelsFromOctree seems to have bug on coppeliasim side.") + raise NotImplementedError( + "removeVoxelsFromOctree seems to have bug on coppeliasim side." + ) object_type_to_class[ObjectType.OCTREE] = Octree diff --git a/pyrep/objects/proximity_sensor.py b/pyrep/objects/proximity_sensor.py index 15314cb..63a06dd 100644 --- a/pyrep/objects/proximity_sensor.py +++ b/pyrep/objects/proximity_sensor.py @@ -1,6 +1,5 @@ from math import sqrt -from pyrep.backend import sim from pyrep.objects.object import Object, object_type_to_class from pyrep.const import ObjectType @@ -22,7 +21,9 @@ def read(self) -> float: :return: Float distance to the first detected object """ - state, dist, points, obj, norm_vec = self._sim_api.readProximitySensor(self._handle) + state, dist, points, obj, norm_vec = self._sim_api.readProximitySensor( + self._handle + ) if state: return sqrt(points[0] ** 2 + points[1] ** 2 + points[2] ** 2) return -1.0 @@ -34,7 +35,8 @@ def is_detected(self, obj: Object) -> bool: :return: Bool indicating if the object was detected. """ res, dist, point, obj, norm_vec = self._sim_api.checkProximitySensor( - self._handle, obj.get_handle()) + self._handle, obj.get_handle() + ) return res == 1 diff --git a/pyrep/objects/shape.py b/pyrep/objects/shape.py index c02b323..1c420c3 100644 --- a/pyrep/objects/shape.py +++ b/pyrep/objects/shape.py @@ -1,6 +1,5 @@ from typing import List, Tuple import numpy as np -from pyrep.backend import sim from pyrep.backend.sim import SimBackend from pyrep.objects.object import Object, object_type_to_class from pyrep.const import ObjectType, PrimitiveShape, TextureMappingMode @@ -11,32 +10,40 @@ SShapeVizInfo = collections.namedtuple( - 'SShapeVizInfo', + "SShapeVizInfo", [ - 'vertices', - 'indices', - 'normals', - 'shading_angle', - 'colors', - 'texture', - 'texture_id', - 'texture_coords', - 'texture_apply_mode', - 'texture_options', + "vertices", + "indices", + "normals", + "shading_angle", + "colors", + "texture", + "texture_id", + "texture_coords", + "texture_apply_mode", + "texture_options", ], ) class Shape(Object): - """Shapes are rigid mesh objects that are composed of triangular faces. - """ + """Shapes are rigid mesh objects that are composed of triangular faces.""" @staticmethod - def create(type: PrimitiveShape, size: List[float], - mass=1., backface_culling=False, visible_edges=False, - smooth=False, respondable=True, - static=False, renderable=True, position=None, - orientation=None, color=None) -> 'Shape': + def create( + type: PrimitiveShape, + size: List[float], + mass=1.0, + backface_culling=False, + visible_edges=False, + smooth=False, + respondable=True, + static=False, + renderable=True, + position=None, + orientation=None, + color=None, + ) -> "Shape": """Creates a primitive shape in the scene. :param type: The type of primitive to shape. One of: @@ -81,10 +88,16 @@ def create(type: PrimitiveShape, size: List[float], return ob @classmethod - def import_shape(cls, filename: str, scaling_factor=1.0, - keep_identical_vertices=False, ignore_color=False, - ignore_texture=False, reorient_bounding_box=False, - ignore_up_vector=False) -> 'Shape': + def import_shape( + cls, + filename: str, + scaling_factor=1.0, + keep_identical_vertices=False, + ignore_color=False, + ignore_texture=False, + reorient_bounding_box=False, + ignore_up_vector=False, + ) -> "Shape": """Imports a shape with visuals from a file. :param filename: The location of the file to import. @@ -98,7 +111,7 @@ def import_shape(cls, filename: str, scaling_factor=1.0, :return: The Shape object. """ if not os.path.isfile(filename): - raise ValueError('Filename does not exist: ' + filename) + raise ValueError("Filename does not exist: " + filename) options = 0 if keep_identical_vertices: @@ -116,9 +129,12 @@ def import_shape(cls, filename: str, scaling_factor=1.0, return cls(handle) @staticmethod - def import_mesh(filename: str, scaling_factor=1.0, - keep_identical_vertices=False, - ignore_up_vector=False) -> 'Shape': + def import_mesh( + filename: str, + scaling_factor=1.0, + keep_identical_vertices=False, + ignore_up_vector=False, + ) -> "Shape": """Imports a mesh from a file. :param filename: The location of the file to import. @@ -129,7 +145,7 @@ def import_mesh(filename: str, scaling_factor=1.0, """ if not os.path.isfile(filename): - raise ValueError('Filename does not exist: ' + filename) + raise ValueError("Filename does not exist: " + filename) options = 0 if keep_identical_vertices: @@ -140,8 +156,7 @@ def import_mesh(filename: str, scaling_factor=1.0, # fileformat is 0 as it is automatically detected. # identicalVerticeTolerance has no effect. Setting to zero. sim_api = SimBackend().sim_api - verticies, indices = sim_api.importMesh( - 0, filename, options, 0, scaling_factor) + verticies, indices = sim_api.importMesh(0, filename, options, 0, scaling_factor) mesh_objects = [] for v, i in zip(verticies, indices): mesh_ob = Shape.create_mesh(v, i) @@ -154,9 +169,13 @@ def import_mesh(filename: str, scaling_factor=1.0, return grouped @staticmethod - def create_mesh(vertices: List[float], indices: List[int], - shading_angle=None, backface_culling=False, - visible_edges=False) -> 'Shape': + def create_mesh( + vertices: List[float], + indices: List[int], + shading_angle=None, + backface_culling=False, + visible_edges=False, + ) -> "Shape": """Creates a mesh shape. :param vertices: A list of vertices. @@ -174,8 +193,7 @@ def create_mesh(vertices: List[float], indices: List[int], if shading_angle is None: shading_angle = 20.0 * 3.1415 / 180.0 sim_api = SimBackend().sim_api - handle = sim_api.createMeshShape( - options, shading_angle, vertices, indices) + handle = sim_api.createMeshShape(options, shading_angle, vertices, indices) return Shape(handle) def _get_requested_type(self) -> ObjectType: @@ -187,7 +205,8 @@ def is_respondable(self) -> bool: :return: If the shape is respondable. """ return self._sim_api.getObjectInt32Param( - self._handle, simc.sim_shapeintparam_respondable) + self._handle, simc.sim_shapeintparam_respondable + ) def set_respondable(self, value: bool) -> None: """Set whether the shape is respondable or not. @@ -195,7 +214,8 @@ def set_respondable(self, value: bool) -> None: :param value: The new value of the respondable state of the shape. """ self._sim_api.setObjectInt32Param( - self._handle, simc.sim_shapeintparam_respondable, int(value)) + self._handle, simc.sim_shapeintparam_respondable, int(value) + ) self.reset_dynamic_object() def is_dynamic(self) -> bool: @@ -204,7 +224,8 @@ def is_dynamic(self) -> bool: :return: If the shape is dynamic. """ return not self._sim_api.getObjectInt32Param( - self._handle, simc.sim_shapeintparam_static) + self._handle, simc.sim_shapeintparam_static + ) def set_dynamic(self, value: bool) -> None: """Set whether the shape is dynamic or not. @@ -212,7 +233,8 @@ def set_dynamic(self, value: bool) -> None: :param value: The new value of the dynamic state of the shape. """ self._sim_api.setObjectInt32Param( - self._handle, simc.sim_shapeintparam_static, int(not value)) + self._handle, simc.sim_shapeintparam_static, int(not value) + ) self.reset_dynamic_object() def get_color(self) -> List[float]: @@ -221,7 +243,8 @@ def get_color(self) -> List[float]: :return: The r, g, b values of the shape. """ result, data = self._sim_api.getShapeColor( - self._handle, "", simc.sim_colorcomponent_ambient_diffuse) + self._handle, "", simc.sim_colorcomponent_ambient_diffuse + ) assert result == 1 return data @@ -231,7 +254,8 @@ def set_color(self, color: List[float]) -> None: :param color: The r, g, b values of the shape. """ self._sim_api.setShapeColor( - self._handle, "", simc.sim_colorcomponent_ambient_diffuse, color) + self._handle, "", simc.sim_colorcomponent_ambient_diffuse, color + ) def get_transparency(self) -> float: """Sets the transparency of the shape. @@ -239,7 +263,8 @@ def get_transparency(self) -> float: :return: The transparency values of the shape. """ result, data = self._sim_api.getShapeColor( - self._handle, "", simc.sim_colorcomponent_transparency) + self._handle, "", simc.sim_colorcomponent_transparency + ) assert result == 1 return data[0] @@ -249,17 +274,19 @@ def set_transparency(self, value: float) -> None: :param value: Value between 0 and 1. """ if 0 > value > 1: - raise ValueError('Value must be between 0 and 1.') + raise ValueError("Value must be between 0 and 1.") self._sim_api.setShapeColor( - self._handle, "", simc.sim_colorcomponent_transparency, [value]) + self._handle, "", simc.sim_colorcomponent_transparency, [value] + ) def get_mass(self) -> float: """Gets the mass of the shape. :return: A float representing the mass. """ - return self._sim_api.getObjectFloatParam(self._handle, - simc.sim_shapefloatparam_mass) + return self._sim_api.getObjectFloatParam( + self._handle, simc.sim_shapefloatparam_mass + ) def set_mass(self, mass: float) -> None: """Sets the mass of the shape. @@ -267,7 +294,8 @@ def set_mass(self, mass: float) -> None: :param mass: The new mass value. """ self._sim_api.setObjectFloatParam( - self._handle, simc.sim_shapefloatparam_mass, mass) + self._handle, simc.sim_shapefloatparam_mass, mass + ) def compute_mass_and_inertia(self, density: float) -> None: """Computes and applies the mass and inertia properties for a @@ -278,8 +306,8 @@ def compute_mass_and_inertia(self, density: float) -> None: ret = self._sim_api.computeMassAndInertia(self._handle, density) if ret == 0: raise ValueError( - 'The shape must be a convex shape (or convex compound shape)') - + "The shape must be a convex shape (or convex compound shape)" + ) def get_mesh_data(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """Retrieves a shape's mesh information. @@ -292,38 +320,54 @@ def get_mesh_data(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: normals = np.array(normals, dtype=np.float64).reshape(-1, 3) return vertices, indices, normals - def decimate_mesh(self, percentage: float) -> 'Shape': + def decimate_mesh(self, percentage: float) -> "Shape": """Retrieves a shape's mesh information. :param percentage: The percentage of the desired decimation (0.1-0.9). :return: A new shape that has a decimated mesh. """ if percentage < 0.1 or percentage > 0.9: - raise ValueError('percentage param must be between 0.1 and 0.9.') + raise ValueError("percentage param must be between 0.1 and 0.9.") # verts, inds, _ = self.get_mesh_data() verts, inds, _ = self._sim_api.getShapeMesh(self._handle) new_verts, new_inds = self._sim_api.getDecimatedMesh( # verts.reshape(-1).tolist(), inds.reshape(-1).tolist(), percentage) - verts, inds, percentage) + verts, + inds, + percentage, + ) s = Shape.create_mesh(new_verts, new_inds) s.set_matrix(self.get_matrix()) return s - def get_convex_decomposition(self, morph=False, same=False, use_vhacd=False, - individual_meshes=False, - hacd_extra_points=True, hacd_face_points=True, - hacd_min_clusters=1, hacd_tri_target=500, - hacd_max_vertex=200, hacd_max_iter=4, - hacd_max_concavity=100, hacd_max_dist=30, - hacd_cluster_thresh=0.25, - vhacd_pca=False, vhacd_tetrahedron=False, - vhacd_res=100000, vhacd_depth=20, - vhacd_plane_downsample=4, - vhacd_hull_downsample=4, - vhacd_max_vertex=64, vhacd_concavity=0.0025, - vhacd_alpha=0.05, vhacd_beta=0.05, - vhacd_gamma=0.00125, vhacd_min_vol=0.0001 - ) -> 'Shape': + def get_convex_decomposition( + self, + morph=False, + same=False, + use_vhacd=False, + individual_meshes=False, + hacd_extra_points=True, + hacd_face_points=True, + hacd_min_clusters=1, + hacd_tri_target=500, + hacd_max_vertex=200, + hacd_max_iter=4, + hacd_max_concavity=100, + hacd_max_dist=30, + hacd_cluster_thresh=0.25, + vhacd_pca=False, + vhacd_tetrahedron=False, + vhacd_res=100000, + vhacd_depth=20, + vhacd_plane_downsample=4, + vhacd_hull_downsample=4, + vhacd_max_vertex=64, + vhacd_concavity=0.0025, + vhacd_alpha=0.05, + vhacd_beta=0.05, + vhacd_gamma=0.00125, + vhacd_min_vol=0.0001, + ) -> "Shape": """ Compute the convex decomposition of the shape using HACD or V-HACD algorithms @@ -384,33 +428,36 @@ def get_convex_decomposition(self, morph=False, same=False, use_vhacd=False, options |= 512 int_params = [ - hacd_min_clusters, # [0] - hacd_tri_target, # [1] - hacd_max_vertex, # [2] - hacd_max_iter, # [3] - 0, # [4] - vhacd_res, # [5] - vhacd_depth, # [6] - vhacd_plane_downsample, # [7] - vhacd_hull_downsample, # [8] - vhacd_max_vertex # [9] + hacd_min_clusters, # [0] + hacd_tri_target, # [1] + hacd_max_vertex, # [2] + hacd_max_iter, # [3] + 0, # [4] + vhacd_res, # [5] + vhacd_depth, # [6] + vhacd_plane_downsample, # [7] + vhacd_hull_downsample, # [8] + vhacd_max_vertex, # [9] ] float_params = [ - hacd_max_concavity, # [0] - hacd_max_dist, # [1] - hacd_cluster_thresh, # [2] - 0.0, # [3] - 0.0, # [4] - vhacd_concavity, # [5] - vhacd_alpha, # [6] - vhacd_beta, # [7] - vhacd_gamma, # [8] - vhacd_min_vol # [9] + hacd_max_concavity, # [0] + hacd_max_dist, # [1] + hacd_cluster_thresh, # [2] + 0.0, # [3] + 0.0, # [4] + vhacd_concavity, # [5] + vhacd_alpha, # [6] + vhacd_beta, # [7] + vhacd_gamma, # [8] + vhacd_min_vol, # [9] ] - return Shape(self._sim_api.convexDecompose(self.get_handle(), options, - int_params, float_params)) + return Shape( + self._sim_api.convexDecompose( + self.get_handle(), options, int_params, float_params + ) + ) def get_texture(self): """Retrieves the texture from the shape. @@ -419,15 +466,21 @@ def get_texture(self): return Texture(self._sim_api.getShapeTextureId(self.get_handle())) def remove_texture(self): - """Removes the texture from the shape. - """ + """Removes the texture from the shape.""" self._sim_api.setShapeTexture(self.get_handle(), -1, 0, 0, [1, 1], None, None) - def set_texture(self, texture: Texture, mapping_mode: TextureMappingMode, - interpolate=True, decal_mode=False, repeat_along_u=False, - repeat_along_v=False, uv_scaling=[1., 1.], - position: List[float] = None, - orientation: List[float] = None): + def set_texture( + self, + texture: Texture, + mapping_mode: TextureMappingMode, + interpolate=True, + decal_mode=False, + repeat_along_u=False, + repeat_along_v=False, + uv_scaling=[1.0, 1.0], + position: List[float] = None, + orientation: List[float] = None, + ): """Applies a texture to a shape :param texture: The texture to add. @@ -458,10 +511,16 @@ def set_texture(self, texture: Texture, mapping_mode: TextureMappingMode, if repeat_along_v: options |= 8 self._sim_api.setShapeTexture( - self.get_handle(), texture.get_texture_id(), mapping_mode.value, - options, list(uv_scaling), position, orientation) + self.get_handle(), + texture.get_texture_id(), + mapping_mode.value, + options, + list(uv_scaling), + position, + orientation, + ) - def ungroup(self) -> List['Shape']: + def ungroup(self) -> List["Shape"]: """Ungroups a compound shape into several simple shapes. :return: A list of shapes. @@ -483,9 +542,11 @@ def get_shape_viz(self, index): normals = np.array(info["normals"], dtype=float).reshape(-1, 3) colors = np.array(info["colors"], dtype=float) texture = np.frombuffer(info["texture"]["texture"], dtype=np.uint8).reshape( - info["texture"]["resolution"][1], info["texture"]["resolution"][0], 4) + info["texture"]["resolution"][1], info["texture"]["resolution"][0], 4 + ) textureCoords = np.array(info["texture"]["coordinates"], dtype=float).reshape( - -1, 2) + -1, 2 + ) res = SShapeVizInfo( vertices=vertices, @@ -505,8 +566,9 @@ def reorient_bounding_box(self, relative_to=None) -> None: relto = -1 if relative_to is None else relative_to.get_handle() self._sim_api.reorientShapeBoundingBox(self._handle, relto) - def add_force(self, position: np.ndarray, force: np.ndarray, - reset_force_torque: bool = False) -> None: + def add_force( + self, position: np.ndarray, force: np.ndarray, reset_force_torque: bool = False + ) -> None: """ Adds a non-central force to a shape object that is dynamically enabled. Added forces are cumulative. @@ -515,13 +577,20 @@ def add_force(self, position: np.ndarray, force: np.ndarray, :param force: The force (in relative coordinates) to add. :param reset_force_torque: Clears the accumulated force and torque. """ - h = (self._handle | simc.sim_handleflag_resetforcetorque - if reset_force_torque else self._handle) + h = ( + self._handle | simc.sim_handleflag_resetforcetorque + if reset_force_torque + else self._handle + ) self._sim_api.addForce(h, list(position), list(force)) - def add_force_and_torque(self, force: np.ndarray, torque: np.ndarray, - reset_force: bool = False, - reset_torque: bool = False) -> None: + def add_force_and_torque( + self, + force: np.ndarray, + torque: np.ndarray, + reset_force: bool = False, + reset_torque: bool = False, + ) -> None: """ Adds a force and/or torque to a shape object that is dynamically enabled. Forces are applied at the center of mass. @@ -537,9 +606,11 @@ def add_force_and_torque(self, force: np.ndarray, torque: np.ndarray, h |= simc.sim_handleflag_resetforce if reset_torque: h |= simc.sim_handleflag_resettorque - self._sim_api.addForceAndTorque(h, - None if force is None else list(force), - None if torque is None else list(torque)) + self._sim_api.addForceAndTorque( + h, + None if force is None else list(force), + None if torque is None else list(torque), + ) object_type_to_class[ObjectType.SHAPE] = Shape diff --git a/pyrep/objects/vision_sensor.py b/pyrep/objects/vision_sensor.py index 7781c7c..ff5a79a 100644 --- a/pyrep/objects/vision_sensor.py +++ b/pyrep/objects/vision_sensor.py @@ -1,6 +1,5 @@ import math from typing import List, Union, Sequence -from pyrep.backend import sim from pyrep.backend.sim import SimBackend from pyrep.backend import sim_const as simc from pyrep.objects.object import Object, object_type_to_class @@ -9,23 +8,32 @@ class VisionSensor(Object): - """A camera-type sensor, reacting to light, colors and images. - """ + """A camera-type sensor, reacting to light, colors and images.""" def __init__(self, name_or_handle: Union[str, int]): super().__init__(name_or_handle) self.resolution = self._sim_api.getVisionSensorRes(self._handle) @staticmethod - def create(resolution: List[int], explicit_handling=False, - perspective_mode=True, show_volume_not_detecting=True, - show_volume_detecting=True, passive=False, - use_local_lights=False, show_fog=True, - near_clipping_plane=1e-2, far_clipping_plane=10.0, - view_angle=60.0, ortho_size=1.0, sensor_size=None, - render_mode=RenderMode.OPENGL, - position=None, orientation=None) -> 'VisionSensor': - """ Create a Vision Sensor + def create( + resolution: List[int], + explicit_handling=False, + perspective_mode=True, + show_volume_not_detecting=True, + show_volume_detecting=True, + passive=False, + use_local_lights=False, + show_fog=True, + near_clipping_plane=1e-2, + far_clipping_plane=10.0, + view_angle=60.0, + ortho_size=1.0, + sensor_size=None, + render_mode=RenderMode.OPENGL, + position=None, + orientation=None, + ) -> "VisionSensor": + """Create a Vision Sensor :param resolution: List of the [x, y] resolution. :param explicit_handling: Sensor will be explicitly handled. @@ -72,28 +80,23 @@ def create(resolution: List[int], explicit_handling=False, if not show_fog: options |= 64 - int_params = [ - resolution[0], # 0 - resolution[1], # 1 - 0, # 2 - 0 # 3 - ] + int_params = [resolution[0], resolution[1], 0, 0] # 0 # 1 # 2 # 3 if sensor_size is None: sensor_size = [0.01, 0.01, 0.03] float_params = [ - near_clipping_plane, # 0 - far_clipping_plane, # 1 + near_clipping_plane, # 0 + far_clipping_plane, # 1 math.radians(view_angle) if perspective_mode else ortho_size, # 2 - sensor_size[0], # 3 - sensor_size[1], # 4 - sensor_size[2], # 5 - 0.0, # 6 - 0.0, # 7 - 0.0, # 8 - 0.0, # 9 - 0.0, # 10 + sensor_size[0], # 3 + sensor_size[1], # 4 + sensor_size[2], # 5 + 0.0, # 6 + 0.0, # 7 + 0.0, # 8 + 0.0, # 9 + 0.0, # 10 ] sim_api = SimBackend().sim_api @@ -113,12 +116,14 @@ def _get_requested_type(self) -> ObjectType: def handle_explicitly(self) -> None: """Handle sensor explicitly. - This enables capturing image (e.g., capture_rgb()) - without PyRep.step(). + This enables capturing image (e.g., capture_rgb()) + without PyRep.step(). """ if not self.get_explicit_handling(): - raise RuntimeError('The explicit_handling is disabled. ' - 'Call set_explicit_handling(value=1) to enable explicit_handling first.') + raise RuntimeError( + "The explicit_handling is disabled. " + "Call set_explicit_handling(value=1) to enable explicit_handling first." + ) self._sim_api.handleVisionSensor(self._handle) def capture_rgb(self) -> np.ndarray: @@ -127,7 +132,9 @@ def capture_rgb(self) -> np.ndarray: :return: A numpy array of size (width, height, 3) """ enc_img, resolution = self._sim_api.getVisionSensorImg(self._handle) - img = np.frombuffer(enc_img, dtype=np.uint8).reshape(resolution[1], resolution[0], 3) + img = np.frombuffer(enc_img, dtype=np.uint8).reshape( + resolution[1], resolution[0], 3 + ) img = np.flip(img, 0).copy() return img @@ -137,8 +144,12 @@ def capture_depth(self, in_meters=False) -> np.ndarray: :param in_meters: Whether the depth should be returned in meters. :return: A numpy array of size (width, height) """ - enc_img, resolution = self._sim_api.getVisionSensorDepth(self._handle, int(in_meters)) - img = np.frombuffer(enc_img, dtype=np.float32).reshape(resolution[1], resolution[0]) + enc_img, resolution = self._sim_api.getVisionSensorDepth( + self._handle, int(in_meters) + ) + img = np.frombuffer(enc_img, dtype=np.float32).reshape( + resolution[1], resolution[0] + ) img = np.flip(img, 0).copy() return img @@ -157,12 +168,13 @@ def pointcloud_from_depth(self, depth: np.ndarray) -> np.ndarray: """ intrinsics = self.get_intrinsic_matrix() return VisionSensor.pointcloud_from_depth_and_camera_params( - depth, self.get_matrix(), intrinsics) + depth, self.get_matrix(), intrinsics + ) @staticmethod def pointcloud_from_depth_and_camera_params( - depth: np.ndarray, extrinsics: np.ndarray, - intrinsics: np.ndarray) -> np.ndarray: + depth: np.ndarray, extrinsics: np.ndarray, intrinsics: np.ndarray + ) -> np.ndarray: """Converts depth (in meters) to point cloud in word frame. :return: A numpy array of size (width, height, 3) """ @@ -174,11 +186,11 @@ def pointcloud_from_depth_and_camera_params( R_inv_C = np.matmul(R_inv, C) extrinsics = np.concatenate((R_inv, -R_inv_C), -1) cam_proj_mat = np.matmul(intrinsics, extrinsics) - cam_proj_mat_homo = np.concatenate( - [cam_proj_mat, [np.array([0, 0, 0, 1])]]) + cam_proj_mat_homo = np.concatenate([cam_proj_mat, [np.array([0, 0, 0, 1])]]) cam_proj_mat_inv = np.linalg.inv(cam_proj_mat_homo)[0:3] - world_coords_homo = np.expand_dims(_pixel_to_world_coords( - pc, cam_proj_mat_inv), 0) + world_coords_homo = np.expand_dims( + _pixel_to_world_coords(pc, cam_proj_mat_inv), 0 + ) world_coords = world_coords_homo[..., :-1][0] return world_coords @@ -194,19 +206,22 @@ def get_intrinsic_matrix(self): persp_angles = np.array([pa_x, pa_y]) focal_lengths = -res / (2 * np.tan(persp_angles / 2)) return np.array( - [[focal_lengths[0], 0., pp_offsets[0]], - [0., focal_lengths[1], pp_offsets[1]], - [0., 0., 1.]]) + [ + [focal_lengths[0], 0.0, pp_offsets[0]], + [0.0, focal_lengths[1], pp_offsets[1]], + [0.0, 0.0, 1.0], + ] + ) def get_resolution(self) -> np.ndarray: - """ Return the Sensor's resolution. + """Return the Sensor's resolution. :return: Resolution [x, y] """ return np.array(self._sim_api.getVisionSensorRes(self._handle)) def set_resolution(self, resolution: np.ndarray) -> None: - """ Set the Sensor's resolution. + """Set the Sensor's resolution. :param resolution: New resolution [x, y] """ @@ -219,29 +234,31 @@ def set_resolution(self, resolution: np.ndarray) -> None: self.resolution = resolution def get_perspective_mode(self) -> PerspectiveMode: - """ Retrieve the Sensor's perspective mode. + """Retrieve the Sensor's perspective mode. :return: The current PerspectiveMode. """ perspective_mode = self._sim_api.getObjectInt32Param( - self._handle, simc.sim_visionintparam_perspective_operation, + self._handle, + simc.sim_visionintparam_perspective_operation, ) return PerspectiveMode(perspective_mode) def set_perspective_mode(self, perspective_mode: PerspectiveMode) -> None: - """ Set the Sensor's perspective mode. + """Set the Sensor's perspective mode. :param perspective_mode: The new perspective mode, one of: PerspectiveMode.ORTHOGRAPHIC PerspectiveMode.PERSPECTIVE """ self._sim_api.setObjectInt32Param( - self._handle, simc.sim_visionintparam_perspective_operation, - perspective_mode.value + self._handle, + simc.sim_visionintparam_perspective_operation, + perspective_mode.value, ) def get_render_mode(self) -> RenderMode: - """ Retrieves the Sensor's rendering mode + """Retrieves the Sensor's rendering mode :return: RenderMode for the current rendering mode. """ @@ -251,7 +268,7 @@ def get_render_mode(self) -> RenderMode: return RenderMode(render_mode) def set_render_mode(self, render_mode: RenderMode) -> None: - """ Set the Sensor's rendering mode + """Set the Sensor's rendering mode :param render_mode: The new sensor rendering mode, one of: RenderMode.OPENGL @@ -264,8 +281,7 @@ def set_render_mode(self, render_mode: RenderMode) -> None: RenderMode.OPENGL3_WINDOWED """ self._sim_api.setObjectInt32Param( - self._handle, simc.sim_visionintparam_render_mode, - render_mode.value + self._handle, simc.sim_visionintparam_render_mode, render_mode.value ) def get_windowed_size(self) -> Sequence[int]: @@ -274,9 +290,11 @@ def get_windowed_size(self) -> Sequence[int]: :return: The (x, y) resolution of the window. 0 for full-screen. """ size_x = self._sim_api.getObjectInt32Param( - self._handle, simc.sim_visionintparam_windowed_size_x) + self._handle, simc.sim_visionintparam_windowed_size_x + ) size_y = self._sim_api.getObjectInt32Param( - self._handle, simc.sim_visionintparam_windowed_size_y) + self._handle, simc.sim_visionintparam_windowed_size_y + ) return size_x, size_y def set_windowed_size(self, resolution: Sequence[int] = (0, 0)) -> None: @@ -286,33 +304,36 @@ def set_windowed_size(self, resolution: Sequence[int] = (0, 0)) -> None: 0 for full-screen. """ self._sim_api.setObjectInt32Param( - self._handle, simc.sim_visionintparam_windowed_size_x, - resolution[0]) + self._handle, simc.sim_visionintparam_windowed_size_x, resolution[0] + ) self._sim_api.setObjectInt32Param( - self._handle, simc.sim_visionintparam_windowed_size_y, - resolution[1]) + self._handle, simc.sim_visionintparam_windowed_size_y, resolution[1] + ) def get_perspective_angle(self) -> float: - """ Get the Sensor's perspective angle. + """Get the Sensor's perspective angle. :return: The sensor's perspective angle (in degrees). """ - return math.degrees(self._sim_api.getObjectFloatParam( - self._handle, simc.sim_visionfloatparam_perspective_angle - )) + return math.degrees( + self._sim_api.getObjectFloatParam( + self._handle, simc.sim_visionfloatparam_perspective_angle + ) + ) def set_perspective_angle(self, angle: float) -> None: - """ Set the Sensor's perspective angle. + """Set the Sensor's perspective angle. :param angle: New perspective angle (in degrees) """ self._sim_api.setObjectFloatParam( - self._handle, simc.sim_visionfloatparam_perspective_angle, - math.radians(angle) + self._handle, + simc.sim_visionfloatparam_perspective_angle, + math.radians(angle), ) def get_orthographic_size(self) -> float: - """ Get the Sensor's orthographic size. + """Get the Sensor's orthographic size. :return: The sensor's orthographic size (in metres). """ @@ -321,7 +342,7 @@ def get_orthographic_size(self) -> float: ) def set_orthographic_size(self, ortho_size: float) -> None: - """ Set the Sensor's orthographic size. + """Set the Sensor's orthographic size. :param angle: New orthographic size (in metres) """ @@ -330,7 +351,7 @@ def set_orthographic_size(self, ortho_size: float) -> None: ) def get_near_clipping_plane(self) -> float: - """ Get the Sensor's near clipping plane. + """Get the Sensor's near clipping plane. :return: Near clipping plane (metres) """ @@ -339,7 +360,7 @@ def get_near_clipping_plane(self) -> float: ) def set_near_clipping_plane(self, near_clipping: float) -> None: - """ Set the Sensor's near clipping plane. + """Set the Sensor's near clipping plane. :param near_clipping: New near clipping plane (in metres) """ @@ -348,7 +369,7 @@ def set_near_clipping_plane(self, near_clipping: float) -> None: ) def get_far_clipping_plane(self) -> float: - """ Get the Sensor's far clipping plane. + """Get the Sensor's far clipping plane. :return: Near clipping plane (metres) """ @@ -357,7 +378,7 @@ def get_far_clipping_plane(self) -> float: ) def set_far_clipping_plane(self, far_clipping: float) -> None: - """ Set the Sensor's far clipping plane. + """Set the Sensor's far clipping plane. :param far_clipping: New far clipping plane (in metres) """ @@ -366,7 +387,9 @@ def set_far_clipping_plane(self, far_clipping: float) -> None: ) def set_entity_to_render(self, entity_to_render: int) -> None: - """ Set the entity to render to the Sensor, this can be an object or more usefully a collection. + """Set the entity to render to the Sensor. + + This can be an object or more usefully a collection. -1 to render all objects in scene. :param entity_to_render: Handle of the entity to render @@ -376,7 +399,9 @@ def set_entity_to_render(self, entity_to_render: int) -> None: ) def get_entity_to_render(self) -> int: - """ Get the entity to render to the Sensor, this can be an object or more usefully a collection. + """Get the entity to render to the Sensor. + + This can be an object or more usefully a collection. None if all objects in scene are rendered. :return: Handle of the entity to render @@ -389,13 +414,16 @@ def get_entity_to_render(self) -> int: def _create_uniform_pixel_coords_image(resolution: np.ndarray): pixel_x_coords = np.reshape( np.tile(np.arange(resolution[1]), [resolution[0]]), - (resolution[0], resolution[1], 1)).astype(np.float32) + (resolution[0], resolution[1], 1), + ).astype(np.float32) pixel_y_coords = np.reshape( np.tile(np.arange(resolution[0]), [resolution[1]]), - (resolution[1], resolution[0], 1)).astype(np.float32) + (resolution[1], resolution[0], 1), + ).astype(np.float32) pixel_y_coords = np.transpose(pixel_y_coords, (1, 0, 2)) uniform_pixel_coords = np.concatenate( - (pixel_x_coords, pixel_y_coords, np.ones_like(pixel_x_coords)), -1) + (pixel_x_coords, pixel_y_coords, np.ones_like(pixel_x_coords)), -1 + ) return uniform_pixel_coords @@ -404,19 +432,15 @@ def _transform(coords, trans): coords = np.reshape(coords, (h * w, -1)) coords = np.transpose(coords, (1, 0)) transformed_coords_vector = np.matmul(trans, coords) - transformed_coords_vector = np.transpose( - transformed_coords_vector, (1, 0)) - return np.reshape(transformed_coords_vector, - (h, w, -1)) + transformed_coords_vector = np.transpose(transformed_coords_vector, (1, 0)) + return np.reshape(transformed_coords_vector, (h, w, -1)) def _pixel_to_world_coords(pixel_coords, cam_proj_mat_inv): h, w = pixel_coords.shape[:2] - pixel_coords = np.concatenate( - [pixel_coords, np.ones((h, w, 1))], -1) + pixel_coords = np.concatenate([pixel_coords, np.ones((h, w, 1))], -1) world_coords = _transform(pixel_coords, cam_proj_mat_inv) - world_coords_homo = np.concatenate( - [world_coords, np.ones((h, w, 1))], axis=-1) + world_coords_homo = np.concatenate([world_coords, np.ones((h, w, 1))], axis=-1) return world_coords_homo diff --git a/pyrep/pyrep.py b/pyrep/pyrep.py index 47cce49..873178a 100644 --- a/pyrep/pyrep.py +++ b/pyrep/pyrep.py @@ -1,15 +1,16 @@ import numpy as np -from contextlib import contextmanager from pyrep.backend import utils from pyrep.backend.sim import SimBackend from pyrep.backend.sim_const import sim_floatparam_simulation_time_step + # from pyrep.backend import sim, utils from pyrep.const import Verbosity from pyrep.objects.object import Object from pyrep.objects.shape import Shape from pyrep.textures.texture import Texture from pyrep.errors import PyRepError + # from pyrep.backend import sim import os import sys @@ -34,14 +35,16 @@ def __init__(self): self._sim_api = None # check later self._shutting_down = False - if 'COPPELIASIM_ROOT' not in os.environ: + if "COPPELIASIM_ROOT" not in os.environ: raise PyRepError( - 'COPPELIASIM_ROOT not defined. See installation instructions.') - self._coppeliasim_root = os.environ['COPPELIASIM_ROOT'] + "COPPELIASIM_ROOT not defined. See installation instructions." + ) + self._coppeliasim_root = os.environ["COPPELIASIM_ROOT"] if not os.path.exists(self._coppeliasim_root): raise PyRepError( - 'COPPELIASIM_ROOT was not a correct path. ' - 'See installation instructions') + "COPPELIASIM_ROOT was not a correct path. " + "See installation instructions" + ) def _run_responsive_ui_thread(self) -> None: while True: @@ -55,9 +58,14 @@ def _run_responsive_ui_thread(self) -> None: if not self._shutting_down: self.shutdown() - def launch(self, scene_file: str = "", headless: bool = False, - responsive_ui: bool = False, blocking: bool = False, - verbosity: Verbosity = Verbosity.NONE) -> None: + def launch( + self, + scene_file: str = "", + headless: bool = False, + responsive_ui: bool = False, + blocking: bool = False, + verbosity: Verbosity = Verbosity.NONE, + ) -> None: """Launches CoppeliaSim. Launches the UI thread, waits until the UI thread has finished, this @@ -76,11 +84,13 @@ def launch(self, scene_file: str = "", headless: bool = False, """ abs_scene_file = os.path.abspath(scene_file) if len(scene_file) > 0 and not os.path.isfile(abs_scene_file): - raise PyRepError('Scene file does not exist: %s' % scene_file) + raise PyRepError("Scene file does not exist: %s" % scene_file) self._sim_backend = SimBackend() self._ui_thread = self._sim_backend.create_ui_thread(headless) self._ui_thread.start() - self._sim_api = self._sim_backend.simInitialize(self._coppeliasim_root, verbosity.value) + self._sim_api = self._sim_backend.simInitialize( + self._coppeliasim_root, verbosity.value + ) if len(scene_file) > 0: self._sim_api.loadScene(abs_scene_file) @@ -90,7 +100,8 @@ def launch(self, scene_file: str = "", headless: bool = False, self.shutdown() elif responsive_ui: self._responsive_ui_thread = threading.Thread( - target=self._run_responsive_ui_thread) + target=self._run_responsive_ui_thread + ) self._responsive_ui_thread.daemon = True try: self._responsive_ui_thread.start() @@ -103,11 +114,9 @@ def launch(self, scene_file: str = "", headless: bool = False, self.step() def shutdown(self) -> None: - """Shuts down the CoppeliaSim simulation. - """ + """Shuts down the CoppeliaSim simulation.""" if self._ui_thread is None: - raise PyRepError( - 'CoppeliaSim has not been launched. Call launch first.') + raise PyRepError("CoppeliaSim has not been launched. Call launch first.") if self._ui_thread is not None: # self._shutting_down = True self.stop() @@ -125,21 +134,17 @@ def shutdown(self) -> None: # self._shutting_down = False def start(self) -> None: - """Starts the physics simulation if it is not already running. - """ + """Starts the physics simulation if it is not already running.""" if self._ui_thread is None: - raise PyRepError( - 'CoppeliaSim has not been launched. Call launch first.') + raise PyRepError("CoppeliaSim has not been launched. Call launch first.") if not self.running: self._sim_backend.simStartSimulation() self.running = True def stop(self) -> None: - """Stops the physics simulation if it is running. - """ + """Stops the physics simulation if it is running.""" if self._ui_thread is None: - raise PyRepError( - 'CoppeliaSim has not been launched. Call launch first.') + raise PyRepError("CoppeliaSim has not been launched. Call launch first.") if self.running: self._sim_backend.simStopSimulation() self.running = False @@ -173,9 +178,11 @@ def set_simulation_timestep(self, dt: float) -> None: """ self._sim_api.setFloatParameter(sim_floatparam_simulation_time_step, dt) if not np.allclose(self.get_simulation_timestep(), dt): - warnings.warn('Could not change simulation timestep. You may need ' - 'to change it to "custom dt" using simulation ' - 'settings dialog.') + warnings.warn( + "Could not change simulation timestep. You may need " + 'to change it to "custom dt" using simulation ' + "settings dialog." + ) def get_simulation_timestep(self) -> float: """Gets the simulation time step. @@ -216,7 +223,7 @@ def export_scene(self, filename: str) -> None: self._sim_api.saveScene(filename) def import_model(self, filename: str) -> Object: - """ Loads a previously saved model. + """Loads a previously saved model. :param filename: model filename. The filename extension is required ("ttm"). An optional "@copy" can be appended to the filename, in @@ -227,9 +234,14 @@ def import_model(self, filename: str) -> Object: handle = self._sim_api.loadModel(filename) return utils.to_type(handle) - def create_texture(self, filename: str, interpolate=True, decal_mode=False, - repeat_along_u=False, repeat_along_v=False - ) -> Tuple[Shape, Texture]: + def create_texture( + self, + filename: str, + interpolate=True, + decal_mode=False, + repeat_along_u=False, + repeat_along_v=False, + ) -> Tuple[Shape, Texture]: """Creates a planar shape that is textured. :param filename: Path to the texture to load. @@ -253,8 +265,7 @@ def create_texture(self, filename: str, interpolate=True, decal_mode=False, s = Shape(handle) return s, s.get_texture() - def get_objects_in_tree(self, root_object=None, *args, **kwargs - ) -> List[Object]: + def get_objects_in_tree(self, root_object=None, *args, **kwargs) -> List[Object]: """Retrieves the objects in a given hierarchy tree. :param root_object: The root object in the tree. Pass None to retrieve @@ -271,7 +282,7 @@ def get_objects_in_tree(self, root_object=None, *args, **kwargs def get_collection_handle_by_name(self, collection_name: str) -> int: """Retrieves the integer handle for a given collection. - :param collection_name: Name of the collection to retrieve the integer handle for - :return: An integer handle for the collection + :param collection_name: Name of the collection to retrieve the integer handle. + :return: An integer handle for the collection. """ return self._sim_api.getCollectionHandle(collection_name) diff --git a/pyrep/robots/arms/arm.py b/pyrep/robots/arms/arm.py index 16a9912..20624b0 100644 --- a/pyrep/robots/arms/arm.py +++ b/pyrep/robots/arms/arm.py @@ -1,10 +1,9 @@ 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, Joint +from pyrep.objects import Object from pyrep.objects.dummy import Dummy -from pyrep.robots.configuration_paths.arm_configuration_path import ( - ArmConfigurationPath) +from pyrep.robots.configuration_paths.arm_configuration_path import ArmConfigurationPath from pyrep.robots.robot_component import RobotComponent from pyrep.objects.cartesian_path import CartesianPath from pyrep.errors import ConfigurationError, ConfigurationPathError, IKError @@ -27,14 +26,20 @@ def collision_check_callback(state, data): class Arm(RobotComponent): - """Base class representing a robot arm with path planning support. - """ - - def __init__(self, count: int, name: str, num_joints: int, - base_name: str = None, - max_velocity=1.0, max_acceleration=4.0, max_jerk=1000): + """Base class representing a robot arm with path planning support.""" + + def __init__( + self, + count: int, + name: str, + num_joints: int, + base_name: str = None, + max_velocity=1.0, + max_acceleration=4.0, + max_jerk=1000, + ): """Count is used for when we have multiple copies of arms""" - joint_names = ['%s_joint%d' % (name, i+1) for i in range(num_joints)] + joint_names = ["%s_joint%d" % (name, i + 1) for i in range(num_joints)] super().__init__(count, name, joint_names, base_name) # Used for motion planning @@ -43,12 +48,17 @@ def __init__(self, count: int, name: str, num_joints: int, self.max_jerk = max_jerk # Motion planning handles - suffix = '' if count == 0 else '#%d' % (count - 1) - self._ik_target = Dummy('%s_target%s' % (name, suffix)) - self._ik_tip = Dummy('%s_tip%s' % (name, suffix)) + suffix = "" if count == 0 else "#%d" % (count - 1) + 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_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() @@ -59,10 +69,24 @@ def __init__(self, count: int, name: str, num_joints: int, target_handle = self._ik_target.get_handle() tip_handle = self._ik_tip.get_handle() - self._ik_element, self._sim_to_ik_map, self._ik_to_sim_map = self._sim_ik_api.addElementFromScene( - self._ik_env_handle, self._ik_group_handle, base_handle, tip_handle, target_handle, - self._sim_ik_api.constraint_pose) - self._sim_ik_api.setElementPrecision(self._ik_env_handle, self._ik_group_handle, self._ik_element, [0.00005, 0.0017]) + ( + self._ik_element, + self._sim_to_ik_map, + self._ik_to_sim_map, + ) = self._sim_ik_api.addElementFromScene( + self._ik_env_handle, + self._ik_group_handle, + base_handle, + tip_handle, + target_handle, + self._sim_ik_api.constraint_pose, + ) + self._sim_ik_api.setElementPrecision( + self._ik_env_handle, + self._ik_group_handle, + self._ik_element, + [0.00005, 0.0017], + ) self._ik_joint_handles = [self._sim_to_ik_map[h] for h in self._joint_handles] self._ik_base_handle = self._sim_to_ik_map[base_handle] self._ik_target_handle = self._sim_to_ik_map[target_handle] @@ -73,10 +97,14 @@ def __init__(self, count: int, name: str, num_joints: int, SimBackend().lib.simRegCallback(0, self._collision_callback) self._coll_callback_args = [count, name, num_joints, base_name] - def set_ik_element_properties(self, constraint_x=True, constraint_y=True, - constraint_z=True, - constraint_alpha_beta=True, - constraint_gamma=True) -> None: + def set_ik_element_properties( + self, + constraint_x=True, + constraint_y=True, + constraint_z=True, + constraint_alpha_beta=True, + constraint_gamma=True, + ) -> None: constraints = 0 if constraint_x: constraints |= self._sim_ik_api.constraint_x @@ -88,30 +116,44 @@ def set_ik_element_properties(self, constraint_x=True, constraint_y=True, constraints |= self._sim_ik_api.constraint_alpha_beta if constraint_gamma: constraints |= self._sim_ik_api.constraint_gamma - self._sim_ik_api.setElementConstraints(self._ik_env_handle, self._ik_group_handle, self._ik_element, constraints) + self._sim_ik_api.setElementConstraints( + self._ik_env_handle, self._ik_group_handle, self._ik_element, constraints + ) - def set_ik_group_properties(self, resolution_method='pseudo_inverse', max_iterations=6, dls_damping=0.1) -> None: + def set_ik_group_properties( + self, resolution_method="pseudo_inverse", max_iterations=6, dls_damping=0.1 + ) -> None: try: - res_method = {'pseudo_inverse': self._sim_ik_api.method_pseudo_inverse, - 'damped_least_squares': self._sim_ik_api.method_damped_least_squares, - 'jacobian_transpose': self._sim_ik_api.method_jacobian_transpose}[resolution_method] + res_method = { + "pseudo_inverse": self._sim_ik_api.method_pseudo_inverse, + "damped_least_squares": self._sim_ik_api.method_damped_least_squares, + "jacobian_transpose": self._sim_ik_api.method_jacobian_transpose, + }[resolution_method] except KeyError: - raise Exception('Invalid resolution method,' - 'Must be one of ["pseudo_inverse" | "damped_least_squares" | "jacobian_transpose"]') - self._sim_ik_api.setGroupCalculation(self._ik_env_handle, self._ik_group_handle, res_method, dls_damping, max_iterations) - - - def solve_ik_via_sampling(self, - position: Union[List[float], np.ndarray], - euler: Union[List[float], np.ndarray] = None, - quaternion: Union[List[float], np.ndarray] = None, - ignore_collisions: bool = False, - trials: int = 300, - max_configs: int = 1, - distance_threshold: float = 0.65, - max_time_ms: float = 100, - relative_to: Object = None - ) -> np.ndarray: + raise Exception( + "Invalid resolution method. Must be one of " + "['pseudo_inverse' | 'damped_least_squares' | 'jacobian_transpose']" + ) + self._sim_ik_api.setGroupCalculation( + self._ik_env_handle, + self._ik_group_handle, + res_method, + dls_damping, + max_iterations, + ) + + def solve_ik_via_sampling( + self, + position: Union[List[float], np.ndarray], + euler: Union[List[float], np.ndarray] = None, + quaternion: Union[List[float], np.ndarray] = None, + ignore_collisions: bool = False, + trials: int = 300, + max_configs: int = 1, + distance_threshold: float = 0.65, + max_time_ms: float = 100, + relative_to: Object = None, + ) -> np.ndarray: """Solves an IK group and returns the calculated joint values. This IK method performs a random searches for manipulator configurations @@ -146,7 +188,8 @@ def solve_ik_via_sampling(self, """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationError( - 'Specify either euler or quaternion values, but not both.') + "Specify either euler or quaternion values, but not both." + ) prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position, relative_to) @@ -163,13 +206,28 @@ def solve_ik_via_sampling(self, metric = [1, 1, 1, 0.1] valid_joint_positions = [] max_time_s = float(max_time_ms) * 0.001 - self._sim_ik_api.setObjectPose(self._ik_env_handle, self._ik_target_handle, self._ik_target.get_pose().tolist(), self._sim_ik_api.handle_world) + self._sim_ik_api.setObjectPose( + self._ik_env_handle, + self._ik_target_handle, + self._ik_target.get_pose().tolist(), + self._sim_ik_api.handle_world, + ) for i in range(trials): - config = self._sim_ik_api.findConfig(self._ik_env_handle, self._ik_group_handle, self._ik_joint_handles, distance_threshold, max_time_s, metric, validation_callback, self._coll_callback_args) + config = self._sim_ik_api.findConfig( + self._ik_env_handle, + self._ik_group_handle, + self._ik_joint_handles, + distance_threshold, + max_time_s, + metric, + validation_callback, + self._coll_callback_args, + ) if config is None: continue # TODO: Look to get alternative config - # config = self._sim_ik_api.getAlternateConfigs(self._ik_env_handle, self._ik_joint_handles) + # config = self._sim_ik_api.getAlternateConfigs( + # self._ik_env_handle, self._ik_joint_handles) if len(config) > 0: valid_joint_positions.append(config) if len(valid_joint_positions) >= max_configs: @@ -178,22 +236,24 @@ def solve_ik_via_sampling(self, self._ik_target.set_pose(prev_pose) if len(valid_joint_positions) == 0: raise ConfigurationError( - 'Could not find a valid joint configuration for desired ' - 'end effector pose.') + "Could not find a valid joint configuration for desired " + "end effector pose." + ) if len(valid_joint_positions) > 1: current_config = np.array(self.get_joint_positions()) # Sort based on angular distance - valid_joint_positions.sort( - key=lambda x: np.linalg.norm(current_config - x)) + valid_joint_positions.sort(key=lambda x: np.linalg.norm(current_config - x)) return np.array(valid_joint_positions) def solve_ik_via_jacobian( - self, position: Union[List[float], np.ndarray], - euler: Union[List[float], np.ndarray] = None, - quaternion: Union[List[float], np.ndarray] = None, - relative_to: Object = None) -> np.ndarray: + self, + position: Union[List[float], np.ndarray], + euler: Union[List[float], np.ndarray] = None, + quaternion: Union[List[float], np.ndarray] = None, + relative_to: Object = None, + ) -> np.ndarray: """Solves an IK group and returns the calculated joint values. This IK method performs a linearisation around the current robot @@ -223,25 +283,36 @@ def solve_ik_via_jacobian( # simIK.syncFromSim and followed by simIK.syncToSim "syncWorlds": False, } - self._sim_ik_api.setObjectPose(self._ik_env_handle, self._ik_target_handle, self._ik_target.get_pose().tolist(), self._sim_ik_api.handle_world) - result, reason, precision = self._sim_ik_api.handleGroup(self._ik_env_handle, self._ik_group_handle, options) + self._sim_ik_api.setObjectPose( + self._ik_env_handle, + self._ik_target_handle, + self._ik_target.get_pose().tolist(), + self._sim_ik_api.handle_world, + ) + result, reason, precision = self._sim_ik_api.handleGroup( + self._ik_env_handle, self._ik_group_handle, options + ) if reason == self._sim_ik_api.calc_notperformed: - raise IKError('IK failed. Calculation not performed.') + raise IKError("IK failed. Calculation not performed.") elif reason == self._sim_ik_api.calc_cannotinvert: - raise IKError('IK failed. Could not invert Jacobian.') + raise IKError("IK failed. Could not invert Jacobian.") elif reason == self._sim_ik_api.calc_notwithintolerance: - raise IKError('IK failed. Calculation not within tolerance.') + raise IKError("IK failed. Calculation not within tolerance.") elif reason == self._sim_ik_api.calc_stepstoobig: - raise IKError('IK failed. Calculation step too big.') + raise IKError("IK failed. Calculation step too big.") elif reason == self._sim_ik_api.calc_limithit: - raise IKError('IK failed. Calculation limit hit.') + raise IKError("IK failed. Calculation limit hit.") if result != self._sim_ik_api.result_success: - raise IKError('IK failed for unknown reason.') - joint_values = np.array([self._sim_ik_api.getJointPosition(self._ik_env_handle, jh) for jh in self._ik_joint_handles]) + raise IKError("IK failed for unknown reason.") + joint_values = np.array( + [ + self._sim_ik_api.getJointPosition(self._ik_env_handle, jh) + for jh in self._ik_joint_handles + ] + ) return joint_values - def get_path_from_cartesian_path(self, path: CartesianPath - ) -> ArmConfigurationPath: + def get_path_from_cartesian_path(self, path: CartesianPath) -> ArmConfigurationPath: """Translate a path from cartesian space, to arm configuration space. Note: It must be possible to reach the start of the path via a linear @@ -256,7 +327,7 @@ def get_path_from_cartesian_path(self, path: CartesianPath full_path = [] initial_config = self.get_joint_positions() delta = 0.05 - for i in np.linspace(delta, 1, int(1/delta)): + for i in np.linspace(delta, 1, int(1 / delta)): pos, ori = path.get_pose_on_path(i) intermediate_path = self.get_linear_path(pos, ori, steps=5) full_path.extend(intermediate_path._path_points) @@ -264,11 +335,15 @@ def get_path_from_cartesian_path(self, path: CartesianPath self.set_joint_positions(initial_config) return ArmConfigurationPath(self, full_path) - def get_linear_path(self, position: Union[List[float], np.ndarray], - euler: Union[List[float], np.ndarray] = None, - quaternion: Union[List[float], np.ndarray] = None, - steps=50, ignore_collisions=False, - relative_to: Object = None) -> ArmConfigurationPath: + def get_linear_path( + self, + position: Union[List[float], np.ndarray], + euler: Union[List[float], np.ndarray] = None, + quaternion: Union[List[float], np.ndarray] = None, + steps=50, + ignore_collisions=False, + relative_to: Object = None, + ) -> ArmConfigurationPath: """Gets a linear configuration path given a target pose. Generates a path that drives a robot from its current configuration @@ -294,7 +369,8 @@ def get_linear_path(self, position: Union[List[float], np.ndarray], """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( - 'Specify either euler or quaternion values, but not both.') + "Specify either euler or quaternion values, but not both." + ) prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position, relative_to) @@ -307,25 +383,40 @@ def get_linear_path(self, position: Union[List[float], np.ndarray], if not ignore_collisions: validation_callback = CALLBACK_NAME - self._sim_ik_api.setObjectPose(self._ik_env_handle, self._ik_target_handle, self._ik_target.get_pose().tolist(), self._sim_ik_api.handle_world) - ret_floats = self._sim_ik_api.generatePath(self._ik_env_handle, self._ik_group_handle, self._ik_joint_handles, self._ik_tip_handle, steps, validation_callback, self._coll_callback_args) + self._sim_ik_api.setObjectPose( + self._ik_env_handle, + self._ik_target_handle, + self._ik_target.get_pose().tolist(), + self._sim_ik_api.handle_world, + ) + ret_floats = self._sim_ik_api.generatePath( + self._ik_env_handle, + self._ik_group_handle, + self._ik_joint_handles, + self._ik_tip_handle, + steps, + validation_callback, + self._coll_callback_args, + ) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: - raise ConfigurationPathError('Could not create path.') + raise ConfigurationPathError("Could not create path.") return ArmConfigurationPath(self, ret_floats) - def get_nonlinear_path(self, position: Union[List[float], np.ndarray], - euler: Union[List[float], np.ndarray] = None, - quaternion: Union[List[float], np.ndarray] = None, - ignore_collisions=False, - trials=300, - max_configs=1, - distance_threshold: float = 0.65, - max_time_ms: int = 100, - trials_per_goal=1, - algorithm=Algos.SBL, - relative_to: Object = None - ) -> ArmConfigurationPath: + def get_nonlinear_path( + self, + position: Union[List[float], np.ndarray], + euler: Union[List[float], np.ndarray] = None, + quaternion: Union[List[float], np.ndarray] = None, + ignore_collisions=False, + trials=300, + max_configs=1, + distance_threshold: float = 0.65, + max_time_ms: int = 100, + trials_per_goal=1, + algorithm=Algos.SBL, + relative_to: Object = None, + ) -> ArmConfigurationPath: """Gets a non-linear (planned) configuration path given a target pose. A path is generated by finding several configs for a pose, and ranking @@ -358,21 +449,36 @@ def get_nonlinear_path(self, position: Union[List[float], np.ndarray], """ try: configs = self.solve_ik_via_sampling( - position, euler, quaternion, ignore_collisions, trials, - max_configs, distance_threshold, max_time_ms, relative_to) + position, + euler, + quaternion, + ignore_collisions, + trials, + max_configs, + distance_threshold, + max_time_ms, + relative_to, + ) except ConfigurationError as e: - raise ConfigurationPathError('Could not create path.') from e + raise ConfigurationPathError("Could not create path.") from e task_handle = self._sim_ompl_api.createTask("pyrep_task") self._sim_ompl_api.setVerboseLevel(task_handle, 0) algo = getattr(self._sim_ompl_api.Algorithm, algorithm.value) self._sim_ompl_api.setAlgorithm(task_handle, algo) - ss_handles = [self._sim_ompl_api.createStateSpaceForJoint(f"joint_{jh}_state_space", jh, 1) for jh in self._joint_handles] + ss_handles = [ + self._sim_ompl_api.createStateSpaceForJoint( + f"joint_{jh}_state_space", jh, 1 + ) + for jh in self._joint_handles + ] self._sim_ompl_api.setStateSpace(task_handle, ss_handles) use_for_projection = weights = np.ones_like(self._joint_handles).tolist() - self._sim_ompl_api.setStateSpaceForJoints(task_handle, self._joint_handles, use_for_projection, weights) + self._sim_ompl_api.setStateSpaceForJoints( + task_handle, self._joint_handles, use_for_projection, weights + ) self._sim_ompl_api.setStartState(task_handle, self.get_joint_positions()) self._sim_ompl_api.setGoalState(task_handle, configs[0].tolist()) if not ignore_collisions: @@ -381,25 +487,29 @@ def get_nonlinear_path(self, position: Union[List[float], np.ndarray], self._sim_ompl_api.setup(task_handle) max_time_s = float(max_time_ms) * 0.001 - solved, planned_path = self._sim_ompl_api.compute(task_handle, max_time_s, -1, 300) + solved, planned_path = self._sim_ompl_api.compute( + task_handle, max_time_s, -1, 300 + ) self._sim_ompl_api.destroyTask(task_handle) if not solved or len(planned_path) == 0: - raise ConfigurationPathError('Could not create path.') + raise ConfigurationPathError("Could not create path.") return ArmConfigurationPath(self, planned_path) - def get_path(self, position: Union[List[float], np.ndarray], - euler: Union[List[float], np.ndarray] = None, - quaternion: Union[List[float], np.ndarray] = None, - ignore_collisions=False, - trials=300, - max_configs=1, - distance_threshold: float = 0.65, - max_time_ms: int = 100, - trials_per_goal=1, - algorithm=Algos.SBL, - relative_to: Object = None - ) -> ArmConfigurationPath: + def get_path( + self, + position: Union[List[float], np.ndarray], + euler: Union[List[float], np.ndarray] = None, + quaternion: Union[List[float], np.ndarray] = None, + ignore_collisions=False, + trials=300, + max_configs=1, + distance_threshold: float = 0.65, + max_time_ms: int = 100, + trials_per_goal=1, + algorithm=Algos.SBL, + relative_to: Object = None, + ) -> ArmConfigurationPath: """Tries to get a linear path, failing that tries a non-linear path. Must specify either rotation in euler or quaternions, but not both! @@ -428,18 +538,31 @@ def get_path(self, position: Union[List[float], np.ndarray], :return: A linear or non-linear path in the arm configuration space. """ try: - p = self.get_linear_path(position, euler, quaternion, - ignore_collisions=ignore_collisions, - relative_to=relative_to) + p = self.get_linear_path( + position, + euler, + quaternion, + ignore_collisions=ignore_collisions, + relative_to=relative_to, + ) return p except ConfigurationPathError: pass # Allowed. Try again, but with non-linear. # This time if an exception is thrown, we dont want to catch it. p = self.get_nonlinear_path( - position, euler, quaternion, ignore_collisions, trials, max_configs, - distance_threshold, max_time_ms, trials_per_goal, algorithm, - relative_to) + position, + euler, + quaternion, + ignore_collisions, + trials, + max_configs, + distance_threshold, + max_time_ms, + trials_per_goal, + algorithm, + relative_to, + ) return p def get_tip(self) -> Dummy: @@ -457,11 +580,13 @@ def get_jacobian(self): :return: the row-major Jacobian matix. """ self._ik_target.set_matrix(self._ik_tip.get_matrix()) - jacobian, errorVector = self._sim_ik_api.computeGroupJacobian(self._ik_env_handle, self._ik_group_handle) - jacobian = np.array(jacobian).reshape((len(self.joints), 6), order='F') + jacobian, errorVector = self._sim_ik_api.computeGroupJacobian( + self._ik_env_handle, self._ik_group_handle + ) + jacobian = np.array(jacobian).reshape((len(self.joints), 6), order="F") return jacobian - def check_arm_collision(self, obj: 'Object' = None) -> bool: + def check_arm_collision(self, obj: "Object" = None) -> bool: """Checks whether two entities are colliding. :param obj: The other collidable object to check collision against, @@ -470,5 +595,7 @@ def check_arm_collision(self, obj: 'Object' = None) -> bool: :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) + result, colliding_handles = self._sim_api.checkCollision( + self._collision_collection_handle, handle + ) return result == 1 diff --git a/pyrep/robots/arms/baxter.py b/pyrep/robots/arms/baxter.py index b446284..60a27f9 100644 --- a/pyrep/robots/arms/baxter.py +++ b/pyrep/robots/arms/baxter.py @@ -2,12 +2,10 @@ class BaxterLeft(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'Baxter_leftArm', 7, base_name='Baxter') + super().__init__(count, "Baxter_leftArm", 7, base_name="Baxter") class BaxterRight(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'Baxter_rightArm', 7, base_name='Baxter') + super().__init__(count, "Baxter_rightArm", 7, base_name="Baxter") diff --git a/pyrep/robots/arms/jaco.py b/pyrep/robots/arms/jaco.py index 181dcfe..f3ad0c4 100644 --- a/pyrep/robots/arms/jaco.py +++ b/pyrep/robots/arms/jaco.py @@ -2,6 +2,5 @@ class Jaco(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'Jaco', 6) + super().__init__(count, "Jaco", 6) diff --git a/pyrep/robots/arms/lbr_iiwa_14_r820.py b/pyrep/robots/arms/lbr_iiwa_14_r820.py index 012a602..1e79907 100644 --- a/pyrep/robots/arms/lbr_iiwa_14_r820.py +++ b/pyrep/robots/arms/lbr_iiwa_14_r820.py @@ -2,6 +2,5 @@ class LBRIwaa14R820(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'LBR_iiwa_14_R820', 7) + super().__init__(count, "LBR_iiwa_14_R820", 7) diff --git a/pyrep/robots/arms/lbr_iiwa_7_r800.py b/pyrep/robots/arms/lbr_iiwa_7_r800.py index 971b240..b5d50db 100644 --- a/pyrep/robots/arms/lbr_iiwa_7_r800.py +++ b/pyrep/robots/arms/lbr_iiwa_7_r800.py @@ -2,6 +2,5 @@ class LBRIwaa7R800(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'LBR_iiwa_7_R800', 7) + super().__init__(count, "LBR_iiwa_7_R800", 7) diff --git a/pyrep/robots/arms/locobot_arm.py b/pyrep/robots/arms/locobot_arm.py index 4a233dd..ef6c41c 100644 --- a/pyrep/robots/arms/locobot_arm.py +++ b/pyrep/robots/arms/locobot_arm.py @@ -2,6 +2,5 @@ class LoCoBotArm(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'LoCoBotArm', 5) + super().__init__(count, "LoCoBotArm", 5) diff --git a/pyrep/robots/arms/mico.py b/pyrep/robots/arms/mico.py index 11052cf..8d55d52 100644 --- a/pyrep/robots/arms/mico.py +++ b/pyrep/robots/arms/mico.py @@ -2,6 +2,5 @@ class Mico(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'Mico', 6) + super().__init__(count, "Mico", 6) diff --git a/pyrep/robots/arms/panda.py b/pyrep/robots/arms/panda.py index 855cb60..d445895 100644 --- a/pyrep/robots/arms/panda.py +++ b/pyrep/robots/arms/panda.py @@ -2,6 +2,5 @@ class Panda(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'Panda', 7) + super().__init__(count, "Panda", 7) diff --git a/pyrep/robots/arms/sawyer.py b/pyrep/robots/arms/sawyer.py index 3623758..7e4d67d 100644 --- a/pyrep/robots/arms/sawyer.py +++ b/pyrep/robots/arms/sawyer.py @@ -2,6 +2,5 @@ class Sawyer(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'Sawyer', 7) + super().__init__(count, "Sawyer", 7) diff --git a/pyrep/robots/arms/ur10.py b/pyrep/robots/arms/ur10.py index c9bf987..5f22252 100644 --- a/pyrep/robots/arms/ur10.py +++ b/pyrep/robots/arms/ur10.py @@ -2,6 +2,5 @@ class UR10(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'UR10', 6) + super().__init__(count, "UR10", 6) diff --git a/pyrep/robots/arms/ur3.py b/pyrep/robots/arms/ur3.py index 9d49d58..5f795af 100644 --- a/pyrep/robots/arms/ur3.py +++ b/pyrep/robots/arms/ur3.py @@ -2,6 +2,5 @@ class UR3(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'UR3', 6) + super().__init__(count, "UR3", 6) diff --git a/pyrep/robots/arms/ur5.py b/pyrep/robots/arms/ur5.py index 9e5ee23..a7d9e2e 100644 --- a/pyrep/robots/arms/ur5.py +++ b/pyrep/robots/arms/ur5.py @@ -2,6 +2,5 @@ class UR5(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'UR5', 6) + super().__init__(count, "UR5", 6) diff --git a/pyrep/robots/arms/xarm7.py b/pyrep/robots/arms/xarm7.py index 5a241f7..63fb75d 100644 --- a/pyrep/robots/arms/xarm7.py +++ b/pyrep/robots/arms/xarm7.py @@ -2,6 +2,5 @@ class XArm7(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'xarm', 7) + super().__init__(count, "xarm", 7) diff --git a/pyrep/robots/arms/youBot.py b/pyrep/robots/arms/youBot.py index 86fb9ee..81bb606 100644 --- a/pyrep/robots/arms/youBot.py +++ b/pyrep/robots/arms/youBot.py @@ -2,6 +2,5 @@ class youBot(Arm): - def __init__(self, count: int = 0): - super().__init__(count, 'YouBot', 5) + super().__init__(count, "YouBot", 5) diff --git a/pyrep/robots/configuration_paths/arm_configuration_path.py b/pyrep/robots/configuration_paths/arm_configuration_path.py index 908b616..f98ad37 100644 --- a/pyrep/robots/configuration_paths/arm_configuration_path.py +++ b/pyrep/robots/configuration_paths/arm_configuration_path.py @@ -1,10 +1,15 @@ from pyrep.backend import sim_const as simc from pyrep.backend.sim import SimBackend -from pyrep.robots.configuration_paths.configuration_path import ( - ConfigurationPath) +from pyrep.robots.configuration_paths.configuration_path import ConfigurationPath 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. @@ -18,8 +23,7 @@ class ArmConfigurationPath(ConfigurationPath): control systems. """ - def __init__(self, arm: 'Arm', # type: ignore - path_points: Union[List[float], np.ndarray]): + def __init__(self, arm: Arm, path_points: Union[List[float], np.ndarray]): self._arm = arm self._path_points = np.asarray(path_points) self._rml_handle: Optional[int] = None @@ -47,8 +51,10 @@ def step(self) -> bool: :return: If the end of the trajectory has been reached. """ if self._path_done: - raise RuntimeError('This path has already been completed. ' - 'If you want to re-run, then call set_to_start.') + raise RuntimeError( + "This path has already been completed. " + "If you want to re-run, then call set_to_start." + ) if self._rml_handle is None: self._rml_handle = self._get_rml_handle() done = self._step_motion() == 1 @@ -56,16 +62,14 @@ def step(self) -> bool: return done def set_to_start(self, disable_dynamics=False) -> None: - """Sets the arm to the beginning of this path. - """ - start_config = self._path_points[:len(self._arm.joints)] + """Sets the arm to the beginning of this path.""" + start_config = self._path_points[: len(self._arm.joints)] self._arm.set_joint_positions(start_config, disable_dynamics=disable_dynamics) self._path_done = False def set_to_end(self, disable_dynamics=False) -> None: - """Sets the arm to the end of this path. - """ - final_config = self._path_points[-len(self._arm.joints):] + """Sets the arm to the end of this path.""" + final_config = self._path_points[-len(self._arm.joints) :] self._arm.set_joint_positions(final_config, disable_dynamics=disable_dynamics) def visualize(self) -> None: @@ -78,16 +82,17 @@ def visualize(self) -> None: raise RuntimeError("Can't visualise a path with no points.") tip = self._arm.get_tip() self._drawing_handle = self._sim_api.addDrawingObject( - simc.sim_drawing_lines, 3, 0, -1, 99999, [1, 0, 1]) + simc.sim_drawing_lines, 3, 0, -1, 99999, [1, 0, 1] + ) self._sim_api.addDrawingObjectItem(self._drawing_handle, None) init_angles = self._arm.get_joint_positions() - self._arm.set_joint_positions( - self._path_points[0: len(self._arm.joints)]) + self._arm.set_joint_positions(self._path_points[0 : len(self._arm.joints)]) prev_point = list(tip.get_position()) - for i in range(len(self._arm.joints), len(self._path_points), - len(self._arm.joints)): - points = self._path_points[i:i + len(self._arm.joints)] + for i in range( + len(self._arm.joints), len(self._path_points), len(self._arm.joints) + ): + points = self._path_points[i : i + len(self._arm.joints)] self._arm.set_joint_positions(points) p = list(tip.get_position()) self._sim_api.addDrawingObjectItem(self._drawing_handle, prev_point + p) @@ -97,8 +102,7 @@ def visualize(self) -> None: self._arm.set_joint_positions(init_angles) def clear_visualization(self) -> None: - """Clears/removes a visualization of the path in the scene. - """ + """Clears/removes a visualization of the path in the scene.""" if self._drawing_handle is not None: self._sim_api.addDrawingObjectItem(self._drawing_handle, None) @@ -113,35 +117,40 @@ def _get_rml_handle(self) -> int: max_accel = self._arm.max_acceleration max_jerk = self._arm.max_jerk lengths = self._get_path_point_lengths() - target_pos_vel = [lengths[-1],0] - previous_q = self._path_points[0:len(self._arm.joints)] + target_pos_vel = [lengths[-1], 0] + previous_q = self._path_points[0 : len(self._arm.joints)] # TODO sim.ruckigPos while True: pos_vel_accel = [0, 0, 0] rMax = 0 rml_handle = self._sim_api.ruckigPos( - 1, 0.0001, -1, pos_vel_accel, + 1, + 0.0001, + -1, + pos_vel_accel, [max_vel * vel_correction, max_accel, max_jerk], - [1], target_pos_vel) + [1], + target_pos_vel, + ) state = 0 while state == 0: - state, pos_vel_accel, synchronization_time = self._sim_api.ruckigStep(rml_handle, dt) + state, pos_vel_accel, synchronization_time = self._sim_api.ruckigStep( + rml_handle, dt + ) if state < 0: raise RuntimeError("Issue with stepping along path,") # state, pos_vel_accel = sim.simRMLStep(rml_handle, dt) if state >= 0: pos = pos_vel_accel[0] - for i in range(len(lengths)-1): + for i in range(len(lengths) - 1): if lengths[i] <= pos <= lengths[i + 1]: t = (pos - lengths[i]) / (lengths[i + 1] - lengths[i]) # For each joint offset = len(self._arm.joints) * i - p1 = self._path_points[ - offset:offset + self._num_joints] + p1 = self._path_points[offset : offset + self._num_joints] offset = len(self._arm.joints) * (i + 1) - p2 = self._path_points[ - offset:offset + self._num_joints] + p2 = self._path_points[offset : offset + self._num_joints] dx = p2 - p1 qs = p1 + dx * t dq = qs - previous_q @@ -158,9 +167,14 @@ def _get_rml_handle(self) -> int: break pos_vel_accel = [0, 0, 0] rml_handle = self._sim_api.ruckigPos( - 1, 0.0001, -1, pos_vel_accel, + 1, + 0.0001, + -1, + pos_vel_accel, [max_vel * vel_correction, max_accel, max_jerk], - [1], target_pos_vel) + [1], + target_pos_vel, + ) return rml_handle def _step_motion(self) -> int: @@ -168,7 +182,8 @@ def _step_motion(self) -> int: dt = self._sim_api.getSimulationTimeStep() lengths = self._get_path_point_lengths() state, pos_vel_accel, synchronization_time = self._sim_api.ruckigStep( - self._rml_handle, dt) + self._rml_handle, dt + ) if state >= 0: pos = pos_vel_accel[0] for i in range(len(lengths) - 1): @@ -176,11 +191,9 @@ def _step_motion(self) -> int: t = (pos - lengths[i]) / (lengths[i + 1] - lengths[i]) # For each joint offset = len(self._arm.joints) * i - p1 = self._path_points[ - offset:offset + len(self._arm.joints)] + p1 = self._path_points[offset : offset + len(self._arm.joints)] offset = self._arm._num_joints * (i + 1) - p2 = self._path_points[ - offset:offset + len(self._arm.joints)] + p2 = self._path_points[offset : offset + len(self._arm.joints)] dx = p2 - p1 qs = p1 + dx * t self._joint_position_action = qs @@ -192,12 +205,13 @@ def _step_motion(self) -> int: def _get_path_point_lengths(self) -> List[float]: path_points = self._path_points - prev_points = path_points[0:len(self._arm.joints)] - dists = [0.] + prev_points = path_points[0 : len(self._arm.joints)] + dists = [0.0] d = 0 - for i in range(len(self._arm.joints), len(self._path_points), - len(self._arm.joints)): - points = path_points[i:i + len(self._arm.joints)] + for i in range( + len(self._arm.joints), len(self._path_points), len(self._arm.joints) + ): + points = path_points[i : i + len(self._arm.joints)] d += np.sqrt(np.sum(np.square(prev_points - points))) dists.append(d) prev_points = points diff --git a/pyrep/robots/configuration_paths/configuration_path.py b/pyrep/robots/configuration_paths/configuration_path.py index cb0f1ec..894be14 100644 --- a/pyrep/robots/configuration_paths/configuration_path.py +++ b/pyrep/robots/configuration_paths/configuration_path.py @@ -1,6 +1,5 @@ class ConfigurationPath(object): - """Base class for paths expressed in joint configuration space. - """ + """Base class for paths expressed in joint configuration space.""" def step(self) -> bool: raise NotImplementedError() diff --git a/pyrep/robots/configuration_paths/holonomic_configuration_path.py b/pyrep/robots/configuration_paths/holonomic_configuration_path.py index 5f14f95..b2edc4c 100644 --- a/pyrep/robots/configuration_paths/holonomic_configuration_path.py +++ b/pyrep/robots/configuration_paths/holonomic_configuration_path.py @@ -1,6 +1,7 @@ -from pyrep.backend import sim, utils +from pyrep.backend import utils from pyrep.robots.configuration_paths.mobile_configuration_path import ( - MobileConfigurationPath) + MobileConfigurationPath, +) from pyrep.const import PYREP_SCRIPT_TYPE from math import sqrt @@ -27,11 +28,14 @@ def step(self) -> bool: """ if self._path_done: - raise RuntimeError('This path has already been completed. ' - 'If you want to re-run, then call set_to_start.') + raise RuntimeError( + "This path has already been completed. " + "If you want to re-run, then call set_to_start." + ) pos_inter = self._mobile.intermediate_target_base.get_position( - relative_to=self._mobile) + relative_to=self._mobile + ) if len(self._path_points) > 2: # Non-linear path if self.inter_done: @@ -40,19 +44,22 @@ def step(self) -> bool: self.inter_done = False handleBase = self._mobile.get_handle() - handleInterTargetBase = self._mobile.intermediate_target_base.get_handle() + handleInterTargetBase = ( + self._mobile.intermediate_target_base.get_handle() + ) __, ret_floats, _, _ = utils.script_call( - 'getBoxAdjustedMatrixAndFacingAngle@PyRep', + "getBoxAdjustedMatrixAndFacingAngle@PyRep", PYREP_SCRIPT_TYPE, - ints=[handleBase, handleInterTargetBase]) + ints=[handleBase, handleInterTargetBase], + ) m = ret_floats[:-1] angle = ret_floats[-1] self._mobile.intermediate_target_base.set_position( - [m[3], m[7], self._mobile.target_z]) - self._mobile.intermediate_target_base.set_orientation( - [0, 0, angle]) + [m[3], m[7], self._mobile.target_z] + ) + self._mobile.intermediate_target_base.set_orientation([0, 0, angle]) if sqrt((pos_inter[0]) ** 2 + (pos_inter[1]) ** 2) < 0.1: self.inter_done = True diff --git a/pyrep/robots/configuration_paths/mobile_configuration_path.py b/pyrep/robots/configuration_paths/mobile_configuration_path.py index c52b593..7f4c359 100644 --- a/pyrep/robots/configuration_paths/mobile_configuration_path.py +++ b/pyrep/robots/configuration_paths/mobile_configuration_path.py @@ -1,9 +1,6 @@ -from pyrep.backend import sim, utils -from pyrep.robots.configuration_paths.configuration_path import ( - ConfigurationPath) +from pyrep.backend import sim +from pyrep.robots.configuration_paths.configuration_path import ConfigurationPath from pyrep.robots.mobiles.mobile_base import MobileBase -from pyrep.const import PYREP_SCRIPT_TYPE -from math import sqrt from typing import List @@ -72,9 +69,13 @@ def visualize(self) -> None: tip = self._mobile self._drawing_handle = sim.simAddDrawingObject( - objectType=sim.sim_drawing_lines, size=3, duplicateTolerance=0, - parentObjectHandle=-1, maxItemCount=99999, - ambient_diffuse=[1, 0, 1]) + objectType=sim.sim_drawing_lines, + size=3, + duplicateTolerance=0, + parentObjectHandle=-1, + maxItemCount=99999, + ambient_diffuse=[1, 0, 1], + ) sim.simAddDrawingObjectItem(self._drawing_handle, None) init_pose = self._mobile.get_2d_pose() self._mobile.set_2d_pose(self._path_points[0][:3]) @@ -91,8 +92,7 @@ def visualize(self) -> None: self._mobile.set_2d_pose(init_pose[:3]) def clear_visualization(self) -> None: - """Clears/removes a visualization of the path in the scene. - """ + """Clears/removes a visualization of the path in the scene.""" if self._drawing_handle is not None: sim.simAddDrawingObjectItem(self._drawing_handle, None) @@ -108,7 +108,8 @@ def _next_i_path(self): def _set_inter_target(self, i): self._mobile.intermediate_target_base.set_position( - [self._path_points[i][0], self._path_points[i][1], - self._mobile.target_z]) + [self._path_points[i][0], self._path_points[i][1], self._mobile.target_z] + ) self._mobile.intermediate_target_base.set_orientation( - [0, 0, self._path_points[i][2]]) + [0, 0, self._path_points[i][2]] + ) diff --git a/pyrep/robots/configuration_paths/nonholonomic_configuration_path.py b/pyrep/robots/configuration_paths/nonholonomic_configuration_path.py index aff4fac..4ae965c 100644 --- a/pyrep/robots/configuration_paths/nonholonomic_configuration_path.py +++ b/pyrep/robots/configuration_paths/nonholonomic_configuration_path.py @@ -1,5 +1,6 @@ from pyrep.robots.configuration_paths.mobile_configuration_path import ( - MobileConfigurationPath) + MobileConfigurationPath, +) from math import sqrt @@ -25,11 +26,14 @@ def step(self) -> bool: """ if self._path_done: - raise RuntimeError('This path has already been completed. ' - 'If you want to re-run, then call set_to_start.') + raise RuntimeError( + "This path has already been completed. " + "If you want to re-run, then call set_to_start." + ) pos_inter = self._mobile.intermediate_target_base.get_position( - relative_to=self._mobile) + relative_to=self._mobile + ) if len(self._path_points) > 2: # Non-linear path if self.inter_done: diff --git a/pyrep/robots/end_effectors/baxter_gripper.py b/pyrep/robots/end_effectors/baxter_gripper.py index c016c90..a150fae 100644 --- a/pyrep/robots/end_effectors/baxter_gripper.py +++ b/pyrep/robots/end_effectors/baxter_gripper.py @@ -2,8 +2,9 @@ class BaxterGripper(Gripper): - def __init__(self, count: int = 0): - super().__init__(count, 'BaxterGripper', - ['BaxterGripper_leftJoint', - 'BaxterGripper_rightJoint']) + super().__init__( + count, + "BaxterGripper", + ["BaxterGripper_leftJoint", "BaxterGripper_rightJoint"], + ) diff --git a/pyrep/robots/end_effectors/baxter_suction_cup.py b/pyrep/robots/end_effectors/baxter_suction_cup.py index 4fc9ef5..fcc03fe 100644 --- a/pyrep/robots/end_effectors/baxter_suction_cup.py +++ b/pyrep/robots/end_effectors/baxter_suction_cup.py @@ -2,6 +2,5 @@ class BaxterSuctionCup(SuctionCup): - def __init__(self, count: int = 0): - super().__init__(count, 'BaxterSuctionCup') + super().__init__(count, "BaxterSuctionCup") diff --git a/pyrep/robots/end_effectors/gripper.py b/pyrep/robots/end_effectors/gripper.py index 8cb4c30..62e7f32 100644 --- a/pyrep/robots/end_effectors/gripper.py +++ b/pyrep/robots/end_effectors/gripper.py @@ -1,4 +1,4 @@ -from typing import List, Union +from typing import List from pyrep.objects.object import Object from pyrep.objects.proximity_sensor import ProximitySensor from pyrep.objects.force_sensor import ForceSensor @@ -9,14 +9,13 @@ class Gripper(RobotComponent): - """Represents all types of end-effectors, e.g. grippers. - """ + """Represents all types of end-effectors, e.g. grippers.""" def __init__(self, count: int, name: str, joint_names: List[str]): super().__init__(count, name, joint_names) - suffix = '' if count == 0 else '#%d' % (count - 1) - prox_name = '%s_attachProxSensor%s' % (name, suffix) - attach_name = '%s_attachPoint%s' % (name, suffix) + suffix = "" if count == 0 else "#%d" % (count - 1) + prox_name = "%s_attachProxSensor%s" % (name, suffix) + attach_name = "%s_attachPoint%s" % (name, suffix) self._proximity_sensor = ProximitySensor(prox_name) self._attach_point = ForceSensor(attach_name) self._old_parents: List[Object] = [] @@ -27,7 +26,7 @@ def __init__(self, count: int, name: str, joint_names: List[str]): self._touch_sensors = [] i = 0 while True: - fname = '%s_touchSensor%d%s' % (name, i, suffix) + fname = "%s_touchSensor%d%s" % (name, i, suffix) if not ForceSensor.exists(fname): break self._touch_sensors.append(ForceSensor(fname)) @@ -56,8 +55,7 @@ def release(self) -> None: Note: The does not actuate the gripper, but instead simply detaches any grasped objects. """ - for grasped_obj, old_parent in zip( - self._grasped_objects, self._old_parents): + for grasped_obj, old_parent in zip(self._grasped_objects, self._old_parents): # Check if the object still exists if grasped_obj.still_exists(): grasped_obj.set_parent(old_parent, keep_in_place=True) @@ -104,16 +102,14 @@ def actuate(self, amount: float, velocity: float) -> bool: current_positions = self.get_joint_positions() done = True - for i, (j, target, cur, prev) in enumerate(zip( - self.joints, target_pos, current_positions, - self._prev_positions)): + for i, (j, target, cur, prev) in enumerate( + zip(self.joints, target_pos, current_positions, self._prev_positions) + ): # Check if the joint has moved much - not_moving = (prev is not None and - np.fabs(cur - prev) < POSITION_ERROR) + not_moving = prev is not None and np.fabs(cur - prev) < POSITION_ERROR reached_target = np.fabs(target - cur) < POSITION_ERROR vel = -velocity if cur - target > 0 else velocity - oscillating = (self._prev_vels[i] is not None and - vel != self._prev_vels[i]) + oscillating = self._prev_vels[i] is not None and vel != self._prev_vels[i] if not_moving or reached_target or oscillating: j.set_joint_target_velocity(0) continue @@ -136,11 +132,16 @@ def get_open_amount(self) -> List[float]: _, joint_intervals_list = self.get_joint_intervals() joint_intervals = np.array(joint_intervals_list) joint_range = joint_intervals[:, 1] - joint_intervals[:, 0] - return list(np.clip((np.array( - self.get_joint_positions()) - joint_intervals[:, 0]) / - joint_range, 0.0, 1.0)) + return list( + np.clip( + (np.array(self.get_joint_positions()) - joint_intervals[:, 0]) + / joint_range, + 0.0, + 1.0, + ) + ) def get_touch_sensor_forces(self) -> List[List[float]]: if len(self._touch_sensors) == 0: - raise NotImplementedError('No touch sensors for this robot!') + raise NotImplementedError("No touch sensors for this robot!") return [ts.read()[0] for ts in self._touch_sensors] diff --git a/pyrep/robots/end_effectors/jaco_gripper.py b/pyrep/robots/end_effectors/jaco_gripper.py index 3e2693b..7df90f5 100644 --- a/pyrep/robots/end_effectors/jaco_gripper.py +++ b/pyrep/robots/end_effectors/jaco_gripper.py @@ -2,9 +2,13 @@ class JacoGripper(Gripper): - def __init__(self, count: int = 0): - super().__init__(count, 'JacoHand', - ['JacoHand_joint1_finger1', - 'JacoHand_joint1_finger2', - 'JacoHand_joint1_finger3']) + super().__init__( + count, + "JacoHand", + [ + "JacoHand_joint1_finger1", + "JacoHand_joint1_finger2", + "JacoHand_joint1_finger3", + ], + ) diff --git a/pyrep/robots/end_effectors/locobot_gripper.py b/pyrep/robots/end_effectors/locobot_gripper.py index 7b703ed..1a72eea 100644 --- a/pyrep/robots/end_effectors/locobot_gripper.py +++ b/pyrep/robots/end_effectors/locobot_gripper.py @@ -2,7 +2,7 @@ class LoCoBotGripper(Gripper): - def __init__(self, count: int = 0): - super().__init__(count, 'LoCoBotGripper', - ['LoCoBotGripper_joint1', 'LoCoBotGripper_joint2']) + super().__init__( + count, "LoCoBotGripper", ["LoCoBotGripper_joint1", "LoCoBotGripper_joint2"] + ) diff --git a/pyrep/robots/end_effectors/mico_gripper.py b/pyrep/robots/end_effectors/mico_gripper.py index a10240c..d1a3cb4 100644 --- a/pyrep/robots/end_effectors/mico_gripper.py +++ b/pyrep/robots/end_effectors/mico_gripper.py @@ -2,8 +2,7 @@ class MicoGripper(Gripper): - def __init__(self, count: int = 0): - super().__init__(count, 'MicoHand', - ['MicoHand_joint1_finger1', - 'MicoHand_joint1_finger3']) + super().__init__( + count, "MicoHand", ["MicoHand_joint1_finger1", "MicoHand_joint1_finger3"] + ) diff --git a/pyrep/robots/end_effectors/panda_gripper.py b/pyrep/robots/end_effectors/panda_gripper.py index a6e19ef..fd8a811 100644 --- a/pyrep/robots/end_effectors/panda_gripper.py +++ b/pyrep/robots/end_effectors/panda_gripper.py @@ -2,7 +2,7 @@ class PandaGripper(Gripper): - def __init__(self, count: int = 0): - super().__init__(count, 'Panda_gripper', - ['Panda_gripper_joint1', 'Panda_gripper_joint2']) + super().__init__( + count, "Panda_gripper", ["Panda_gripper_joint1", "Panda_gripper_joint2"] + ) diff --git a/pyrep/robots/end_effectors/robotiq85_gripper.py b/pyrep/robots/end_effectors/robotiq85_gripper.py index d4beac5..42da5dc 100644 --- a/pyrep/robots/end_effectors/robotiq85_gripper.py +++ b/pyrep/robots/end_effectors/robotiq85_gripper.py @@ -1,35 +1,35 @@ from typing import List from pyrep.objects import Joint -from pyrep.robots.end_effectors.gripper import Gripper, POSITION_ERROR +from pyrep.robots.end_effectors.gripper import Gripper import numpy as np POSITION_ERROR = 0.00001 class Robotiq85Gripper(Gripper): - def __init__(self, count: int = 0): - super().__init__(count, 'ROBOTIQ_85', ['ROBOTIQ_85_active1', - 'ROBOTIQ_85_active2']) + super().__init__( + count, "ROBOTIQ_85", ["ROBOTIQ_85_active1", "ROBOTIQ_85_active2"] + ) self._open_amount_query_joints = [ - Joint(jname) for jname in [ - 'ROBOTIQ_85_Rjoint1', 'ROBOTIQ_85_Ljoint1']] + Joint(jname) for jname in ["ROBOTIQ_85_Rjoint1", "ROBOTIQ_85_Ljoint1"] + ] def actuate(self, amount: float, velocity: float) -> bool: if amount != 0.0 and amount != 1.0: raise ValueError( - 'This gripper currently only supports fully open or closed.') + "This gripper currently only supports fully open or closed." + ) current_positions = self.get_joint_positions() vel = velocity if amount == 1.0 else -velocity done = True - for i, (j, cur, prev) in enumerate(zip( - self.joints, current_positions, - self._prev_positions)): + for i, (j, cur, prev) in enumerate( + zip(self.joints, current_positions, self._prev_positions) + ): # Check if the joint has moved much - not_moving = (prev is not None and - np.fabs(cur - prev) < POSITION_ERROR) + not_moving = prev is not None and np.fabs(cur - prev) < POSITION_ERROR if not_moving: j.set_joint_target_velocity(0) continue @@ -49,10 +49,22 @@ def get_open_amount(self) -> List[float]: :return: A list of floats between 0 and 1 representing the gripper open state for each joint. 1 means open, whilst 0 means closed. """ - joint_intervals = np.array([j.get_joint_interval()[1] - for j in self._open_amount_query_joints]) + joint_intervals = np.array( + [j.get_joint_interval()[1] for j in self._open_amount_query_joints] + ) joint_range = joint_intervals[:, 1] - joint_intervals[:, 0] # Flip open amount - return list(1. - np.clip((np.array( - [j.get_joint_position() for j in self._open_amount_query_joints] - ) - joint_intervals[:, 0]) / joint_range, 0.0, 1.0)) + return list( + 1.0 + - np.clip( + ( + np.array( + [j.get_joint_position() for j in self._open_amount_query_joints] + ) + - joint_intervals[:, 0] + ) + / joint_range, + 0.0, + 1.0, + ) + ) diff --git a/pyrep/robots/end_effectors/suction_cup.py b/pyrep/robots/end_effectors/suction_cup.py index 8d87fda..b30aea3 100644 --- a/pyrep/robots/end_effectors/suction_cup.py +++ b/pyrep/robots/end_effectors/suction_cup.py @@ -6,14 +6,13 @@ class SuctionCup(Shape): - """Represents all suction cups. - """ + """Represents all suction cups.""" def __init__(self, count: int, name: str, base_name: str = None): - suffix = '' if count == 0 else '#%d' % (count - 1) + suffix = "" if count == 0 else "#%d" % (count - 1) super().__init__(name + suffix if base_name is None else base_name + suffix) - self._proximity_sensor = ProximitySensor('%s_sensor%s' % (name, suffix)) - self._attach_point = ForceSensor('%s_connection%s' % (name, suffix)) + self._proximity_sensor = ProximitySensor("%s_sensor%s" % (name, suffix)) + self._attach_point = ForceSensor("%s_connection%s" % (name, suffix)) self._old_parents: List[Object] = [] self._grasped_objects: List[Object] = [] @@ -41,8 +40,7 @@ def release(self) -> None: Note: The does not actuate the gripper, but instead simply detaches any grasped objects. """ - for grasped_obj, old_parent in zip( - self._grasped_objects, self._old_parents): + for grasped_obj, old_parent in zip(self._grasped_objects, self._old_parents): # Check if the object still exists if grasped_obj.still_exists(): grasped_obj.set_parent(old_parent, keep_in_place=True) diff --git a/pyrep/robots/end_effectors/xarm_gripper.py b/pyrep/robots/end_effectors/xarm_gripper.py index 5a8af03..c15a424 100644 --- a/pyrep/robots/end_effectors/xarm_gripper.py +++ b/pyrep/robots/end_effectors/xarm_gripper.py @@ -2,12 +2,16 @@ class XArmGripper(Gripper): - def __init__(self, count: int = 0): - super().__init__(count, 'xarm_gripper', - ['xarm_left_inner_knuckle_joint', - 'xarm_right_inner_knuckle_joint', - 'xarm_drive_joint', - 'xarm_left_finger_joint', - 'xarm_right_outer_knuckle_joint', - 'xarm_right_finger_joint']) + super().__init__( + count, + "xarm_gripper", + [ + "xarm_left_inner_knuckle_joint", + "xarm_right_inner_knuckle_joint", + "xarm_drive_joint", + "xarm_left_finger_joint", + "xarm_right_outer_knuckle_joint", + "xarm_right_finger_joint", + ], + ) diff --git a/pyrep/robots/robot_component.py b/pyrep/robots/robot_component.py index 31ab419..690ff66 100644 --- a/pyrep/robots/robot_component.py +++ b/pyrep/robots/robot_component.py @@ -9,22 +9,20 @@ class RobotComponent(Object): - """Collection of joints representing arms, end effectors, mobile bases, etc. - """ - - def __init__(self, count: int, name: str, joint_names: List[str], - base_name: str = None): - suffix = '' if count == 0 else '#%d' % (count - 1) - super().__init__( - name + suffix if base_name is None else base_name + suffix) + """Collection of joints representing arms, end effectors, mobile bases, etc.""" + + def __init__( + self, count: int, name: str, joint_names: List[str], base_name: str = None + ): + suffix = "" if count == 0 else "#%d" % (count - 1) + super().__init__(name + suffix if base_name is None else base_name + suffix) self._num_joints = len(joint_names) # Joint handles - self.joints = [Joint(jname + suffix) - for jname in joint_names] + self.joints = [Joint(jname + suffix) for jname in joint_names] self._joint_handles = [j.get_handle() for j in self.joints] - def copy(self) -> 'RobotComponent': + def copy(self) -> "RobotComponent": """Copy and pastes the arm in the scene. The arm is copied together with all its associated calculation @@ -36,7 +34,7 @@ def copy(self) -> 'RobotComponent': handle = self._sim_api.copyPasteObjects([self._handle], 1)[0] name = self._sim_api.getObjectName(handle) # Find the number of this arm - num = name[name.rfind('#') + 1:] + num = name[name.rfind("#") + 1 :] if len(num) > 0: num = int(num) + 1 else: @@ -74,8 +72,9 @@ def get_joint_positions(self) -> List[float]: """ return [j.get_joint_position() for j in self.joints] - def set_joint_positions(self, positions: List[float], - disable_dynamics: bool = False) -> None: + def set_joint_positions( + self, positions: List[float], disable_dynamics: bool = False + ) -> None: """Sets the intrinsic position of the joints. See :py:meth:`Joint.set_joint_position` for more information. @@ -89,8 +88,10 @@ def set_joint_positions(self, positions: List[float], """ self._assert_len(positions) if not disable_dynamics: - [self._sim_api.setJointPosition(jh, p) # type: ignore - for jh, p in zip(self._joint_handles, positions)] + [ + self._sim_api.setJointPosition(jh, p) # type: ignore + for jh, p in zip(self._joint_handles, positions) + ] return is_model = self.is_model() @@ -104,10 +105,14 @@ def set_joint_positions(self, positions: List[float], sim_instance = SimBackend() with utils.step_lock: sim_instance.simLoop() # Have to step for changes to take effect - [self._sim_api.setJointPosition(jh, p) # type: ignore - for jh, p in zip(self._joint_handles, positions)] - [j.set_joint_target_position(p) # type: ignore - for j, p in zip(self.joints, positions)] + [ + self._sim_api.setJointPosition(jh, p) # type: ignore + for jh, p in zip(self._joint_handles, positions) + ] + [ + j.set_joint_target_position(p) # type: ignore + for j, p in zip(self.joints, positions) + ] with utils.step_lock: sim_instance.simLoop() # Have to step for changes to take effect # Re-enable the dynamics @@ -131,17 +136,18 @@ def set_joint_target_positions(self, positions: List[float]) -> None: linear values depending on the joint type). """ self._assert_len(positions) - [j.set_joint_target_position(p) # type: ignore - for j, p in zip(self.joints, positions)] + [ + j.set_joint_target_position(p) # type: ignore + for j, p in zip(self.joints, positions) + ] def get_joint_target_velocities(self) -> List[float]: """Retrieves the intrinsic target velocities of the joints. - :return: List of the target velocity of the joints (linear or angular - velocity depending on the joint-type). - """ - return [j.get_joint_target_velocity() # type: ignore - for j in self.joints] + :return: List of the target velocity of the joints (linear or angular + velocity depending on the joint-type). + """ + return [j.get_joint_target_velocity() for j in self.joints] # type: ignore def set_joint_target_velocities(self, velocities: List[float]) -> None: """Sets the intrinsic target velocities of the joints. @@ -150,8 +156,10 @@ def set_joint_target_velocities(self, velocities: List[float]) -> None: or angular velocities depending on the joint-type). """ self._assert_len(velocities) - [j.set_joint_target_velocity(v) # type: ignore - for j, v in zip(self.joints, velocities)] + [ + j.set_joint_target_velocity(v) # type: ignore + for j, v in zip(self.joints, velocities) + ] def get_joint_forces(self) -> List[float]: """Retrieves the forces or torques of the joints. @@ -172,8 +180,7 @@ def set_joint_forces(self, forces: List[float]) -> None: These cannot be negative values. """ self._assert_len(forces) - [j.set_joint_force(f) # type: ignore - for j, f in zip(self.joints, forces)] + [j.set_joint_force(f) for j, f in zip(self.joints, forces)] # type: ignore def get_joint_velocities(self) -> List[float]: """Get the current joint velocities. @@ -199,8 +206,9 @@ def get_joint_intervals(self) -> Tuple[List[bool], List[List[float]]]: intervals.append(i) return cyclics, intervals - def set_joint_intervals(self, cyclic: List[bool], - intervals: List[List[float]]) -> None: + def set_joint_intervals( + self, cyclic: List[bool], intervals: List[List[float]] + ) -> None: """Sets the interval parameters of the joints (i.e. range values). See :py:meth:`Joint.set_joint_interval` for more information. @@ -211,14 +219,16 @@ def set_joint_intervals(self, cyclic: List[bool], """ self._assert_len(cyclic) self._assert_len(intervals) - [j.set_joint_interval(c, i) # type: ignore - for j, c, i in zip( self.joints, cyclic, intervals)] + [ + j.set_joint_interval(c, i) # type: ignore + for j, c, i in zip(self.joints, cyclic, intervals) + ] def get_joint_upper_velocity_limits(self) -> List[float]: """Gets upper velocity limits of the joints. - :return: List of the upper velocity limits. - """ + :return: List of the upper velocity limits. + """ return [j.get_joint_upper_velocity_limit() for j in self.joints] def set_control_loop_enabled(self, value: bool) -> None: @@ -226,8 +236,7 @@ def set_control_loop_enabled(self, value: bool) -> None: :param value: The new value for the control loop state. """ - [j.set_control_loop_enabled(value) # type: ignore - for j in self.joints] + [j.set_control_loop_enabled(value) for j in self.joints] # type: ignore def set_motor_locked_at_zero_velocity(self, value: bool) -> None: """Sets if motor is locked when target velocity is zero for all joints. @@ -237,16 +246,17 @@ def set_motor_locked_at_zero_velocity(self, value: bool) -> None: :param value: If the motors should be locked at zero velocity. """ - [j.set_motor_locked_at_zero_velocity(value) # type: ignore - for j in self.joints] + [ + j.set_motor_locked_at_zero_velocity(value) # type: ignore + for j in self.joints + ] def set_joint_mode(self, value: JointMode) -> None: """Sets the operation mode of the joint group. :param value: The new joint mode value. """ - [j.set_joint_mode(value) # type: ignore - for j in self.joints] + [j.set_joint_mode(value) for j in self.joints] # type: ignore def get_joint_modes(self) -> List[JointMode]: """Gets the operation mode of the joint group. @@ -264,10 +274,11 @@ def get_visuals(self) -> List[Object]: :return: A list of visual shapes. """ tree = self.get_objects_in_tree(ObjectType.SHAPE, exclude_base=False) - return [obj for obj in tree if 'visual' in obj.get_name()] + return [obj for obj in tree if "visual" in obj.get_name()] def _assert_len(self, inputs: list) -> None: if len(self.joints) != len(inputs): raise RuntimeError( - 'Tried to set values for %d joints, but joint group consists ' - 'of %d joints.' % (len(inputs), len(self.joints))) + "Tried to set values for %d joints, but joint group consists " + "of %d joints." % (len(inputs), len(self.joints)) + ) diff --git a/pyrep/sensors/accelerometer.py b/pyrep/sensors/accelerometer.py index 4f2910f..90fcab6 100644 --- a/pyrep/sensors/accelerometer.py +++ b/pyrep/sensors/accelerometer.py @@ -1,6 +1,5 @@ from typing import List -from pyrep.backend import sim from pyrep.const import ObjectType from pyrep.objects.force_sensor import ForceSensor from pyrep.objects.object import Object @@ -8,13 +7,12 @@ class Accelerometer(Object): - """An object able to measure accelerations that are applied to it. - """ + """An object able to measure accelerations that are applied to it.""" def __init__(self, name): super().__init__(name) - self._mass_object = Shape('%s_mass' % (self.get_name())) - self._sensor = ForceSensor('%s_force_sensor' % (self.get_name())) + self._mass_object = Shape("%s_mass" % (self.get_name())) + self._sensor = ForceSensor("%s_force_sensor" % (self.get_name())) def _get_requested_type(self) -> ObjectType: return ObjectType(self._sim_api.getObjectType(self.get_handle())) diff --git a/pyrep/sensors/gyroscope.py b/pyrep/sensors/gyroscope.py index 7287776..8e1062f 100644 --- a/pyrep/sensors/gyroscope.py +++ b/pyrep/sensors/gyroscope.py @@ -1,18 +1,18 @@ -from pyrep.backend import sim from pyrep.const import ObjectType from pyrep.objects.object import Object class Gyroscope(Object): - """An object able to measure angular velocities that are applied to it. - """ + """An object able to measure angular velocities that are applied to it.""" + def __init__(self, name): super().__init__(name) - self._ref = '%s_reference' % (self.get_name()) + self._ref = "%s_reference" % (self.get_name()) self._last_time = self._sim_api.getSimulationTime() - self._old_transformation_matrix = self.get_matrix()[:3, :4].reshape( - (12, )).tolist() + self._old_transformation_matrix = ( + self.get_matrix()[:3, :4].reshape((12,)).tolist() + ) def _get_requested_type(self) -> ObjectType: return ObjectType(self._sim_api.getObjectType(self.get_handle())) @@ -27,8 +27,7 @@ def read(self): dt = current_time - self._last_time inv_old_matrix = self._sim_api.getMatrixInverse(self._old_transformation_matrix) - transformation_matrix = self.get_matrix()[:3, :4].reshape( - (12, )).tolist() + transformation_matrix = self.get_matrix()[:3, :4].reshape((12,)).tolist() mat = self._sim_api.multiplyMatrices(inv_old_matrix, transformation_matrix) euler_angles = self._sim_api.getEulerAnglesFromMatrix(mat) diff --git a/pyrep/sensors/spherical_vision_sensor.py b/pyrep/sensors/spherical_vision_sensor.py index 30829bc..76f84b6 100644 --- a/pyrep/sensors/spherical_vision_sensor.py +++ b/pyrep/sensors/spherical_vision_sensor.py @@ -1,29 +1,29 @@ import numpy as np from typing import List, Sequence -from pyrep.backend import sim, utils from pyrep.backend.sim import SimBackend from pyrep.objects.object import Object from pyrep.objects.vision_sensor import VisionSensor -from pyrep.const import PYREP_SCRIPT_TYPE, RenderMode, ObjectType +from pyrep.const import RenderMode, ObjectType MIN_DIVISOR = 1e-12 class SphericalVisionSensor(Object): - """An object able capture 360 degree omni-directional images. - """ + """An object able capture 360 degree omni-directional images.""" def __init__(self, name): super().__init__(name) # final omni-directional sensors - self._sensor_depth = VisionSensor('%s_sensorDepth' % (self.get_name())) - self._sensor_rgb = VisionSensor('%s_sensorRGB' % (self.get_name())) + self._sensor_depth = VisionSensor("%s_sensorDepth" % (self.get_name())) + self._sensor_rgb = VisionSensor("%s_sensorRGB" % (self.get_name())) # directed sub-sensors - names = ['front', 'top', 'back', 'bottom', 'left', 'right'] - self._six_sensors = [VisionSensor('%s_%s' % (self.get_name(), n)) for n in names] + names = ["front", "top", "back", "bottom", "left", "right"] + self._six_sensors = [ + VisionSensor("%s_%s" % (self.get_name(), n)) for n in names + ] self._front = self._six_sensors[0] # directed sub-sensor handles list @@ -49,7 +49,9 @@ def __init__(self, name): self._clipping_plane_diff = self._far_clipping_plane - self._near_clipping_plane # projective cam region image - self._planar_to_radial_depth_scalars = self._get_planar_to_radial_depth_scalars() + self._planar_to_radial_depth_scalars = ( + self._get_planar_to_radial_depth_scalars() + ) # Private # # --------# @@ -61,25 +63,35 @@ def _set_all_to_explicit_handling(self): sensor.set_explicit_handling(1) def _assert_matching_resolutions(self): - assert np.array_equal(self._sensor_depth.get_resolution(), self._sensor_rgb.get_resolution()) + assert np.array_equal( + self._sensor_depth.get_resolution(), self._sensor_rgb.get_resolution() + ) front_sensor_res = self._front.get_resolution() for sensor in self._six_sensors[1:]: assert np.array_equal(sensor.get_resolution(), front_sensor_res) def _assert_matching_render_modes(self): - assert self._sensor_depth.get_render_mode() == self._sensor_rgb.get_render_mode() + assert ( + self._sensor_depth.get_render_mode() == self._sensor_rgb.get_render_mode() + ) front_sensor_render_mode = self._front.get_render_mode() for sensor in self._six_sensors[1:]: assert sensor.get_render_mode() == front_sensor_render_mode def _assert_matching_entities_to_render(self): - assert self._sensor_depth.get_entity_to_render() == self._sensor_rgb.get_entity_to_render() + assert ( + self._sensor_depth.get_entity_to_render() + == self._sensor_rgb.get_entity_to_render() + ) front_sensor_entity_to_render = self._front.get_entity_to_render() for sensor in self._six_sensors[1:]: assert sensor.get_entity_to_render() == front_sensor_entity_to_render def _assert_matching_window_sizes(self): - assert self._sensor_depth.get_windowed_size() == self._sensor_rgb.get_windowed_size() + assert ( + self._sensor_depth.get_windowed_size() + == self._sensor_rgb.get_windowed_size() + ) def _assert_matching_near_clipping_planes(self): front_sensor_near = self._front.get_near_clipping_plane() @@ -96,38 +108,56 @@ def _get_requested_type(self) -> ObjectType: @staticmethod def _create_uniform_pixel_coords_image(image_dims): - pixel_x_coords = np.reshape(np.tile(np.arange(image_dims[1]), [image_dims[0]]), - (image_dims[0], image_dims[1], 1)).astype(np.float32) - pixel_y_coords_ = np.reshape(np.tile(np.arange(image_dims[0]), [image_dims[1]]), - (image_dims[1], image_dims[0], 1)).astype(np.float32) + pixel_x_coords = np.reshape( + np.tile(np.arange(image_dims[1]), [image_dims[0]]), + (image_dims[0], image_dims[1], 1), + ).astype(np.float32) + pixel_y_coords_ = np.reshape( + np.tile(np.arange(image_dims[0]), [image_dims[1]]), + (image_dims[1], image_dims[0], 1), + ).astype(np.float32) pixel_y_coords = np.transpose(pixel_y_coords_, (1, 0, 2)) return np.concatenate((pixel_x_coords, pixel_y_coords), -1) def _get_planar_to_radial_depth_scalars(self): - # reference: https://en.wikipedia.org/wiki/Spherical_coordinate_system#Cartesian_coordinates # polar coord image - coord_img = self._create_uniform_pixel_coords_image([self._omni_resolution[1], self._omni_resolution[0]]) + coord_img = self._create_uniform_pixel_coords_image( + [self._omni_resolution[1], self._omni_resolution[0]] + ) pixels_per_rad = self._omni_resolution[1] / np.pi polar_angles = coord_img / pixels_per_rad phi = polar_angles[..., 0:1] theta = polar_angles[..., 1:2] # image borders wrt the 6 projective cameras - phis_rel = ((phi + np.pi/4) % (np.pi/2)) / (np.pi/2) - lower_borders = np.arccos(1/((phis_rel*2-1)**2 + 2)**0.5) + phis_rel = ((phi + np.pi / 4) % (np.pi / 2)) / (np.pi / 2) + lower_borders = np.arccos(1 / ((phis_rel * 2 - 1) ** 2 + 2) ** 0.5) upper_borders = np.pi - lower_borders # omni image regions from the 6 projective cameras xy_region = np.logical_and(theta <= upper_borders, theta >= lower_borders) - pos_x = np.logical_and(xy_region, np.logical_or(phi <= 45 * np.pi/180, phi >= (45 + 270) * np.pi/180)) - pos_y = np.logical_and(xy_region, - np.logical_and(phi >= 45 * np.pi/180, phi <= (45 + 90) * np.pi/180)) - neg_x = np.logical_and(xy_region, - np.logical_and(phi >= (45 + 90) * np.pi/180, phi <= (45 + 180) * np.pi/180)) - neg_y = np.logical_and(xy_region, - np.logical_and(phi >= (45 + 180) * np.pi/180, phi <= (45 + 270) * np.pi/180)) + pos_x = np.logical_and( + xy_region, + np.logical_or(phi <= 45 * np.pi / 180, phi >= (45 + 270) * np.pi / 180), + ) + pos_y = np.logical_and( + xy_region, + np.logical_and(phi >= 45 * np.pi / 180, phi <= (45 + 90) * np.pi / 180), + ) + neg_x = np.logical_and( + xy_region, + np.logical_and( + phi >= (45 + 90) * np.pi / 180, phi <= (45 + 180) * np.pi / 180 + ), + ) + neg_y = np.logical_and( + xy_region, + np.logical_and( + phi >= (45 + 180) * np.pi / 180, phi <= (45 + 270) * np.pi / 180 + ), + ) pos_z = theta <= lower_borders neg_z = theta >= upper_borders @@ -138,18 +168,34 @@ def _get_planar_to_radial_depth_scalars(self): cos_phi = np.cos(phi) # reciprocal terms - recip_sin_theta_cos_phi = 1/(sin_theta * cos_phi + MIN_DIVISOR) - recip_sin_theta_sin_phi = 1/(sin_theta * sin_phi + MIN_DIVISOR) - recip_cos_theta = 1/(cos_theta + MIN_DIVISOR) + recip_sin_theta_cos_phi = 1 / (sin_theta * cos_phi + MIN_DIVISOR) + recip_sin_theta_sin_phi = 1 / (sin_theta * sin_phi + MIN_DIVISOR) + recip_cos_theta = 1 / (cos_theta + MIN_DIVISOR) # planar to radial depth scalars - return np.where(pos_x, recip_sin_theta_cos_phi, - np.where(neg_x, -recip_sin_theta_cos_phi, - np.where(pos_y, recip_sin_theta_sin_phi, - np.where(neg_y, -recip_sin_theta_sin_phi, - np.where(pos_z, recip_cos_theta, - np.where(neg_z, -recip_cos_theta, - np.zeros_like(recip_cos_theta)))))))[..., 0] + return np.where( + pos_x, + recip_sin_theta_cos_phi, + np.where( + neg_x, + -recip_sin_theta_cos_phi, + np.where( + pos_y, + recip_sin_theta_sin_phi, + np.where( + neg_y, + -recip_sin_theta_sin_phi, + np.where( + pos_z, + recip_cos_theta, + np.where( + neg_z, -recip_cos_theta, np.zeros_like(recip_cos_theta) + ), + ), + ), + ), + ), + )[..., 0] # Public # # -------# @@ -157,10 +203,16 @@ def _get_planar_to_radial_depth_scalars(self): def handle_explicitly(self) -> None: """Handle spherical vision sensor explicitly. - This enables capturing image (e.g., capture_rgb()) - without PyRep.step(). + This enables capturing image (e.g., capture_rgb()) + without PyRep.step(). """ - SimBackend().sim_vision_api.handleSpherical(self._sensor_rgb._handle, self._six_sensor_handles, 360, 180, self._sensor_depth._handle) + SimBackend().sim_vision_api.handleSpherical( + self._sensor_rgb._handle, + self._six_sensor_handles, + 360, + 180, + self._sensor_depth._handle, + ) def capture_rgb(self) -> np.ndarray: """Retrieves the rgb-image of a spherical vision sensor. @@ -175,43 +227,48 @@ def capture_depth(self, in_meters=False) -> np.ndarray: :param in_meters: Whether the depth should be returned in meters. :return: A numpy array of size (width, height) """ - planar_depth = self._sensor_depth.capture_rgb()[:, :, 0]*self._clipping_plane_diff + self._near_clipping_plane + planar_depth = ( + self._sensor_depth.capture_rgb()[:, :, 0] * self._clipping_plane_diff + + self._near_clipping_plane + ) radial_depth = planar_depth * self._planar_to_radial_depth_scalars if in_meters: return radial_depth - return (radial_depth - self._near_clipping_plane)/self._clipping_plane_diff + return (radial_depth - self._near_clipping_plane) / self._clipping_plane_diff def get_resolution(self) -> List[int]: - """ Return the spherical vision sensor's resolution. + """Return the spherical vision sensor's resolution. :return: Resolution [x, y] """ return self._sensor_depth.get_resolution() def set_resolution(self, resolution: List[int]) -> None: - """ Set the spherical vision sensor's resolution. + """Set the spherical vision sensor's resolution. :param resolution: New resolution [x, y] """ - if resolution[0] != 2*resolution[1]: - raise Exception('Spherical vision sensors must have an X resolution 2x the Y resolution') + if resolution[0] != 2 * resolution[1]: + raise Exception( + "Spherical vision sensors must have an X resolution 2x the Y resolution" + ) if resolution[1] % 2 != 0: - raise Exception('Spherical vision sensors must have an even Y resolution') - box_sensor_resolution = [int(resolution[1]/2), int(resolution[1]/2)] + raise Exception("Spherical vision sensors must have an even Y resolution") + box_sensor_resolution = [int(resolution[1] / 2), int(resolution[1] / 2)] for sensor in self._six_sensors: sensor.set_resolution(box_sensor_resolution) self._sensor_depth.set_resolution(resolution) self._sensor_rgb.set_resolution(resolution) def get_render_mode(self) -> RenderMode: - """ Retrieves the spherical vision sensor's rendering mode + """Retrieves the spherical vision sensor's rendering mode :return: RenderMode for the current rendering mode. """ return self._sensor_depth.get_render_mode() def set_render_mode(self, render_mode: RenderMode) -> None: - """ Set the spherical vision sensor's rendering mode + """Set the spherical vision sensor's rendering mode :param render_mode: The new sensor rendering mode, one of: RenderMode.OPENGL @@ -245,14 +302,14 @@ def set_windowed_size(self, resolution: Sequence[int] = (0, 0)) -> None: self._sensor_rgb.set_windowed_size(resolution) def get_near_clipping_plane(self) -> float: - """ Get the spherical depth sensor's near clipping plane. + """Get the spherical depth sensor's near clipping plane. :return: Near clipping plane (metres) """ return self._front.get_near_clipping_plane() def set_near_clipping_plane(self, near_clipping: float) -> None: - """ Set the spherical depth sensor's near clipping plane. + """Set the spherical depth sensor's near clipping plane. :param near_clipping: New near clipping plane (in metres) """ @@ -262,14 +319,14 @@ def set_near_clipping_plane(self, near_clipping: float) -> None: sensor.set_near_clipping_plane(near_clipping) def get_far_clipping_plane(self) -> float: - """ Get the Sensor's far clipping plane. + """Get the Sensor's far clipping plane. :return: Near clipping plane (metres) """ return self._front.get_far_clipping_plane() def set_far_clipping_plane(self, far_clipping: float) -> None: - """ Set the spherical depth sensor's far clipping plane. + """Set the spherical depth sensor's far clipping plane. :param far_clipping: New far clipping plane (in metres) """ @@ -279,8 +336,10 @@ def set_far_clipping_plane(self, far_clipping: float) -> None: sensor.set_far_clipping_plane(far_clipping) def set_entity_to_render(self, entity_to_render: int) -> None: - """ Set the entity to render to the spherical vision sensor, - this can be an object or more usefully a collection. -1 to render all objects in scene. + """Set the entity to render to the spherical vision sensor. + + This can be an object or more usefully a collection. + -1 to render all objects in scene. :param entity_to_render: Handle of the entity to render """ @@ -288,8 +347,10 @@ def set_entity_to_render(self, entity_to_render: int) -> None: self._sensor_rgb.set_entity_to_render(entity_to_render) def get_entity_to_render(self) -> None: - """ Get the entity to render to the spherical vision sensor, - this can be an object or more usefully a collection. -1 if all objects in scene are rendered. + """Get the entity to render to the spherical vision sensor. + + This can be an object or more usefully a collection. + -1 if all objects in scene are rendered. :return: Handle of the entity to render """ diff --git a/pyrep/textures/texture.py b/pyrep/textures/texture.py index 996d0d0..147449b 100644 --- a/pyrep/textures/texture.py +++ b/pyrep/textures/texture.py @@ -15,4 +15,3 @@ def get_texture_id(self) -> int: :return: The internal texture id. """ return self._texture_id - diff --git a/robot_ttms/README.md b/robot_ttms/README.md index 108dcfe..c5b7e74 100644 --- a/robot_ttms/README.md +++ b/robot_ttms/README.md @@ -2,4 +2,4 @@ Here are a number of modified model files (.ttm) ready to be used. If using the ```pyrep.robots``` module, then use these models. -Models have been adapted from V-REP. \ No newline at end of file +Models have been adapted from V-REP. diff --git a/setup.py b/setup.py index 246eebe..9024479 100644 --- a/setup.py +++ b/setup.py @@ -5,13 +5,13 @@ def read(rel_path): here = os.path.abspath(os.path.dirname(__file__)) - with codecs.open(os.path.join(here, rel_path), 'r') as fp: + with codecs.open(os.path.join(here, rel_path), "r") as fp: return fp.read() def get_version(rel_path): for line in read(rel_path).splitlines(): - if line.startswith('__version__'): + if line.startswith("__version__"): delim = '"' if '"' in line else "'" return line.split(delim)[1] else: @@ -19,42 +19,40 @@ def get_version(rel_path): # Temp fix for CoppeliaSim 4.1 -if 'COPPELIASIM_ROOT' not in os.environ: - raise RuntimeError('COPPELIASIM_ROOT not defined.') +if "COPPELIASIM_ROOT" not in os.environ: + raise RuntimeError("COPPELIASIM_ROOT not defined.") -usrset_file = os.path.join(os.environ['COPPELIASIM_ROOT'], 'system', 'usrset.txt') -usrset = '' +usrset_file = os.path.join(os.environ["COPPELIASIM_ROOT"], "system", "usrset.txt") +usrset = "" if os.path.isfile(usrset_file): - with open(usrset_file, 'r') as f: + with open(usrset_file, "r") as f: usrset = f.read() -if 'allowOldEduRelease' not in usrset: - with open(usrset_file, 'a+') as f: - f.write('\nallowOldEduRelease=7501\n') +if "allowOldEduRelease" not in usrset: + with open(usrset_file, "a+") as f: + f.write("\nallowOldEduRelease=7501\n") core_requirements = [ "numpy", "cbor", - "zmq" # Not used, but installed to stop coppeliasim complaining about module + "zmq", # Not used, but installed to stop coppeliasim complaining about module ] setuptools.setup( - name='PyRep', + name="PyRep", # Version A.B.C. # A.B info corresponds to the CoppeliaSim version needed. # C info corresponds to the PyRep patch version. version=get_version("pyrep/__init__.py"), - description='Python CoppeliaSim wrapper', - author='Stephen James', - author_email='stepjamuk@gmail.com', - url='https://stepjam.github.io/', + description="Python CoppeliaSim wrapper", + author="Stephen James", + author_email="stepjamuk@gmail.com", + url="https://stepjam.github.io/", packages=setuptools.find_packages(), python_requires=">=3.10", install_requires=core_requirements, extras_require={ - "dev": [ - "pre-commit" - ], + "dev": ["pre-commit"], }, ) diff --git a/system/usrset.txt b/system/usrset.txt index 2a74cac..f403354 100644 --- a/system/usrset.txt +++ b/system/usrset.txt @@ -186,7 +186,7 @@ useExternalLuaLibrary = false // if true, will call all Lua functions via the si additionalLuaPath = // e.g. d:/myLuaRoutines desktopRecordingIndex = 0 desktopRecordingWidth = -1 // -1=default. -externalScriptEditor = +externalScriptEditor = xmlExportSplitSize = 0 // 0=generate a single file. xmlExportKnownFormats = true // true=if several files are generated, mesh and image files are saved under known formats. diff --git a/tests/assets/cracker_box/textured_simple.obj b/tests/assets/cracker_box/textured_simple.obj index ce754c9..cf4e7f5 100644 --- a/tests/assets/cracker_box/textured_simple.obj +++ b/tests/assets/cracker_box/textured_simple.obj @@ -39768,4 +39768,4 @@ f 7866/7855/7866 7857/8279/7857 7864/8291/7864 f 6749/7163/6749 7435/7896/7435 7444/7164/7444 # 15728 faces, 8291 coords texture -# End of File \ No newline at end of file +# End of File diff --git a/tests/assets/cracker_box/textured_simple.obj.mtl b/tests/assets/cracker_box/textured_simple.obj.mtl index 8dfc1f6..af195d2 100644 --- a/tests/assets/cracker_box/textured_simple.obj.mtl +++ b/tests/assets/cracker_box/textured_simple.obj.mtl @@ -11,4 +11,3 @@ Tr 1.000000 illum 2 Ns 0.000000 map_Kd texture_map.png - diff --git a/tests/assets/test_mesh_bowl.obj b/tests/assets/test_mesh_bowl.obj index 9b87287..a43a315 100644 --- a/tests/assets/test_mesh_bowl.obj +++ b/tests/assets/test_mesh_bowl.obj @@ -7182,4 +7182,3 @@ f 1007//2039 1004//2033 1006//2037 f 1006//2038 1004//2034 1007//2040 f 1008//2041 1005//2035 1007//2039 f 1007//2040 1005//2036 1008//2042 - diff --git a/tests/core.py b/tests/core.py index 1310008..2c058fe 100644 --- a/tests/core.py +++ b/tests/core.py @@ -4,15 +4,16 @@ from pyrep import PyRep from os import path -ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), 'assets') +ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), "assets") pyrep.testing = True class TestCore(unittest.TestCase): - def setUp(self): self.pyrep = PyRep() - self.pyrep.launch(path.join(ASSET_DIR, 'test_scene.ttt'), headless=True, responsive_ui=False) + self.pyrep.launch( + path.join(ASSET_DIR, "test_scene.ttt"), headless=True, responsive_ui=False + ) self.pyrep.step() self.pyrep.start() diff --git a/tests/test_accelerometer.py b/tests/test_accelerometer.py index cb1aaef..dc699f2 100644 --- a/tests/test_accelerometer.py +++ b/tests/test_accelerometer.py @@ -4,15 +4,14 @@ class TestAccelerometer(TestCore): - def setUp(self): super().setUp() - self.sensor = Accelerometer('accelerometer') + self.sensor = Accelerometer("accelerometer") def test_read(self): accelerations = self.sensor.read() self.assertEqual(len(accelerations), 3) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_arms_and_configuration_paths.py b/tests/test_arms_and_configuration_paths.py index e690fc6..9baf222 100644 --- a/tests/test_arms_and_configuration_paths.py +++ b/tests/test_arms_and_configuration_paths.py @@ -19,31 +19,29 @@ from pyrep.robots.arms.ur10 import UR10 from pyrep.robots.arms.xarm7 import XArm7 -ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), 'assets') +ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), "assets") ARMS = [ - ('Panda', Panda), - ('Sawyer', Sawyer), - ('Mico', Mico), - ('Jaco', Jaco), - ('Baxter_leftArm', BaxterLeft), - ('Baxter_rightArm', BaxterRight), - ('LBR_iiwa_7_R800', LBRIwaa7R800), - ('LBR_iiwa_14_R820', LBRIwaa14R820), - ('UR3', UR3), - ('UR5', UR5), - ('UR10', UR10), - ('XArm7', XArm7), + ("Panda", Panda), + ("Sawyer", Sawyer), + ("Mico", Mico), + ("Jaco", Jaco), + ("Baxter_leftArm", BaxterLeft), + ("Baxter_rightArm", BaxterRight), + ("LBR_iiwa_7_R800", LBRIwaa7R800), + ("LBR_iiwa_14_R820", LBRIwaa14R820), + ("UR3", UR3), + ("UR5", UR5), + ("UR10", UR10), + ("XArm7", XArm7), ] class TestArmsAndConfigurationPaths(TestCore): - def setUp(self): self.pyrep = PyRep() - self.pyrep.launch(path.join( - ASSET_DIR, 'test_scene_robots.ttt'), headless=True) + self.pyrep.launch(path.join(ASSET_DIR, "test_scene_robots.ttt"), headless=True) self.pyrep.step() self.pyrep.start() @@ -63,42 +61,46 @@ def test_set_ik_element_properties(self): def test_set_ik_group_properties(self): arm = Panda() - arm.set_ik_group_properties('damped_least_squares') - arm.set_ik_group_properties('pseudo_inverse') + arm.set_ik_group_properties("damped_least_squares") + arm.set_ik_group_properties("pseudo_inverse") def test_solve_ik_via_jacobian(self): arm = Panda() - waypoint = Dummy('Panda_waypoint_jacobian') + waypoint = Dummy("Panda_waypoint_jacobian") new_pose = arm.get_tip().get_pose() new_pose[2] += 0.01 waypoint.set_pose(new_pose) new_config = arm.solve_ik_via_jacobian( - waypoint.get_position(), waypoint.get_orientation()) + waypoint.get_position(), waypoint.get_orientation() + ) arm.set_joint_positions(new_config) - self.assertTrue(np.allclose( - arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001)) + self.assertTrue( + np.allclose(arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001) + ) def test_solve_ik_via_sampling(self): arm = Panda() - waypoint = Dummy('Panda_waypoint') + waypoint = Dummy("Panda_waypoint") configs = arm.solve_ik_via_sampling( - waypoint.get_position(), waypoint.get_orientation(), max_configs=5) + waypoint.get_position(), waypoint.get_orientation(), max_configs=5 + ) self.assertIsNotNone(configs) self.assertEqual(len(configs), 5) current_config = arm.get_joint_positions() # Checks correct config (last) arm.set_joint_positions(configs[-1]) - self.assertTrue(np.allclose( - arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001)) + self.assertTrue( + np.allclose(arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001) + ) # Checks correct config (first) arm.set_joint_positions(configs[0]) - self.assertTrue(np.allclose( - arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001)) + self.assertTrue( + np.allclose(arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001) + ) # Checks order prev_config_dist = 0 for config in configs: - config_dist = sum( - [(c - f)**2 for c, f in zip(current_config, config)]) + config_dist = sum([(c - f) ** 2 for c, f in zip(current_config, config)]) # This test requires that the metric scale for each joint remains at # 1.0 in _getConfigDistance lua function self.assertLessEqual(prev_config_dist, config_dist) @@ -106,67 +108,69 @@ def test_solve_ik_via_sampling(self): def test_get_path_from_cartesian_path(self): arm = Panda() - cartesian_path = CartesianPath('Panda_cartesian_path') + cartesian_path = CartesianPath("Panda_cartesian_path") path = arm.get_path_from_cartesian_path(cartesian_path) self.assertIsNotNone(path) def test_get_linear_path(self): arm = Panda() - waypoint = Dummy('Panda_waypoint') - path = arm.get_linear_path( - waypoint.get_position(), waypoint.get_orientation()) + waypoint = Dummy("Panda_waypoint") + path = arm.get_linear_path(waypoint.get_position(), waypoint.get_orientation()) self.assertIsNotNone(path) def test_get_linear_path_relative(self): arm = Panda() - path = arm.get_linear_path([0, 0, 0.01], [0, 0, 0], - relative_to=arm.get_tip()) + path = arm.get_linear_path([0, 0, 0.01], [0, 0, 0], relative_to=arm.get_tip()) self.assertIsNotNone(path) def test_get_nonlinear_path(self): arm = Panda() - waypoint = Dummy('Panda_waypoint') + waypoint = Dummy("Panda_waypoint") path = arm.get_nonlinear_path( - waypoint.get_position(), waypoint.get_orientation(), max_time_ms=1000) + waypoint.get_position(), waypoint.get_orientation(), max_time_ms=1000 + ) self.assertIsNotNone(path) def test_get_nonlinear_path_out_of_reach(self): arm = Panda() with self.assertRaises(ConfigurationPathError): - arm.get_nonlinear_path([99, 99, 99], [0.] * 3) + arm.get_nonlinear_path([99, 99, 99], [0.0] * 3) def test_get_linear_path_out_of_reach(self): arm = Panda() with self.assertRaises(ConfigurationPathError): - arm.get_linear_path([99, 99, 99], [0.] * 3) + arm.get_linear_path([99, 99, 99], [0.0] * 3) def test_get_linear_path_and_step(self): arm = Panda() - waypoint = Dummy('Panda_waypoint') - path = arm.get_linear_path( - waypoint.get_position(), waypoint.get_orientation()) + waypoint = Dummy("Panda_waypoint") + path = arm.get_linear_path(waypoint.get_position(), waypoint.get_orientation()) self.assertIsNotNone(path) done = False while not done: done = path.step() self.pyrep.step() - self.assertTrue(np.allclose( - arm.get_tip().get_position(), waypoint.get_position(), atol=0.01)) + self.assertTrue( + np.allclose( + arm.get_tip().get_position(), waypoint.get_position(), atol=0.01 + ) + ) def test_get_linear_path_and_get_end(self): arm = Panda() - waypoint = Dummy('Panda_waypoint') - path = arm.get_linear_path( - waypoint.get_position(), waypoint.get_orientation()) + waypoint = Dummy("Panda_waypoint") + path = arm.get_linear_path(waypoint.get_position(), waypoint.get_orientation()) path.set_to_end() - self.assertTrue(np.allclose( - arm.get_tip().get_position(), waypoint.get_position(), atol=0.001)) + self.assertTrue( + np.allclose( + arm.get_tip().get_position(), waypoint.get_position(), atol=0.001 + ) + ) def test_get_linear_path_visualize(self): arm = Panda() - waypoint = Dummy('Panda_waypoint') - path = arm.get_linear_path( - waypoint.get_position(), waypoint.get_orientation()) + waypoint = Dummy("Panda_waypoint") + path = arm.get_linear_path(waypoint.get_position(), waypoint.get_orientation()) # Check that it does not error path.visualize() @@ -185,5 +189,5 @@ def test_get_jacobian(self): self.assertEqual(jacobian.shape, (7, 6)) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_cameras.py b/tests/test_cameras.py index 67ab49b..e68f355 100644 --- a/tests/test_cameras.py +++ b/tests/test_cameras.py @@ -8,11 +8,10 @@ class TestCameras(TestCore): - def setUp(self): super().setUp() - self.camera = Camera('DefaultCamera') - self.dummy = Dummy('dummy') + self.camera = Camera("DefaultCamera") + self.dummy = Dummy("dummy") def test_create(self): with self.assertRaises(NotImplementedError): @@ -24,15 +23,15 @@ def test_get_set_position(self): self.assertEqual(position.shape, (3,)) self.camera.set_position([0.1, 0.1, 0.1], self.dummy) - self.assertTrue(np.allclose( - self.camera.get_position(self.dummy), [0.1, 0.1, 0.1])) + self.assertTrue( + np.allclose(self.camera.get_position(self.dummy), [0.1, 0.1, 0.1]) + ) self.camera.set_position([0.2, 0.2, 0.2]) self.assertTrue(np.allclose(self.camera.get_position(), [0.2, 0.2, 0.2])) def test_get_object_type(self): - self.assertEqual(Object.get_object_type('DefaultCamera'), - ObjectType.CAMERA) + self.assertEqual(Object.get_object_type("DefaultCamera"), ObjectType.CAMERA) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_cartesian_paths.py b/tests/test_cartesian_paths.py index 18eb72e..5d0c2da 100644 --- a/tests/test_cartesian_paths.py +++ b/tests/test_cartesian_paths.py @@ -7,18 +7,19 @@ class TestCartesianPaths(TestCore): - def test_create_cartesian_path(self): p = CartesianPath.create() self.assertIsInstance(p, CartesianPath) def test_get_pose_on_path(self): - p = CartesianPath.create(paths_points=[0, 0, 0, 0, 0, 0, 1, 0.1, 0.1, 0.2, 0, 0, 0, 1]) + p = CartesianPath.create( + paths_points=[0, 0, 0, 0, 0, 0, 1, 0.1, 0.1, 0.2, 0, 0, 0, 1] + ) pos, ori = p.get_pose_on_path(0.5) self.assertEqual(len(pos), 3) self.assertEqual(len(ori), 3) self.assertTrue(np.allclose(pos, [0.05, 0.05, 0.1])) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_dummies.py b/tests/test_dummies.py index a4233a7..a49adac 100644 --- a/tests/test_dummies.py +++ b/tests/test_dummies.py @@ -4,14 +4,13 @@ class TestDummies(TestCore): - def test_get_dummy(self): - Dummy('dummy') + Dummy("dummy") def test_create_dummy(self): d = Dummy.create(0.01) self.assertIsInstance(d, Dummy) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_force_sensors.py b/tests/test_force_sensors.py index c16956f..e84aee1 100644 --- a/tests/test_force_sensors.py +++ b/tests/test_force_sensors.py @@ -4,10 +4,9 @@ class TestForceSensors(TestCore): - def setUp(self): super().setUp() - self.sensor = ForceSensor('force_sensor') + self.sensor = ForceSensor("force_sensor") def test_read(self): force_vector, torque_vector = self.sensor.read() @@ -19,5 +18,5 @@ def test_create(self): sensor.remove() -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_grippers.py b/tests/test_grippers.py index 6a149d1..e8b66cf 100644 --- a/tests/test_grippers.py +++ b/tests/test_grippers.py @@ -11,24 +11,22 @@ from pyrep.robots.end_effectors.baxter_gripper import BaxterGripper from pyrep.robots.end_effectors.xarm_gripper import XArmGripper -ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), 'assets') +ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), "assets") GRIPPERS = [ - ('PandaGripper', PandaGripper, 0.04), - ('BaxterGripper', BaxterGripper, 0.04), - ('MicoGripper', MicoGripper, 0.2), - ('JacoGripper', JacoGripper, 0.2), - ('Robotiq85Gripper', Robotiq85Gripper, 0.04), - ('XArmGripper', XArmGripper, 0.04), + ("PandaGripper", PandaGripper, 0.04), + ("BaxterGripper", BaxterGripper, 0.04), + ("MicoGripper", MicoGripper, 0.2), + ("JacoGripper", JacoGripper, 0.2), + ("Robotiq85Gripper", Robotiq85Gripper, 0.04), + ("XArmGripper", XArmGripper, 0.04), ] class TestArmsAndConfigurationPaths(TestCore): - def setUp(self): self.pyrep = PyRep() - self.pyrep.launch(path.join( - ASSET_DIR, 'test_scene_robots.ttt'), headless=True) + self.pyrep.launch(path.join(ASSET_DIR, "test_scene_robots.ttt"), headless=True) self.pyrep.step() self.pyrep.start() @@ -50,18 +48,19 @@ def test_close_open_gripper(self): self.pyrep.step() i += 1 if i > 1000: - self.fail('Took too many steps to close') + self.fail("Took too many steps to close") done = False i = 0 - open_amount = 1.0 if gripper_name == 'Robotiq85Gripper' else 0.8 + open_amount = 1.0 if gripper_name == "Robotiq85Gripper" else 0.8 while not done: done = gripper.actuate(open_amount, velocity=vel) self.pyrep.step() i += 1 if i > 1000: - self.fail('Took too many steps to open') + self.fail("Took too many steps to open") self.assertAlmostEqual( - gripper.get_open_amount()[0], open_amount, delta=0.05) + gripper.get_open_amount()[0], open_amount, delta=0.05 + ) def test_get_duplicate_gripper(self): g = BaxterGripper(1) @@ -73,5 +72,5 @@ def test_copy_gripper(self): self.assertNotEqual(g, new_g) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_gyroscope.py b/tests/test_gyroscope.py index 77d8f14..7c99e93 100644 --- a/tests/test_gyroscope.py +++ b/tests/test_gyroscope.py @@ -4,15 +4,14 @@ class TestGyroscope(TestCore): - def setUp(self): super().setUp() - self.sensor = Gyroscope('gyroscope') + self.sensor = Gyroscope("gyroscope") def test_read(self): angular_velocities = self.sensor.read() self.assertEqual(len(angular_velocities), 3) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_joint_groups.py b/tests/test_joint_groups.py index e90de16..9b2f667 100644 --- a/tests/test_joint_groups.py +++ b/tests/test_joint_groups.py @@ -2,21 +2,20 @@ from tests.core import TestCore from pyrep.const import JointType from pyrep.robots.arms.panda import Panda -import numpy as np # Pick one arm to test all of the joint group functionality. # Simply checks for wiring mistakes between the joints. class TestJointGroups(TestCore): - def setUp(self): super().setUp() self.robot = Panda() self.num_joints = len(self.robot.joints) def test_get_joint_types(self): - self.assertEqual(self.robot.get_joint_types(), - [JointType.REVOLUTE] * self.num_joints) + self.assertEqual( + self.robot.get_joint_types(), [JointType.REVOLUTE] * self.num_joints + ) def test_get_set_joint_positions(self): self.robot.set_joint_positions([0.1] * self.num_joints) @@ -24,13 +23,11 @@ def test_get_set_joint_positions(self): def test_get_set_joint_target_positions(self): self.robot.set_joint_target_positions([0.1] * self.num_joints) - self.assertEqual( - len(self.robot.get_joint_target_positions()), self.num_joints) + self.assertEqual(len(self.robot.get_joint_target_positions()), self.num_joints) def test_get_set_joint_target_velocities(self): self.robot.set_joint_target_velocities([0.1] * self.num_joints) - self.assertEqual( - len(self.robot.get_joint_target_velocities()), self.num_joints) + self.assertEqual(len(self.robot.get_joint_target_velocities()), self.num_joints) def test_get_set_joint_forces(self): self.robot.set_joint_target_velocities([-999] * self.num_joints) @@ -39,23 +36,24 @@ def test_get_set_joint_forces(self): self.assertEqual(len(self.robot.get_joint_forces()), self.num_joints) def test_get_velocities(self): - self.assertEqual( - len(self.robot.get_joint_velocities()), self.num_joints) + self.assertEqual(len(self.robot.get_joint_velocities()), self.num_joints) def test_get_set_joint_intervals(self): self.robot.set_joint_intervals( - [False] * self.num_joints, [[-2.0, 2.0]] * self.num_joints) + [False] * self.num_joints, [[-2.0, 2.0]] * self.num_joints + ) cyclics, intervals = self.robot.get_joint_intervals() self.assertEqual(len(cyclics), self.num_joints) self.assertEqual(len(intervals), self.num_joints) def test_get_joint_upper_velocity_limits(self): self.assertEqual( - len(self.robot.get_joint_upper_velocity_limits()), self.num_joints) + len(self.robot.get_joint_upper_velocity_limits()), self.num_joints + ) def test_get_visuals(self): self.assertEqual(len(self.robot.get_visuals()), 10) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_joints.py b/tests/test_joints.py index b4026e1..4d8e090 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -5,12 +5,11 @@ class TestJoints(TestCore): - def setUp(self): super().setUp() - self.prismatic = Joint('prismatic_joint') - self.prismatic_ctr = Joint('prismatic_joint_control_loop') - self.revolute = Joint('revolute_joint') + self.prismatic = Joint("prismatic_joint") + self.prismatic_ctr = Joint("prismatic_joint_control_loop") + self.revolute = Joint("revolute_joint") def test_get_joint_type(self): self.assertEqual(self.prismatic.get_joint_type(), JointType.PRISMATIC) @@ -32,8 +31,7 @@ def test_get_set_joint_target_position(self): self.assertEqual(pos, 0.5) # Now step a few times to drive the joint [self.pyrep.step() for _ in range(10)] - self.assertAlmostEqual( - self.prismatic_ctr.get_joint_position(), 0.5, delta=0.01) + self.assertAlmostEqual(self.prismatic_ctr.get_joint_position(), 0.5, delta=0.01) def test_get_set_joint_target_velocity(self): self.prismatic.set_joint_target_velocity(5.0) @@ -41,8 +39,7 @@ def test_get_set_joint_target_velocity(self): self.assertEqual(vel, 5.0) # Now step a few times to drive the joint [self.pyrep.step() for _ in range(10)] - self.assertAlmostEqual( - self.prismatic.get_joint_position(), 0.5, delta=0.01) + self.assertAlmostEqual(self.prismatic.get_joint_position(), 0.5, delta=0.01) def test_get_set_joint_force(self): [self.pyrep.step() for _ in range(10)] @@ -84,5 +81,5 @@ def test_get_set_motor_locked_at_zero_velocity(self): self.assertTrue(self.prismatic.is_motor_locked_at_zero_velocity()) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_lights.py b/tests/test_lights.py index 523c5e9..649a1da 100644 --- a/tests/test_lights.py +++ b/tests/test_lights.py @@ -5,13 +5,12 @@ class TestLights(TestCore): - def setUp(self): super().setUp() [self.pyrep.step() for _ in range(10)] - self.spot_light = Light('spot_light') - self.omni_light = Light('omni_light') - self.directed_light = Light('directed_light') + self.spot_light = Light("spot_light") + self.omni_light = Light("omni_light") + self.directed_light = Light("directed_light") self.lights = [self.spot_light, self.omni_light, self.directed_light] self.light_types = [1, 2, 3] @@ -73,37 +72,6 @@ def test_get_set_spot_cutoff(self): light.set_intensity_properties(spot_cutoff=orig) self.assertAlmostEqual(light.get_intensity_properties()[2], orig) - # ToDo: re-add these tests once attenuation factor setting is supported in CoppeliaSim. - # setObjectFloatParams() does not change the properties of the light even with in-scene lua code. - ''' - def test_get_set_const_atten_factor(self): - for light in self.lights: - orig = light.get_intensity_properties()[3] - new = orig + 1.5 - light.set_intensity_properties(const_atten_factor=new) - assert light.get_intensity_properties()[3] == new - light.set_intensity_properties(const_atten_factor=orig) - assert light.get_intensity_properties()[3] == orig - - def test_get_set_linear_atten_factor(self): - for light in self.lights: - orig = light.get_intensity_properties()[4] - new = orig + 1.5 - light.set_intensity_properties(linear_atten_factor=new) - assert light.get_intensity_properties()[4] == new - light.set_intensity_properties(linear_atten_factor=orig) - assert light.get_intensity_properties()[4] == orig - - def test_get_set_quad_atten_factor(self): - for light in self.lights: - orig = light.get_intensity_properties()[5] - new = orig + 1.5 - light.set_intensity_properties(quad_atten_factor=new) - assert light.get_intensity_properties()[5] == new - light.set_intensity_properties(quad_atten_factor=orig) - assert light.get_intensity_properties()[5] == orig - ''' - -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_misc.py b/tests/test_misc.py index e4865e7..cc5157c 100644 --- a/tests/test_misc.py +++ b/tests/test_misc.py @@ -1,21 +1,15 @@ import unittest -from pyrep.errors import PyRepError from tests.core import TestCore from pyrep.misc.signals import IntegerSignal, FloatSignal, StringSignal class TestSignal(TestCore): - - SIGNALS = [ - (IntegerSignal, 99), - (FloatSignal, 55.3), - (StringSignal, 'hello') - ] + SIGNALS = [(IntegerSignal, 99), (FloatSignal, 55.3), (StringSignal, "hello")] def test_set_get_clear_signals(self): for signal_class, test_value in TestSignal.SIGNALS: with self.subTest(signal=str(signal_class)): - sig = signal_class('my_signal') + sig = signal_class("my_signal") sig.set(test_value) ret_value = sig.get() if isinstance(test_value, float): @@ -27,9 +21,9 @@ def test_set_get_clear_signals(self): def test_get_signal_fails_when_empty(self): for signal_class, test_value in TestSignal.SIGNALS: with self.subTest(signal=str(signal_class)): - sig = signal_class('my_signal') + sig = signal_class("my_signal") self.assertIsNone(sig.get()) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_objects.py b/tests/test_objects.py index d9f7e12..36b3d43 100644 --- a/tests/test_objects.py +++ b/tests/test_objects.py @@ -11,40 +11,34 @@ class TestObjects(TestCore): - def setUp(self): super().setUp() - self.dynamic_cube = Shape('dynamic_cube') - self.cubes_under_dummy = Dummy('cubes_under_dummy') - self.cube0 = Shape('cube0') - self.dummy = Dummy('dummy') - self.simple_model = Shape('simple_model') + self.dynamic_cube = Shape("dynamic_cube") + self.cubes_under_dummy = Dummy("cubes_under_dummy") + self.cube0 = Shape("cube0") + self.dummy = Dummy("dummy") + self.simple_model = Shape("simple_model") def test_get_object_type(self): - self.assertEqual(Object.get_object_type('dynamic_cube'), - ObjectType.SHAPE) - self.assertEqual(Object.get_object_type('dummy'), - ObjectType.DUMMY) + self.assertEqual(Object.get_object_type("dynamic_cube"), ObjectType.SHAPE) + self.assertEqual(Object.get_object_type("dummy"), ObjectType.DUMMY) def test_get_object_name(self): - self.assertEqual(Object.get_object_name('dynamic_cube'), - 'dynamic_cube') + self.assertEqual(Object.get_object_name("dynamic_cube"), "dynamic_cube") self.assertEqual( - Object.get_object_name(self.dynamic_cube.get_handle()), - 'dynamic_cube') + Object.get_object_name(self.dynamic_cube.get_handle()), "dynamic_cube" + ) def test_get_object(self): - self.assertEqual(Object.get_object('dynamic_cube'), - self.dynamic_cube) - self.assertEqual(Object.get_object('dummy'), - self.dummy) - self.assertEqual(Object.get_object(self.dynamic_cube.get_handle()), - self.dynamic_cube) - self.assertEqual(Object.get_object(self.dummy.get_handle()), - self.dummy) + self.assertEqual(Object.get_object("dynamic_cube"), self.dynamic_cube) + self.assertEqual(Object.get_object("dummy"), self.dummy) + self.assertEqual( + Object.get_object(self.dynamic_cube.get_handle()), self.dynamic_cube + ) + self.assertEqual(Object.get_object(self.dummy.get_handle()), self.dummy) def test_equality(self): - cube2 = Shape('dynamic_cube') + cube2 = Shape("dynamic_cube") self.assertEqual(self.dynamic_cube, cube2) def test_get_handle(self): @@ -59,14 +53,14 @@ def test_still_exists(self): self.assertFalse(self.dynamic_cube.still_exists()) def test_object_exists(self): - yes = Object.exists('dynamic_cube') - no = Object.exists('dynamic_cubeee') + yes = Object.exists("dynamic_cube") + no = Object.exists("dynamic_cubeee") self.assertTrue(yes) self.assertFalse(no) def test_get_set_name(self): - self.dynamic_cube.set_name('test1') - self.assertEqual(self.dynamic_cube.get_name(), 'test1') + self.dynamic_cube.set_name("test1") + self.assertEqual(self.dynamic_cube.get_name(), "test1") def test_get_set_position(self): position = self.cube0.get_position() @@ -74,8 +68,11 @@ def test_get_set_position(self): self.assertEqual(position.shape, (3,)) self.cube0.set_position([0.1, 0.1, 0.1], self.cubes_under_dummy) - self.assertTrue(np.allclose( - self.cube0.get_position(self.cubes_under_dummy), [0.1, 0.1, 0.1])) + self.assertTrue( + np.allclose( + self.cube0.get_position(self.cubes_under_dummy), [0.1, 0.1, 0.1] + ) + ) self.cube0.set_position([0.2, 0.2, 0.2]) self.assertTrue(np.allclose(self.cube0.get_position(), [0.2, 0.2, 0.2])) @@ -85,12 +82,13 @@ def test_get_set_orientation(self): self.assertEqual(orientation.shape, (3,)) self.cube0.set_orientation([0.0, 0.0, 0.2], self.cubes_under_dummy) - self.assertTrue(np.allclose( - self.cube0.get_orientation(self.cubes_under_dummy), - [0.0, 0.0, 0.2])) + self.assertTrue( + np.allclose( + self.cube0.get_orientation(self.cubes_under_dummy), [0.0, 0.0, 0.2] + ) + ) self.cube0.set_orientation([0.0, 0.0, 0.3]) - self.assertTrue(np.allclose( - self.cube0.get_orientation(), [0.0, 0.0, 0.3])) + self.assertTrue(np.allclose(self.cube0.get_orientation(), [0.0, 0.0, 0.3])) def test_get_set_quaternion(self): quaternion = self.cube0.get_quaternion() @@ -98,14 +96,18 @@ def test_get_set_quaternion(self): self.assertEqual(quaternion.shape, (4,)) # x, y, z, w - self.cube0.set_quaternion([1., 0., 0., 0.], self.cubes_under_dummy) - self.assertTrue(np.allclose( - self.cube0.get_quaternion(self.cubes_under_dummy), - [1., 0., 0., 0.])) - self.cube0.set_quaternion([np.sqrt(0.5), 0, 0., np.sqrt(0.5)]) - self.assertTrue(np.allclose( - self.cube0.get_quaternion(), - [np.sqrt(0.5), 0, 0., np.sqrt(0.5)])) + self.cube0.set_quaternion([1.0, 0.0, 0.0, 0.0], self.cubes_under_dummy) + self.assertTrue( + np.allclose( + self.cube0.get_quaternion(self.cubes_under_dummy), [1.0, 0.0, 0.0, 0.0] + ) + ) + self.cube0.set_quaternion([np.sqrt(0.5), 0, 0.0, np.sqrt(0.5)]) + self.assertTrue( + np.allclose( + self.cube0.get_quaternion(), [np.sqrt(0.5), 0, 0.0, np.sqrt(0.5)] + ) + ) def test_get_velocity(self): linear_vel, angular_vel = self.cube0.get_velocity() @@ -124,8 +126,7 @@ def test_get_set_parent_not_in_place(self): self.dynamic_cube.set_parent(self.dummy, keep_in_place=False) parent = self.dynamic_cube.get_parent() self.assertEqual(parent, self.dummy) - self.assertFalse(np.allclose( - init_pos, self.dynamic_cube.get_position())) + self.assertFalse(np.allclose(init_pos, self.dynamic_cube.get_position())) def test_get_parent_when_orphan(self): parent = self.dummy.get_parent() @@ -148,8 +149,8 @@ def test_get_contact(self): self.assertIsNone(contact_data) for _ in range(20): self.pyrep.step() - c1 = Shape('colliding_cube1') - c0 = Shape('colliding_cube0') + c1 = Shape("colliding_cube1") + c0 = Shape("colliding_cube0") contact = c1.get_contacts() self.assertTrue(len(contact) > 0) contact = c0.get_contacts() @@ -188,8 +189,8 @@ def test_remove(self): self.simple_model.remove() self.assertFalse(self.dynamic_cube.still_exists()) self.assertFalse(self.simple_model.still_exists()) - self.assertFalse(Object.exists('dynamic_cube')) - self.assertFalse(Object.exists('simple_model')) + self.assertFalse(Object.exists("dynamic_cube")) + self.assertFalse(Object.exists("simple_model")) def test_dynamic_object(self): # Can't really test this. So lets just make sure it doesn't error @@ -200,23 +201,30 @@ def test_get_bounding_box(self): self.assertTrue(np.allclose(bb, [-0.05, 0.05] * 3)) def test_get_objects_in_tree(self): - dummys = [Dummy('nested_dummy%d' % i) for i in range(3)] + dummys = [Dummy("nested_dummy%d" % i) for i in range(3)] objects = dummys[0].get_objects_in_tree( - exclude_base=False, first_generation_only=False) + exclude_base=False, first_generation_only=False + ) self.assertListEqual(objects, dummys) for obj in objects: self.assertIs(type(obj), Dummy) self.assertListEqual( dummys[0].get_objects_in_tree( - exclude_base=True, first_generation_only=False), dummys[1:]) + exclude_base=True, first_generation_only=False + ), + dummys[1:], + ) self.assertListEqual( dummys[0].get_objects_in_tree( - exclude_base=False,first_generation_only=True), dummys[:-1]) + exclude_base=False, first_generation_only=True + ), + dummys[:-1], + ) def test_get_extention_string(self): - self.assertEqual(self.dynamic_cube.get_extension_string(), 'test') + self.assertEqual(self.dynamic_cube.get_extension_string(), "test") def test_get_configuration_tree(self): config = self.dynamic_cube.get_configuration_tree() @@ -224,8 +232,9 @@ def test_get_configuration_tree(self): def test_rotate(self): self.dynamic_cube.rotate([0.02, 0.04, 0.06]) - self.assertTrue(np.allclose( - self.dynamic_cube.get_orientation(), [0.02, 0.04, 0.06])) + self.assertTrue( + np.allclose(self.dynamic_cube.get_orientation(), [0.02, 0.04, 0.06]) + ) def test_get_set_model_collidable(self): self.simple_model.set_model_collidable(False) @@ -264,12 +273,12 @@ def test_get_set_model_respondable(self): self.assertTrue(self.simple_model.is_model_respondable()) def test_check_collision(self): - c1 = Shape('colliding_cube0') - c2 = Shape('colliding_cube1') + c1 = Shape("colliding_cube0") + c2 = Shape("colliding_cube1") self.assertTrue(c1.check_collision(c2)) def test_check_collision_all(self): - c1 = Shape('colliding_cube0') + c1 = Shape("colliding_cube0") self.assertTrue(c1.check_collision(None)) def test_copy(self): @@ -303,5 +312,5 @@ def test_set_get_explicit_handling(self): cam.remove() -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_octrees.py b/tests/test_octrees.py index 9091d26..4b3887b 100644 --- a/tests/test_octrees.py +++ b/tests/test_octrees.py @@ -3,38 +3,38 @@ from pyrep.objects.shape import Shape from pyrep.objects.octree import Octree -class TestOctrees(TestCore): +class TestOctrees(TestCore): def setUp(self): super().setUp() self.octree = Octree.create(0.025) - self.shape = Shape('Panda_link0_visual') + self.shape = Shape("Panda_link0_visual") def test_octree_insert_and_remove_voxels(self): point = [0.0, 0.0, 0.0] self.octree.insert_voxels(point) voxels = self.octree.get_voxels() # As many as 8 voxels may be added from a single point insertion. - self.assertTrue(1 <= len(voxels)//3 and len(voxels)/3 <= 8) + self.assertTrue(1 <= len(voxels) // 3 and len(voxels) / 3 <= 8) self.assertTrue(self.octree.check_point_occupancy(point)) self.octree.remove_voxels(point) voxels = self.octree.get_voxels() - self.assertTrue(len(voxels)//3 is 0) + self.assertTrue(len(voxels) // 3 == 0) self.assertFalse(self.octree.check_point_occupancy(point)) def test_octree_insert_and_subtract_object(self): self.octree.insert_object(self.shape) voxels = self.octree.get_voxels() - self.assertTrue(1 <= len(voxels)//3) + self.assertTrue(1 <= len(voxels) // 3) self.octree.subtract_object(self.shape) voxels = self.octree.get_voxels() - self.assertTrue(len(voxels)//3 is 0) + 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() - self.assertTrue(1 <= len(voxels)//3) + self.assertTrue(1 <= len(voxels) // 3) self.octree.clear_voxels() voxels = self.octree.get_voxels() - self.assertTrue(len(voxels)//3 is 0) + self.assertTrue(len(voxels) // 3 == 0) diff --git a/tests/test_proximity_sensors.py b/tests/test_proximity_sensors.py index 01ef58c..1bb6ca4 100644 --- a/tests/test_proximity_sensors.py +++ b/tests/test_proximity_sensors.py @@ -5,10 +5,9 @@ class TestProximitySensors(TestCore): - def setUp(self): super().setUp() - self.sensor = ProximitySensor('proximity_sensor') + self.sensor = ProximitySensor("proximity_sensor") def test_read(self): self.pyrep.step() @@ -17,11 +16,11 @@ def test_read(self): self.assertAlmostEqual(distance, 0.1) def test_is_detected(self): - ob1 = Shape('simple_model') - ob2 = Shape('dynamic_cube') + ob1 = Shape("simple_model") + ob2 = Shape("dynamic_cube") self.assertTrue(self.sensor.is_detected(ob1)) self.assertFalse(self.sensor.is_detected(ob2)) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_pyrep.py b/tests/test_pyrep.py index d0eab35..2af536c 100644 --- a/tests/test_pyrep.py +++ b/tests/test_pyrep.py @@ -1,5 +1,4 @@ import unittest -import warnings import tempfile from tests.core import TestCore from tests.core import ASSET_DIR @@ -17,98 +16,101 @@ class TestPyrep(TestCore): - def test_get_object_wrong_type(self): with self.assertRaises(WrongObjectTypeError): - ProximitySensor('dynamic_cube') + ProximitySensor("dynamic_cube") def test_get_shape(self): - cube = Shape('dynamic_cube') + cube = Shape("dynamic_cube") self.assertIsInstance(cube, Shape) def test_get_joint(self): - cube = Joint('prismatic_joint') + cube = Joint("prismatic_joint") self.assertIsInstance(cube, Joint) def test_get_proximity_sensor(self): - cube = ProximitySensor('proximity_sensor') + cube = ProximitySensor("proximity_sensor") self.assertIsInstance(cube, ProximitySensor) def test_get_force_sensor(self): - cube = ForceSensor('force_sensor') + cube = ForceSensor("force_sensor") self.assertIsInstance(cube, ForceSensor) def test_get_cartesian_path(self): - cube = CartesianPath('cartesian_path') + cube = CartesianPath("cartesian_path") self.assertIsInstance(cube, CartesianPath) def test_step(self): - cube = Shape('dynamic_cube') + cube = Shape("dynamic_cube") start_pos = cube.get_position() [self.pyrep.step() for _ in range(2)] end_pos = cube.get_position() self.assertFalse(np.allclose(start_pos, end_pos)) def test_load_model(self): - m = self.pyrep.import_model(path.join(ASSET_DIR, 'loadable_model.ttm')) + m = self.pyrep.import_model(path.join(ASSET_DIR, "loadable_model.ttm")) self.assertIsInstance(m, Shape) def test_export_scene(self): - scene_file = tempfile.mktemp('.ttt') + scene_file = tempfile.mktemp(".ttt") self.pyrep.export_scene(scene_file) os.remove(scene_file) def test_group_objects(self): - top = Dummy('cubes_under_dummy') - self.assertEqual( - len(top.get_objects_in_tree(exclude_base=True)), 3) - cubes = [Shape('cube%d' % i) for i in range(3)] + top = Dummy("cubes_under_dummy") + self.assertEqual(len(top.get_objects_in_tree(exclude_base=True)), 3) + cubes = [Shape("cube%d" % i) for i in range(3)] ob = self.pyrep.group_objects(cubes) self.assertIsInstance(ob, Object) - self.assertEqual( - len(top.get_objects_in_tree(exclude_base=True)), 1) + self.assertEqual(len(top.get_objects_in_tree(exclude_base=True)), 1) def test_merge_objects(self): - top = Dummy('cubes_under_dummy') - self.assertEqual( - len(top.get_objects_in_tree(exclude_base=True)), 3) - cubes = [Shape('cube%d' % i) for i in range(3)] + top = Dummy("cubes_under_dummy") + self.assertEqual(len(top.get_objects_in_tree(exclude_base=True)), 3) + cubes = [Shape("cube%d" % i) for i in range(3)] ob = self.pyrep.merge_objects(cubes) self.assertIsInstance(ob, Object) - self.assertEqual( - len(top.get_objects_in_tree(exclude_base=True)), 1) + self.assertEqual(len(top.get_objects_in_tree(exclude_base=True)), 1) def test_create_texture_and_get_texture(self): plane, texture = self.pyrep.create_texture( - path.join(ASSET_DIR, 'wood_texture.jpg')) + path.join(ASSET_DIR, "wood_texture.jpg") + ) self.assertGreaterEqual(texture.get_texture_id(), 0) - self.assertEqual(texture.get_texture_id(), - plane.get_texture().get_texture_id()) + self.assertEqual(texture.get_texture_id(), plane.get_texture().get_texture_id()) def test_get_objects_in_tree(self): objects = self.pyrep.get_objects_in_tree() for obj in objects: self.assertIsInstance(obj, Object) - dummys = [Dummy('nested_dummy%d' % i) for i in range(3)] + dummys = [Dummy("nested_dummy%d" % i) for i in range(3)] for root_obj in [dummys[0], dummys[0].get_handle()]: objects = self.pyrep.get_objects_in_tree( - root_obj, exclude_base=False, first_generation_only=False) + root_obj, exclude_base=False, first_generation_only=False + ) self.assertListEqual(objects, dummys) for obj in objects: self.assertIs(type(obj), Dummy) self.assertListEqual( self.pyrep.get_objects_in_tree( - root_obj, exclude_base=True, first_generation_only=False), - dummys[1:]) + root_obj, exclude_base=True, first_generation_only=False + ), + dummys[1:], + ) self.assertListEqual( self.pyrep.get_objects_in_tree( - root_obj, exclude_base=False,first_generation_only=True), - dummys[:-1]) + root_obj, exclude_base=False, first_generation_only=True + ), + dummys[:-1], + ) def test_get_collection_by_name(self): - self.assertIsInstance(self.pyrep.get_collection_handle_by_name('Panda_arm'), int) + self.assertIsInstance( + self.pyrep.get_collection_handle_by_name("Panda_arm"), int + ) + -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_shapes.py b/tests/test_shapes.py index e0fa0b7..55db31b 100644 --- a/tests/test_shapes.py +++ b/tests/test_shapes.py @@ -8,47 +8,47 @@ class TestShapes(TestCore): - def setUp(self): super().setUp() - self.dynamic_cube = Shape('dynamic_cube') + self.dynamic_cube = Shape("dynamic_cube") def test_create_primitive_simple(self): - pr = Shape.create( - PrimitiveShape.CUBOID, size=[1., 1., 1.]) + pr = Shape.create(PrimitiveShape.CUBOID, size=[1.0, 1.0, 1.0]) self.assertIsInstance(pr, Shape) def test_create_primitive_complex(self): pr = Shape.create( - PrimitiveShape.CUBOID, size=[1., 1., 1.], mass=2.0, - smooth=True, respondable=True, static=False, - position=[1.1, 1.2, 1.3], orientation=[0.1, 0.2, 0.3], - color=[0.7, 0.8, 0.9]) + PrimitiveShape.CUBOID, + size=[1.0, 1.0, 1.0], + mass=2.0, + smooth=True, + respondable=True, + static=False, + position=[1.1, 1.2, 1.3], + orientation=[0.1, 0.2, 0.3], + color=[0.7, 0.8, 0.9], + ) self.assertIsInstance(pr, Shape) self.assertTrue(np.allclose(pr.get_position(), [1.1, 1.2, 1.3])) self.assertTrue(np.allclose(pr.get_orientation(), [0.1, 0.2, 0.3])) self.assertTrue(np.allclose(pr.get_color(), [0.7, 0.8, 0.9])) def test_import_shape(self): - ob = Shape.import_shape( - path.join(ASSET_DIR, 'cracker_box/textured_simple.obj')) + ob = Shape.import_shape(path.join(ASSET_DIR, "cracker_box/textured_simple.obj")) self.assertIsInstance(ob, Shape) def test_import_mesh(self): - ob = Shape.import_mesh( - path.join(ASSET_DIR, 'test_mesh_bowl.obj')) + ob = Shape.import_mesh(path.join(ASSET_DIR, "test_mesh_bowl.obj")) self.assertIsInstance(ob, Shape) def test_create_mesh(self): ob = Shape.create_mesh( - vertices=[-0.1, -0.1, 0.0, - -0.1, 0.1, 0.0, - 0.1, 0.0, 0.0], indices=[0, 1, 2]) + vertices=[-0.1, -0.1, 0.0, -0.1, 0.1, 0.0, 0.1, 0.0, 0.0], indices=[0, 1, 2] + ) self.assertIsInstance(ob, Shape) def test_convex_decompose(self): - ob = Shape.import_mesh( - path.join(ASSET_DIR, 'test_mesh_bowl.obj')) + ob = Shape.import_mesh(path.join(ASSET_DIR, "test_mesh_bowl.obj")) self.assertIsInstance(ob, Shape) cd_1 = ob.get_convex_decomposition() self.assertIsInstance(cd_1, Shape) @@ -58,8 +58,8 @@ def test_convex_decompose(self): self.assertEqual(ob, cd_2) def test_get_set_color(self): - self.dynamic_cube.set_color([.5] * 3) - self.assertEqual(self.dynamic_cube.get_color(), [.5] * 3) + self.dynamic_cube.set_color([0.5] * 3) + self.assertEqual(self.dynamic_cube.get_color(), [0.5] * 3) def test_get_set_transparency(self): self.dynamic_cube.set_transparency(0.6) @@ -90,14 +90,14 @@ def test_get_mesh_data(self): self.assertEqual(normals.shape, (n_faces * 3, 3)) def test_set_texture(self): - _, texture = self.pyrep.create_texture( - path.join(ASSET_DIR, 'wood_texture.jpg')) + _, texture = self.pyrep.create_texture(path.join(ASSET_DIR, "wood_texture.jpg")) self.dynamic_cube.set_texture(texture, TextureMappingMode.CUBE) - self.assertEqual(texture.get_texture_id(), - self.dynamic_cube.get_texture().get_texture_id()) + self.assertEqual( + texture.get_texture_id(), self.dynamic_cube.get_texture().get_texture_id() + ) def test_get_shape_viz(self): - visual = Shape('cracker_box_visual') + visual = Shape("cracker_box_visual") info = visual.get_shape_viz(index=0) self.assertIsInstance(info.vertices, np.ndarray) self.assertEqual(info.vertices.shape[1], 3) @@ -117,7 +117,7 @@ def test_get_shape_viz(self): self.assertIsInstance(info.texture_options, int) def test_decimate_mesh(self): - visual = Shape('cracker_box_visual') + visual = Shape("cracker_box_visual") _, old_indices, _ = visual.get_mesh_data() new_mesh = visual.decimate_mesh(0.2) _, new_indices, _ = new_mesh.get_mesh_data() @@ -144,5 +144,5 @@ def test_add_force_and_torque(self): self.assertTrue(np.all(before != after)) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_spherical_vision_sensors.py b/tests/test_spherical_vision_sensors.py index 87cd94c..922a66f 100644 --- a/tests/test_spherical_vision_sensors.py +++ b/tests/test_spherical_vision_sensors.py @@ -8,11 +8,12 @@ class TestSphericalVisionSensors(TestCore): - def setUp(self): super().setUp() [self.pyrep.step() for _ in range(10)] - self.spherical_vision_sensor = SphericalVisionSensor('sphericalVisionRGBAndDepth') + self.spherical_vision_sensor = SphericalVisionSensor( + "sphericalVisionRGBAndDepth" + ) def test_handle_explicitly(self): # non-blank image @@ -34,21 +35,31 @@ def test_capture_depth(self): def test_get_set_resolution(self): self.spherical_vision_sensor.set_resolution([480, 240]) - self.assertTrue(np.array_equal(self.spherical_vision_sensor.get_resolution(), [480, 240])) - self.assertEqual(self.spherical_vision_sensor.capture_rgb().shape, (240, 480, 3)) + self.assertTrue( + np.array_equal(self.spherical_vision_sensor.get_resolution(), [480, 240]) + ) + self.assertEqual( + self.spherical_vision_sensor.capture_rgb().shape, (240, 480, 3) + ) def test_get_set_render_mode(self): for render_mode in [RenderMode.OPENGL, RenderMode.OPENGL3]: self.spherical_vision_sensor.set_render_mode(render_mode) - self.assertEqual(self.spherical_vision_sensor.get_render_mode(), render_mode) + self.assertEqual( + self.spherical_vision_sensor.get_render_mode(), render_mode + ) def test_get_set_near_clipping_plane(self): self.spherical_vision_sensor.set_near_clipping_plane(0.1) - self.assertAlmostEqual(self.spherical_vision_sensor.get_near_clipping_plane(), 0.1) + self.assertAlmostEqual( + self.spherical_vision_sensor.get_near_clipping_plane(), 0.1 + ) def test_get_set_far_clipping_plane(self): self.spherical_vision_sensor.set_far_clipping_plane(0.1) - self.assertAlmostEqual(self.spherical_vision_sensor.get_far_clipping_plane(), 0.1) + self.assertAlmostEqual( + self.spherical_vision_sensor.get_far_clipping_plane(), 0.1 + ) def test_get_set_windowed_size(self): self.spherical_vision_sensor.set_windowed_size((640, 480)) @@ -59,5 +70,5 @@ def test_get_set_entity_to_render(self): self.assertIsNone(self.spherical_vision_sensor.get_entity_to_render()) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_suction_cups.py b/tests/test_suction_cups.py index ae65bc0..b36dd4f 100644 --- a/tests/test_suction_cups.py +++ b/tests/test_suction_cups.py @@ -7,20 +7,18 @@ 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') +ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), "assets") SUCTION_CUPS = [ - ('DobotSuctionCup', DobotSuctionCup), - ('BaxterSuctionCup', BaxterSuctionCup), + ("DobotSuctionCup", DobotSuctionCup), + ("BaxterSuctionCup", BaxterSuctionCup), ] class TestSuctionCups(TestCore): - def setUp(self): self.pyrep = PyRep() - self.pyrep.launch(path.join( - ASSET_DIR, 'test_scene_robots.ttt'), headless=True) + self.pyrep.launch(path.join(ASSET_DIR, "test_scene_robots.ttt"), headless=True) self.pyrep.step() self.pyrep.start() @@ -33,8 +31,8 @@ def test_get_suction_cup(self): def test_grasp_and_release_with_suction(self): for cup_name, cup_type in SUCTION_CUPS: with self.subTest(suction_cup=cup_name): - suction_cube = Shape('%s_cube' % cup_name) - cube = Shape('cube') + suction_cube = Shape("%s_cube" % cup_name) + cube = Shape("cube") cup = cup_type() self.pyrep.step() grasped = cup.grasp(cube) @@ -47,5 +45,5 @@ def test_grasp_and_release_with_suction(self): self.assertEqual(len(cup.get_grasped_objects()), 0) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/test_vision_sensors.py b/tests/test_vision_sensors.py index 69d10cc..c391c8a 100644 --- a/tests/test_vision_sensors.py +++ b/tests/test_vision_sensors.py @@ -5,11 +5,10 @@ class TestVisionSensors(TestCore): - def setUp(self): super().setUp() [self.pyrep.step() for _ in range(10)] - self.cam = VisionSensor('cam0') + self.cam = VisionSensor("cam0") def test_handle_explicitly(self): cam = VisionSensor.create((640, 480), explicit_handling=True) @@ -49,12 +48,14 @@ def test_capture_pointcloud(self): self.assertFalse(img.min() == img.max() == 1.0) def test_create(self): - cam = VisionSensor.create([640, 480], - perspective_mode=True, - view_angle=35.0, - near_clipping_plane=0.25, - far_clipping_plane=5.0, - render_mode=RenderMode.OPENGL3) + cam = VisionSensor.create( + [640, 480], + perspective_mode=True, + view_angle=35.0, + near_clipping_plane=0.25, + far_clipping_plane=5.0, + render_mode=RenderMode.OPENGL3, + ) self.assertEqual(cam.capture_rgb().shape, (480, 640, 3)) self.assertEqual(cam.get_perspective_mode(), PerspectiveMode.PERSPECTIVE) self.assertAlmostEqual(cam.get_perspective_angle(), 35.0, 3) @@ -70,10 +71,7 @@ def test_get_set_resolution(self): def test_get_set_perspective_mode(self): for perspective_mode in PerspectiveMode: self.cam.set_perspective_mode(perspective_mode) - self.assertEqual( - self.cam.get_perspective_mode(), - perspective_mode - ) + self.assertEqual(self.cam.get_perspective_mode(), perspective_mode) def test_get_set_render_mode(self): for render_mode in [RenderMode.OPENGL, RenderMode.OPENGL3]: @@ -109,5 +107,5 @@ def test_get_intrinsic_matrix(self): self.assertEqual(i.shape, (3, 3)) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tools/generate_simConst.py b/tools/generate_simConst.py index c790c3d..1abf910 100755 --- a/tools/generate_simConst.py +++ b/tools/generate_simConst.py @@ -1,8 +1,8 @@ #!/usr/bin/env python -'''Generate sim_const.py from simConst.h distributed from CoppeliaSim +"""Generate sim_const.py from simConst.h distributed from CoppeliaSim $ ./generate_simConst.py > ../pyrep/backend/sim_const.py -''' +""" import contextlib import importlib @@ -18,30 +18,25 @@ try: import CppHeaderParser except ImportError: - print('Please run following:\n\tpip install CppHeaderParser', - file=sys.stderr) + print("Please run following:\n\tpip install CppHeaderParser", file=sys.stderr) sys.exit(1) def get_coppeliasim_root(): - if 'COPPELIASIM_ROOT' not in os.environ: - raise RuntimeError('Please set env COPPELIASIM_ROOT') - return os.environ['COPPELIASIM_ROOT'] + if "COPPELIASIM_ROOT" not in os.environ: + raise RuntimeError("Please set env COPPELIASIM_ROOT") + return os.environ["COPPELIASIM_ROOT"] def import_as(filename, module): - spec = importlib.util.spec_from_file_location( - module, filename - ) + spec = importlib.util.spec_from_file_location(module, filename) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) return module def generate_simConst_py(): - header_file = osp.join( - get_coppeliasim_root(), 'programming/include/simConst.h' - ) + header_file = osp.join(get_coppeliasim_root(), "programming/include/simConst.h") with contextlib.redirect_stdout(sys.stderr): header = CppHeaderParser.CppHeader(header_file) @@ -52,44 +47,42 @@ def generate_simConst_py(): names = [] for define in header.defines: - m = re.match('^(.*)/\*.*\*/', define) + m = re.match("^(.*)/\*.*\*/", define) if m: define = m.groups()[0] splits = define.split() if len(splits) == 2: name, _ = splits - if name in ['SIM_PROGRAM_VERSION']: + if name in ["SIM_PROGRAM_VERSION"]: continue names.append(name) for enum in header.enums: - assert enum['namespace'] == '' - for value in enum['values']: - names.append(value['name']) - names.append('') + assert enum["namespace"] == "" + for value in enum["values"]: + names.append(value["name"]) + names.append("") out_dir = tempfile.mkdtemp() - cpp_file = osp.join(out_dir, 'generate_simConst_py.cpp') - with open(cpp_file, 'w') as f: - f.write('#include \n') - f.write('#include \n') - f.write('int main() {\n') + cpp_file = osp.join(out_dir, "generate_simConst_py.cpp") + with open(cpp_file, "w") as f: + f.write("#include \n") + f.write("#include \n") + f.write("int main() {\n") for name in names: if name: - f.write( - f'\tstd::cout << "{name} = " << {name} << std::endl;\n' - ) + f.write(f'\tstd::cout << "{name} = " << {name} << std::endl;\n') else: - f.write('\tstd::cout << std::endl;\n') - f.write('}\n') + f.write("\tstd::cout << std::endl;\n") + f.write("}\n") - out_file = osp.join(out_dir, 'generate_simConst_py') - cmd = f'g++ {cpp_file} -o {out_file} -I{osp.dirname(header_file)}' + out_file = osp.join(out_dir, "generate_simConst_py") + cmd = f"g++ {cpp_file} -o {out_file} -I{osp.dirname(header_file)}" subprocess.check_call(shlex.split(cmd)) - cmd = osp.join(out_dir, 'generate_simConst_py') + cmd = osp.join(out_dir, "generate_simConst_py") code = subprocess.check_output(cmd).strip().decode() shutil.rmtree(out_dir) @@ -100,37 +93,45 @@ def generate_simConst_py(): def merge_simConst_py(code): official_file = osp.join( get_coppeliasim_root(), - 'programming/remoteApiBindings/python/python/sim_const.py') - official = import_as(official_file, 'official') + "programming/remoteApiBindings/python/python/sim_const.py", + ) + official = import_as(official_file, "official") out_dir = tempfile.mkdtemp() - generated_file = osp.join(out_dir, 'simConst_generated.py') - with open(generated_file, 'w') as f: + generated_file = osp.join(out_dir, "simConst_generated.py") + with open(generated_file, "w") as f: f.write(code) - generated = import_as(generated_file, 'generated') + generated = import_as(generated_file, "generated") name_and_value = [] for name in dir(official): - if re.match('__.*__', name): + if re.match("__.*__", name): continue if name in dir(generated): value_official = getattr(official, name) value_generated = getattr(generated, name) if value_official != value_generated: - print(f"WARNING: The values of var '{name}' is not equal: " - f'official={value_official}, ' - f'generated={value_generated}', file=sys.stderr) + print( + f"WARNING: The values of var '{name}' is not equal: " + f"official={value_official}, " + f"generated={value_generated}", + file=sys.stderr, + ) else: name_and_value.append((name, getattr(official, name))) - code = ('# This file is automatically generated by ' - f'{osp.basename(__file__)} from simConst.h\n\n' + code) - code += ('\n\n# Followings are copied from official sim_const.py ' - '(so possibly deprecated in C++ API)') + code = ( + "# This file is automatically generated by " + f"{osp.basename(__file__)} from simConst.h\n\n" + code + ) + code += ( + "\n\n# Followings are copied from official sim_const.py " + "(so possibly deprecated in C++ API)" + ) for name, value in name_and_value: - code += f'\n{name} = {value}' - code += '\n' + code += f"\n{name} = {value}" + code += "\n" return code @@ -138,8 +139,8 @@ def merge_simConst_py(code): def main(): code = generate_simConst_py() code = merge_simConst_py(code) - print(code, end='') + print(code, end="") -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/tutorials/adding_robots.md b/tutorials/adding_robots.md index 7cd4f14..7901947 100644 --- a/tutorials/adding_robots.md +++ b/tutorials/adding_robots.md @@ -6,40 +6,40 @@ You may want to take a look at the CoppeliaSim [tutorials](http://www.coppeliaro #### 1. CoppeliaSim documentation -First read through the '[building a clean model tutorial](http://www.coppeliarobotics.com/helpFiles/en/buildingAModelTutorial.htm)', -which will show you how to first import CAD data (OBJ, STL, DXF), Collada, or +First read through the '[building a clean model tutorial](http://www.coppeliarobotics.com/helpFiles/en/buildingAModelTutorial.htm)', +which will show you how to first import CAD data (OBJ, STL, DXF), Collada, or URDF files, and then how to configure the dynamic shapes. #### 2. Object naming -After step 1, you should now have your model in the CoppeliaSim scene. -Ensure that all of your joints are in Torque/force mode and that both -the motor and control loop is enabled. Also ensure that the robot root is a 'model'. -Do this by double clicking the robot root in the scene hierarchy to open -up the 'Scene Object Properties' dialog, navigate to the 'Common' tab, and +After step 1, you should now have your model in the CoppeliaSim scene. +Ensure that all of your joints are in Torque/force mode and that both +the motor and control loop is enabled. Also ensure that the robot root is a 'model'. +Do this by double clicking the robot root in the scene hierarchy to open +up the 'Scene Object Properties' dialog, navigate to the 'Common' tab, and check 'Object is model base'. -Make sure that each of the scene objects are named appropriately. For example, -for the UR10 robot, we name the visual objects 'UR10_link1_visible', 'UR10_link2_visible', etc, +Make sure that each of the scene objects are named appropriately. For example, +for the UR10 robot, we name the visual objects 'UR10_link1_visible', 'UR10_link2_visible', etc, and the joints 'UR10_joint1', 'UR10_joint2', etc. ![image missing](images/scene_hierarchy.png) #### 3. Adding child script -On the root node of the robot, create a 'Non-threaded child script'. -**You do not need to modify the file**. This is needed to ensure that if the +On the root node of the robot, create a 'Non-threaded child script'. +**You do not need to modify the file**. This is needed to ensure that if the robot is cloned, then the object names in the clone behave as expected. #### 4. Collision collections -Now we will make a collision collection. In the toolbar on the left, -click to open the 'Collections' dialog. Click 'Add new collection' and a new item -will appear in the 'Collections list'. Rename this (by double clicking on the item) +Now we will make a collision collection. In the toolbar on the left, +click to open the 'Collections' dialog. Click 'Add new collection' and a new item +will appear in the 'Collections list'. Rename this (by double clicking on the item) to '\_arm' where '\' -is the name of the arm. For example, for the UR10, this would be 'UR10_arm'. In the scene hierarchy, -click the root of the robot, and then in the 'Collections' dialog (making sure -the correct item in the 'Collection list' is selected), click the +is the name of the arm. For example, for the UR10, this would be 'UR10_arm'. In the scene hierarchy, +click the root of the robot, and then in the 'Collections' dialog (making sure +the correct item in the 'Collection list' is selected), click the 'Tree of selected object' radio button, and then click 'Add'. ![image missing](images/collision_collections.png) @@ -49,39 +49,39 @@ the correct item in the 'Collection list' is selected), click the ##### Dummys -First create 2 Dummy objects, named '\_target' and '\_tip' (e.g. UR10_target and UR10_tip). +First create 2 Dummy objects, named '\_target' and '\_tip' (e.g. UR10_target and UR10_tip). In the scene hierarchy, the 'target' dummy should be a child of the robot root node, and the tip -dummy should be a child of the last node in the robot arm. -Position both the dummies at the tip of the arm, and orient the tip dummy such -that the z-axis points out from the arm. +dummy should be a child of the last node in the robot arm. +Position both the dummies at the tip of the arm, and orient the tip dummy such +that the z-axis points out from the arm. ![image missing](images/tip_dummy.png) ##### Kinematics group -Now we will add a kinematics group (needed for motion planning). In the toolbar on the left, -click to open the 'Calculation Modules' dialog, and navigate to the 'Kinematics' tab. -Click 'Add new IK group' and a new item will appear in the list. Rename this +Now we will add a kinematics group (needed for motion planning). In the toolbar on the left, +click to open the 'Calculation Modules' dialog, and navigate to the 'Kinematics' tab. +Click 'Add new IK group' and a new item will appear in the list. Rename this (by double clicking on item) to '\_ik' where '\' is the name of the arm. For example, for the UR10, this would be 'UR10_ik'. You should be able to leave the default values. Ensure that 'Explicit handling' is checked. ![image missing](images/kinematics_group.png) -Before you leave this dialog, click on 'Edit IK elements', which will open a new dialog. -In this dialog, select the '\_tip' from the dropdown and click 'Add new IK -element with tip'. In the 'Base' dropdown that now becomes visible, select the base/root +Before you leave this dialog, click on 'Edit IK elements', which will open a new dialog. +In this dialog, select the '\_tip' from the dropdown and click 'Add new IK +element with tip'. In the 'Base' dropdown that now becomes visible, select the base/root of your robot, for the UR10, this will be 'UR10'. Close this dialog. -Finally, double click on the 'target' dummy in the scene hierarchy and choose the -'\_tip' option from the 'Linked dummy' dropdown, +Finally, double click on the 'target' dummy in the scene hierarchy and choose the +'\_tip' option from the 'Linked dummy' dropdown, and choose the 'IK, tip-target' option from the 'Link type' dropdown. ![image missing](images/dummy_linking.png) ##### Python Time -Now we are done with the CoppeliaSim side of things. If you have been +Now we are done with the CoppeliaSim side of things. If you have been keeping to the naming conventions, then there is very little code needed! Navigate to `pyrep/arms/` and create a new python file called 'my_robot.py', e.g. 'ur10.py'. @@ -101,7 +101,7 @@ The string: 'MyRobot', should match exactly the prefix used in the previous step ##### Tests and finishing up -Adding unit tests for the robot is easy! +Adding unit tests for the robot is easy! Open up the scene `tests/assets/test_scene_robots.ttt` and place your robot in there. This scene file contains all of the robot arms that have been brought in to PyRep and is used for testing