if not res[0]:
raise Exception('Goal configuration could not be projected.')
q_goal = res[1]
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(args.N):
ps.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
t1 = dt.datetime.now()
ps.solve()
t2 = dt.datetime.now()
totalTime += t2 - t1
print(t2 - t1)
n = ps.numberNodes()
totalNumberNodes += n
print("Number nodes: " + str(n))
print("Average time: " +
str((totalTime.seconds + 1e-6 * totalTime.microseconds) / float(args.N)))
print("Average number nodes: " + str(totalNumberNodes / float(args.N)))
if args.display:
v = vf.createViewer()
pp = PathPlayer(v, robot.client.basic)
if args.run:
pp(0)
},
lLegId: {
'file': "hrp2/LL_com.ineq",
'effector': 'LLEG_JOINT5'
},
rarmId: {
'file': "hrp2/RA_com.ineq",
'effector': rHand
}
}
#~ larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean, stats, saveAllData, play_traj
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
def act(i,
numOptim=0,
use_window=0,
friction=0.5,
optim_effectors=True,
verbose=False,
draw=False):
return step(fullBody,
configs,
i,
numOptim,
pp,
limbsCOMConstraints,
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
#ps.selectSteeringMethod("RBPRMKinodynamic")
#ps.selectDistance("Kinodynamic")
#ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t = ps.solve ()
print "Guide planning time : ",t
try :
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayPath(0)#pp.displayVelocityPath(0) #
#v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(0)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
r = Viewer(ps)
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [-1, -0.82, 0.5]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal = q_init[::]
q_goal[3:7] = [0., 0., 0.14943813, 0.98877108]
q_goal[0:3] = [1.49, -0.65, 1.2]
r(q_goal)
ps.addPathOptimizer("RandomShortcut")
#~ ps.setInitialConfig (q_init)
#~ ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
#~ afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r)
afftool.loadObstacleModel(packageName, "bauzil_stairs_10cm", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
ps.client.problem.selectConfigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
#~ t = ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer(r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation", 0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t = ps.solve()
print "Guide planning time : ", t
#pId = ps.numberPaths()-1
pId = 0
# display solution :
try:
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.1
pp.displayVelocityPath(pId)
v.client.gui.setVisibility("path_0_root", "ALWAYS_ON_TOP")
pp.dt = 0.01
pp(pId)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
robot = Robot ('puzzle') # object5
robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1., 1.])
#robot.setJointBounds('base_joint_xyz', [-0.6, 0.6, -0.6, 0.6, -0.3, 1.0])
ps = ProblemSolver (robot)
cl = robot.client
q1 = [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#q1 = [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] # simpler
#q1 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0] # simpler2
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
r.loadObstacleModel ("puzzle_description","decor_easy","decor_easy")
r(q2) # r(q1)
#q1bis = q2; q2bis = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#ps.resetGoalConfigs (); ps.setInitialConfig (q1bis); ps.addGoalConfig (q2bis); ps.solve ()
# ps.resetGoalConfigs (); ps.setInitialConfig (q1); ps.addGoalConfig (q2bis); ps.solve ()
#i = ps.numberPaths()-1
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/puzzle_veryEasy_PRM.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_RRT.rdm')
ps.selectPathPlanner("DynamicPlanner")
r(q_init)
ps.client.problem.prepareSolveStepByStep()
ps.client.problem.finishSolveStepByStep()
#i = 0
#r.displayRoadmap("rm"+str(i),0.02)
#ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
#t = ps.solve ()
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 1. / 30.
#r.client.gui.removeFromGroup("rm0",r.sceneName)
pp.displayVelocityPath(0)
pp.speed = 0.2
pp(0)
"""
#r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmPath",r.sceneName)
r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
math.sqrt((np.linalg.norm(u)*np.linalg.norm(u)) * (np.linalg.norm(v)*np.linalg.norm(v))
# You can use the colors defined in viewer.color or give an array of 4 float which define a normalized RGBA color
v.displayRoadmap("rmR",0.02,1,v.color.blue,v.color.lightBlue,'r_gripper_tool_joint')
v.displayRoadmap("rmL",0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint')
# hide roadmap in the scene
#v.client.gui.removeFromGroup("rmL",v.sceneName)
#v.client.gui.removeFromGroup("rmR",v.sceneName)
# alternative method : replace ps.solve() and v.displayRoadmap() with :
# v.solveAndDisplay("rmL1",2,0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint')
# v.displayRoadmap( "rmR1" ,0.02,1,v.color.blue ,v.color.lightBlue ,'r_gripper_tool_joint')
# solveAndDisplay show the construction of the roadmap, the first int set the refresh rate of the display (here, every 2 iteration of the algorithm)
################################################################
pp = PathPlayer (robot.client, v)
#display path
# The edge of the roadmap are always straight line, if you want to display the accurate trajectory of a body of the robot, you should use the following method :
# displayPath(pathId, color(default = green), jointName (default = root)
pp.displayPath(0 , v.color.lightBlack ,'r_gripper_tool_joint')
pp (0)
#display path after optimisation
pp.displayPath(1,v.color.lightRed,'r_gripper_tool_joint')
pp (1)
def generateConstrainedBezierTraj(cfg,
time_interval,
placement_init,
placement_end,
numTry,
q_t=None,
phase_previous=None,
phase=None,
phase_next=None,
fullBody=None,
eeName=None,
viewer=None):
if numTry == 0:
return generateSmoothBezierTraj(cfg, time_interval, placement_init,
placement_end)
else:
if not q_t or not phase_previous or not phase or not phase_next or not fullBody or not eeName:
raise ValueError(
"Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments"
)
predef_curves = generatePredefBeziers(cfg, time_interval, placement_init,
placement_end)
bezier_takeoff = predef_curves.curves[predef_curves.idFirstNonZero()]
bezier_landing = predef_curves.curves[predef_curves.idLastNonZero()]
id_middle = int(math.floor(len(predef_curves.curves) / 2.))
pos_init = bezier_takeoff(bezier_takeoff.max())
pos_end = bezier_landing(0)
if VERBOSE:
print("bezier takeoff end : ", pos_init)
print("bezier landing init : ", pos_end)
t_begin = predef_curves.times[id_middle]
t_middle = predef_curves.curves[id_middle].max()
t_end = t_begin + t_middle
if VERBOSE:
print("t begin : ", t_begin)
print("t end : ", t_end)
q_init = q_t[:, int(t_begin / cfg.IK_dt)] # after the predef takeoff
q_end = q_t[:, int(t_end / cfg.IK_dt)]
# compute limb-rrt path :
pathId = limb_rrt.generateLimbRRTPath(q_init, q_end, phase_previous, phase,
phase_next, fullBody)
if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT:
from hpp.gepetto import PathPlayer
pp = PathPlayer(viewer)
if DISPLAY_JOINT_LEVEL:
pp.displayPath(pathId, jointName=fullBody.getLinkNames(eeName)[0])
else:
#TODO
pp.displayPath(
pathId,
jointName=fullBody.getLinkNames(eeName)[0],
offset=cfg.Robot.dict_offset[eeName].translation.tolist())
# compute constraints for the end effector trajectories :
pData = bezier_com.ProblemData()
pData.c0_ = bezier_takeoff(bezier_takeoff.max())
pData.dc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 1)
pData.ddc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 2)
pData.j0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 3)
pData.c1_ = bezier_landing(0)
pData.dc1_ = bezier_landing.derivate(0, 1)
pData.ddc1_ = bezier_landing.derivate(0, 2)
pData.j1_ = bezier_landing.derivate(0, 3)
pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK
# now compute additional inequalities around the rrt path :
pDef = computeProblemConstraints(pData, fullBody, pathId, t_middle, eeName,
viewer)
solved = False
# loop and increase the number of variable control point until a solution is found
flagData = pData.constraints_.flag_
flags = [
bezier_com.ConstraintFlag.ONE_FREE_VAR, None,
bezier_com.ConstraintFlag.THREE_FREE_VAR, None,
bezier_com.ConstraintFlag.FIVE_FREE_VAR
]
numVars = 1
while not solved and numVars <= 5:
pData.constraints_.flag_ = flagData | flags[numVars - 1]
ineqEff = bezier_com.computeEndEffectorConstraints(pData, t_middle)
Hg = bezier_com.computeEndEffectorVelocityCost(pData, t_middle)
res = bezier_com.computeEndEffector(
pData, t_middle) #only used for comparison/debug ?
bezier_unconstrained = res.c_of_t
pDef.degree = bezier_unconstrained.degree
ineqConstraints = curves.generate_problem(pDef)
# _ prefix = previous notation (in bezier_com_traj)
# min x' H x + 2 g' x
# subject to A*x <= b
_A = np.vstack([ineqEff.A, ineqConstraints.A])
_b = np.vstack([ineqEff.b, ineqConstraints.b])
_H = Hg.A
_g = Hg.b
#_H = ineqConstraints.cost.A
#_g = ineqConstraints.cost.b
# quadprog notation :
#min (1/2)x' P x + q' x
#subject to G x <= h
#subject to C x = d
G = _A
h = _b.reshape((-1))
P = _H * 2.
q = (_g * 2.).flatten()
try:
if VERBOSE:
print("try to solve quadprog with " + str(numVars) +
" variable")
res = quadprog_solve_qp(P, q, G, h)
solved = True
except ValueError:
if VERBOSE:
print("Failed.")
numVars += 2
if solved:
if VERBOSE:
print("Succeed.")
else:
print(
"Constrained End effector Bezier method failed for all numbers of variables control points."
)
print("Return predef trajectory (may be in collision).")
return
# retrieve the result of quadprog and create a bezier curve :
vars = np.split(res, numVars)
wps = bezier_unconstrained.waypoints()
id_firstVar = 4
i = id_firstVar
for x in vars:
wps[:, i] = x
i += 1
bezier_middle = curves.bezier(wps, 0., t_middle)
# create concatenation with takeoff/landing
curves = predef_curves.curves[::]
curves[id_middle] = bezier_middle
pBezier = PolyBezier(curves)
if VERBOSE:
print("time interval = ", time_interval[1] - time_interval[0])
print("polybezier length = ", pBezier.max())
ref_traj = trajectories.BezierTrajectory(pBezier, placement_init,
placement_end, time_interval)
return ref_traj
ps.addPathOptimizer("RandomShortcutDynamic")
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
t = ps.solve()
print("Guide planning time : ", t)
#v.solveAndDisplay("rm",10,0.01)
for i in range(20):
print("Optimize path, " + str(i + 1) + "/20 ... ")
ps.optimizePath(ps.numberPaths() - 1)
pathId = ps.numberPaths() - 1
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.01
pp.displayPath(pathId)
#v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(pathId)
#v.client.gui.writeNodeFile("path_"+str(pathId)+"_root","guide_path_maze_hard.obj")
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
ps.addGoalConfig (q_goal)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer ("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t = ps.solve ()
print "Guide planning time : ",t
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
#pp.displayVelocityPath(0)
v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(0)
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
t = ps.solve()
#r.displayRoadmap('rm',radiusSphere=0.01)
#r.displayPathMap("pm",0)
#tf=r.solveAndDisplay("rm",1,0.01)
#t = [0,0,tf,0]
#r.client.gui.removeFromGroup("rm_group",r.sceneName)
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(ps.numberPaths() - 1)
#r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
#display path
pp.speed = 0.5
#pp (0)
import parse_bench
parse_bench.parseBenchmark(t)
print "path lenght = ", str(ps.client.problem.pathLength(ps.numberPaths() - 1))
###########################
#display path with post-optimisation
"""
#ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
#t = ps.solve ()
r.solveAndDisplay("rm",1,0.02)
#t = ps.solve ()
#r.displayRoadmap("rm",0.02)
r.displayPathMap("rmPath",0,0.02)
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.displayPath(0,r.color.lightGreen)
pp(0)
pp.displayPath(1,blue)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.displayPath(1,black)
pp (1)
#r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmPath",r.sceneName)
r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
afftool.loadObstacleModel(packageName, "plane_hole", "planning", r)
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
# Solve the problem
t = ps.solve()
if isinstance(t, list):
t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
q_far = q_init[::]
q_far[0:3] = [-2, -3, 0.63]
r(q_far)
for i in range(1, 10):
rbprmBuilder.client.basic.problem.optimizePath(i)
from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent
class Robot(Parent):
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
def generateLimbRRTOptimizedTraj(time_interval,
placement_init,
placement_end,
numTry,
q_t=None,
phase_previous=None,
phase=None,
phase_next=None,
fullBody=None,
eeName=None,
viewer=None):
if numTry == 0:
return generateSmoothBezierTraj(time_interval, placement_init,
placement_end)
else:
if q_t is None or phase_previous is None or phase is None or phase_next is None or not fullBody or not eeName:
raise ValueError(
"Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments"
)
if cfg.EFF_T_PREDEF > 0:
predef_curves = generatePredefBeziers(time_interval, placement_init,
placement_end)
else:
predef_curves = generateSmoothBezierTraj(time_interval, placement_init,
placement_end).curves
id_middle = int(math.floor(len(predef_curves.curves) / 2.))
predef_middle = predef_curves.curves[id_middle]
pos_init = predef_middle(0)
pos_end = predef_middle(predef_middle.max())
if VERBOSE:
print "generateLimbRRTOptimizedTraj, try number " + str(numTry)
print "bezier takeoff end : ", pos_init
print "bezier landing init : ", pos_end
t_begin = predef_curves.times[id_middle]
t_middle = predef_middle.max()
t_end = t_begin + t_middle
if VERBOSE:
print "t begin : ", t_begin
print "t end : ", t_end
q_init = q_t[:, int(math.floor(t_begin /
cfg.IK_dt))] # after the predef takeoff
id_end = int(math.ceil(t_end / cfg.IK_dt)) - 1
if id_end >= q_t.shape[
1]: # FIXME : why does it happen ? usually it's == to the size when the bug occur
id_end = q_t.shape[1] - 1
q_end = q_t[:, id_end]
global current_limbRRT_id
# compute new limb-rrt path if needed:
if not current_limbRRT_id or (numTry in recompute_rrt_at_tries):
current_limbRRT_id = generateLimbRRTPath(q_init, q_end, phase_previous,
phase, phase_next, fullBody)
if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT_PATH:
from hpp.gepetto import PathPlayer
pp = PathPlayer(viewer)
pp.displayPath(
current_limbRRT_id,
jointName=fullBody.getLinkNames(eeName)[0],
offset=cfg.Robot.dict_offset[eeName].translation.T.tolist()[0])
# find weight and number of variable to use from the numTry :
for offset in reversed(recompute_rrt_at_tries):
if numTry >= offset:
id = numTry - offset
break
if VERBOSE:
print "weights_var id = ", id
if id >= len(weights_vars):
raise ValueError(
"Max number of try allow to find a collision-end effector trajectory reached."
)
weight = weights_vars[id][0]
varFlag = weights_vars[id][1]
numVars = weights_vars[id][2]
if VERBOSE:
print "use weight " + str(weight) + " with num free var = " + str(
numVars)
# compute constraints for the end effector trajectories :
pData = bezier_com.ProblemData()
pData.c0_ = predef_middle(0)
pData.dc0_ = predef_middle.derivate(0, 1)
pData.ddc0_ = predef_middle.derivate(0, 2)
pData.j0_ = predef_middle.derivate(0, 3)
pData.c1_ = predef_middle(predef_middle.max())
pData.dc1_ = predef_middle.derivate(predef_middle.max(), 1)
pData.ddc1_ = predef_middle.derivate(predef_middle.max(), 2)
pData.j1_ = predef_middle.derivate(predef_middle.max(), 3)
pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK | varFlag
Constraints = bezier_com.computeEndEffectorConstraints(pData, t_middle)
Cost_smooth = bezier_com.computeEndEffectorVelocityCost(pData, t_middle)
Cost_distance = computeDistanceCostMatrices(fullBody, current_limbRRT_id,
pData, t_middle, eeName)
# formulate QP matrices :
# _ prefix = previous notation (in bezier_com_traj)
# min x' H x + 2 g' x
# subject to A*x <= b
_A = Constraints.A
_b = Constraints.b
_H = ((1. - weight) * Cost_smooth.A + weight * Cost_distance.A)
_g = ((1. - weight) * Cost_smooth.b + weight * Cost_distance.b)
if VERBOSE > 1:
print "A = ", _A
print "b = ", _b
print "H = ", _H
print "h = ", _g
"""
_A = np.array(_A)
_b = np.array(_b)
_H = np.array(_H)
_g = np.array(_g)
"""
# quadprog notation :
#min (1/2)x' P x + q' x
#subject to G x <= h
#subject to C x = d
G = _A
h = _b.flatten().T # remove the transpose when working with array
P = _H * 2.
q = (_g * 2.).flatten().T
if VERBOSE > 1:
print "G = ", G
print "h = ", h
print "P = ", P
print "q = ", q
print "Shapes : "
print "G : ", G.shape
print "h : ", h.shape
print "P : ", P.shape
print "q : ", q.shape
# solve the QP :
solved = False
try:
res = quadprog_solve_qp(P, q, G, h)
solved = True
except ValueError, e:
print "Quadprog error : "
print e.message
raise ValueError(
"Quadprog failed to solve QP for optimized limb-RRT end-effector trajectory, for try number "
+ str(numTry))
# Script which goes with gravity.launch, to simulate Hrp2 in the space with a spaceship from a movie and an emu character from Nasa.gov. See gravity_description package.
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
#Robot.urdfSuffix = '_capsule'
#Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
#robot.setJointBounds('base_joint_xyz', [-5, 10, -10, 10, -5, 5])
robot.setJointBounds('base_joint_xyz', [-3, 10, -4, 4, -3, 5])
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
pp = PathPlayer (cl, r)
#r.loadObstacleModel ("gravity_description","emu","emu")
r.loadObstacleModel ("gravity_description","gravity_decor","gravity_decor")
# Difficult init config
q1 = [1.45, 1.05, -0.8, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.0]
q2 = [6.55, -2.91, 1.605, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
q2hard = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0]
robot.isConfigValid(q1)
robot.isConfigValid(q2)
# qf should be invalid
qf = [1, -3, 3, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
#~ V0list = tp.V0list
#~ Vimplist = tp.Vimplist
base_joint_xyz_limits = tp.base_joint_xyz_limits
fullBody = FullBody()
robot = fullBody.client.basic.robot
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", base_joint_xyz_limits)
#psf = ProblemSolver(fullBody); rr = Viewer (psf); gui = rr.client.gui
ps = path_planner.ProblemSolver(fullBody)
r = path_planner.Viewer(ps, viewerClient=path_planner.r.client)
rr = r
#~ psf = tp.ProblemSolver( fullBody ); rr = tp.Viewer (psf); gui = rr.client.gui
pp = PathPlayer(fullBody.client.basic, rr)
pp.speed = 0.6
q_0 = fullBody.getCurrentConfig()
rr(q_0)
rLegId = 'RFoot'
lLegId = 'LFoot'
rarmId = 'RHand'
larmId = 'LHand'
rfoot = 'SpidermanRFootSphere'
lfoot = 'SpidermanLFootSphere'
lHand = 'SpidermanLHandSphere'
rHand = 'SpidermanRHandSphere'
nbSamples = 50000
x = 0.03
y = 0.08
success1, q_touch_3, residual_error1 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_front", q_init) # Collision between legs
success2, q_touch_4, residual_error2 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_drawer", q_init) # fail
# q_touch_1 redefinition
q_touch_1 = coda.q_touch_1
# q_touch_2 redefinition
q_touch_2 = coda.q_touch_2
# q_touch_4 redefinition
q_touch_4 = coda.q_touch_4
ps.addPathOptimizer("Graph-RandomShortcut")
from hpp.gepetto import PathPlayer
pp = PathPlayer(v, ps.client.basic)
def solveAll():
# trajectory 1
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_touch_1)
graph.setWeight("talos/right_gripper > desk/touchpoint_right_front | f", 0)
graph.setWeight("talos/left_gripper > desk/touchpoint_left_front | f", 0)
graph.setWeight("talos/left_gripper > desk/touchpoint_left_drawer | f", 0)
graph.setWeight("talos/right_gripper > desk/upper_drawer_spherical_handle | f", 0)
print ps.solve()
# trajectory 2
ps.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q_touch_1)
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("KinodynamicDistance")
ps.selectPathPlanner("DynamicPlanner")
ps.selectPathProjector('Progressive', 0.05)
#solve the problem :
r(q_init)
#r.solveAndDisplay("rm",1,0.01)
tStart = time.time()
t = ps.solve()
tPlanning = time.time() - tStart
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
#pp.displayVelocityPath(0)
r.client.gui.setVisibility("path_0_root", "ALWAYS_ON_TOP")
"""
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
"""
"""
for i in range(0,9):
t = ps.solve()
if isinstance(t, list):
ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.
q_init = fullBody.generateContacts(q_init, [0, 0, 1])
q_goal = fullBody.generateContacts(q_goal, [0, 0, 1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(
q_init,
[fullBody.rLegId, fullBody.lArmId, fullBody.lLegId, fullBody.rArmId])
fullBody.setEndState(
q_goal,
[fullBody.rLegId, fullBody.lArmId, fullBody.lLegId, fullBody.rArmId])
v(q_init)
configs = []
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client, v)
import time
#DEMO METHODS
def initConfig():
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_init)
def endConfig():
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:2] = [-60, -4]
vf.loadObstacleModel("iai_maps", "ville1", "ville1_link")
v = vf.createViewer()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
print(ps.solve())
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.displayPath(1)
#pp (0)
import json
a = ps.getWaypoints(1)
with open('roadmap.txt', 'w') as f:
for i in a[0]:
json.dump(i, f)
f.write("\n")
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath(0)
ps.numberPaths()
ps.pathLength(ps.numberPaths() - 1)
pp(ps.numberPaths() - 1)
ps.clearPathOptimizers()
len(ps.getWaypoints(0))
from hpp.gepetto import Viewer, PathPlayer
r = Viewer(ps)
pp = PathPlayer(cl, r)
r.loadObstacleModel("potential_description", "obstacles_concaves",
"obstacles_concaves")
import numpy as np
dt = 0.1
nPath = 0
points = []
for t in np.arange(0., cl.problem.pathLength(nPath), dt):
points.append([
cl.problem.configAtParam(nPath, t)[0],
cl.problem.configAtParam(nPath, t)[1], 0
])
r.client.gui.addCurve("initCurvPath", points, [1, 0.3, 0, 1])
r.client.gui.addToGroup("initCurvPath", r.sceneName)
q_goal_proj = res[1]
ps.setInitialConfig(q_init_proj)
ps.addGoalConfig(q_goal_proj)
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
N = 20
for i in range(N):
ps.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q_init_proj)
ps.addGoalConfig(q_goal_proj)
t1 = dt.datetime.now()
ps.solve()
t2 = dt.datetime.now()
totalTime += t2 - t1
print(t2 - t1)
n = ps.numberNodes()
totalNumberNodes += n
print("Number nodes: " + str(n))
if N != 0:
print("Average time: " +
str((totalTime.seconds + 1e-6 * totalTime.microseconds) / float(N)))
print("Average number nodes: " + str(totalNumberNodes / float(N)))
v = vf.createViewer()
pp = PathPlayer(robot.client.basic, v)
loopFound = True
if edgeFound and loopFound:
break
if not edgeFound : raise RuntimeError \
('cannot find edge from node "{0}" to "{1}"'.format (n1, n2))
if not loopFound : raise RuntimeError \
('cannot find edge from node "{0}" to "{1}"'.format (n1, n1))
return edges, loops
# infinite norm between vectors
dC = lambda q1,q2: reduce (lambda x,y : x if fabs (y [0]- y [1]) < x \
else fabs (y [0]- y [1]), zip (q1, q2), 0)
if args.display:
v = vf.createViewer ()
pp = PathPlayer (v)
else:
v = lambda x: None
## Initial configuration of manipulator arms
q0_r0 = [pi/6, -pi/2, pi/2, 0, 0, 0,]
q0_r1 = q0_r0 [::]
## Generate initial configurations of spheres
q0_spheres = list ()
i = 0
y = 0.04
while i < nSphere:
q0_spheres.append ([- .1*(i//2), -.12 + y, 0.025, 0, 0, 0, 1])
i+=1; y = -y
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.1, 1, 0)
r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
# calling draw with increasing i will display the sequence
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
#~ collisions = 0;
#~ for config in configs:
#~ if fullBody.isConfigValid(config)[0] == False:
#~ collisions += 1
#~
#~ collisions
from hpp.gepetto import PathPlayer
pp = PathPlayer (ps.robot.client.basic, r)
class AbstractPathPlanner:
rbprmBuilder = None
ps = None
v = None
afftool = None
pp = None
extra_dof_bounds = None
robot_node_name = None # name of the robot in the node list of the viewer
def __init__(self, context=None):
"""
Constructor
:param context: An optional string that give a name to a corba context instance
"""
self.v_max = -1 # bounds on the linear velocity for the root, negative values mean unused
self.a_max = -1 # bounds on the linear acceleration for the root, negative values mean unused
self.root_translation_bounds = [0] * 6 # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
self.root_rotation_bounds = [-3.14, 3.14, -0.01, 0.01, -0.01,
0.01] # bounds on the rotation of the root (-z, z, -y, y, -x, x)
# The rotation bounds are only used during the random sampling, they are not enforced along the path
self.extra_dof = 6 # number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
self.mu = 0.5 # friction coefficient between the robot and the environment
self.used_limbs = [] # names of the limbs that must be in contact during all the motion
self.size_foot_x = 0 # size of the feet along the x axis
self.size_foot_y = 0 # size of the feet along the y axis
self.q_init = []
self.q_goal = []
self.context = context
if context:
createContext(context)
loadServerPlugin(context, 'rbprm-corba.so')
loadServerPlugin(context, 'affordance-corba.so')
self.hpp_client = Client(context=context)
self.hpp_client.problem.selectProblem(context)
self.rbprm_client = RbprmClient(context=context)
else:
self.hpp_client = None
self.rbprm_client = None
@abstractmethod
def load_rbprm(self):
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
pass
def set_configurations(self):
self.rbprmBuilder.client.robot.setDimensionExtraConfigSpace(self.extra_dof)
self.q_init = self.rbprmBuilder.getCurrentConfig()
self.q_goal = self.rbprmBuilder.getCurrentConfig()
self.q_init[2] = self.rbprmBuilder.ref_height
self.q_goal[2] = self.rbprmBuilder.ref_height
def compute_extra_config_bounds(self):
"""
Compute extra dof bounds from the current values of v_max and a_max
By default, set symmetrical bounds on x and y axis and bounds z axis values to 0
"""
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
self.extra_dof_bounds = [
-self.v_max, self.v_max, -self.v_max, self.v_max, 0, 0, -self.a_max, self.a_max, -self.a_max, self.a_max,
0, 0
]
def set_joints_bounds(self):
"""
Set the root translation and rotation bounds as well as the the extra dofs bounds
"""
self.rbprmBuilder.setJointBounds("root_joint", self.root_translation_bounds)
self.rbprmBuilder.boundSO3(self.root_rotation_bounds)
self.rbprmBuilder.client.robot.setExtraConfigSpaceBounds(self.extra_dof_bounds)
def set_rom_filters(self):
"""
Define which ROM must be in collision at all time and with which kind of affordances
By default it set all the roms in used_limbs to be in contact with 'support' affordances
"""
self.rbprmBuilder.setFilter(self.used_limbs)
for limb in self.used_limbs:
self.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
def init_problem(self):
"""
Load the robot, set the bounds and the ROM filters and then
Create a ProblemSolver instance and set the default parameters.
The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
"""
self.load_rbprm()
self.set_configurations()
self.compute_extra_config_bounds()
self.set_joints_bounds()
self.set_rom_filters()
self.ps = ProblemSolver(self.rbprmBuilder)
# define parameters used by various methods :
if self.v_max >= 0:
self.ps.setParameter("Kinodynamic/velocityBound", self.v_max)
if self.a_max >= 0:
self.ps.setParameter("Kinodynamic/accelerationBound", self.a_max)
if self.size_foot_x > 0:
self.ps.setParameter("DynamicPlanner/sizeFootX", self.size_foot_x)
if self.size_foot_y > 0:
self.ps.setParameter("DynamicPlanner/sizeFootY", self.size_foot_y)
self.ps.setParameter("DynamicPlanner/friction", 0.5)
# sample only configuration with null velocity and acceleration :
self.ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
def init_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[]):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf = ViewerFactory(self.ps)
if self.context:
self.afftool = AffordanceTool(context=self.context)
self.afftool.client.affordance.affordance.resetAffordanceConfig(
) # FIXME: this should be called in afftool constructor
else:
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" + env_name + ".urdf",
"planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
def init_planner(self, kinodynamic=True, optimize=True):
"""
Select the rbprm methods, and the kinodynamic ones if required
:param kinodynamic: if True, also select the kinodynamic methods
:param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
"""
self.ps.selectConfigurationShooter("RbprmShooter")
self.ps.selectPathValidation("RbprmPathValidation", 0.05)
if kinodynamic:
self.ps.selectSteeringMethod("RBPRMKinodynamic")
self.ps.selectDistance("Kinodynamic")
self.ps.selectPathPlanner("DynamicPlanner")
if optimize:
if kinodynamic:
self.ps.addPathOptimizer("RandomShortcutDynamic")
else:
self.ps.addPathOptimizer("RandomShortcut")
def solve(self, display_roadmap=False):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
"""
if len(self.q_init) != self.rbprmBuilder.getConfigSize():
raise ValueError("Initial configuration vector do not have the right size")
if len(self.q_goal) != self.rbprmBuilder.getConfigSize():
raise ValueError("Goal configuration vector do not have the right size")
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
self.v(self.q_init)
if display_roadmap and self.v.client.gui.getNodeList() is not None:
t = self.v.solveAndDisplay("roadmap", 5, 0.001)
else:
t = self.ps.solve()
print("Guide planning time : ", t)
def display_path(self, path_id=-1, dt=0.1):
"""
Display the path in the viewer, if no path specified display the last one
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.1)
"""
if self.pp is not None:
if path_id < 0:
path_id = self.ps.numberPaths() - 1
self.pp.dt = dt
self.pp.displayVelocityPath(path_id)
def play_path(self, path_id=-1, dt=0.01):
"""
play the path in the viewer, if no path specified display the last one
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.01)
"""
self.show_rom()
if self.pp is not None:
if path_id < 0:
path_id = self.ps.numberPaths() - 1
self.pp.dt = dt
self.pp(path_id)
def hide_rom(self):
"""
Remove the current robot from the display
"""
self.v.client.gui.setVisibility(self.robot_node_name, "OFF")
def show_rom(self):
"""
Add the current robot to the display
"""
self.v.client.gui.setVisibility(self.robot_node_name, "ON")
@abstractmethod
def run(self):
"""
Must be defined in the child class to run all the methods with the correct arguments.
"""
# example of definition:
"""
self.init_problem()
# define initial and goal position
self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
self.play_path()
"""
pass
#robot.setJointBounds('base_joint_xyz', [1.6, 6.9, -2.2, 1.5, 0, 3]) # first goal
#robot.setJointBounds('base_joint_xyz', [-0.3, 6.9, -2.2, 2.4, 0, 3]) # second goal
#robot.setJointBounds('base_joint_xyz', [-2.6, 6.9, -2.2, 2.4, 0, 3]) # third goal
robot.setJointBounds('base_joint_xyz', [-6, 6.9, -2.8, 3.2, 0, 3]) # start to bottom
#robot.setJointBounds('base_joint_xyz', [-6, -2.2, -2.4, 3, 0, 8]) # bottom to ultimate
#robot.setJointBounds('base_joint_xyz', [-5, -2.2, -0.1, 2.8, 0, 6]) # bottom to middle column
#robot.setJointBounds('base_joint_xyz', [-5, -2.2, -0.1, 2.8, 0, 3]) # bottom to bottom 1
#robot.setJointBounds('base_joint_xyz', [-6, 6.9, -2.5, 3.2, 0, 3]) # first to bottom
ps = ProblemSolver (robot)
cl = robot.client
#cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d')
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder")
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [6.4, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, Pi] # start
#q11 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, Pi] # bottom of column
r(q11)
#q22 = [2.6, -1.4, 0.35, 0, 0, 0, 1, 0, 0, 1, Pi] # first plateform
#q22 = [0.7, 1.55, 0.4, 0, 0, 0, 1, 0, 0, 1, Pi] # second plateform
#q22 = [-1.7, -1.5, 0.4, 0, 0, 0, 1, 0, 0, 1, Pi] # third plateform
q22 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, Pi] # bottom of column
#q22 = [-3.3, 1.5, 3.4, 0, 0, 0, 1, 0, 0, 1, Pi] # in column
#q22 = [-4.2, 0.9, 1.7, 0, 0, 0, 1, 0, 0, 1, Pi] # bottom 1 of column
#q22 = [-4.4, 0.9, 4.1, 0, 0, 0, 1, 0, 0, 1, Pi] # bottom 3 of column
#q22 = [-4.4, -1.8, 6.5, 0, 0, 0, 1, 0, 0, 1, Pi] # ultimate goal!
r(q22)
ps = ProblemSolver (robot)
cl = robot.client
#cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d')
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
#q1 = [-1.5, -1.5, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]; q2 = [2.6, 3.7, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]
#q11 = [2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0] # plane with theta = Pi
#q11 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 8, 0, 0, 0, 1, 0, 0, 1, 0] # plane with theta ~= 0
#q11 = [-2.5, 3, 4, 0, 0, 0, 1,-0.1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 8, 0, 0, 0, 1, -0.1, 0, 0, 1, 0] # plane with theta ~= 0 MONOPOD
#q11 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 9, 0, 0, 0, 1, 0, 0, 1, 0] # multiple-planes (3 planes)
q11 = [-5, 3.1, 4.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [5.2, -5.2, 4, 0, 0, 0, 1, 0, 0, 1, 0] # environment_3d
#r(q22)
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("animals_description","plane_3d","plane_3d")
#r.loadObstacleModel ("animals_description","multiple_planes_3d","multiple_planes_3d")
#r.loadObstacleModel ("animals_description","inclined_plane_3d","inclined_plane_3d")
r.loadObstacleModel ("animals_description","environment_3d","environment_3d")
#r.loadObstacleModel ("animals_description","environment_3d_with_window","environment_3d_with_window")
addLight (r, [-3,3,7,1,0,0,0], "li"); addLight (r, [3,-3,7,1,0,0,0], "li1")
r(q11)
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
ps.solve ()
# PROBLEM !! not finding solution for environment_3d_window with mu=0.5 V0max=6.5 Projectionshooter .... V0 or Vimp too much limiting ?? 'cause V0max=7 gives a too "easy" solution ...
q_goal = s_goal.q()
# copy extraconfig for start and init configurations
q_init[configSize:configSize + 3] = dir_init[::]
q_init[configSize + 3:configSize + 6] = acc_init[::]
q_goal[configSize:configSize + 3] = dir_goal[::]
q_goal[configSize + 3:configSize + 6] = acc_goal[::]
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartStateId(s_init.sId)
fullBody.setEndStateId(s_goal.sId)
q_far = q_init[::]
q_far[2] = -5
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
pp.dt = 0.0001
r(q_init)
# computing the contact sequence
#~ configs = fullBody.interpolate(0.08,pathId=1,robustnessTreshold = 2, filterStates = True)
configs = fullBody.interpolate(0.001,
pathId=0,
robustnessTreshold=1,
filterStates=True)
r(configs[-1])
print "number of configs =", len(configs)
r(configs[-1])
0.9323806762695312,
0.36073973774909973,
0.008668755181133747,
0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
q = [
1.0220000000000002, -0.01, 0.6955999999999999, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.3, 0.0, -0.17000000000000023, 0.0, 0.0, 0.0
] # BUG
# Playing the computed path
pi = ps.numberPaths() - 1
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(pi)
r.client.gui.setVisibility("path_" + str(pi) + "_root", "ALWAYS_ON_TOP")
#display path
pp.speed = 1
#pp (pi)
"""
import parse_bench
parse_bench.parseBenchmark(t)
"""
print("path lenght = ",
str(ps.client.problem.pathLength(ps.numberPaths() - 1)))
###########################
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(args.N):
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) / float(args.N)))
print("Average number nodes: " + str(totalNumberNodes / float(args.N)))
if args.display:
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
#v = vf.createViewer()
from hpp.gepetto import PathPlayer
pp = PathPlayer(v, robot.client)
if args.run:
pp(0)
from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints
import math
import numpy as np
robot = Robot ('sphere')
robot.setJointBounds('base_joint_xyz', [-5, 5, -5, 5, -1, 7])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [-3.8, 2.4, 1.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [3.5, -3.3, 0.4, 0, 0, 0, 1, 0, 0, 1, 0]
#cl.obstacle.loadObstacleModel('animals_description','envir3d_window_mesh','')
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","envir3d_window_mesh","envir3d_window_mesh")
addLight (r, [-3,3,4,1,0,0,0], "li"); addLight (r, [3,-3,4,1,0,0,0], "li1")
r(q11)
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
r(q1)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
plotSphere (q2, cl, r, "sphere_q2", [0,1,0,1], 0.02) # same as robot
nPointsPlot = 50
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
plotFrame (r, "_", [0,0,2.8], 0.5)
print(ps.solve())
# display the computed roadmap. Note that the edges are all represented as straight line
# and may not show the real motion of the robot between the nodes :
v.displayRoadmap("rm")
# Alternatively, use the following line instead of ps.solve() to display the roadmap as
# it's computed (note that it slow down a lot the computation)
# v.solveAndDisplay('rm',1)
# Highlight the solution path used in the roadmap
v.displayPathMap("pm", 0)
# remove the roadmap for the scene :
# v.client.gui.removeFromGroup("rm",v.sceneName)
# v.client.gui.removeFromGroup("pm",v.sceneName)
# Connect to a viewer server and play solution paths
pp = PathPlayer(v)
# play path before optimization
pp(0)
# Display the optimized path, with a color variation depending on the velocity along the
# path (green for null velocity, red for maximal velocity)
pp.dt = 0.1
pp.displayVelocityPath(1)
# play path after random shortcut
pp.dt = 0.001
pp(1)
# b hpp::core::pathOptimization::GradientBased::initialize (const PathVectorPtr_t& path)
from hpp.corbaserver.puzzle import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
import math
robot = Robot ('puzzle')
ps = ProblemSolver (robot)
cl = robot.client
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
robot.setJointBounds('base_joint_xyz', [-2.2, +2.2, -0.2, 2.2, -0.5, 0.5])
Q = []
Q.append ( [0, 0, 0, 1.0, 0.0, 0.0, 0.0])
"""
Q.append ( [0.0, 0.0, 0.0, 0.8573289792052967, 0.5110043077178016, 0.0474382998474562, 0.04014009987092448])
Q.append ( [0.0, 0.0, 0.0, 0.47002595717039686, 0.8761976030104256, 0.08134045836690892, 0.06882654169507678])
Q.append ( [0.0, 0.0, 0.0, -0.05139523108351973, 0.9913748854243123, 0.09203276443212992, 0.07787387759641762]) # !
Q.append ( [0.0, 0.0, 0.0, -0.5581511991721069, 0.8236712340507641, 0.07646425360117025, 0.06470052227791329])
Q.append ( [0.0, 0.0, 0.0, -0.9056431645733478, 0.420939551154707, 0.03907727653904272, 0.033065387840728454]) # ! one turn
Q.append ( [0.0, 0.0, 0.0, -0.7409238777634497, -0.6666775704281703, 0.06188998802924064, 0.05236845140935746])
Q.append ( [0.0, 0.0, 0.0, 0.2445263320841038, -0.9625514882482619, 0.08935698863687985, 0.07560975961582142])
Q.append ( [0.0, 0.0, 0.0, 0.708781393086984, -0.7002692759274627, 0.0650084224020931, 0.05500712664792493])
Q.append ( [0.0, 0.0, 0.0, 0.9707913243458437, -0.23817079875118724, 0.02211022019858673, 0.01870864786034262])
# display the computed roadmap. Note that the edges are all represented as straight line and may not show the real motion of the robot between the nodes :
v.displayRoadmap("rm")
#Alternatively, use the following line instead of ps.solve() to display the roadmap as it's computed (note that it slow down a lot the computation)
#v.solveAndDisplay('rm',1)
# Highlight the solution path used in the roadmap
v.displayPathMap('pm',0)
# remove the roadmap for the scene :
#v.client.gui.removeFromGroup("rm",v.sceneName)
#v.client.gui.removeFromGroup("pm",v.sceneName)
# Connect to a viewer server and play solution paths
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
#play path before optimization
pp (0)
# Display the optimized path, with a color variation depending on the velocity along the path (green for null velocity, red for maximal velocity)
pp.dt=0.1
pp.displayVelocityPath(1)
# play path after random shortcut
pp.dt=0.001
pp (1)