CMakeLists.txt

@@ -54,8 +54,10 @@ 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})

README.md

@@ -5,6 +5,20 @@ Contains different examples for the planning and execution of robotic motions wi
 ##### The different examples:
 
 - Planning and simulation based on rviz: `roslaunch panda_simulation simulation.launch`
-- Execution of a simple motion: `roslaunch sample_applications sample_simple_simulation.launch`
+    - RViz window to control the robot ([tutorial](http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html))
+    - Gazebo window to observe the executed robot motion
+    - rqt to show log messages
+- Minimal example of an execution of a simple motion: `roslaunch sample_applications sample_minimal_simulation.launch`
+    - Gazebo window to observe the executed robot motion
+- Execution of a simple motion (with tooling): `roslaunch sample_applications sample_simple_simulation.launch`
+    - RViz window to see the plan and trigger the motion (press *next* in the RvizVisualToolGUI)
+    - Gazebo window to observe the executed robot motion
+    - rqt to show log messages
 - Execution of a simple motion costraint by a blocking object: `roslaunch sample_applications sample_constraint_simulation.launch`
+    - RViz window to see the plan and trigger the motion (press *next* in the RvizVisualToolGUI)
+    - Gazebo window to observe the executed robot motion
+    - rqt to show log messages
 - Execution of a velocity constraint cartesian trajectory: `roslaunch sample_applications simulation.launch`
+    - RViz window to see the plan and trigger the motion (press *next* in the RvizVisualToolGUI)
+    - Gazebo window to observe the executed robot motion
+    - rqt to show log messages

launch/sample_minimal_simulation.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>

src/MinimalSimpleMotion.cpp

+#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 <trajectory_msgs/JointTrajectoryPoint.h>
+
+/**
+ * minimal demo of constraint aware planning
+ */
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "SIMPLE 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 <<<<<<<<<<<<<<<<");
+
+    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;
+
+    // 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:");
+    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
+              std::ostream_iterator<std::string>(std::cout, ", "));
+
+    move_group.setStartState(*move_group.getCurrentState());
+    geometry_msgs::Pose another_pose;
+    another_pose.orientation.w = 1.0;
+    another_pose.position.x = 0.4;
+    another_pose.position.y = -0.4;
+    another_pose.position.z = 0.9;
+    move_group.setPoseTarget(another_pose);
+
+    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");
+
+    // Move the simulated robot in gazebo
+    move_group.move();
+
+    ros::shutdown();
+    return 0;
+}