hrp2_wall_interp work

script/dynamic/fullBodyPlayer.py

@@ -84,13 +84,13 @@ class Player (object):
 	    end = time.clock() 
 	    print "Contact plan generated in " + str(end-start) + "seconds"
 	
-    def displayContactPlan(self):
+    def displayContactPlan(self,pause = 0.5):
 	    self.viewer.client.gui.setVisibility("hyq", "ON")
 	    self.tp.r.client.gui.setVisibility("toto", "OFF")
 	    self.tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
 	    for i in range(0,len(self.configs)):
 		    self.viewer(self.configs[i]);
-		    time.sleep(0.5)		
+		    time.sleep(pause)		
 		
     def interpolate(self,begin=1,end=0):
         if end==0:

script/dynamic/hrp2_wall_path.py

@@ -31,7 +31,7 @@ srdfSuffix = ""
 rbprmBuilder = Builder ()
 
 rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,3, -2, 0, 0.3, 1])
 rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
 rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean'])
 rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',])
@@ -66,15 +66,16 @@ afftool.visualiseAffordances('Lean', r, r.color.blue)
 
 
 q_init = rbprmBuilder.getCurrentConfig ();
-q_init[0:3] = [2.1, -1, 0.45];
+#q_init[0:3] = [2.1, -1, 0.58];
+q_init[0:3] = [2, -1, 0.58];
 q_init[3:7] = [0.7071,0,0,0.7071]
 q_init[indexECS:indexECS+3] = [2,0,0]
 rbprmBuilder.setCurrentConfig (q_init); r (q_init)
 
 
 q_goal = q_init [::]
-q_goal [0:3] = [1, -1, 0.45]; 
-q_goal[indexECS:indexECS+3] = [0,0,0]
+q_goal [0:3] = [1.2, -1, 0.58]; 
+q_goal[indexECS:indexECS+3] = [-1,0,0]
 
 
 #r(q_goal)
@@ -90,23 +91,26 @@ ps.selectSteeringMethod("RBPRMKinodynamic")
 ps.selectDistance("KinodynamicDistance")
 ps.selectPathPlanner("DynamicPlanner")
 
+#t = ps.solve ()
+
+
 ps.client.problem.prepareSolveStepByStep()
 
 
 ps.client.problem.finishSolveStepByStep()
 
 
-#t = ps.solve ()
+
 
 
 # Playing the computed path
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
-pp.dt=1/30.
+pp.dt=1./30.
 #r.client.gui.removeFromGroup("rm0",r.sceneName)
 pp.displayVelocityPath(0)
-pp.speed=0.2
-pp(0)
+pp.speed=0.5
+#pp(0)
 
 
 

script/dynamic/wall_hrp2_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import hrp2_wall_path as tp
+import time
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [0,3, -2, 0, 0.3, 1])
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps,viewerClient=tp.r.client)
+
+#~ AFTER loading obstacles
+rLegId = '0rLeg'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,-0.105,0,]
+rLegNormal = [0,1,0]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "manipulability", 0.1)
+
+lLegId = '1lLeg'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.105,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "manipulability", 0.1)
+
+rarmId = '3Rarm'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [0,0,-0.1]
+rArmNormal = [0,0,1]
+rArmx = 0.024; rArmy = 0.024
+#disabling collision for hook
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 50000, "EFORT_Normal", 0.05, "_6_DOF", True)
+
+"""
+
+#~ AFTER loading obstacles
+larmId = '4Larm'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [-0.05,-0.050,-0.050]
+lArmNormal = [1,0,0]
+lArmx = 0.024; lArmy = 0.024
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
+
+rKneeId = '0RKnee'
+rLeg = 'RLEG_JOINT0'
+rKnee = 'RLEG_JOINT3'
+rLegOffset = [0.105,0.055,0.017]
+rLegNormal = [-1,0,0]
+rLegx = 0.05; rLegy = 0.05
+#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
+#~ 
+lKneeId = '1LKnee'
+lLeg = 'LLEG_JOINT0'
+lKnee = 'LLEG_JOINT3'
+lLegOffset = [0.105,0.055,0.017]
+lLegNormal = [-1,0,0]
+lLegx = 0.05; lLegy = 0.05
+#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
+ #~ 
+
+#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+
+#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
+#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+
+
+"""
+
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
+
+
+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)
+fullBody.setCurrentConfig (q_init)
+
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:7]
+dir_init = tp.ps.configAtParam(0,0.01)[tp.indexECS:tp.indexECS+3]
+acc_init = tp.ps.configAtParam(0,0.01)[tp.indexECS+3:tp.indexECS+6]
+dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[tp.indexECS:tp.indexECS+3]
+acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[tp.indexECS+3:tp.indexECS+6]
+
+
+
+fullBody.setStaticStability(False)
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init)
+r(q_init)
+q_init = fullBody.generateContacts(q_init,dir_init,acc_init)
+r(q_init)
+
+# Randomly generating a contact configuration at q_end
+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
+
+r(q_init)
+
+
+fullBody.setStartState(q_init,[rLegId,lLegId,rarmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])
+
+
+
+configs = fullBody.interpolate(0.08,pathId=0,robustnessTreshold = 1, filterStates = True)
+print "number of configs :", len(configs)
+
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+from fullBodyPlayer import Player
+player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=True,pathId = 0)
+
+player.displayContactPlan()
+
+
+

script/scenarios/demos/stair_bauzil_hrp2_interp.py

@@ -19,7 +19,7 @@ srdfSuffix = ""
 fullBody = FullBody ()
 
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,3, -2, 0, 0.3, 1])
 
 
 ps = tp.ProblemSolver( fullBody )
@@ -31,14 +31,14 @@ rLeg = 'RLEG_JOINT0'
 rLegOffset = [0,-0.105,0,]
 rLegNormal = [0,1,0]
 rLegx = 0.09; rLegy = 0.05
-fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "manipulability", 0.1)
 
 lLegId = '1lLeg'
 lLeg = 'LLEG_JOINT0'
 lLegOffset = [0,-0.105,0]
 lLegNormal = [0,1,0]
 lLegx = 0.09; lLegy = 0.05
-fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "manipulability", 0.1)
 
 rarmId = '3Rarm'
 rarm = 'RARM_JOINT0'
@@ -47,7 +47,7 @@ rArmOffset = [0,0,-0.1]
 rArmNormal = [0,0,1]
 rArmx = 0.024; rArmy = 0.024
 #disabling collision for hook
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 50000, "manipulability", 0.05, "_3_DOF", True)
 
 
 #~ AFTER loading obstacles