Skip to content
GitLab
Commit 52645b4a authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add (non working) scripts for pyrene in airbus stairs

parent 39e5a2a0
Hide whitespace changes
Inline Side-by-side
script/dynamic/talos/airbus_no_plane_bigStairs_interp.py 0 → 100644
View file @ 52645b4a
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import time
#from constraint_to_dae import *
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
#from display_tools import *
import talos.airbus_no_plane_bigStairs_path as tp
import time
from planning.robot_config.talos import *
from display_tools import *
tPlanning = tp.tPlanning
##
# Information to retrieve urdf and srdf files.
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-13,10,-3.85,-3.75,-1.75,0.5])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.basic.robot.setExtraConfigSpaceBounds([-0,0,-0,0,-0,0,0,0,0,0,0,0])
ps = tp.ProblemSolver( fullBody )
ps.client.problem.setParameter("aMax",tp.aMax)
ps.client.problem.setParameter("vMax",tp.vMax)
r = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True)
q_ref = [
0.0, 0.0, 1.0232773, 1 , 0.0, 0.0, 0.0, #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]; r (q_ref)
"""
# with arms in front
q_ref = [
0.0, 0.0, 1.0232773, 1 , 0.0, 0.0, 0.0, #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.5 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1,-0.005, #Left Arm
0.5 , -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]; r (q_ref)
"""
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
"""
#test correspondance with reduced :
q_init[19] = -0.5
q_init[20] = 0.8
r(q_init)
"""
fullBody.setCurrentConfig (q_init)
qfar=q_ref[::]
qfar[2] = -5
tStart = time.time()
# generate databases :
nbSamples = 50000
rLegOffset = MRsole_offset.translation.transpose().tolist()[0]
rLegOffset[2] += 0.006
rLegNormal = [0,0,1]
rLegx = 0.1; rLegy = 0.06
fullBody.addLimb(rLegId,rleg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "fixedStep06", 0.01)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset = MLsole_offset.translation.transpose().tolist()[0]
lLegOffset[2] += 0.006
lLegNormal = [0,0,1]
lLegx = 0.1; lLegy = 0.06
fullBody.addLimb(lLegId,lleg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "fixedStep06", 0.01)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
rArmOffset = [0.055,-0.04,-0.13]
rArmNormal = [0,0,1]
rArmx = 0.005; rArmy = 0.005
fullBody.addLimb(rArmId,rarm,rhand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.01,"_6_DOF", False)
fullBody.runLimbSampleAnalysis(rArmId, "ReferenceConfiguration", True)
lArmOffset = [0.055,0.04,-0.13]
lArmNormal = [0,0,1]
lArmx = 0.005; lArmy = 0.005
fullBody.addLimb(lArmId,larm,lhand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "manipulability", 0.01,"_6_DOF", False)
fullBody.runLimbSampleAnalysis(lArmId, "ReferenceConfiguration", True)
"""
# load databases from files :
fullBody.addLimbDatabase("./db/talos_rLeg_walk.db",rLegId,"fixedStep06")
fullBody.addLimbDatabase("./db/talos_lLeg_walk.db",lLegId,"fixedStep06")
"""
tGenerate = time.time() - tStart
print "generate databases in : "+str(tGenerate)+" s"
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
configSize = fullBody.getConfigSize() -fullBody.client.basic.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 = 1
# 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]
q_init[2] = -1.57
fullBody.setStaticStability(False)
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
r(q_init)
#q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
r(q_init)
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
r(q_goal)
# specifying the full body configurations as start and goal state of the problem
r.addLandmark('talos/base_link',0.3)
r(q_init)
fullBody.setStartState(q_init,[rLegId,lLegId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rArmId,lArmId])
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
tStart = time.time()
configsFull = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 0, filterStates = True,testReachability=False)
tInterpolateConfigs = time.time() - tStart
print "number of configs :", len(configsFull)
displayContactSequence(r,configsFull)
"""
from planning.configs.talos_flatGround import *
from generate_contact_sequence import *
beginState = 0
endState = len(configsFull)-2
configs=configsFull[beginState:endState+1]
cs = generateContactSequence(fullBody,configs,beginState, endState,r)
#player.displayContactPlan()
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs.saveAsXML(filename, "ContactSequence")
print "save contact sequence : ",filename
"""
createSphere('s',r,color=r.color.red)
q_init[2] = -1.57
s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
#s1,success = StateHelper.addNewContact(s0,rLegId,p,[0,0,1])
#assert(success)
#r.addLandmark('talos/arm_left_7_link',0.2)
p = [-12.3,-3.26,-1.2]
moveSphere('s',r,p)
s1 = s0
p = [-12.55,-3.26,-1.65]
s2,success = StateHelper.addNewContact(s1,lArmId,p,[0,0,1],10000)
r(s2.q())
p = [-12.55,-4.32,-1.65]
s3,success = StateHelper.addNewContact(s2,rArmId,p,[0,0,1],10000)
r(s3.q())
s3.projectToCOM(s0.getCenterOfMass(),10000)
r(s3.q())
p = [-12.5, -3.65, -2.39] # l foot first step
s4,success = StateHelper.addNewContact(s3,lLegId,p,[0,0,1],10000)
r(s4.q())
script/dynamic/talos/airbus_no_plane_bigStairs_path.py 0 → 100644
View file @ 52645b4a
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
import omniORB.any
from planning.configs.talos_airbus_bigStairs import *
import time
class Robot (Parent):
rootJointType = 'freeflyer'
packageName = 'talos-rbprm'
meshPackageName = 'talos-rbprm'
# URDF file describing the trunk of the robot HyQ
urdfName = 'talos_trunk'
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 = 'talos-rbprm'
meshPackageName = 'talos-rbprm'
urdfName = 'talos_trunk'
urdfNameRom = ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = omniORB.any.to_any(0.1)
aMax = omniORB.any.to_any(0.1)
aMaxZ = aMax
#aMax = omniORB.any.to_any(0.3);
extraDof = 6
mu=omniORB.any.to_any(MU)
# 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", [-13,10,-3.8,-3.8,-1.75,0.5])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['talos_lleg_rom','talos_rleg_rom','talos_larm_rom','talos_rarm_rom'])
rbprmBuilder.setAffordanceFilter('talos_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('talos_larm_rom', ['Support45'])
rbprmBuilder.setAffordanceFilter('talos_rarm_rom', ['Support45'])
# 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([-0.1,0.1,0,0,-0.1,0.1,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("aMaxZ",aMaxZ)
ps.client.problem.setParameter("vMax",vMax)
ps.client.problem.setParameter("sizeFootX",omniORB.any.to_any(0.2))
ps.client.problem.setParameter("sizeFootY",omniORB.any.to_any(0.12))
ps.client.problem.setParameter("friction",omniORB.any.to_any(3.))
# set parameter for approximation of contact points :
p_lLeg = [-0.008846952891378526, 0.0848172440888579,-1.019272022956703]
p_rLeg = [-0.008846952891378526, -0.0848172440888579,-1.019272022956703]
p_lArm = [0.13028765672452458, 0.44360498616312666, -0.2881211563246389]
p_rArm = [0.13028765672452458,- 0.44360498616312666, -0.2881211563246389]
ps.client.problem.setParameter(lLegId+"_ref_x",omniORB.any.to_any(p_lLeg[0]))
ps.client.problem.setParameter(lLegId+"_ref_y",omniORB.any.to_any(p_lLeg[1]))
ps.client.problem.setParameter(lLegId+"_ref_z",omniORB.any.to_any(p_lLeg[2]))
ps.client.problem.setParameter(rLegId+"_ref_x",omniORB.any.to_any(p_rLeg[0]))
ps.client.problem.setParameter(rLegId+"_ref_y",omniORB.any.to_any(p_rLeg[1]))
ps.client.problem.setParameter(rLegId+"_ref_z",omniORB.any.to_any(p_rLeg[2]))
ps.client.problem.setParameter(lArmId+"_ref_x",omniORB.any.to_any(p_lArm[0]))
ps.client.problem.setParameter(lArmId+"_ref_y",omniORB.any.to_any(p_lArm[1]))
ps.client.problem.setParameter(lArmId+"_ref_z",omniORB.any.to_any(p_lArm[2]))
ps.client.problem.setParameter(rArmId+"_ref_x",omniORB.any.to_any(p_rArm[0]))
ps.client.problem.setParameter(rArmId+"_ref_y",omniORB.any.to_any(p_rArm[1]))
ps.client.problem.setParameter(rArmId+"_ref_z",omniORB.any.to_any(p_rArm[2]))
r = Viewer (ps,displayArrows=True)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.2])
afftool.loadObstacleModel (ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r,reduceSizes=[0.12,0,0.03])
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r, r.color.lightBrown)
afftool.visualiseAffordances('Support45', r, r.color.lightRed)
r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init[3:7] = [1,0,0,0]
q_init[0:3] = [-12.72,-3.8,-1.6]; r (q_init)
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
#q_goal[0:3] = [-10.85,-3.8,0.4]; r (q_goal) #upstair
q_goal[0:3] = [-11.95,-3.8,-0.7]; r (q_goal) #mid
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("RbprmDynamicPathValidation",0.05)
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("KinodynamicDistance")
ps.selectPathPlanner("DynamicPlanner")
ps.selectPathProjector('Progressive',0.05)
#solve the problem :
r(q_init)
#r.solveAndDisplay("rm",1,0.01)
#ps.client.problem.prepareSolveStepByStep()
tStart=time.time()
t = ps.solve ()
tPlanning = time.time() -tStart
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
pp.displayVelocityPath(0)
r.client.gui.setVisibility("path_0_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] = -5
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
# 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)
"""
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