anymal_flatGround.py 3.32 KB
Newer Older
1
from hpp.corbaserver.rbprm.anymal import Robot
Pierre Fernbach's avatar
Pierre Fernbach committed
2
3
4
5
6
7
8
9
10
11
12
13
14
15
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import anymal_flatGround_path as tp
print "Done."
import time



pId = tp.ps.numberPaths() -1
fullBody = Robot ()
# Set the bounds for the root, take slightly larger bounding box than during root planning
root_bounds = tp.root_bounds[::]
16
17
18
19
20
21
root_bounds[0] -= 0.5
root_bounds[1] += 0.5
root_bounds[2] -= 0.5
root_bounds[3] += 0.5
root_bounds[-1] = 0.6
root_bounds[-2] = 0.3
Pierre Fernbach's avatar
Pierre Fernbach committed
22
fullBody.setJointBounds ("root_joint",  root_bounds)
23
fullBody.setVeryConstrainedJointsBounds()
Pierre Fernbach's avatar
Pierre Fernbach committed
24
25
26
27
28
29
30
31
32
33
34
35
36
37
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
#load the viewer
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)

# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0,0,0,0,0,0]
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
38
39
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.usePosturalTaskContactCreation(True)
Pierre Fernbach's avatar
Pierre Fernbach committed
40
41
42

print "Generate limb DB ..."
tStart = time.time()
43
fullBody.loadAllLimbs("fixedStep04","ReferenceConfiguration")
Pierre Fernbach's avatar
Pierre Fernbach committed
44
45
46
47
48
49
50
51
tGenerate =  time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
fullBody.setReferenceConfig(q_ref)

#define initial and final configurations : 
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()

52
q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation
Pierre Fernbach's avatar
Pierre Fernbach committed
53
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
54
55
56
dir_init = tp.ps.configAtParam(pId,0.)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0.)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
Pierre Fernbach's avatar
Pierre Fernbach committed
57
58
acc_goal = [0,0,0]

59
robTreshold = 2
Pierre Fernbach's avatar
Pierre Fernbach committed
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
# 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]

fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)

fullBody.setCurrentConfig (q_goal)
v(q_goal)

76
v.addLandmark('anymal/base',0.3)
Pierre Fernbach's avatar
Pierre Fernbach committed
77
v(q_init)
stevet's avatar
stevet committed
78
z = [0.,0.,1]
Pierre Fernbach's avatar
Pierre Fernbach committed
79
# specify the full body configurations as start and goal state of the problem
stevet's avatar
stevet committed
80
81
fullBody.setStartState(q_init,fullBody.limbs_names, [z for _ in range(4)])
fullBody.setEndState(q_goal,fullBody.limbs_names, [z for _ in range(4)])
Pierre Fernbach's avatar
Pierre Fernbach committed
82
83
84
85


print "Generate contact plan ..."
tStart = time.time()
86
configs = fullBody.interpolate(0.002,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
Pierre Fernbach's avatar
Pierre Fernbach committed
87
88
89
90
91
92
93
tInterpolateConfigs = time.time() - tStart
print "Done. "
print "Contact sequence computed in "+str(tInterpolateConfigs)+" s."
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)

94
fullBody.resetJointsBounds()
Pierre Fernbach's avatar
Pierre Fernbach committed
95