diff --git a/.travis.yml b/.travis.yml index d4ee7d2..4d72f17 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,6 +13,8 @@ env: # Install package dependencies before_install: + # Testing $DISPLAY + - echo $DISPLAY # Install OpenRAVE - mkdir -p ~/git; cd ~/git - git clone https://github.com/crigroup/openrave-installation.git @@ -29,6 +31,8 @@ before_install: # Prepare rosdep to install dependencies. - sudo rosdep init - rosdep update + # Install coveralls + - pip install coveralls --user # Create a catkin workspace with the package under integration. install: @@ -41,6 +45,8 @@ install: - cd ~/catkin_ws/src - git clone https://github.com/crigroup/openrave_catkin.git - ln -s $CI_SOURCE_PATH . + # TODO: Remove this once baldor is released into the public repositories + - git clone https://github.com/crigroup/baldor.git # Install all dependencies before_script: @@ -52,6 +58,9 @@ script: - cd ~/catkin_ws - catkin_make install - source devel/setup.bash - - cd ~/catkin_ws/src/raveutils/tests - # Test all the modules except for visual - - nosetests -e test_visual -v + - cd ~/catkin_ws/src/raveutils + # Run the tests with coverage + - nosetests tests/ --with-coverage --cover-package=raveutils -v + +after_success: + - coveralls diff --git a/README.md b/README.md index 10667aa..0e7702d 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,11 @@ # raveutils +[![Build Status](https://travis-ci.org/crigroup/raveutils.svg?branch=master)](https://travis-ci.org/crigroup/raveutils) [![Coverage Status](https://coveralls.io/repos/github/crigroup/raveutils/badge.svg)](https://coveralls.io/github/crigroup/raveutils) + ROS package developed by [CRI Group](http://www.ntu.edu.sg/home/cuong/), [Nanyang Technological University, Singapore](http://www.ntu.edu.sg). ### Maintainer -* [Francisco Suárez Ruiz](http://fsuarez6.github.io) - -## Travis - Continuous Integration - -[![Build Status](https://travis-ci.org/crigroup/raveutils.svg?branch=master)](https://travis-ci.org/crigroup/raveutils) +* [Francisco Suárez-Ruiz](http://fsuarez6.github.io) ### Documentation * Throughout the various files in this repository. diff --git a/package.xml b/package.xml index 3da947d..be82767 100644 --- a/package.xml +++ b/package.xml @@ -16,13 +16,12 @@ openrave_catkin rospy + baldor image_geometry python-numpy sensor_msgs trajectory_msgs - python-sphinx - python-sphinx-rtd-theme rosdoc_lite diff --git a/src/raveutils/body.py b/src/raveutils/body.py index 96f5513..6357dbc 100644 --- a/src/raveutils/body.py +++ b/src/raveutils/body.py @@ -1,6 +1,7 @@ #!/usr/bin/env python import itertools import numpy as np +import baldor as br import openravepy as orpy # Local modules import raveutils as ru @@ -45,7 +46,7 @@ def get_bounding_box_corners(body, transform=None, scale=1.): List containing the 8 box corners. Each corner is a XYZ ``np.array`` """ if transform is not None: - Tinv = ru.transforms.transform_inv( body.GetTransform() ) + Tinv = br.transform.inverse(body.GetTransform()) aabb = body.ComputeAABB() corners = [] for k in itertools.product([-1,1],[-1,1],[-1,1]): diff --git a/src/raveutils/conversions.py b/src/raveutils/conversions.py index e21cf99..66dd83a 100644 --- a/src/raveutils/conversions.py +++ b/src/raveutils/conversions.py @@ -1,5 +1,6 @@ #!/usr/bin/env python import numpy as np +import baldor as br import openravepy as orpy # Local modules import raveutils as ru @@ -19,7 +20,7 @@ def from_ray(ray): transform: array_like The resulting homogeneous transformation """ - transform = ru.transforms.transform_between_axes(ru.transforms.Z_AXIS, ray.dir()) + transform = br.transform.between_axes(br.Z_AXIS, ray.dir()) transform[:3,3] = ray.pos() return transform diff --git a/src/raveutils/kinematics.py b/src/raveutils/kinematics.py index 964c87e..ed3c3be 100644 --- a/src/raveutils/kinematics.py +++ b/src/raveutils/kinematics.py @@ -1,11 +1,13 @@ #!/usr/bin/env python import numpy as np +import baldor as br import openravepy as orpy # Local modules import raveutils as ru -def compute_jacobian(robot, link_name=None, translation_only=False): +def compute_jacobian(robot, link_name=None, link_idx=None, + translation_only=False): """ Compute the Jacobian matrix @@ -16,6 +18,9 @@ def compute_jacobian(robot, link_name=None, translation_only=False): link_name: str, optional The name of link. If it's `None`, the last link of the kinematic chain is used + link_idx: int, optional + The index of link. If it's `None`, the last link of the kinematic chain is + used translation_only: bool, optional If set, only the translation Jacobian is computed @@ -23,23 +28,33 @@ def compute_jacobian(robot, link_name=None, translation_only=False): ------- J: array_like The computed Jacobian matrix + + Notes + ----- + If both `link_idx` and `link_name` are given, `link_idx` will have priority. """ - if link_name is None: - link_name = robot.GetLinks()[-1].GetName() - names = [l.GetName() for l in robot.GetLinks()] - if link_name not in names: - raise KeyError('Invalid link name: {0}'.format(link_name)) - idx = names.index(link_name) - origin = robot.GetLink(link_name).GetTransform()[:3,3] + num_links = len(robot.GetLinks()) + if link_idx is None: + if link_name is None: + link_name = robot.GetLinks()[-1].GetName() + link_idx = num_links - 1 + else: + names = [l.GetName() for l in robot.GetLinks()] + if link_name not in names: + raise KeyError('Invalid link name: {0}'.format(link_name)) + link_idx = names.index(link_name) + elif not (0 <= num_links < num_links): + raise IndexError('Invalid link index: {0}'.format(num_links)) + origin = robot.GetLinks()[link_idx].GetTransform()[:3,3] manip = robot.GetActiveManipulator() indices = manip.GetArmIndices() - Jtrans = robot.ComputeJacobianTranslation(idx, origin)[:,indices] + Jtrans = robot.ComputeJacobianTranslation(link_idx, origin)[:,indices] if translation_only: J = Jtrans else: J = np.zeros((6, Jtrans.shape[1])) J[:3,:] = Jtrans - J[3:,:] = robot.ComputeJacobianAxisAngle(idx)[:,indices] + J[3:,:] = robot.ComputeJacobianAxisAngle(link_idx)[:,indices] return J def compute_yoshikawa_index(robot, link_name=None, translation_only=False, @@ -120,7 +135,7 @@ def find_ik_solutions(robot, target, iktype, collision_free=True, freeinc=0.1): target_list = [] if iktype == orpy.IkParameterizationType.TranslationDirection5D: if type(target) is not orpy.Ray: - ray = ru.conversions.to_ray(goal) + ray = ru.conversions.to_ray(target) target_list.append(ray) else: target_list.append(target) @@ -128,7 +143,7 @@ def find_ik_solutions(robot, target, iktype, collision_free=True, freeinc=0.1): if type(target) is orpy.Ray: Tray = ru.conversions.from_ray(target) for angle in np.arange(0, 2*np.pi, freeinc): - Toffset = orpy.matrixFromAxisAngle(angle*ru.transforms.Z_AXIS) + Toffset = orpy.matrixFromAxisAngle(angle*br.Z_AXIS) target_list.append(np.dot(Tray, Toffset)) else: target_list.append(target) @@ -155,8 +170,8 @@ def load_ikfast(robot, iktype, freejoints=['J6'], freeinc=[0.01], iktype: orpy.IkParameterizationType Inverse kinematics type to be used freeinc: list - The free increment (discretization) to be used for the free DOF when the - target is the `iktype` is `TranslationDirection5D` + The increment (discretization) to be used for the free DOF when the target + `iktype` is `TranslationDirection5D` autogenerate: bool, optional If true, auto-generate the IKFast solver @@ -217,7 +232,7 @@ def load_link_stats(robot, xyzdelta=0.01, autogenerate=True): success: bool `True` if succeeded, `False` otherwise """ - success = False + success = True statsmodel = orpy.databases.linkstatistics.LinkStatisticsModel(robot) if not statsmodel.load() and autogenerate: print 'Generating LinkStatistics database. Will take ~1 minute...' @@ -225,25 +240,24 @@ def load_link_stats(robot, xyzdelta=0.01, autogenerate=True): if statsmodel.load(): statsmodel.setRobotWeights() statsmodel.setRobotResolutions(xyzdelta=xyzdelta) - success = True else: manip = robot.GetActiveManipulator() indices = manip.GetArmIndices() - if robot.GetActiveDOF() == 6: - origins = [l.GetTransform()[:3,3] for l in robot.GetLinks()] - jweights = [np.linalg.norm(origins[1] - origins[2]), - np.linalg.norm(origins[2] - origins[3]), - np.linalg.norm(origins[3] - origins[4]), - np.linalg.norm(origins[4] - origins[5]), - np.linalg.norm(origins[5] - origins[6]), - np.linalg.norm(origins[6] - origins[-1])] - for i in range(robot.GetActiveDOF()): - jweights[i] = np.sum(jweights[i:]) - robot_weights = np.ones(robot.GetDOF()) - robot_weights[indices] = np.array(jweights) / np.max(jweights) + jweights = [] + for j in indices: + joint = robot.GetJoints()[j] + parent_origin = joint.GetHierarchyParentLink().GetTransform()[:3,3] + child_origin = joint.GetHierarchyChildLink().GetTransform()[:3,3] + jweights.append(np.linalg.norm(child_origin - parent_origin)) + for i in range(len(jweights)): + jweights[i] = np.sum(jweights[i:]) + robot_weights = np.ones(robot.GetDOF()) + robot_weights[indices] = np.array(jweights) / np.max(jweights) + if np.all(robot_weights >= br._EPS): + # All the weights have to be greater than 0 robot.SetDOFWeights(robot_weights) else: - robot.SetDOFWeights([1]*robot.GetDOF()) + success = False return success def random_joint_values(robot): diff --git a/src/raveutils/planning.py b/src/raveutils/planning.py index 145aaa2..56e14f3 100644 --- a/src/raveutils/planning.py +++ b/src/raveutils/planning.py @@ -25,8 +25,8 @@ def plan_to_joint_configuration(robot, qgoal, pname='BiRRT', max_iters=20, Maximum iterations for the post-processing stage. It will use a parabolic smoother wich short-cuts the trajectory and then smooths it try_swap: bool - If set will compute two trajectories: first `qstart` -> `qgoal`, second - `qgoal` -> `qstart` and will select the minimum duration one. + If set, will compute the direct and reversed trajectory. The minimum + duration trajectory is used. Returns ------- @@ -46,7 +46,7 @@ def plan_to_joint_configuration(robot, qgoal, pname='BiRRT', max_iters=20, # Plan trajectory best_traj = None min_duration = float('inf') - is_best_reversed = False + reversed_is_better = False count = 0 for qa, qb in itertools.permutations([qstart, qgoal], 2): count += 1 @@ -65,11 +65,11 @@ def plan_to_joint_configuration(robot, qgoal, pname='BiRRT', max_iters=20, best_traj = orpy.RaveCreateTrajectory(env, traj.GetXMLId()) best_traj.Clone(traj, 0) if count == 2: - is_best_reversed = True + reversed_is_better = True if not try_swap: break # Check if we need to reverse the trajectory - if is_best_reversed: + if reversed_is_better: best_traj = orpy.planningutils.ReverseTrajectory(best_traj) return best_traj diff --git a/src/raveutils/transforms.py b/src/raveutils/transforms.py index 14346c3..ac58783 100644 --- a/src/raveutils/transforms.py +++ b/src/raveutils/transforms.py @@ -4,11 +4,6 @@ import openravepy as orpy -X_AXIS = np.array([1., 0., 0.], dtype=np.float64) -Y_AXIS = np.array([0., 1., 0.], dtype=np.float64) -Z_AXIS = np.array([0., 0., 1.], dtype=np.float64) - - def counterclockwise_hull(hull): """ Make the edges counterclockwise order @@ -25,97 +20,3 @@ def counterclockwise_hull(hull): vccw = np.cross((y - x), (z - y)) if np.inner(vccw, voutward) < 0: hull.simplices[i] = [simplex[0], simplex[2], simplex[1]] - -def perpendicular_vector(vector): - """ - Find an arbitrary perpendicular vector - - Parameters - ---------- - vector: array_like - The input vector - - Returns - ------- - perpendicular: array_like - The perpendicular vector - """ - unit = unit_vector(vector) - if np.allclose(unit[:2], np.zeros(2)): - if np.isclose(unit[2], 0.): - # unit is (0, 0, 0) - raise ValueError('Input vector cannot be a zero vector') - # unit is (0, 0, Z) - perpendicular = np.array(Y_AXIS, dtype=np.float64, copy=True) - perpendicular = np.array([-unit[1], unit[0], 0], dtype=np.float64) - return perpendicular - -def transform_between_axes(axis_a, axis_b): - """ - Compute the transformation that aligns two vectors/axes. - - Parameters - ---------- - axis_a: array_like - The initial axis - axis_b: array_like - The goal axis - - Returns - ------- - transform: array_like - The transformation that transforms axis ``axis_a`` into axis ``axis_b`` - """ - a_unit = unit_vector(axis_a) - b_unit = unit_vector(axis_b) - c = np.dot(a_unit, b_unit) - angle = np.arccos(c) - if np.isclose(c, -1.0) or np.allclose(a_unit, b_unit): - axis = perpendicular_vector(b_unit) - else: - axis = unit_vector(np.cross(a_unit, b_unit)) - transform = orpy.matrixFromAxisAngle(angle*axis) - return transform - -def transform_inv(transform): - """ - Compute the inverse of an homogeneous transformation. - - .. note:: This function is more efficient than :obj:`numpy.linalg.inv` given - the special properties of homogeneous transformations. - - Parameters - ---------- - transform: array_like - The input homogeneous transformation - - Returns - ------- - transform: array_like - The resulting homogeneous transformation inverse - """ - R = transform[:3,:3].T - p = transform[:3,3] - inv = np.eye(4) - inv[:3,:3] = R - inv[:3,3] = np.dot(-R, p) - return inv - - -def unit_vector(vector): - """ - Return unit vector (normalized) - - Parameters - ---------- - vector: array_like - The input vector - - Returns - ------- - unit: array_like - The resulting unit vector (normalized) - """ - unit = np.array(vector, dtype=np.float64, copy=True) - unit /= math.sqrt(np.dot(vector, vector)) - return unit diff --git a/tests/test_body.py b/tests/test_body.py index 3beac2d..a12e3bf 100644 --- a/tests/test_body.py +++ b/tests/test_body.py @@ -39,6 +39,11 @@ def test_get_bounding_box_corners(self): makita = env.GetBodies()[0] corners = ru.body.get_bounding_box_corners(makita) self.assertEqual(len(corners), 8) + # Transform given + makita = env.GetBodies()[0] + transform = makita.GetTransform() + corners = ru.body.get_bounding_box_corners(makita, transform) + self.assertEqual(len(corners), 8) def test_set_body_transparency(self): env = self.env diff --git a/tests/test_conversions.py b/tests/test_conversions.py index c166924..8ff7eb0 100644 --- a/tests/test_conversions.py +++ b/tests/test_conversions.py @@ -1,6 +1,7 @@ #! /usr/bin/env python import unittest import numpy as np +import baldor as br import openravepy as orpy # Tested package import raveutils as ru @@ -10,7 +11,7 @@ class Test_conversions(unittest.TestCase): def test_ray_conversions(self): # Create the ray position = np.array([0.5, -0.25, 0.1]) - direction = ru.transforms.unit_vector([-0.2506, 0.6846, 0.6846]) + direction = br.vector.unit([-0.2506, 0.6846, 0.6846]) ray = orpy.Ray(position, direction) # Check that the conversion works in both direction transform = ru.conversions.from_ray(ray) diff --git a/tests/test_kinematics.py b/tests/test_kinematics.py index 63a7308..977e399 100644 --- a/tests/test_kinematics.py +++ b/tests/test_kinematics.py @@ -29,30 +29,70 @@ def test_compute_jacobian(self): # Try the function with its variants Jtrans = ru.kinematics.compute_jacobian(robot, translation_only=True) J = ru.kinematics.compute_jacobian(robot, translation_only=False) + link_name = robot.GetLinks()[-1].GetName() + J = ru.kinematics.compute_jacobian(robot, link_name=link_name) + # Raise KeyError invalid link name + try: + J = ru.kinematics.compute_jacobian(robot, link_name='invalid_name') + raised_error = False + except KeyError: + raised_error = True + self.assertTrue(raised_error) + # Raise IndexError invalid link index + num_links = len(robot.GetLinks()) + try: + J = ru.kinematics.compute_jacobian(robot, link_idx=num_links) + raised_error = False + except IndexError: + raised_error = True + self.assertTrue(raised_error) def test_compute_yoshikawa_index(self): robot = self.robot - # Try the function with its variants + # Try all the variants idx = ru.kinematics.compute_yoshikawa_index(robot, penalize_jnt_limits=True) idx = ru.kinematics.compute_yoshikawa_index(robot, penalize_jnt_limits=False) + idx = ru.kinematics.compute_yoshikawa_index(robot, translation_only=True) + idx = ru.kinematics.compute_yoshikawa_index(robot, translation_only=False) def test_load_ikfast_and_find_ik_solutions(self): robot = self.robot - # Test loading IKFast + # Test loading IKFast: Transform6D iktype = orpy.IkParameterizationType.Transform6D success = ru.kinematics.load_ikfast(robot, iktype, autogenerate=False) self.assertTrue(success) - # Test find IK solutions + # Test find IK solutions: Transform6D + iktype = orpy.IkParameterizationType.Transform6D manip = robot.GetActiveManipulator() - q = robot.GetActiveDOFValues() T = manip.GetEndEffectorTransform() - iktype = orpy.IkParameterizationType.Transform6D solutions = ru.kinematics.find_ik_solutions(robot, T, iktype, collision_free=False) self.assertTrue(len(solutions) > 0) + ray = ru.conversions.to_ray(T) + solutions = ru.kinematics.find_ik_solutions(robot, ray, iktype, + collision_free=False) + self.assertTrue(len(solutions) > 0) + return + # TODO: Speed-up this test + # Test loading IKFast: TranslationDirection5D + iktype = orpy.IkParameterizationType.TranslationDirection5D + success = ru.kinematics.load_ikfast(robot, iktype, autogenerate=True, + freejoints=['Shoulder_Roll', 'Wrist_Roll']) + self.assertTrue(success) + # Test find IK solutions: TranslationDirection5D + iktype = orpy.IkParameterizationType.TranslationDirection5D + solutions = ru.kinematics.find_ik_solutions(robot, T, iktype, + collision_free=False) + self.assertTrue(len(solutions) > 0) + ray = ru.conversions.to_ray(T) + solutions = ru.kinematics.find_ik_solutions(robot, ray, iktype, + collision_free=False) + self.assertTrue(len(solutions) > 0) def test_load_link_stats(self): - robot = self.robot - # Generate the Link Statistics database - success = ru.kinematics.load_link_stats(robot, autogenerate=True) + # Autogenerate: False + success = ru.kinematics.load_link_stats(self.robot, autogenerate=False) + self.assertTrue(success) + # Autogenerate: True + success = ru.kinematics.load_link_stats(self.robot, autogenerate=True) self.assertTrue(success) diff --git a/tests/test_planning.py b/tests/test_planning.py index 673d893..edf6d9a 100644 --- a/tests/test_planning.py +++ b/tests/test_planning.py @@ -41,6 +41,10 @@ def test_plan_to_joint_configuration(self): traj2 = ru.planning.plan_to_joint_configuration(robot, qgoal, pname='BiRRT', try_swap=True) self.assertNotEqual(traj2, None) + # Test without post-processing + traj3 = ru.planning.plan_to_joint_configuration(robot, qgoal, pname='BiRRT', + max_ppiters=-1) + self.assertNotEqual(traj3, None) def test_retime_trajectory(self): robot = self.robot @@ -69,6 +73,7 @@ def test_ros_trajectory_from_openrave(self): np.testing.assert_almost_equal(ros_traj_duration, traj.GetDuration()) # Check num of waypoints self.assertEqual(len(ros_traj.points), traj.GetNumWaypoints()) + # TODO: Send trajectory with repeated waypoints def test_trajectory_from_waypoints(self): diff --git a/tests/test_transforms.py b/tests/test_transforms.py deleted file mode 100644 index 279798a..0000000 --- a/tests/test_transforms.py +++ /dev/null @@ -1,42 +0,0 @@ -#! /usr/bin/env python -import unittest -import numpy as np -import openravepy as orpy -# Tested package -import raveutils as ru - - -class Test_transforms(unittest.TestCase): - def test_perpendicular_vector(self): - np.random.seed(123) - vector = np.random.randn(3) - vector /= np.linalg.norm(vector) - pervector = ru.transforms.perpendicular_vector(vector) - np.testing.assert_almost_equal(0, np.dot(vector, pervector)) - - def test_transform_between_axes(self): - np.random.seed(123) - axis = np.random.randn(3) - axis /= np.linalg.norm(axis) - angle = np.deg2rad(45) - transform = orpy.matrixFromAxisAngle(angle*axis) - newaxis = np.dot(transform[:3,:3], ru.transforms.Z_AXIS) - est_transform = ru.transforms.transform_between_axes(ru.transforms.Z_AXIS, - newaxis) - np.testing.assert_allclose(transform[:3,2], est_transform[:3,2]) - - def test_transform_inv(self): - np.random.seed(123) - axis = np.random.randn(3) - axis /= np.linalg.norm(axis) - angle = np.deg2rad(45) - transform = orpy.matrixFromAxisAngle(angle*axis) - transform[:3,3] = np.random.randn(3)*0.5 - inv = ru.transforms.transform_inv(transform) - np.testing.assert_array_almost_equal(np.eye(4), np.dot(transform, inv)) - - def test_unit_vector(self): - np.random.seed(123) - vector = np.random.randn(3) - unit = vector / np.linalg.norm(vector) - np.testing.assert_allclose(unit, ru.transforms.unit_vector(vector)) diff --git a/tests/test_visual.py b/tests/test_visual.py index 093452f..00f51e4 100644 --- a/tests/test_visual.py +++ b/tests/test_visual.py @@ -1,4 +1,5 @@ #! /usr/bin/env python +import os import unittest import numpy as np import openravepy as orpy @@ -9,10 +10,18 @@ class Test_visual(unittest.TestCase): @classmethod def setUpClass(cls): + # Check there is a display available + display_available = False + if os.environ.has_key('DISPLAY'): + if len(os.environ['DISPLAY']) > 0: + display_available = True + # Setup the environment np.set_printoptions(precision=6, suppress=True) env = orpy.Environment() - env.SetViewer('qtcoin') + if display_available: + env.SetViewer('qtcoin') cls.env = env + cls.display_available = display_available print('') # dummy line @classmethod @@ -28,8 +37,9 @@ def test_draw_axes(self): transform = orpy.matrixFromAxisAngle(angle*axis) transform[:3,3] = np.random.randn(3)*0.5 h = ru.visual.draw_axes(self.env, transform) - self.assertEqual(len(h), 1) - self.assertEqual(type(h[0]), orpy.GraphHandle) + if self.display_available: + self.assertEqual(len(h), 1) + self.assertEqual(type(h[0]), orpy.GraphHandle) def test_draw_plane(self): np.random.seed(123) @@ -39,14 +49,16 @@ def test_draw_plane(self): transform = orpy.matrixFromAxisAngle(angle*axis) transform[:3,3] = np.random.randn(3)*0.5 h = ru.visual.draw_plane(self.env, transform) - self.assertEqual(type(h), orpy.GraphHandle) + if self.display_available: + self.assertEqual(type(h), orpy.GraphHandle) def test_draw_point(self): np.random.seed(123) point = np.random.randn(3) h = ru.visual.draw_point(self.env, point) - self.assertEqual(len(h), 1) - self.assertEqual(type(h[0]), orpy.GraphHandle) + if self.display_available: + self.assertEqual(len(h), 1) + self.assertEqual(type(h[0]), orpy.GraphHandle) def test_draw_ray(self): np.random.seed(123) @@ -55,7 +67,12 @@ def test_draw_ray(self): position = np.random.randn(3)*0.5 ray = orpy.Ray(position, direction) handles = ru.visual.draw_ray(self.env, ray) - self.assertEqual(len(handles), 3) - types = [type(h) for h in handles] - self.assertEqual(len(set(types)), 1) - self.assertEqual(set(types), {orpy.GraphHandle}) + if self.display_available: + self.assertEqual(len(handles), 3) + types = [type(h) for h in handles] + self.assertEqual(len(set(types)), 1) + self.assertEqual(set(types), {orpy.GraphHandle}) + # Use negative distance + handles = ru.visual.draw_ray(self.env, ray, dist=-0.03) + if self.display_available: + self.assertEqual(len(handles), 3)