d74e778eec99349a97c8190025b25127bf80edea to master · CeTI / ROS / ROS Packages / sample_applications

CMakeLists.txt

+36
−16

Original line number
Original line
Diff line number
Diff line

@@ -5,8 +5,8 @@ add_compile_options(-std=c++14)

set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")

set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")

# Find catkin macros and libraries if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) is used, also find other

# Find catkin macros and libraries if COMPONENTS list like `find_package(catkin REQUIRED COMPONENTS xyz)` is used,

# catkin packages

# also find other catkin packages

find_package(catkin REQUIRED

find_package(catkin REQUIRED

        COMPONENTS controller_manager

        COMPONENTS controller_manager

        effort_controllers

        effort_controllers

@@ -17,6 +17,8 @@ find_package(catkin REQUIRED

        joint_trajectory_controller

        joint_trajectory_controller

        robot_state_publisher

        robot_state_publisher

        roscpp

        roscpp

        tf2

        tf2_geometry_msgs

        std_msgs

        std_msgs

        xacro

        xacro

        moveit_core

        moveit_core

@@ -24,15 +26,17 @@ find_package(catkin REQUIRED

        moveit_ros_planning

        moveit_ros_planning

        moveit_ros_planning_interface

        moveit_ros_planning_interface

        controller_interface

        controller_interface

        hardware_interface)

        hardware_interface

        simulation_util)

# System dependencies are found with CMake's conventions

# System dependencies are found with CMake's conventions

find_package(PkgConfig REQUIRED)

find_package(PkgConfig REQUIRED)

# ################################################################################################################################

# ######################################################################################################################

# catkin specific configuration ##

# catkin specific configuration ##

# ################################################################################################################################

# ######################################################################################################################

# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects

# The catkin_package macro generates cmake config files for your package and declare things to be passed to dependent

# projects. This has to be done in addition to the find_package command above.

catkin_package(CATKIN_DEPENDS

catkin_package(CATKIN_DEPENDS

        moveit_core

        moveit_core

        moveit_visual_tools

        moveit_visual_tools

@@ -40,22 +44,38 @@ catkin_package(CATKIN_DEPENDS

        controller_interface

        controller_interface

        hardware_interface

        hardware_interface

        pluginlib

        pluginlib

        tf2

        simulation_util

        DEPENDS

        DEPENDS

        # system_lib

        # system_lib

        )

        )

# ################################################################################################################################

# ######################################################################################################################

# Build ##

# Build ##

# ################################################################################################################################

# ######################################################################################################################

# Specify additional locations of header files Your package locations should be listed before other locations

# Specify additional locations of header files Your package locations should be listed before other locations

include_directories(${catkin_INCLUDE_DIRS})

include_directories(src ${catkin_INCLUDE_DIRS})

add_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp)

add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp)

add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp)

add_executable(PickPlacePlanner src/PickPlacePlanner.cpp src/PickPlacePlannerMain.cpp)

## Add cmake target dependencies of the executable

target_link_libraries(BasicCartesianPlanner ${catkin_LIBRARIES})

target_link_libraries(BasicJointSpacePlanner ${catkin_LIBRARIES})

target_link_libraries(ObstacleAwarePlanner ${catkin_LIBRARIES})

target_link_libraries(PickPlacePlanner ${catkin_LIBRARIES})

#############

## Testing ##

#############

add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)

## Add gtest based cpp test target and link libraries

add_executable(SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp)

add_executable(SampleSimpleMotion src/SampleSimpleMotion.cpp)

# Specify libraries to link a library or executable target against

if(CATKIN_ENABLE_TESTING)

target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})

    find_package(rostest REQUIRED)

target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})

    add_rostest_gtest(tests_grasping test/grasping.test test/test.cpp src/PickPlacePlanner.cpp)

target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES})

    target_link_libraries(tests_grasping ${catkin_LIBRARIES})

 No newline at end of file

endif()

 No newline at end of file

README.md

+85
−6

Original line number
Original line
Diff line number
Diff line

### Sample applications for ROS combined with a (simulated) Franka Emika Panda robots

# Sample applications for ROS combined with a (simulated) Franka Emika Panda robots

