panda teaching extracted from panda_simulation repo

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(panda_teaching)
+
+# Compile as C++11, supported in ROS Kinetic and newer
+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_package(catkin REQUIRED
+             COMPONENTS controller_manager
+                        effort_controllers
+                        gazebo_ros
+                        joint_state_controller
+                        joint_state_publisher
+                        joint_trajectory_controller
+                        robot_state_publisher
+                        roscpp
+                        std_msgs
+                        tf
+                        xacro
+                        moveit_core
+                        moveit_visual_tools
+                        moveit_ros_planning
+                        moveit_ros_planning_interface
+                        controller_interface
+                        hardware_interface)
+
+# System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS filesystem)
+
+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
+catkin_package(CATKIN_DEPENDS
+               moveit_core
+               moveit_visual_tools
+               moveit_ros_planning_interface
+               controller_interface
+               hardware_interface
+               pluginlib
+        )
+
+# ################################################################################################################################
+# Build ##
+# ################################################################################################################################
+
+# Specify additional locations of header files Your package locations should be listed before other locations
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_executable(pose_collector src/teaching/pose_collector.cpp)
+add_executable(pose_replayer src/teaching/pose_replayer.cpp)
+add_executable(pose_tf_listener src/teaching/pose_tf_listener.cpp)
+
+target_link_libraries(pose_tf_listener ${catkin_LIBRARIES})
+target_link_libraries(pose_replayer ${catkin_LIBRARIES})
+target_link_libraries(pose_collector ${catkin_LIBRARIES})

README.md

+# panda_simulation
+
+![Panda in Gazebo](assets/panda-in-gazebo.png?raw=true "Panda in Gazebo")
+
+#### Getting started
+
+This package only works together with the fully git-modules cloned workspace repository. So first you have to clone everythink like it is described on https://git-st.inf.tu-dresden.de/ceti/ros-internal/panda_gazebo_workspace. Second you have to clone this repository inside the src-folder of the workspace.
+
+#### Running the simulation
+
+Build the catkin workspace and run the simulation with:
+```
+catkin build
+source devel/setup.bash
+roslaunch panda_simulation simulation.launch
+```
+
+Depending on your operating systems language you might need to export the numeric type so that rviz can read the floating point numbers in the robot model correctly:
+
+```
+export LC_NUMERIC="en_US.UTF-8"
+```
+Otherwise, the robot will appear in rviz in a collapsed state.
+
+
+#### Teaching poses
+
+You can teach the (simulated) robot several poses which can be replayed in the order there where teached.
+
+##### Steps to teach the robot poses
+
+1. Drag the robot in rviz to the target destination.
+2. Plan and execute the trajectory.
+3. Use a sourced console and save the pose by typing `rostopic pub /collectPose std_msgs/String 'collect'`.
+4. Do this as much as you want to.
+5. Replay the poses by typing `rostopic pub /replayPose std_msgs/String 'replay'` in a sourced console.
+
+You can also reset the saved poses by typing `rostopic pub /collectPose std_msgs/String 'clear'`.
+

launch/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="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="panda_teaching" type="pose_tf_listener" name="pose_tf_listener" output="screen" />
+    <node pkg="panda_teaching" type="pose_collector" name="pose_collector" output="screen" />
+    <node pkg="panda_teaching" type="pose_replayer" name="pose_replayer" output="screen" />
+</launch>

package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>panda_teaching</name>
+  <version>0.0.0</version>
+  <description>The panda_teaching package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="sebastian@todo.todo">sebastian</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>
+
+  <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>
+
+  <exec_depend>pluginlib</exec_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/teaching/pose_collector.cpp

+//
+// Created by sebastian on 04.05.20.
+//
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+
+void collectPose(){
+    ros::NodeHandle node_handle;
+    node_handle.setParam("collect_pose", true);
+}
+
+void cleanUp(){
+    ros::NodeHandle node_handle;
+    std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;
+
+    node_handle.getParam("x_transform", x_transform);
+    node_handle.getParam("y_transform", y_transform);
+    node_handle.getParam("z_transform", z_transform);
+
+    node_handle.getParam("x_rotation", x_rotation);
+    node_handle.getParam("y_rotation", y_rotation);
+    node_handle.getParam("z_rotation", z_rotation);
+    node_handle.getParam("w_rotation", w_rotation);
+
+    x_transform.clear();
+    y_transform.clear();
+    z_transform.clear();
+    z_transform.clear();
+
+    x_rotation.clear();
+    y_rotation.clear();
+    z_rotation.clear();
+
+    node_handle.setParam("x_transform", x_transform);
+    node_handle.setParam("y_transform", y_transform);
+    node_handle.setParam("z_transform", z_transform);
+
+    node_handle.setParam("x_rotation", x_rotation);
+    node_handle.setParam("y_rotation", y_rotation);
+    node_handle.setParam("z_rotation", z_rotation);
+    node_handle.setParam("w_rotation", w_rotation);
+}
+
+void collectorCallback(const std_msgs::String::ConstPtr& msg){
+
+    ROS_INFO("Retrieved new pose-message...");
+
+    if(msg->data.compare("collect") == 0){
+        ROS_INFO("Collecting new pose for gripper...");
+        collectPose();
+        return;
+    }
+
+    if(msg->data.compare("clear") == 0){
+        ROS_INFO("Deleting saved poses...");
+        cleanUp();
+        return;
+    }
+}
+
+int main(int argc, char** argv) {
+
+    ros::init(argc, argv, "pose_collector");
+    ros::NodeHandle node_handle;
+
+    ros::Subscriber sub = node_handle.subscribe("collectPose", 1000, collectorCallback);
+    ros::spin();
+
+    return 0;
+}
\ No newline at end of file

