stair_bauzil_hrp2_interp.py 5.5 KB
Newer Older
Pierre Fernbach's avatar
Pierre Fernbach committed
1
2
3
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
4
import omniORB.any
5
from . import stair_bauzil_hrp2_path as tp
Pierre Fernbach's avatar
Pierre Fernbach committed
6
7
8
9
10
11
12
13
14
15
import time



packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
#  Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
16
urdfSuffix = "_reduced_safe"
Pierre Fernbach's avatar
Pierre Fernbach committed
17
18
19
20
21
22
23
24
25
srdfSuffix = ""

fullBody = FullBody ()

fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz",  [0,2, -1, 1, 0, 2.2])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)

ps = tp.ProblemSolver( fullBody )
26
27
28
29
30
31


ps.client.problem.setParameter("aMax",omniORB.any.to_any(tp.aMax))
ps.client.problem.setParameter("vMax",omniORB.any.to_any(tp.vMax))
ps.client.problem.setParameter("friction",tp.mu)

32
33
34
35
r = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True)
tStart = time.time()
#~ AFTER loading obstacles

Pierre Fernbach's avatar
Pierre Fernbach committed
36
37

#~ AFTER loading obstacles
38
rLegId = 'hrp2_rleg_rom'
39
40
41
42
lLegId = 'hrp2_lleg_rom'
tStart = time.time()


Pierre Fernbach's avatar
Pierre Fernbach committed
43
rLeg = 'RLEG_JOINT0'
44
45
rLegOffset = [0,0,-0.0955]
rLegLimbOffset=[0,0,-0.035]#0.035
Pierre Fernbach's avatar
Pierre Fernbach committed
46
rLegNormal = [0,0,1]
Pierre Fernbach's avatar
Pierre Fernbach committed
47
48
rLegx = 0.09; rLegy = 0.05

49
50
51
52
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")

Pierre Fernbach's avatar
Pierre Fernbach committed
53
lLeg = 'LLEG_JOINT0'
54
55
lLegOffset = [0,0,-0.0955]
lLegLimbOffset=[0,0,0.035]
Pierre Fernbach's avatar
Pierre Fernbach committed
56
lLegNormal = [0,0,1]
Pierre Fernbach's avatar
Pierre Fernbach committed
57
lLegx = 0.09; lLegy = 0.05
58
59
60
61
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")

Pierre Fernbach's avatar
Pierre Fernbach committed
62

63
rarmId = 'hrp2_rarm_rom'
Pierre Fernbach's avatar
Pierre Fernbach committed
64
65
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
66
67
rArmOffset = [0,0,0.1]
rArmNormal = [0,0,-1]
Pierre Fernbach's avatar
Pierre Fernbach committed
68
69
rArmx = 0.024; rArmy = 0.024
#disabling collision for hook
Pierre Fernbach's avatar
Pierre Fernbach committed
70
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 100000, "manipulability", 0.01, "_6_DOF", True)
Pierre Fernbach's avatar
Pierre Fernbach committed
71
72
73



74
tGenerate =  time.time() - tStart
75
print("generate databases in : "+str(tGenerate)+" s")
Pierre Fernbach's avatar
Pierre Fernbach committed
76
77
78
79
80
81
82
83



q_0 = fullBody.getCurrentConfig(); 
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)


q_init =[0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,0.0, 0.0, 0.0, 0.0,0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init)
84
q_ref = q_init[::]
Pierre Fernbach's avatar
Pierre Fernbach committed
85
fullBody.setCurrentConfig (q_init)
86
fullBody.setReferenceConfig (q_ref)
Pierre Fernbach's avatar
Pierre Fernbach committed
87
88
89

configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()

90
91
q_init = fullBody.getCurrentConfig(); q_init[0:3] = tp.ps.configAtParam(0,0.01)[0:3] # use this to get the correct orientation
q_goal = fullBody.getCurrentConfig(); q_goal[0:3] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:3]
Pierre Fernbach's avatar
Pierre Fernbach committed
92
93
94
95
96
97
98
dir_init = tp.ps.configAtParam(0,0.01)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(0,0.01)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[tp.indexECS:tp.indexECS+3]
acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[tp.indexECS+3:tp.indexECS+6]



99
100
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
101
102


103
# FIXME : test
104
105
106
107
108
#q_init[2] = q_init[2]+0.0
#q_goal[2] = q_goal[2]+0.02
q_init[2] = q_ref[2] + 0.01
q_goal[2] = 1.25

109

Pierre Fernbach's avatar
Pierre Fernbach committed
110

111

Pierre Fernbach's avatar
Pierre Fernbach committed
112
fullBody.setStaticStability(True)
113
"""
Pierre Fernbach's avatar
Pierre Fernbach committed
114
115
116
117
118
# 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)
r(q_init)
119
"""
Pierre Fernbach's avatar
Pierre Fernbach committed
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134

# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal)

# 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] = acc_goal[::]
# specifying the full body configurations as start and goal state of the problem

r(q_init)


135
fullBody.setStartState(q_init,[rLegId,lLegId])
136
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId])
Pierre Fernbach's avatar
Pierre Fernbach committed
137
138


139
140
141
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=0,robustnessTreshold = 0, filterStates = True,testReachability=False,quasiStatic=False)
tInterpolateConfigs = time.time()-tStart
142
print("number of configs :", len(configs))
Pierre Fernbach's avatar
Pierre Fernbach committed
143
r(configs[-1])
Pierre Fernbach's avatar
Pierre Fernbach committed
144
145
146
147
148


from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)

149
from .fullBodyPlayerHrp2 import Player
150
151
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=False,pathId = 0)

Pierre Fernbach's avatar
Pierre Fernbach committed
152

153

154
player.displayContactPlan()
155
156


157
158
beginState=0
endState=len(configs)-1
159
160


161
from configs.stairs_config import *
162
from generate_contact_sequence import *
163
cs = generateContactSequence(fullBody,configs,beginState,endState,r)
164
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
165
cs.saveAsXML(filename, "ContactSequence")
166
print("save contact sequence : ",filename)
Pierre Fernbach's avatar
Pierre Fernbach committed
167
168
169