diff --git a/script/dynamic/slalom_hrp2_interKino.py b/script/dynamic/slalom_hrp2_interKino.py index 0796579eb8039ebe3dba9924908dc48b38cb7da1..a4c9f48d0ace21306e4d78054c8720fdf68dfd36 100644 --- a/script/dynamic/slalom_hrp2_interKino.py +++ b/script/dynamic/slalom_hrp2_interKino.py @@ -45,7 +45,7 @@ rLeg = 'RLEG_JOINT0' rLegOffset = [0,0,-0.105] rLegNormal = [0,0,1] rLegx = 0.09; rLegy = 0.05 -fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1) +fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.05,"_6_DOF") fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True) #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db") @@ -53,7 +53,7 @@ lLeg = 'LLEG_JOINT0' lLegOffset = [0,0,-0.105] lLegNormal = [0,0,1] lLegx = 0.09; lLegy = 0.05 -fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1) +fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.05,"_6_DOF") fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True) #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db") tGenerate = time.time() - tStart @@ -83,7 +83,7 @@ acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6] dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3] acc_goal = [0,0,0] -robTreshold = 1 +robTreshold = 5 # copy extraconfig for start and init configurations q_init[configSize:configSize+3] = dir_init[::] q_init[configSize+3:configSize+6] = acc_init[::] @@ -125,7 +125,7 @@ pp = PathPlayer (fullBody.client.basic, r) import fullBodyPlayerHrp2 tStart = time.time() -configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True) +configs = fullBody.interpolate(0.02,pathId=pId,robustnessTreshold = robTreshold, filterStates = True) tInterpolate = time.time()-tStart print "number of configs : ", len(configs) print "generated in "+str(tInterpolate)+" s" @@ -146,7 +146,7 @@ player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window= from planning.config import * from generate_contact_sequence import * -cs = generateContactSequence(fullBody,configs[0:3],r) +cs = generateContactSequence(fullBody,configs,r) filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE cs.saveAsXML(filename, "ContactSequence") print "save contact sequence : ",filename diff --git a/script/dynamic/slalom_hrp2_pathKino.py b/script/dynamic/slalom_hrp2_pathKino.py index 8e5d53b05d0db7053cd9288f5b562f28e7610736..f886fe8a8bff36647d596edc2897df4d05a168f1 100644 --- a/script/dynamic/slalom_hrp2_pathKino.py +++ b/script/dynamic/slalom_hrp2_pathKino.py @@ -50,8 +50,8 @@ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom']) #rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean']) rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',]) rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support']) -vMax = 1.; -aMax = 5.; +vMax = 0.3; +aMax = 0.5; extraDof = 6 rbprmBuilder.setJointBounds ("base_joint_xyz", [-5.5,5.5, -2.5, 2.5, 0.55, 0.6]) @@ -163,7 +163,7 @@ pp.dt=0.03 pp.displayVelocityPath(pi) r.client.gui.setVisibility("path_"+str(pi)+"_root","ALWAYS_ON_TOP") #display path -pp.speed=0.5 +pp.speed=1 #pp (0) diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index 8740520ede39419358559201dbad1035accb93b6..d6f6c1be1c6d9c8b3d34ce1cc69bb0327a977797 100755 --- a/src/rbprmbuilder.impl.cc +++ b/src/rbprmbuilder.impl.cc @@ -2037,7 +2037,7 @@ assert(s2 == s1 +1); } core::PathVectorPtr_t resPath = core::PathVector::create(fullBody()->device_->configSize(), fullBody()->device_->numberDof()); - + hppDout(notice,"ComRRT : projections done. begin rrt"); try{ hppDout(notice,"begin comRRT states 1 and 1bis"); core::PathPtr_t p1 = interpolation::comRRT(fullBody(),problemSolver()->problem(), paths[cT1],