diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..990936c24302e65fd41e59db2fbbe963f607a9c8 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.idea/ +cmake-build-debug/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..f33c65745ad5006cc6a15dcec5e6affa02054dd3 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sample_applications) + +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 + genmsg + joint_state_controller + joint_state_publisher + joint_trajectory_controller + robot_state_publisher + roscpp + std_msgs + 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(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 + 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) + +# 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}) \ No newline at end of file diff --git a/README.md b/README.md index 0d5976bbf6e4413234b80f95f8daa1479a4ea80b..2db6ee477bac9ff2cd46f1cf4193cd4277aed48f 100644 --- a/README.md +++ b/README.md @@ -1 +1,10 @@ -# Sample applications for ROS combined with (simulated) Franka Emika Panda robots \ No newline at end of file +### Sample applications for ROS combined with a (simulated) Franka Emika Panda robots + +Contains different examples for the planning and execution of robotic motions with MoveIt in ROS. + +##### 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` +- 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 diff --git a/launch/sample_constraint_simulation.launch b/launch/sample_constraint_simulation.launch new file mode 100644 index 0000000000000000000000000000000000000000..6b03481665d84c81c3e41862fc516e0e8da5ebd8 --- /dev/null +++ b/launch/sample_constraint_simulation.launch @@ -0,0 +1,63 @@ +<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> diff --git a/launch/sample_simple_simulation.launch b/launch/sample_simple_simulation.launch new file mode 100644 index 0000000000000000000000000000000000000000..a9359144cb50538e6a68f465946e555c26db5273 --- /dev/null +++ b/launch/sample_simple_simulation.launch @@ -0,0 +1,63 @@ +<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> diff --git a/launch/sample_timed_cartesian_simulation.launch b/launch/sample_timed_cartesian_simulation.launch new file mode 100644 index 0000000000000000000000000000000000000000..e0186208c98dc862c983e93a81fb8f9ab803268a --- /dev/null +++ b/launch/sample_timed_cartesian_simulation.launch @@ -0,0 +1,63 @@ +<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> diff --git a/package.xml b/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..610aabae5c5631e24ae10db34a7b81b5a0ccbcb7 --- /dev/null +++ b/package.xml @@ -0,0 +1,51 @@ +<?xml version="1.0"?> +<package format="2"> + <name>sample_applications</name> + <version>0.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> + + + <!-- 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> + + + <buildtool_depend>catkin</buildtool_depend> + + <depend>controller_manager</depend> + <depend>effort_controllers</depend> + <depend>gazebo_ros</depend> + <depend>gazebo_ros_control</depend> + <depend>joint_state_controller</depend> + <depend>joint_state_publisher</depend> + <depend>joint_trajectory_controller</depend> + <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>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> + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> + + </export> +</package> diff --git a/src/SampleConstraintPlanner.cpp b/src/SampleConstraintPlanner.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d49ca191aef019c1a7d0ca98afe88c247381dc01 --- /dev/null +++ b/src/SampleConstraintPlanner.cpp @@ -0,0 +1,115 @@ +#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/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> + +/** + * simple demo of constraint aware planning + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "CONSTRAINT 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 <<<<<<<<<<<<<<<<"); + + 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 + 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(); + + // 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, ", ")); + + // Define a collision object ROS message. + moveit_msgs::CollisionObject collision_object; + collision_object.header.frame_id = move_group.getPlanningFrame(); + + // The id of the object is used to identify it. + collision_object.id = "box1"; + + // Define a box to add to the world. + shape_msgs::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[0] = 0.4; + primitive.dimensions[1] = 0.1; + primitive.dimensions[2] = 0.4; + + // 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.y = -0.2; + box_pose.position.z = 1.0; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + collision_object.operation = collision_object.ADD; + + 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"); + planning_scene_interface.addCollisionObjects(collision_objects); + + // Show text in RViz of status + visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); + + // Now when we plan a trajectory it will avoid the obstacle + 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); + //move_group.setMaxVelocityScalingFactor(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"); + + visual_tools.prompt("Press 'next' to move the simulated robot."); + visual_tools.trigger(); + + // Move the simulated robot in gazebo + move_group.move(); + + ros::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/SampleSimpleMotion.cpp b/src/SampleSimpleMotion.cpp new file mode 100644 index 0000000000000000000000000000000000000000..97a46fc5f148455ccc222d92b78bf1abf28b44ce --- /dev/null +++ b/src/SampleSimpleMotion.cpp @@ -0,0 +1,75 @@ +#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/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> + +/** + * simple 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); + + // 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, "simple_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(); + + // 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"); + + visual_tools.prompt("Press 'next' to move the simulated robot."); + visual_tools.trigger(); + + // Move the simulated robot in gazebo + move_group.move(); + + ros::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/SampleTimedCartesianPlanner.cpp b/src/SampleTimedCartesianPlanner.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c28975bbb004d184de623b86e721962e671d8077 --- /dev/null +++ b/src/SampleTimedCartesianPlanner.cpp @@ -0,0 +1,118 @@ +#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 + */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "sample_timed_cartesian"); + 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"); + + ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); + moveit_msgs::DisplayTrajectory display_trajectory; + + moveit::planning_interface::MoveGroupInterface::Plan plan; + group.setStartStateToCurrentState(); + + std::vector<geometry_msgs::Pose> waypoints; + + // initial pose of the robot + geometry_msgs::Pose start_pose = group.getCurrentPose().pose; + + // waypoints of the target trajectory + geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose; + geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose; + geometry_msgs::Pose target_pose_3 = group.getCurrentPose().pose; + + target_pose_1.position.z = start_pose.position.z - 0.4; + target_pose_1.position.y = start_pose.position.y; + target_pose_1.position.x = start_pose.position.x + 0.5; + waypoints.push_back(target_pose_1); + + target_pose_2.position.z = 0.6; + target_pose_2.position.y = -0.6; + target_pose_2.position.x = 0; + waypoints.push_back(target_pose_2); + + target_pose_3.position.z = 0.6; + target_pose_3.position.y = -0.032; + 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 IterativeParabolicTimeParameterization object + 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; + } + } + + 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); + + group.execute(plan); + + ros::shutdown(); + return 0; +} \ No newline at end of file