class TestCore(unittest.TestCase):

    def setUp(self):
        self.pyrep = PyRep()
        self.pyrep.launch(path.join(ASSET_DIR, 'test_scene.ttt'), headless=True)
        self.pyrep.step()
        self.pyrep.start()

    def tearDown(self):
        self.pyrep.stop()
        self.pyrep.step_ui()
        self.pyrep.shutdown() class PyRep(object):
    """Used for interfacing with the CoppeliaSim simulation.

    Can be used for starting, stopping, and stepping the simulation. As well
    as getting, and creating scene objects and robots.
    """

    def __init__(self):
        self.running = False
        self._process = None
        self._robot_to_count = {}
        self.connected = False

        self._ui_thread = None
        self._responsive_ui_thread = None

        self._init_thread_id = None
        self._shutting_down = False

        self._handles_to_objects = {}

        if 'COPPELIASIM_ROOT' not in os.environ:
            raise PyRepError(
                'COPPELIASIM_ROOT not defined. See installation instructions.')
        self._vrep_root = os.environ['COPPELIASIM_ROOT']
        if not os.path.exists(self._vrep_root):
            raise PyRepError(
                'COPPELIASIM_ROOT was not a correct path. '
                'See installation instructions')

    def _run_ui_thread(self, scene_file: str, headless: bool,
                       verbosity: Verbosity) -> None:
        # Need this otherwise extensions will not be loaded
        os.chdir(self._vrep_root)
        options = sim.sim_gui_headless if headless else sim.sim_gui_all
        sim.simSetStringParameter(
            sim.sim_stringparam_verbosity, verbosity.value)
        sim.simExtLaunchUIThread(
            options=options, scene=scene_file, pyrep_root=self._vrep_root)

    def _run_responsive_ui_thread(self) -> None:
        while True:
            if not self.running:
                with utils.step_lock:
                    if self._shutting_down or sim.simExtGetExitRequest():
                        break
                    sim.simExtStep(False)
            time.sleep(0.01)
        # If the exit request was from the UI, then call shutdown, otherwise
        # shutdown caused this thread to terminate.
        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:
        """Launches CoppeliaSim.

        Launches the UI thread, waits until the UI thread has finished, this
        results in the current thread becoming the simulation thread.

        :param scene_file: The scene file to load. Empty string for empty scene.
        :param headless: Run CoppeliaSim in simulation mode.
        :param responsive_ui: If True, then a separate thread will be created to
            asynchronously step the UI of CoppeliaSim. Note, that will reduce
            the responsiveness of the simulation thread.
        :param blocking: Causes CoppeliaSim to launch as if running the default
            c++ client application. This is causes the function to block.
            For most users, this will be set to False.
        :param verbosity: The verbosity level for CoppeliaSim.
            Usually Verbosity.NONE or Verbosity.LOAD_INFOS.
        """
        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)
        cwd = os.getcwd()
        self._ui_thread = threading.Thread(
            target=self._run_ui_thread,
            args=(abs_scene_file, headless, verbosity))
        self._ui_thread.daemon = True
        self._ui_thread.start()

        while not sim.simExtCanInitSimThread():
            time.sleep(0.1)

        sim.simExtSimThreadInit()
        time.sleep(0.2)  # Stops CoppeliaSim crashing if restarted too quickly.

        if blocking:
            while not sim.simExtGetExitRequest():
                sim.simExtStep()
            self.shutdown()
        elif responsive_ui:
            self._responsive_ui_thread = threading.Thread(
                target=self._run_responsive_ui_thread)
            self._responsive_ui_thread.daemon = True
            try:
                self._responsive_ui_thread.start()
            except (KeyboardInterrupt, SystemExit):
                if not self._shutting_down:
                    self.shutdown()
                sys.exit()
            self.step()
        else:
            self.step()
        os.chdir(cwd)  # Go back to the previous cwd

    def script_call(self, 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.

        :param function_name_at_script_name: A string representing the function
            name and script name, e.g. myFunctionName@theScriptName. When the
            script is not associated with an object, then just specify the
            function name.
        :param script_handle_or_type: The handle of the script, otherwise the
            type of the script.
        :param ints: The input ints to the script.
        :param floats: The input floats to the script.
        :param strings: The input strings to the script.
        :param bytes: The input bytes to the script (as a string).
        :return: Any number of return values from the called Lua function.
        """
        return utils.script_call(
            function_name_at_script_name, script_handle_or_type, ints, floats,
            strings, bytes)

    def shutdown(self) -> None:
        """Shuts down the CoppeliaSim simulation.
        """
        if self._ui_thread is None:
            raise PyRepError(
                'CoppeliaSim has not been launched. Call launch first.')
        if self._ui_thread is not None:
            self._shutting_down = True
            self.stop()
            self.step_ui()
            sim.simExtPostExitRequest()
            sim.simExtSimThreadDestroy()
            self._ui_thread.join()
            if self._responsive_ui_thread is not None:
                self._responsive_ui_thread.join()
            # CoppeliaSim crashes if new instance opened too quickly after shutdown.
            # TODO: A small sleep stops this for now.
            time.sleep(0.1)
        self._ui_thread = None
        self._shutting_down = False

    def start(self) -> None:
        """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.')
        if not self.running:
            sim.simStartSimulation()
            self.running = True

    def stop(self) -> None:
        """Stops the physics simulation if it is running.
        """
        if self._ui_thread is None:
            raise PyRepError(
                'CoppeliaSim has not been launched. Call launch first.')
        if self.running:
            sim.simStopSimulation()
            self.running = False
            # Need this so the UI updates
            [self.step() for _ in range(5)]  # type: ignore

    def step(self) -> None:
        """Execute the next simulation step.

        If the physics simulation is not running, then this will only update
        the UI.
        """
        with utils.step_lock:
            sim.simExtStep()

    def step_ui(self) -> None:
        """Update the UI.

        This will not execute the next simulation step, even if the physics
        simulation is running.
        This is only applicable when PyRep was launched without a responsive UI.
        """
        with utils.step_lock:
            sim.simExtStep(False)

    def set_simulation_timestep(self, dt: float) -> None:
        """Sets the simulation time step. Default is 0.05.

        :param dt: The time step value in seconds.
        """
        sim.simSetFloatParameter(sim.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.')

    def get_simulation_timestep(self) -> float:
        """Gets the simulation time step.

        :return: The time step value in seconds.
        """
        return sim.simGetSimulationTimeStep()

    def set_configuration_tree(self, config_tree: bytes) -> None:
        """Restores configuration information previously retrieved.

        Configuration information (object relative positions/orientations,
        joint/path values) can be retrieved with
        :py:meth:`Object.get_configuration_tree`. Dynamically simulated
        objects will implicitly be reset before the command is applied
        (i.e. similar to calling :py:meth:`Object.reset_dynamic_object` just
        before).

        :param config_tree: The configuration tree to restore.
        """
        sim.simSetConfigurationTree(config_tree)

    def group_objects(self, objects: List[Shape]) -> Shape:
        """Groups several shapes into a compound shape (or simple shape).

        :param objects: The list of shapes to group.
        :return: A single grouped shape.
        """
        handles = [o.get_handle() for o in objects]
        handle = sim.simGroupShapes(handles)
        return Shape(handle)

    def merge_objects(self, objects: List[Shape]) -> Shape:
        """Merges several shapes into a compound shape (or simple shape).

        :param objects: The list of shapes to group.
        :return: A single merged shape.
        """
        handles = [o.get_handle() for o in objects]
        # FIXME: sim.simGroupShapes(merge=True) won't return correct handle,
        # so we use name to find correct handle of the merged shape.
        name = objects[-1].get_name()
        sim.simGroupShapes(handles, merge=True)
        return Shape(name)

    def export_scene(self, filename: str) -> None:
        """Saves the current scene.

        :param filename: scene filename. The filename extension is required
            ("ttt").
        """
        sim.simSaveScene(filename)

    def import_model(self, filename: str) -> Object:
        """	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
            which case the model's objects will be named/renamed as if an
            associated script was attached to the model.
        :return: The imported model.
        """
        handle = sim.simLoadModel(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]:
        """Creates a planar shape that is textured.

        :param filename: Path to the texture to load.
        :param interpolate: Adjacent texture pixels are not interpolated.
        :param decal_mode: Texture is applied as a decal (its appearance
            won't be influenced by light conditions).
        :param repeat_along_u: Texture will be repeated along the U direction.
        :param repeat_along_v: Texture will be repeated along the V direction.
        :return: A tuple containing the textured plane and the texture.
        """
        options = 0
        if not interpolate:
            options |= 1
        if decal_mode:
            options |= 2
        if repeat_along_u:
            options |= 3
        if repeat_along_v:
            options |= 4
        handle = sim.simCreateTexture(filename, options)
        s = Shape(handle)
        return s, s.get_texture()

    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
            all objects in the configuration tree. :py:class:`Object` or `int`.
        :param object_type: The object type to retrieve.
            One of :py:class:`.ObjectType`.
        :param exclude_base: Exclude the tree base from the returned list.
        :param first_generation_only: Include in the returned list only the
            object's first children. Otherwise, entire hierarchy is returned.
        :return: A list of objects in the hierarchy tree.
        """
        return Object._get_objects_in_tree(root_object, *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
        """
        return sim.simGetCollectionHandle(collection_name) class CartesianPath(Object):
    """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 = sim.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.)) -> 'CartesianPath':
        """Creates a cartesian path and inserts in the scene.

        :param show_line: Shows line in UI.
        :param show_position: Shows line in UI.
        :param show_orientation: Shows orientation in UI.
        :param closed_path: If set, then a path's last control point will be
            linked to its first control point to close the path and make its
            operation cyclic. A minimum of 3 control points are required for
            a path to be closed.
        :param automatic_orientation: If set, then all control points and
            Bezier point's orientation will automatically be calculated in
            order to have a point's z-axis along the path, and its y-axis
            pointing outwards its curvature (if keep x up is enabled, the
            y-axis is not particularly constrained). If disabled, the user
            determines the control point's orientation and the Bezier points'
            orientation will be interpolated from the path's control points'
            orientation.
        :param flat_path: If set, then all control points (and subsequently all
            Bezier points) will be constraint to the z=0 plane of the path
            object's local reference frame.
        :param keep_x_up: If set, then the automatic orientation functionality
            will align each Bezier point's z-axis along the path and keep its
            x-axis pointing along the path object's z-axis.
        :param line_size: Size of the line in pixels.
        :param length_calculation_method: Method for calculating the path length. See
            https://www.coppeliarobotics.com/helpFiles/en/apiConstants.htm#distanceCalculationMethods
        :param control_point_size: Size of the control points in the path.
        :param ang_to_lin_conv_coeff: The angular to linear conversion coefficient.
        :param virt_dist_scale_factor: The virtual distance scaling factor.
        :param path_color: Ambient diffuse rgb color of the path.

        :return: The newly created cartesian path.
        """
        attributes = 0
        if show_line:
            attributes |= sim.sim_pathproperty_show_line
        if show_orientation:
            attributes |= sim.sim_pathproperty_show_orientation
        if closed_path:
            attributes |= sim.sim_pathproperty_closed_path
        if automatic_orientation:
            attributes |= sim.sim_pathproperty_automatic_orientation
        if flat_path:
            attributes |= sim.sim_pathproperty_flat_path
        if show_position:
            attributes |= sim.sim_pathproperty_show_position
        if keep_x_up:
            attributes |= sim.sim_pathproperty_keep_x_up

        handle = sim.simCreatePath(attributes, [line_size, length_calculation_method, 0],
                                   [control_point_size, ang_to_lin_conv_coeff, virt_dist_scale_factor],
                                   list(path_color))
        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]]:
        """Retrieves the absolute interpolated pose of a point along the path.

        :param relative_distance: A value between 0 and 1, where 0 is the
            beginning of the path, and 1 the end of the path.
        :return: A tuple containing the x, y, z position, and the x, y, z
            orientation of the point on the path (in radians).
        """
        pos = sim.simGetPositionOnPath(self._handle, relative_distance)
        ori = sim.simGetOrientationOnPath(self._handle, relative_distance)
        return pos, ori

    def insert_control_points(self, poses: List[List[float]]) -> None:
        """Inserts one or several control points into the path.

        :param poses: A list of lists containing 6 values representing the pose
            of each of the new control points. Orientation in radians.
        """
        data = []
        for p in poses:
            data.extend(p)
        self._script_call('insertPathControlPoint@PyRep',
                          ints=[self._handle, len(poses)], floats=data)

    def _script_call(self, func: str, ints=(), floats=(), strings=(), bytes=''):
        return sim.simExtCallScriptFunction(
            func, sim.sim_scripttype_addonscript,
            list(ints), list(floats), list(strings), bytes) class Dummy(Object):
    """A point with orientation.

    Dummies are multipurpose objects that can have many different applications.
    """

    @staticmethod
    def create(size=0.01) -> 'Dummy':
        """Creates a dummy object and inserts in the scene.

        :param size: The size of the dummy object.
        :return: The newly created Dummy.
        """
        handle = sim.simCreateDummy(size, None)
        return Dummy(handle)

    def _get_requested_type(self) -> ObjectType:
        return ObjectType.DUMMY class ConfigurationPathError(Exception):
    pass class Panda(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'Panda', 7) class Mico(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'Mico', 6) class Jaco(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'Jaco', 6) class Sawyer(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'Sawyer', 7) class BaxterLeft(Arm):

    def __init__(self, count: int = 0):
        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') class LBRIwaa7R800(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'LBR_iiwa_7_R800', 7) class LBRIwaa14R820(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'LBR_iiwa_14_R820', 7) class UR3(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'UR3', 6) class UR5(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'UR5', 6) class UR10(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'UR10', 6) class Dobot(Arm):

    def __init__(self, count: int = 0):
        super().__init__(count, 'Dobot', 4)
import unittest
import numpy as np
from tests.core import TestCore
from pyrep import PyRep
from pyrep.objects.cartesian_path import CartesianPath
from pyrep.objects.dummy import Dummy
from os import path
from pyrep.errors import ConfigurationPathError
from pyrep.robots.arms.panda import Panda
from pyrep.robots.arms.mico import Mico
from pyrep.robots.arms.jaco import Jaco
from pyrep.robots.arms.sawyer import Sawyer
from pyrep.robots.arms.baxter import BaxterLeft, BaxterRight
from pyrep.robots.arms.lbr_iiwa_7_r800 import LBRIwaa7R800
from pyrep.robots.arms.lbr_iiwa_14_r820 import LBRIwaa14R820
from pyrep.robots.arms.ur3 import UR3
from pyrep.robots.arms.ur5 import UR5
from pyrep.robots.arms.ur10 import UR10
from pyrep.robots.arms.dobot import Dobot
            with self.subTest(arm=arm_name):
                arm = arm_type()
                self.assertIsInstance(arm, arm_type)

    def test_set_ik_element_properties(self):
        arm = Panda()
        arm.set_ik_element_properties(constraint_gamma=False)
        arm.set_ik_element_properties()

    def test_set_ik_group_properties(self):
        arm = Panda()
        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')
        new_config = arm.solve_ik_via_jacobian(
            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))

    def test_solve_ik_via_sampling(self):
        arm = Panda()
        waypoint = Dummy('Panda_waypoint')
        configs = arm.solve_ik_via_sampling(
            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))
        # 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))
        # Checks order
        prev_config_dist = 0
        for config in configs:
            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)
            prev_config_dist = config_dist

    def test_get_path_from_cartesian_path(self):
        arm = Panda()
        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())
        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())
        self.assertIsNotNone(path)

    def test_get_nonlinear_path(self):
        arm = Panda()
        waypoint = Dummy('Panda_waypoint')
        path = arm.get_nonlinear_path(
            waypoint.get_position(), waypoint.get_orientation())
        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)

    def test_get_linear_path_out_of_reach(self):
        arm = Panda()
        with self.assertRaises(ConfigurationPathError):
            arm.get_linear_path([99, 99, 99], [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())
        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))

    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())
        path.set_to_end()
        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())
        # Check that it does not error
        path.visualize()

    def test_get_duplicate_arm(self):
        arm = UR3(1)
        self.assertIsInstance(arm, UR3)

    def test_copy_arm(self):

        arm = UR10()