Commit e7ac0877 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

data for ground crouch

parent 3af852e4
...@@ -106,6 +106,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId]) ...@@ -106,6 +106,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init) r(q_init)
configs = fullBody.interpolate(0.1,1,10, True) #hole 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 #~ configs = fullBody.interpolate(0.08,1,5) # bridge
r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact") r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
...@@ -143,91 +144,39 @@ def saveAll(name): ...@@ -143,91 +144,39 @@ def saveAll(name):
from numpy import array, zeros gui = r.client.gui
from numpy import matrix scene = "oddct"
from numpy import matlib 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): import hpp.corbaserver.rbprm.tools.cwc_trajectory_helper as cwc_trajectory_helper
#~ act(i, 60, optim_effectors = True) import time
def applycom():
for i in range(7,8): global gui
act(i,0, use_window=0, verbose=True, optim_effectors=False, draw=False) global com
#~ 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)
c = fullBody.getCenterOfMass() c = fullBody.getCenterOfMass()
res[:,i] = c gui.applyConfiguration(boxname,[c[0],c[1],0,1,0,0,0])
gui.refresh()
#~ for i,q in enumerate(trajec):
#~ fullBody.setCurrentConfig(q) def go(dt_framerate=1./24.):
#~ c = fullBody.getCenterOfMass() path_player = pp
#~ testc += [array(c)] configs = cwc_trajectory_helper.trajec
for q in configs:
start = time.time()
#~ testddc = [2 *(testc[i-1] - testc[i]) for i in range(1, len(testc))] pp.publisher.robotConfig = q
#~ testddcx = [ddc[0] for ddc in testddc] pp.publisher.publishRobots ()
#~ testcx = [c[0] for c in testc] elapsed = time.time() - start
applycom()
#~ cs = res if elapsed < dt_framerate :
#~ res = zeros((3, 100)) time.sleep(dt_framerate-elapsed)
#~ 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()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment