hyq darpa

CMakeLists.txt

@@ -83,6 +83,8 @@ install(FILES
   data/urdf/climb.urdf
   data/urdf/stepladder.urdf
   data/urdf/ground.urdf
+  data/urdf/groundcrouch.urdf
+  data/urdf/darpa.urdf
   #~ data/urdf/scene2.urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
   )
@@ -109,10 +111,14 @@ install(FILES
   data/srdf/climb.srdf
   data/srdf/stepladder.srdf
   data/srdf/ground.srdf
+  data/srdf/groundcrouch.srdf
+  data/srdf/darpa.srdf
   #~ data/srdf/scene2.srdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
   )
 install(FILES
+	data/meshes/darpa.stl
+	data/meshes/darpareduced.stl
 	data/meshes/car.stl
 	data/meshes/car_simple.stl
 	data/meshes/hrp2_trunk.stl
@@ -129,7 +135,7 @@ install(FILES
 	data/meshes/robot_box.stl
 	data/meshes/chair_simple.stl
 	data/meshes/ground.stl
-	data/meshes/ground_table.stl
+	data/meshes/groundcrouch.stl
 	data/meshes/truck.stl
 	data/meshes/truck_vision.stl
 	data/meshes/stair_bauzil.stl

data/srdf/darpa.srdf

+<?xml version="1.0"?>
+<robot name="darpa">
+  <handle name="handle">
+    <position> 0 0 0 1 0 0 0 </position>
+    <link name="base_link"/>
+  </handle>
+  <handle name="handle2">
+    <position> 0 0 0
+      0 0 0.7071067811865476 0.7071067811865476 </position>
+    <link name="base_link"/>
+  </handle>
+  <contact name="box_surface">
+    <link name="base_link"/>
+    <point>-0.025 -0.025 -0.025   -0.025 0.025 -0.025   -0.025 -0.025 0.025   -0.025 0.025 0.025
+           +0.025 -0.025 -0.025   +0.025 0.025 -0.025   +0.025 -0.025 0.025   +0.025 0.025 0.025 </point>
+    <triangle> 0 2 1 4 5 6</triangle>
+  </contact>
+</robot>

data/srdf/groundcrouch.srdf

+<?xml version="1.0"?>
+<robot name="groundcrouch">
+  <handle name="handle">
+    <position> 0 0 0 1 0 0 0 </position>
+    <link name="base_link"/>
+  </handle>
+  <handle name="handle2">
+    <position> 0 0 0
+      0 0 0.7071067811865476 0.7071067811865476 </position>
+    <link name="base_link"/>
+  </handle>
+  <contact name="box_surface">
+    <link name="base_link"/>
+    <point>-0.025 -0.025 -0.025   -0.025 0.025 -0.025   -0.025 -0.025 0.025   -0.025 0.025 0.025
+           +0.025 -0.025 -0.025   +0.025 0.025 -0.025   +0.025 -0.025 0.025   +0.025 0.025 0.025 </point>
+    <triangle> 0 2 1 4 5 6</triangle>
+  </contact>
+</robot>

data/urdf/darpa.urdf

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

data/urdf/groundcrouch.urdf

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

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -106,6 +106,13 @@ module hpp
     ///
     void setNormalFilter(in string romName, in floatSeq normal, in double range) raises (Error);
 
+    /// Sets limits on robot orientation, described according to Euler's ZYX rotation order
+    ///
+    /// \param limitszyx 6D vector with the lower and upperBound for each rotation axis in sequence
+    /// expressed in gradients
+    /// [z_inf, z_sup, y_inf, y_sup, x_inf, x_sup]
+    void boundSO3(in floatSeq limitszyx) raises (Error);
+
     /// Get Sample configuration by its id
     /// \param sampleName name of the limb from which to retrieve a sample
     /// \param sampleId id of the desired samples

script/scenarios/darpa_hyq_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import darpa_hyq_path as tp
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 10000
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#~ AFTER loading obstacles
+rLegId = 'rfleg'
+rLeg = 'rf_haa_joint'
+rfoot = 'rf_foot_joint'
+rLegOffset = [0.,0,0.]
+rLegNormal = [0,1,0]
+rLegx = 0.02; rLegy = 0.02
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.05)
+#~ fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.05)
+#~ fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "random", 0.05)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+lLegOffset = [0,0,0]
+lLegNormal = [0,1,0]
+lLegx = 0.02; lLegy = 0.02
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "forward", 0.05)
+#~ fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.05)
+#~ fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "random", 0.05)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+rArmOffset = [0.,0,-0.]
+rArmNormal = [0,1,0]
+rArmx = 0.02; rArmy = 0.02
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "forward", 0.05)
+#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.05)
+#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "random", 0.05)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+lArmOffset = [0.,0,-0.]
+lArmNormal = [0,1,0]
+lArmx = 0.02; lArmy = 0.02
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05)
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "manipulability", 0.05)
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "random", 0.05)
+
+q_0 = fullBody.getCurrentConfig(); 
+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])
+q_0 = fullBody.getCurrentConfig(); 
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+fullBody.setStartState(q_init,[])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+r(q_init)
+
+configs = fullBody.interpolate(0.1)
+
+r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
+
+i = 0;
+#~ r (configs[i]); i=i+1; i-1
+
+q0 = configs[2]
+q0 = fullBody.generateContacts(q0, [0,0,1])
+#~ r(q0)
+q_init [0:3] = [0, 0, 0.63]; r(q_init)
+c = fullBody.getContactSamplesIds("rfleg",q_init, [1,0,0])
+r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
+
+

