Commit 5b29e004 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add interp script for hyq_sideWall

parent a9cd5b8d
......@@ -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(0,len(self.configs)):
for i in range(0,len(self.configs)-1):
self.viewer(self.configs[i]);
time.sleep(pause)
......
#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.gepetto import Viewer
#calling script darpa_hyq_path to compute root path
import sideWall_hyq_pathKino as tp
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
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",[0.8,5.6, -0.5, 0.5, 0.4, 1.2])
# Setting a number of sample configurations used
nbSamples = 20000
dynamic=True
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps,viewerClient=tp.r.client)
rootName = 'base_joint_xyz'
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
rLegId = 'rfleg'
lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
addLimbDb(rLegId, "EFORT_Normal")
addLimbDb(lLegId, "EFORT_Normal")
addLimbDb(rarmId, "EFORT_Normal")
addLimbDb(larmId, "EFORT_Normal")
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(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)
# Randomly generating a contact configuration at q_end
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,[larmId,rLegId,rarmId,lLegId])
fullBody.setEndState(q_goal,[larmId,rLegId,rarmId,lLegId])
r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.05,pathId=1,robustnessTreshold = 2, filterStates = True)
print "number of configs =", len(configs)
r(configs[-1])
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
from fullBodyPlayer import Player
player = Player(fullBody,pp,tp,configs,draw=True,optim_effector=False,use_velocity=dynamic,pathId = 1)
#player.displayContactPlan()
r(configs[15])
player.interpolate(10,100)
#player.play()
"""
camera = [0.5681925415992737,
-6.707448482513428,
2.5206544399261475,
0.8217507600784302,
0.5693002343177795,
0.020600343123078346,
0.01408931240439415]
r.client.gui.setCameraTransform(0,camera)
"""
......@@ -20,7 +20,7 @@ extraDof = 6
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,6, -0.5, 0.5, 0.4, 1.2])
rbprmBuilder.setJointBounds ("base_joint_xyz", [0.8,5.6, -0.5, 0.5, 0.4, 1.2])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
......@@ -29,7 +29,7 @@ rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
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.boundSO3([-0.2,0.2,-0.3,0.3,-0.3,0.3])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
......@@ -74,10 +74,14 @@ r(q_init)
#ps.client.problem.prepareSolveStepByStep()
r.solveAndDisplay("rm",1,0.01)
q_far = q_init[::]
q_far[2] = -3
r(q_far)
#r.solveAndDisplay("rm",1,0.01)
#ps.solve ()
ps.solve ()
"""
camera = [0.6293167471885681,
......@@ -90,14 +94,16 @@ camera = [0.6293167471885681,
r.client.gui.setCameraTransform(0,camera)
"""
ps.client.problem.extractPath(0,0,5.5)
# 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)
r.client.gui.removeFromGroup("rm",r.sceneName)
pp.displayVelocityPath(1)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
#display path
pp.speed=0.3
#pp (0)
......@@ -108,10 +114,15 @@ r.client.gui.removeFromGroup("path_0_root",r.sceneName)
pp.displayVelocityPath(1)
pp (1)
"""
"""
q_far = q_init[::]
q_far[2] = -3
r(q_far)
"""
"""
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
......
......@@ -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 = ['end_reached_constraint']
constraints = ['end_reached_constraint','cones_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_constraint']
param_constraints = []
mass = fullBody.getMass()
......
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