Commit 9ba04125 authored by Florent Lamiraux's avatar Florent Lamiraux

Add benchmark Lydia.

parent d71cde16
[submodule "cmake"]
path = cmake
url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
#
# Copyright (c) 2014 CNRS
# Authors: Florent Lamiraux
#
#
# This file is part of hpp_benchmark
# hpp_benchmark is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp_benchmark is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp_benchmark If not, see
# <http://www.gnu.org/licenses/>.
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
SET(CXX_DISABLE_WERROR TRUE)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAME hpp_benchmark)
SET(PROJECT_DESCRIPTION
"Tutorial for humanoid path planner platform."
)
SET(PROJECT_URL "")
FINDPYTHON()
SETUP_PROJECT()
# Catkin stuff
FIND_PACKAGE(catkin REQUIRED COMPONENTS
hpp_ros)
CATKIN_PACKAGE()
# Activate hpp-util logging if requested
SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
IF (HPP_DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
ENDIF()
ADD_DOC_DEPENDENCY("hpp-core >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 3")
ADD_REQUIRED_DEPENDENCY("hpp_ros")
SET(CATKIN_PACKAGE_SHARE_DESTINATION
${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME})
install(FILES launch/lydia.launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
install(FILES
urdf/lydia.urdf
urdf/obstacle.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
install(FILES srdf/lydia.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
install(FILES rviz/lydia/config.rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz
)
install (FILES
src/hpp/corbaserver/lydia/robot.py
src/hpp/corbaserver/lydia/__init__.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/lydia)
SETUP_PROJECT_FINALIZE()
Subproject commit a831f5403a93b67c6cb8675625e79dd676a565f3
<launch>
<param name="obstacle_description"
textfile="$(find hpp_benchmark)/urdf/obstacle.urdf"/>
<param name="robot_description"
textfile="$(find hpp_benchmark)/urdf/lydia.urdf"/>
<node name="robot_state_publisher"
pkg="robot_state_publisher"
type="state_publisher"
respawn="true">
<param name="tf_prefix" value=""/>
</node>
<node pkg="tf" type="static_transform_publisher"
name="obstacle_base"
args="0 0 0 0 0 0 /map /obstacle/obstacle_base 200">
</node>
<node name="rviz" pkg="rviz" type="rviz" respawn="false"
args="-d $(find hpp_benchmark)/rviz/lydia/config.rviz">
</node>
</launch>
<package>
<name>hpp_benchmark</name>
<version>0.1.0</version>
<description>
Benchmark for Humanoid path planner platform
</description>
<author>Florent Lamiraux</author>
<maintainer email="hpp@laas.fr">Florent Lamiraux</maintainer>
<license>LGPL v2</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>hpp_ros</build_depend>
<build_depend>hpp-corbaserver</build_depend>
</package>
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /RobotModel2
Splitter Ratio: 0.5
Tree Height: 540
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_object_one:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_object_three:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_object_two:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
l_decor_one:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_decor_three:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_decor_two:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
obstacle_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: obstacle_description
TF Prefix: obstacle
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 7.73245
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.58277
Y: -0.82885
Z: -5.19753e-05
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.389408
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.36771
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000013c000002abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002ab000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000036e000002ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 718
Y: 36
from hpp.corbaserver.lydia import Robot
from hpp.corbaserver import ProblemSolver
from hpp_ros import ScenePublisher
robot = Robot ('lydia')
robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1.1, 1.1])
ps = ProblemSolver (robot)
ps.loadObstacleFromUrdf ("hpp_benchmark", "obstacle", "")
s = ScenePublisher (robot)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [2] = -0.6
q_goal [2] = 0.6
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.solve ()
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Florent Lamiraux
#
# This file is part of hpp-corbaserver.
# hpp-corbaserver is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
from robot import Robot
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Florent Lamiraux
#
# This file is part of hpp_benchmark.
# hpp_benchmark is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp_benchmark is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp_benchmark. If not, see
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.robot import Robot as Parent
##
# Control of device Lydia in hpp
#
# This class implements a client to the corba server implemented in
# hpp_benchmark. It derive from class hpp.corbaserver.robot.Robot.
#
# This class is also used to initialize a client to rviz in order to
# display configurations of the PR2 robot.
#
# At creation of an instance, the urdf and srdf files are loaded using
# idl interface hpp::corbaserver::Robot::loadRobotModel.
class Robot (Parent):
##
# Information to retrieve urdf and srdf files.
packageName = "hpp_benchmark"
##
# Information to retrieve urdf and srdf files.
urdfName = "lydia"
urdfSuffix = ""
srdfSuffix = ""
def __init__ (self, robotName, load = True):
Parent.__init__ (self, robotName, "freeflyer", load)
self.tf_root = "base_link"
<?xml version="1.0"?>
<robot name="object">
<!-- Disable collision pairs of inner links -->
<disable_collisions link1="base_link"
link2="l_object_one" />
<disable_collisions link1="base_link"
link2="l_object_two" />
<disable_collisions link1="l_object_one"
link2="l_object_three" />
</robot>
<?xml version="1.0"?>
<robot name="object"> <!-- simple puzzle robot-->
<link name="base_link"> <!-- required name-->
<inertial>
<origin xyz="0 0 0" />
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="100.0" />
</inertial>
<visual>
<origin xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.5" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.5" />
</geometry>
</collision>
</link>
<joint name="j_object_one" type="fixed">
<parent link="base_link"/>
<child link="l_object_one"/>
<origin xyz="0 0 0.25"/>
</joint>
<link name="l_object_one"> <!-- upper part -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="0 0.3 0.05"/>
<geometry>
<box size="0.1 0.7 0.1" />
</geometry>
</visual>
<collision>
<origin xyz="0 0.3 0.05"/>
<geometry>
<box size="0.1 0.7 0.1" />
</geometry>
</collision>
</link>
<joint name="j_object_two" type="fixed">
<parent link="base_link"/>
<child link="l_object_two"/>
<origin xyz="0 0 -0.25"/>
</joint>
<link name="l_object_two"> <!-- lower part -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.0" />
<inertia ixx="100.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="-0.3 0 -0.05"/>
<geometry>
<box size="0.7 0.1 0.1"/>
</geometry>
</visual>
<collision>
<origin xyz="-0.3 0 -0.05"/>
<geometry>
<box size="0.7 0.1 0.1"/>
</geometry>
</collision>
</link>
<joint name="j_object_three" type="fixed">
<parent link="base_link"/>
<child link="l_object_three"/>
<origin xyz="0 0 0.25"/>
</joint>
<link name="l_object_three"> <!-- upper second part / fourth branch -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.0" />
<inertia ixx="100.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="0.35 0.6 0.05"/>
<geometry>
<box size="0.6 0.1 0.1"/>
</geometry>
</visual>
<collision>
<origin xyz="0.35 0.6 0.05"/>
<geometry>
<box size="0.6 0.1 0.1"/>
</geometry>
</collision>
</link>
</robot>
<?xml version="1.0"?>
<robot name="decor"> <!-- puzzle hard environment -->
<link name="obstacle_base">
<inertial>
<origin xyz="0 0 0" />
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="-0.54 0 -0.05"/>
<geometry>
<box size="0.92 2 0.1" />
</geometry>
<material name="Blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<collision>
<origin xyz="-0.54 0 -0.05"/>
<geometry>
<box size="0.92 2 0.1" />
</geometry>
</collision>
</link>
<joint name="j_decor_one" type="fixed">
<parent link="obstacle_base"/>
<child link="l_decor_one"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="l_decor_one">
<inertial>
<origin xyz="0 0 0" />
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="0.54 0 -0.05"/>
<geometry>
<box size="0.92 2 0.1" />
</geometry>
<material name="Blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0.54 0 -0.05"/>
<geometry>
<box size="0.92 2 0.1" />
</geometry>
</collision>
</link>
<joint name="j_decor_two" type="fixed">
<parent link="l_decor_one"/>
<child link="l_decor_two"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="l_decor_two">
<inertial>
<origin xyz="0 0 0" />
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="0 -0.65 -0.05"/>
<geometry>
<box size="0.16 0.7 0.1" />
</geometry>
<material name="Blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 -0.65 -0.05"/>
<geometry>
<box size="0.16 0.7 0.1" />
</geometry>
</collision>
</link>
<joint name="j_decor_three" type="fixed">
<parent link="l_decor_two"/>
<child link="l_decor_three"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="l_decor_three">
<inertial>
<origin xyz="0 0 0" />
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="0 0.65 -0.05"/>
<geometry>
<box size="0.2 0.7 0.1" />
</geometry>
<material name="Blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0.65 -0.05"/>
<geometry>
<box size="0.2 0.7 0.1" />
</geometry>
</collision>
</link>
</robot>
Markdown is supported
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