⚠ This repository must be checked out in a [catkin](http://wiki.ros.org/catkin)

workspace containing the [franka_description](https://git-st.inf.tu-dresden.de/ceti/ros/franka_description),

[panda_moveit_config](https://git-st.inf.tu-dresden.de/ceti/ros/panda_moveit_config),

and [panda_simulation](https://git-st.inf.tu-dresden.de/ceti/ros/panda_simulation) ROS packages provided here.

For more information and installation instructions, please consult the [main project](https://git-st.inf.tu-dresden.de/ceti/ros/panda_gazebo_workspace).

Contains different examples for the planning and execution of robotic motions with MoveIt in ROS.

Contains different examples for the planning and execution of robotic motions with MoveIt in ROS.

##### The different examples:

## Example Applications

Please note that all examples must be run in a built catkin workspace from within

a shell with all ROS paths properly set up (i.e., after running `source devel/setup.sh`).

### Planning and Simulation based on RViz

This is an example to use RViz and Gazebo interactively. Motions can be planned and executed from the RViz GUI.

- Command: `roslaunch sample_applications interactive_planner.launch`

- Expected results:

    - RViz window to control the robot

    - Gazebo window to observe the executed robot motion

    - rqt to show log messages

    - The robot does not move automatically, but can be controlled with RViz (cf. the [tutorial](http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html))

### Minimal "Hello World" Example

This example of an execution of a simple motion.

- Command: `roslaunch sample_applications sample_minimal_simulation.launch`

- Expected results:

    - A Gazebo window opens in which the executed robot motion can be observed.

    - The motion starts immediately after the the system has loaded.

### Execution of a Simple Motion with Tooling

This is the same example as the first one, but this time, additional tooling is loaded.

- Command: `roslaunch sample_applications sample_simple_simulation.launch`

- Expected results:

    - A Gazebo window to observe the executed robot motion opens

    - An rqt window to show log messages opens

    - An RViz window to see the plan and trigger the motion open.

    - The motion does **not** start automatically, instead it must be triggered by pressing *next* in the RvizVisualToolsGUI within RViz

### Execution of a Simple Motion Constrained by a Blocking Object

In this example, an object can be observed in the RViz planning window. This object constrains the motion of the robot, such that the robot moves around it.

- Command: `roslaunch sample_applications sample_constraint_simulation.launch`

- Expected results:

    - A Gazebo window to observe the executed robot motion opens

    - An rqt window to show log messages opens

    - An RViz window to see the plan and trigger the motion open.

    - The motion does **not** start automatically, instead it must be triggered by pressing *next* in the RvizVisualToolsGUI within RViz

### Execution of a Velocity Constraint Cartesian Trajectory

- Command: `roslaunch sample_applications timed_cartesian_simulation.launch`

- Expected results:

    - A Gazebo window to observe the executed robot motion opens

    - An rqt window to show log messages opens

    - An RViz window to show the plan and trigger the motion open

### Execution of a Pick-and-Place Task

- Command: `roslaunch sample_applications pick_place_planner.launch`

- Expected results:

    - An RViz window to show the pick and place plan

    - A Gazebo window to observe the executed robot motion opens

    - Two tables an a "stick" on top of one of them are spawned in Gazebo

    - The Robot positions itself behind the stick and grasps it from the side.

    - It then moves to the other table and places it on top of it.

- Remarks and potential issues:

    - The grasping in *not* done using the physical properties of the object (which would require extensive tweaking

      of the surface properties of it), but using a Gazebo plugin [grasp fix](https://github.com/JenniferBuehler/gazebo-pkgs/wiki/The-Gazebo-grasp-fix-plugin).

    - The process of this tutorial is a bit fragile. It may happen that the gripper fails to grab the object, that it

      slips out of the gripper during the motion or that is is not placed correctly on the second table (and thus falls

      down). We believe that these problems might be reduced by tweaking of some properties, but in the end, all of the

      problems can also happen during an actual execution, so we have to live with them.

    - If a problem like this occurs, be thankful that it was just a simulation and simply restart.

- Planning and simulation based on rviz: `roslaunch panda_simulation simulation.launch`

- Execution of a simple motion: `roslaunch sample_applications sample_simple_simulation.launch`

- Execution of a simple motion costraint by a blocking object: `roslaunch sample_applications sample_constraint_simulation.launch`

- Execution of a velocity constraint cartesian trajectory: `roslaunch sample_applications simulation.launch`

 No newline at end of file

launch/cartesian_planner.launch0 → 100644

+9
−0

Original line number
Original line
Diff line number
Diff line

<launch>

    <include file="$(find panda_simulation)/launch/simulation.launch"/>

    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    <node pkg="sample_applications" type="BasicCartesianPlanner" name="BasicCartesianPlannerInstance" respawn="false" output="screen"/>

</launch>

launch/interactive_planner.launch0 → 100644

+7
−0

Original line number
Original line
Diff line number
Diff line

<launch>

    <include file="$(find panda_simulation)/launch/simulation.launch"/>

    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

</launch>

launch/joint_space_planner.launch0 → 100644

+9
−0

Original line number
Original line
Diff line number
Diff line

<launch>

    <include file="$(find panda_simulation)/launch/simulation.launch"/>

    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    <node pkg="sample_applications" type="BasicJointSpacePlanner" name="BasicJointSpacePlannerInstance" respawn="false" output="screen"/>

</launch>

launch/obstacle_aware_planner.launch0 → 100644

+11
−0

Original line number
Original line
Diff line number
Diff line

<launch>

    <include file="$(find panda_simulation)/launch/simulation.launch"/>

    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    <node name="spawn_box" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find sample_applications)/models/box.urdf -model box" respawn="false" output="screen" />

    <node pkg="sample_applications" type="ObstacleAwarePlanner" name="ObstacleAwarePlannerInstance" respawn="false" output="screen"/>

</launch>

launch/pick_place_planner.launch0 → 100644

+9
−0

Original line number
Original line
Diff line number
Diff line

<launch>

    <include file="$(find panda_simulation)/launch/simulation.launch"/>

    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    <node pkg="sample_applications" type="PickPlacePlanner" name="PickPlacePlanner" />

</launch>

launch/sample_constraint_simulation.launchdeleted100644 → 0

+0
−63

Original line number
Original line
Diff line number
Diff line

<launch>

    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />

    <!-- GAZEBO arguments -->

    <arg name="paused" default="false" />

    <arg name="use_sim_time" default="true" />

    <arg name="gui" default="true" />

    <arg name="headless" default="false" />

    <arg name="debug" default="false" />

    <arg name="load_gripper" default="true" />

    <!--launch GAZEBO with own world configuration -->

    <include file="$(find gazebo_ros)/launch/empty_world.launch">

        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->

        <arg name="debug" value="$(arg debug)" />

        <arg name="gui" value="$(arg gui)" />

        <arg name="paused" value="$(arg paused)" />

        <arg name="use_sim_time" value="$(arg use_sim_time)" />

        <arg name="headless" value="$(arg headless)" />

    </include>

    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />

    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />

    <!-- Load joint controller configurations from YAML file to parameter server -->

    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />

    <!-- load the controllers -->

    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />

    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />

    <!-- convert joint states to TF transforms for rviz, etc -->

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

    <include file="$(find panda_moveit_config)/launch/planning_context.launch">

        <arg name="load_robot_description" value="true" />

        <arg name="load_gripper" value="$(arg load_gripper)" />

    </include>

    <include file="$(find panda_moveit_config)/launch/move_group.launch">

        <arg name="load_gripper" value="$(arg load_gripper)" />

    </include>

    <group if="$(arg gui)">

        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    </group>

    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />

    <!-- launch robot control node for moveit motion planning -->

    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />

    <!-- load (not start!) custom joint position controller -->

    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />

    <!-- run custom node for automatic intialization -->

    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />

    <node pkg="sample_applications" type="SampleConstraintPlanner" name="SampleConstraintPlanner" />

</launch>

launch/sample_simple_simulation.launchdeleted100644 → 0

+0
−63

Original line number
Original line
Diff line number
Diff line

<launch>

    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />

    <!-- GAZEBO arguments -->

    <arg name="paused" default="false" />

    <arg name="use_sim_time" default="true" />

    <arg name="gui" default="true" />

    <arg name="headless" default="false" />

    <arg name="debug" default="false" />

    <arg name="load_gripper" default="true" />

    <!--launch GAZEBO with own world configuration -->

    <include file="$(find gazebo_ros)/launch/empty_world.launch">

        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->

        <arg name="debug" value="$(arg debug)" />

        <arg name="gui" value="$(arg gui)" />

        <arg name="paused" value="$(arg paused)" />

        <arg name="use_sim_time" value="$(arg use_sim_time)" />

        <arg name="headless" value="$(arg headless)" />

    </include>

    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />

    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />

    <!-- Load joint controller configurations from YAML file to parameter server -->

    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />

    <!-- load the controllers -->

    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />

    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />

    <!-- convert joint states to TF transforms for rviz, etc -->

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

    <include file="$(find panda_moveit_config)/launch/planning_context.launch">

        <arg name="load_robot_description" value="true" />

        <arg name="load_gripper" value="$(arg load_gripper)" />

    </include>

    <include file="$(find panda_moveit_config)/launch/move_group.launch">

        <arg name="load_gripper" value="$(arg load_gripper)" />

    </include>

    <group if="$(arg gui)">

        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    </group>

    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />

    <!-- launch robot control node for moveit motion planning -->

    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />

    <!-- load (not start!) custom joint position controller -->

    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />

    <!-- run custom node for automatic intialization -->

    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />

    <node pkg="sample_applications" type="SampleSimpleMotion" name="SampleSimpleMotion" />

</launch>

launch/sample_timed_cartesian_simulation.launchdeleted100644 → 0

+0
−63

Original line number
Original line
Diff line number
Diff line

<launch>

    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />

    <!-- GAZEBO arguments -->

    <arg name="paused" default="false" />

    <arg name="use_sim_time" default="true" />

    <arg name="gui" default="true" />

    <arg name="headless" default="false" />

    <arg name="debug" default="false" />

    <arg name="load_gripper" default="true" />

    <!--launch GAZEBO with own world configuration -->

    <include file="$(find gazebo_ros)/launch/empty_world.launch">

        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->

        <arg name="debug" value="$(arg debug)" />

        <arg name="gui" value="$(arg gui)" />

        <arg name="paused" value="$(arg paused)" />

        <arg name="use_sim_time" value="$(arg use_sim_time)" />

        <arg name="headless" value="$(arg headless)" />

    </include>

    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />

    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />

    <!-- Load joint controller configurations from YAML file to parameter server -->

    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />

    <!-- load the controllers -->

    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />

    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />

    <!-- convert joint states to TF transforms for rviz, etc -->

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

    <include file="$(find panda_moveit_config)/launch/planning_context.launch">

        <arg name="load_robot_description" value="true" />

        <arg name="load_gripper" value="$(arg load_gripper)" />

    </include>

    <include file="$(find panda_moveit_config)/launch/move_group.launch">

        <arg name="load_gripper" value="$(arg load_gripper)" />

    </include>

    <group if="$(arg gui)">

        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    </group>

    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />

    <!-- launch robot control node for moveit motion planning -->

    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />

    <!-- load (not start!) custom joint position controller -->

    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />

    <!-- run custom node for automatic intialization -->

    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />

    <node pkg="sample_applications" type="SampleTimedCartesianPlanner" name="SampleTimedCartesianPlanner" />

</launch>

mainpage.dox0 → 100644

+11
−0

Original line number
Original line
Diff line number
Diff line

/**

\mainpage

\htmlinclude manifest.html

<!--

Provide an overview of your package.

-->

*/

models/box.urdf0 → 100644

+29
−0

Original line number
Original line
Diff line number
Diff line

<robot name="box">

    <link name="world"/>

    <gazebo reference="world">

        <static>true</static>

    </gazebo>

    <joint name="fixed" type="fixed">

        <parent link="world"/>

        <child link="real_box"/>

    </joint>

    <link name="real_box">

        <inertial>

            <origin xyz="0.45 -0.2 0.0" />

            <mass value="1.0" />

            <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />

        </inertial>

        <visual>

            <origin xyz="0.45 -0.2 0.0"/>

            <geometry>

                <box size="0.4 0.1 0.3" />

            </geometry>

        </visual>

        <collision>

            <origin xyz="0.45 -0.2 0.0"/>

            <geometry>

                <box size="0.4 0.1 0.3" />

            </geometry>

        </collision>

    </link>

</robot>

 No newline at end of file

package.xml

+20
−5

Original line number
Original line
Diff line number
Diff line

<?xml version="1.0"?>

<?xml version="1.0"?>

<package format="2">

<package format="2">

  <name>sample_applications</name>

  <name>sample_applications</name>

  <version>0.0.0</version>

  <version>1.0.0</version>

  <description>The sample_applications package</description>

  <description>The sample_applications package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->

  <!-- One maintainer tag required, multiple allowed, one person per tag -->

  <!-- Example:  -->

  <!-- Example:  -->

  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->

  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->

  <maintainer email="smt@todo.todo">Chair of Software Technology</maintainer>

  <maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>

  <maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->

  <!-- One license tag required, multiple allowed, one license per tag -->

  <!-- Commonly used license strings: -->

  <!-- Commonly used license strings: -->

  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->

  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->

  <license>TODO</license>

  <license>BSD</license>

  <!-- Url tags are optional, but multiple are allowed, one per tag -->

  <!-- Optional attribute type can be: website, bugtracker, or repository -->

  <url type="website">http://ceti.pages.st.inf.tu-dresden.de/robotics/pkgs/sample_applications.html</url>

  <url type="repository">https://git-st.inf.tu-dresden.de/ceti/ros/sample_applications</url>

  <!-- Author tags are optional, multiple are allowed, one per tag -->

  <!-- Authors do not have to be maintainers, but could be -->

  <!-- Example: -->

  <author email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</author>

  <author email="johannes.mey@tu-dresden.de">Johannes Mey</author>

  <buildtool_depend>catkin</buildtool_depend>

  <buildtool_depend>catkin</buildtool_depend>

@@ -28,18 +42,19 @@

  <depend>robot_state_publisher</depend>

  <depend>robot_state_publisher</depend>

  <depend>roscpp</depend>

  <depend>roscpp</depend>

  <depend>std_msgs</depend>

  <depend>std_msgs</depend>

  <depend>tf</depend>

  <depend>xacro</depend>

  <depend>xacro</depend>

  <depend>libjsoncpp-dev</depend>

  <depend>moveit_simple_controller_manager</depend>

  <depend>moveit_simple_controller_manager</depend>

  <!-- CUSTOM -->

  <!-- CUSTOM -->

  <depend>tf2</depend>

  <depend>tf2_geometry_msgs</depend>

  <depend>moveit_core</depend>

  <depend>moveit_core</depend>

  <depend>moveit_ros_planning_interface</depend>

  <depend>moveit_ros_planning_interface</depend>

  <depend>moveit_ros_perception</depend>

  <depend>moveit_ros_perception</depend>

  <depend>moveit_visual_tools</depend>

  <depend>moveit_visual_tools</depend>

  <depend>controller_interface</depend>

  <depend>controller_interface</depend>

  <depend>hardware_interface</depend>

  <depend>hardware_interface</depend>

  <depend>simulation_util</depend>

  <exec_depend>pluginlib</exec_depend>

  <exec_depend>pluginlib</exec_depend>

rosdoc.yaml0 → 100644

+4
−0

Original line number
Original line
Diff line number
Diff line

 - builder: doxygen

   name: C++ API

   file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'

   homepage: https://st.inf.tu-dresden.de/ceti-robots

src/SampleTimedCartesianPlanner.cpp→src/BasicCartesianPlanner.cpp

+82
−0

Original line number
Original line
Diff line number
Diff line

#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit_msgs/DisplayRobotState.h>

#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>

#include <moveit_msgs/CollisionObject.h>

#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include <trajectory_msgs/JointTrajectoryPoint.h>

#include <trajectory_msgs/JointTrajectoryPoint.h>

/**

/**

 * allows time/velocity-constraint planning for cartesian paths

 * simple demo of a robotic motions based on cartesian paths

 */

 */

int main(int argc, char **argv)

int main(int argc, char **argv) {

{

    // init the node

    ros::init(argc, argv, "sample_timed_cartesian");

    ros::init(argc, argv, "basic_cartesian_planner");

    ros::NodeHandle node_handle;

    ros::NodeHandle node_handle;

    ros::AsyncSpinner spinner(1);

    ros::AsyncSpinner spinner(1);

    spinner.start();

    spinner.start();

    // wait for robot init of robot_state_initializer

    // setup the planning environment

    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");

    static const std::string PLANNING_GROUP = "panda_arm";

    ros::Duration(5.0).sleep();

    moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);

    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");

    // Visualization Setup

    namespace rvt = rviz_visual_tools;

    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");

    visual_tools.deleteAllMarkers();

    visual_tools.loadRemoteControl();

    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();

    text_pose.translation().z() = 1.75;

    visual_tools.publishText(text_pose, "Sample timed cartesian planner node", rvt::WHITE, rvt::XLARGE);

    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations

    visual_tools.trigger();

    moveit::planning_interface::MoveGroupInterface group("panda_arm");

    ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);

    ros::Duration(3.0).sleep();

    moveit_msgs::DisplayTrajectory display_trajectory;

    moveit::planning_interface::MoveGroupInterface::Plan plan;

    group.setStartStateToCurrentState();

    group.setStartStateToCurrentState();

    // You can plan a Cartesian path directly by specifying a list of waypoints

    // for the end-effector to go through. Note that we are starting

    // from the new start state above.

    std::vector<geometry_msgs::Pose> waypoints;

    std::vector<geometry_msgs::Pose> waypoints;

    // initial pose of the robot

    // initial pose of the robot

@@ -71,46 +54,27 @@ int main(int argc, char **argv)

    target_pose_3.position.x = -0.6;

    target_pose_3.position.x = -0.6;

    waypoints.push_back(target_pose_3);

    waypoints.push_back(target_pose_3);

    moveit_msgs::RobotTrajectory trajectory_msg;

    group.setPlanningTime(10.0);

    group.setPlanningTime(10.0);

    double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);

    // create a plan object and compute the cartesian motion

    moveit::planning_interface::MoveGroupInterface::Plan plan;

    // The trajectory needs to be modified so it will include velocities as well.

    moveit_msgs::RobotTrajectory trajectory_msg;

    // Create a RobotTrajectory object

    group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);

    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");

    plan.trajectory_ = trajectory_msg;

    // Get a RobotTrajectory from trajectory

    rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);

    // Create a IterativeParabolicTimeParameterization object

    // uncomment to modify maximum velocity and acceleration of the motion to be planned

    /*

    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), PLANNING_GROUP);

    trajectory_processing::IterativeParabolicTimeParameterization iptp;

    trajectory_processing::IterativeParabolicTimeParameterization iptp;

    // Compute computeTimeStamps

    bool success = iptp.computeTimeStamps(rt);

    ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

    rt.getRobotTrajectoryMsg(trajectory_msg);

    // adjust velocities, accelerations and time_from_start

    double max_velocity_scaling_factor  = 0.1;

    for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)

    double max_acceleration_scaling_factor = 0.1;

    {

        trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2);

        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)

        {

            if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){

                trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;

            }

        }

        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)

    iptp.computeTimeStamps(rt, max_velocity_scaling_factor, max_acceleration_scaling_factor);

        {

    rt.getRobotTrajectoryMsg(trajectory_msg);

            trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =

    */

                    sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));

        }

    }

    plan.trajectory_ = trajectory_msg;

    ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);

    // execute the planned motion

    group.execute(plan);

    group.execute(plan);

    ros::shutdown();

    ros::shutdown();

src/SampleSimpleMotion.cpp→src/BasicJointSpacePlanner.cpp

+62
−0

Original line number
Original line
Diff line number
Diff line

#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>

#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>

#include <moveit_msgs/CollisionObject.h>

#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

@@ -12,47 +8,32 @@

#include <trajectory_msgs/JointTrajectoryPoint.h>

#include <trajectory_msgs/JointTrajectoryPoint.h>

/**

/**

 * simple demo of constraint aware planning

 * simple demo of a robotic motion

 */

 */

int main(int argc, char** argv)

int main(int argc, char **argv) {

{

    // init the node

    ros::init(argc, argv, "SIMPLE PLANNER");

    ros::init(argc, argv, "basic_joint_space_planner");

    ros::NodeHandle node_handle;

    ros::NodeHandle node_handle;

    ros::AsyncSpinner spinner(1);

    ros::AsyncSpinner spinner(1);

    spinner.start();

    spinner.start();

    // wait for robot init of robot_state_initializer

    // setup the planning environment

    ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");

    ros::Duration(5.0).sleep();

    ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");

    static const std::string PLANNING_GROUP = "panda_arm";

    static const std::string PLANNING_GROUP = "panda_arm";

    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    const robot_state::JointModelGroup* joint_model_group =

            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    // Visualization Setup

    namespace rvt = rviz_visual_tools;

    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");

    visual_tools.deleteAllMarkers();

    visual_tools.loadRemoteControl();

    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();

    ros::Duration(3.0).sleep();

    text_pose.translation().z() = 1.75;

    visual_tools.publishText(text_pose, "simple_planner node", rvt::WHITE, rvt::XLARGE);

    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations

    // getting basic information

    visual_tools.trigger();

    ROS_INFO("Planning frame: %s", move_group.getPlanningFrame().c_str());

    ROS_INFO("End effector link: %s", move_group.getEndEffectorLink().c_str());

    // Getting Basic Information

    ROS_INFO("Available Planning Groups:");

    ROS_INFO_NAMED("simple_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());

    ROS_INFO_NAMED("simple_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());

    ROS_INFO_NAMED("simple_planner", "Available Planning Groups:");

    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),

    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),

              std::ostream_iterator<std::string>(std::cout, ", "));

              std::ostream_iterator<std::string>(std::cout, ", "));

    move_group.setStartState(*move_group.getCurrentState());

    move_group.setStartStateToCurrentState();

    // define start and target pose (by specifying the desired pose of the end-effector)

    geometry_msgs::Pose another_pose;

    geometry_msgs::Pose another_pose;

    another_pose.orientation.w = 1.0;

    another_pose.orientation.w = 1.0;

    another_pose.position.x = 0.4;

    another_pose.position.x = 0.4;

