Commit 5b133877 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

flexible trunk

parent f7e432f7
......@@ -62,6 +62,7 @@ install(FILES
)
install(FILES
data/urdf/hrp2_trunk.urdf
data/urdf/hrp2_trunk_flexible.urdf
data/urdf/hrp2_rom.urdf
data/urdf/hrp2_larm_rom.urdf
data/urdf/hrp2_rarm_rom.urdf
......@@ -75,11 +76,13 @@ install(FILES
data/urdf/stair_bauzil.urdf
data/urdf/climb.urdf
data/urdf/stepladder.urdf
data/urdf/chair.urdf
#~ data/urdf/scene2.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
install(FILES
data/srdf/hrp2_trunk.srdf
data/srdf/hrp2_trunk_flexible.srdf
data/srdf/hrp2_rom.srdf
data/srdf/hrp2_larm_rom.srdf
data/srdf/hrp2_rarm_rom.srdf
......@@ -93,6 +96,7 @@ install(FILES
data/srdf/stair_bauzil.srdf
data/srdf/climb.srdf
data/srdf/stepladder.srdf
data/srdf/chair.srdf
#~ data/srdf/scene2.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
......@@ -100,6 +104,8 @@ install(FILES
data/meshes/car.stl
data/meshes/car_simple.stl
data/meshes/hrp2_trunk.stl
data/meshes/hrp2_trunk_body.stl
data/meshes/hrp2_trunk_torso.stl
data/meshes/hrp2_rom.stl
data/meshes/hrp2_larm_rom.stl
data/meshes/hrp2_rarm_rom.stl
......@@ -116,6 +122,7 @@ install(FILES
data/meshes/stair_bauzil.stl
data/meshes/climb.stl
data/meshes/stepladder.stl
data/meshes/chair.stl
#~ data/meshes/pedal.stl
#~ data/meshes/board.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
......
No preview for this file type
No preview for this file type
<?xml version="1.0"?>
<robot name="chair">
<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>
<?xml version="1.0"?>
<robot name="hrp2_trunk_flexible">
<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>
<disable_collisions link1="BODY"
link2="torso" />
</robot>
<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/chair.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/chair.stl"/>
</geometry>
</collision>
</link>
</robot>
<robot name="hrp2_larm_rom">
<link name="base_link">
<link name="base_link"/>
<joint name="WAIST" type="fixed">
<parent link="base_link"/>
<child link="BODY"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="BODY">
</link>
<!-- Chest -->
<joint name="CHEST_JOINT0" type="revolute">
<parent link="BODY"/>
<child link="CHEST_LINK0"/>
<origin rpy="0 0 0" xyz="0.032 0 0.3507"/>
<axis xyz="0 0 1"/>
<limit effort="151.1" lower="-0.785398" upper="0.785398" velocity="4.40217"/>
</joint>
<link name="CHEST_LINK0">
</link>
<joint name="CHEST_JOINT1" type="revolute">
<parent link="CHEST_LINK0"/>
<child link="torso"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
<limit effort="278.9" lower="-0.0872665" upper="1.0472" velocity="2.3963"/>
</joint>
<link name="torso">
<visual>
<origin xyz="0 0 0" rpy="0 0 0.003" />
<origin xyz="-0.032 -0 -0.3507" rpy="0 0 0.003" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_larm_rom.stl"/>
</geometry>
......
<robot name="hrp2_lleg_rom">
<link name="base_link">
<link name="base_link"/>
<joint name="WAIST" type="fixed">
<parent link="base_link"/>
<child link="BODY"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="BODY">
<visual>
<origin xyz="0 0 0" rpy="0 0 0.003" />
<geometry>
......@@ -16,4 +22,23 @@
</geometry>
</collision>
</link>
<!-- Chest -->
<joint name="CHEST_JOINT0" type="revolute">
<parent link="BODY"/>
<child link="CHEST_LINK0"/>
<origin rpy="0 0 0" xyz="0.032 0 0.3507"/>
<axis xyz="0 0 1"/>
<limit effort="151.1" lower="-0.785398" upper="0.785398" velocity="4.40217"/>
</joint>
<link name="CHEST_LINK0">
</link>
<joint name="CHEST_JOINT1" type="revolute">
<parent link="CHEST_LINK0"/>
<child link="torso"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
<limit effort="278.9" lower="-0.0872665" upper="1.0472" velocity="2.3963"/>
</joint>
<link name="torso">
</link>
</robot>
<robot name="hrp2_rarm_rom">
<link name="base_link">
<link name="base_link"/>
<joint name="WAIST" type="fixed">
<parent link="base_link"/>
<child link="BODY"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="BODY">
</link>
<!-- Chest -->
<joint name="CHEST_JOINT0" type="revolute">
<parent link="BODY"/>
<child link="CHEST_LINK0"/>
<origin rpy="0 0 0" xyz="0.032 0 0.3507"/>
<axis xyz="0 0 1"/>
<limit effort="151.1" lower="-0.785398" upper="0.785398" velocity="4.40217"/>
</joint>
<link name="CHEST_LINK0">
</link>
<joint name="CHEST_JOINT1" type="revolute">
<parent link="CHEST_LINK0"/>
<child link="torso"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
<limit effort="278.9" lower="-0.0872665" upper="1.0472" velocity="2.3963"/>
</joint>
<link name="torso">
<visual>
<origin xyz="0 0 0" rpy="0 0 0.003" />
<origin xyz="-0.032 -0 -0.3507" rpy="0 0 0.003" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rarm_rom.stl"/>
</geometry>
......
<robot name="hrp2_rleg_rom">
<link name="base_link">
<link name="base_link"/>
<joint name="WAIST" type="fixed">
<parent link="base_link"/>
<child link="BODY"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="BODY">
<visual>
<origin xyz="0 0 0" rpy="0 0 0.003" />
<geometry>
......@@ -16,4 +22,23 @@
</geometry>
</collision>
</link>
<!-- Chest -->
<joint name="CHEST_JOINT0" type="revolute">
<parent link="BODY"/>
<child link="CHEST_LINK0"/>
<origin rpy="0 0 0" xyz="0.032 0 0.3507"/>
<axis xyz="0 0 1"/>
<limit effort="151.1" lower="-0.785398" upper="0.785398" velocity="4.40217"/>
</joint>
<link name="CHEST_LINK0">
</link>
<joint name="CHEST_JOINT1" type="revolute">
<parent link="CHEST_LINK0"/>
<child link="torso"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
<limit effort="278.9" lower="-0.0872665" upper="1.0472" velocity="2.3963"/>
</joint>
<link name="torso">
</link>
</robot>
<robot name="hrp2_trunk08">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0.003" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk08.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_trunk08.stl"/>
</geometry>
</collision>
</link>
</robot>
<robot name="hrp2_trunk_flexible">
<link name="base_link"/>
<joint name="WAIST" type="fixed">
<parent link="base_link"/>
<child link="BODY"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="BODY">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk_body.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/hrp2_trunk_body.stl"/>
</geometry>
</collision>
</link>
<!-- Chest -->
<joint name="CHEST_JOINT0" type="revolute">
<parent link="BODY"/>
<child link="CHEST_LINK0"/>
<origin rpy="0 0 0" xyz="0.032 0 0.3507"/>
<axis xyz="0 0 1"/>
<limit effort="151.1" lower="-0.785398" upper="0.785398" velocity="4.40217"/>
</joint>
<link name="CHEST_LINK0">
</link>
<joint name="CHEST_JOINT1" type="revolute">
<parent link="CHEST_LINK0"/>
<child link="torso"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
<limit effort="278.9" lower="-0.0872665" upper="1.0472" velocity="2.3963"/>
</joint>
<link name="torso">
<visual>
<origin xyz="-0.032 -0 -0.3507" rpy="0 0 0.003" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk_torso.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_trunk_torso.stl"/>
</geometry>
</collision>
</link>
</robot>
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'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1, 1, -0.5, 0.5, 0, 5])
rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
#~ 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.15, -0.2, 0.6, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]
q_init = [-0.3, -0.2, 0.6, 1, 0.0, 0., 0.0]; r (q_init)
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_goal = [0, 0, 2, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]; r (q_goal)
#~ q_goal = [0.2, -0.2, 1.6, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]; r (q_goal)
q_goal = [0.1, -0.2, 1.6, 0.98877107793604235, 0.0, 0.14943813247359924, 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.1)
r.loadObstacleModel (packageName, "chair", "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")
......@@ -5,14 +5,15 @@ rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk08'
urdfNameRom = 'hrp2_rom'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1, 1, -0.5, 0.5, 0, 5])
rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
......@@ -33,7 +34,7 @@ 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)
q_goal = [-0.1, 0, 2, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]; r (q_goal)
ps.addPathOptimizer("RandomShortcut")
#~ ps.addPathOptimizer("GradientBased")
......@@ -41,7 +42,7 @@ ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
ps.client.problem.selectPathValidation("RbprmPathValidation",0.3)
r.loadObstacleModel (packageName, "climb", "planning")
ps.solve ()
......
......@@ -76,7 +76,7 @@ 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)
configs = fullBody.interpolate(0.1)
#~ configs2 = fullBody.interpolate(0.05)
i = 0;
r (configs[i]); i=i+1; i-1
......
......@@ -31,14 +31,14 @@ 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)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, 0.05)
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)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, 0.05)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
......@@ -76,7 +76,7 @@ q_goal = fullBody.generateContacts(q_goal, [0,0,-1])
fullBody.setStartState(q_init,[rLegId,lLegId])
fullBody.setEndState(q_goal,[rLegId,lLegId])
#~
configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.05)
#~ configs2 = fullBody.interpolate(0.05)
i = 0;
r (configs[i]); i=i+1; i-1
......
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