plateform_hrp2_interp_testTransition.py 7.25 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
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 plateform_hrp2_path as tp
import time

tPlanning = tp.tPlanning


packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
#  Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()

fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,2, -0.5, 0.5, 0.5, 0.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
28
fullBody.client.basic.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-0.5,0.5,0,0,0,0,0,0])
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
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,displayArrows = True, displayCoM = True)


q_init =[0., 0., 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)
q_ref = q_init[::]
fullBody.setCurrentConfig (q_init)
qfar=q_ref[::]
qfar[2] = -5

#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
lLegId = 'hrp2_lleg_rom'
tStart = time.time()


rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")

lLeg = 'LLEG_JOINT0'
lLegOffset = [0,0,-0.105]
lLegLimbOffset=[0,0,0.035]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
fullBody.setReferenceConfig (q_ref)
## Add arms (not used for contact) : 


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 = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
q_goal = fullBody.getCurrentConfig(); 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]


# FIXME : test
q_init[2] = q_ref[2]+0.01
q_goal[2] = q_ref[2]+0.01

fullBody.setStaticStability(True)
# 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('hrp2_14/BODY',0.3)
r(q_init)


fullBody.setStartState(q_init,[lLegId,rLegId])
fullBody.setEndState(q_goal,[lLegId,rLegId])


#p = fullBody.computeContactPointsAtState(init_sid)
#p = fullBody.computeContactPointsAtState(int_sid)


"""
q = q_init[::]
q[0] += 0.3
q = fullBody.generateContacts(q,dir_init,acc_init,robTreshold)
mid_sid = fullBody.addState(q,[lLegId,rLegId])
"""

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

import fullBodyPlayerHrp2

"""
tStart = time.time()
configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 1, filterStates = False)
tInterpolateConfigs = time.time() - tStart
print "number of configs :", len(configsFull)
"""

q_init[0] += 0.05
147
createSphere('s',r)
148

149
150
151
n = [0,0,1]
p = [0,0.1,0]

Pierre Fernbach's avatar
Pierre Fernbach committed
152
153
q_init[-6:-3] = [0.2,0,0]
q_goal[-6:-3] = [0.1,0,0]
154
sf = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
155
156
si = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId])

157
158

n = [0.0, -0.42261828000211843, 0.9063077785212101]
Pierre Fernbach's avatar
Pierre Fernbach committed
159
p = [0.775, 0.23, -0.02]
160
moveSphere('s',r,p)
161
smid,success = StateHelper.addNewContact(si,lLegId,p,n)
162
assert(success)
163
smid2,success = StateHelper.addNewContact(sf,lLegId,p,n)
164
assert(success)
165
r(smid.q())
166
167

sf2 = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
Pierre Fernbach's avatar
Pierre Fernbach committed
168
169
170
171
172



pid = fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,True)
import disp_bezier
Pierre Fernbach's avatar
Pierre Fernbach committed
173
pp.dt = 0.00001
Pierre Fernbach's avatar
Pierre Fernbach committed
174
175
disp_bezier.showPath(r,pp,pid)

Pierre Fernbach's avatar
Pierre Fernbach committed
176
177
178
179
180
181
182
183
184
185
186
187
x = [0.776624, 0.219798, 0.846351]


moveSphere('s',r,x)
displayBezierConstraints(r)

path = "/local/dev_hpp/screenBlender/iros2018/polytopes/platform/path"
for i in range(1,4):
  r.client.gui.writeNodeFile('path_'+str(int(pid[i]))+'_root',path+str(i-1)+'.obj')

r.client.gui.writeNodeFile('s',path+'_S.stl')

188
189
190
191
"""
com = fullBody.getCenterOfMass()
com[1] = 0
"""
Pierre Fernbach's avatar
Pierre Fernbach committed
192
"""
193
194
195
pids = []
pids += [fullBody.isDynamicallyReachableFromState(si.sId,smid.sId)]
pids += [fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId)]
196
pids += [fullBody.isDynamicallyReachableFromState(smid2.sId,sf2.sId)]
197
198
199
200

for pid in pids :
  if pid > 0:
    print "success"
201
202
    #pp.displayPath(pid,color=r.color.blue)
    #r.client.gui.setVisibility('path_'+str(pid)+'_root','ALWAYS_ON_TOP')
203
204
  else:
    print "fail."
Pierre Fernbach's avatar
Pierre Fernbach committed
205
"""
206
"""
207

208
n = [0,0,1]
209
p = [1.15,0.1,0]
210
211
212
moveSphere('s',r,p)

sE,success = StateHelper.addNewContact(si,lLegId,p,n)
213
assert(success)
214
p = [1.15,-0.1,0] 
215
sfe, success = StateHelper.addNewContact(sE,rLegId,p,n)
216
assert(success)
217
218
219
220
221
222
223
224
225
226
227
228

pids = []
pids += [fullBody.isDynamicallyReachableFromState(si.sId,sE.sId)]
pids += [fullBody.isDynamicallyReachableFromState(sE.sId,sfe.sId)]
for pid in pids :
  if pid > 0:
    print "success"
    pp.displayPath(pid,color=r.color.blue)
    r.client.gui.setVisibility('path_'+str(pid)+'_root','ALWAYS_ON_TOP')
  else:
    print "fail."

229
"""
230

231
232
233
234
235
configs = []
configs += [si.q()]
configs += [smid.q()]
configs += [smid2.q()]
configs += [sf2.q()]
236

237

238

239
240
241
242
243
244
245
246
from planning.config import *
from generate_contact_sequence import *

beginState = si.sId
endState = sf2.sId
cs = generateContactSequence(fullBody,configs,beginState, endState,r)


247

248
249
250
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs.saveAsXML(filename, "ContactSequence")
print "save contact sequence : ",filename
251
252
253
254
255
256