diff --git a/CMakeLists.txt b/CMakeLists.txt index dfcaa53b07ca67374b0676942b0e7c79643caf8b..b9da816581a1293d9d81875cf6f89bb64e509282 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,15 +59,24 @@ include_directories(${catkin_INCLUDE_DIRS}) add_executable(robot_control_node src/robot_control_node.cpp) add_executable(box_publisher_node src/box_publisher.cpp) add_executable(robot_state_initializer_node src/robot_state_initializer.cpp) -add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp) +#add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp) +#add_executable(motion_planning_api_tutorial src/motion_planning_api_tutorial.cpp) +add_executable(constraint_planner src/constraint_planner.cpp) target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES}) target_link_libraries(box_publisher_node ${catkin_LIBRARIES}) target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES}) -target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +#target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +#target_link_libraries(motion_planning_api_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES}) # add custom controller as library add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp) # Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES}) + +#install(TARGETS move_group_interface_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +#install(TARGETS motion_planning_api_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +#install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) \ No newline at end of file diff --git a/launch/simulation.launch b/launch/simulation.launch index f2583abfa5cf6c0d2b59d151b9cae3dd44a05a39..a982c720e046458a10b05d96603481485084859e 100644 --- a/launch/simulation.launch +++ b/launch/simulation.launch @@ -27,7 +27,6 @@ <!-- 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" /> @@ -58,4 +57,14 @@ <!-- run custom node for automatic intialization --> <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" /> + <!--<node name="move_group_interface_tutorial" pkg="panda_simulation" type="move_group_interface_tutorial" respawn="false" output="screen"/>--> + + <node name="constraint_planner" pkg="panda_simulation" type="constraint_planner" respawn="false" output="screen"/> + + <!--<node name="motion_planning_api_tutorial" pkg="panda_simulation" type="motion_planning_api_tutorial" respawn="false" output="screen"> + <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> + <param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/> + <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/> + </node>--> + </launch> diff --git a/src/constraint_planner.cpp b/src/constraint_planner.cpp new file mode 100644 index 0000000000000000000000000000000000000000..95259b9685588c1f0f7ddc92f53d02f95c508044 --- /dev/null +++ b/src/constraint_planner.cpp @@ -0,0 +1,104 @@ +#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> + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "CONSTRAINT PLANNER"); + ros::NodeHandle node_handle; + ros::AsyncSpinner spinner(1); + spinner.start(); + + 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("tutorial", "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); + + 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