diff --git a/CMakeLists.txt b/CMakeLists.txt index f33c65745ad5006cc6a15dcec5e6affa02054dd3..2b315d3ee71d0bb78402ba2a107db379b880afb0 100644 --- a/CMakeLists.txt +++ b/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}) \ No newline at end of file +target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES}) +target_link_libraries(MinimalSimpleMotion ${catkin_LIBRARIES}) diff --git a/README.md b/README.md index 2db6ee477bac9ff2cd46f1cf4193cd4277aed48f..002a369fd2b59a979537f3a1fa7f0deb9d09d075 100644 --- a/README.md +++ b/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` -- Execution of a velocity constraint cartesian trajectory: `roslaunch sample_applications simulation.launch` \ No newline at end of file + - 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 diff --git a/launch/sample_minimal_simulation.launch b/launch/sample_minimal_simulation.launch new file mode 100644 index 0000000000000000000000000000000000000000..f999a387c709c08ed7a468a3efb0141b327ab890 --- /dev/null +++ b/launch/sample_minimal_simulation.launch @@ -0,0 +1,57 @@ +<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> diff --git a/src/MinimalSimpleMotion.cpp b/src/MinimalSimpleMotion.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1665ccde94a5ef18a1d0e5914d5b3c0ffcd7ad2f --- /dev/null +++ b/src/MinimalSimpleMotion.cpp @@ -0,0 +1,57 @@ +#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; +}