Merge pull request #3 from crigroup/wip/use-baldor

Use baldor library and add test coverage

.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

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.

package.xml

@@ -16,13 +16,12 @@
   <depend>openrave_catkin</depend>
   <depend>rospy</depend>
 
+  <exec_depend>baldor</exec_depend>
   <exec_depend>image_geometry</exec_depend>
   <exec_depend>python-numpy</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>trajectory_msgs</exec_depend>
 
-  <doc_depend>python-sphinx</doc_depend>
-  <doc_depend>python-sphinx-rtd-theme</doc_depend>
   <doc_depend>rosdoc_lite</doc_depend>
 
   <export>

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]):

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
 

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,30 +18,43 @@ 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
 
   Returns
   -------
   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,15 +135,15 @@ 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)
   elif iktype == orpy.IkParameterizationType.Transform6D:
     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,33 +232,32 @@ 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...'
     statsmodel.autogenerate()
   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):

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
 

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