Skip to content
GitLab
Commit 523b464e authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[script] update scripts to change of robot-data location

parent f7829c9b
Hide whitespace changes
Inline Side-by-side
script/scenarios/demos/darpa_hyq.py
View file @ 523b464e
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.hyq import Robot
from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
#reference pose for hyq
from hyq_ref_pose import hyq_ref
#calling script darpa_hyq_path to compute root path
import darpa_hyq_path as tp
......@@ -16,18 +14,9 @@ db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
from hpp.corbaserver import Client
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody = Robot ()
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used
......@@ -37,40 +26,21 @@ ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps, viewerClient=tp.r.client)
rootName = 'base_joint_xyz'
cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
rfoot = 'rf_foot_joint'
offset = [0.,-0.021,0.]
normal = [0,1,0]
legx = 0.02; legy = 0.02
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "random", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "random", 0.05, cType)
#~
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "random", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random", 0.05, cType)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rHand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lHand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
"""
q_init = [-2.0,
0.0,
0.6838277139631803,
......@@ -90,14 +60,15 @@ q_init = [-2.0,
0.03995660287873871,
-0.9577096766517215,
0.9384602821326071]
q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=hyq_ref[2]+0.02
"""
q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]+0.02
q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]+0.02
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId,fullBody.rArmId,fullBody.lArmId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId,fullBody.rArmId,fullBody.lArmId])
r(q_init)
......
script/scenarios/demos/darpa_hyq_path.py
View file @ 523b464e
# Importing helper class for setting up a reachability planning problem
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.hyq_abstract import Robot
# Importing Gepetto viewer helper class
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName = 'hyq_trunk_large'
# URDF files describing the reachable workspace of each limb of HyQ
urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder = Robot ()
rbprmBuilder.setJointBounds ("root_joint", [-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 ...
......@@ -48,7 +38,7 @@ ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
afftool.loadObstacleModel (packageName, "darpa", "planning", r)
afftool.loadObstacleModel ("hpp-rbprm-corba", "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
......@@ -79,22 +69,6 @@ for i in range(1,10):
from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent
class Robot (Parent):
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName = 'hyq_trunk_large'
urdfSuffix = ""
srdfSuffix = ""
def __init__ (self, robotName, load = True):
Parent.__init__ (self, robotName, self.rootJointType, load)
self.tf_root = "base_footprint"
self.client = Client ()
self.load = load
#DEMO code to play root path and final contact plan
cl = Client()
cl.problem.selectProblem("rbprm_path")
......
script/scenarios/demos/hyq_ref_pose.py deleted 100644 → 0
View file @ f7829c9b
hyq_ref = [-1.652528468457493,
0.06758953014152885,
0.6638277139631803,
0.9995752585582991,
-0.016391515572502728,
-0.011295242081121508,
0.02128469407050025,
0.17905666752078864,
0.9253512562075908,
-0.8776870832724601,
0.11147422537786231,
-0.15843632504615043,
1.150049183494211,
-0.1704998924604114,
0.6859376445755911,
-1.1831277202117043,
0.06262698472369518,
-0.42708925470675,
1.2855999319965081]
script/scenarios/demos/talos_flatGround.py
View file @ 523b464e
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from display_tools import *
import time
......@@ -8,37 +8,10 @@ import talos_flatGround_path as tp
print "Done."
import time
##
# Information to retrieve urdf and srdf files.
packageName = "talos_data"
meshPackageName = "talos_data"
rootJointType = "freeflyer"
urdfName = "talos"
urdfSuffix = "_reduced"
srdfSuffix = ""
rLegId = 'talos_rleg_rom'
rleg = 'leg_right_1_joint'
rfoot = 'leg_right_6_joint'
lLegId = 'talos_lleg_rom'
lleg = 'leg_left_1_joint'
lfoot = 'leg_left_6_joint'
rArmId = 'talos_rarm_rom'
rarm = 'arm_right_1_joint'
rhand = 'arm_right_7_joint'
lArmId = 'talos_larm_rom'
larm = 'arm_left_1_joint'
lhand = 'arm_left_7_joint'
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody = Robot ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.95, 1.05])
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
......@@ -48,15 +21,7 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
q_ref = [
0.0, 0.0, 1.0232773, 0.0 , 0.0, 0.0, 1., #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
0.0 , 0.006761, #Chest
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1,-0.005, #Left Arm
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005,#Right Arm
0., 0. , #Head
0,0,0,0,0,0]; v (q_ref)
q_ref = fullBody.referenceConfig[::]+[0]*6
q_init = q_ref[::]
......@@ -69,20 +34,12 @@ tStart = time.time()
# generate databases :
nbSamples = 10000
rLegOffset = [0., -0.00018, -0.107]
rLegOffset[2] += 0.006
rLegNormal = [0,0,1]
rLegx = 0.1; rLegy = 0.06
fullBody.addLimb(rLegId,rleg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset = [0., -0.00018, -0.107]
lLegOffset[2] += 0.006
lLegNormal = [0,0,1]
lLegx = 0.1; lLegy = 0.06
fullBody.addLimb(lLegId,lleg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
......@@ -125,8 +82,8 @@ v(q_goal)
v.addLandmark('talos/base_link',0.3)
v(q_init)
fullBody.setStartState(q_init,[rLegId,lLegId])
fullBody.setEndState(q_goal,[rLegId,lLegId])
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
from hpp.gepetto import PathPlayer
......@@ -134,7 +91,7 @@ pp = PathPlayer ( v)
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = False)
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=False)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
......
script/scenarios/demos/talos_flatGround_path.py
View file @ 523b464e
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.talos_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
......@@ -6,21 +6,13 @@ import time
rootJointType = 'freeflyer'
packageName = 'talos-rbprm'
meshPackageName = 'talos-rbprm'
urdfName = 'talos_trunk'
urdfNameRom = ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = 0.3
aMax = 0.1
extraDof = 6
mu=0.5
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder = Robot ()
rbprmBuilder.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.95, 1.05])
......@@ -45,14 +37,6 @@ ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps.setParameter("DynamicPlanner/friction",0.5)
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
p_lLeg = [-0.008846952891378526, 0.0848172440888579,-1.019272022956703]
p_lLeg[0]=0. # assure symetry of dynamic constraints on flat ground
p_rLeg = [-0.008846952891378526,-0.0848172440888579,-1.019272022956703]
p_rLeg[0] = 0.
p_lArm = [0.13028765672452458, 0.44360498616312666,-0.2881211563246389]
p_rArm = [0.13028765672452458,- 0.44360498616312666,-0.2881211563246389]
rbprmBuilder.setReferenceEndEffector('talos_lleg_rom',p_lLeg)
rbprmBuilder.setReferenceEndEffector('talos_rleg_rom',p_rLeg)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
......@@ -62,7 +46,7 @@ afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel ("hpp-rbprm-corba", "ground", "planning", vf)
v = vf.createViewer(displayArrows = True)
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
......
script/scenarios/demos/talos_navBauzil.py
View file @ 523b464e
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from display_tools import *
import time
......@@ -8,37 +8,11 @@ import talos_navBauzil_path as tp
print "Done."
import time
##
# Information to retrieve urdf and srdf files.
packageName = "talos_data"
meshPackageName = "talos_data"
rootJointType = "freeflyer"
urdfName = "talos"
urdfSuffix = "_reduced"
srdfSuffix = ""
rLegId = 'talos_rleg_rom'
rleg = 'leg_right_1_joint'
rfoot = 'leg_right_6_joint'
lLegId = 'talos_lleg_rom'
lleg = 'leg_left_1_joint'
lfoot = 'leg_left_6_joint'
rArmId = 'talos_rarm_rom'
rarm = 'arm_right_1_joint'
rhand = 'arm_right_7_joint'
lArmId = 'talos_larm_rom'
larm = 'arm_left_1_joint'
lhand = 'arm_left_7_joint'
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody = Robot ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.2
root_bounds[1] += 0.2
......@@ -55,18 +29,9 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
q_ref = [
0.0, 0.0, 1.0232773, 0.0 , 0.0, 0.0, 1., #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
0.0 , 0.006761, #Chest
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1,-0.005, #Left Arm
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005,#Right Arm
0., 0. , #Head
0,0,0,0,0,0]; v (q_ref)
q_ref = fullBody.referenceConfig[::]+[0,0,0,0,0,0]
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
......@@ -76,20 +41,12 @@ tStart = time.time()
# generate databases :
nbSamples = 10000
rLegOffset = [0., -0.00018, -0.107]
rLegOffset[2] += 0.006
rLegNormal = [0,0,1]
rLegx = 0.1; rLegy = 0.06
fullBody.addLimb(rLegId,rleg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset = [0., -0.00018, -0.107]
lLegOffset[2] += 0.006
lLegNormal = [0,0,1]
lLegx = 0.1; lLegy = 0.06
fullBody.addLimb(lLegId,lleg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
......@@ -132,8 +89,8 @@ v(q_goal)
v.addLandmark('talos/base_link',0.3)
v(q_init)
fullBody.setStartState(q_init,[lLegId,rLegId])
fullBody.setEndState(q_goal,[lLegId,rLegId])
fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId])
fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
from hpp.gepetto import PathPlayer
......@@ -141,7 +98,7 @@ pp = PathPlayer ( v)
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = False)
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=False)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
......
script/scenarios/demos/talos_navBauzil_hard.py 0 → 100644
View file @ 523b464e
from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from display_tools import *
import time
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
print "Plan guide trajectory ..."
import talos_navBauzil_hard_path as tp
print "Done."
import time
##
# Information to retrieve urdf and srdf files.
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.2
root_bounds[1] += 0.2
root_bounds[2] -= 0.2
root_bounds[3] += 0.2
root_bounds[-1] = 1.2
root_bounds[-2] = 0.8
fullBody.setJointBounds ("root_joint", root_bounds)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
q_ref = fullBody.referenceConfig[::] + [0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 10000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
q_0 = fullBody.getCurrentConfig();
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 3
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = dir_goal[::]
q_goal[configSize+3:configSize+6] = [0,0,0]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
# specifying the full body configurations as start and goal state of the problem
v.addLandmark('talos/base_link',0.3)
v(q_init)
fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId])
fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
from hpp.gepetto import PathPlayer
pp = PathPlayer ( v)
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = False)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
raw_input("Press Enter to display the contact sequence ...")
displayContactSequence(v,configs)
script/scenarios/demos/talos_navBauzil_hard_path.py
View file @ 523b464e
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.talos_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
rootJointType = 'freeflyer'
packageName = 'talos-rbprm'
meshPackageName = 'talos-rbprm'
urdfName = 'talos_trunk'
urdfNameRom = ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = 0.3
aMax = 0.1
extraDof = 6
mu=0.5
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder = Robot ()
root_bounds = [-2.3,4.6,-1.5,3.3, 0.98, 0.98]
rbprmBuilder.setJointBounds ("root_joint", root_bounds)
rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
#rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
#rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
......@@ -47,17 +37,8 @@ ps.setParameter("DynamicPlanner/sizeFootX",0.2)
ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps.setParameter("DynamicPlanner/friction",mu)
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)