stair

data/urdf/box_rom.urdf

-<robot name="hrp2_rom">
+<robot name="box">
   <link name="base_link">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rom.stl"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl"/>
       </geometry>
       <material name="white">
         <color rgba="1 1 1 1"/>
@@ -12,7 +12,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rom.stl"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl"/>
       </geometry>
     </collision>
   </link>

data/urdf/hrp2_rom.urdf

+<robot name="hrp2_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0.003" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rom.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0.003" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

data/urdf/hrp2_trunk.urdf

 <robot name="hrp2_trunk">
   <link name="base_link">
     <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0.003" />
       <geometry>
         <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk.stl"/>
       </geometry>
@@ -10,7 +10,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0.003" />
       <geometry>
         <mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk.stl"/>
       </geometry>

script/loadhrp2stair.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import stair_path_hrp2 as tp
+
+
+
+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", [-1,1, -4, -1, 1, 2.2])
+
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+r.loadObstacleModel ('hpp-rbprm-corba', "scene_wall", "contact")
+
+#~ AFTER loading obstacles
+rLegId = '7rLeg'
+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, 0.01)
+
+lLegId = '8lLeg'
+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, 0.01)
+
+rarmId = '3Rarm'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [-0.05,-0.050,-0.050]
+rArmNormal = [1,0,0]
+rArmx = 0.024; rArmy = 0.024
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, 0.01)
+
+
+#~ 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, 20000, 0.01)
+
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+
+fullBody.setCurrentConfig (q_init)
+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,[rLegId,lLegId,rarmId,larmId])
+#~ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+#~ 
+#~ configs = fullBody.interpolate(0.1)
+#~ configs2 = fullBody.interpolate(0.05)
+i = 0; 
+r (configs[i]); i=i+1; i-1
+
+

script/loadhrp2truck.py

@@ -2,6 +2,9 @@ from hpp.corbaserver.rbprm.rbprmbuilder import Builder
 from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
 from hpp.gepetto import Viewer
 
+import truck_path_hrp2 as tp
+
+
 
 packageName = "hrp2_14_description"
 meshPackageName = "hrp2_14_description"
@@ -17,13 +20,18 @@ fullBody = FullBody ()
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
 fullBody.setJointBounds ("base_joint_xyz", [-1,1, -4, -1, 1, 2.2])
 
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+#~ ps = ProblemSolver( fullBody )
+#~ ps.addPathOptimizer("RandomShortcut")
+#~ ps.addPathOptimizer("GradientBased")
+#~ r = Viewer (ps)
+#~ r.loadObstacleModel ('hpp-rbprm-corba', "truck", "car")
+
 
-ps = ProblemSolver( fullBody )
-ps.addPathOptimizer("RandomShortcut")
-ps.addPathOptimizer("GradientBased")
-r = Viewer (ps)
-r.loadObstacleModel ('hpp-rbprm-corba', "truck", "car")
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+r.loadObstacleModel ('hpp-rbprm-corba', "truck", "contact")
 
 #~ AFTER loading obstacles
 rLegId = '7rLeg'
@@ -87,23 +95,26 @@ q_0 = fullBody.getCurrentConfig (); r (q_0)
 
 q_init = fullBody.getCurrentConfig (); r (q_init)
 
-fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
+#~ 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:6] = [0.0, -2.1, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; fullBody.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:7] = [0.0, -4.0, 2.0, 1.0, 0.0, 0.0, 0.0]
+
 
-q_goal = [0.0, -4.0, 2.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0]
- 
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-r(q_goal)
-ps.solve ()
+q_init = fullBody.generateContacts(q_init, [0,0,-1])
+r (q_init)
 
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (fullBody.client.basic, r)
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,-1])
 
-#~ pp (1)
+fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+#~ 
+configs = fullBody.interpolate(0.5)
+#~ configs2 = fullBody.interpolate(0.05)
+i = 0; 
+r (configs[i]); i=i+1; i-1
 
-#~ pp.toFile(0, "/home/stonneau/test.txt")
 

script/stair_path_hrp2.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hrp2_trunk'
+urdfNameRom = 'hrp2_rom'
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,1, -1, 1, 0, 2.2])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
+
+#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-0.3, 0, 0.7]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0)
+
+
+q_goal = q_init [::]
+q_goal [0:3] = [0.1, 0, 1.7]; r (q_goal)
+#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; 
+#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+
+ps.addPathOptimizer("RandomShortcut")
+ps.addPathOptimizer("GradientBased")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+#~ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+r.loadObstacleModel (packageName, "scene_wall", "planning")
+ps.solve ()
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ 
+pp (2)
+#~ pp (1)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
+#~ pp (1)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
+

script/truck_path_hrp2.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hrp2_trunk'
+urdfNameRom = 'hrp2_rom'
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,1, -4, -1, 1, 2.2])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
+
+#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
+
+q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
+q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+q_goal = [0.0, -4.0, 2.0, 1.0, 0.0, 0.0, 0.0]
+
+
+ps.addPathOptimizer("RandomShortcut")
+#~ ps.addPathOptimizer("GradientBased")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+#~ ps.client.problem.selectConFigurationShooter("RbprmShooter")
+#~ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+r.loadObstacleModel (packageName, "truck", "planning")
+ps.solve ()
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+
+#~ pp (0)
+#~ pp (1)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
+pp (1)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
+

src/rbprmbuilder.impl.cc

@@ -233,10 +233,12 @@ namespace hpp {
                 dir[i] = direction[i];
             }
             model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+std::cout << "configuration loaded " << config << std::endl;
             rbprm::State state = rbprm::ComputeContacts(fullBody_,config,
                                             problemSolver_->collisionObstacles(), dir);
             hpp::floatSeq* dofArray = new hpp::floatSeq();
-            dofArray->length(_CORBA_ULong(config.rows()));
+std::cout << "configuration out " << state.configuration_ << std::endl;
+            dofArray->length(_CORBA_ULong(state.configuration_.rows()));
             for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
               (*dofArray)[(_CORBA_ULong)i] = state.configuration_ [i];
             return dofArray;