From 11c71ee67e8ae9a2f99ce5e30b9d72428296a20a Mon Sep 17 00:00:00 2001 From: Rachel Date: Tue, 26 Jul 2016 13:54:33 -0400 Subject: [PATCH 1/4] Initial mimic traj support, broken --- src/prpy/planning/cbirrt.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/prpy/planning/cbirrt.py b/src/prpy/planning/cbirrt.py index 255125b..5bc6334 100644 --- a/src/prpy/planning/cbirrt.py +++ b/src/prpy/planning/cbirrt.py @@ -236,12 +236,14 @@ 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 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;' @@ -249,13 +251,15 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, robot.GetActiveDOF(), len(goal_config) ) ) - + ''' args += ['jointgoals'] + self.serialize_dof_values(goal_config) if tsr_chains is not None: for tsr_chain in tsr_chains: args += ['TSRChain', SerializeTSRChain(tsr_chain)] + print args + # FIXME: Why can't we write to anything other than cmovetraj.txt or # /tmp/cmovetraj.txt with CBiRRT? traj_path = 'cmovetraj.txt' @@ -276,6 +280,14 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, self.env, 'GenericTrajectory') traj.deserialize(traj_xml) + #TODO Mimic traj processing. Change format? + self.mimic_traj = prpy.util.CopyTrajectory(traj) + full_cspec = self.mimic_traj.GetConfigurationSpecification() + traj_bodies = full_cspec.ExtractUsedBodies(robot.GetEnv()) + object_body = [body for body in traj_bodies if body.GetName() != robot.GetName()][0] + object_cspec = object_body.GetActiveConfigurationSpecification('GenericTrajectory') + openravepy.planningutils.ConvertTrajectorySpecification(self.mimic_traj, object_cspec) + # Tag the trajectory as constrained if a constraint TSR is present. if (tsr_chains is not None and any(tsr_chain.constrain for tsr_chain in tsr_chains)): @@ -288,6 +300,10 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, return traj + def GetLastMimicTraj(self): + #FIXME how to handle if no traj exists? + return self.mimic_traj + def ClearIkSolver(self, manip): manip.SetIkSolver(None) manip.SetLocalToolTransform(manip.GetLocalToolTransform()) From df38aa6bba14b42833d064d1877aea6ec232881c Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Fri, 16 Sep 2016 16:48:38 -0400 Subject: [PATCH 2/4] Updating cbirrt to work with contrained trajectories. Fixed TSR serialization. Added logic to get mimic trajectories. Changed parameter ordering to ensure parameters read in proper order --- src/prpy/planning/cbirrt.py | 52 +++++++++++++++++++++++++------------ 1 file changed, 36 insertions(+), 16 deletions(-) diff --git a/src/prpy/planning/cbirrt.py b/src/prpy/planning/cbirrt.py index 453bd71..091914e 100644 --- a/src/prpy/planning/cbirrt.py +++ b/src/prpy/planning/cbirrt.py @@ -52,6 +52,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 @@ -166,7 +167,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): from openravepy import CollisionOptionsStateSaver if timelimit is None: @@ -189,11 +190,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'] @@ -230,7 +226,7 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, ) args += (['jointstarts'] + self.serialize_dof_values(start_config)) - + #TODO need to change robot.GetActiveDOF() to something else # That can take into account another robot if jointgoals is not None: @@ -246,6 +242,7 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, ''' args += ['jointgoals'] + self.serialize_dof_values(goal_config) + if len(jointgoals) > 1: is_endpoint_deterministic = False @@ -258,6 +255,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' @@ -269,6 +271,7 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, options = collision_checker.collision_options with CollisionOptionsStateSaver(env.GetCollisionChecker(), options): response = self.problem.SendCommand(args_str, True) + print args_str if not response.strip().startswith('1'): raise PlanningError('Unknown error: ' + response, @@ -282,12 +285,25 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, #TODO Mimic traj processing. Change format? - self.mimic_traj = prpy.util.CopyTrajectory(traj) - full_cspec = self.mimic_traj.GetConfigurationSpecification() - traj_bodies = full_cspec.ExtractUsedBodies(robot.GetEnv()) - object_body = [body for body in traj_bodies if body.GetName() != robot.GetName()][0] - object_cspec = object_body.GetActiveConfigurationSpecification('GenericTrajectory') - openravepy.planningutils.ConvertTrajectorySpecification(self.mimic_traj, object_cspec) + if save_mimic_trajectories: + from prpy.util import CopyTrajectory + + cspec = traj.GetConfigurationSpecification() + traj_bodies = cspec.ExtractUsedBodies(robot.GetEnv()) + + 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) +# object_traj = CopyTrajectory(traj) + 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 @@ -305,9 +321,12 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, return traj - def GetLastMimicTraj(self): - #FIXME how to handle if no traj exists? - return self.mimic_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) @@ -392,4 +411,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 From eb399c95e518d2beb5ff4cfecc0ad85660ac55eb Mon Sep 17 00:00:00 2001 From: Rachel Date: Fri, 4 Nov 2016 16:10:25 -0400 Subject: [PATCH 3/4] Clean up some comments --- src/prpy/planning/cbirrt.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/prpy/planning/cbirrt.py b/src/prpy/planning/cbirrt.py index 9ca6059..0908a24 100644 --- a/src/prpy/planning/cbirrt.py +++ b/src/prpy/planning/cbirrt.py @@ -292,7 +292,7 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, response = problem.SendCommand(args_str, True) with CollisionOptionsStateSaver(env.GetCollisionChecker(), options): - response = self.problem.SendCommand(args_str, True) + response = problem.SendCommand(args_str, True) if not response.strip().startswith('1'): raise PlanningError('Unknown error: ' + response, @@ -304,25 +304,25 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, traj = openravepy.RaveCreateTrajectory(env, '') traj.deserialize(traj_xml) - - #TODO Mimic traj processing. Change format? + # 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) -# object_traj = CopyTrajectory(traj) openravepy.planningutils.ConvertTrajectorySpecification(object_traj, object_cspec) self.mimic_trajectories[body.GetName()] = object_traj From 1b622016d315d4db8399d629984cf4d06348a4e9 Mon Sep 17 00:00:00 2001 From: Rachel Date: Fri, 4 Nov 2016 16:28:18 -0400 Subject: [PATCH 4/4] Comment regarding checking joint goals --- src/prpy/planning/cbirrt.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/prpy/planning/cbirrt.py b/src/prpy/planning/cbirrt.py index 0908a24..cfb8154 100644 --- a/src/prpy/planning/cbirrt.py +++ b/src/prpy/planning/cbirrt.py @@ -235,6 +235,12 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, #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: ''' @@ -252,6 +258,7 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, 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)]