finally good normals for contacts

script/loadhrp2crawl.py

@@ -26,24 +26,24 @@ r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
 #~ AFTER loading obstacles
 rLeg = 'RLEG_JOINT0'
 rKnee = 'RLEG_JOINT3'
-rLegOffset = [0.055,0.105,0.017]
-rLegNormal = [0,-1,0]
+rLegOffset = [0.105,0.055,0.017]
+rLegNormal = [-1,0,0]
 rLegx = 0.05; rLegy = 0.05
 fullBody.addLimb(rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 5000, 0.01)
 
 lLeg = 'LLEG_JOINT0'
 lKnee = 'LLEG_JOINT3'
-lLegOffset = [0.105,0.055,-0.017]
-lLegNormal = [0,-1,0]
+lLegOffset = [0.105,0.055,0.017]
+lLegNormal = [-1,0,0]
 lLegx = 0.05; lLegy = 0.05
-fullBody.addLimb(lLeg,lKnee,lLegOffset,rLegNormal, lLegx, lLegy, 5000, 0.01)
+fullBody.addLimb(lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 5000, 0.01)
 #~  	
 
 #~ AFTER loading obstacles
 rarm = 'RARM_JOINT0'
 rHand = 'RARM_JOINT5'
 rArmOffset = [0.03,-0.050,-0.050]
-rArmNormal = [0,1,0]
+rArmNormal = [1,0,0]
 rArmx = 0.024; rArmy = 0.024
 fullBody.addLimb(rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 5000, 0.01)
 
@@ -52,7 +52,7 @@ fullBody.addLimb(rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 5000, 0.01)
 larm = 'LARM_JOINT0'
 lHand = 'LARM_JOINT5'
 lArmOffset = [0.03,-0.050,-0.050]
-lArmNormal = [0,1,0]
+lArmNormal = [1,0,0]
 lArmx = 0.024; lArmy = 0.024
 fullBody.addLimb(larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 5000, 0.01)
 
@@ -67,17 +67,45 @@ fullBody.client.basic.robot.setJointConfig('RLEG_JOINT3',[1.5])
 fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
 
 q_init = fullBody.getCurrentConfig (); r (q_init)
-q_init [0:3] = [0, -0.5, 0.2]; fullBody.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [0, -0.5, 0.3]; fullBody.setCurrentConfig (q_init); r (q_init)
+
+
+q_goal = q_init [::]
+q_goal [0:3] = [0.1, -0.5, 0.3]
+
+#~ ps.setInitialConfig (q_init)
+#~ ps.addGoalConfig (q_goal)
+#~ r(q_goal)
+#~ ps.solve ()
+#~ 
+#~ from hpp.gepetto import PathPlayer
+#~ pp = PathPlayer (fullBody.client.basic, r)
+
+#~ pp (0)
+
+q_init = fullBody.generateContacts(q_init, [0,0,-1])
+r (q_init)
+
+#~ fullBody.setCurrentConfig (q_goal)
+#~ q_goal = fullBody.generateContacts(q_goal, [0,0,-1])
+#~ 
+#~ fullBody.setStartState(q_init,[rLeg,lLeg,rarm,larm])
+#~ fullBody.setEndState(q_goal,[rLeg,lLeg,rarm,larm])
+#~ 
+#~ configs = fullBody.interpolate(0.005)
+#~ i = 0; 
+#~ r (configs[i]); i=i+1; i-1
+
 #~ configs = fullBody.getContactSamplesIds(rLeg, q_init, [0,1,0])
 #~ i = 0
 #~ q_init = fullBody.getSample(rLeg, int(configs[i])); i = i+1;r(q_init)
 #~ 
 #~ fullBody.setCurrentConfig (q_init)
-q_init = fullBody.generateContacts(q_init, [-0.1,0,1]) ; r(q_init)
+#~ q_init = fullBody.generateContacts(q_init, [-0.1,0,1]) ; r(q_init)
 #~ r (q_init)
-
-q_goal = q_init [::]
-q_goal [0:3] = [1, -0.5, 0.6]
+#~ 
+#~ q_goal = q_init [::]
+#~ q_goal [0:3] = [1, -0.5, 0.6]
 
 #~ r (q_0)
 #~ fullBody.setCurrentConfig (q_0)