Skip to content
GitLab
Commit 31b586cd authored by stevet's avatar stevet
Browse files

hrp2 plan working

parent 5a3c5f97
Hide whitespace changes
Inline Side-by-side
script/scenarios/sandbox/geom.py 0 → 100644
View file @ 31b586cd
from numpy import array, cross
from numpy.linalg import norm
def flat(pts):
return [item for sublist in pts for item in sublist]
__EPS = 1e-5
def __filter_points(points):
res = []
for el in points:
el_arr = array(el)
add = True
for al in res:
if(norm(al - el_arr) < __EPS):
add = False
break
if add:
res += [array(el)]
return res
def __normal(points):
p1 = array(points[0])
p2 = array(points[1])
p3 = array(points[2])
normal = cross((p2 - p1),(p3 - p1))
normal /= norm(normal)
return normal.tolist()
def __centroid(points):
return sum(points) / len(points)
def __centroid_list(list_points):
return [[__centroid(__filter_points(flat(pts))).tolist(), __normal(pts[0]) ] for pts in list_points]
def computeAffordanceCentroids(afftool, affordances=["Support","Lean"]):
all_points = []
for _, el in enumerate(affordances):
all_points += afftool.getAffordancePoints(el)
return __centroid_list(all_points)
b_id = 0
def draw_centroid(gui, winId, pt, scene="n_name", color = [1,1,1,0.3]):
p = pt[0]
n = array(pt[0]) + 0.03 * array(pt[1])
resolution = 0.01
global b_id
boxname = scene+"/"+str(b_id)
boxnameN = scene+"/"+str(b_id)+"n"
b_id += 1
gui.addBox(boxname,resolution,resolution,resolution, color)
gui.addBox(boxnameN,resolution,resolution,resolution, color)
gui.applyConfiguration(boxname,[p[0],p[1],p[2],1,0,0,0])
gui.applyConfiguration(boxnameN,[n[0],n[1],n[2],1,0,0,0])
gui.addSceneToWindow(scene,winId)
gui.refresh()
def draw_centroids(gui, winId, pts_lists, scene="n_name", color = [1,0,0,1]):
gui.createScene(scene)
for _, pt in enumerate(pts_lists):
draw_centroid(gui, winId, pt, scene=scene, color = color)
script/scenarios/sandbox/stair_bauzil_hrp2_interp.py
View file @ 31b586cd
......@@ -19,7 +19,7 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
fullBody.setJointBounds ("root_joint", [-0.135,2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
......@@ -28,15 +28,17 @@ r = tp.Viewer (ps, viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegOffset = [0,0,-0.105]
#~ rLegNormal = [0,1,0]
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegOffset = [0,0,-0.105]
#~ lLegNormal = [0,1,0]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
......@@ -57,7 +59,7 @@ lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
rKneeId = '0RKnee'
rLeg = 'RLEG_JOINT0'
......@@ -87,30 +89,36 @@ 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]
fullBody.setCurrentConfig (q_init)
q_init = [
0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
q_init[:3] = [0.1, -0.82, 0.648702]
q_init[7:] = [ 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, -0.523598776, 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)
];
r (q_init)
fullBody.setCurrentConfig (q_init)
#~ q_init = [
#~ 0.1, -0.82, 0.648702, 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, -0.523598776, 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)
fullBody.setCurrentConfig (q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1], robustnessThreshold = 0.)
r(q_goal)
fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
configs = fullBody.interpolate(0.1) #TODO DEBUG
#~ i = 0;
#~ fullBody.draw(configs[i],r); i=i+1; i-1
r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
#~ fullBody.exportAll(r, configs, 'stair_bauzil_hrp2_robust_2');
......@@ -203,11 +211,6 @@ def d():
def e():
contactPlan()
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
#~ larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
......@@ -225,5 +228,70 @@ def saveAll(name):
saveAllData(fullBody, r, name)
print "Root path generated in " + str(tp.t) + " ms."
genPlan()
print "go"
#~ genPlan()
from hpp.corbaserver.rbprm.rbprmstate import *
com = fullBody.client.basic.robot.getCenterOfMass
s = None
def d1():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rLegId])
a = s.q()
a[2]=a[2]-0.1
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d2():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rLegId, lLegId])
a = s.q()
a[2]=a[2]-0.1
#~ a[0]=a[0]+0.05
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d3():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rarmId])
a = s.q()
a[2]=a[2]+0.01
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
from hpp.corbaserver.rbprm.state_alg import addNewContact, isContactReachable, closestTransform, removeContact, addNewContactIfReachable, projectToFeasibleCom
from geom import *
def dist(q0,q1):
#~ return norm(array(q0[7:]) - array(q1[7:]) )
return norm(array(q0[7:]) - array(q1[7:]) )
def distq_ref(q0):
return lambda s: dist(s.q(),q0)
a = computeAffordanceCentroids(tp.afftool, ["Support"])
def computeNext(state, limb, projectToCom = False, max_num_samples = 10, mu = 0.6):
global a
t1 = time.clock()
#~ candidates = [el for el in a if isContactReachable(state, limb, el[0], el[1], limbsCOMConstraints)[0] ]
#~ print "num candidates", len(candidates)
#~ t3 = time.clock()
results = [addNewContactIfReachable(state, limb, el[0], el[1], limbsCOMConstraints, projectToCom, max_num_samples, mu) for el in a]
t2 = time.clock()
#~ t4 = time.clock()
resultsFinal = [el[0] for el in results if el[1]]
print "time to filter", t2 - t1
#~ print "time to create", t4 - t3
print "num res", len(resultsFinal)
#sorting
sortedlist = sorted(resultsFinal, key=distq_ref(state.q()))
return sortedlist
d3()
b = computeNext(s, rarmId, max_num_samples = 1)
script/scenarios/sandbox/stair_bauzil_hrp2_path.py
View file @ 31b586cd
......@@ -29,7 +29,7 @@ srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
rbprmBuilder.setJointBounds ("root_joint", [0,2, -1, 1, 0, 2.2])
#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('3Rarm', ['Support'])
rbprmBuilder.setAffordanceFilter('0rLeg', ['Support',])
......@@ -42,9 +42,7 @@ rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
......@@ -55,7 +53,7 @@ q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init [::]
q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal [3:7] = [ 0., 0. , 0.14943813, 0.98877108 ]
q_goal [0:3] = [1.49, -0.65, 1.25]; r (q_goal)
#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
......@@ -69,10 +67,10 @@ afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r)
#~ afftool.analyseAll()
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectConfigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
#~ ps.solve ()
t = ps.solve ()
......@@ -86,7 +84,7 @@ f.close()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp = PathPlayer (r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
......@@ -101,7 +99,7 @@ cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot ("toto")
ps2 = ProblemSolver( rbprmBuilder2 )
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(1,"rbprm_path",rbprmBuilder.getAllJointNames())
#~ cl.problem.movePathToProblem(1,"rbprm_path",rbprmBuilder.getAllJointNames())
r2 = Viewer (ps2, viewerClient=r.client)
r.client.gui.setVisibility("toto", "OFF")
r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
......
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