Commit ff10031f authored by Steve Tonneau's avatar Steve Tonneau
Browse files

hyq and blender export

parent f0da4e4d
......@@ -69,6 +69,7 @@ install(FILES
data/urdf/hrp2_lleg_rom.urdf
data/urdf/hrp2_rleg_rom.urdf
data/urdf/hyq/hyq_trunk.urdf
data/urdf/hyq/hyq_trunk_large.urdf
data/urdf/hyq/hyq_rhleg_rom.urdf
data/urdf/hyq/hyq_rfleg_rom.urdf
data/urdf/hyq/hyq_lhleg_rom.urdf
......@@ -97,6 +98,7 @@ install(FILES
data/srdf/hrp2_lleg_rom.srdf
data/srdf/hrp2_rleg_rom.srdf
data/srdf/hyq/hyq_trunk.srdf
data/srdf/hyq/hyq_trunk_large.srdf
data/srdf/hyq/hyq_rhleg_rom.srdf
data/srdf/hyq/hyq_rfleg_rom.srdf
data/srdf/hyq/hyq_lhleg_rom.srdf
......@@ -148,6 +150,7 @@ install(FILES
install(FILES
data/meshes/hyq/hyq_all.stl
data/meshes/hyq/hyq_trunk.stl
data/meshes/hyq/hyq_trunk_large.stl
data/meshes/hyq/hyq_rom.stl
data/meshes/hyq/hyq_rhleg_rom.stl
data/meshes/hyq/hyq_rfleg_rom.stl
......@@ -156,5 +159,9 @@ install(FILES
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq
)
install(DIRECTORY data/hyq_description
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/../
)
SETUP_PROJECT_FINALIZE()
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset>
<contributor>
<author>Blender User</author>
<authoring_tool>Blender 2.75.0 commit date:2015-07-07, commit time:14:56, hash:c27589e</authoring_tool>
</contributor>
<created>2015-10-19T13:54:27</created>
<modified>2015-10-19T13:54:27</modified>
<unit name="meter" meter="1"/>
<up_axis>Z_UP</up_axis>
</asset>
<library_images/>
<library_geometries>
<geometry id="Cube_001-mesh" name="Cube.001">
<mesh>
<source id="Cube_001-mesh-positions">
<float_array id="Cube_001-mesh-positions-array" count="24">-3.202436 -0.9731922 -0.9147082 -3.202436 -0.9731922 1.085292 -3.202436 1.026808 -0.9147082 -3.202436 1.026808 1.085292 -1.202436 -0.9731922 -0.9147082 -1.202436 -0.9731922 1.085292 -1.202436 1.026808 -0.9147082 -1.202436 1.026808 1.085292</float_array>
<technique_common>
<accessor source="#Cube_001-mesh-positions-array" count="8" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_001-mesh-normals">
<float_array id="Cube_001-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
<technique_common>
<accessor source="#Cube_001-mesh-normals-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Cube_001-mesh-vertices">
<input semantic="POSITION" source="#Cube_001-mesh-positions"/>
</vertices>
<polylist count="12">
<input semantic="VERTEX" source="#Cube_001-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_001-mesh-normals" offset="1"/>
<vcount>3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
<p>3 0 2 0 0 0 7 1 6 1 2 1 5 2 4 2 6 2 1 3 0 3 4 3 2 4 6 4 4 4 7 5 3 5 1 5 1 0 3 0 0 0 3 1 7 1 2 1 7 2 5 2 6 2 5 3 1 3 4 3 0 4 2 4 4 4 5 5 7 5 1 5</p>
</polylist>
</mesh>
</geometry>
<geometry id="Cube-mesh" name="Cube">
<mesh>
<source id="Cube-mesh-positions">
<float_array id="Cube-mesh-positions-array" count="24">-3.222331 -0.9820953 -0.9556123 -3.222331 -0.9820953 1.044388 -3.222331 1.017905 -0.9556123 -3.222331 1.017905 1.044388 -1.222331 -0.9820953 -0.9556123 -1.222331 -0.9820953 1.044388 -1.222331 1.017905 -0.9556123 -1.222331 1.017905 1.044388</float_array>
<technique_common>
<accessor source="#Cube-mesh-positions-array" count="8" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube-mesh-normals">
<float_array id="Cube-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
<technique_common>
<accessor source="#Cube-mesh-normals-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Cube-mesh-vertices">
<input semantic="POSITION" source="#Cube-mesh-positions"/>
</vertices>
<polylist count="12">
<input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
<vcount>3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
<p>3 0 2 0 0 0 7 1 6 1 2 1 5 2 4 2 6 2 1 3 0 3 4 3 2 4 6 4 4 4 7 5 3 5 1 5 1 0 3 0 0 0 3 1 7 1 2 1 7 2 5 2 6 2 5 3 1 3 4 3 0 4 2 4 4 4 5 5 7 5 1 5</p>
</polylist>
</mesh>
</geometry>
</library_geometries>
<library_controllers/>
<library_visual_scenes>
<visual_scene id="Scene" name="Scene">
<node id="Cube_001" name="Cube_001" type="NODE">
<matrix sid="transform">-0.1032963 0 0 9.31323e-10 0 -0.01948144 0 0 0 0 -0.02072426 0 0 0 0 1</matrix>
<instance_geometry url="#Cube_001-mesh" name="Cube_001"/>
</node>
<node id="Cube" name="Cube" type="NODE">
<matrix sid="transform">-0.03529432 0 0 -3.72529e-9 0 -0.02916849 0 0 0 0 -0.03982198 0 0 0 0 1</matrix>
<instance_geometry url="#Cube-mesh" name="Cube"/>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
</COLLADA>
\ No newline at end of file
<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="hyq">
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<!--
<disable_collisions link1="base_link" link2="lf_foot" reason="Adjacent" />
<disable_collisions link1="base_link" link2="lh_foot" reason="Adjacent" />
<disable_collisions link1="base_link" link2="rf_foot" reason="Adjacent" />
<disable_collisions link1="base_link" link2="rh_foot" reason="Adjacent" />
<disable_collisions link1="lf_foot" link2="lh_foot" reason="Adjacent" />
<disable_collisions link1="lf_foot" link2="rf_foot" reason="Adjacent" />
<disable_collisions link1="lf_foot" link2="rh_foot" reason="Adjacent" />
<disable_collisions link1="lh_foot" link2="rf_foot" reason="Adjacent" />
<disable_collisions link1="lh_foot" link2="rh_foot" reason="Adjacent" />
<disable_collisions link1="rf_foot" link2="rh_foot" reason="Adjacent" />
-->
<disable_collisions link1="lf_hipassembly" link2="trunk" reason="Adjacent" />
<disable_collisions link1="lh_hipassembly" link2="trunk" reason="Adjacent" />
<disable_collisions link1="rf_hipassembly" link2="trunk" reason="Adjacent" />
<disable_collisions link1="rh_hipassembly" link2="trunk" reason="Adjacent" />
<disable_collisions link1="lf_hipassembly" link2="lf_upperleg" reason="Adjacent" />
<disable_collisions link1="lh_hipassembly" link2="lh_upperleg" reason="Adjacent" />
<disable_collisions link1="rf_hipassembly" link2="rf_upperleg" reason="Adjacent" />
<disable_collisions link1="rh_hipassembly" link2="rh_upperleg" reason="Adjacent" />
<disable_collisions link1="lf_lowerleg" link2="lf_upperleg" reason="Adjacent" />
<disable_collisions link1="lh_lowerleg" link2="lh_upperleg" reason="Adjacent" />
<disable_collisions link1="rf_lowerleg" link2="rf_upperleg" reason="Adjacent" />
<disable_collisions link1="rh_lowerleg" link2="rh_upperleg" reason="Adjacent" />
<disable_collisions link1="lf_lowerleg" link2="lf_foot" reason="Adjacent" />
<disable_collisions link1="lh_lowerleg" link2="lh_foot" reason="Adjacent" />
<disable_collisions link1="rf_lowerleg" link2="rf_foot" reason="Adjacent" />
<disable_collisions link1="rh_lowerleg" link2="rh_foot" reason="Adjacent" />
</robot>
This diff is collapsed.
<?xml version="1.0"?>
<robot name="hyq_trunk_large">
</robot>
<robot name="hyq_trunk_large">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_all.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/hyq/hyq_trunk_large.stl"/>
</geometry>
</collision>
</link>
</robot>
......@@ -19,7 +19,7 @@ fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
nbSamples = 10000
nbSamples = 20000
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
......
......@@ -4,7 +4,7 @@ from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hyq_trunk'
urdfName = 'hyq_trunk_large'
urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""
......@@ -29,7 +29,7 @@ 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_init [0:3] = [2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
......@@ -41,7 +41,7 @@ ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "darpa", "planning")
ps.solve ()
......@@ -53,6 +53,6 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (2)
#~ pp (0)
pp (1)
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init)
......@@ -19,7 +19,7 @@ fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
nbSamples = 10000
nbSamples = 20000
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
......@@ -33,7 +33,7 @@ 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)
fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.03)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
......@@ -41,7 +41,7 @@ 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)
fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.03)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
......@@ -49,7 +49,7 @@ 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)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.03)
larmId = 'lfleg'
larm = 'lf_haa_joint'
......@@ -57,7 +57,7 @@ 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)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "manipulability", 0.03)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
......@@ -76,7 +76,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.2)
r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
......
......@@ -5,20 +5,20 @@ 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']
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", [-1,5, -1, 1, 0, 2])
rbprmBuilder.setFilter(['hyq_lhleg_rom' , 'hyq_rfleg_rom'])
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,5, -0.1, 0.1, 0.35, 2])
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_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])
rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
......@@ -30,15 +30,11 @@ 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)
......@@ -51,11 +47,6 @@ 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)
......@@ -110,7 +110,7 @@ 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 = open("hyq_crouch_20_10_15","w+")
f1.write(str(configs))
f1.close()
......@@ -13,6 +13,12 @@ rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.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
......
......@@ -23,7 +23,6 @@ nbSamples = 10000
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
rootName = 'base_joint_xyz'
......@@ -31,10 +30,10 @@ rootName = 'base_joint_xyz'
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
rfoot = 'rf_foot_joint'
rLegOffset = [0,0,0]
rLegOffset = [0.,0,0.]
rLegNormal = [0,1,0]
rLegx = 0.02; rLegy = 0.02
fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "EFORT", 0.03)
fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.1)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
......@@ -42,23 +41,23 @@ 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, "EFORT", 0.03)
fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.1)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
rArmOffset = [0,0,0]
rArmNormal = [1,0,0]
rArmOffset = [0.,0,-0.]
rArmNormal = [0,1,0]
rArmx = 0.02; rArmy = 0.02
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.03)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.1)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
lArmOffset = [0,0,0]
lArmNormal = [1,0,0]
lArmOffset = [0.,0,-0.]
lArmNormal = [0,1,0]
lArmx = 0.02; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.03)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "manipulability", 0.1)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
......@@ -72,13 +71,20 @@ q_0 = fullBody.getCurrentConfig();
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
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', "stair_bauzil", "contact")
i = 0;
r (configs[i]); i=i+1; i-1
q0 = configs[2]
q0 = fullBody.generateContacts(q0, [0,0,1])
r(q0)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment