def plugin_template(world):
"""Demonstrates the GLPluginInterface functionality"""
#create a subclass of GLPluginInterface
plugin = MyGLPlugin(world)
vis.pushPlugin(plugin) #put the plugin on top of the standard visualization functionality.
#vis.setPlugin(plugin) #use this to completely replace the standard visualization functionality with your own.
vis.add("world",world)
vis.setWindowTitle("GLPluginInterface template")
#run the visualizer
if not MULTITHREADED:
def callback(plugin=plugin):
if plugin.quit:
vis.show(False)
vis.loop(callback=callback,setup=vis.show)
else:
#if plugin.quit is True
vis.show()
while vis.shown() and not plugin.quit:
vis.lock()
#TODO: you may modify the world here
vis.unlock()
#changes to the visualization must be done outside the lock
time.sleep(0.01)
if plugin.quit:
#if you want to do other stuff after the window quits, the window needs to be hidden
vis.show(False)
print("Waiting for 2 s...")
time.sleep(2.0)
#quit the visualization thread nicely
vis.kill()
def Robot_Config_Plot(world,
DOF,
state_ref,
contact_link_dictionary,
convex_edges_list,
delta_t=0.5):
# This function is used to plot the robot motion
# The optimized solution is used to plot the robot motion and the contact forces
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
sim_robot = world.robot(0)
sim_robot.setConfig(state_ref[0:DOF])
contact_link_list = contact_link_dictionary.keys()
while vis.shown():
# This is the main plot program
vis.lock()
sim_robot.setConfig(state_ref[0:DOF])
# Convex_Edges_Plot(sim_robot, convex_edges_list, vis)
# Robot_COM_Plot(sim_robot, vis)
vis.unlock()
time.sleep(delta_t)
def PIP_Config_Plot(world, DOF, state_ref, pips_list, CPFlag, delta_t=0.5):
# This function is used to plot the robot motion
# The optimized solution is used to plot the robot motion and the contact forces
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
sim_robot = world.robot(0)
sim_robot.setConfig(state_ref[0:DOF])
InfeasiFlag = 0
while vis.shown():
# This is the main plot program
vis.lock()
sim_robot.setConfig(state_ref[0:DOF])
PIPs_Number = len(pips_list[0])
COM_Pos = sim_robot.getCom()
EdgeAList = pips_list[0]
EdgeBList = pips_list[1]
EdgeCOMList = pips_list[2]
EdgexList = pips_list[3]
EdgeyList = pips_list[4]
EdgezList = pips_list[5]
for i in range(0, PIPs_Number):
EdgeA = EdgeAList[i]
EdgeB = EdgeBList[i]
EdgeCOM = EdgeCOMList[i]
Edgey = EdgexList[i]
Edgez = EdgeyList[i]
Edgex = EdgezList[i]
PIP_Subplot(i, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez, COM_Pos,
vis)
Robot_COM_Plot(sim_robot, vis)
if CPFlag is 1:
try:
h = ConvexHull(EdgeAList)
except:
InfeasiFlag = 1
if InfeasiFlag is 0 and CPFlag is 1:
h = ConvexHull(EdgeAList)
hrender = draw_hull.PrettyHullRenderer(h)
vis.add("blah", h)
vis.setDrawFunc("blah", my_draw_hull)
vis.unlock()
time.sleep(delta_t)
def run_ex3():
world = WorldModel()
res = world.readFile("ex3_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.setWindowTitle("Pick and place test, use a/b/c/d to select target")
vis.pushPlugin(GLPickAndPlacePlugin(world))
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def Robot_Config_Plot(world, DOF, config_init):
robot_viewer = MyGLPlugin(world)
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
# Here we would like to read point cloud for visualization of planning.
# 1. All Reachable Points
# IdealReachableContacts_data = ContactDataLoader("IdealReachableContact")
# # 2. Active Reachable Points
# ReachableContacts_data = ContactDataLoader("ReachableContacts")
# # 3. Contact Free Points
# CollisionFreeContacts_data = ContactDataLoader("CollisionFreeContacts")
# # 4. Supportive Points
# SupportiveContacts_data = ContactDataLoader("SupportiveContacts")
OptimalContact_data = ContactDataLoader("OptimalContact")
OptimalContactWeights_data = ContactDataLoader("OptimalContactWeights")
OptimalContact_data, OptimalContactWeights_data = ContactDataRefine(
OptimalContact_data, OptimalContactWeights_data)
# # 6.
# TransitionPoints_data = ContactDataLoader("TransitionPoints")
# import ipdb; ipdb.set_trace()
# 7.
InitialTransitionPoints_data = ContactDataLoader("InitialPathWayPoints")
# 8.
ShiftedTransitionPoints_data = ContactDataLoader("ShiftedPathWayPoints")
# 9.
# FineShiftedPathWayPoints_data = ContactDataLoader("FineShiftedPathWayPoints")
#
# ReducedOptimalContact_data = ContactDataLoader("ReducedOptimalContact")
ContactChoice = ShiftedTransitionPoints_data
# ContactChoice = SupportiveContacts_data
SimRobot = world.robot(0)
SimRobot.setConfig(config_init)
while vis.shown():
# This is the main plot program
vis.lock()
SimRobot.setConfig(config_init)
WeightedContactDataPlot(vis, OptimalContact_data,
OptimalContactWeights_data)
ContactDataPlot(vis, ContactChoice)
vis.unlock()
time.sleep(0.1)
import ipdb
ipdb.set_trace()
WeightedContactDataUnPlot(vis, OptimalContact_data)
ContactDataUnplot(vis, ContactChoice)
def run_ex2():
world = WorldModel()
res = world.readFile("ex2_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.pushPlugin(GLTransferPlanPlugin(world))
vis.setWindowTitle(
"Transfer plan test, press r/f to plan with right/forward target")
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def run_ex1():
world = WorldModel()
res = world.readFile("ex1_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.setWindowTitle(
"Transit plan test, press l/r to plan with left/right arm")
vis.pushPlugin(GLTransitPlanPlugin(world))
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def main():
#creates a world and loads all the items on the command line
world = WorldModel()
res = world.readFile("./HRP2_Robot.xml")
if not res:
raise RuntimeError("Unable to load model")
robot = world.robot(0)
# coordinates.setWorldModel(world)
plugin = MyGLPlugin(world)
vis.pushPlugin(plugin)
#add the world to the visualizer
vis.add("world", world)
vis.add("robot", world.robot(0))
vis.show()
# ipdb.set_trace()
Robotstate_Traj, Contact_Force_Traj = Traj_Loader()
h = 0.0068 # 0.0043: vert
# 0.0033: flat
playspeed = 2.5
norm = 500
while vis.shown():
# This is the main plot program
for i in range(0, Robotstate_Traj.shape[1]):
vis.lock()
Robotstate_Traj_i = Robotstate_Traj[:, i]
Robotstate_Traj_Full_i = Dimension_Recovery(Robotstate_Traj_i)
# Now it is the plot of the contact force at the contact extremities
robot.setConfig(Robotstate_Traj_Full_i)
Contact_Force_Traj_i = Contact_Force_Traj[:, i]
left_ft, rght_ft, left_hd, rght_hd, left_ft_end, rght_ft_end, left_hd_end, rght_hd_end = Contact_Force_vec(
robot, Contact_Force_Traj_i, norm)
vis.add("left foot force",
Trajectory([0, 1], [left_ft, left_ft_end]))
vis.add("right foot force",
Trajectory([0, 1], [rght_ft, rght_ft_end]))
vis.add("left hand force",
Trajectory([0, 1], [left_hd, left_hd_end]))
vis.add("right hand force",
Trajectory([0, 1], [rght_hd, rght_hd_end]))
COMPos_start = robot.getCom()
COMPos_end = COMPos_start
COMPos_end[2] = COMPos_end[2] + 100
vis.add("Center of Mass",
Trajectory([0, 1], [COMPos_start, COMPos_end]))
# ipdb.set_trace()
vis.unlock()
time.sleep(h * playspeed)
def coordinates_template(world):
"""Tests integration with the coordinates module."""
#add the world to the visualizer
vis.add("world", world)
coordinates.setWorldModel(world)
#add the coordinate Manager to the visualizer
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("Coordinates visualiation test")
if MANUAL_EDITING:
#manually adds a poser, and adds a callback whenever the widget changes
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.pushPlugin(widgets)
if not MULTITHREADED:
vis.loop(callback=None, setup=vis.show)
else:
vis.show()
while vis.shown():
time.sleep(0.01)
else:
vis.edit(("world", world.robot(0).getName()))
if not MULTITHREADED:
def callback(coordinates=coordinates):
coordinates.updateFromWorld()
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#reads the coordinates from the world
coordinates.updateFromWorld()
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def Robot_Traj_Plot(world,
DOF,
state_traj,
contact_link_dictionary,
delta_t=0.5):
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
sim_robot = world.robot(0)
contact_link_list = contact_link_dictionary.keys()
while vis.shown():
# This is the main plot program
for config_i in state_traj:
vis.lock()
sim_robot.setConfig(config_i[0:DOF])
# Robot_COM_Plot(sim_robot, vis)
vis.unlock()
time.sleep(delta_t)
def mousefunc(self, button, state, x, y):
print("Mouse button", button, "state", state, "at point", x, y)
def motionfunc(self, x, y, dx, dy):
if 'shift' in self.modifiers():
self.q[2] = float(y) / 400
self.q[3] = float(x) / 400
self.world.robot(0).setConfig(self.q)
return True
return False
if __name__ == "__main__":
print("""================================================================
mouse_capture.py: A simple program where the mouse motion, when
shift-clicking, gets translated into joint values for an animated robot.
========================================================================
""")
world = klampt.WorldModel()
res = world.readFile("../../data/tx90blocks.xml")
if not res:
raise RuntimeError("Unable to load world")
plugin = MouseCapture(world)
vis.add("world", world)
vis.pushPlugin(plugin)
vis.setWindowTitle("mouse_capture.py")
vis.spin(float('inf')) #shows the window
vis.kill()
coordinates.setRobotModel(robot)
eenames = [robot.link(e).getName() for e in endeffectors]
eeobjectives = []
for e in endeffectors:
f = coordinates.frame(robot.link(e).getName())
fw = coordinates.addFrame(robot.link(e).getName()+"_tgt",robot.link(e).getTransform())
assert f != None
vis.add("ee_"+robot.link(e).getName(),fw)
vis.edit("ee_"+robot.link(e).getName())
obj = coordinates.ik_fixed_objective(f)
eeobjectives.append(obj)
#this tests the cartesian interpolation stuff
print "***** BEGINNING CARTESIAN INTERPOLATION TEST *****"
vis.setWindowTitle("Klamp't Cartesian interpolation test")
vis.pushPlugin(InterpKeyCapture(endeffectors,eeobjectives))
vis.show()
while vis.shown():
coordinates.updateFromWorld()
time.sleep(0.1)
vis.popPlugin()
vis.hide("ghost1")
vis.hide("ghost2")
vis.animate(("world",world.robot(0).getName()),None)
print
print
#this tests the "bump" function stuff
print "***** BEGINNING BUMP FUNCTION TEST *****"
configs = resource.get("cartesian_test"+world.robot(0).getName()+".configs",description="Make a reference trajectory for bump test",world=world)
if configs is None:
def Robot_Config_Plot(world, DOF, config_init):
robot_viewer = MyGLPlugin(world)
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
# Here we would like to read point cloud for visualization of planning.
# 1. All Reachable Points
# IdealReachableContacts_data = ContactDataLoader("IdealReachableContact")
# # 2. Active Reachable Points
# ReachableContacts_data = ContactDataLoader("ReachableContacts")
# # 3. Contact Free Points
# CollisionFreeContacts_data = ContactDataLoader("CollisionFreeContacts")
# # # 4. Supportive Points
# SupportiveContacts_data = ContactDataLoader("SupportiveContacts")
# OptimalContact_data = ContactDataLoader("OptimalContact")
# OptimalContactWeights_data = ContactDataLoader("OptimalContactWeights")
# OptimalContact_data, OptimalContactWeights_data = ContactDataRefine(OptimalContact_data, OptimalContactWeights_data)
# # 6.
# TransitionPoints_data = ContactDataLoader("TransitionPoints")
# import ipdb; ipdb.set_trace()
# 7.
# InitialTransitionPoints_data = ContactDataLoader("InitialPathWayPoints")
# AdjusterWayPoints_data = ContactDataLoader("AdjusterWayPoints")
# SegmentWayPoints_data = ContactDataLoader("SegmentWayPoints")
# # 8.
# ShiftedTransitionPoints_data = ContactDataLoader("ShiftedPathWayPoints")
#
# TwoPointLine_data = ContactDataLoader("TwoPointLine")
TestPathWayPoints_data = ContactDataLoader("FineShiftedPathWayPoints")
# 9.
# FineShiftedPathWayPoints_data = ContactDataLoader("FineShiftedPathWayPoints")
#
# ReducedOptimalContact_data = ContactDataLoader("ReducedOptimalContact")
ContactChoice = TestPathWayPoints_data
# ContactChoice = TwoPointLine_data
SimRobot = world.robot(0)
SimRobot.setConfig(config_init)
while vis.shown():
# This is the main plot program
vis.lock()
SimRobot.setConfig(config_init)
# WeightedContactDataPlot(vis, OptimalContact_data, OptimalContactWeights_data)
ContactDataPlot(vis, ContactChoice)
# for i in range(0, 31):
# BBName = "BB" + str(i)
# ConvexHull_data = ContactDataLoader(BBName)
# h = ConvexHull(ConvexHull_data)
# hrender = draw_hull.PrettyHullRenderer(h)
# vis.add("ContactPolytope" + str(i), h)
# vis.setDrawFunc("ContactPolytope" + str(i), my_draw_hull)
vis.unlock()
ipdb.set_trace()
time.sleep(0.1)
WeightedContactDataUnPlot(vis, OptimalContact_data)
ContactDataUnplot(vis, ContactChoice)
def Traj_Vis(world, DOF, robot_traj, PIP_traj, CPFlag, delta_t=0.5):
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
# ipdb.set_trace()
state_traj = robot_traj[0]
sim_robot = world.robot(0)
EdgeAList = PIP_traj[0]
EdgeBList = PIP_traj[1]
EdgeCOMList = PIP_traj[2]
EdgexList = PIP_traj[3]
EdgeyList = PIP_traj[4]
EdgezList = PIP_traj[5]
TotalTrajLength = len(state_traj)
EffectiveTrajLength = len(EdgeAList)
RedundantTrajLength = TotalTrajLength - EffectiveTrajLength
NumberOfConfigs = len(state_traj)
TransScale = 0.35
PVKRBcolor = [0, 0.4470, 0.7410]
PVKCPcolor = [0.6588, 0.6588, 0.1961]
PVKHJBcolor = [0.4940, 0.1840, 0.5560]
ZSCcolor = [0.3010, 0.7450, 0.9330]
OEcolor = [0.6350, 0.0780, 0.1840]
CPcolor = [0.25, 0.25, 0.25]
ZMPcolor = [0.75, 0, 0.75]
if CPFlag is 2:
state_traj = robot_traj[0]
PVKRB_traj = robot_traj[1]
PVKCP_traj = robot_traj[2]
PVKHJB_traj = robot_traj[3]
ZSC_traj = robot_traj[4]
OE_traj = robot_traj[5]
CP_traj = robot_traj[6]
ZMP_traj = robot_traj[7]
PVKRBFlag, PVKRBIndex = FailureIndicator(PVKRB_traj)
PVKCPFlag, PVKCPIndex = FailureIndicator(PVKCP_traj)
PVKHJBFlag, PVKHJBIndex = FailureIndicator(PVKHJB_traj)
ZSCFlag, ZSCIndex = FailureIndicator(ZSC_traj)
OEFlag, OEIndex = FailureIndicator(OE_traj)
CPFlag, CPIndex = FailureIndicator(CP_traj)
ZMPFlag, ZMPIndex = FailureIndicator(ZMP_traj)
Flags = [
PVKRBFlag, PVKCPFlag, PVKHJBFlag, ZSCFlag, OEFlag, CPFlag, ZMPFlag
]
Indices = [
PVKRBIndex, PVKCPIndex, PVKHJBIndex, ZSCIndex, OEIndex, CPIndex,
ZMPIndex
]
if PVKRBFlag is True:
GhostPose = state_traj[PVKRBIndex + RedundantTrajLength]
vis.add("PVKRBGhost", GhostPose)
vis.hide("PVKRBGhost")
vis.setColor("PVKRBGhost", PVKRBcolor[0], PVKRBcolor[1],
PVKRBcolor[2], TransScale)
if PVKCPFlag is True:
GhostPose = state_traj[PVKCPIndex + RedundantTrajLength]
vis.add("PVKCPGhost", GhostPose)
vis.hide("PVKCPGhost")
vis.setColor("PVKCPGhost", PVKCPcolor[0], PVKCPcolor[1],
PVKCPcolor[2], TransScale)
if PVKHJBFlag is True:
GhostPose = state_traj[PVKHJBIndex + RedundantTrajLength]
vis.add("PVKHJBGhost", GhostPose)
vis.hide("PVKHJBGhost")
vis.setColor("PVKHJBGhost", PVKHJBcolor[0], PVKHJBcolor[1],
PVKHJBcolor[2], TransScale)
if ZSCFlag is True:
GhostPose = state_traj[ZSCIndex + RedundantTrajLength]
vis.add("ZSCGhost", GhostPose)
vis.hide("ZSCGhost")
vis.setColor("ZSCGhost", ZSCcolor[0], ZSCcolor[1], ZSCcolor[2],
TransScale)
if OEFlag is True:
GhostPose = state_traj[OEIndex + RedundantTrajLength]
vis.add("OEGhost", GhostPose)
vis.hide("OEGhost")
vis.setColor("OEGhost", OEcolor[0], OEcolor[1], OEcolor[2],
TransScale)
if CPFlag is True:
GhostPose = state_traj[CPIndex + RedundantTrajLength]
vis.add("CPGhost", GhostPose)
vis.hide("CPGhost")
vis.setColor("CPGhost", CPcolor[0], CPcolor[1], CPcolor[2],
TransScale)
if ZMPFlag is True:
GhostPose = state_traj[ZMPIndex + RedundantTrajLength]
vis.add("ZMPGhost", GhostPose)
vis.hide("ZMPGhost")
vis.setColor("ZMPGhost", ZMPcolor[0], ZMPcolor[1], ZMPcolor[2],
TransScale)
while vis.shown():
# This is the main plot program
InfeasiFlag = 0
for i in range(0, EffectiveTrajLength):
vis.lock()
config_i = state_traj[i + RedundantTrajLength]
sim_robot.setConfig(config_i[0:DOF])
COM_Pos = sim_robot.getCom()
Robot_COM_Plot(sim_robot, vis)
EdgeAList_i = EdgeAList[i]
EdgeBList_i = EdgeBList[i]
EdgeCOMList_i = EdgeCOMList[i]
EdgexList_i = EdgexList[i]
EdgeyList_i = EdgeyList[i]
EdgezList_i = EdgezList[i]
for j in range(0, len(EdgeAList_i)):
EdgeA = EdgeAList_i[j]
EdgeB = EdgeBList_i[j]
EdgeCOM = EdgeCOMList_i[j]
Edgex = EdgexList_i[j]
Edgey = EdgeyList_i[j]
Edgez = EdgezList_i[j]
PIP_Subplot(j, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez,
COM_Pos, vis)
# # For the 7 fall indicators
# if (PVKRBFlag is True) and (i>=PVKRBIndex):
# vis.hide("PVKRBGhost", False)
# if (PVKCPFlag is True) and (i>=PVKCPIndex):
# vis.hide("PVKCPGhost", False)
# if (PVKHJBFlag is True) and (i>=PVKHJBIndex):
# vis.hide("PVKHJBGhost", False)
# if (ZSCFlag is True) and (i>=ZSCIndex):
# vis.hide("ZSCGhost", False)
# if (OEFlag is True) and (i>OEIndex):
# vis.hide("OEGhost", False)
# if (CPFlag is True) and (i>CPIndex):
# vis.hide("CPGhost", False)
# if (ZMPFlag is True) and (i>ZMPIndex):
# vis.hide("ZMPGhost", False)
# print Flags
# print Indices
if CPFlag is 1 or 2:
try:
h = ConvexHull(EdgeAList_i)
except:
InfeasiFlag = 1
if InfeasiFlag is 0:
h = ConvexHull(EdgeAList_i)
hrender = draw_hull.PrettyHullRenderer(h)
vis.add("blah", h)
vis.setDrawFunc("blah", my_draw_hull)
else:
print "Input Contact Polytope Infeasible!"
vis.unlock()
time.sleep(delta_t)
for j in range(0, len(EdgeAList_i)):
PIP_Remove(j, vis)
if ((CPFlag is 1 or 2) and (InfeasiFlag is 0)):
vis.remove("blah")
# if (PVKRBFlag is True):
# vis.hide("PVKRBGhost")
# if (PVKCPFlag is True):
# vis.hide("PVKCPGhost")
# if (PVKHJBFlag is True):
# vis.hide("PVKHJBGhost")
# if (ZSCFlag is True):
# vis.hide("ZSCGhost")
# if (OEFlag is True):
# vis.hide("OEGhost")
# if (CPFlag is True):
# vis.hide("CPGhost")
# if (ZMPFlag is True):
# vis.hide("ZMPGhost")
print "End"
def ExperimentVisualizer(world, ContactLinkDictionary, ExpTraj, PIPInfoList,
ImpulseObj, PlanningObj, SpecificPath, Para):
ExpViewer = MyGLPlugin(world)
vis.pushPlugin(ExpViewer)
vis.add("world", world)
vis.show()
SimRobot = world.robot(0)
FailureStateTraj = ExpTraj[0]
CtrlStateTraj = ExpTraj[1]
PlanStateTraj = ExpTraj[2]
EdgeAList = PIPInfoList[0]
EdgeBList = PIPInfoList[1]
EdgeCOMList = PIPInfoList[2]
EdgexList = PIPInfoList[3]
EdgeyList = PIPInfoList[4]
EdgezList = PIPInfoList[5]
EdgeVertexList = PIPInfoList[6]
TimeStep = CtrlStateTraj.times[1] - CtrlStateTraj.times[0]
PlotDuration = 10 * TimeStep
StateTrajLength = len(CtrlStateTraj.times)
PIPTrajLength = len(
EdgeAList) # Here PIPTraj could be less than the length of state
StateTraj = []
StateType = Para[0]
VisMode = Para[1]
if StateType == "F":
print "Failure State Traj!"
StateTraj = FailureStateTraj.milestones
elif StateType == "C":
print "Controlled State Traj!"
StateTraj = CtrlStateTraj.milestones
else:
print "Planned State Traj!"
StateTraj = PlanStateTraj.milestones
if (len(PlanningObj) == 0):
while vis.shown():
# This is the main plot program
for i in range(0, StateTrajLength):
SimulationTime = 1.0 * i * TimeStep
vis.lock()
Config = StateTraj[i]
SimRobot.setConfig(Config)
ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj)
vis.unlock()
time.sleep(1.0 * TimeStep)
return
else:
pass
PlanningTimeList = PlanningObj[0]
PlanningResList = PlanningObj[1]
SimulationTime = 0.0
while vis.shown():
# This is the main plot program
for i in range(0, StateTrajLength):
SimulationTime = 1.0 * i * TimeStep
vis.lock()
Config = StateTraj[i]
SimRobot.setConfig(Config)
ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj)
if VisMode == "PIP":
COMPos = SimRobot.getCom()
if (i >= (StateTrajLength - PIPTrajLength)):
EdgeIndex = i + PIPTrajLength - StateTrajLength
EdgeAList_i = EdgeAList[EdgeIndex]
EdgeBList_i = EdgeBList[EdgeIndex]
EdgeCOMList_i = EdgeCOMList[EdgeIndex]
EdgexList_i = EdgexList[EdgeIndex]
EdgeyList_i = EdgeyList[EdgeIndex]
EdgezList_i = EdgezList[EdgeIndex]
EdgeVertexList_i = EdgeVertexList[EdgeIndex]
for j in range(0, len(EdgeAList_i)):
EdgeA = EdgeAList_i[j]
EdgeB = EdgeBList_i[j]
EdgeCOM = EdgeCOMList_i[j]
Edgex = EdgexList_i[j]
Edgey = EdgeyList_i[j]
Edgez = EdgezList_i[j]
PIPVisualizer(j, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey,
Edgez, COMPos, vis)
elif VisMode == "Poly":
FeasiFlag = 1 # for j in range(0, len(EdgeAList_i)):
if (i >= (StateTrajLength - PIPTrajLength)):
EdgeIndex = i + PIPTrajLength - StateTrajLength
EdgeVertexList_i = EdgeVertexList[EdgeIndex]
try:
h = ConvexHull(EdgeVertexList_i)
except:
FeasiFlag = 0
if FeasiFlag == 1:
h = ConvexHull(EdgeVertexList_i)
hrender = draw_hull.PrettyHullRenderer(h)
vis.add("ContactPolytope", h)
vis.setDrawFunc("ContactPolytope", my_draw_hull)
else:
print "Input Contact Polytope Infeasible!"
else:
EndEffectorTrajPlot(vis, SimulationTime, PlanningObj,
SpecificPath, PlotDuration, TimeStep)
vis.unlock()
time.sleep(1.0 * TimeStep)
if VisMode == "PIP" and i >= (StateTrajLength - PIPTrajLength):
EdgeIndex = i + PIPTrajLength - StateTrajLength
for i in range(0, len(EdgeAList[EdgeIndex])):
PIPCleaner(i, vis)
if VisMode == "Traj":
PlanningPairList = PlanningObj[1]
StepLimbPair = PlanningPairList[len(PlanningPairList) - 1]
StepNo = StepLimbPair[0]
LimbNo = StepLimbPair[1]
for i in range(0, LimbNo + 1):
CandidateContacts_data = ContactDataLoader(
SpecificPath, StepNo, i, "CandidateContacts")
CandidateContactWeights_data = ContactDataLoader(
SpecificPath, StepNo, i, "CandidateContactWeights")
CandidateContacts_data, CandidateContactWeights_data = ContactDataRefine(
CandidateContacts_data, CandidateContactWeights_data)
WeightedContactDataUnPlot(vis, StepNo, i,
CandidateContacts_data)
TransitionDataUnplot(vis, StepNo, i)
if VisMode == "Poly":
vis.hide("ContactPolytope", True)
def Total_Robot_Motion_Plot(world, DOF, contact_link_dictionary, terr_model,
robot_option, grids, time_duration_list,
solution_list):
Contact_Force_Length = 0
for contact_link_index_i in contact_link_dictionary.keys():
contact_link_index_i_extremities = contact_link_dictionary[
contact_link_index_i]
for contact_link_index_i_extremity in contact_link_index_i_extremities:
Contact_Force_Length = Contact_Force_Length + 1
Contact_Force_List_Length = Contact_Force_Length * 3 # 3-dimensional force
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
robot_viewer_time = time.time()
# robot_sim = robot_viewer.sim
sim_robot = world.robot(0)
# sim_robot_controller = robot_sim.controller(0)
contact_link_list = contact_link_dictionary.keys()
robot_mass = Robot_Total_Mass(world)
robot_gravity = robot_mass * 9.81
force_unit_length = 0.5
interpolated_times = 5
# Segment number
Motion_Number = len(time_duration_list)
while vis.shown():
for index in range(0, Motion_Number):
time_duration_i = time_duration_list[index]
solution_i = solution_list[index]
optimized_solution_i = time_duration_i + solution_i
Duration, State_List_Array, Control_List_Array, Contact_Force_List_Array = Seed_Guess_Unzip(
optimized_solution_i, DOF, Contact_Force_List_Length, grids)
# Data interpolation
Inter_Time_Array, Inter_State_Array = Trajectory_Interpolation(
Duration, grids, State_List_Array, interpolated_times)
Inter_Time_Array, Inter_Contact_Force_Array = Trajectory_Interpolation(
Duration, grids, Contact_Force_List_Array, interpolated_times)
# This is the main plot program
for i in range(0, grids * interpolated_times):
vis.lock()
Robotstate_Traj_i = Inter_State_Array[i]
RobotConfig_Traj_i = Robotstate_Traj_i[0:DOF]
sim_robot.setConfig(RobotConfig_Traj_i)
# Now it is the plot of the contact force at the contact extremities
Contact_Force_Traj_i = Inter_Contact_Force_Array[i]
# Here Contact_Link_PosNVel_List is a list of dictionaries and the list is
Contact_Link_PosNVel_List = Contact_Link_PosNVel(
sim_robot, contact_link_dictionary, -1)
Contact_Force_Index = 0
for i in range(0, len(contact_link_list)):
Contact_Link_i_PosNVel = Contact_Link_PosNVel_List[i]
Contact_Link_i_Pos_List = Contact_Link_i_PosNVel["Pos"]
for j in range(0, len(Contact_Link_i_Pos_List)):
Contact_Link_i_Pos_j = Contact_Link_i_Pos_List[j]
Contact_Link_i_Pos_j_Contact_Force = Contact_Force_Element_from_Index(
Contact_Force_Traj_i, Contact_Force_Index)
contact_start_pos, contact_terminal_pos = Contact_Force_Mag_2_Vec(
Contact_Link_i_Pos_j,
Contact_Link_i_Pos_j_Contact_Force, robot_gravity,
force_unit_length)
force_string_name = " " * Contact_Force_Index
vis.add(
force_string_name,
Trajectory(
[0, 1],
[contact_start_pos, contact_terminal_pos]))
Contact_Force_Index = Contact_Force_Index + 1
vis.unlock()
time.sleep(interpolated_times *
(Inter_Time_Array[1] - Inter_Time_Array[0]))
smoothing=None,
zerotol=10.0,
vmax=robot.getVelocityLimits(),
amax=robot.getAccelerationLimits(),
speed=1.0,
)
print "*** Resulting duration", traj.endTime(), "***"
#vis.animate("robot",ltraj)
#vis.animate("robot",dtraj)
vis.animate("robot", traj)
vis.addText("label", "Hermite trajectory")
class MyVisPlugin(GLPluginInterface):
def __init__(self):
GLPluginInterface.__init__(self)
self.add_action(setHalfSpeed, "0.5x speed", '-')
self.add_action(setDoubleSpeed, "2x speed", '+')
self.add_action(setParabolic, "Unsmoothed, parabolic", 'p')
self.add_action(setMinJerk, "Start/stop minimum jerk", 'm')
self.add_action(setStartStop, "Start/stop trapezoidal", 't')
self.add_action(setCosine, "Start/stop cosine", 'c')
self.add_action(setHermite, "Hermite spline created manually", 'h')
vis.pushPlugin(MyVisPlugin())
vis.show()
while vis.shown():
time.sleep(0.1)
vis.kill()
def main(*arg):
Exp_Name = arg[0]
VisFlag = arg[1]
# The default robot to be loaded is the HRP2 robot.
Robot_Option = "../user/hrp2/"
world = WorldModel() # WorldModel is a pre-defined class
ipdb.set_trace()
if ".path" in Exp_Name:
# Then this means that we are given the robot trajectories for experimentations.
Real_Exp_Name = copy.deepcopy(Exp_Name)
Realer_Exp_Name = ""
for str_i in Real_Exp_Name:
if str_i == ".":
break
Realer_Exp_Name = Realer_Exp_Name + str_i
delta_t, DOF, robot_traj, PVKRB_traj, PVKCP_traj, PVKHJB_traj, ZSC_traj, OE_traj, CP_traj, ZMP_traj = Traj_Loader_fn(
Realer_Exp_Name)
# The next step is to load in robot's XML file
XML_path = ExpName + "Envi/Envi" + Realer_Exp_Name + ".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 " + fn)
Contact_Link_Dictionary = Contact_Link_Reader("ContactLink.txt",
ExpName + "User/")
Contact_Status_Dictionary = Contact_Status_Reader(
"InitContact.txt", ExpName + "User/")
# There are five Vis flag options:
"""
* VisFlag = 0 : Pure Animation
* VisFlag = 1 : Pure Animation with ePIPs and edges
* VisFlag = 2 : Pure Animation with eCH and ePIPs
* VisFlag = 3 : Pure Animation With eCH, ePIPs, and Ghosts
"""
if (VisFlag == 0):
Robot_Traj_Plot(world, DOF, robot_traj, Contact_Link_Dictionary,
delta_t)
else:
if (VisFlag == 1):
PIPList = PIP_Traj_Reader(Realer_Exp_Name)
CPFlag = 0
Traj_Vis(world, DOF, robot_traj, PIPList, CPFlag, delta_t)
else:
if VisFlag is 2:
PIPList = PIP_Traj_Reader(Realer_Exp_Name)
CPFlag = 1
Traj_Vis(world, DOF, robot_traj, PIPList, CPFlag, delta_t)
else:
if VisFlag is 3:
PIPList = PIP_Traj_Reader(Realer_Exp_Name)
CPFlag = 2
Traj_Vis(world, DOF, [
robot_traj, PVKRB_traj, PVKCP_traj, PVKHJB_traj,
ZSC_traj, OE_traj, CP_traj, ZMP_traj
], PIPList, CPFlag, delta_t)
else:
raise RuntimeError(
"Flag value not feasible for trajectory visualization!"
)
else:
# In this case, what we have is a config
# The following function can be used in two ways: the first way is to load the Config_Init.config file while the second way is to load two
DOF, Config_Init, Velocity_Init = State_Loader_fn(
Exp_Name + ".config", Robot_Option)
# DOF, Config_Init, Velocity_Init = State_Loader_fn("Init_Config.txt", "Init_Velocity.txt", Robot_Option)
# DOF, Config_Init, Velocity_Init = State_Loader_fn("Opt_Init_Config.txt", "Opt_Init_Velocity.txt", Robot_Option)
# State_Writer_fn(Config_Init, "Init_Config_from_txt.config", Robot_Option)
# State_Writer_fn(Config_Init, Velocity_Init, "Inn_Config.txt", "Inn_Velo.txt", Robot_Option)
# According to the initial condition of the robot contact status, a basic optimization may have to be contacted to enforce the initial constraints.
# Now it is the validation of the feasibility of the given initial condition
State_Init = Config_Init + Velocity_Init
Convex_Edges_List = Convex_Edge_Reader(Exp_Name + "CHEdges.txt",
Robot_Option)
delta_t = 0.5
if (PIP_Flag == 0):
Robot_Config_Plot(world, DOF, State_Init, Contact_Link_Dictionary,
Convex_Edges_List)
else:
if PIP_Flag is 1:
# In this case, we have to load in another sets of information for visualization
PIPList = PIP_Info_Reader(Exp_Name + "PIPs.txt", Robot_Option)
PIP_Config_Plot(world, DOF, State_Init, PIPList, 0)
else:
if PIP_Flag is 2:
PIPList = PIP_Info_Reader(Exp_Name + "PIPs.txt",
Robot_Option)
PIP_Config_Plot(world, DOF, State_Init, PIPList, 1)
else:
if PIP_Flag is 3:
# This function is specific for the four visualization of robot contact polytope
# Four plots should be shown alternatively
# 1. Contact Polytope with Edges
# 2. Contact Polytope with PIPsExpName
# 3. Contact Polytope with EPIPs
# 4. E Contact Polytope with EPIPs
Convex_Edges_List = Convex_Edge_Reader(
Exp_Name + "CHEdges.txt", Robot_Option)
ContactVerticies = ContactVerticesGenerator(
Convex_Edges_List)
Intersection_List = Intersection_Reader(
Exp_Name + "Intersections.txt", Robot_Option)
PIPList = PIP_Info_Reader(Exp_Name + "PIPs.txt",
Robot_Option)
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
sim_robot = world.robot(0)
sim_robot.setConfig(State_Init[0:DOF])
COM_Pos = sim_robot.getCom()
# t_orbital = ...
# q_orbital = ...
# vis.add("q_orbital",q_orbital)
# vis.hide("q_orbital")
# vis.setColor("q_orbital",1,0,0,0.5)
while vis.shown():
# This is the main plot program
vis.lock()
# sim_robot.setConfig(Config_Init[0:DOF])
# if vis.animationTime() < t_orbital:
# vis.hide("q_orbital")
# else:
# vis.hide("q_orbital",False)
if robot_viewer.mode_no == 1:
# Here we plot the full contact polytope
h = ConvexHull(ContactVerticies)
hrender = draw_hull.PrettyHullRenderer(h)
vis.add("blah", h)
vis.setDrawFunc("blah", my_draw_hull)
if robot_viewer.mode_no == 2:
h = ConvexHull(ContactVerticies)
hrender = draw_hull.PrettyHullRenderer(h)
vis.add("blah", h)
vis.setDrawFunc("blah", my_draw_hull)
# Also edges
Convex_Edges_Plot(sim_robot, Convex_Edges_List,
vis)
# Then intersection from COM
for i in range(0, len(Intersection_List)):
COM2IntersectionPlot(
i, vis, COM_Pos, Intersection_List[i])
if robot_viewer.mode_no == 3:
h = ConvexHull(ContactVerticies)
hrender = draw_hull.PrettyHullRenderer(h)
vis.add("blah", h)
vis.setDrawFunc("blah", my_draw_hull)
# Also edges
Convex_Edges_Plot(sim_robot, Convex_Edges_List,
vis)
PIPs_Number = len(PIPList[0])
EdgeAList = PIPList[0]
EdgeBList = PIPList[1]
EdgeCOMList = PIPList[2]
EdgexList = PIPList[3]
EdgeyList = PIPList[4]
EdgezList = PIPList[5]
for i in range(0, PIPs_Number):
EdgeA = EdgeAList[i]
EdgeB = EdgeBList[i]
EdgeCOM = EdgeCOMList[i]
Edgey = EdgexList[i]
Edgez = EdgeyList[i]
Edgex = EdgezList[i]
PIP_Subplot(i, EdgeA, EdgeB, EdgeCOM,
Edgex, Edgey, Edgez, COM_Pos,
vis)
if robot_viewer.mode_no == 4:
# Effective Polytope with Effective PIPs
PIPs_Number = len(PIPList[0])
EdgeAList = PIPList[0]
# EdgeBList = PIPList[1]
# for i in EdgeBList:
# EdgeAList.append(i);
h = ConvexHull(EdgeAList)
hrender = draw_hull.PrettyHullRenderer(h)
vis.add("blah", h)
vis.setDrawFunc("blah", my_draw_hull)
EdgeAList = PIPList[0]
EdgeBList = PIPList[1]
EdgeCOMList = PIPList[2]
EdgexList = PIPList[3]
EdgeyList = PIPList[4]
EdgezList = PIPList[5]
for i in range(0, PIPs_Number):
EdgeA = EdgeAList[i]
EdgeB = EdgeBList[i]
EdgeCOM = EdgeCOMList[i]
Edgey = EdgexList[i]
Edgez = EdgeyList[i]
Edgex = EdgezList[i]
PIP_Subplot(i, EdgeA, EdgeB, EdgeCOM,
Edgex, Edgey, Edgez, COM_Pos,
vis)
vis.unlock()
print "mode number: " + str(robot_viewer.mode_no)
time.sleep(delta_t)
# Robot_COM_Plot(sim_robot, vis)
# Now we should delete the imposed object
if robot_viewer.mode_no == 1:
vis.remove("blah")
if robot_viewer.mode_no == 2:
vis.remove("blah")
EdgeNo = len(Convex_Edges_List) / 2
for i in range(0, EdgeNo):
Edge_Index = str(i)
vis.remove("Edge:" + Edge_Index)
for i in range(0, len(Intersection_List)):
Edge_Index = str(i)
vis.remove("PIPEdgefromCOM:" + Edge_Index)
if robot_viewer.mode_no == 3:
vis.remove("blah")
EdgeNo = len(Convex_Edges_List) / 2
for i in range(0, EdgeNo):
Edge_Index = str(i)
vis.remove("Edge:" + Edge_Index)
EdgeAList = PIPList[0]
for i in range(0, len(EdgeAList)):
Edge_Index = str(i)
vis.remove("PIPEExpNamedge:" + Edge_Index)
vis.remove("PIPEdgeCOM:" + Edge_Index)
vis.remove("PIPEdgex:" + Edge_Index)
vis.remove("PIPEdgey:" + Edge_Index)
vis.remove("PIPEdgez:" + Edge_Index)
if robot_viewer.mode_no == 4:
vis.remove("blah")
EdgeAList = PIPList[0]
for i in range(0, len(EdgeAList)):
Edge_Index = str(i)
vis.remove("PIPEdge:" + Edge_Index)
vis.remove("PIPEdgeCOM:" + Edge_Index)
vis.remove("PIPEdgex:" + Edge_Index)
vis.remove("PIPEdgey:" + Edge_Index)
vis.remove("PIPEdgez:" + Edge_Index)
robot_viewer.mode_no = robot_viewer.mode_no + 1
if robot_viewer.mode_no == 5:
robot_viewer.mode_no = 1
robot_viewer.mode_no = 3
glPointSize(5.0)
glColor3f(0, 0, 0)
glBegin(GL_POINTS)
for pt in self.pc:
if len(pt) == 6:
glColor3f(*pt[3:6])
if read_local:
glVertex3fv(se3.apply(T, pt[0:3]))
else:
glVertex3fv(pt[0:3])
glEnd()
#print "Draw PC time",time.time()-t0
return True
vis.pushPlugin(SensorTestWorld())
vis.spin(float('inf'))
vis.kill()
"""
#Note: GLEW doesn't work outside of the main thread, hence the use of the GLPluginInterface.
#The below code falls back to the non-accelerated sensor simulation
vis.show()
time.sleep(0.5)
for sample in range(10):
vis.lock()
robot.randomizeConfig()
#sensor.kinematicSimulate(world,0.01)
sim.controller(0).setPIDCommand(robot.getConfig(),[0.0]*7)
vis.unlock()
def Single_Robot_Motion_Plot(world, DOF, control_force_len,
contact_link_dictionary, terr_model, robot_option,
grids, optimized_solution):
# This function is used to plot the robot motion
# The optimized solution is used to plot the robot motion and the contact forces
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
Duration, State_List_Array, Control_List_Array, Contact_Force_List_Array = Seed_Guess_Unzip(
optimized_solution, DOF, control_force_len, grids)
# Data interpolation
interpolated_times = 5
Inter_Time_Array, Inter_State_Array = Trajectory_Interpolation(
Duration, grids, State_List_Array, interpolated_times)
Inter_Time_Array, Inter_Contact_Force_Array = Trajectory_Interpolation(
Duration, grids, Contact_Force_List_Array, interpolated_times)
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
robot_viewer_time = time.time()
# robot_sim = robot_viewer.sim
sim_robot = world.robot(0)
# sim_robot_controller = robot_sim.controller(0)
contact_link_list = contact_link_dictionary.keys()
robot_mass = Robot_Total_Mass(world)
robot_gravity = robot_mass * 9.81
force_unit_length = 0.5
while vis.shown():
# This is the main plot program
for i in range(0, grids * interpolated_times):
vis.lock()
Robotstate_Traj_i = Inter_State_Array[i]
RobotConfig_Traj_i = Robotstate_Traj_i[0:DOF]
sim_robot.setConfig(RobotConfig_Traj_i)
# Now it is the plot of the contact force at the contact extremities
Contact_Force_Traj_i = Inter_Contact_Force_Array[i]
# Here Contact_Link_PosNVel_List is a list of dictionaries and the list is
Contact_Link_PosNVel_List = Contact_Link_PosNVel(
sim_robot, contact_link_dictionary, -1)
Contact_Force_Index = 0
for i in range(0, len(contact_link_list)):
Contact_Link_i_PosNVel = Contact_Link_PosNVel_List[i]
Contact_Link_i_Pos_List = Contact_Link_i_PosNVel["Pos"]
for j in range(0, len(Contact_Link_i_Pos_List)):
Contact_Link_i_Pos_j = Contact_Link_i_Pos_List[j]
Contact_Link_i_Pos_j_Contact_Force = Contact_Force_Element_from_Index(
Contact_Force_Traj_i, Contact_Force_Index)
contact_start_pos, contact_terminal_pos = Contact_Force_Mag_2_Vec(
Contact_Link_i_Pos_j,
Contact_Link_i_Pos_j_Contact_Force, robot_gravity,
force_unit_length)
force_string_name = " " * Contact_Force_Index
vis.add(
force_string_name,
Trajectory([0, 1],
[contact_start_pos, contact_terminal_pos]))
Contact_Force_Index = Contact_Force_Index + 1
vis.unlock()
time.sleep(interpolated_times *
(Inter_Time_Array[1] - Inter_Time_Array[0]))
