res = ps.applyConstraints(q2)
if res[0]:
q2proj = res[1]
else:
raise RuntimeError("Failed to apply constraint.")
ps.selectPathOptimizer("None")
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(20):
ps.client.problem.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q1proj)
ps.addGoalConfig(q2proj)
t1 = dt.datetime.now()
ps.solve()
t2 = dt.datetime.now()
totalTime += t2 - t1
print(t2 - t1)
n = len(ps.client.problem.nodes())
totalNumberNodes += n
print("Number nodes: " + str(n))
print("Average time: " +
str((totalTime.seconds + 1e-6 * totalTime.microseconds) / 20.))
print("Average number nodes: " + str(totalNumberNodes / 20.))
pp = PathPlayer(cl, r)
pp(1)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
r (q_init)
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
r (q_goal)
ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathPlanner ("PRM")
ps.solve ()
from hpp_ros import PathPlayer
pp = PathPlayer (robot.client, r)
pp (0)
pp (1)
solver = ProblemSolver (robot)
print "diffusing planner (general RRT implementation)"
solver.selectPathPlanner("DiffusingPlanner")
solver.loadObstacleFromUrdf("hpp_ros","wall")
solver.setInitialConfig (q1)
solver.addGoalConfig (q2)
#pc.invokeIrreduciblePlanner()
#mpc = MPClient()
#pc = mpc.precomputation
#cnames = pc.getNumericalConstraints (robot.getInitialConfig())
#solver.setNumericalConstraints ("stability-constraints", cnames)
## add obstacles
print "solve"
start_time = float(time.time())
solver.solve ()
end_time = float(time.time())
seconds = end_time - start_time
hours = seconds/3600
print("--- %s seconds ---" % seconds)
print("--- %s hours ---" % hours)
print "replay path"
fname = "../data-traj/rrt-wall.tau"
pathplayer = PathPlayer (client, publisher)
pathplayer(1)
pathplayer.toFile(1,fname)
#/usr/bin/env python
import time
import sys
from hpp.corbaserver.motion_prior.client import Client as MPClient
#from hpp.corbaserver.wholebody_step.client import Client as WsClient
import rospy
from hrp2 import Robot
from hpp_ros import ScenePublisher, PathPlayer
robot = Robot ()
#robot.setTranslationBounds (-0.5, 0.5, -3, 3, 0, 1)
client = robot.client
publisher = ScenePublisher(robot)
pathplayer = PathPlayer (client, publisher)
if len(sys.argv) < 2:
print " 2014 LAAS CNRS "
print "---------------------------------------------"
print "usage: traj-replay.py <TRAJECTORY-FILE-NAME> <FILE2> ..."
sys.exit()
import re
for i in range(1,len(sys.argv)):
#m = re.search('([0-9]+)-([0-9]+)\.', sys.argv[i])
#minutes = str(int(m.group(2))*60/100)
#print minutes
#posText = [2,1,0]
#publisher.addText(">>Replaying Trajectory\nPlanning Time: "+m.group(1)+"h"+minutes+"m", posText)
#publisher.publishObjects()
#time.sleep(1)
#publisher.publishObjects()
#print q
#sys.exit(0)
qqinit = solver.getInitialConfig()[0:3]
qqgoal = solver.getGoalConfigs()[0][0:3]
print "planning from",qqinit,"to",qqgoal
#print "Irreducible Planner"
#solver.selectPathPlanner("PRM")
#solver.selectPathPlanner("DiffusingPlanner")
print "solve"
start_time = float(time.time())
solver.selectPathPlanner("IrreduciblePlanner")
solver.solve ()
end_time = float(time.time())
seconds = end_time - start_time
hours = seconds/3600
print("--- %s seconds ---" % seconds)
print("--- %s hours ---" % hours)
fname0 = "../data-traj/rrt-wall0.tau"
fname1 = "../data-traj/rrt-wall1.tau"
pathplayer = PathPlayer (robot.client, publisher)
pathplayer(0)
pathplayer(1)
pathplayer.toFile(0,fname0)
pathplayer.toFile(1,fname1)
print "path was written to file",fname