Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Jason Chemin
hpp-rbprm-corba
Commits
e7ac0877
Commit
e7ac0877
authored
Nov 21, 2016
by
Steve Tonneau
Browse files
data for ground crouch
parent
3af852e4
Changes
1
Hide whitespace changes
Inline
Side-by-side
script/scenarios/ground_crouch_hyq_interp.py
View file @
e7ac0877
...
...
@@ -106,6 +106,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r
(
q_init
)
configs
=
fullBody
.
interpolate
(
0.1
,
1
,
10
,
True
)
#hole
#~ configs = fullBody.interpolate(0.01,1,10, True) #hole
#~ configs = fullBody.interpolate(0.08,1,5) # bridge
r
.
loadObstacleModel
(
'hpp-rbprm-corba'
,
"groundcrouch"
,
"contact"
)
...
...
@@ -143,91 +144,39 @@ def saveAll(name):
from
numpy
import
array
,
zeros
from
numpy
import
matrix
from
numpy
import
matlib
gui
=
r
.
client
.
gui
scene
=
"oddct"
r
.
client
.
gui
.
createScene
(
scene
)
resolution
=
0.03
i
=
0
boxname
=
scene
+
"/b"
+
str
(
i
)
gui
.
addBox
(
boxname
,
resolution
,
resolution
,
resolution
,
[
1
,
1
,
1
,
1
])
gui
.
applyConfiguration
(
boxname
,[
0
,
0
,
0
,
1
,
0
,
0
,
0
])
gui
.
addSceneToWindow
(
scene
,
0
)
gui
.
refresh
()
#~ for i in range(6,25):
#~ act(i, 60, optim_effectors = True)
import
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
as
cwc_trajectory_helper
import
time
for
i
in
range
(
7
,
8
):
act
(
i
,
0
,
use_window
=
0
,
verbose
=
True
,
optim_effectors
=
False
,
draw
=
False
)
#~ pid = act(8,0,optim_effectors = False)
from
derivative_filters
import
*
#~ res = computeSecondOrderPolynomialFitting(m, dt, 11)
#~ (x, dx, ddx) = res
import
matplotlib.pyplot
as
plt
import
plot_utils
#~ plt.plot(ddx[0,:].A.squeeze())
#~ plt.show()
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
trajec_mil
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
trajec
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
coms
#~ plt.plot(x[0,:].A.squeeze())
#~ plt.plot(dx[0,:].A.squeeze())
#~ plt.plot(ddx[2,:].A.squeeze())
#~ plt.plot(dx[0,:].A.squeeze())
#~ plt.plot(x[0,:].A.squeeze())
#~ plt.show()
#~ testc= []
#~ trajec_mil = trajec
sample
=
len
(
trajec_mil
)
dt_me
=
1.
/
24.
dt
=
1.
/
1000.
res
=
zeros
((
3
,
sample
))
for
i
,
q
in
enumerate
(
trajec_mil
[:
sample
]):
fullBody
.
setCurrentConfig
(
q
)
def
applycom
():
global
gui
global
com
c
=
fullBody
.
getCenterOfMass
()
res
[:,
i
]
=
c
#~ for i,q in enumerate(trajec):
#~ fullBody.setCurrentConfig(q)
#~ c = fullBody.getCenterOfMass()
#~ testc += [array(c)]
#~ testddc = [2 *(testc[i-1] - testc[i]) for i in range(1, len(testc))]
#~ testddcx = [ddc[0] for ddc in testddc]
#~ testcx = [c[0] for c in testc]
#~ cs = res
#~ res = zeros((3, 100))
#~ for c in cs:
#~ res[:,i] = c
m
=
matrix
(
res
)
from
derivative_filters
import
*
res
=
computeSecondOrderPolynomialFitting
(
m
,
dt
,
51
)
(
x
,
dx
,
ddx
)
=
res
import
matplotlib.pyplot
as
plt
import
plot_utils
#~ if(conf.SHOW_FIGURES and conf.PLOT_REF_COM_TRAJ):
#~ plt.plot(x[0,:].A.squeeze())
#~ plt.plot(dx[0,:].A.squeeze())
plt
.
plot
(
ddx
[
0
,:].
A
.
squeeze
())
#~ plt.plot(x[0,:].A.squeeze())
#~ plt.plot(testddcx)
#~ plt.plot(testcx)
#~ plt.show()
gui
.
applyConfiguration
(
boxname
,[
c
[
0
],
c
[
1
],
0
,
1
,
0
,
0
,
0
])
gui
.
refresh
()
def
go
(
dt_framerate
=
1.
/
24.
):
path_player
=
pp
configs
=
cwc_trajectory_helper
.
trajec
for
q
in
configs
:
start
=
time
.
time
()
pp
.
publisher
.
robotConfig
=
q
pp
.
publisher
.
publishRobots
()
elapsed
=
time
.
time
()
-
start
applycom
()
if
elapsed
<
dt_framerate
:
time
.
sleep
(
dt_framerate
-
elapsed
)
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment