hyq model

CMakeLists.txt

@@ -140,6 +140,7 @@ install(FILES
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
   )
 install(FILES
+	data/meshes/hyq/hyq_all.stl
 	data/meshes/hyq/hyq_trunk.stl
 	data/meshes/hyq/hyq_rom.stl
 	data/meshes/hyq/hyq_rhleg_rom.stl

data/urdf/hyq/hyq_trunk.urdf

@@ -3,7 +3,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_trunk.stl"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_all.stl"/>
       </geometry>
       <material name="white">
         <color rgba="1 1 1 1"/>

script/scenarios/stair_bauzil_hyq_interp.py

@@ -2,81 +2,63 @@ from hpp.corbaserver.rbprm.rbprmbuilder import Builder
 from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
 from hpp.gepetto import Viewer
 
-import stair_bauzil_hrp2_path as tp
+import stair_bauzil_hyq_path as tp
 
-
-
-packageName = "hrp2_14_description"
-meshPackageName = "hrp2_14_description"
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
 rootJointType = "freeflyer"
 ##
 #  Information to retrieve urdf and srdf files.
-urdfName = "hrp2_14"
-urdfSuffix = "_reduced"
+urdfName = "hyq"
+urdfSuffix = ""
 srdfSuffix = ""
 
 fullBody = FullBody ()
-
+ 
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
 
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 10000
 
 ps = tp.ProblemSolver( fullBody )
 r = tp.Viewer (ps)
 r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
 
+rootName = 'base_joint_xyz'
+
 #~ AFTER loading obstacles
-rLegId = '0rLeg'
-rLeg = 'RLEG_JOINT0'
-rLegOffset = [0,-0.105,0,]
+rLegId = 'rfleg'
+rLeg = 'rf_haa_joint'
+rfoot = 'rf_foot_joint'
+rLegOffset = [0,0,0]
 rLegNormal = [0,1,0]
-rLegx = 0.09; rLegy = 0.05
-fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "EFORT", 0.01)
+rLegx = 0.02; rLegy = 0.02
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "EFORT", 0.03)
 
-lLegId = '1lLeg'
-lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,-0.105,0]
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+lLegOffset = [0,0,0]
 lLegNormal = [0,1,0]
-lLegx = 0.09; lLegy = 0.05
-fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "EFORT", 0.01)
-
-rarmId = '3Rarm'
-rarm = 'RARM_JOINT0'
-rHand = 'RARM_JOINT5'
-rArmOffset = [0,0,-0.1]
-rArmNormal = [0,0,1]
-rArmx = 0.024; rArmy = 0.024
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 30000, "EFORT", 0.05)
-
-
-#~ AFTER loading obstacles
-larmId = '4Larm'
-larm = 'LARM_JOINT0'
-lHand = 'LARM_JOINT5'
-lArmOffset = [-0.05,-0.050,-0.050]
+lLegx = 0.02; lLegy = 0.02
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "EFORT", 0.03)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+rArmOffset = [0,0,0]
+rArmNormal = [1,0,0]
+rArmx = 0.02; rArmy = 0.02
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.03)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+lArmOffset = [0,0,0]
 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.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
-#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+lArmx = 0.02; lArmy = 0.02
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.03)
 
 q_0 = fullBody.getCurrentConfig(); 
 q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
@@ -84,33 +66,19 @@ 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(); 
-q_init =  [
-        0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,                         # Free flyer 0-6
-        0.0, 0.0, 0.0, 0.0,                                                  # CHEST HEAD 7-10
-        0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM       11-17
-        0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM       18-24
-        0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,               # LLEG       25-30
-        0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,               # RLEG       31-36
-        ]; r (q_init)
 
 fullBody.setCurrentConfig (q_goal)
-#~ r(q_goal)
 q_goal = fullBody.generateContacts(q_goal, [0,0,1])
-#~ r(q_goal)
 
-fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
-fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
-#~ 
-#~ configs = fullBody.interpolate(0.1)
+fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+r(q_init)
+
 configs = fullBody.interpolate(0.1)
-#~ configs = fullBody.interpolate(0.15)
+
 i = 0;
 r (configs[i]); i=i+1; i-1
-#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
-
-#~ f1 = open("secondchoice","w+")
-f1 = open("new","w+")
-f1.write(str(configs))
-f1.close()
 

script/scenarios/stair_bauzil_hyq_path.py

@@ -4,15 +4,15 @@ from hpp.gepetto import Viewer
 rootJointType = 'freeflyer'
 packageName = 'hpp-rbprm-corba'
 meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'hrp2_trunk'
-urdfNameRom = 'hrp2_rom'
+urdfName = 'hyq_trunk'
+urdfNameRom = 'hyq_rom'
 urdfSuffix = ""
 srdfSuffix = ""
 
 rbprmBuilder = Builder ()
 
 rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-5,2, -1, 1, 0, 2.2])
 
 #~ from hpp.corbaserver.rbprm. import ProblemSolver
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -23,14 +23,13 @@ r = Viewer (ps)
 
 
 q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [0, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [-2, 0, 0.5]; 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] = [1.49, -0.65, 1.25]; r (q_goal)
+#~ q_goal [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
+q_goal [0:3] = [0, 0, 0.5]; r (q_goal)
 #~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
 
 #~ ps.addPathOptimizer("GradientBased")

script/scenarios/standing_hrp2_interp.py

@@ -46,7 +46,7 @@ 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, 10000, "EFORT", 0.01)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "manipulability", 0.03)
 
 larmId = '4Larm'
 larm = 'LARM_JOINT0'
@@ -54,7 +54,7 @@ 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, "EFORT", 0.01)
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "manipulability", 0.03)
 
 rKneeId = '0RKnee'
 rLeg = 'RLEG_JOINT0'

script/scenarios/standing_hrp2_path.py

@@ -44,8 +44,8 @@ ps.addPathOptimizer("RandomShortcut")
 ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+#~ ps.client.problem.selectConFigurationShooter("RbprmShooter")
+#~ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
 r.loadObstacleModel (packageName, "scene_wall", "planning")
 ps.solve ()