Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
  • grasping_sample
  • master
2 results

Target

Select target project
  • ceti/ros/packages/sample_applications
  • johannes.mey/sample_applications
  • nikaviator/sample_applications
  • chbraun/sample_applications
4 results
Select Git revision
  • master
1 result
Show changes
Commits on Source (17)
Showing
with 747 additions and 362 deletions
......@@ -5,8 +5,8 @@ add_compile_options(-std=c++14)
set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
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
# catkin packages
# Find catkin macros and libraries if COMPONENTS list like `find_package(catkin REQUIRED COMPONENTS xyz)` is used,
# also find other catkin packages
find_package(catkin REQUIRED
COMPONENTS controller_manager
effort_controllers
......@@ -17,6 +17,8 @@ find_package(catkin REQUIRED
joint_trajectory_controller
robot_state_publisher
roscpp
tf2
tf2_geometry_msgs
std_msgs
xacro
moveit_core
......@@ -24,15 +26,17 @@ find_package(catkin REQUIRED
moveit_ros_planning
moveit_ros_planning_interface
controller_interface
hardware_interface)
hardware_interface
simulation_util)
# System dependencies are found with CMake's conventions
find_package(PkgConfig REQUIRED)
# ################################################################################################################################
# ######################################################################################################################
# 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
moveit_core
moveit_visual_tools
......@@ -40,24 +44,38 @@ catkin_package(CATKIN_DEPENDS
controller_interface
hardware_interface
pluginlib
tf2
simulation_util
DEPENDS
# system_lib
)
# ################################################################################################################################
# ######################################################################################################################
# Build ##
# ################################################################################################################################
# ######################################################################################################################
# Specify additional locations of header files Your package locations should be listed before other locations
include_directories(${catkin_INCLUDE_DIRS})
add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
add_executable(SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp)
add_executable(SampleSimpleMotion src/SampleSimpleMotion.cpp)
add_executable(MinimalSimpleMotion src/MinimalSimpleMotion.cpp)
# Specify libraries to link a library or executable target against
target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})
target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})
target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES})
target_link_libraries(MinimalSimpleMotion ${catkin_LIBRARIES})
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 gtest based cpp test target and link libraries
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest_gtest(tests_grasping test/grasping.test test/test.cpp src/PickPlacePlanner.cpp)
target_link_libraries(tests_grasping ${catkin_LIBRARIES})
endif()
\ No newline at end of file
......@@ -13,6 +13,18 @@ Contains different examples for the planning and execution of robotic motions wi
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.
......@@ -34,7 +46,7 @@ This is the same example as the first one, but this time, additional tooling is
- The motion does **not** start automatically, instead it must be triggered by pressing *next* in the RvizVisualToolsGUI within RViz
### Execution of a Simple Motion Costrained by a Blocking Object
### 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.
......@@ -47,23 +59,30 @@ In this example, an object can be observed in the RViz planning window. This obj
### Execution of a Velocity Constraint Cartesian Trajectory
- Command: `roslaunch sample_applications simulation.launch`
- 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 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
### Planning and Simulation based on RViz
- An RViz window to show the plan and trigger the motion open
This is an example to use RViz and Gazebo interactively. Motions can be planned and executed from the RViz GUI.
### Execution of a Pick-and-Place Task
- Command: `roslaunch panda_simulation simulation.launch`
- Command: `roslaunch sample_applications pick_place_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))
- 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.
......
<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>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
</launch>
<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>
<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>
<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>
<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" />
<param name="box_description" command="$(find xacro)/xacro --inorder $(find sample_applications)/models/box.xacro"/>
<node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-param box_description -urdf -model box"/>
<!-- 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>
<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="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>
<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>
<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>
<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
\htmlinclude manifest.html
<!--
Provide an overview of your package.
-->
*/
<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
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="simple_box">
<link name="object_base_link">
</link>
<joint name="object_base_joint" type="fixed">
<parent link="object_base_link"/>
<child link="object_link"/>
<axis xyz="0 0 1" />
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<link name="object_link">
<inertial>
<origin xyz="0.4 -0.2 1.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.4 -0.2 1.0"/>
<geometry>
<box size="0.4 0.1 0.4" />
</geometry>
</visual>
<collision>
<origin xyz="0.4 -0.2 1.0"/>
<geometry>
<box size="0.4 0.1 0.4" />
</geometry>
</collision>
</link>
<gazebo reference="object_base_link">
<gravity>0</gravity>
</gazebo>
<gazebo reference="object_link">
<material>Gazebo/Blue</material>
</gazebo>
</robot>
\ No newline at end of file
<?xml version="1.0"?>
<package format="2">
<name>sample_applications</name>
<version>0.0.0</version>
<version>1.0.0</version>
<description>The sample_applications package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <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 -->
<!-- Commonly used license strings: -->
<!-- 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>
......@@ -28,18 +42,19 @@
<depend>robot_state_publisher</depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>tf</depend>
<depend>xacro</depend>
<depend>libjsoncpp-dev</depend>
<depend>moveit_simple_controller_manager</depend>
<!-- CUSTOM -->
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_ros_perception</depend>
<depend>moveit_visual_tools</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>simulation_util</depend>
<exec_depend>pluginlib</exec_depend>
......
- builder: doxygen
name: C++ API
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
homepage: https://st.inf.tu-dresden.de/ceti-robots
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.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)
{
ros::init(argc, argv, "sample_timed_cartesian");
int main(int argc, char **argv) {
// init the node
ros::init(argc, argv, "basic_cartesian_planner");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
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");
// setup the planning environment
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
ros::Duration(3.0).sleep();
moveit::planning_interface::MoveGroupInterface::Plan plan;
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;
// initial pose of the robot
......@@ -71,46 +54,27 @@ int main(int argc, char **argv)
target_pose_3.position.x = -0.6;
waypoints.push_back(target_pose_3);
moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);
// The trajectory needs to be modified so it will include velocities as well.
// Create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
// Get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// create a plan object and compute the cartesian motion
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit_msgs::RobotTrajectory trajectory_msg;
group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);
plan.trajectory_ = 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;
// 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
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
{
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;
}
}
double max_velocity_scaling_factor = 0.1;
double max_acceleration_scaling_factor = 0.1;
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
{
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);
iptp.computeTimeStamps(rt, max_velocity_scaling_factor, max_acceleration_scaling_factor);
rt.getRobotTrajectoryMsg(trajectory_msg);
*/
// execute the planned motion
group.execute(plan);
ros::shutdown();
......
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
/**
* minimal demo of constraint aware planning
* simple demo of a robotic motion
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "SIMPLE PLANNER");
int main(int argc, char **argv) {
// init the node
ros::init(argc, argv, "basic_joint_space_planner");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
// setup the planning environment
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
ros::Duration(3.0).sleep();
// Getting Basic Information
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:");
// getting basic information
ROS_INFO("Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO("End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO("Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
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;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
......@@ -45,11 +41,20 @@ int main(int argc, char** argv)
another_pose.position.z = 0.9;
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;
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");
// Move the simulated robot in gazebo
// Move the (simulated) robot in gazebo
move_group.move();
ros::shutdown();
......
#include <moveit/move_group_interface/move_group_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_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.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)
{
ros::init(argc, argv, "CONSTRAINT PLANNER");
int main(int argc, char **argv) {
// init the node
ros::init(argc, argv, "obstacle_aware_planner");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
// setup the planning environment
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization Setup
// MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
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 constraint 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();
visual_tools.enableBatchPublishing();
// Getting Basic Information
ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// remote control is an introspection tool that allows users to step through a high level script
// via buttons and keyboard shortcuts in RViz
visual_tools.loadRemoteControl();
ros::Duration(3.0).sleep();
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
......@@ -65,12 +54,12 @@ int main(int argc, char** argv)
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
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)
geometry_msgs::Pose box_pose;
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.z = 1.0;
......@@ -81,14 +70,23 @@ int main(int argc, char** argv)
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// Now, let's add the collision object into the world
ROS_INFO_NAMED("constraint_planner", "Add an object into the world");
// We can use visual_tools to wait for user input
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);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
shape_msgs::SolidPrimitive shape;
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.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
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.z = 0.9;
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;
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.trigger();
......
/*********************************************************************
* 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