Slalom contact sequence ~ OK

script/dynamic/slalom_hrp2_interKino.py

@@ -19,7 +19,7 @@ pId = tp.ps.numberPaths() -1
 fullBody = FullBody ()
 
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-fullBody.setJointBounds ("base_joint_xyz",  [-5.5,5.5, -2.5, 2.5, 0.55, 0.6])
+fullBody.setJointBounds ("base_joint_xyz",  [-5.5,5.5, -2.5, 2.5, 0.55, 0.65])
 fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
 
 ps = tp.ProblemSolver( fullBody )
@@ -28,16 +28,17 @@ ps.client.problem.setParameter("vMax",omniORB.any.to_any(tp.vMax))
 
 r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
 
-q_init =[0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,0.0, 0.0, 0.0, 0.0,0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init)
+q_init =[0, 0, 0.648702, 1.0, 0.0 , 0.0, 0.0,0.0, 0.0, 0.0, 0.0,0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init)
 q_ref = q_init[::]
 fullBody.setCurrentConfig (q_init)
 fullBody.setReferenceConfig (q_ref)
-fullBody.setStaticStability(False)
+
 
 
 #~ AFTER loading obstacles
 rLegId = 'hrp2_rleg_rom'
 lLegId = 'hrp2_lleg_rom'
+tStart = time.time()
 
 
 rLeg = 'RLEG_JOINT0'
@@ -48,9 +49,6 @@ fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "for
 fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
 
-
-
-
 lLeg = 'LLEG_JOINT0'
 lLegOffset = [0,0,-0.105]
 lLegNormal = [0,0,1]
@@ -58,10 +56,16 @@ lLegx = 0.09; lLegy = 0.05
 fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1)
 fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
+tGenerate =  time.time() - tStart
+print "generate databases in : "+str(tGenerate)+" s"
 
-#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
-#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
 
+"""
+fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
+fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
+tLoad =  time.time() - tStart
+print "Load databases in : "+str(tLoad)+" s"
+"""
 
 
 q_0 = fullBody.getCurrentConfig(); 
@@ -72,11 +76,11 @@ q_0 = fullBody.getCurrentConfig();
 
 configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
 
-q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(pId,0)[0:7] # use this to get the correct orientation
 q_goal = fullBody.getCurrentConfig(); 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_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
@@ -93,6 +97,7 @@ q_goal[2] = q_goal[2]+0.1
 
 
 # Randomly generating a contact configuration at q_init
+fullBody.setStaticStability(True)
 fullBody.setCurrentConfig (q_init)
 r(q_init)
 q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
@@ -110,7 +115,7 @@ r(q_init)
 
 fullBody.setStartState(q_init,[rLegId,lLegId])
 fullBody.setEndState(q_goal,[rLegId,lLegId])
-
+fullBody.setStaticStability(False) # only set it after the init/goal configuration are computed
 
 
 
@@ -120,11 +125,11 @@ pp = PathPlayer (fullBody.client.basic, r)
 import fullBodyPlayerHrp2
 
 tStart = time.time()
-configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 1, filterStates = True)
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True)
 tInterpolate = time.time()-tStart
 print "number of configs : ", len(configs)
 print "generated in "+str(tInterpolate)+" s"
-r(configs[len(configs)-1])
+r(configs[len(configs)-2])
 
 
 player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=1,optim_effector=True,use_velocity=False,pathId = pId)
@@ -141,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,r)
+cs = generateContactSequence(fullBody,configs[0:3],r)
 filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
 cs.saveAsXML(filename, "ContactSequence")
 print "save contact sequence : ",filename

script/tools/screenCap.py

@@ -45,7 +45,7 @@ tp.r.stopCapture ()
 """
 
 ## avconv (bash) commands
-avconv -i capture_0_%d.png  -r 25 -vcodec mpeg4 -filter:v "setpts=2.*PTS" -qscale 1 -mbd rd -flags +mv4+aic -trellis 2 -cmp 2 -subcmp 2 -g 300 -y comRRT_flatGroundV2.mp4
+avconv -i capture_0_%d.png  -r 25 -vcodec mpeg4 -filter:v "setpts=2.*PTS" -qscale 1 -mbd rd -flags +mv4+aic -trellis 2 -cmp 2 -subcmp 2 -g 300 -y slalom_contact_sequence0.mp4
 
 avconv -i capture_0_%d.png -r 30 -vcodec mpeg4 -qscale 1 -mbd rd -flags +mv4+aic -trellis 2 -cmp 2 -subcmp 2 -g 300 -pass 2  hyq_darpa.mp4