script/scenarios/darpa_hyq_path.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hyq_trunk'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0, 4])
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.5)
+rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.5)
+rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.5)
+rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.5)
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-1.5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [0:3] = [2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+q_goal = q_init [::]
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
+r.loadObstacleModel (packageName, "darpa", "planning")
+ps.solve ()
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+#~ 
+#~ pp (2)
+#~ pp (0)
+
+pp (1)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+r (q_init)

script/scenarios/ground_crouch_hyq_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import ground_crouch_hyq_path as tp
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 10000
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#~ AFTER loading obstacles
+rLegId = 'rfleg'
+rLeg = 'rf_haa_joint'
+rfoot = 'rf_foot_joint'
+rLegOffset = [0.,0,0.]
+rLegNormal = [0,1,0]
+rLegx = 0.02; rLegy = 0.02
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "random", 0.03)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+lLegOffset = [0,0,0]
+lLegNormal = [0,1,0]
+lLegx = 0.02; lLegy = 0.02
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "random", 0.03)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+rArmOffset = [0.,0,-0.]
+rArmNormal = [0,1,0]
+rArmx = 0.02; rArmy = 0.02
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "random", 0.03)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+lArmOffset = [0.,0,-0.]
+lArmNormal = [0,1,0]
+lArmx = 0.02; lArmy = 0.02
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "random", 0.03)
+
+q_0 = fullBody.getCurrentConfig(); 
+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])
+q_0 = fullBody.getCurrentConfig(); 
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+fullBody.setStartState(q_init,[])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+r(q_init)
+
+#~ configs = fullBody.interpolate(0.1)
+
+r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
+
+i = 0;
+r (configs[i]); i=i+1; i-1
+
+q0 = configs[2]
+q0 = fullBody.generateContacts(q0, [0,0,1])
+r(q0)
+c = fullBody.getContactSamplesIds("rfleg",q_init, [0,0,1])
+r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
+

script/scenarios/ground_crouch_hyq_path.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hyq_trunk'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lhleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,5, -1, 1, 0, 2])
+rbprmBuilder.setFilter(['hyq_lhleg_rom' , 'hyq_rfleg_rom'])
+rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
+rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
+rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
+rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
+rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-1, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
+
+q_goal = q_init [::]
+#~ q_goal [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
+q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
+#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+r.loadObstacleModel (packageName, "groundcrouch", "planning")
+ps.solve ()
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+#~ 
+#~ pp (2)
+#~ pp (0)
+
+pp (1)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+r (q_init)

src/rbprmbuilder.impl.cc

@@ -319,7 +319,7 @@ namespace hpp {
             for(model::ObjectVector_t::const_iterator oit = problemSolver_->collisionObstacles().begin();
                 oit != problemSolver_->collisionObstacles().end(); ++oit, ++i)
             {
-                sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i]);
+                sampling::GetCandidates(limb->sampleContainer_, transform, transform, *oit, dir, reports[i]);
             }
             for(std::vector<sampling::T_OctreeReport>::const_iterator cit = reports.begin();
                 cit != reports.end(); ++cit)