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)
#~ pp (1)
[-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]
#q_init [0:3] = [1, 1, 0.65]
rbprmBuilder.setCurrentConfig(q_init)
q_goal = q_init[::]
#q_goal[3:7] = [0.7071,0,0,0.7071]
#q_goal [0:3] = [1, 5, 0.65]; r(q_goal)
q_goal[3:7] = [1, 0, 0, 0]
q_goal[0:3] = [2, 1, 0.60]
0,
0,
0.1, # arm_right
0,
0,
0,
0,
0,
0,
0, # gripper_right
0,
0, # head
]
r(q_0)
r.addLandmark(r.sceneName, 1)
r.addLandmark("talos/gripper_left_inner_single_link", 0.3)
r.addLandmark("talos/gripper_right_inner_single_link", 0.3)
r.addLandmark("talos/left_sole_link", 0.3)
r.addLandmark("talos/right_sole_link", 0.3)
rLegId = "rleg"
rLeg = "leg_right_1_joint"
rfoot = "leg_right_sole_fix_joint"
rLegOffset = [0, 0, 0.01]
rLegNormal = [0, 0, 1]
rLegx = 0.06
rLegy = 0.1
fullBody.addLimb(rLegId, rLeg, rfoot, rLegOffset, rLegNormal, rLegx, rLegy,
nbSamples, "EFORT", 0.01)
