Commit 916029ca authored by Steve Tonneau's avatar Steve Tonneau
Browse files

updated profile scenarios

parent 7cb806bd
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
#calling script darpa_hyq_path to compute root path
import darpa_hyq_path as tp
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
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.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used
nbSamples = 20000
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_3_DOF"
# string identifying the limb
rLegId = 'rfleg'
# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfoot = 'rf_foot_joint'
# Specifying the distance between last joint and contact surface
offset = [0.,-0.021,0.]
# Specifying the contact surface direction when the limb is in rest pose
normal = [0,1,0]
# Specifying the rectangular contact surface length
legx = 0.02; legy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
#~ addLimbDb(rLegId, "static")
#~ addLimbDb(lLegId, "static")
#~ addLimbDb(rarmId, "static")
#~ addLimbDb(larmId, "static")
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
#~
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])
#~ from pickle import load
#~ f = open("config_"+str(tp.config_i), 'r')
#~ q_init = load(f)
#~ f.close()
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[])
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
# computing the contact sequence
#~ configs = fullBody.interpolate(0.05, 10, 1, True)
configs = fullBody.interpolate(0.08, 10, 10, True)
#~ configs = fullBody.interpolate(0.11, 7, 10, True)
#~ configs = fullBody.interpolate(0.1, 1, 10, True)
#~ configs = fullBody.interpolate(0.02, 10, 10, True)
import time
try:
time.sleep(2)
fullBody.dumpProfile()
except Exception as e:
pass
......@@ -35,9 +35,21 @@ r = Viewer (ps)
# 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_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init [0:3] = [-1.8, 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)
config_i = 0
#~ from pickle import load
#~ f = open("config_"+str(config_i), 'r')
#~ q_init [0:7] = load(f)[0:7]
#~ f.close()
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
......@@ -58,14 +70,12 @@ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Solve the problem
t = ps.solve ()
print t;
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()
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
......@@ -74,3 +84,6 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
r(q_far)
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
......@@ -58,15 +58,46 @@ lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
addLimbDb(rLegId, "static")
addLimbDb(lLegId, "static")
addLimbDb(rarmId, "static")
addLimbDb(larmId, "static")
#~ addLimbDb(rLegId, "static")
#~ addLimbDb(lLegId, "static")
#~ addLimbDb(rarmId, "static")
#~ addLimbDb(larmId, "static")
lfoot = 'lh_foot_joint'
rHand = 'rh_foot_joint'
lHand = 'lf_foot_joint'
#~ lLegId = 'lhleg'
#~ rarmId = 'rhleg'
#~ larmId = 'lfleg'
#~
#~ addLimbDb(rLegId, "static")
#~ addLimbDb(lLegId, "static")
#~ addLimbDb(rarmId, "static")
#~ addLimbDb(larmId, "static")
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
#~
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
......@@ -84,49 +115,10 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
configs = fullBody.interpolate(0.08,1,3) #hole
#~ configs = fullBody.interpolate(0.08,1,5) # bridge
#~ r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
i = 0;
r (configs[i]); i=i+1; i-1
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj, profile
limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},
lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},
rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
def act(i, numOptim = 0):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
def saveAll(name):
saveAllData(fullBody, r, name)
#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
#~ profile(fullBody, configs, 4, 10, limbsCOMConstraints)
profile(fullBody, configs, 4, len(configs)-4, limbsCOMConstraints)
import time
try:
time.sleep(2)
fullBody.dumpProfile()
except Exception as e:
pass
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import polaris_hrp2_path_no_step as tp
from numpy import array, matrix
import numpy as np
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,2, -2, 1, 0.5, 2.5])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.03, "_6_DOF")
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.115,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03, "_6_DOF")
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.040,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.040,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
#~
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
confsize = len(tp.q_init)
q_init = [
#~ 0.12, -0.45, 0.95, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.36, 0, 1.01, 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, -1, 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);
q_init[0:confsize] = tp.q_init[0:confsize]
q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize]
fullBody.setCurrentConfig (q_init)
#~ q_0 = fullBody.getCurrentConfig();
q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
fullBody.setCurrentConfig (q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
#~ r(q_goal)
#~ gui = r.client.gui
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setStartState(q_init,[rLegId,lLegId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])
#~
#~ r(q_init)
configs = fullBody.interpolate(0.03, 10, 5, True)
import time
try:
time.sleep(2)
fullBody.dumpProfile()
except Exception as e:
pass
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk_flexible'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, 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'])
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] = [0.36, -0.32, 1.01]; r(q_init)
q_init[0:3] = [0.36, 0, 1.01]; 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.5]; r(q_goal)
q_goal[0:3] = [0.36, -1, 0.9]; r(q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005])
afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005])
afftool.loadObstacleModel (packageName, "polaris", "planning", r)
#~ afftool.analyseAll()
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [1, 0, 0])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
t = ps.solve ()
print t;
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()
#~ print ("solving time " + str(t));
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (0)
#~ pp (1)
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
rob = rbprmBuilder.client.basic.robot
r(q_init)
q_loin = q_init[::]
r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
#~ q_loin[0:3] = [100,100,100]
#~ r (q_loin)
#~ configs = []
#~ problem = ps.client.problem
#~ length = problem.pathLength (0)
#~ t = 0
#~ i = 0
#~ configs = []
#~ dt = 0.1 / length
#~ while t < length :
#~ q = rbprmBuilder.getCurrentConfig()
#~ q[0:9]= problem.configAtParam (0, t)[0:9]
#~ configs.append(q)
#~ t += dt
#~ i = i+1
#~
#~ i = 0;
#~ r(configs[i]); i = +1
......@@ -131,6 +131,12 @@ def play(frame_rate = 1./24.):
def saveAll(name):
saveAllData(fullBody, r, name)
import time
try:
time.sleep(2)
fullBody.dumpProfile()
except Exception as e:
pass
#~ for i in range(4,25):
#~ act(i,0)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import standing_hrp2_path as tp
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,1, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.03, "_6_DOF")
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.115,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03, "_6_DOF")
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.040,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.040,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
#~
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
confsize = len(tp.q_init)
q_init = fullBody.getCurrentConfig(); q_init[0:confsize] = tp.q_init[0:confsize]
q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize]
fullBody.setCurrentConfig (q_init)
#~ q_0 = fullBody.getCurrentConfig();
q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
fullBody.setCurrentConfig (q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
#~ r(q_goal)
#~ gui = r.client.gui
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
#~
#~ r(q_init)
configs = fullBody.interpolate(0.1)
import time
try:
time.sleep(2)
fullBody.dumpProfile()
except Exception as e:
pass
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer