Commit 9e80a2ac authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[script] update darpa_hyq script to new python API

parent 611bd2cc
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
#reference pose for hyq
from hyq_ref_pose import hyq_ref
......@@ -10,7 +9,7 @@ from hyq_ref_pose import hyq_ref
import darpa_hyq_path as tp
from os import environ
ins_dir = environ['DEVEL_DIR']
ins_dir = environ['DEVEL_HPP_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
......@@ -117,27 +116,9 @@ configs = []
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
pp = PathPlayer (fullBody.client, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},
lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},
rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = False, use_window = use_window,
verbose = verbose, draw = draw)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
import time
#DEMO METHODS
......@@ -234,7 +215,7 @@ d(0.01);e(0.01)
from hpp.corbaserver.rbprm.rbprmstate import *
com = fullBody.client.basic.robot.getCenterOfMass
com = fullBody.getCenterOfMass()
s = None
def d1():
global s
......
......@@ -29,7 +29,7 @@ rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
rbprmBuilder.boundSO3([-0.4,0.4,-0.3,0.3,-0.3,0.3])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
......@@ -73,7 +73,7 @@ q_far [0:3] = [-2, -3, 0.63];
r(q_far)
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
rbprmBuilder.client.problem.optimizePath(i)
#~ pp(9)
......@@ -92,7 +92,7 @@ class Robot (Parent):
def __init__ (self, robotName, load = True):
Parent.__init__ (self, robotName, self.rootJointType, load)
self.tf_root = "base_footprint"
self.client.basic = Client ()
self.client = Client ()
self.load = load
#DEMO code to play root path and final contact plan
......
Markdown is supported
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