Unverified Commit 7947aa71 authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #14 from pFernbach/topic/kino

Topic/kino
parents 338503cc 9b69166e
......@@ -316,6 +316,7 @@ install(FILES
data/meshes/cross_gap2.stl
data/meshes/floor_bauzil.stl
data/meshes/floor_bauzil_obstacles.stl
data/meshes/bauzil_stairs_noRamp_simplified.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
......
......@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/bauzil_walk_stairs.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/bauzil_stairs_noRamp_simplified.stl"/>
</geometry>
</collision>
</link>
......
......@@ -5,7 +5,7 @@
<inertia ixx="1." ixy="0.0" ixz="0.0" iyy="1." iyz="0.0" izz="1."/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" scale="2 2 2"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_all.stl"/>
</geometry>
......@@ -14,9 +14,9 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" scale="5 5 5"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_trunk_large2.stl" scale="1 1 1"/>
<mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_trunk_large.stl" />
</geometry>
</collision>
</link>
......
......@@ -106,6 +106,10 @@ module hpp
void setReferenceConfig(in floatSeq referenceConfig)
raises (Error);
/// set a reference position of the end effector for the given ROM
void setReferenceEndEffector(in string romName, in floatSeq ref)
raises (Error);
/// Set Rom constraints for the configuration shooter
/// a configuration will only be valid if all roms indicated
/// are colliding with the environment.
......
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import time
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
import time
rLegId = 'talos_rleg_rom'
rLeg = 'leg_right_1_joint'
rFoot = 'leg_right_6_joint'
lLegId = 'talos_lleg_rom'
lLeg = 'leg_left_1_joint'
lFoot = 'leg_left_6_joint'
rArmId = 'talos_rarm_rom'
rArm = 'arm_right_1_joint'
rHand = 'arm_right_7_joint'
lArmId = 'talos_larm_rom'
lArm = 'arm_left_1_joint'
lHand = 'arm_left_7_joint'
packageName = "talos_data"
meshPackageName = "talos_data"
rootJointType = "freeflyer"
urdfName = "talos"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.95, 1.05])
ps = ProblemSolver( fullBody )
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
vf.loadObstacleModel ("hpp-rbprm-corba", "table_140_70_73", "planning")
q_ref = [
0.0, 0.0, 1.0232773, 0.0 , 0.0, 0.0, 1, #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
0.0 , 0.006761, #Chest
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1,-0.005, #Left Arm
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005,#Right Arm
0., 0.]; # head
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
tStart = time.time()
# generate databases :
nbSamples = 1000
rLegOffset = [0,-0.00018,-0.107]
rLegOffset[2] += 0.006
rLegNormal = [0,0,1]
rLegx = 0.1; rLegy = 0.06
fullBody.addLimb(rLegId,rLeg,rFoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "static", 0.01)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset = [0,-0.00018,-0.107]
lLegOffset[2] += 0.006
lLegNormal = [0,0,1]
lLegx = 0.1; lLegy = 0.06
fullBody.addLimb(lLegId,lLeg,lFoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "static", 0.01)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
#rArmOffset = [0.055,-0.04,-0.13]
rArmOffset = [-0.01,0.,-0.154]
rArmNormal = [0,0,1]
rArmx = 0.005; rArmy = 0.005
fullBody.addLimb(rArmId,rArm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
fullBody.runLimbSampleAnalysis(rArmId, "ReferenceConfiguration", True)
"""
lArmOffset = [0.055,0.04,-0.13]
lArmNormal = [0,0,1]
lArmx = 0.02; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
"""
tGenerate = time.time() - tStart
print "generate databases in : "+str(tGenerate)+" s"
v = vf.createViewer(displayCoM=True)
v(q_init)
v.addLandmark(v.sceneName,0.5)
v.addLandmark('talos/arm_right_7_link',0.1)
q_init[0:2] = [-0.5,0.8]
q_init[32] -=1.5 # right elbow
v(q_init)
# sphere = target
from display_tools import *
createSphere('target',v,size=0.05,color=v.color.red)
v.client.gui.setVisibility('target','ON')
moveSphere('target',v,[0,0,0.5])
# create contact :
fullBody.setStartState(q_init,[lLegId,rLegId])
q_ref[0:3] = q_init[0:3]
sref = State(fullBody,q=q_ref,limbsIncontact=[lLegId,rLegId])
s0 = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId])
createSphere('s',v)
p0 = [-0.25,0.5,0.75]
#p1 = [-0,0.45,0.8]
p = [0.1,0.5,0.75]
moveSphere('s',v,p)
s0_bis,success = StateHelper.addNewContact(sref,rArmId,p0,[0,0,1])
#s0_bis2,success = StateHelper.addNewContact(s0_bis,rArmId,p1,[0,0,1])
s1,success = StateHelper.addNewContact(s0_bis,rArmId,p,[0,0,1])
assert(success)
v(s1.q())
#project com
v(q_init)
com_i = fullBody.getCenterOfMass()
com_i[2] -= 0.03
com_i[0] += 0.06
createSphere("com",v)
moveSphere("com",v,com_i)
s1.projectToCOM(com_i)
v(s1.q())
s1_feet = State(fullBody,q=s1.q(),limbsIncontact=[lLegId,rLegId])
s2,success = StateHelper.addNewContact(s0_bis,rArmId,p,[0,0,1])
com=s2.getCenterOfMass()
#com[0] += 0.03
com[1] -= 0.06
com[2] -= 0.02
moveSphere("com",v,com)
s2.projectToCOM(com)
v(s2.q())
q3=q_init[::]
q3[20]=1.2
s3_0 = State(fullBody,q=q3,limbsIncontact=[lLegId,rLegId])
s3,success = StateHelper.addNewContact(s3_0,rArmId,p,[0,0,1])
assert(success)
com=s3.getCenterOfMass()
#com[0] += 0.03
com[1] -= 0.1
com[2] -= 0.02
moveSphere("com",v,com)
s3.projectToCOM(com)
v(s3.q())
"""
jointsName = [rFoot,lFoot,rHand]
contactPos = []
contactNormal = []
pn = s1.getCenterOfContactForLimb(rLegId)
contactPos += [pn[0]]
contactNormal += [pn[1]]
pn = s1.getCenterOfContactForLimb(lLegId)
contactPos += [pn[0]]
contactNormal += [pn[1]]
pn = s1.getCenterOfContactForLimb(rArmId)
contactPos += [pn[0]]
contactNormal += [pn[1]]
ps.client.problem.createStaticStabilityConstraint ('staticStability',
jointsName, contactPos, contactNormal,'root_joint')
"""
q2 = q_ref[::]
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from display_tools import *
import time
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
print "Plan guide trajectory ..."
import talos_flatGround_path as tp
print "Done."
import time
##
# Information to retrieve urdf and srdf files.
packageName = "talos_data"
meshPackageName = "talos_data"
rootJointType = "freeflyer"
urdfName = "talos"
urdfSuffix = "_reduced"
srdfSuffix = ""
rLegId = 'talos_rleg_rom'
rleg = 'leg_right_1_joint'
rfoot = 'leg_right_6_joint'
lLegId = 'talos_lleg_rom'
lleg = 'leg_left_1_joint'
lfoot = 'leg_left_6_joint'
rArmId = 'talos_rarm_rom'
rarm = 'arm_right_1_joint'
rhand = 'arm_right_7_joint'
lArmId = 'talos_larm_rom'
larm = 'arm_left_1_joint'
lhand = 'arm_left_7_joint'
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.95, 1.05])
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
q_ref = [
0.0, 0.0, 1.0232773, 0.0 , 0.0, 0.0, 1., #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
0.0 , 0.006761, #Chest
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1,-0.005, #Left Arm
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005,#Right Arm
0., 0. , #Head
0,0,0,0,0,0]; v (q_ref)
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 10000
rLegOffset = [0., -0.00018, -0.107]
rLegOffset[2] += 0.006
rLegNormal = [0,0,1]
rLegx = 0.1; rLegy = 0.06
fullBody.addLimb(rLegId,rleg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset = [0., -0.00018, -0.107]
lLegOffset[2] += 0.006
lLegNormal = [0,0,1]
lLegx = 0.1; lLegy = 0.06
fullBody.addLimb(lLegId,lleg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
q_0 = fullBody.getCurrentConfig();
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 3
# 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] = [0,0,0]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
# specifying the full body configurations as start and goal state of the problem
v.addLandmark('talos/base_link',0.3)
v(q_init)
fullBody.setStartState(q_init,[rLegId,lLegId])
fullBody.setEndState(q_goal,[rLegId,lLegId])
from hpp.gepetto import PathPlayer
pp = PathPlayer ( v)
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = False)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
raw_input("Press Enter to display the contact sequence ...")
displayContactSequence(v,configs)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
rootJointType = 'freeflyer'
packageName = 'talos-rbprm'
meshPackageName = 'talos-rbprm'
urdfName = 'talos_trunk'
urdfNameRom = ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = 0.3
aMax = 0.1
extraDof = 6
mu=0.5
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.95, 1.05])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['talos_lleg_rom','talos_rleg_rom'])
rbprmBuilder.setAffordanceFilter('talos_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver and the viewer
ps = ProblemSolver( rbprmBuilder )
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("DynamicPlanner/sizeFootX",0.2)
ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps.setParameter("DynamicPlanner/friction",0.5)
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
p_lLeg = [-0.008846952891378526, 0.0848172440888579,-1.019272022956703]
p_lLeg[0]=0. # assure symetry of dynamic constraints on flat ground
p_rLeg = [-0.008846952891378526,-0.0848172440888579,-1.019272022956703]
p_rLeg[0] = 0.
p_lArm = [0.13028765672452458, 0.44360498616312666,-0.2881211563246389]
p_rArm = [0.13028765672452458,- 0.44360498616312666,-0.2881211563246389]
rbprmBuilder.setReferenceEndEffector('talos_lleg_rom',p_lLeg)
rbprmBuilder.setReferenceEndEffector('talos_rleg_rom',p_rLeg)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel ("hpp-rbprm-corba", "ground", "planning", vf)
v = vf.createViewer(displayArrows = True)
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init[3:7] = [0,0,0,1]
q_init [0:3] = [0, 0, 1.]
v (q_init)
# set goal config
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
q_goal[0] = 1.5
v(q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer ("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
t = ps.solve ()
print "Guide planning time : ",t
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.03
pp.displayVelocityPath(0)
v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp(0)
q_far = q_init[::]
q_far[2] = -2
v(q_far)
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from display_tools import *
import time
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
print "Plan guide trajectory ..."
import talos_navBauzil_path as tp
print "Done."
import time
##
# Information to retrieve urdf and srdf files.
packageName = "talos_data"
meshPackageName = "talos_data"
rootJointType = "freeflyer"
urdfName = "talos"
urdfSuffix = "_reduced"
srdfSuffix = ""
rLegId = 'talos_rleg_rom'
rleg = 'leg_right_1_joint'
rfoot = 'leg_right_6_joint'
lLegId = 'talos_lleg_rom'
lleg = 'leg_left_1_joint'
lfoot = 'leg_left_6_joint'
rArmId = 'talos_rarm_rom'
rarm = 'arm_right_1_joint'
rhand = 'arm_right_7_joint'
lArmId = 'talos_larm_rom'
larm = 'arm_left_1_joint'
lhand = 'arm_left_7_joint'
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.2
root_bounds[1] += 0.2
root_bounds[2] -= 0.2
root_bounds[3] += 0.2
root_bounds[-1] = 1.2
root_bounds[-2] = 0.8
fullBody.setJointBounds ("root_joint", root_bounds)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
q_ref = [
0.0, 0.0, 1.0232773, 0.0 , 0.0, 0.0, 1., #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
0.0 , 0.006761, #Chest
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1,-0.005, #Left Arm
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005,#Right Arm
0., 0. , #Head
0,0,0,0,0,0]; v (q_ref)
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 10000
rLegOffset = [0., -0.00018, -0.107]
rLegOffset[2] += 0.006
rLegNormal = [0,0,1]
rLegx = 0.1; rLegy = 0.06
fullBody.addLimb(rLegId,rleg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset = [0., -0.00018, -0.107]
lLegOffset[2] += 0.006
lLegNormal = [0,0,1]
lLegx = 0.1; lLegy = 0.06
fullBody.addLimb(lLegId,lleg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")