@@ -60,14 +41,20 @@ int main(int argc, char** argv)

    another_pose.position.z = 0.9;

    another_pose.position.z = 0.9;

    move_group.setPoseTarget(another_pose);

    move_group.setPoseTarget(another_pose);

    // uncomment to modify maximum velocity and acceleration of the motion to be planned

    /*

    move_group.setMaxVelocityScalingFactor(0.1);

    move_group.setMaxAccelerationScalingFactor(0.1);

     */

    // create a plan object and plan the motion

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;

    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("simple_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");

    ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");

    visual_tools.prompt("Press 'next' to move the simulated robot.");

    visual_tools.trigger();

    // Move the simulated robot in gazebo

    // Move the (simulated) robot in gazebo

    move_group.move();

    move_group.move();

    ros::shutdown();

    ros::shutdown();

src/SampleConstraintPlanner.cpp→src/ObstacleAwarePlanner.cpp

+37
−37

Original line number
Original line
Diff line number
Diff line

#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include <moveit_msgs/DisplayRobotState.h>

#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>

#include <moveit_msgs/CollisionObject.h>

#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include <trajectory_msgs/JointTrajectoryPoint.h>

#include <trajectory_msgs/JointTrajectoryPoint.h>

#include <gazebo_msgs/SpawnModel.h> // service definition for spawning things in gazebo

#include <ros/ros.h>

#include <ros/package.h>

/**

/**

 * simple demo of constraint aware planning

 * simple demo of obstacle aware planning

 */

 */

int main(int argc, char** argv)

int main(int argc, char **argv) {

{

    // init the node

    ros::init(argc, argv, "CONSTRAINT PLANNER");

    ros::init(argc, argv, "obstacle_aware_planner");

    ros::NodeHandle node_handle;

    ros::NodeHandle node_handle;

    ros::AsyncSpinner spinner(1);

    ros::AsyncSpinner spinner(1);

    spinner.start();

    spinner.start();

    // wait for robot init of robot_state_initializer

    // setup the planning environment

    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");

    ros::Duration(5.0).sleep();

    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");

    static const std::string PLANNING_GROUP = "panda_arm";

    static const std::string PLANNING_GROUP = "panda_arm";

    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    const robot_state::JointModelGroup* joint_model_group =

            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    // Visualization Setup

    // Visualization Setup

    // MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz

    namespace rvt = rviz_visual_tools;

    namespace rvt = rviz_visual_tools;

    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");

    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");

    visual_tools.deleteAllMarkers();

    visual_tools.deleteAllMarkers();

    visual_tools.loadRemoteControl();

    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();

    text_pose.translation().z() = 1.75;

    visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);

    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations

    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations

    visual_tools.trigger();

    visual_tools.enableBatchPublishing();

    // Getting Basic Information

    // remote control is an introspection tool that allows users to step through a high level script

    ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());

    // via buttons and keyboard shortcuts in RViz

    ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());

    visual_tools.loadRemoteControl();

    ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");

    ros::Duration(3.0).sleep();

    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),

              std::ostream_iterator<std::string>(std::cout, ", "));

    // Define a collision object ROS message.

    // Define a collision object ROS message.

    moveit_msgs::CollisionObject collision_object;

    moveit_msgs::CollisionObject collision_object;

