use the same extraCOnfig in interp script as in path script, and copy them

script/dynamic/flatGround_hyq_interpKino.py

@@ -24,6 +24,7 @@ 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
@@ -58,7 +59,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]
-
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
 #fullBody.setStaticStability(False)
 # Randomly generating a contact configuration at q_init
 fullBody.setCurrentConfig (q_init)
@@ -68,8 +69,13 @@ 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.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
 fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
 
 
@@ -90,7 +96,7 @@ player = Player(fullBody,pp,tp,configs)
 
 #player.displayContactPlan()
 
-player.interpolate(2,10)
-
+player.interpolate(20,75)
 
+player.play()
 

script/dynamic/flatGround_hyq_pathKino.py

@@ -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