def init_viewer(self):
"""
Create a Viewer instance
"""
self.v = Viewer(self.ps,
viewerClient=self.path_planner.v.client,
displayCoM=True)
ps.pathLength(ps.numberPaths() - 1)
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])
from hpp.corbaserver.puzzle import Robot
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')
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [
'Support',
])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0., 0, -1, 1, -1, 1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
f = open("scale_1_save", "r+")
from pickle import load
q = load(f)
f.close()
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = q[3:7]
q_init[0:3] = q[0:3]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
#~ q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ 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. ]
# 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)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
pId = ps.numberPaths() - 1
from hpp.corbaserver import Client
#~ #DEMO code to play root path and final contact plan
cl = Client()
cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot("toto")
ps2 = ProblemSolver(rbprmBuilder2)
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(pId, "rbprm_path",
rbprmBuilder.getAllJointNames()[1:])
r2 = Viewer(ps2, viewerClient=v.client)
r2(q_far)
#~ v(q_far)
packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [-1.3, 1, -0.5, 0.5, 0, 0.9])
rbprmBuilder.setFilter(
['hrp2_larm_rom', 'hrp2_rarm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [-1,0,0], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [-1,0,0], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.5)
rbprmBuilder.boundSO3([-0.4, 0.4, -3, 3, -3, 3])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
q0 = rbprmBuilder.getCurrentConfig()
q_init = rbprmBuilder.getCurrentConfig()
r(q_init)
q_goal = q_init[::]
q_goal[0:3] = [0.19, 0.05, 0.9]
r(q_goal)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
rbprmBuilder.client.basic.robot.setJointConfig(
'base_joint_SO3', [0.7316888688738209, 0, 0.6816387600233341, 0])
q_init = rbprmBuilder.getCurrentConfig()
r(q_init)
q_init = rbprmBuilder.getCurrentConfig()
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
# 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 ()
def __init__(self, problemSolver, viewerClient=None, collisionURDF=False):
self.compositeRobotName = problemSolver.robot.client.basic.robot.getRobotName(
)
Parent.__init__(self, problemSolver, viewerClient, collisionURDF)
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import Viewer
from hpp.gepetto import PathPlayer
# define colors for the roadmap
white=[1.0,1.0,1.0,1.0]
green=[0.23,0.75,0.2,0.5]
brown=[0.85,0.75,0.15,0.5]
blue = [0.0, 0.0, 0.8, 1.0]
grey = [0.7,0.7,0.7,1.0]
red = [0.8,0.0,0.0,1.0]
robot = Robot ('pr2')
robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3])
ps = ProblemSolver (robot)
v = Viewer (ps)
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
v (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
from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles
from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones
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] # environment_3d mesh
r(q22)
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, [-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(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])
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2, -1.5, 1, 0.5, 0.9])
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom','hrp2_larm_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
rbprmBuilder.boundSO3([-1.5,1.5,0,3,-0.0,0.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q0 = rbprmBuilder.getCurrentConfig ();
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
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)
lArmNormal = [1,0,0]
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])
# 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()
#------------------------------------------------------------------
# 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)
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [0, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ 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")
from hpp.corbaserver import ProblemSolver
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)
from hpp.corbaserver.pr2 import Robot
robot = Robot("pr2")
robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
from hpp.gepetto import Viewer
r = Viewer(ps)
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)
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
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)
#/usr/bin/env python
# Script which goes with ur_description package.
# Load 6-DoF arm robot to test methods.
# b /local/mcampana/devel/hpp/src/hpp-fcl/src/narrowphase/narrowphase.cpp:317
from hpp.corbaserver.ur5_robot import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import numpy as np
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)
import time
white = [1.0, 1.0, 1.0, 1.0]
green = [0.23, 0.75, 0.2, 0.5]
brown = [0.85, 0.75, 0.15, 0.5]
print("chargement robot")
robot = Robot('robot_boule', True)
robot.setJointBounds("base_joint_xyz", [0, 9, 0, 12, 0.5, 0.5])
#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")
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
scene = sys.argv[len(sys.argv)-1]
tested = Builder ()
tested.loadModel(urdfNameTested, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
#~ tested.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ tested.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
ps = ProblemSolver( tested )
r = Viewer (ps)
r.loadObstacleModel (packageName, scene, "planning")
tested.setJointBounds ("base_joint_xyz", [-10.,10,-10,10,0,20])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
q_init = tested.getCurrentConfig ();
q_init [0:3] = [-10, -0.82, 1.25]; tested.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [-9, -0.65, 1.25]; r (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t = ps.solve ()
res = {}
x_start = 0
# Easy way to test planning algo (no internal DoF) on SO3 joint.
# 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])
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.3,1, -0.5, 0.5, 0, 0.9])
rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support','Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support','Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q0 = rbprmBuilder.getCurrentConfig ();
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [0.19, 0.05, 0.9]; r (q_goal)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-1, 0.05, 0.4]; rbprmBuilder.setCurrentConfig (q_init); 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)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
[-4.5, 4.5, 0, 0, -2, 2, 0, 0, 0, 0, 0, 0])
indexECS = rbprmBuilder.getConfigSize(
) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver and the viewer
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax))
ps.client.problem.setParameter("aMaxZ", omniORB.any.to_any(10.))
ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax))
ps.client.problem.setParameter("tryJump", omniORB.any.to_any(1.))
ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24))
ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14))
ps.client.problem.setTimeOutPathPlanning(3600)
r = Viewer(ps, displayArrows=True)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(packageName, "downSlope", "planning", r)
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
r.addLandmark(r.sceneName, 1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = [1, 0, 0, 0]
q_init[8] = -0.2
q_init[0:3] = [-1.6, 1, 1.75]
r(q_init)
def __init__ (self, problemSolver, viewerClient = None) :
Parent.__init__ (self, problemSolver, viewerClient)
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
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 ()
from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles
from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones
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
from viewer_display_library import normalizeDir, plotVerticalCone, plotCone, plotPath, plotVerticalConeWaypoints, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath
from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles
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 ()
from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones
import math
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
#/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)
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import Viewer
from hpp.gepetto import PathPlayer
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 ()
robot.setJointBounds('base_joint_xyz', [-6, 6, -6, 6, 2, 9]) # environment_3d
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 ...
def __init__ (self, problemSolver, viewerClient = None) :
Parent.__init__ (self, problemSolver, viewerClient)
self.compositeRobotName = self.robot.client.basic.robot.getRobotName()
self.client.gui.createGroup (self.compositeRobotName)
self.client.gui.addToGroup (self.displayName, self.compositeRobotName)
#/usr/bin/env python
# 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
robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.5, 3.2, 0, 8]) # ultimate goal!
#robot.setJointBounds('base_joint_xyz', [1.6, 6.8, -2.2, 1.5, 0, 3]) # first goal
#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
lArmNormal = [1, 0, 0]
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,
#/usr/bin/env python
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.squel_robot import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient
import time
robot = Robot('squel_robot')
robot.setJointBounds('base_joint_xyz', [-3, 3, -3, 3, 0, 1])
ps = ProblemSolver(robot)
cl = robot.client
Viewer.withFloor = True
r = Viewer(ps)
#r.loadObstacleModel ("room_description","room","room")
#r.loadObstacleModel ("room_description","walls","walls")
#pp = PathPlayer (cl, r)
#r.loadObstacleModel ("room_description","squel","squel")
# Add constraints
"""
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
from hpp.corbaserver.pr2 import Robot
robot = Robot('pr2', False)
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
from gepetto.corbaserver import Client as GuiClient
guiClient = GuiClient()
from hpp.gepetto import Viewer
Viewer.sceneName = '0_scene_hpp_'
r = Viewer(ps, guiClient)
from hpp.gepetto import PathPlayer
pp = PathPlayer(robot.client, r)
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
import time
import sys
sys.path.append('/local/mcampana/devel/hpp/src/test-hpp/script')
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)
# Difficult init config
q3 = [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]
q4 = [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]
# Load obstacles in HPP
cl.obstacle.loadObstacleModel('gravity_description','gravity_decor','') # do not move (position in urdf)
cl.obstacle.loadObstacleModel('gravity_description','emu','') # loaded as an obstacle for now
position_emu= (0,-1,0.2,1,0,0,0)
cl.obstacle.moveObstacle ('emu_base', position_emu)
from hpp.corbaserver import *
from hpp.corbaserver.nassime import RobotChaine
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 Viewer
import time
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,0.5]
print("chargement robot")
robot = Robot ('robot_boule', True)
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")
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 ('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
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()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
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 ()
#plt = addCorbaPathPlot (cl, 0, 'rk', 1.6, plt)
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_"
rbprmBuilder.setAffordanceFilter('0rLeg', [
'Support',
])
rbprmBuilder.setAffordanceFilter('1lLeg', ['Support'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0., 0, -1, 1, -1, 1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [1.49, -0.65, 1.25]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
#~ q_init [0:3] = [0, 0, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init[3:7] = [0, 0, 0, 1]
#~ 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] = [0, 0, 0.648702]
r(q_goal)
#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
cl.problem.setAlphaInit (0.3)
ps.optimizePath (0); tGB3 = cl.problem.getTimeGB ()
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 = []
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
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)
from hpp.corbaserver.rod import Robot
robot = Robot('rod')
robot.setJointBounds("base_joint_xy", [-3, 3, -2, 2])
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()
packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [0, 2, -1.4, 1.01, 0, 1.01])
rbprmBuilder.setFilter(['hrp2_larm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom'])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support', 'Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support', 'Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
rbprmBuilder.boundSO3([-0.7, 0.7, 0, 0, -0.0, 0.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
q0 = rbprmBuilder.getCurrentConfig()
q_init = rbprmBuilder.getCurrentConfig()
r(q_init)
q_goal = q_init[::]
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [1.1035428951774233, -1.060021251429404, 0.68]
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.9, -1.1, 0.6]; r(q_goal)
q_goal[0:3] = [1.5, -1.1, 0.57]
r(q_goal)
rbprmBuilder.setJointBounds("base_joint_xyz", [0, 2.2, -1, 1, 0.7, 2.5])
#~ rbprmBuilder.setJointBounds ("base_joint_xyz", [-20,20.2, -10, 10, 0.7, 2.5])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setFilter(['hrp2_lleg_rom', 'hrp2_rleg_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
rbprmBuilder.boundSO3([-0.1, 0.1, -3, 3, -1.0, 1.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
q0 = rbprmBuilder.getCurrentConfig()
q_init = rbprmBuilder.getCurrentConfig()
r(q_init)
q_goal = q_init[::]
q_goal[0:3] = [0.19, 0.05, 0.9]
r(q_goal)
rbprmBuilder.client.basic.robot.setJointConfig(
'base_joint_SO3', [0.7316888688738209, 0, 0.6816387600233341, 0])
q_init = rbprmBuilder.getCurrentConfig()
r(q_init)
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:9] = [0.27, -0.05, 1.2, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0472]
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', [
'Support',
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.1, 0.1, -0.65, 0.65, -0.2, 0.2])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
[-vMax, vMax, -1, 1, -2, 2, 0, 0, 0, 0, 0, 0])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", aMax)
ps.client.problem.setParameter("vMax", vMax)
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(packageName, "downSlope", "planning", r)
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
r.addLandmark(r.sceneName, 1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = [0.9659, 0, 0.2588, 0]
q_init[0:3] = [-1.25, 1, 1.7]
r(q_init)
#q_init[3:7] = [0.7071,0,0,0.7071]
#from hpp.corbaserver.ant import Robot
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)
