def main(*arg): Robot_Option = "../user/" world = WorldModel() # WorldModel is a pre-defined class EnviName = "/home/motion/Desktop/Whole-Body-Planning-for-Push-Recovery-Data/flat_1Contact/" # The next step is to load in robot's XML file XML_path = EnviName + "Envi.xml" result = world.readFile( XML_path ) # Here result is a boolean variable indicating the result of this loading operation if not result: raise RuntimeError("Unable to load model " + XML_path) # In this case, what we have is a config CtrlStateTraj = Trajectory(world.robot(0)) CaseNo = 1 TestNo = 5 SpecificPath = EnviName + "/" + str(CaseNo) + "/" + str(TestNo) CtrlStateTraj.load(SpecificPath + "/CtrlStateTraj.path") import ipdb ipdb.set_trace() Config_Init = CtrlStateTraj.milestones[-1] Robot_Config_Plot(world, len(Config_Init), Config_Init)
def main(): ExpNo = 0 StateType = "P" VisMode = "Traj" if (len(sys.argv[1:]) == 1): ExpNo = int(sys.argv[1:][0]) elif (len(sys.argv[1:]) == 2): ExpNo = int(sys.argv[1:][0]) StateType = sys.argv[1:][1] else: ExpNo = int(sys.argv[1:][0]) StateType = sys.argv[1:][1] VisChoice = str(sys.argv[1:][2]) if VisChoice == "T" or VisChoice == "Traj": VisMode = "Traj" if VisChoice == "Poly" or VisChoice == "Polytope": VisMode = "Poly" if VisChoice == "PIP": VisMode = "PIP" world = WorldModel() # WorldModel is a pre-defined class curDir = os.getcwd() CurCasePath = curDir[0:-4] + "-Data/result/" + CurCase XML_path = CurCasePath + "/Environment.xml" result = world.readFile( XML_path ) # Here result is a boolean variable indicating the result of this loading operation if not result: raise RuntimeError("Unable to load model " + XML_path) ContactLinkDictionary = ContactLinkReader(curDir + "/../user/ContactLink.txt") PlanStateTraj = Trajectory(world.robot(0)) CtrlStateTraj = Trajectory(world.robot(0)) FailureStateTraj = Trajectory(world.robot(0)) # SpecificPath = CurCasePath + "/" + str(ExpNo) + '/Backup' SpecificPath = CurCasePath + "/" + str(ExpNo) PlanStateTraj.load(SpecificPath + "/PlanStateTraj.path") CtrlStateTraj.load(SpecificPath + "/CtrlStateTraj.path") FailureStateTraj.load(SpecificPath + "/FailureStateTraj.path") ExpTraj = [FailureStateTraj, CtrlStateTraj, PlanStateTraj] PIPInfoList = PIPTrajReader(SpecificPath) StartTime, EndTime, ImpulForce = ImpulseInfoReader(SpecificPath) ImpulseObj = [StartTime, EndTime, ImpulForce] PlanningObj = [] try: PlanningTimeList, PlanningResList = PlanningInfoReader(SpecificPath) PlanningObj = [PlanningTimeList, PlanningResList] except IOError: print "Initial Push Does Not Cause Fall!" ExperimentVisualizer(world, ContactLinkDictionary, ExpTraj, PIPInfoList, ImpulseObj, PlanningObj, SpecificPath, [StateType, VisMode])
def main(): ipdb.set_trace() ExpNo = int(sys.argv[1:][0]) StateType = str(sys.argv[1:][1]) SwitchType = str(sys.argv[1:][2]) PlanningType = SwitchType # VisMode = "Traj" # "Traj", "Poly", "PIP" # VisMode = "Poly" # "Traj", "Poly", "PIP" VisMode = "PIP" # "Traj", "Poly", "PIP" if (len(sys.argv[1:]) >= 4): VisChoice = str(sys.argv[1:][3]) VisMode = "Traj" if VisChoice == "T" or VisChoice == "Traj": VisMode = "Traj" if VisChoice == "Poly" or VisChoice == "Polytope": VisMode = "Poly" if VisChoice == "PIP": VisMode = "PIP" Robot_Option = "../user/hrp2/" world = WorldModel() # WorldModel is a pre-defined class curDir = os.getcwd() ExpName = curDir + "/../result/" + ExpType XML_path = ExpName + "/../../" + EnviName + ".xml" result = world.readFile( XML_path ) # Here result is a boolean variable indicating the result of this loading operation if not result: raise RuntimeError("Unable to load model " + XML_path) ContactLinkDictionary = ContactLinkReader("ContactLink.txt", Robot_Option) PlanStateTraj = Trajectory(world.robot(0)) CtrlStateTraj = Trajectory(world.robot(0)) FailureStateTraj = Trajectory(world.robot(0)) SpecificPath = ExpName + ContactType + "/" + str(ExpNo) + "/" + str( PlanningType) PlanStateTraj.load(SpecificPath + "/PlanStateTraj.path") CtrlStateTraj.load(SpecificPath + "/CtrlStateTraj.path") FailureStateTraj.load(SpecificPath + "/FailureStateTraj.path") ExpTraj = [FailureStateTraj, CtrlStateTraj, PlanStateTraj] PIPInfoList = PIPTrajReader(SpecificPath) StartTime, EndTime, ImpulForce = ImpulseInfoReader(SpecificPath) ImpulseObj = [StartTime, EndTime, ImpulForce] PlanningTimeList, PlanningResList = PlanningInfoReader(SpecificPath) PlanningObj = [PlanningTimeList, PlanningResList] ExperimentVisualizer(world, ContactLinkDictionary, ExpTraj, PIPInfoList, ImpulseObj, PlanningObj, SpecificPath, [StateType, VisMode])
class GeneralizedPath: def __init__(self): self.type = None self.data = None def load(self, fn): if fn.endswith('xml'): self.type = 'MultiPath' self.data = MultiPath() self.data.load(fn) elif fn.endswith('path') or fn.endswith('traj'): self.type = 'Trajectory' self.data = Trajectory() self.data.load(fn) elif fn.endswith('configs') or fn.endswith('milestones'): self.type = 'Configs' self.data = Trajectory() self.data.configs = loader.load('Configs', fn) self.data.times = list(range(len(self.data.configs))) else: print("Unknown extension on file", fn, ", loading as Trajectory") self.type = 'Trajectory' self.data = Trajectory() self.data.load(fn) def save(self, fn): if fn.endswith('xml') and self.type != 'MultiPath': self.toMultipath() self.data.save(fn) def hasTiming(self): if self.type == 'Configs': return False if self.type == 'Trajectory': return True return self.data.hasTiming() def duration(self): if self.type == 'MultiPath': return self.data.endTime() else: return self.data.duration() def start(self): if self.type == 'MultiPath': return self.data.sections[0].configs[0] else: return self.data.milestones[0] def end(self): if self.type == 'MultiPath': return self.data.sections[-1].configs[-1] else: return self.data.milestones[-1] def timescale(self, scale, ofs): rev = (scale < 0) if rev: scale = -scale if self.type == 'MultiPath': if not self.data.hasTiming(): print("MultiPath timescale(): Path does not have timing") return et = self.data.endTime() for s in self.data.sections: s.times = [ofs + scale * (et - t) for t in s.times] s.times.reverse() s.configs.reverse() if s.velocities: s.velocities.reverse() self.data.sections.reverse() else: et = self.data.times[-1] self.data.times = [ ofs + scale * (et - t) for t in self.data.times ] self.data.times.reverse() self.data.milestones.reverse() if self.type == 'Configs': if scale != 1 or ofs != 0: self.type = 'Trajectory' else: if self.type == 'MultiPath': if not self.hasTiming(): print("MultiPath timescale(): Path does not have timing") return for s in self.data.sections: s.times = [ofs + scale * t for t in s.times] else: self.data.times = [ofs + scale * t for t in self.data.times] if self.type == 'Configs': if scale != 1 or ofs != 0: self.type = 'Trajectory' def toMultipath(self): if self.type == 'MultiPath': return else: traj = self.data mpath = MultiPath() mpath.sections.append(MultiPath.Section()) mpath.sections[0].times = traj.times mpath.sections[0].configs = traj.milestones self.data = mpath self.type = 'MultiPath' def toTrajectory(self): if self.type == 'Trajectory': return elif self.type == 'Configs': self.type = 'Trajectory' else: times = [] milestones = [] for i, s in enumerate(self.data.sections): if i == 0: if s.times is not None: times += s.times milestones += s.configs else: if s.times is not None: times += s.times[1:] milestones += s.configs[1:] if len(times) == 0: times = list(range(len(milestones))) else: assert len(times) == len(milestones) self.data = Trajectory(times, milestones) self.type = 'Trajectory' def toConfigs(self): if self.type == 'Trajectory': self.type = 'Configs' elif self.type == 'Configs': return else: self.toTrajectory() self.data.times = list(range(len(self.data.milestones))) self.type = 'Configs' def concat(self, other): """Warning: destructive""" if self.type == 'MultiPath': other.toMultipath() elif other.type == 'MultiPath': self.toMultipath() if self.type == 'MultiPath': self.data.concat(other.data) else: self.data.concat(other.data, relative=True) def discretize(self, dt): if self.type == 'MultiPath': print("Warning, MultiPath discretize is not supported yet") else: self.data = self.data.discretize(dt) def __str__(self): if self.type == 'MultiPath': ndofs = 0 if len(self.data.sections) > 0: ndofs = len(self.data.sections[0].configs[0]) res = "MultiPath with %d DOFs, %d sections\n" % ( ndofs, len(self.data.sections)) sstrs = [] for i, s in enumerate(self.data.sections): sres = " Section %d: %d milestones" % (i, len(s.configs)) if s.times != None: sres += " / times" if s.velocities != None: sres += " / velocities" sres += ", " + str(len(self.data.getStance(i))) + " holds" sstrs.append(sres) return res + '\n'.join(sstrs) else: ndofs = 0 if len(self.data.milestones) > 0: ndofs = len(self.data.milestones[0]) return '%s with %d DOFs, %d milestones' % ( self.type, ndofs, len(self.data.milestones))
if __name__=="__main__": import sys if len(sys.argv) != 4: print "config_to_driver_trajectory.py: Converts a trajectory file specified in a" print "robot's configuration space into a trajectory in the robot's driver (actuator)" print "space." print print "Usage: python config_to_driver_trajectory.py robot config_traj driver_traj" exit(0) robotfn = sys.argv[1] trajfn = sys.argv[2] outfn = sys.argv[3] #load the robot file world = klampt.WorldModel() #this makes it a little faster to load the robot -- you don't need the geometry world.enableGeometryLoading(False) robot = world.loadRobot(robotfn) if robot.getName()=='': print "Unable to load robot file",robotfn exit(1) traj = Trajectory() try: traj.load(trajfn) except IOError: print "Unable to load trajectory file",trajfn exit(1) outtraj = configToDriverTrajectory(robot,traj) print "Saving to",outfn,"..." outtraj.save(outfn)