Commit be757898 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

demo jump hyq work

parent 5e04384e
......@@ -100,6 +100,7 @@ install(FILES
data/urdf/robot_test/robot_test_trunk.urdf
data/urdf/ground_bigStep.urdf
data/urdf/ground_jump_easy.urdf
data/urdf/ground_jump_low.urdf
data/urdf/ground_jump_bar.urdf
data/urdf/ground_jump_med.urdf
data/urdf/ground_with_bridge.urdf
......@@ -140,6 +141,7 @@ install(FILES
data/srdf/robot_test/robot_test_trunk.srdf
data/srdf/ground_bigStep.srdf
data/srdf/ground_jump_easy.srdf
data/srdf/ground_jump_low.srdf
data/srdf/ground_jump_bar.srdf
data/srdf/ground_jump_med.srdf
data/srdf/ground_with_bridge.srdf
......@@ -184,6 +186,7 @@ install(FILES
data/meshes/ground_jump_easyStart.stl
data/meshes/ground_jump_easyStart2.stl
data/meshes/ground_jump_easyGoal.stl
data/meshes/ground_jump_lowGoal.stl
data/meshes/ground_jump_bar.stl
data/meshes/ground_jump_med.stl
data/meshes/ground_with_bridge.stl
......@@ -194,6 +197,7 @@ install(FILES
data/meshes/hyq/hyq_all_realist.stl
data/meshes/hyq/hyq_trunk.stl
data/meshes/hyq/hyq_trunk_large.stl
data/meshes/hyq/hyq_trunk_large2.stl
data/meshes/hyq/hyq_rom.stl
data/meshes/hyq/hyq_rhleg_rom.stl
data/meshes/hyq/hyq_rfleg_rom.stl
......
......@@ -10,7 +10,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_easyStart.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_easyStart2.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
......@@ -19,7 +19,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_easyStart.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_easyStart2.stl"/>
</geometry>
</collision>
</link>
......
......@@ -76,12 +76,21 @@ r(q_init)
ps.solve ()
camera = [0.6293167471885681,
-9.560577392578125,
10.504343032836914,
0.9323806762695312,
0.36073973774909973,
0.008668755181133747,
0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
#r.client.gui.removeFromGroup("rm0",r.sceneName)
pp.displayVelocityPath(0)
pp.displayVelocityPath(1)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
#display path
pp.speed=0.3
......
......@@ -88,7 +88,7 @@ class Player (object):
self.viewer.client.gui.setVisibility("hyq", "ON")
self.tp.r.client.gui.setVisibility("toto", "OFF")
self.tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
for i in range(1,len(self.configs)-1):
for i in range(0,len(self.configs)):
self.viewer(self.configs[i]);
time.sleep(0.5)
......
......@@ -24,13 +24,15 @@ srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.setJointBounds ("base_joint_xyz", [-5,5, -1.5, 1.5, 0.5, 0.8])
# Setting a number of sample configurations used
nbSamples = 20000
dynamic=True
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps)
r = tp.Viewer (ps,viewerClient=tp.r.client)
rootName = 'base_joint_xyz'
......@@ -46,22 +48,21 @@ lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
addLimbDb(rLegId, "static")
addLimbDb(lLegId, "static")
addLimbDb(rarmId, "static")
addLimbDb(larmId, "static")
addLimbDb(rLegId, "forward")
addLimbDb(lLegId, "forward")
addLimbDb(rarmId, "forward")
addLimbDb(larmId, "forward")
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:7]
dir_init = tp.ps.configAtParam(0,0.01)[7:10]
acc_init = tp.ps.configAtParam(0,0.01)[10:13]
dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(1))[7:10]
acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(1))[10:13]
dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[7:10]
acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[10:13]
configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
fullBody.setStaticStability(False)
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init,dir_init,acc_init)
......@@ -70,64 +71,56 @@ q_init = fullBody.generateContacts(q_init,dir_init,acc_init)
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal)
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = dir_goal[::]
q_goal[configSize+3:configSize+6] = acc_goal[::]
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
fullBody.setStartState(q_init,[larmId,rLegId,rarmId,lLegId])
fullBody.setEndState(q_goal,[larmId,rLegId,rarmId,lLegId])
r(q_init)
# computing the contact sequence
# configs = fullBody.interpolate(0.12, 10, 10, True) #Was this (Pierre)
configs = fullBody.interpolate(0.05,pathId=0)
#~ configs = fullBody.interpolate(0.11, 7, 10, True)
#~ configs = fullBody.interpolate(0.1, 1, 5, True)
configs = fullBody.interpolate(0.08,pathId=0,robustnessTreshold = 3, filterStates = True)
print "configs size = ",len(configs)
r(configs[-1])
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
# calling draw with increasing i will display the sequence
"""
i = 0;
import time
for i in range(0,len(configs)):
fullBody.draw(configs[i],r)
time.sleep(0.5)
"""
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
from fullBodyPlayer import Player
player = Player(fullBody,pp,tp,configs)
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=dynamic,pathId = 0)
#player.displayContactPlan()
player.interpolate()
player.interpolate(1,len(configs)-1)
#player.play()
"""
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} }
camera = [0.5681925415992737,
-6.707448482513428,
2.5206544399261475,
0.8217507600784302,
0.5693002343177795,
0.020600343123078346,
0.01408931240439415]
r.client.gui.setCameraTransform(0,camera)
"""
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 = True, use_window = use_window,
verbose = verbose, draw = draw)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
def saveAll(name):
saveAllData(fullBody, r, name)
#~ fullBody.exportAll(r, trajec, 'hole_hyq_t_var_04f_andrea');
#~ fullBody.exportAll(r, configs, 'hole_hyq_t_var_04f_andrea_contact_planning');
#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')
"""
......@@ -16,6 +16,7 @@ urdfSuffix = ""
srdfSuffix = ""
vMax = 2;
aMax = 1;
extraDof = 6
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
......@@ -29,7 +30,7 @@ rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
# We also bound the rotations of the torso.
rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(2*3)
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
# Creating an instance of HPP problem solver and the viewer
......@@ -48,17 +49,19 @@ r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init [0:3] = [3, 1, 0.63]
q_init[7:10]=[0,0,0]#velocity
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [4, 0, 0.71]
q_goal[7:10]=[2,0,0.9]#velocity
q_goal [0:3] = [4, 1, 0.76]
q_goal[7:10]=[2,0,0.7]#velocity
r (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
ps.addPathOptimizer ("RandomShortcutOriented")
#ps.addPathOptimizer ("RandomShortcutOriented")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Choosing RBPRM shooter and path validation methods.
......
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
white=[1.0,1.0,1.0,1.0]
green=[0.23,0.75,0.2,0.5]
yellow=[0.85,0.75,0.15,1]
pink=[1,0.6,1,1]
orange=[1,0.42,0,1]
brown=[0.85,0.75,0.15,0.5]
blue = [0.0, 0.0, 0.8, 1.0]
grey = [0.7,0.7,0.7,1.0]
red = [0.8,0.0,0.0,1.0]
rootJointType = 'freeflyer'
......@@ -18,7 +9,9 @@ urdfName = 'robot_test_trunk'
urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = 4;
aMax = 1;
extraDof = 6
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 2.5])
......@@ -26,21 +19,22 @@ rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
rbprmBuilder.setAffordanceFilter('robot_test_lleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('robot_test_rleg_rom', ['Support'])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(3)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
ps.client.problem.setParameter("aMax",aMax)
ps.client.problem.setParameter("vMax",vMax)
r = Viewer (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "ground_jump_easy", "planning", r)
afftool.visualiseAffordances('Support', r, brown)
afftool.visualiseAffordances('Support', r, r.color.brown)
q_init = rbprmBuilder.getCurrentConfig ();
#q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config
......@@ -49,20 +43,20 @@ q_init [0:3] = [-4, 1, 0.9]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
q_goal [0:3] = [3, 1, 0.9]; r (q_goal) # pont
q_goal [0:3] = [1, 1, 0.9]; r (q_goal) # pont
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
#ps.client.problem.selectSteeringMethod("SteeringDynamic")
ps.selectPathPlanner("ParabolaPlanner")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Choosing RBPRM shooter and path validation methods.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("KinodynamicDistance")
ps.selectPathPlanner("DynamicPlanner")
r(q_init)
......
......@@ -106,7 +106,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
#assert( len(phase_dt) >= 2 + use_window * 2 ), "phase_dt does not describe all phases"
configSize = len(states[state_id])
#constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
constraints = ['end_reached_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_constraint']
param_constraints = []
mass = fullBody.getMass()
......
......@@ -133,9 +133,9 @@ def __getTimes(fullBody, configs, i, time_scale,use_window=0):
# TODO : si t = 0, hardcoded ...
if t <= dt:
print "WARNING : in getTime, t=0"
t = 0.15
t = 0.1
use_window = use_window+1
times = [0.03 , 0] #FIXME : hardcoded value depend on interpolation step choosen (not available here)
times = [0.02 , 0] #FIXME : hardcoded value depend on interpolation step choosen (not available here)
times[1] = t - 2*times[0]
print "times : ",times
return times, dt, distance,use_window
......
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