@@ -65,12 +54,12 @@ int main(int argc, char** argv)

    primitive.dimensions.resize(3);

    primitive.dimensions.resize(3);

    primitive.dimensions[0] = 0.4;

    primitive.dimensions[0] = 0.4;

    primitive.dimensions[1] = 0.1;

    primitive.dimensions[1] = 0.1;

    primitive.dimensions[2] = 0.4;

    primitive.dimensions[2] = 0.3;

    // Define a pose for the box (specified relative to frame_id)

    // Define a pose for the box (specified relative to frame_id)

    geometry_msgs::Pose box_pose;

    geometry_msgs::Pose box_pose;

    box_pose.orientation.w = 1.0;

    box_pose.orientation.w = 1.0;

    box_pose.position.x = 0.4;

    box_pose.position.x = 0.45;

    box_pose.position.y = -0.2;

    box_pose.position.y = -0.2;

    box_pose.position.z = 1.0;

    box_pose.position.z = 1.0;

@@ -81,14 +70,23 @@ int main(int argc, char** argv)

    std::vector<moveit_msgs::CollisionObject> collision_objects;

    std::vector<moveit_msgs::CollisionObject> collision_objects;

    collision_objects.push_back(collision_object);

    collision_objects.push_back(collision_object);

    // Now, let's add the collision object into the world

    // We can use visual_tools to wait for user input

    ROS_INFO_NAMED("constraint_planner", "Add an object into the world");

    visual_tools.trigger();

    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

    // Add the collision object into the world

    ROS_INFO("Add an object into the world");

    planning_scene_interface.addCollisionObjects(collision_objects);

    planning_scene_interface.addCollisionObjects(collision_objects);

    // Show text in RViz of status

    shape_msgs::SolidPrimitive shape;

    visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);

    shape.dimensions.resize(3);

    shape.dimensions[0] = 0.4;

    shape.dimensions[1] = 0.1;

    shape.dimensions[2] = 0.3;

    shape.type = shape.BOX;

    visual_tools.trigger();

    visual_tools.trigger();

    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

    visual_tools.prompt("Press 'next' to plan the obstacle aware motion");

    // Now when we plan a trajectory it will avoid the obstacle

    // Now when we plan a trajectory it will avoid the obstacle

    move_group.setStartState(*move_group.getCurrentState());

    move_group.setStartState(*move_group.getCurrentState());

@@ -98,11 +96,13 @@ int main(int argc, char** argv)

    another_pose.position.y = -0.4;

    another_pose.position.y = -0.4;

    another_pose.position.z = 0.9;

    another_pose.position.z = 0.9;

    move_group.setPoseTarget(another_pose);

    move_group.setPoseTarget(another_pose);

    //move_group.setMaxVelocityScalingFactor(0.5);

    move_group.setMaxVelocityScalingFactor(0.5);

    move_group.setMaxAccelerationScalingFactor(0.5);

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;

    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");

    ROS_INFO_NAMED("obstacle_aware_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");

    visual_tools.prompt("Press 'next' to move the simulated robot.");

    visual_tools.prompt("Press 'next' to move the simulated robot.");

    visual_tools.trigger();

    visual_tools.trigger();

src/PickPlacePlanner.cpp0 → 100644

+388
−0

Original line number
Original line
Diff line number
Diff line

/*********************************************************************

* Software License Agreement (BSD License)

*

*  Copyright (c) 2012, Willow Garage, Inc.

*  All rights reserved.

*

*  Redistribution and use in source and binary forms, with or without

*  modification, are permitted provided that the following conditions

*  are met:

*

*   * Redistributions of source code must retain the above copyright

*     notice, this list of conditions and the following disclaimer.

*   * Redistributions in binary form must reproduce the above

*     copyright notice, this list of conditions and the following

*     disclaimer in the documentation and/or other materials provided

*     with the distribution.

*   * Neither the name of Willow Garage nor the names of its

*     contributors may be used to endorse or promote products derived

*     from this software without specific prior written permission.

*

*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

*  POSSIBILITY OF SUCH DAMAGE.

*********************************************************************/

/* Author: Ioan Sucan, Ridhwan Luthra*/

/*********************************************************************

* This version of the pick and place tutorial was modified and adapted

* to use the object spawner provided by the simulation_util package.

* Furthermore, additional comments have been added.

*

* The original code can be found at

* https://github.com/ros-planning/moveit_tutorials/blob/master/doc/pick_place/src/pick_place_tutorial.cpp

*

* Author: Johannes Mey

*********************************************************************/

// ROS

#include <ros/ros.h>

// MoveIt

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit/move_group_interface/move_group_interface.h>

// TF2

#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

// Zone Spawner to add the collision objects to Gazebo

#include <GazeboZoneSpawner.h>

#include "PickPlacePlanner.h"

namespace pick_place_planner {

/**

 * Sets the posture argument to an open state for the two gripper fingers (8cm apart).

 * @param posture An empty posture that is filled with a posture for two fingers

 */

void openGripper(trajectory_msgs::JointTrajectory& posture)

{

  /* Add both finger joints of panda robot. */

  posture.joint_names.resize(2);

  posture.joint_names[0] = "panda_finger_joint1";

  posture.joint_names[1] = "panda_finger_joint2";

  /* Set them as open, wide enough for the object to fit. */

  posture.points.resize(1);

  posture.points[0].positions.resize(2);

  posture.points[0].positions[0] = 0.04;

  posture.points[0].positions[1] = 0.04;

  posture.points[0].time_from_start = ros::Duration(0.5);

}

/**

 * Sets the posture argument to a closed state for the two gripper fingers

 * @param posture An empty posture that is filled with a posture for two fingers

 */

void closedGripper(trajectory_msgs::JointTrajectory& posture)

{

  /* Add both finger joints of panda robot. */

  posture.joint_names.resize(2);

  posture.joint_names[0] = "panda_finger_joint1";

  posture.joint_names[1] = "panda_finger_joint2";

  /* Set them as closed. */

  posture.points.resize(1);

  posture.points[0].positions.resize(2);

  posture.points[0].positions[0] = 0.00;

  posture.points[0].positions[1] = 0.00;

  posture.points[0].time_from_start = ros::Duration(0.5);

}

/**

 * Pick an object from a fixed position.

 * @param move_group The move group the pick operation is performed on

 */

moveit::planning_interface::MoveItErrorCode pick(moveit::planning_interface::MoveGroupInterface& move_group)

{

  // Create a vector of grasps to be attempted, currently only creating single grasp.

  // This is essentially useful when using a grasp generator to generate and test multiple grasps.

  std::vector<moveit_msgs::Grasp> grasps;

  grasps.resize(1);

  // Setting grasp pose

  // ++++++++++++++++++++++

  // This is the pose of panda_link8. |br|

  // Make sure that when you set the grasp_pose, you are setting it to be the pose of the last link in

  // your manipulator which in this case would be `"panda_link8"` You will have to compensate for the

  // transform from `"panda_link8"` to the palm of the end effector.

  grasps[0].grasp_pose.header.frame_id = "panda_link0";

  // object orientation (default orientation)

  tf2::Quaternion object_orientation{0, 0, 0, 1};

  // gripper orientation

  tf2::Quaternion gripper_orientation;

  // We know the gripper is rotated -45 degrees from the urdf model:

  // The "hand" definition in franka_description/robots/hand.xacro

  //     <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">

  //     <xacro:macro name="hand" params="connected_to:='' ns:='' rpy:='0 0 0' xyz:='0 0 0' ">

  //       <xacro:unless value="${connected_to == ''}">

  //         <joint name="${ns}_hand_joint" type="fixed">

  //           <parent link="${connected_to}"/>

  //           <child link="${ns}_hand"/>

  //           <origin xyz="${xyz}" rpy="${rpy}"/>

  //         </joint>

  //       </xacro:unless>

  //       <!-- other things... -->

  //       </xacro:macro>

  //     </robot>

  //

  // Its use in franka_description/robots/panda_arm_hand.urdf.xacro

  //     <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>

  gripper_orientation.setRPY(0, 0, -M_PI_4);

  // If we did not know it, we could also look it up!

  // tf2_ros::Buffer tfBuffer;

  // tf2_ros::TransformListener tfListener(tfBuffer);

  // ros::WallDuration(0.1).sleep(); // wait a bit for the tf listener

  // tf2::fromMsg((tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform).rotation, gripper_orientation);

  // approach orientation

  double approach_roll, approach_pitch, approach_yaw;

  approach_roll =  -M_PI_2; // The neutral rotation has the gripper "looking down",

  approach_pitch = -M_PI_4; // with these transformations, it grasps from one side.

  approach_yaw =   -M_PI_4; // This results in (x=-0.5,y=-0.5,z=0,w=0.7071068)

  tf2::Quaternion approach_orientation;

  approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);

  tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;

  grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);

  grasps[0].grasp_pose.pose.position.x = 0.4;

  grasps[0].grasp_pose.pose.position.y = 0;

  grasps[0].grasp_pose.pose.position.z = 0.5;

  // Setting pre-grasp approach

  // ++++++++++++++++++++++++++

  /* Defined with respect to frame_id */

  grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";

  /* Direction is set as positive x axis */

  grasps[0].pre_grasp_approach.direction.vector.x = 1.0;

  grasps[0].pre_grasp_approach.min_distance = 0.095;

  grasps[0].pre_grasp_approach.desired_distance = 0.115;

  // Setting post-grasp retreat

  // ++++++++++++++++++++++++++

  /* Defined with respect to frame_id */

  grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";

  /* Direction is set as positive z axis */

  grasps[0].post_grasp_retreat.direction.vector.z = 1.0;

  grasps[0].post_grasp_retreat.min_distance = 0.1;

  grasps[0].post_grasp_retreat.desired_distance = 0.25;

  // Setting posture of eef before grasp

  // +++++++++++++++++++++++++++++++++++

  openGripper(grasps[0].pre_grasp_posture);

  // Setting posture of eef during grasp

  // +++++++++++++++++++++++++++++++++++

  closedGripper(grasps[0].grasp_posture);

  // Set support surface as table1.

  move_group.setSupportSurfaceName("table1");

  // Call pick to pick up the object using the grasps given

  return move_group.pick("object", grasps);

}

/**

 * Place an object at a fixed position on top of table2.

 * @param group

 */

moveit::planning_interface::MoveItErrorCode place(moveit::planning_interface::MoveGroupInterface& group)

{

  std::vector<moveit_msgs::PlaceLocation> place_location;

  place_location.resize(1);

  // Setting place location pose

  // +++++++++++++++++++++++++++

  place_location[0].place_pose.header.frame_id = "panda_link0";

  tf2::Quaternion orientation;

  orientation.setRPY(0, 0, M_PI / 2);

  place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);

  /* For place location, we set the value to the exact location of the center of the object. */

  place_location[0].place_pose.pose.position.x = 0;

  place_location[0].place_pose.pose.position.y = 0.5;

  place_location[0].place_pose.pose.position.z = 0.5;

  // Setting pre-place approach

  // ++++++++++++++++++++++++++

  /* Defined with respect to frame_id */

  place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";

  /* Direction is set as negative z axis */

  place_location[0].pre_place_approach.direction.vector.z = -1.0;

  place_location[0].pre_place_approach.min_distance = 0.095;

  place_location[0].pre_place_approach.desired_distance = 0.115;

  // Setting post-grasp retreat

  // ++++++++++++++++++++++++++

  /* Defined with respect to frame_id */

  place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";

  /* Direction is set as negative y axis */

  place_location[0].post_place_retreat.direction.vector.y = -1.0;

  place_location[0].post_place_retreat.min_distance = 0.1;

  place_location[0].post_place_retreat.desired_distance = 0.25;

  // Setting posture of eef after placing object

  // +++++++++++++++++++++++++++++++++++++++++++

  /* Similar to the pick case */

  openGripper(place_location[0].post_place_posture);

  // Set support surface as table2.

  group.setSupportSurfaceName("table2");

  // Call place to place the object using the place locations given.

  return group.place("object", place_location);

}

/**

 * Add the collision objects to the planning scene and to gazebo.

 * @param planning_scene_interface The interface to access the planning scene.

 */

void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)

