Feature/mimic traj

src/prpy/planning/cbirrt.py

@@ -54,6 +54,7 @@ def __init__(self, robot_checker_factory=None, timelimit=5.):
         super(CBiRRTPlanner, self).__init__()
 
         self.timelimit = timelimit
+        self.mimic_trajectories = {}
 
         if robot_checker_factory is None:
             robot_checker_factory = DefaultRobotCollisionCheckerFactory
@@ -163,7 +164,7 @@ def PlanToTSR(self, robot, tsr_chains, smoothingitrs=100, **kw_args):
 
     def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
              jointstarts=None, jointgoals=None, psample=None, tsr_chains=None,
-             extra_args=None, **kw_args):
+             extra_args=None, save_mimic_trajectories=False, **kw_args):
         """
         @param allowlimadj If True, adjust the joint limits to include
             the robot's start configuration
@@ -195,11 +196,6 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
         args =  ['RunCBiRRT']
         args += ['timelimit', str(timelimit)]
 
-        # By default, CBiRRT interprets the DOF resolutions as an
-        # L-infinity norm; this flag turns on the L-2 norm instead.
-        args += ['bdofresl2norm', '1']
-        args += ['steplength', '0.05999']
-
         if self._is_baked:
             args += ['bbakedcheckers', '1']
 
@@ -234,25 +230,35 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
                             robot.GetActiveDOF(), len(start_config)
                         )
                     )
-
                 args += (['jointstarts'] +
                          self.serialize_dof_values(start_config))
-
+
+        #TODO need to change robot.GetActiveDOF() to something else
+        # That can take into account another robot
+
+        #FIXME Ordinally we perform this check. However it assumes that
+        # we are only planning for the robot, which is not true
+        # if we are planning a constrained task with an object
+        # (ie opening a door). I'm unsure how to modify this check 
+        # to account for that. 
         if jointgoals is not None:
             for goal_config in jointgoals:
+                '''
                 if len(goal_config) != robot.GetActiveDOF():
                     raise ValueError(
                         'Incorrect number of DOFs in goal configuration;'
                         ' expected {:d}, got {:d}'.format(
                             robot.GetActiveDOF(), len(goal_config)
                         )
                     )
-
+                '''
                 args += ['jointgoals'] + self.serialize_dof_values(goal_config)
 
+
             if len(jointgoals) > 1:
                 is_endpoint_deterministic = False
 
+        raw_input("Continue?")
         if tsr_chains is not None:
             for tsr_chain in tsr_chains:
                 args += ['TSRChain', SerializeTSRChain(tsr_chain)]
@@ -262,6 +268,11 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
                 if tsr_chain.constrain:
                     is_constrained = True
 
+        # By default, CBiRRT interprets the DOF resolutions as an
+        # L-infinity norm; this flag turns on the L-2 norm instead.
+        args += ['bdofresl2norm', '1']
+        args += ['steplength', '0.05999']                
+
         # FIXME: Why can't we write to anything other than cmovetraj.txt or
         # /tmp/cmovetraj.txt with CBiRRT?
         traj_path = 'cmovetraj.txt'
@@ -287,6 +298,8 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
             contextlib.nested(*mimicbody_savers), save_dof_limits(robot):
             response = problem.SendCommand(args_str, True)
 
+        with CollisionOptionsStateSaver(env.GetCollisionChecker(), options):
+            response = problem.SendCommand(args_str, True)
 
         if not response.strip().startswith('1'):
             raise PlanningError('Unknown error: ' + response,
@@ -298,6 +311,28 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
             traj = openravepy.RaveCreateTrajectory(env, '')
             traj.deserialize(traj_xml)
 
+        # Mimic traj processing if requested
+        if save_mimic_trajectories:
+            from prpy.util import CopyTrajectory
+
+            cspec = traj.GetConfigurationSpecification()
+            traj_bodies = cspec.ExtractUsedBodies(robot.GetEnv())
+
+            # Extract non-robot trajecotry
+            self.mimic_trajectories = {}
+            for body in traj_bodies:
+                if body.GetName() == robot.GetName():
+                    continue
+
+
+                object_cspec = body.GetActiveConfigurationSpecification('GenericTrajectory')
+                with open(traj_path, 'rb') as traj_file:
+                    traj_xml = traj_file.read()
+                    object_traj = openravepy.RaveCreateTrajectory(env, '')
+                    object_traj.deserialize(traj_xml)
+                openravepy.planningutils.ConvertTrajectorySpecification(object_traj, object_cspec)
+                self.mimic_trajectories[body.GetName()] = object_traj
+
         # Tag the trajectory as non-determistic since CBiRRT is a randomized
         # planner. Additionally tag the goal as non-deterministic if CBiRRT
         # chose from a set of more than one goal configuration.
@@ -314,6 +349,13 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
 
         return traj
 
+    def GetMimicPath(self, body_name, env=None):
+        traj = self.mimic_trajectories.get(body_name, None)
+        if traj is not None and env is not None:
+            from prpy.util import CopyTrajectory
+            traj = CopyTrajectory(traj, env=env)
+        return traj
+
     def ClearIkSolver(self, manip):
         manip.SetIkSolver(None)
         manip.SetLocalToolTransform(manip.GetLocalToolTransform())
@@ -397,4 +439,5 @@ def SerializeTSRChain(self):
     if len(self.mimicbodyjoints) > 0:
         outstring += ' %d %s' % (len(self.mimicbodyjoints),
                                  SerializeArray(self.mimicbodyjoints))
+#        outstring += ' %s' % SerializeArray(self.mimicbodyjoints)
     return outstring