plt = addCorbaPathPlot(cl, ps.numberPaths() - 1, 'k', 1.5, plt)
plt = addCircleNodePlot(collConstrNodes, 'b', 0.14, plt)
#plt = addCircleNodePlot (collNodes, 'r', 0.14, plt)
plt = addNodePlot(contactPoints, 'ko', '', 5.5, plt)
plt = addNodeAndLinePlot(x1_J1, x2_J1, 'ro', 'bo', 4, 'r', 1.3, plt)
plt = addNodeAndLinePlot(x1_J2, x2_J2, 'ro', 'bo', 4, 'r', 1.3, plt)
plt = addNodePlot(collConstrNodes, 'ko', '', 5, plt)
plt.show()
## same with viewer !
from hpp.gepetto import Viewer, PathPlayer
r = Viewer(ps)
pp = PathPlayer(robot.client, r)
r.loadObstacleModel("robot_2d_description", "cylinder_obstacle",
"cylinder_obstacle")
from viewer_display_library_OPTIM import transformInConfig, plotPoints, plotPointsAndLines, plot2DBaseCurvPath
contactPointsViewer = transformInConfig(contactPoints)
x1_J1Viewer = transformInConfig(x1_J1)
x2_J1Viewer = transformInConfig(x2_J1)
x1_J2Viewer = transformInConfig(x1_J2)
x2_J2Viewer = transformInConfig(x2_J2)
sphereNamePrefix = "sphereContactPoints_"
plotPoints(r, sphereNamePrefix, contactPointsViewer, 0.02)
sphereSize = 0.01
lineNamePrefix = "lineJ1_"
sphereNamePrefix = "sphereJ1_"
plotPointsAndLines(r, lineNamePrefix, sphereNamePrefix, x1_J1Viewer,
x2_J1Viewer, sphereSize)
white=[1.0,1.0,1.0,1.0]
green=[0.23,0.75,0.2,0.5]
yellow=[0.85,0.75,0.15,1]
pink=[1,0.6,1,1]
orange=[1,0.42,0,1]
robot = Robot ('box')
robot.setJointBounds ("base_joint_xyz", [0,5,0,2,0,2])
robot.client.robot.setDimensionExtraConfigSpace(robot.getNumberDof()) # extraDof for velocitiy
ps = ProblemSolver (robot)
ps.selectPathPlanner("dyn")
v = Viewer (ps)
#v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")
v.loadObstacleModel ("iai_maps", "abstract", "abstract")
q_init = [0,1,1,1,0,0,0]
q_goal = [5,1,1,0.9239,0,-0.3827,0]
v (q_init)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.addPathOptimizer ("RandomShortcut")
ps.solve ()
v.displayRoadmap("rmB",white,0.01,1,green)
pp = PathPlayer (robot.client, v)
robot.setJointBounds ("base_joint_xyz", [0,5,0,2,0,2])
# room : -5,4,-7,5,0.5,0.5
robot.tf_root = 'base_link'
ps = ProblemSolver (robot)
v = Viewer (ps)
q_init = robot.getCurrentConfig ()
q_init[0:3] = [0.5, 1, 1] #z=0.4 pour sphere
v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [5,1, 1]
#v (q_goal)
print("chargement map")
v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")
ps.selectPathPlanner("rrtPerso")
print("Debut motion planning")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
v.solveAndDisplay(white,0.02,1,yellow)
ps.solve ()
robot = RobotChaine ('robot_chaine', True)
robot.setJointBounds ("base_joint_xyz", [-4, 12, -4, 12, -2, 4])
robot.tf_root = 'base_link'
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer
v = Viewer (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:3] = [1, 2, 1]
q_goal [0:3] = [9.15, 7.5, 0.5]
v.loadObstacleModel ("iai_maps", "env_simple", "simple")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.clearRoadmap()
ps.selectPathPlanner("interactive")
white=[1.0,1.0,1.0,1.0]
brown=[0.85,0.75,0.15,0.5]
ps.addPathOptimizer ("RandomShortcut")
v.solveAndDisplay("rm1",50,white,0.05,1,brown)
#ps.solve()
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, v)
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init = [-6,-3,0.8,1,0,0,0]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = [4, 4, 0.8, 1, 0, 0, 0]; r (q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, name_of_scene, "planning")
r.addLandmark(r.sceneName,1)
#~ ps.solve ()
t = ps.solve ()
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()
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_robust_10_path.txt")
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (0)
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)
nPath = 1
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]
robot.isConfigValid(qf)
from hpp.corbaserver.lydia import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import Viewer, PathPlayer
robot = Robot ('lydia')
robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1.1, 1.1])
ps = ProblemSolver (robot)
r = Viewer (ps)
r.loadObstacleModel ("hpp_benchmark", "obstacle", "obstacle")
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [2] = -0.6
q_goal [2] = 0.6
ps.selectPathPlanner ("VisibilityPRM")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.solve ()
import math
import numpy as np
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-6, 6, -6, 6, 2, 9])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
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]
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("animals_description","environment_3d","environment_3d")
r.loadObstacleModel ("animals_description","environment_3d_with_window","environment_3d_with_window")
addLight (r, [-5,5,7,1,0,0,0], "li"); addLight (r, [5,-5,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)
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
# First parabola(s): vmax = 8m/s, mu = 1.2
plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2")
from hpp.tp_rrt import Robot
robot = Robot("buggy")
robot.setJointBounds ("base_joint_xy", [-5, 16, -4.5, 4.5])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer
gui = Viewer (ps)
gui.loadObstacleModel ('tp-rrt', "scene", "scene")
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = q_init[0:2]=[-3.7, -4]; gui(q_init)
gui (q_init)
q_goal [0:2] = [15,2]
gui (q_goal)
#~ ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathPlanner ("PlannerTP")
ps.addPathOptimizer ("RandomShortcut")
t = ps.solve ()
print ("solving time", t)
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
# q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF#
q1 = [-0.07, -1.3, 1.4, -0.8, 3.14, 0];
q2 = [-0.2, -1.65, -1.6, 0.4, 0.8, 0]
r(q1)
robot.isConfigValid(q1); robot.isConfigValid(q2)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
r.loadObstacleModel ("ur_description","table","table")
r.loadObstacleModel ("ur_description","obstacle_sphere","obstacle_sphere")
r(q1)
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT.rdm')
ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve ()
ps.pathLength(0)
ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-PRM1.rdm')
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer
v = Viewer (ps)
q_init = robot.getCurrentConfig ()
q_init[0:3] = [1, 1, 0.4] #z=0.4 pour sphere
v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [-2.5, -3.5, 0.4]
#v (q_goal)
print("chargement map")
v.loadObstacleModel ("iai_maps", "room", "room")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathPlanner("rrtPerso")
print("Debut motion planning")
tps1 = time.clock()
ps.solve ()
tps2 = time.clock()
print(tps2-tps1)
display.displayRoadmap(ps,v)
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, v)
# fullBody.setCurrentConfig (q_goal)
# q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# RW: update the problem solver and viewer
ps = ProblemSolver(fullBody)
r = Viewer (ps)
# fullBody.setStartState(q_init,[])
# fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r.loadObstacleModel ('hpp-rbprm-corba', name_of_scene, "contact")
r.client.gui.setColor('contact', [1,1,1,0.5])
def importTrajectory(filename):
index = -1
allT = {}
f = open(filename, 'r')
content = f.readlines()
configs = []
for l in content:
if ('agent' in l):
if index != -1:
allT[index] = configs
i = l.split(' ')
index = int(i[1])
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
robot = Robot ('ur5')
cl = robot.client
ps = ProblemSolver (robot)
# q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF#
q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("ur_description","obstacles","obstacles")
r.loadObstacleModel ("ur_description","table","table")
r.loadObstacleModel ("ur_description","wall","wall")
r(q1)
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT.rdm')
ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve ()
ps.pathLength(0)
ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-PRM1.rdm')
ps.addPathOptimizer('RandomShortcut')
ps = ProblemSolver(robot)
from hpp.gepetto import Viewer
r = Viewer(ps)
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q1 = q_init[::]
q_init[0] = -.5
q_goal[0] = .5
r(q_init)
r(q_goal)
q1[:2] = (0., .5)
r.loadObstacleModel("hpp_tutorial", "box", "box-1")
ps.selectPathValidation("Dichotomy", 0.)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q1)
ps.solve()
ps.resetGoalConfigs()
ps.addGoalConfig(q_goal)
ps.solve()
ps.addPathOptimizer("GradientBased")
#ps.optimizePath (ps.numberPaths () - 1)
from hpp.gepetto import PathPlayer
pp = PathPlayer(robot.client, r)
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer
r = Viewer (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q1 = q_init [::]
q_init [0] = -.5
q_goal [0] = .5
r (q_init)
r (q_goal)
q1 [:2] = (0.,.5)
r.loadObstacleModel ("hpp_tutorial", "box", "box-1")
ps.selectPathValidation ("Dichotomy", 0.)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q1)
ps.solve ()
ps.resetGoalConfigs ()
ps.addGoalConfig (q_goal)
ps.solve ()
ps.addPathOptimizer ("GradientBased")
#ps.optimizePath (ps.numberPaths () - 1)
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, r)
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.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM1.rdm') # srand # problem ?
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM1.rdm') # srand 1453110445(909sec) [COLL!]
from hpp.corbaserver.ant_sphere import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import numpy as np
from viewer_display_library import normalizeDir, plotVerticalCone, plotCone, plotPath, plotVerticalConeWaypoints, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints
from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-5, 5, -5, 5, -0.01, 3]) # room
ps = ProblemSolver (robot)
cl = robot.client
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("room_description","room","room")
# Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta]
#q11 = [0, -1, 0.002, 0, 0, 0, 0, 0, 1]; #q22 = [-0.1, 1.75, 0.302, 1.57, 0, 0, 0, 0, 1]
q11 = [1.781, 1.4, 1.3, 0, 0, 0, 1, 0, 0, 1]; #q22 = [0.8, -2.6, 2.35, 0, 0, 0, 1, 0, 0, 1]
#q22 = [0.45, 1.4, 0.8, 0, 0, 0, 1, 0, 0, 1] # easier
q22 = [0, 2, 0.23, 1, 0, 0, 0, 0, 0, 1] # under table
q1 = cl.robot.projectOnObstacle (q11, 3, 0.03)
robot.isConfigValid(q1)
q2 = cl.robot.projectOnObstacle (q22, 3, 0.03)
robot.isConfigValid(q2)
r(q2)
index = cl.robot.getConfigSize () - 3
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init [::]
q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal [0:3] = [1.49, -0.65, 1.25]; r (q_goal)
#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "stair_bauzil", "planning")
#~ ps.solve ()
t = ps.solve ()
print t;
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()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "groundcrouch", "planning")
#~ ps.solve ()
t = ps.solve ()
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()
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_robust_10_path.txt")
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints, plotConesRoadmap, plotEdgesRoadmap
import math
import numpy as np
robot = Robot ('robot')
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]
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps); gui = r.client.gui
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)
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
#plotFrame (r, 'frame_group', [0,0,0], 0.6)
# First parabola(s): vmax = 6.8m/s, mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(6.8)
green = [0.23, 0.75, 0.2, 0.5]
yellow = [0.85, 0.75, 0.15, 1]
pink = [1, 0.6, 1, 1]
orange = [1, 0.42, 0, 1]
robot = Robot('box')
robot.setJointBounds("base_joint_xyz", [0, 5, 0, 2, 0, 2])
robot.client.robot.setDimensionExtraConfigSpace(
robot.getNumberDof()) # extraDof for velocitiy
ps = ProblemSolver(robot)
ps.selectPathPlanner("dyn")
v = Viewer(ps)
#v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")
v.loadObstacleModel("iai_maps", "abstract", "abstract")
q_init = [0, 1, 1, 1, 0, 0, 0]
q_goal = [5, 1, 1, 0.9239, 0, -0.3827, 0]
v(q_init)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.solve()
v.displayRoadmap("rmB", white, 0.01, 1, green)
pp = PathPlayer(robot.client, v)
import numpy as np
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -3, 4.2, -2.5, 4])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [-0.3, 3.65, -0.25, 1, 0, 0, 0, 0, 0, 1, 0];
q22 = [-0.18, -2.2, -0.4, 1, 0, 0, 0, 0, 0, 1, 0]
#cl.obstacle.loadObstacleModel('animals_description','cave','')
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","cave","cave")
#addLight (r, [-0.3, 3.8, 0,1,0,0,0], "li"); addLight (r, [-0.18, -3, 0.1,1,0,0,0], "li1"); addLight (r, [-0.3, 4, 0,1,0,0,0], "li3")
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,1.5], 0.5)
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
robot = Robot ('ur5')
cl = robot.client
ps = ProblemSolver (robot)
# q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF#
q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("ur_description","obstacles","obstacles")
r.loadObstacleModel ("ur_description","table","table")
r.loadObstacleModel ("ur_description","wall","wall")
r(q1)
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT1.rdm')
#ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve ()
ps.pathLength(0)
ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT-testConv1.rdm')
#import time
#import sys
#import matplotlib.pyplot as plt
#sys.path.append('/local/mcampana/devel/hpp/src/test-hpp/script')
robot = Robot ('robot_2d')
ps = ProblemSolver (robot)
cl = robot.client
#cl.obstacle.loadObstacleModel('robot_2d_description','environment_2d','')
#cl.obstacle.loadObstacleModel('robot_2d_description','environment_2d_harder','')
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (cl, r)
#r.loadObstacleModel ("robot_2d_description","environment_2d","environment_2d")
r.loadObstacleModel ("robot_2d_description","environment_2d_harder","environment_2d_harder")
#r.loadObstacleModel ("iai_maps", "labyrinth", "labyrinth")
#r.loadObstacleModel ("iai_maps", "labyrinth2", "labyrinth2")
# q = [x, y, dir.x, dir.y] # limits in URDF file
#q1 = [-1.3, 0.2]; q2 = [0.2, 0.2]; q3 = [5, -2.8]; q4 = [9, 2.2]; q5 = [12.4, -5.8];
#q1 = [-1.3, 0.2, 0]; q2 = [0.2, 0.2, 0]; q3 = [5, -2.8, 0]; q4 = [9, 2.2, 0]; q5 = [12.4, -5.8, 0];
q1 = [-1.3, 0.2, 0, 1]; q5 = [12.4, -5.8, 0, 1];
ps.setInitialConfig (q1); ps.addGoalConfig (q5); ps.solve ()
# with sub-paths
q1 = [-1.3, 0.2, 0, 1]; q2 = [0.2, 0.2, 0, 1]; q3 = [5, -2.8, 0, 1]; q4 = [9, 2.2, 0, 1];
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () # pp(0)
ps.resetGoalConfigs ()
ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve () # pp(1)
#------------------------------------------------------------------
# name = pipedream
# robot = ring
# world = pipedream_env
from math import pi
from hpp.corbaserver.hpp_ompl_benchmark.pipedream import Robot
robot = Robot('ompl')
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer
v = Viewer (ps)
v.loadObstacleModel("hpp-ompl-benchmark", "pipedream_env", "env")
q_init = [2.02,-0.0912,0.786,0.707,0.707,0,0]
q_goal = [1.587,-0.345,0.634,1,0,0,0]
robot.setJointBounds ('base_joint_xyz', [1.08, 2.43, -0.824, 0.28, 0.3, 1.7])
# robot.setJointBounds ('base_joint_SO3', [-pi, pi, -pi, pi, -pi, pi, -pi, pi])
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectPathValidation("Progressive",0.03)
ps.client.problem.clearPathOptimizers()
v(q_init)
from hpp.corbaserver import Benchmark
benchmark = Benchmark (robot.client, robot, ps)
#ps.client.problem.setRandomSeed(3530408688)
benchmark.seedRange = range (50)
#robot.setJointBounds('base_joint_xyz', [-0.3, 6.8, -2.2, 2.4, 0, 3]) # second goal
#robot.setJointBounds('base_joint_xyz', [-2.6, 6.8, -2.2, 2.4, 0, 3]) # third goal
#robot.setJointBounds('base_joint_xyz', [-6, 6.8, -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.8, -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")
addLight (r, [-5,0,2,1,0,0,0], "li"); addLight (r, [2,-2,5,1,0,0,0], "li1")
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [6.2, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0] # start
#q11 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # bottom of column
r(q11)
#q22 = [2.6, -1.4, 0.35, 0, 0, 0, 1, 0, 0, 1, 0] # first goal
#q22 = [0.7, 1.55, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # second goal
#q22 = [-1.7, -1.5, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # third goal
#q22 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # bottom of column
#q22 = [-3.3, 1.5, 3.4, 0, 0, 0, 1, 0, 0, 1, 0] # in column
#q22 = [-4.2, 0.9, 1.7, 0, 0, 0, 1, 0, 0, 1, 0] # bottom 1 of column
#q22 = [-4.4, 0.9, 4.1, 0, 0, 0, 1, 0, 0, 1, 0] # bottom 3 of column
q22 = [-4.4, -1.5, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] # ultimate goal!
r(q22)
# Script to solve ompl problem in hpp:
# name = Abstract
# robot = Absrtact_robot
# world = Abstract_env
from math import pi
from hpp.corbaserver.hpp_ompl_benchmark.Abstract import Robot
robot = Robot('ompl')
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer
v = Viewer (ps)
v.loadObstacleModel("hpp-ompl-benchmark", "Abstract_env", "env")
q_init = [2.1, -5.81216, -1, 1 , 0, 0, 0]
q_goal = [-3, -4.3, 1, 0.7071, 0.7071, 0, 0]
v(q_init)
robot.setJointBounds ('base_joint_xyz', [-5.5, 6.5, -12, 0, -5, 6])
# robot.setJointBounds ('base_joint_SO3', [-pi, pi, -pi, pi, -pi, pi, -pi, pi])
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver import Benchmark
benchmark = Benchmark (robot.client, robot, ps)
ps.client.problem.setRandomSeed(1927402002)
benchmark.seedRange = range (50)
benchmark.iterPerCase = 1
results = benchmark.do()
from hpp.corbaserver.lydia import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import Viewer, PathPlayer
robot = Robot('lydia')
robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1.1, 1.1])
ps = ProblemSolver(robot)
r = Viewer(ps)
r.loadObstacleModel("hpp_benchmark", "obstacle", "obstacle")
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[2] = -0.6
q_goal[2] = 0.6
ps.selectPathPlanner("VisibilityPrmPlanner")
ps.selectPathValidation("Dichotomy", 0.)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.readRoadmap("/local/mcampana/devel/hpp/src/hpp_benchmark/roadmap/lydia.rdm")
#ps.solve ()
pp = PathPlayer(robot.client, r)
"""
ps.addPathOptimizer ("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
q_init[0:3] = [-5, 0, 0.63]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal = q_init[::]
q_goal[0:3] = [5, 0, 0.63]
r(q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.01)
r.loadObstacleModel(packageName, "groundcrouch", "planning")
#~ ps.solve ()
t = ps.solve()
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()
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_robust_10_path.txt")
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
#~ pp (0)
#~ pp (1)
r(q_init)
timeValuesGB3 = cl.problem.getTimeValues (); gainValuesGB3 = cl.problem.getGainValues ()
newGainValuesGB3 = ((1-np.array(gainValuesGB3))*100).tolist() #percentage of initial length-value
cl.problem.setAlphaInit (0.4)
ps.optimizePath (0); tGB4 = cl.problem.getTimeGB ()
timeValuesGB4 = cl.problem.getTimeValues (); gainValuesGB4 = cl.problem.getGainValues ()
newGainValuesGB4 = ((1-np.array(gainValuesGB4))*100).tolist() #percentage of initial length-value
## -------------------------------------
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)
nPath = 1
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])
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)
r.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.solve()
from hpp.gepetto import PathPlayer
pp = PathPlayer(robot.client, r)
pp(0)
pp(1)
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal[0:3] = [2, 0, 1.7]
q_goal[0:3] = [1.6, -0.1, 1.5]
#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
r.loadObstacleModel(packageName, "truck", "planning")
t = ps.solve()
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
#~ pp (0)
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "EFORT", 0.05)
scale = sys.argv[len(sys.argv)-2]
scene = sys.argv[len(sys.argv)-1]
#~ configFile = sys.argv[len(sys.argv)-1]
import pickle
sFile = "false_negative_configs_"+scene+'_'+scale+'.pkl'
pkl_file = open(sFile, 'rb')
falseNeg = pickle.load(pkl_file)
pkl_file.close()
ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', scene, "planning")
q_init = [
0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
fullBody.setCurrentConfig (q_init)
confsize = len(falseNeg[0])
falseFalseNegative = 0
from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones
import math
import numpy as np
robot = Robot ('robot')
#robot.setJointBounds('base_joint_xyz', [-6, 7, -4, 4, 0, 8]) # ultimate goal!
#robot.setJointBounds('base_joint_xyz', [1.2, 7, -3, -0.5, 0, 2.2]) # first goal
robot.setJointBounds('base_joint_xyz', [0, 7, -3, 2.4, 0, 2.2]) # second goal
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","scene_jump")
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [6, -1.6, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]
#q22 = [2.2, -2.7, 0.8, 0, 0, 0, 1, 0, 0, 1, 0] # first goal
q22 = [1.2, 1.4, 0.5, 0, 0, 0, 1, 0, 0, 1, 0] # second goal
r(q22)
#q22 = [-4.4, -1.6, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] # ultimate goal!
q1 = cl.robot.projectOnObstacle (q11, 0.02); q2 = cl.robot.projectOnObstacle (q22, 0.02)
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_envir3d_with_window.rdm')
q_goal = q_init [::]
q_init = rbprmBuilder.getCurrentConfig ();
q_init[0:3] = [0.15, -0.45, 0.8]; r(q_init)
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_goal[0:3] = [1.2,-1,0.5]; r(q_goal)
q_goal[0:3] = [0.2,-1.1,0.58]; r(q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "car", "planning")
t = ps.solve ()
print (t)
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()
#~ print ("solving time " + str(t));
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (0)
#/usr/bin/env python
import time
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.pr2_with_set import Robot
robot = Robot ('pr2_with_set') # will containt the set fixed on the left hand... (moving with l_wrist_flex_joint)
robot.setJointBounds ("base_joint_xy", [-4, -0.5, -7, -1.5])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
Viewer.withFloor = True
r = Viewer (ps)
r.loadObstacleModel ("iai_maps","kitchen_area","kitchen_area") # visual kitchen
r.loadObstacleModel ("iai_maps","floor","floor")
q1 = robot.getCurrentConfig ()
q1 [0:2] = [-3.4, -6]
q1 [robot.rankInConfiguration ['torso_lift_joint']] = 0.13
q1 [robot.rankInConfiguration ['l_shoulder_pan_joint']] = 0.5 # ecarte les bras
q1 [robot.rankInConfiguration ['r_shoulder_pan_joint']] = -0.5
q1 [robot.rankInConfiguration ['l_upper_arm_roll_joint']] = 1.57 # tourne bras sur lui-mm
q1 [robot.rankInConfiguration ['r_upper_arm_roll_joint']] = -1.57
q1 [robot.rankInConfiguration ['l_elbow_flex_joint']] = -0.49 # plier coude
q1 [robot.rankInConfiguration ['r_elbow_flex_joint']] = -0.50
q1 [robot.rankInConfiguration ['l_wrist_flex_joint']] = -0.2 # plie poignet
q1 [robot.rankInConfiguration ['r_wrist_flex_joint']] = -0.2
q1 [robot.rankInConfiguration ['l_gripper_r_finger_joint']] = 0.09 # ouvrir pince
q1 [robot.rankInConfiguration ['l_gripper_l_finger_joint']] = 0.12
q1 [robot.rankInConfiguration ['r_gripper_r_finger_joint']] = 0.13
q1 [robot.rankInConfiguration ['r_gripper_l_finger_joint']] = 0.095
r(q1)
robot = Robot('robot')
robot.setJointBounds('base_joint_xyz', [-3.9, 3.9, -3.9, 3.9, 2, 9])
ps = ProblemSolver(robot)
cl = robot.client
# 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 = [-1.8, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]
q22 = [1.8, 2.7, 8, 0, 0, 0, 1, 0, 0, 1, 0] # theta ~= 0
from hpp.gepetto import Viewer, PathPlayer
r = Viewer(ps)
pp = PathPlayer(robot.client, r)
r.loadObstacleModel("animals_description", "plane_3d", "plane_3d")
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)
r(q1)
ps.setInitialConfig(q1)
ps.addGoalConfig(q2)
plotSphere(q2, cl, r, "sphere1", [0, 1, 0, 1], 0.02)
nPointsPlot = 60
# 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 ...
samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1])
plotConeWaypoints (cl, 0, r, "wp", "friction_cone")
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)
r.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.solve()
from hpp.gepetto import PathPlayer
pp = PathPlayer(robot.client, r)
pp(0)
pp(1)
lArmx = 0.024
lArmy = 0.024
fullBody.addLimb(larmId, larm, lHand, lArmOffset, lArmNormal, lArmx, lArmy,
10000, "EFORT", 0.05)
scene = sys.argv[len(sys.argv) - 1]
configFile = "cgeom_feasible_2DGrid_" + scene + '.pkl'
import pickle
pkl_file = open(configFile, 'rb')
ok_configs = pickle.load(pkl_file)
pkl_file.close()
ps = ProblemSolver(fullBody)
r = Viewer(ps)
r.loadObstacleModel('hpp-rbprm-corba', scene, "planning")
limbs = [larmId, rarmId, lLegId, rLegId]
q_init = [
0,
0,
0,
1.0,
0.0,
0.0,
0.0, # Free flyer 0-6
0.0,
0.0,
0.0,
0.0, # CHEST HEAD 7-10
robot = Robot ('ur5')
cl = robot.client
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
# q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF#
q1 = [-0.07, -1.3, 1.4, -0.8, 3.14, 0];
q2 = [-0.2, -1.65, -1.6, 0.4, 0.8, 0]
r(q1)
qcoll = [0.412954,-1.53138,0.55877,-1.09925,3.28763,-0.878376]
r.loadObstacleModel ("ur_description","table","table")
r.loadObstacleModel ("ur_description","obstacle_sphere","obstacle_sphere")
r(qcoll)
robot.isConfigValid(qcoll)
robot.isConfigValid(q1); robot.isConfigValid(q2)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-PRM.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-RRT.rdm')
ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
# ... and only if a contact surface includes the gravity
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean'])
# We also bound the rotations of the torso.
rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
r.loadObstacleModel (packageName, "darpa", "planning")
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance import Client
c = Client ()
c.affordance.analyseAll ()
q_goal [0:3] = [0.13, 0.05, 0.8]; r (q_goal)
#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
#~ afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005])
#~ afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005])
r.loadObstacleModel (packageName, "scene_wall", "planning")
afftool.analyseAll()
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
afftool.visualiseAffordances('Lean', r, [1, 0, 0])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
t = ps.solve ()
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()
from hpp.gepetto import PathPlayer
#robot.setJointBounds("base_joint_SO3",[1,1,0,0,0,0,0,0])
# room : -5,4,-7,5,0.5,0.5
robot.tf_root = 'base_link'
ps = ProblemSolver(robot)
v = Viewer(ps)
q_init = robot.getCurrentConfig()
q_init[0:3] = [9, 7.5, 0.5]
v(q_init)
q_goal = q_init[::]
q_goal[0:3] = [0, 1.5, 0.5]
#v (q_goal)
print("chargement map")
v.loadObstacleModel("iai_maps", "labyrinth2", "labyrinth2")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.selectPathPlanner("rrtConnect")
v.solveAndDisplay("rm1", 50, white, 0.02, 1, brown)
ps.solve()
v.displayRoadmap("rm1", white, 0.02, 1, brown)
v.client.gui.removeFromGroup("rm1", v.sceneName)
pp = PathPlayer(robot.client, v)
#print("affichage solution")
import math
import numpy as np
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-3.9, 3.9, -3.9, 3.9, 2, 9])
ps = ProblemSolver (robot)
cl = robot.client
# 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 = [-1.8, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [1.8, 2.7, 8, 0, 0, 0, 1, 0, 0, 1, 0] # theta ~= 0
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","plane_3d","plane_3d")
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)
r(q1)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
plotSphere (q2, cl, r, "sphere1", [0,1,0,1], 0.02)
nPointsPlot = 60
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
plotFrame (r, "_", [0,0,3.1], 0.5)
r(q_init)
#~ q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0)
q_goal[0:3] = [0.13, 0.05, 0.8]
r(q_goal)
#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.01)
r.loadObstacleModel(packageName, "scene_wall", "planning")
t = ps.solve()
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()
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
#~ pp (1)
import numpy as np
robot = Robot('robot')
robot.setJointBounds('base_joint_xyz',
[-2.6, 2.6, -6, 4.5, -2.5, 7]) # top end
ps = ProblemSolver(robot)
cl = robot.client
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [-0.18, 3.5, -0.11, 1, 0, 0, 0, 0, 0, 1, 0]
q22 = [0.27, -5.75, 5.90, 1, 0, 0, 0, 0, 0, 1, 0] # cave top end
from hpp.gepetto import Viewer, PathPlayer
r = Viewer(ps)
pp = PathPlayer(robot.client, r)
r.loadObstacleModel("animals_description", "cave", "cave")
#addLight (r, [0.2,-13.5,-2.3,1,0,0,0], "li"); addLight (r, [0,-5,-2.5,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
from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints, plotConesRoadmap
import math
import numpy as np
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.5, 3.2, 0, 8])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [6.2, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-4.4, -1.5, 6.5, 0, 0, 0, 1, 0, 0, 1, 0]
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps); gui = r.client.gui
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder")
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)
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
#plotFrame (r, 'frame_group', [0,0,0], 0.6)
# First parabolas: vmax = 7m/s, mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(7)
ps.clearRoadmap();
solveTime = ps.solve () # 299 nodes
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init[::]
q_goal[3:7] = [0.98877108, 0., 0.14943813, 0.]
q_goal[0:3] = [1.49, -0.65, 1.25]
r(q_goal)
#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
r.loadObstacleModel(packageName, "stair_bauzil", "planning")
#~ ps.solve ()
t = ps.solve()
print t
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()
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