{

  // Besides the robot, our scene comprises three objects. Two tables, table1 and table2, and the object to be picked

  // and placed. Thus, three collision objects have to be added to the planning scene and to Gazebo.

  // Create vector to hold 3 collision objects.

  std::vector<moveit_msgs::CollisionObject> collision_objects;

  collision_objects.resize(3);

  /********************************************************************************

   * table1 (the one on which the pick object is positioned before the operation)

   ********************************************************************************/

  // part 1: add the object to the scene

  collision_objects[0].id = "table1";

  collision_objects[0].header.frame_id = "panda_link0";

  /* Define the primitive and its dimensions. */

  collision_objects[0].primitives.resize(1);

  collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;

  collision_objects[0].primitives[0].dimensions.resize(3);

  collision_objects[0].primitives[0].dimensions[0] = 0.2;

  collision_objects[0].primitives[0].dimensions[1] = 0.4;

  collision_objects[0].primitives[0].dimensions[2] = 0.4;

  /* Define the pose of the table. */

  collision_objects[0].primitive_poses.resize(1);

  collision_objects[0].primitive_poses[0].position.x = 0.5;

  collision_objects[0].primitive_poses[0].position.y = 0;

  collision_objects[0].primitive_poses[0].position.z = 0.2;

  collision_objects[0].primitive_poses[0].orientation.w = 1.0;

  collision_objects[0].operation = collision_objects[0].ADD;

  // part 2: add the object to gazebo

  /* Define the primitive and its dimensions *again*. */

  shape_msgs::SolidPrimitive table1;

  table1.type = shape_msgs::SolidPrimitive::BOX;

  table1.dimensions = collision_objects[0].primitives[0].dimensions;

  /* Define the pose of the table *again*. */

  geometry_msgs::Pose table1pose;

  table1pose.position.x = collision_objects[0].primitive_poses[0].position.x;

  table1pose.position.y = collision_objects[0].primitive_poses[0].position.y;

  table1pose.position.z = collision_objects[0].primitive_poses[0].position.z;

  table1pose.orientation.w = 1; // the orientation is always the default

  /* spawn the table in Gazebo, weighing 20 kg, but without physics enabled to increase stability */

  GazeboZoneSpawner::spawnPrimitive("table1", table1, table1pose, std_msgs::ColorRGBA(), 20, true);

  /********************************************************************************

   * table2 (the one to which the pick object is move to by the operation)

   ********************************************************************************/

  // part 1: add the object to the scene

  collision_objects[1].id = "table2";

  collision_objects[1].header.frame_id = "panda_link0";

  /* Define the primitive and its dimensions. */

  collision_objects[1].primitives.resize(1);

  collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;

  collision_objects[1].primitives[0].dimensions.resize(3);

  collision_objects[1].primitives[0].dimensions[0] = 0.4;

  collision_objects[1].primitives[0].dimensions[1] = 0.2;

  collision_objects[1].primitives[0].dimensions[2] = 0.4;

  /* Define the pose of the table. */

  collision_objects[1].primitive_poses.resize(1);

  collision_objects[1].primitive_poses[0].position.x = 0;

  collision_objects[1].primitive_poses[0].position.y = 0.5;

  collision_objects[1].primitive_poses[0].position.z = 0.2;

  collision_objects[1].primitive_poses[0].orientation.w = 1.0;

  collision_objects[1].operation = collision_objects[1].ADD;

  // part 2: add the object to gazebo

  /* Define the primitive and its dimensions *again*. */

  shape_msgs::SolidPrimitive table2;

  table2.type = shape_msgs::SolidPrimitive::BOX;

  table2.dimensions = collision_objects[1].primitives[0].dimensions;

  /* Define the pose of the table *again*. */

  geometry_msgs::Pose table2pose;

  table2pose.position.x = collision_objects[1].primitive_poses[0].position.x;

  table2pose.position.y = collision_objects[1].primitive_poses[0].position.y;

  table2pose.position.z = collision_objects[1].primitive_poses[0].position.z;

  table2pose.orientation.w = 1; // the orientation is always the default

  /* spawn the table in Gazebo, weighing 20 kg, but without physics enabled to increase stability */

  GazeboZoneSpawner::spawnPrimitive("table2", table2, table2pose, std_msgs::ColorRGBA(), 20, true);

  /********************************************************************************

   * object (the object to be picked and placed)

   ********************************************************************************/

  // Define the object that we will be manipulating

  collision_objects[2].header.frame_id = "panda_link0";

  collision_objects[2].id = "object";

  /* Define the primitive and its dimensions. */

  collision_objects[2].primitives.resize(1);

  collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;

  collision_objects[2].primitives[0].dimensions.resize(3);

  collision_objects[2].primitives[0].dimensions[0] = 0.02;

  collision_objects[2].primitives[0].dimensions[1] = 0.02;

  collision_objects[2].primitives[0].dimensions[2] = 0.2;

  /* Define the pose of the object. */

  collision_objects[2].primitive_poses.resize(1);

  collision_objects[2].primitive_poses[0].position.x = 0.5;

  collision_objects[2].primitive_poses[0].position.y = 0;

  collision_objects[2].primitive_poses[0].position.z = 0.5;

  collision_objects[2].primitive_poses[0].orientation.w = 1.0;

  collision_objects[2].operation = collision_objects[2].ADD;

  // part 2: add the object to gazebo

  /* Define the primitive and its dimensions *again*. */

  shape_msgs::SolidPrimitive object;

  object.type = shape_msgs::SolidPrimitive::BOX;

  object.dimensions = collision_objects[2].primitives[0].dimensions;

  /* Define the pose of the object *again*. */

  geometry_msgs::Pose objectpose;

  objectpose.position.x = collision_objects[2].primitive_poses[0].position.x;

  objectpose.position.y = collision_objects[2].primitive_poses[0].position.y;

  objectpose.position.z = collision_objects[2].primitive_poses[0].position.z;

  objectpose.orientation.w = 1; // the orientation is always the default

  /* spawn the object in Gazebo, weighing 1 kg, with physics enabled */

  GazeboZoneSpawner::spawnPrimitive("pickObject", object, objectpose, std_msgs::ColorRGBA(), 1, false);

  // now that we have defined all three objects, we add them to the planning scene

  planning_scene_interface.applyCollisionObjects(collision_objects);

}

} // end namespace pick_place_planner

src/PickPlacePlanner.h0 → 100644

+46
−0

Original line number
Original line
Diff line number
Diff line

#ifndef SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H

#define SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H

// ROS

#include <ros/ros.h>

// MoveIt

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit/move_group_interface/move_group_interface.h>

namespace pick_place_planner {

/**

 * Sets the posture argument to an open state for the two gripper fingers (8cm apart).

 * @param posture An empty posture that is filled with a posture for two fingers

 */

void openGripper(trajectory_msgs::JointTrajectory &posture);

/**

 * Sets the posture argument to a closed state for the two gripper fingers

 * @param posture An empty posture that is filled with a posture for two fingers

 */

void closedGripper(trajectory_msgs::JointTrajectory &posture);

/**

 * Pick an object from a fixed position.

 * @param move_group The move group the pick operation is performed on

 */

moveit::planning_interface::MoveItErrorCode pick(moveit::planning_interface::MoveGroupInterface &move_group);

/**

* Place an object at a fixed position on top of table2.

* @param group

*/

moveit::planning_interface::MoveItErrorCode place(moveit::planning_interface::MoveGroupInterface &group);

/**

* Add the collision objects to the planning scene and to gazebo.

* @param planning_scene_interface The interface to access the planning scene.

*/

void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface &planning_scene_interface);

}

#endif // SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H

 No newline at end of file

src/PickPlacePlannerMain.cpp0 → 100644

+33
−0

Original line number
Original line
Diff line number
Diff line

#include "PickPlacePlanner.h"

int main(int argc, char** argv)

{

    ros::init(argc, argv, "panda_arm_pick_place");

    ros::NodeHandle nh;

    ros::AsyncSpinner spinner(1);

    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");

    ros::Duration(3.0).sleep();

    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");

    spinner.start();

    ros::WallDuration(1.0).sleep();

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    moveit::planning_interface::MoveGroupInterface group("panda_arm");

    group.setPlanningTime(45.0);

    pick_place_planner::addCollisionObjects(planning_scene_interface);

    // Wait a bit for ROS things to initialize

    ros::WallDuration(1.0).sleep();

    pick_place_planner::pick(group);

    ros::WallDuration(1.0).sleep();

    pick_place_planner::place(group);

    ros::waitForShutdown();

    return 0;

}

 No newline at end of file

test/grasping.test0 → 100644

+12
−0

Original line number
Original line
Diff line number
Diff line

<launch>

    <include file="$(find panda_simulation)/launch/simulation.launch">

        <arg name="slow_simulation" value="false" />

    </include>

    <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"

          args="-d $(find sample_applications)/test/plan_visualizer.rviz" output="screen">

        <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>

    </node>

    <test test-name="PickPlacePlannerTest" pkg="sample_applications" type="tests_grasping"  time-limit="3600.0" />

</launch>

 No newline at end of file

test/plan_visualizer.rviz0 → 100644

+286
−0

Original line number
Original line
Diff line number
Diff line

Panels:

  []

Preferences:

  PromptSaveOnExit: true

Toolbars:

  toolButtonStyle: 2

