diff --git a/script/dynamic/flatGround_hyq_interpKino.py b/script/dynamic/flatGround_hyq_interpKino.py index 2e90c8cd6fc17e737f404d93890d8f1f4ccbbfb1..faf904583a3e5c8f10e52c3cf03ff0ff149bb233 100644 --- a/script/dynamic/flatGround_hyq_interpKino.py +++ b/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() diff --git a/script/dynamic/flatGround_hyq_pathKino.py b/script/dynamic/flatGround_hyq_pathKino.py index 5cc61175107362e6031025d257ef5230445b71b2..ee41b7a460dc5d92433564b7787f379fb4fa2398 100644 --- a/script/dynamic/flatGround_hyq_pathKino.py +++ b/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