src/teaching/pose_replayer.cpp

+//
+// Created by sebastian on 04.05.20.
+//
+#include <ros/ros.h>
+
+#include <std_msgs/String.h>
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit_msgs/CollisionObject.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+
+void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
+
+    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);
+
+    std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;
+
+    // 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 constraint planner node", rvt::WHITE, rvt::XLARGE);
+    visual_tools.trigger();
+
+    node_handle.getParam("x_transform", x_transform);
+    node_handle.getParam("y_transform", y_transform);
+    node_handle.getParam("z_transform", z_transform);
+
+    node_handle.getParam("x_rotation", x_rotation);
+    node_handle.getParam("y_rotation", y_rotation);
+    node_handle.getParam("z_rotation", z_rotation);
+    node_handle.getParam("w_rotation", w_rotation);
+
+    for (int i = 0; i < x_transform.size(); i++) {
+
+        move_group.setStartState(*move_group.getCurrentState());
+        geometry_msgs::Pose another_pose;
+        another_pose.orientation.x = x_rotation.at(i);
+        another_pose.orientation.y = y_rotation.at(i);
+        another_pose.orientation.z = z_rotation.at(i);
+        another_pose.orientation.w = w_rotation.at(i);
+        another_pose.position.x = x_transform.at(i);
+        another_pose.position.y = y_transform.at(i);
+        another_pose.position.z = z_transform.at(i);
+        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("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED");
+        move_group.move();
+    }
+}
+
+int main(int argc, char **argv) {
+
+    ros::init(argc, argv, "pose_replayer");
+    ros::NodeHandle node_handle;
+    ros::AsyncSpinner spinner(2);
+    spinner.start();
+
+    ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("replayPose", 1000, boost::bind(&replayCallback, _1, boost::ref(node_handle)));
+    ros::waitForShutdown();
+    return 0;
+}

src/teaching/pose_tf_listener.cpp

+//
+// Created by sebastian on 04.05.20.
+//
+#include <ros/ros.h>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Twist.h>
+
+void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t){
+
+    std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;
+
+    node_handle.getParam("x_transform", x_transform);
+    node_handle.getParam("y_transform", y_transform);
+    node_handle.getParam("z_transform", z_transform);
+
+    node_handle.getParam("x_rotation", x_rotation);
+    node_handle.getParam("y_rotation", y_rotation);
+    node_handle.getParam("z_rotation", z_rotation);
+    node_handle.getParam("w_rotation", w_rotation);
+
+    x_transform.push_back(t.transform.translation.x);
+    y_transform.push_back(t.transform.translation.y);
+    z_transform.push_back(t.transform.translation.z);
+
+    x_rotation.push_back(t.transform.rotation.x);
+    y_rotation.push_back(t.transform.rotation.y);
+    z_rotation.push_back(t.transform.rotation.z);
+    w_rotation.push_back(t.transform.rotation.w);
+
+    node_handle.setParam("x_transform", x_transform);
+    node_handle.setParam("y_transform", y_transform);
+    node_handle.setParam("z_transform", z_transform);
+
+    node_handle.setParam("x_rotation", x_rotation);
+    node_handle.setParam("y_rotation", y_rotation);
+    node_handle.setParam("z_rotation", z_rotation);
+    node_handle.setParam("w_rotation", w_rotation);
+}
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "pose_tf_listener");
+
+    ros::NodeHandle node_handle;
+
+    tf2_ros::Buffer tfBuffer;
+    tf2_ros::TransformListener tfListener(tfBuffer);
+
+    ros::Rate rate(10.0);
+
+    while (node_handle.ok()){
+
+        geometry_msgs::TransformStamped transformStamped;
+
+        try{
+            // world == (0, 0, 0, 0)
+            transformStamped = tfBuffer.lookupTransform("world", "panda_hand",
+                                                        ros::Time(0));
+
+            bool collect_pose = false;
+            node_handle.getParam("collect_pose", collect_pose);
+
+            if(collect_pose){
+                std::cout << "Collected pose: " << transformStamped << std::endl;
+                node_handle.setParam("collect_pose", false);
+                collect(node_handle, transformStamped);
+            }
+        }
+        catch (tf2::TransformException &ex) {
+            std::cout << "transform_error: " << ex.what() << std::endl;
+            ros::Duration(1.0).sleep();
+            continue;
+        }
+    }
+    return 0;
+};
\ No newline at end of file