Visualization Manager:

  Class: ""

  Displays:

    - Alpha: 0.5

      Cell Size: 1

      Class: rviz/Grid

      Color: 160; 160; 164

      Enabled: true

      Line Style:

        Line Width: 0.029999999329447746

        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

    - Acceleration_Scaling_Factor: 1

      Class: moveit_rviz_plugin/MotionPlanning

      Enabled: true

      Move Group Namespace: ""

      MoveIt_Allow_Approximate_IK: false

      MoveIt_Allow_External_Program: false

      MoveIt_Allow_Replanning: false

      MoveIt_Allow_Sensor_Positioning: false

      MoveIt_Goal_Tolerance: 0

      MoveIt_Planning_Attempts: 10

      MoveIt_Planning_Time: 5

      MoveIt_Use_Cartesian_Path: false

      MoveIt_Use_Constraint_Aware_IK: true

      MoveIt_Warehouse_Host: 127.0.0.1

      MoveIt_Warehouse_Port: 33829

      MoveIt_Workspace:

        Center:

          X: 0

          Y: 0

          Z: 0

        Size:

          X: 2

          Y: 2

          Z: 2

      Name: MotionPlanning

      Planned Path:

        Color Enabled: false

        Interrupt Display: false

        Links:

          All Links Enabled: true

          Expand Joint Details: false

          Expand Link Details: false

          Expand Tree: false

          Link Tree Style: Links in Alphabetic Order

          panda_hand:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_leftfinger:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link0:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link1:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link2:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link3:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link4:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link5:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link6:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link7:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link8:

            Alpha: 1

            Show Axes: false

            Show Trail: false

          panda_rightfinger:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          world:

            Alpha: 1

            Show Axes: false

            Show Trail: false

        Loop Animation: true

        Robot Alpha: 0.5

        Robot Color: 150; 50; 150

        Show Robot Collision: false

        Show Robot Visual: true

        Show Trail: false

        State Display Time: 0.05 s

        Trail Step Size: 1

        Trajectory Topic: move_group/display_planned_path

      Planning Metrics:

        Payload: 1

        Show Joint Torques: false

        Show Manipulability: false

        Show Manipulability Index: false

        Show Weight Limit: false

        TextHeight: 0.07999999821186066

      Planning Request:

        Colliding Link Color: 255; 0; 0

        Goal State Alpha: 1

        Goal State Color: 250; 128; 0

        Interactive Marker Size: 0

        Joint Violation Color: 255; 0; 255

        Planning Group: panda_arm

        Query Goal State: false

        Query Start State: false

        Show Workspace: false

        Start State Alpha: 1

        Start State Color: 0; 255; 0

      Planning Scene Topic: move_group/monitored_planning_scene

      Robot Description: robot_description

      Scene Geometry:

        Scene Alpha: 0.8999999761581421

        Scene Color: 50; 230; 50

        Scene Display Time: 0.20000000298023224

        Show Scene Geometry: true

        Voxel Coloring: Z-Axis

        Voxel Rendering: Occupied Voxels

      Scene Robot:

        Attached Body Color: 150; 50; 150

        Links:

          All Links Enabled: true

          Expand Joint Details: false

          Expand Link Details: false

          Expand Tree: false

          Link Tree Style: Links in Alphabetic Order

          panda_hand:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_leftfinger:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link0:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link1:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link2:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link3:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link4:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link5:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link6:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link7:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link8:

            Alpha: 1

            Show Axes: false

            Show Trail: false

          panda_rightfinger:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          world:

            Alpha: 1

            Show Axes: false

            Show Trail: false

        Robot Alpha: 0.5

        Show Robot Collision: false

        Show Robot Visual: false

      Value: true

      Velocity_Scaling_Factor: 1

  Enabled: true

  Global Options:

    Background Color: 48; 48; 48

    Default Light: true

    Fixed Frame: panda_link0

    Frame Rate: 30

  Name: root

  Tools:

    - Class: rviz/Interact

      Hide Inactive Objects: true

    - Class: rviz/MoveCamera

    - Class: rviz/Select

    - Class: rviz_visual_tools/KeyTool

  Value: true

  Views:

    Current:

      Class: rviz/XYOrbit

      Distance: 2.9803097248077393

      Enable Stereo Rendering:

        Stereo Eye Separation: 0.05999999865889549

        Stereo Focal Distance: 1

        Swap Stereo Eyes: false

        Value: false

      Focal Point:

        X: 1.0864897966384888

        Y: 0.3387787342071533

        Z: 2.2351798634190345e-7

      Focal Shape Fixed Size: true

      Focal Shape Size: 0.05000000074505806

      Invert Z Axis: false

      Name: Current View

      Near Clip Distance: 0.009999999776482582

      Pitch: 0.270203560590744

      Target Frame: panda_link0

      Value: XYOrbit (rviz)

      Yaw: 0.42677485942840576

    Saved: ~

Window Geometry:

  Height: 1404

  Hide Left Dock: false

  Hide Right Dock: false

  MotionPlanning:

    collapsed: false

  MotionPlanning - Trajectory Slider:

    collapsed: false

  QMainWindow State: 000000ff00000000fd00000002000000000000013b000004e1fc0200000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000080000003640000017200fffffffb0000000800480065006c00700100000334000000e10000000000000000fb0000000a00560069006500770073000000003b000004e1000000000000000000000003000004fe0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000000000004fe0000010000ffffff000004fe000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000

  Width: 1278

  X: 0

  Y: 18

test/test.cpp0 → 100644

+82
−0

Original line number
Original line
Diff line number
Diff line

#include "PickPlacePlanner.h"

#include <gtest/gtest.h>

class Grasping : public ::testing::Test {

public:

    Grasping(): spinner(nullptr) {};

protected:

    ros::AsyncSpinner* spinner;

    ros::NodeHandle* node;

    void SetUp() override {

        ::testing::Test::SetUp();

        this->node = new ros::NodeHandle("~");

        this->spinner = new ros::AsyncSpinner(2);

        this->spinner->start();

        ROS_INFO(">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");

        ros::Duration(3.0).sleep();

        ROS_INFO(">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");

        // clean up scene

        CleanupScene();

    }

    static void CleanupScene() {

        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

        for (auto p : planning_scene_interface.getObjects()) {

            p.second.operation = moveit_msgs::CollisionObject::REMOVE;

            planning_scene_interface.applyCollisionObject(p.second);

        }

        ASSERT_EQ(planning_scene_interface.getObjects().size(), 0);

        ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0);

    }

    void TearDown() override

    {

        ros::shutdown();

        delete this->spinner;

        delete this->node;

        ::testing::Test::TearDown();

    }

};

TEST_F(Grasping, PickPlacePlannerTest) {

    ros::WallDuration(1.0).sleep();

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    moveit::planning_interface::MoveGroupInterface group("panda_arm");

    group.setPlanningTime(45.0);

    pick_place_planner::addCollisionObjects(planning_scene_interface);

    // Wait a bit for ROS things to initialize

    ros::WallDuration(1.0).sleep();

    ASSERT_EQ(pick_place_planner::pick(group), moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ros::WallDuration(1.0).sleep();

    ASSERT_EQ(pick_place_planner::place(group), moveit::planning_interface::MoveItErrorCode::SUCCESS);

}

int main(int argc, char** argv)

{

    ros::init(argc, argv, "panda_arm_pick_place_test");

    ::testing::InitGoogleTest(&argc, argv);

    int result = RUN_ALL_TESTS();

    return result;

}

Compare revisions

Source

Target

Commits on Source 21

Files

CMakeLists.txt

README.md

launch/cartesian_planner.launch

launch/interactive_planner.launch

launch/joint_space_planner.launch

launch/obstacle_aware_planner.launch

launch/pick_place_planner.launch

launch/sample_constraint_simulation.launch

launch/sample_simple_simulation.launch

launch/sample_timed_cartesian_simulation.launch

mainpage.dox

models/box.urdf

package.xml

rosdoc.yaml

src/SampleTimedCartesianPlanner.cpp→src/BasicCartesianPlanner.cpp

src/SampleSimpleMotion.cpp→src/BasicJointSpacePlanner.cpp

src/SampleConstraintPlanner.cpp→src/ObstacleAwarePlanner.cpp

src/PickPlacePlanner.cpp

src/PickPlacePlanner.h

src/PickPlacePlannerMain.cpp

test/grasping.test

test/plan_visualizer.rviz

test/test.cpp