demo.py 797 Bytes
Newer Older
stevet's avatar
stevet committed
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
28
29
30
31
import setup_one_step as sos


fsp = sos.fewStepPlanner #planner instance
q_init =  sos.q_init  #initial configuration
initState = sos.initState #initial state. 
viewer = sos.v

### Go somewhere

n_goal = q_init[:7][:]
n_goal[0] += 2
n_goal[1] += 1
n_goal[3:7] = [0.,0.,0.7071,0.7071]
states, configs = fsp.goToQuasiStatic(initState,n_goal, displayGuidePath = True)
#equivalent to
 # fsp.goToQuasiStatic(self, initState, n_goal, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False)


#display computed States:
sos.dispContactPlan(states,0.051) #2nd argument is frame rateue


s = states[-1] #last state computed

#display configuration
viewer(s.q())

#some helpers: 
s.q() # configuration associated to state
#~ initState. # configuration associated to state