Commit fc249139 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add script with waypoint

parent 51d48a2d
......@@ -2,7 +2,7 @@ from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import downSlope_hrp2_pathKino as tp
import downSlope_hrp2_waypoint as tp
import time
......@@ -70,8 +70,8 @@ q_goal[configSize+3:configSize+6] = acc_goal[::]
# FIXME : test
q_init[2] = q_init[2]+0.15
q_goal[2] = q_goal[2]+0.15
q_init[2] = q_init[2]+0.1
q_goal[2] = q_goal[2]+0.1
fullBody.setStaticStability(False)
# Randomly generating a contact configuration at q_init
......@@ -94,21 +94,20 @@ fullBody.setStartState(q_init,[rLegId,lLegId])
fullBody.setEndState(q_goal,[rLegId,lLegId])
"""
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
"""
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
import fullBodyPlayerHrp2
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 0, filterStates = True)
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 1, filterStates = True)
print "number of configs :", len(configs)
r(configs[-1])
......
......@@ -34,7 +34,7 @@ extraDof = 6
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.25,2, -0.5, 5.5, 0.6, 1.8])
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,4, 0.5, 1.5, 0.2, 1.8])
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,4, 0.5, 1.5, 0.25, 1.8])
rbprmBuilder.setJointBounds('CHEST_JOINT0',[0,0])
rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.35,0.1])
rbprmBuilder.setJointBounds('HEAD_JOINT0',[0,0])
......@@ -56,6 +56,7 @@ indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDim
ps = ProblemSolver( rbprmBuilder )
ps.client.problem.setParameter("aMax",aMax)
ps.client.problem.setParameter("vMax",vMax)
ps.client.problem.setParameter("tryJump",vMax)
ps.client.problem.setParameter("sizeFootX",0.24)
ps.client.problem.setParameter("sizeFootY",0.14)
r = Viewer (ps)
......@@ -83,7 +84,7 @@ q_goal = q_init [::]
q_goal[3:7] = [1,0,0,0]
q_goal[8] = 0
q_goal [0:3] = [2, 1, 0.55]; r (q_goal)
q_goal [0:3] = [2.5, 1, 0.5]; r (q_goal)
r (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
......@@ -195,3 +196,23 @@ r(q_far)
# Manually add waypoints to roadmap:
"""
ps.client.problem.prepareSolveStepByStep()
pbCl = rbprmBuilder.client.basic.problem
q1= [0.6, 1, 0.5, 1, 0, 0, 0, 0.0, 0, 0.0, 0.0, 3, 0.0, -1.5, 0.0, 0.0, 0.0]
pbCl.addConfigToRoadmap (q1)
pbCl.directPath(q1,q_goal,True)
pbCl.directPath(q_init,q1,False)
r.client.gui.removeFromGroup("path_"+str(ps.numberPaths()-2)+"_root",r.sceneName)
pp.displayVelocityPath(ps.numberPaths()-1)
pbCl.addEdgeToRoadmap (q_init, q1, 1, False)
pbCl.addEdgeToRoadmap (q1, q_goal, 0, False)
"""
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
class Robot (Parent):
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName = 'hrp2_trunk_flexible'
urdfSuffix = ""
srdfSuffix = ""
def __init__ (self, robotName, load = True):
Parent.__init__ (self, robotName, self.rootJointType, load)
self.tf_root = "base_footprint"
self.client.basic = Client ()
self.load = load
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk_flexible'
urdfNameRom = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = 4;
aMax = 5;
extraDof = 6
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.25,2, -0.5, 5.5, 0.6, 1.8])
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,4, 0.5, 1.5, 0.25, 1.8])
rbprmBuilder.setJointBounds('CHEST_JOINT0',[0,0])
rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.35,0.1])
rbprmBuilder.setJointBounds('HEAD_JOINT0',[0,0])
rbprmBuilder.setJointBounds('HEAD_JOINT1',[0,0])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_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([-4,4,-1,1,-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",aMax)
ps.client.problem.setParameter("vMax",vMax)
ps.client.problem.setParameter("tryJump",vMax)
ps.client.problem.setParameter("sizeFootX",0.24)
ps.client.problem.setParameter("sizeFootY",0.14)
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] = [1,0,0,0]
q_init[8] = -0.2
q_init [0:3] = [-1.6, 1, 1.75]; 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] = [1,0,0,0]
q_goal[8] = 0
q_goal [0:3] = [3, 1, 0.55]; r (q_goal)
r (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Choosing RBPRM shooter and path validation methods.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("KinodynamicDistance")
ps.selectPathPlanner("DynamicPlanner")
#solve the problem :
r(q_init)
#r.solveAndDisplay("rm",1,0.01)
ps.client.problem.prepareSolveStepByStep()
pbCl = rbprmBuilder.client.basic.problem
q1= [0.2, 1, 0.5, 1, 0, 0, 0, 0.0, 0, 0.0, 0.0, 4, 0.0, -1.5, 0.0, 0.0, 0.0]
pbCl.addConfigToRoadmap (q1)
pbCl.directPath(q1,q_goal,True)
pbCl.directPath(q_init,q1,False)
pbCl.addEdgeToRoadmap (q_init, q1, 1, False)
pbCl.addEdgeToRoadmap (q1, q_goal, 0, False)
ps.client.problem.finishSolveStepByStep()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
pp.speed=0.1
pp.displayVelocityPath(2)
r.client.gui.setVisibility("path_2_root","ALWAYS_ON_TOP")
"""
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
"""
"""
for i in range(0,9):
t = ps.solve()
if isinstance(t, list):
ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.
f= open("/local/dev_hpp/logs/benchHrp2_slope_LP.txt","a")
f.write("t = "+str(ts) + "\n")
f.write("path_length = "+str(ps.client.problem.pathLength(i)) + "\n")
f.close()
print "problem "+str(i)+" solved \n"
ps.clearRoadmap()
"""
#ps.client.problem.prepareSolveStepByStep()
#ps.client.problem.finishSolveStepByStep()
q_far = q_init[::]
q_far[2] = -3
r(q_far)
"""
camera = [0.6293167471885681,
-9.560577392578125,
10.504343032836914,
0.9323806762695312,
0.36073973774909973,
0.008668755181133747,
0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
"""
r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmstart",r.sceneName)
r.client.gui.removeFromGroup("rmgoal",r.sceneName)
for i in range(0,ps.numberNodes()):
r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
"""
"""
# for seed 1486657707
ps.client.problem.extractPath(0,0,2.15)
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
pp.displayVelocityPath(1)
r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
#display path
pp.speed=0.3
#pp (0)
"""
#display path with post-optimisation
"""
q_far = q_init[::]
q_far[2] = -3
r(q_far)
"""
# Manually add waypoints to roadmap:
"""
r.client.gui.removeFromGroup("path_"+str(ps.numberPaths()-2)+"_root",r.sceneName)
pp.displayVelocityPath(ps.numberPaths()-1)
ps.client.problem.prepareSolveStepByStep()
pbCl = rbprmBuilder.client.basic.problem
q1= [0.6, 1, 0.5, 1, 0, 0, 0, 0.0, 0, 0.0, 0.0, 3, 0.0, -1.5, 0.0, 0.0, 0.0]
pbCl.addConfigToRoadmap (q1)
pbCl.directPath(q1,q_goal,True)
pbCl.directPath(q_init,q1,False)
pbCl.addEdgeToRoadmap (q_init, q1, 1, False)
pbCl.addEdgeToRoadmap (q1, q_goal, 0, False)
"""
......@@ -11,15 +11,15 @@ class Player(object):
self.configs=configs
self.rLegId = 'hrp2_rleg_rom'
self.lLegId = 'hrp2_lleg_rom'
self.rarmId = 'hrp2_rarm_rom'
#self.rarmId = 'hrp2_rarm_rom'
self.draw=draw
self.pathId = pathId
self.use_velocity = use_velocity
self.optim_effector = optim_effector
self.use_window = use_window
self.limbsCOMConstraints = { self.rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
self.lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
self.rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : 'RARM_JOINT5'} }
self.lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}
#self.rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : 'RARM_JOINT5'} }
def act(self,i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, time_scale = 1, verbose = True, draw = False, trackedEffectors = []):
......@@ -93,7 +93,7 @@ class Player(object):
for i in range(begin,end):
self.act(i,1,use_window=self.use_window,optim_effectors=self.optim_effector,draw=self.draw)
def play(self,frame_rate = 1./24.):
def play(self,frame_rate = 1./30.):
play_traj(self.fullBody,self.pp,frame_rate)
......@@ -136,7 +136,7 @@ def __getTimes(fullBody, configs, i, time_scale,use_window=0):
if t <= dt*6.:
print "WARNING : in getTime, t=0"
t = dt*6.
use_window = use_window+1
use_window = 2
times = [dt*2. , 0] #FIXME : hardcoded value depend on interpolation step choosen (not available here)
if t > dt*14.:
times = [dt*4. , 0]
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment