darpa_hyq.py 4.48 KB
Newer Older
Steve Tonneau's avatar
Steve Tonneau committed
1
#Importing helper class for RBPRM
2
from hpp.corbaserver.rbprm.hyq import Robot
3
from hpp.corbaserver.problem_solver import ProblemSolver
Steve Tonneau's avatar
Steve Tonneau committed
4
5
6
7
8
9
from hpp.gepetto import Viewer

#calling script darpa_hyq_path to compute root path
import darpa_hyq_path as tp

from os import environ
stevet's avatar
stevet committed
10
ins_dir = environ['DEVEL_HPP_DIR']
Steve Tonneau's avatar
Steve Tonneau committed
11
12
13
14
15
16
17
18
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"


from hpp.corbaserver import Client



#  This time we load the full body model of HyQ
19
fullBody = Robot () 
20
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
Steve Tonneau's avatar
Steve Tonneau committed
21
22

#  Setting a number of sample configurations used
stevet's avatar
stevet committed
23
nbSamples = 10000
Steve Tonneau's avatar
Steve Tonneau committed
24
25

ps = tp.ProblemSolver(fullBody)
26
r = tp.Viewer (ps, viewerClient=tp.r.client)
Steve Tonneau's avatar
Steve Tonneau committed
27
28
29
30
31
32
33

rootName = 'base_joint_xyz'
cType = "_3_DOF"

def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)

34
35
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
36
37
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
Steve Tonneau's avatar
Steve Tonneau committed
38

t steve's avatar
t steve committed
39
40
41
42
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
43
"""
t steve's avatar
t steve committed
44
45
q_init = [-2.0,
 0.0,
46
 0.6838277139631803,
t steve's avatar
t steve committed
47
48
49
 0.0,
 0.0,
 0.0,
50
 1.0,
t steve's avatar
t steve committed
51
52
53
54
55
56
57
58
59
60
61
62
 0.14279812395541294,
 0.934392553166556,
 -0.9968239786882757,
 -0.06521258938340457,
 -0.8831796268418511,
 1.150049183494211,
 -0.06927610020154493,
 0.9507443168724581,
 -0.8739975339028809,
 0.03995660287873871,
 -0.9577096766517215,
 0.9384602821326071]
63
64
65
"""
q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]+0.02
q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]+0.02
t steve's avatar
t steve committed
66

Steve Tonneau's avatar
Steve Tonneau committed
67

68
69
q_init = fullBody.generateContacts(q_init, [0,0,1])
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
Steve Tonneau's avatar
Steve Tonneau committed
70
71

# specifying the full body configurations as start and goal state of the problem
72
73
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
Steve Tonneau's avatar
Steve Tonneau committed
74
75
76
77
78
79
80


r(q_init)
configs = []


from hpp.gepetto import PathPlayer
81
pp = PathPlayer (fullBody.client, r)
Steve Tonneau's avatar
Steve Tonneau committed
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


import time

#DEMO METHODS

def initConfig():
	r.client.gui.setVisibility("hyq", "ON")
	tp.cl.problem.selectProblem("default")
	tp.r.client.gui.setVisibility("toto", "OFF")
	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
	r(q_init)
	
def endConfig():
	r.client.gui.setVisibility("hyq", "ON")
	tp.cl.problem.selectProblem("default")
	tp.r.client.gui.setVisibility("toto", "OFF")
	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
	r(q_goal)
	

def rootPath():
	r.client.gui.setVisibility("hyq", "OFF")
	tp.cl.problem.selectProblem("rbprm_path")
	tp.r.client.gui.setVisibility("toto", "OFF")
	r.client.gui.setVisibility("hyq", "OFF")
	tp.r.client.gui.setVisibility("hyq_trunk_large", "ON")
	tp.pp(0)
	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
	r.client.gui.setVisibility("hyq", "ON")
	tp.cl.problem.selectProblem("default")
	
Steve Tonneau's avatar
Steve Tonneau committed
114
def genPlan(stepsize=0.06):
Steve Tonneau's avatar
Steve Tonneau committed
115
116
117
118
119
120
	tp.cl.problem.selectProblem("default")
	r.client.gui.setVisibility("hyq", "ON")
	tp.r.client.gui.setVisibility("toto", "OFF")
	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
	global configs
	start = time.clock() 
121
	configs = fullBody.interpolate(stepsize, 8, 0, filterStates = False, testReachability=False, quasiStatic=True)
Steve Tonneau's avatar
Steve Tonneau committed
122
123
124
	end = time.clock() 
	print "Contact plan generated in " + str(end-start) + "seconds"
	
Steve Tonneau's avatar
Steve Tonneau committed
125
def contactPlan(step = 0.5):
Steve Tonneau's avatar
Steve Tonneau committed
126
127
128
129
	r.client.gui.setVisibility("hyq", "ON")
	tp.cl.problem.selectProblem("default")
	tp.r.client.gui.setVisibility("toto", "OFF")
	tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
Steve Tonneau's avatar
Steve Tonneau committed
130
	for i in range(0,len(configs)):
Steve Tonneau's avatar
Steve Tonneau committed
131
		r(configs[i]);
132
133
		time.sleep(step)
                		
Steve Tonneau's avatar
Steve Tonneau committed
134
135
		
print "Root path generated in " + str(tp.t) + " ms."
Steve Tonneau's avatar
Steve Tonneau committed
136

stevet's avatar
stevet committed
137
genPlan(0.01);contactPlan(0.01)