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)