Skip to content
GitLab
Commit 4bd41aec authored by Steve Tonneau's avatar Steve Tonneau
Browse files

climb and stepladder scenes

parent 811c5c39
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 4bd41aec
......@@ -69,6 +69,7 @@ install(FILES
data/urdf/scene_wall.urdf
data/urdf/truck.urdf
data/urdf/stair_bauzil.urdf
data/urdf/climb.urdf
#~ data/urdf/scene2.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
......@@ -81,6 +82,7 @@ install(FILES
data/srdf/scene_wall.srdf
data/srdf/truck.srdf
data/srdf/stair_bauzil.srdf
data/srdf/climb.srdf
#~ data/srdf/scene2.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
......@@ -98,6 +100,7 @@ install(FILES
data/meshes/truck.stl
data/meshes/truck_vision.stl
data/meshes/stair_bauzil.stl
data/meshes/climb.stl
#~ data/meshes/pedal.stl
#~ data/meshes/board.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
......
data/meshes/stepladder.mtl 0 → 100644
View file @ 4bd41aec
# Blender MTL File: ''
# Material Count: 1
newmtl
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
data/roms/comlArmSimplified.mtl
View file @ 4bd41aec
# Blender MTL File: 'None'
# Blender MTL File: ''
# Material Count: 1
newmtl None
newmtl
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
data/roms/comlArmSimplified.obj
View file @ 4bd41aec
# Blender v2.62 (sub 0) OBJ File: ''
# www.blender.org
mtllib comlArmSimplified.mtl
o Mesh_foo.001
v 0.134199 0.703673 -0.868478
v -0.098369 -1.008385 -0.478456
v -0.425517 0.764745 0.568656
v 0.667615 -0.576840 0.289054
v 0.043693 1.060394 -0.316520
v 0.828242 0.439627 0.039688
v -0.030162 0.348870 0.865298
v 0.290307 -0.083464 -1.070532
v -0.535702 0.263438 0.650276
v -0.624410 -0.547980 -0.699240
v 0.699149 0.397570 -0.658246
v 0.183406 -1.035148 0.303623
o Mesh.001_Mesh_foo.000
v 0.740978 -0.411636 -0.616968
v 0.523590 0.935191 -0.132838
v -0.004644 -0.576746 -0.968833
v 0.290307 -0.083464 -1.070532
v -0.018070 1.097151 0.215983
v -0.814576 0.594808 -0.015954
v -0.721585 -0.748569 0.063136
v 0.241628 -1.001826 -0.383278
v 0.587224 0.526846 0.619476
v -0.265571 0.150670 -1.071520
v 0.523590 0.935191 -0.132838
v -0.030162 0.348870 0.865298
v -0.535702 0.263438 0.650276
v -0.324619 -0.948166 0.353258
v -0.539998 0.781067 -0.565139
v 0.183406 -1.035148 0.303623
v 0.667615 -0.576840 0.289054
v -0.098369 -1.008385 -0.478456
v 0.241628 -1.001826 -0.383278
v -0.721585 -0.748569 0.063136
v -0.798797 -0.006367 -0.586215
v -0.624410 -0.547980 -0.699240
v 0.043693 1.060394 -0.316520
v 0.134199 0.703673 -0.868478
v -0.539998 0.781067 -0.565139
v 0.699149 0.397570 -0.658246
v 0.828242 0.439627 0.039688
v -0.814576 0.594808 -0.015954
v -0.425517 0.764745 0.568656
v -0.265571 0.150670 -1.071520
v 0.784441 -0.620767 -0.197458
vn 0.461252 -0.435209 -0.773201
vn 0.483358 0.785884 0.385683
vn -0.335541 -0.279027 0.899754
vn 0.271878 -0.257771 0.927166
vn 0.032058 -0.998440 -0.045718
vn -0.945394 -0.264713 -0.190151
vn -0.155396 0.817854 -0.554046
vn 0.709633 0.000741 -0.704571
vn 0.461252 -0.435208 -0.773201
vn 0.483358 0.785884 0.385682
vn -0.335540 -0.279027 0.899754
vn 0.271880 -0.257772 0.927165
vn 0.032059 -0.998440 -0.045718
vn -0.945394 -0.264714 -0.190152
vn -0.155395 0.817854 -0.554046
vn 0.709633 0.000742 -0.704571
vn 0.982030 0.041370 -0.184134
vn -0.836789 0.262183 0.480671
vn -0.691049 0.245319 -0.679904
vn -0.537019 0.843388 0.017543
vn 0.265078 0.963563 -0.035766
vn -0.564559 -0.777676 -0.276574
vn -0.836789 0.262183 0.480672
vn -0.691049 0.245319 -0.679905
vn -0.537019 0.843388 0.017544
vn 0.265079 0.963563 -0.035766
vn -0.564560 -0.777675 -0.276575
vn 0.424182 -0.283571 0.860033
vn 0.614279 -0.678949 -0.402107
vn 0.190329 -0.754668 -0.627895
vn -0.928989 -0.042662 0.367641
vn -0.662658 -0.057535 -0.746709
vn -0.420071 -0.906186 -0.048660
vn 0.114006 0.646470 0.754373
vn -0.166221 -0.985742 0.026154
vn -0.555914 0.820793 0.131370
vn 0.572532 -0.819835 0.008757
vn -0.889765 0.301601 -0.342571
vn 0.614280 -0.678948 -0.402106
vn 0.190330 -0.754667 -0.627895
vn -0.928989 -0.042662 0.367642
vn -0.662658 -0.057536 -0.746709
vn -0.420072 -0.906185 -0.048661
vn 0.114005 0.646471 0.754373
vn -0.166219 -0.985742 0.026155
vn -0.555915 0.820793 0.131370
vn 0.572533 -0.819835 0.008757
vn -0.889766 0.301600 -0.342570
vn 0.473179 0.305911 -0.826148
vn -0.413760 0.234179 0.879752
vn -0.302384 0.513454 -0.803075
vn 0.673313 -0.704205 0.225265
vn -0.302383 0.513454 -0.803075
vn 0.673314 -0.704205 0.225265
vn 0.383990 0.802911 -0.455944
vn -0.654934 -0.285613 0.699633
vn 0.993684 -0.016214 -0.111033
vn 0.972378 0.000948 0.233411
vn 0.786850 0.567858 0.241670
vn 0.114695 0.268273 -0.956491
vn -0.066885 -0.162952 -0.984364
vn 0.114696 0.268273 -0.956491
vn -0.066885 -0.162951 -0.984364
vn 0.864557 0.466045 -0.187996
vn 0.115727 0.652609 0.748805
vn 0.115726 0.652609 0.748805
vn -0.996880 -0.066502 0.042524
vn 0.909117 -0.126320 0.396924
vn 0.026889 -0.372342 0.927706
vn 0.909118 -0.126320 0.396923
vn 0.026888 -0.372341 0.927706
vn -0.391160 0.913910 -0.108453
vn 0.441577 -0.627525 -0.641266
vn -0.318620 -0.680481 -0.659868
vn -0.318621 -0.680480 -0.659869
vn -0.394886 -0.265795 -0.879442
vn 0.548948 0.668986 -0.501112
usemtl
s off
f 13//1 15//1 8//1
f 16//2 20//2 14//2
f 7//3 9//3 22//3
f 12//4 4//4 20//4
f 12//5 2//5 19//5
f 18//6 24//6 10//6
f 5//7 1//7 23//7
f 8//8 11//8 13//8
f 11//9 6//9 13//9
f 17//10 9//10 3//10
f 23//11 21//11 24//11
f 23//12 17//12 16//12
f 5//13 16//13 14//13
f 10//14 2//14 18//14
f 12//15 20//15 7//15
f 25//16 19//16 13//16
f 19//17 2//17 15//17
f 17//18 18//18 9//18
f 21//19 10//19 24//19
f 22//20 18//20 2//20
f 20//21 3//21 7//21
f 22//22 2//22 12//22
f 3//23 16//23 17//23
f 19//24 25//24 12//24
f 24//25 17//25 23//25
f 8//26 1//26 11//26
f 3//27 9//27 7//27
f 23//28 1//28 21//28
f 4//29 12//29 25//29
f 1//30 5//30 14//30
f 22//31 9//31 18//31
f 13//32 6//32 25//32
f 25//33 20//33 4//33
f 20//34 6//34 14//34
f 21//35 1//35 8//35
f 8//36 15//36 21//36
f 11//37 14//37 6//37
f 16//38 3//38 20//38
f 24//39 18//39 17//39
f 6//40 20//40 25//40
f 12//41 7//41 22//41
f 5//42 23//42 16//42
f 13//43 19//43 15//43
f 10//44 15//44 2//44
f 15//45 10//45 21//45
f 11//46 1//46 14//46
f 1//1 2//1 3//1
f 4//2 5//2 6//2
f 7//3 8//3 9//3
f 10//4 11//4 5//4
f 10//5 12//5 13//5
f 14//6 15//6 16//6
f 17//7 18//7 19//7
f 20//8 1//8 3//8
f 21//9 1//9 20//9
f 22//10 8//10 23//10
f 19//11 24//11 15//11
f 19//12 22//12 4//12
f 17//13 4//13 6//13
f 16//14 12//14 14//14
f 10//15 5//15 7//15
f 13//16 1//16 25//16
f 13//17 12//17 2//17
f 22//18 14//18 8//18
f 24//19 16//19 15//19
f 9//20 14//20 12//20
f 5//21 23//21 7//21
f 9//22 12//22 10//22
f 23//23 4//23 22//23
f 13//24 25//24 10//24
f 15//25 22//25 19//25
f 3//26 18//26 20//26
f 23//27 8//27 7//27
f 19//28 18//28 24//28
f 11//29 10//29 25//29
f 18//30 17//30 6//30
f 9//31 8//31 14//31
f 1//32 21//32 25//32
f 25//33 5//33 11//33
f 5//34 21//34 6//34
f 24//35 18//35 3//35
f 3//36 2//36 24//36
f 20//37 6//37 21//37
f 4//38 23//38 5//38
f 15//39 14//39 22//39
f 21//40 5//40 25//40
f 10//41 7//41 9//41
f 17//42 19//42 4//42
f 1//43 13//43 2//43
f 16//44 2//44 12//44
f 2//45 16//45 24//45
f 20//46 18//46 6//46
data/roms/comlLegSimplified.mtl
View file @ 4bd41aec
# Blender MTL File: 'None'
# Blender MTL File: ''
# Material Count: 1
newmtl None
newmtl
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
data/roms/comrArmSimplified.mtl
View file @ 4bd41aec
# Blender MTL File: 'None'
# Blender MTL File: ''
# Material Count: 1
newmtl None
newmtl
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
data/roms/comrLegSimplified.mtl
View file @ 4bd41aec
# Blender MTL File: 'None'
# Blender MTL File: ''
# Material Count: 1
newmtl None
newmtl
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
data/srdf/climb.srdf 0 → 100644
View file @ 4bd41aec
<?xml version="1.0"?>
<robot name="climb">
<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/stepladder.srdf 0 → 100644
View file @ 4bd41aec
<?xml version="1.0"?>
<robot name="stepladder">
<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/climb.urdf 0 → 100644
View file @ 4bd41aec
<robot name="climb">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package:///../hpp-rbprm-corba/meshes/climb.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/climb.stl"/>
</geometry>
</collision>
</link>
</robot>
data/urdf/stepladder.urdf 0 → 100644
View file @ 4bd41aec
<robot name="stepladder">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package:///../hpp-rbprm-corba/meshes/stepladder.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/stepladder.stl"/>
</geometry>
</collision>
</link>
</robot>
script/climb_path_hrp2.py 0 → 100644
View file @ 4bd41aec
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk08'
urdfNameRom = 'hrp2_rom'
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1, 1, -0.5, 0.5, 0, 5])
#~ 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)
q_goal = rbprmBuilder.getCurrentConfig ();
#~ rbprmBuilder.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.05,0, 0.5]; r (q_init)
q_init[0:3] =[-0.1,0, 0.5]; r (q_init)
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_goal[0:3] =[-0.05,0, 2.5]; r (q_goal)
#~ q_goal[0:3] =[-0,0, 2];
q_goal = [0, 0, 2, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]; r (q_goal)
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, "climb", "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")
script/loadhrp2climb.py 0 → 100644
View file @ 4bd41aec
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import climb_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", [0,4, -1, 1, 1, 2.5])
fullBody.setJointBounds ("base_joint_xyz", [-1, 1, -0.5, 0.5, 0, 5])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "climb", "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.1)
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, 10000, 0.1)
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_0 = fullBody.getCurrentConfig (); r (q_0)
q_init = fullBody.getCurrentConfig (); r (q_init)
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]
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.08)
#~ configs2 = fullBody.interpolate(0.05)
i = 0;
r (configs[i]); i=i+1; i-1
script/loadhrp2stepladder.py 0 → 100644
View file @ 4bd41aec
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import stepladder_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", [0,4, -1, 1, 1, 2.5])
fullBody.setJointBounds ("base_joint_xyz", [-1, 1, -0.5, 0.5, 0, 5])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "stepladder", "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.03)
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.03)
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_0 = fullBody.getCurrentConfig (); r (q_0)
q_init = fullBody.getCurrentConfig (); r (q_init)
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]