Merge pull request #82 from personalrobotics/feature/skeletupdate

Added HRC task support with skeleton tracking integration

.gitignore

@@ -1,2 +1,3 @@
 *.pyc
 cmovetraj.txt
+*.sublime-workspace

src/herbpy/action/grasping.py

@@ -122,7 +122,7 @@ def HerbGrasp(robot, obj, push_distance=None, manip=None,
                 with prpy.rave.Disabled(obj):
                     manip.PlanToEndEffectorOffset(direction = push_direction,
                                                   distance = push_distance,
-                                                  execute=True,
+                                                  execute = True,
                                                   **kw_args)
             except PlanningError, e:
                 if push_required:

src/herbpy/herbrobot.py

@@ -193,8 +193,7 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         # Trajectory optimizer.
         try:
             from or_trajopt import TrajoptPlanner
-
-            self.trajopt_planner = TrajoptPlanner()
+            self.trajopt_planner = TrajoptPlanner()  
         except ImportError:
             self.trajopt_planner = None
             logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt'

src/herbpy/tsr/__init__.py

@@ -7,3 +7,4 @@
 from pop_tarts import *
 from generic import *
 from pill_bottle import *
+from box import *

src/herbpy/tsr/box.py

@@ -0,0 +1,45 @@
+import numpy
+import prpy.tsr
+
+@prpy.tsr.tsrlibrary.TSRFactory('herb', 'box', 'stamp')
+def box_stamp(robot, box, manip=None):
+
+    '''
+    This creates a TSR for stamping a box. 
+    It is assumed that when called, the robot is grasping a stamp
+
+    @param robot The robot to perform the stamp
+    @param box The box to stamp
+    @param manip The manipulator to stamp 
+    '''
+
+    with robot.CreateRobotStateSaver(robot.GetActiveManipulatorIndex() | robot.ActiveDOF()):
+      if manip is None:
+        manip_idx = robot.GetActiveManipulatorIndex()
+        manip = robot.GetActiveManipulator()
+      else:
+        manip.SetActive()
+        manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
+
+    T0_w = box.GetTransform()
+
+    ee_to_palm = 0.18
+    palm_to_box_center = .045
+    adjustment = -0.09
+    total_offset = ee_to_palm + palm_to_box_center + adjustment
+    Tw_e = numpy.array([[ 0., 0., 1., -total_offset], 
+                        [1., 0., 0., 0.], 
+                        [0., 1., 0., 0.38], # box height 0.2  0.35
+                        [0., 0., 0., 1.]])
+
+    Bw = numpy.zeros((6,2))
+    Bw[5,:] = [-numpy.pi, numpy.pi]  # Allow any orientation
+
+    grasp_tsr = prpy.tsr.TSR(T0_w = T0_w, Tw_e = Tw_e, Bw = Bw, manip = manip_idx)
+    grasp_chain = prpy.tsr.TSRChain(sample_start=False, sample_goal = True, 
+                                    constrain=False, TSR = grasp_tsr)
+
+    return [grasp_chain]
+
+
+