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)