From 6f34affe90cb0017fd7b969d4b6d0ed1f0c194af Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Fri, 24 Mar 2023 15:48:56 -0500 Subject: [PATCH] Add examples --- CMakeLists.txt | 68 ++++ config/multi_constraint.rviz | 145 +++++++ config/panda.rviz | 396 +++++++++++++++++++ config/panda_planning_kinematics.yaml | 16 + launch/chained_screws.launch | 33 ++ launch/multiple_constraint_tester.launch | 5 + launch/panda_demo.launch | 35 ++ launch/unchained_screws.launch | 30 ++ package.xml | 30 ++ src/chained_screws_demo.cpp | 481 +++++++++++++++++++++++ src/multi_constraint_tester.cpp | 249 ++++++++++++ src/panda_demo.cpp | 429 ++++++++++++++++++++ src/unchained_screws_demo.cpp | 428 ++++++++++++++++++++ 13 files changed, 2345 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 config/multi_constraint.rviz create mode 100644 config/panda.rviz create mode 100644 config/panda_planning_kinematics.yaml create mode 100644 launch/chained_screws.launch create mode 100644 launch/multiple_constraint_tester.launch create mode 100644 launch/panda_demo.launch create mode 100644 launch/unchained_screws.launch create mode 100644 package.xml create mode 100644 src/chained_screws_demo.cpp create mode 100644 src/multi_constraint_tester.cpp create mode 100644 src/panda_demo.cpp create mode 100644 src/unchained_screws_demo.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3805073 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ap_planning_examples) + +add_compile_options(-std=c++17) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 17) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## 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 + ap_planning + moveit_visual_tools +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ap_planning_examples +# CATKIN_DEPENDS ap_planning +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +add_executable(panda_demo src/panda_demo.cpp) +target_link_libraries(panda_demo ${catkin_LIBRARIES}) + +add_executable(chained_screws_demo src/chained_screws_demo.cpp) +target_link_libraries(chained_screws_demo ${catkin_LIBRARIES}) + +add_executable(unchained_screws_demo src/unchained_screws_demo.cpp) +target_link_libraries(unchained_screws_demo ${catkin_LIBRARIES}) + +add_executable(multi_constraint_tester src/multi_constraint_tester.cpp) +target_link_libraries(multi_constraint_tester ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +install( + TARGETS chained_screws_demo multi_constraint_tester panda_demo unchained_screws_demo + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + diff --git a/config/multi_constraint.rviz b/config/multi_constraint.rviz new file mode 100644 index 0000000..63bd64d --- /dev/null +++ b/config/multi_constraint.rviz @@ -0,0 +1,145 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 548 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /multi_constraint_tester/update + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: false + Length: 0.5 + Name: Axes + Radius: 0.029999999329447746 + Reference Frame: + Show Trail: false + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /rviz_visual_tools + Name: MarkerArray + Namespaces: + Arrow: true + Axis: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.885328769683838 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.16111618280410767 + Y: 0.2344551682472229 + Z: -0.4010162055492401 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6853979825973511 + Target Frame: + Yaw: 0.6853980422019958 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 845 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002affc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002af000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ab0000003efc0100000002fb0000000800540069006d00650100000000000005ab000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000033a000002af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1451 + X: 1532 + Y: 308 diff --git a/config/panda.rviz b/config/panda.rviz new file mode 100644 index 0000000..05d82e5 --- /dev/null +++ b/config/panda.rviz @@ -0,0 +1,396 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /PlanningScene1/Scene Geometry1 + Splitter Ratio: 0.6836158037185669 + Tree Height: 618 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /rviz_visual_tools + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Attached Body Color: 150; 50; 150 + Class: moveit_rviz_plugin/RobotState + Collision Enabled: false + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotState + Robot Alpha: 1 + Robot Description: robot_description + Robot State Topic: display_robot_state + Show All Links: true + Show Highlights: true + Value: false + Visual Enabled: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.01s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Use Sim Time: false + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /interactive_screw/update + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.5857731103897095 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: 0.014769500121474266 + Y: 0.002564211841672659 + Z: 0.29984965920448303 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5147964954376221 + Target Frame: panda_link0 + Yaw: 0.44177690148353577 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000100000000000001640000039efc020000000afb000000100044006900730070006c006100790073010000003d000002fb000000c900fffffffb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072010000033e000000450000004100fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000001880000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c00730047007500690100000389000000520000004100fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000003720000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1244 + X: 2366 + Y: 279 diff --git a/config/panda_planning_kinematics.yaml b/config/panda_planning_kinematics.yaml new file mode 100644 index 0000000..6edbc69 --- /dev/null +++ b/config/panda_planning_kinematics.yaml @@ -0,0 +1,16 @@ +panda_arm: + # # Uncomment this block to use TracIK + # kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + # kinematics_solver_search_resolution: 0.005 + # kinematics_solver_timeout: 0.05 + + # Uncomment this block to use BioIK + kinematics_solver: bio_ik/BioIKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 # this timeout parameter is actually ignored by servo + kinematics_solver_attempts: 1.0 + + # # Uncomment this block to use KDL IK solver + # kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + # kinematics_solver_search_resolution: 0.005 + # kinematics_solver_timeout: 0.05 diff --git a/launch/chained_screws.launch b/launch/chained_screws.launch new file mode 100644 index 0000000..47d6303 --- /dev/null +++ b/launch/chained_screws.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/multiple_constraint_tester.launch b/launch/multiple_constraint_tester.launch new file mode 100644 index 0000000..b07d5fa --- /dev/null +++ b/launch/multiple_constraint_tester.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/launch/panda_demo.launch b/launch/panda_demo.launch new file mode 100644 index 0000000..e018712 --- /dev/null +++ b/launch/panda_demo.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/unchained_screws.launch b/launch/unchained_screws.launch new file mode 100644 index 0000000..87f805a --- /dev/null +++ b/launch/unchained_screws.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..650e906 --- /dev/null +++ b/package.xml @@ -0,0 +1,30 @@ + + + ap_planning_examples + 0.0.0 + Examples for planning with screw-based affordance primitives + + Adam Pettinger + + + + + Proprietary, see attached + + Adam Pettinger + + catkin + ap_planning + moveit_visual_tools + + bio_ik + panda_moveit_config + rviz + trac_ik + + + + + + + diff --git a/src/chained_screws_demo.cpp b/src/chained_screws_demo.cpp new file mode 100644 index 0000000..b682c4b --- /dev/null +++ b/src/chained_screws_demo.cpp @@ -0,0 +1,481 @@ +#include +#include + +#include +#include +#include +#include +#include + +void show_multi_screw(const ap_planning::APPlanningRequest &req, + moveit_visual_tools::MoveItVisualTools &visual_tools) { + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); + + // Get constraint + auto constraint = req.toConstraint(); + + visual_tools.publishAxis(constraint->referenceFrame()); + + // Plot each screw + const auto viz_screws = constraint->getVisualScrews(); + for (const auto &screw : viz_screws) { + // Create points for plotting + Eigen::Vector3d origin, axis; + tf2::fromMsg(screw.origin, origin); + tf2::fromMsg(screw.axis, axis); + Eigen::Vector3d end = origin + 0.2 * axis.normalized(); + geometry_msgs::Point end_point = tf2::toMsg(end); + geometry_msgs::Point origin_point = tf2::toMsg(origin); + + // Plot + auto color = screw.is_pure_translation ? rviz_visual_tools::PURPLE + : rviz_visual_tools::ORANGE; + visual_tools.publishArrow(origin_point, end_point, color, + rviz_visual_tools::LARGE); + } + visual_tools.trigger(); +} + +void show_trajectory(const trajectory_msgs::JointTrajectory &traj, + moveit_visual_tools::MoveItVisualTools &visual_tools) { + moveit_msgs::DisplayTrajectory joint_traj; + joint_traj.model_id = "panda"; + joint_traj.trajectory.push_back(moveit_msgs::RobotTrajectory()); + joint_traj.trajectory.at(0).joint_trajectory = traj; + + moveit_msgs::RobotState start_msg; + start_msg.joint_state.name = traj.joint_names; + auto first_waypoint = traj.points.at(0); + start_msg.joint_state.position = first_waypoint.positions; + joint_traj.trajectory_start = start_msg; + + joint_traj.trajectory.at(0).joint_trajectory.header.frame_id = "panda_link0"; + + int time = 0; + + for (auto &wp : joint_traj.trajectory.at(0).joint_trajectory.points) { + wp.time_from_start.sec = time; + ++time; + } + + visual_tools.publishTrajectoryPath(joint_traj); +} + +std::queue +get_planning_queue(const std::vector &default_joint_state) { + std::queue planning_queue; + ap_planning::APPlanningRequest single_request; + ap_planning::ScrewSegment screw_0, screw_1, screw_2, screw_3; + screw_0.screw_msg.header.frame_id = "panda_link0"; + screw_1.screw_msg.header.frame_id = "panda_link0"; + screw_2.screw_msg.header.frame_id = "panda_link0"; + + single_request.ee_frame_name = "panda_link8"; + single_request.planning_time = 10; + + // For now, all requests start at same point + single_request.start_pose.pose.position.x = 0.5; + single_request.start_pose.pose.position.z = 0.4; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + + // Add some test cases + screw_0.start_theta = -0.1; + screw_0.end_theta = 0.1; + screw_0.screw_msg.is_pure_translation = true; + screw_0.screw_msg.origin = single_request.start_pose.pose.position; + screw_0.screw_msg.axis.z = 1; + + screw_1.start_theta = 0.0; + screw_1.end_theta = 0.2; + screw_1.screw_msg.axis.z = 1; + screw_1.screw_msg.axis.y = 1; + screw_1.screw_msg.is_pure_translation = true; + screw_1.screw_msg.origin = single_request.start_pose.pose.position; + + screw_2.start_theta = 0.0; + screw_2.end_theta = 0.5 * M_PI; + screw_2.screw_msg.is_pure_translation = false; + screw_2.screw_msg.axis.z = -1; + screw_2.screw_msg.origin = single_request.start_pose.pose.position; + screw_2.screw_msg.origin.y += 0.1; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + single_request.screw_path.push_back(screw_2); + planning_queue.push(single_request); + + // Send another one + single_request.screw_path.clear(); + screw_0 = ap_planning::ScrewSegment(); + screw_0.screw_msg.header.frame_id = "panda_link0"; + screw_0.start_theta = 0; + screw_0.end_theta = 0.25 * M_PI; + screw_0.screw_msg.origin = single_request.start_pose.pose.position; + screw_0.screw_msg.axis.z = 1; + screw_0.screw_msg.is_pure_translation = false; + screw_0.screw_msg.pitch = 0.2; + + screw_1 = ap_planning::ScrewSegment(); + screw_1.screw_msg.header.frame_id = "panda_link0"; + screw_1.start_theta = 0; + screw_1.end_theta = 0.2; + screw_1.screw_msg.origin = single_request.start_pose.pose.position; + screw_1.screw_msg.axis.x = -1; + screw_1.screw_msg.is_pure_translation = true; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + planning_queue.push(single_request); + + // Fun with pitches + single_request.screw_path.clear(); + screw_0 = ap_planning::ScrewSegment(); + screw_0.screw_msg.header.frame_id = "panda_link0"; + screw_0.start_theta = -0.25 * M_PI; + screw_0.end_theta = 0.0; + screw_0.screw_msg.origin = geometry_msgs::Point(); + screw_0.screw_msg.origin.z -= 0.3; + screw_0.screw_msg.axis.z = 1; + screw_0.screw_msg.is_pure_translation = false; + screw_0.screw_msg.pitch = 0.0; + + screw_1 = ap_planning::ScrewSegment(); + screw_1.screw_msg.header.frame_id = "panda_link0"; + screw_1.start_theta = -0.25 * M_PI; + screw_1.end_theta = 0.0; + screw_1.screw_msg.origin = geometry_msgs::Point(); + screw_1.screw_msg.origin.z -= 0.3; + screw_1.screw_msg.axis.z = 1; + screw_1.screw_msg.is_pure_translation = false; + screw_1.screw_msg.pitch = 0.3; + + screw_2 = ap_planning::ScrewSegment(); + screw_2.screw_msg.header.frame_id = "panda_link0"; + screw_2.start_theta = 0.0; + screw_2.end_theta = 0.25 * M_PI; + screw_2.screw_msg.origin = geometry_msgs::Point(); + screw_2.screw_msg.origin.z -= 0.3; + screw_2.screw_msg.axis.z = 1; + screw_2.screw_msg.is_pure_translation = false; + screw_2.screw_msg.pitch = -0.3; + + screw_3 = ap_planning::ScrewSegment(); + screw_3.screw_msg.header.frame_id = "panda_link0"; + screw_3.start_theta = 0.0; + screw_3.end_theta = 0.25 * M_PI; + screw_3.screw_msg.origin = geometry_msgs::Point(); + screw_3.screw_msg.origin.z -= 0.3; + screw_3.screw_msg.axis.z = 1; + screw_3.screw_msg.is_pure_translation = false; + screw_3.screw_msg.pitch = 0.0; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + single_request.screw_path.push_back(screw_2); + single_request.screw_path.push_back(screw_3); + planning_queue.push(single_request); + + // Mess with the starting pose + single_request.screw_path.clear(); + single_request.start_pose.pose.position.x = 0.525; + single_request.start_pose.pose.position.y = 0.010; + single_request.start_pose.pose.position.z = 0.503; + single_request.start_pose.pose.orientation.x = 0.693; + single_request.start_pose.pose.orientation.y = 0.016; + single_request.start_pose.pose.orientation.z = 0.721; + single_request.start_pose.pose.orientation.w = -0.017; + + screw_0 = ap_planning::ScrewSegment(); + screw_0.screw_msg.header.frame_id = "panda_link0"; + screw_0.start_theta = 0; + screw_0.end_theta = 0.25; + screw_0.screw_msg.origin = single_request.start_pose.pose.position; + screw_0.screw_msg.axis.x = -1; + screw_0.screw_msg.is_pure_translation = true; + + screw_1 = ap_planning::ScrewSegment(); + screw_1.screw_msg.header.frame_id = "panda_link0"; + screw_1.start_theta = 0; + screw_1.end_theta = 0.2; + screw_1.screw_msg.origin = single_request.start_pose.pose.position; + screw_1.screw_msg.axis.z = 1; + screw_1.screw_msg.is_pure_translation = true; + + screw_2.screw_msg.header.frame_id = "panda_link0"; + screw_2.start_theta = 0; + screw_2.end_theta = 0.25; + screw_2.screw_msg.origin = single_request.start_pose.pose.position; + screw_2.screw_msg.axis.x = 1; + screw_2.screw_msg.is_pure_translation = true; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + single_request.screw_path.push_back(screw_2); + planning_queue.push(single_request); + + return planning_queue; +} + +std::queue get_collision_objects() { + // Set up stuff same across all test cases + std::queue output; + moveit_msgs::CollisionObject collision_object; + collision_object.header.frame_id = "panda_link0"; + collision_object.operation = collision_object.ADD; + + geometry_msgs::Pose box_pose; + box_pose.orientation.w = 1.0; + shape_msgs::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + + // Screw one + collision_object.id = "screw1_box1"; + primitive.dimensions[primitive.BOX_X] = 0.1; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 0.4; + box_pose.position.x = 0.25; + box_pose.position.y = 0.0; + box_pose.position.z = 0.25; + + // Add screw one object + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + // Repeat + collision_object.id = "screw2_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 0.1; + box_pose.position.x = 0.0; + box_pose.position.y = 0.0; + box_pose.position.z = 0.81; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + collision_object.id = "screw3_box1"; + primitive.dimensions[primitive.BOX_X] = 0.1; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 1.5; + box_pose.position.x = -0.3; + box_pose.position.y = 0.0; + box_pose.position.z = 0.75; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + + primitive.dimensions[primitive.BOX_X] = 0.75; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 0.2; + box_pose.position.x = 0.75; + box_pose.position.y = 0.0; + box_pose.position.z = 0.1; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + + primitive.dimensions[primitive.BOX_X] = 0.75; + primitive.dimensions[primitive.BOX_Y] = 0.7; + primitive.dimensions[primitive.BOX_Z] = 0.1; + box_pose.position.x = 0.5 * 0.75 - 0.3; + box_pose.position.y = 0.5 * 0.7 + 0.05; + box_pose.position.z = 0.7; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + collision_object.id = "screw4_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 1.5; + box_pose.position.x = 0.0; + box_pose.position.y = -0.2; + box_pose.position.z = 0.75; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 0.4; + box_pose.position.x = 0.0; + box_pose.position.y = 0.3; + box_pose.position.z = 0.2; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + return output; +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "ap_planning"); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(2); + spinner.start(); + + robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); + const moveit::core::RobotModelPtr &kinematic_model = + robot_model_loader.getModel(); + + moveit::core::RobotStatePtr kinematic_state( + new moveit::core::RobotState(kinematic_model)); + std::vector default_joint_state{0, -0.785, 0, -2.356, + 0, 1.571, 0.785}; + kinematic_state->setJointGroupPositions("panda_arm", default_joint_state); + + int num_sample, num_sps; + nh.param(ros::this_node::getName() + "/num_sampling", num_sample, 2); + nh.param(ros::this_node::getName() + "/num_sps", num_sps, 2); + + bool show_trajectories; + nh.param(ros::this_node::getName() + "/show_trajectories", + show_trajectories, true); + + bool use_obstacles; + nh.param(ros::this_node::getName() + "/add_collision_objects", + use_obstacles, true); + + // Add collision objects + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + auto collision_objects = get_collision_objects(); + + ros::Duration(2.0).sleep(); + + moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); + visual_tools.deleteAllMarkers(); + visual_tools.loadRemoteControl(); + visual_tools.setRobotStateTopic("/display_robot_state"); + visual_tools.trigger(); + + auto planning_queue = get_planning_queue(default_joint_state); + + ap_planning::DSSPlanner ap_planner("panda_arm"); + ap_planning::SequentialStepPlanner sequential_step_planner("panda_arm"); + if (!sequential_step_planner.initialize()) { + ROS_ERROR_STREAM("Init failed"); + return EXIT_FAILURE; + } + + std::stringstream ss_dssp, ss_sps; + ss_dssp << "Start of output\n"; + ss_sps << "Start of output\n"; + size_t sample = 0; + ap_planning::APPlanningResponse last_plan; + bool collision_obj_exists = false; + + // Plan each screw request + while (planning_queue.size() > 0 && ros::ok()) { + sample++; + auto req = planning_queue.front(); + planning_queue.pop(); + + if (show_trajectories) { + visual_tools.prompt( + "Press 'next' in the RvizVisualToolsGui window to plan next screw"); + } + + if (use_obstacles && !collision_objects.empty()) { + if (collision_obj_exists) { + std::vector remove_objs; + remove_objs.push_back(collision_objects.front().id); + planning_scene_interface.removeCollisionObjects(remove_objs); + collision_objects.pop(); + } + std::vector collision_obj_vec; + collision_obj_vec.push_back(collision_objects.front()); + planning_scene_interface.addCollisionObjects(collision_obj_vec); + collision_obj_exists = true; + } + + show_multi_screw(req, visual_tools); + + for (size_t i = 0; i < num_sample; ++i) { + std::cout << "Starting i = " << i << "\n"; + ap_planning::APPlanningResponse result; + auto start = std::chrono::high_resolution_clock::now(); + ap_planning::Result success = ap_planner.plan(req, result); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + if (success == ap_planning::SUCCESS) { + std::cout << "\n\n\nDSS planning: Success!!\n\n"; + std::cout << "Trajectory is: " << result.percentage_complete * 100 + << "% complete, and has length: " << result.path_length + << "\n"; + + if (show_trajectories) { + show_trajectory(result.joint_trajectory, visual_tools); + } + last_plan = result; + } else { + std::cout << "\n\n\nDSS planning: Fail (" << ap_planning::toStr(success) + << ")\n\n"; + } + + ss_dssp << sample << ", DSS, " << ap_planning::toStr(success) << ", " + << result.percentage_complete * 100 << ", " << duration.count() + << ", " << result.path_length << ",\n"; + } + + // Now move to sps planner + if (show_trajectories) { + visual_tools.prompt( + "Press 'next' in the RvizVisualToolsGui window to plan again using " + "sps planner"); + } + + // Try planning + for (size_t i = 0; i < num_sps; ++i) { + ap_planning::APPlanningResponse sps_output; + auto start = std::chrono::high_resolution_clock::now(); + auto sps_res = sequential_step_planner.plan(req, sps_output); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + if (sps_res == ap_planning::SUCCESS) { + std::cout << "\n\n\nSPS planning: Success!!\n\n"; + std::cout << "Trajectory is: " << sps_output.percentage_complete * 100 + << "% complete, and has length: " << sps_output.path_length + << "\n"; + if (show_trajectories) { + show_trajectory(sps_output.joint_trajectory, visual_tools); + } + last_plan = sps_output; + } else { + std::cout << "\n\n\nSPS planning: Fail (" << ap_planning::toStr(sps_res) + << ")\n\n"; + std::cout << "Trajectory is: " << sps_output.percentage_complete * 100 + << "% complete, and has length: " << sps_output.path_length + << "\n"; + } + ss_sps << sample << ", SPS, " << ap_planning::toStr(sps_res) << ", " + << sps_output.percentage_complete * 100 << ", " << duration.count() + << ",\n"; + } + } + + if (last_plan.joint_trajectory.points.size() > 1) { + show_trajectory(last_plan.joint_trajectory, visual_tools); + } + + ss_dssp << "End output\n"; + ss_sps << "End output\n"; + std::cout << ss_dssp.str(); + std::cout << ss_sps.str(); + + ros::shutdown(); + return 0; +} diff --git a/src/multi_constraint_tester.cpp b/src/multi_constraint_tester.cpp new file mode 100644 index 0000000..3c31890 --- /dev/null +++ b/src/multi_constraint_tester.cpp @@ -0,0 +1,249 @@ +#include +#include +#include +#include +#include +#include + +using namespace visualization_msgs; + +std::shared_ptr server; +std::shared_ptr robot_pose; +std::shared_ptr constraints; +std::shared_ptr grasp_location; + +std::mutex mutex; + +visualization_msgs::InteractiveMarkerControl & +makeFrameControl(visualization_msgs::InteractiveMarker &msg, + const geometry_msgs::Pose &pose) { + visualization_msgs::InteractiveMarkerControl control; + control.always_visible = true; + + // Make visual marker x-axis + visualization_msgs::Marker marker; + marker.type = visualization_msgs::Marker::ARROW; + marker.scale.x = msg.scale * 0.75; + marker.scale.y = msg.scale * 0.05; + marker.scale.z = msg.scale * 0.05; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + marker.pose = pose; + control.markers.push_back(marker); + + // Make visual marker y-axis + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + Eigen::Quaterniond rotation(0.5 * sqrt(2), 0, 0, + 0.5 * sqrt(2)); // w, x, y, z + Eigen::Quaterniond eig_quat; + tf2::fromMsg(pose.orientation, eig_quat); + auto axis_orientation = eig_quat * rotation; + marker.pose.orientation = tf2::toMsg(axis_orientation); + control.markers.push_back(marker); + + // Make visual marker z-axis + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.color.a = 1.0; + rotation = + Eigen::Quaterniond(0.5 * sqrt(2), 0, -0.5 * sqrt(2), 0); // w, x, y, z + axis_orientation = eig_quat * rotation; + marker.pose.orientation = tf2::toMsg(axis_orientation); + control.markers.push_back(marker); + + msg.controls.push_back(control); + + return msg.controls.back(); +} + +void processMovingFeedback(const InteractiveMarkerFeedbackConstPtr &feedback) { + const std::lock_guard lock(mutex); + *robot_pose = feedback->pose; + server->applyChanges(); +} + +void makeMovingFrameMarker() { + visualization_msgs::InteractiveMarker moving_frame; + moving_frame.header.frame_id = "map"; + moving_frame.scale = 0.25; + + moving_frame.name = "moving_frame"; + moving_frame.description = "Moving Frame"; + + moving_frame.pose.position.x = 1.0; + moving_frame.pose.position.y = 0.5; + + // insert the arrow + geometry_msgs::Pose start_pose; + start_pose.orientation.w = 1.0; + makeFrameControl(moving_frame, start_pose); + moving_frame.controls.at(0).interaction_mode = + visualization_msgs::InteractiveMarkerControl::NONE; + + visualization_msgs::InteractiveMarkerControl control; + + control.orientation.x = 0.5 * sqrt(2); + control.orientation.y = 0; + control.orientation.z = 0; + control.orientation.w = 0.5 * sqrt(2); + control.name = "rotate_x"; + control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + moving_frame.controls.push_back(control); + control.name = "move_x"; + control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + moving_frame.controls.push_back(control); + + control.orientation.x = 0; + control.orientation.y = 0.5 * sqrt(2); + control.orientation.z = 0; + control.orientation.w = 0.5 * sqrt(2); + control.name = "rotate_z"; + control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + moving_frame.controls.push_back(control); + control.name = "move_z"; + control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + moving_frame.controls.push_back(control); + + control.orientation.x = 0; + control.orientation.y = 0; + control.orientation.z = 0.5 * sqrt(2); + control.orientation.w = 0.5 * sqrt(2); + control.name = "rotate_y"; + control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + moving_frame.controls.push_back(control); + control.name = "move_y"; + control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + moving_frame.controls.push_back(control); + + server->insert(moving_frame); + server->setCallback(moving_frame.name, &processMovingFeedback); +} + +void timerFn(const ros::TimerEvent &) { + const std::lock_guard lock(mutex); + + // Get the robot pose right now + Eigen::Isometry3d tf_m_to_q; + tf2::fromMsg(*robot_pose, tf_m_to_q); + + // Set up the output + affordance_primitives::ScrewConstraintSolution sol; + + // Call the constraint function + auto res = constraints->constraintFn(tf_m_to_q, sol); + if (!res) { + ROS_ERROR_STREAM("Failed Constraints Fn"); + return; + } + + // Print the solution + std::stringstream ss; + ss << "Solved phi: ["; + for (const auto &val : sol.solved_phi) { + ss << val << ", "; + } + ss << "]"; + ROS_WARN_STREAM(ss.str()); + ROS_WARN_STREAM("Solved error: " << sol.error); + + // Get the solved pose + Eigen::Isometry3d solved_closest = constraints->getPose(sol.solved_phi); + + // Display the closest pose + visualization_msgs::InteractiveMarker waypoints; + waypoints.header.frame_id = "map"; + waypoints.scale = 0.5; + waypoints.name = "waypoints"; + makeFrameControl(waypoints, tf2::toMsg(solved_closest)); + waypoints.controls.back().interaction_mode = + visualization_msgs::InteractiveMarkerControl::NONE; + + server->erase("waypoints"); + server->insert(waypoints); + server->applyChanges(); +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "multi_constraint_tester"); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(0); + spinner.start(); + + server = std::make_shared( + "multi_constraint_tester", "map", false); + + robot_pose = std::make_shared(); + + // Make the constraints + constraints = std::make_shared(); + + // Add some screw axes + affordance_primitive_msgs::ScrewStamped axis_msg; + axis_msg.header.frame_id = "map"; + axis_msg.is_pure_translation = true; + axis_msg.axis.x = 1; + constraints->addScrewAxis(axis_msg, 0, 1); + + axis_msg.axis.x = 0; + axis_msg.axis.y = 1; + constraints->addScrewAxis(axis_msg, 0, 1); + + axis_msg.origin.y = 0.5; + axis_msg.axis.x = 0; + axis_msg.axis.y = 0; + axis_msg.axis.z = 1; + axis_msg.is_pure_translation = false; + axis_msg.pitch = 0; + constraints->addScrewAxis(axis_msg, 0, 0.5 * M_PI); + + // Add the frame information + geometry_msgs::Transform grasp; + grasp.rotation.w = 1; + grasp_location = std::make_shared(grasp); + constraints->setReferenceFrame(tf2::transformToEigen(*grasp_location)); + + // Visualize the axes, grasp, etc + rviz_visual_tools::RvizVisualTools rvt("map"); + ros::Duration(1.5).sleep(); + rvt.publishAxis(tf2::transformToEigen(grasp)); + + std::vector viz_screws = + constraints->getVisualScrews(); + for (const auto &screw : viz_screws) { + // Create points for plotting + Eigen::Vector3d origin, axis; + tf2::fromMsg(screw.origin, origin); + tf2::fromMsg(screw.axis, axis); + Eigen::Vector3d end = origin + 0.2 * axis.normalized(); + geometry_msgs::Point end_point = tf2::toMsg(end); + geometry_msgs::Point origin_point = tf2::toMsg(origin); + + // Plot + auto color = screw.is_pure_translation ? rviz_visual_tools::PURPLE + : rviz_visual_tools::ORANGE; + rvt.publishArrow(origin_point, end_point, color, rviz_visual_tools::LARGE); + } + rvt.trigger(); + + makeMovingFrameMarker(); + + server->applyChanges(); + + ros::Timer timer = nh.createTimer(ros::Duration(3), timerFn); + + ros::waitForShutdown(); + server.reset(); + return 0; +} diff --git a/src/panda_demo.cpp b/src/panda_demo.cpp new file mode 100644 index 0000000..2bcfd91 --- /dev/null +++ b/src/panda_demo.cpp @@ -0,0 +1,429 @@ +#include +#include + +#include +#include +#include +#include +#include + +void show_screw(const affordance_primitives::ScrewStamped &screw_msg, + moveit_visual_tools::MoveItVisualTools &visual_tools) { + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); + Eigen::Vector3d axis, origin; + tf2::fromMsg(screw_msg.axis, axis); + tf2::fromMsg(screw_msg.origin, origin); + + Eigen::Vector3d end_point = origin + 0.2 * axis.normalized(); + geometry_msgs::Point end = tf2::toMsg(end_point); + + visual_tools.publishArrow(screw_msg.origin, end, rviz_visual_tools::ORANGE, + rviz_visual_tools::LARGE); + visual_tools.trigger(); +} + +void show_trajectory(const trajectory_msgs::JointTrajectory &traj, + moveit_visual_tools::MoveItVisualTools &visual_tools) { + moveit_msgs::DisplayTrajectory joint_traj; + joint_traj.model_id = "panda"; + joint_traj.trajectory.push_back(moveit_msgs::RobotTrajectory()); + joint_traj.trajectory.at(0).joint_trajectory = traj; + + moveit_msgs::RobotState start_msg; + start_msg.joint_state.name = traj.joint_names; + auto first_waypoint = traj.points.at(0); + start_msg.joint_state.position = first_waypoint.positions; + joint_traj.trajectory_start = start_msg; + + joint_traj.trajectory.at(0).joint_trajectory.header.frame_id = "panda_link0"; + + int time = 0; + + for (auto &wp : joint_traj.trajectory.at(0).joint_trajectory.points) { + wp.time_from_start.sec = time; + ++time; + } + + visual_tools.publishTrajectoryPath(joint_traj); +} + +std::queue get_collision_objects() { + // Set up stuff same across all test cases + std::queue output; + moveit_msgs::CollisionObject collision_object; + collision_object.header.frame_id = "panda_link0"; + collision_object.operation = collision_object.ADD; + + geometry_msgs::Pose box_pose; + box_pose.orientation.w = 1.0; + shape_msgs::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + + // Screw one + collision_object.id = "screw1_box1"; + primitive.dimensions[primitive.BOX_X] = 0.1; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 0.4; + box_pose.position.x = 0.25; + box_pose.position.y = 0.0; + box_pose.position.z = 0.25; + + // Add screw one object + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + // Repeat + collision_object.id = "screw2_box1"; + primitive.dimensions[primitive.BOX_X] = 0.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 0.6; + box_pose.position.x = -0.1; + box_pose.position.y = -0.25; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + collision_object.id = "screw3_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 0.1; + box_pose.position.x = 0.5; + box_pose.position.y = 0.0; + box_pose.position.z = 0.75; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + collision_object.id = "screw4_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 1.5; + box_pose.position.x = 0.5; + box_pose.position.y = -0.2; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 1.5; + box_pose.position.x = 0.5; + box_pose.position.y = 0.2; + box_pose.position.z = 0.25; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + collision_object.id = "screw5_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 0.1; + box_pose.position.x = 0.5; + box_pose.position.y = 0.0; + box_pose.position.z = 0.65; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + collision_object.id = "screw6_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 0.1; + box_pose.position.x = 0.5; + box_pose.position.y = 0.0; + box_pose.position.z = 0.65; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + collision_object.id = "screw7_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 1.5; + box_pose.position.x = 0.5; + box_pose.position.y = -0.2; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 1.5; + box_pose.position.x = 0.5; + box_pose.position.y = 0.2; + box_pose.position.z = 0.25; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + return output; +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "ap_planning"); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(2); + spinner.start(); + + robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); + const moveit::core::RobotModelPtr &kinematic_model = + robot_model_loader.getModel(); + + moveit::core::RobotStatePtr kinematic_state( + new moveit::core::RobotState(kinematic_model)); + std::vector default_joint_state{0, -0.785, 0, -2.356, + 0, 1.571, 0.785}; + kinematic_state->setJointGroupPositions("panda_arm", default_joint_state); + + int num_sample, num_sps; + nh.param(ros::this_node::getName() + "/num_sampling", num_sample, 2); + nh.param(ros::this_node::getName() + "/num_sps", num_sps, 2); + + bool show_trajectories; + nh.param(ros::this_node::getName() + "/show_trajectories", + show_trajectories, true); + + bool use_obstacles; + nh.param(ros::this_node::getName() + "/add_collision_objects", + use_obstacles, true); + + // Set planner type + std::string planner_name; + nh.param(ros::this_node::getName() + "/planner", planner_name, + "prm"); + std::transform(planner_name.begin(), planner_name.end(), planner_name.begin(), + ::tolower); + + ap_planning::PlannerType planner_type; + if (planner_name == "prm") { + planner_type = ap_planning::PlannerType::PRM; + } else if (planner_name == "prmstar") { + planner_type = ap_planning::PlannerType::PRMstar; + } else if (planner_name == "rrt") { + planner_type = ap_planning::PlannerType::RRT; + } else if (planner_name == "rrtconnect") { + planner_type = ap_planning::PlannerType::RRTconnect; + } else { + ROS_WARN_STREAM("Unknown planner type: '" << planner_name + << "', using PRM"); + planner_type = ap_planning::PlannerType::PRM; + } + + // Add collision objects + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + auto collision_objects = get_collision_objects(); + + ros::Duration(2.0).sleep(); + + moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); + visual_tools.deleteAllMarkers(); + visual_tools.loadRemoteControl(); + visual_tools.setRobotStateTopic("/display_robot_state"); + visual_tools.trigger(); + + std::queue planning_queue; + ap_planning::APPlanningRequest single_request; + ap_planning::ScrewSegment single_screw; + single_screw.screw_msg.header.frame_id = "panda_link0"; + single_request.screw_path.push_back(single_screw); + single_request.ee_frame_name = "panda_link8"; + single_request.planning_time = 10; + single_request.planner = planner_type; + + // For now, all requests start at same point + single_request.start_pose.pose.position.x = 0.5; + single_request.start_pose.pose.position.z = 0.3; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + + // Add some test cases + single_request.screw_path.at(0).start_theta = 0.0; + single_request.screw_path.at(0).end_theta = 0.25 * M_PI; + single_request.screw_path.at(0).screw_msg.origin = + single_request.start_pose.pose.position; + single_request.screw_path.at(0).screw_msg.axis.x = 1; + planning_queue.push(single_request); + + // This time, send a joint configuration + single_request.start_joint_state = default_joint_state; + planning_queue.push(single_request); + + single_request.start_joint_state.clear(); + single_request.screw_path.at(0).start_theta = 0.0; + single_request.screw_path.at(0).end_theta = 0.5 * M_PI; + single_request.screw_path.at(0).screw_msg.axis.x = -1; + planning_queue.push(single_request); + + single_request.screw_path.at(0).start_theta = 0.0; + single_request.screw_path.at(0).end_theta = 0.25 * M_PI; + single_request.screw_path.at(0).screw_msg.axis.x = 0; + single_request.screw_path.at(0).screw_msg.axis.z = 1; + planning_queue.push(single_request); + + single_request.screw_path.at(0).screw_msg.origin.x -= 0.1; + single_request.screw_path.at(0).screw_msg.pitch = 0.1; + planning_queue.push(single_request); + + single_request.screw_path.at(0).screw_msg.origin = geometry_msgs::Point(); + single_request.screw_path.at(0).screw_msg.origin.z = -0.25; + planning_queue.push(single_request); + + single_request.screw_path.at(0).start_theta = 0.0; + single_request.screw_path.at(0).end_theta = 0.75; // meters + single_request.screw_path.at(0).screw_msg.origin = + single_request.start_pose.pose.position; + single_request.screw_path.at(0).screw_msg.axis.x = -1; + single_request.screw_path.at(0).screw_msg.axis.z = 1; + single_request.screw_path.at(0).screw_msg.is_pure_translation = true; + planning_queue.push(single_request); + + // This is the moveit ompl_constrained_planning tutorial line + if (!use_obstacles) { + single_request.start_joint_state = default_joint_state; + single_request.screw_path.at(0).end_theta = 0.3; + single_request.screw_path.at(0).screw_msg.origin.x = 0.307; + single_request.screw_path.at(0).screw_msg.origin.y = 0; + single_request.screw_path.at(0).screw_msg.origin.z = 0.59; + single_request.screw_path.at(0).screw_msg.axis.x = 0; + single_request.screw_path.at(0).screw_msg.axis.y = 1; + single_request.screw_path.at(0).screw_msg.axis.z = -1; + planning_queue.push(single_request); + } + + ap_planning::DSSPlanner ap_planner("panda_arm"); + ap_planning::SequentialStepPlanner sequential_step_planner("panda_arm"); + if (!sequential_step_planner.initialize()) { + ROS_ERROR_STREAM("Init failed"); + return EXIT_FAILURE; + } + + std::stringstream ss_dssp, ss_sps; + ss_dssp << "Start of output\n"; + ss_sps << "Start of output\n"; + size_t sample = 0; + ap_planning::APPlanningResponse last_plan; + bool collision_obj_exists = false; + + // Plan each screw request + while (planning_queue.size() > 0 && ros::ok()) { + sample++; + auto req = planning_queue.front(); + planning_queue.pop(); + + if (show_trajectories) { + visual_tools.prompt( + "Press 'next' in the RvizVisualToolsGui window to plan next screw"); + } + + if (use_obstacles && !collision_objects.empty()) { + if (collision_obj_exists) { + std::vector remove_objs; + remove_objs.push_back(collision_objects.front().id); + planning_scene_interface.removeCollisionObjects(remove_objs); + collision_objects.pop(); + } + std::vector collision_obj_vec; + collision_obj_vec.push_back(collision_objects.front()); + planning_scene_interface.addCollisionObjects(collision_obj_vec); + collision_obj_exists = true; + } + + show_screw(req.screw_path.at(0).screw_msg, visual_tools); + + for (size_t i = 0; i < num_sample; ++i) { + std::cout << "Starting i = " << i << "\n"; + ap_planning::APPlanningResponse result; + auto start = std::chrono::high_resolution_clock::now(); + ap_planning::Result success = ap_planner.plan(req, result); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + if (success == ap_planning::SUCCESS) { + std::cout << "\n\n\nDSS planning: Success!!\n\n"; + std::cout << "Trajectory is: " << result.percentage_complete * 100 + << "% complete, and has length: " << result.path_length + << "\n"; + + if (show_trajectories) { + show_trajectory(result.joint_trajectory, visual_tools); + } + last_plan = result; + } else { + std::cout << "\n\n\nDSS planning: Fail (" << ap_planning::toStr(success) + << ")\n\n"; + } + + ss_dssp << sample << ", DSS, " << ap_planning::toStr(success) << ", " + << result.percentage_complete * 100 << ", " << duration.count() + << ", " << result.path_length << ",\n"; + } + + // Now move to SPS planner + if (show_trajectories) { + visual_tools.prompt( + "Press 'next' in the RvizVisualToolsGui window to plan again using " + "SPS planner"); + } + + // Try planning + for (size_t i = 0; i < num_sps; ++i) { + ap_planning::APPlanningResponse sps_output; + auto start = std::chrono::high_resolution_clock::now(); + auto sps_res = sequential_step_planner.plan(req, sps_output); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + if (sps_res == ap_planning::SUCCESS) { + std::cout << "\n\n\nSPS planning: Success!!\n\n"; + std::cout << "Trajectory is: " << sps_output.percentage_complete * 100 + << "% complete, and has length: " << sps_output.path_length + << "\n"; + if (show_trajectories) { + show_trajectory(sps_output.joint_trajectory, visual_tools); + } + last_plan = sps_output; + } else { + std::cout << "\n\n\nSPS planning: Fail (" << ap_planning::toStr(sps_res) + << ")\n\n"; + std::cout << "Trajectory is: " << sps_output.percentage_complete * 100 + << "% complete, and has length: " << sps_output.path_length + << "\n"; + } + ss_sps << sample << ", SPS, " << ap_planning::toStr(sps_res) << ", " + << sps_output.percentage_complete * 100 << ", " << duration.count() + << ",\n"; + } + } + + show_trajectory(last_plan.joint_trajectory, visual_tools); + + ss_dssp << "End output\n"; + ss_sps << "End output\n"; + std::cout << ss_dssp.str(); + std::cout << ss_sps.str(); + + ros::shutdown(); + return 0; +} diff --git a/src/unchained_screws_demo.cpp b/src/unchained_screws_demo.cpp new file mode 100644 index 0000000..ff905eb --- /dev/null +++ b/src/unchained_screws_demo.cpp @@ -0,0 +1,428 @@ +#include +#include + +#include +#include +#include +#include +#include + +void show_multi_screw(const ap_planning::APPlanningRequest &req, + moveit_visual_tools::MoveItVisualTools &visual_tools) { + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); + + // Get constraint + auto constraint = req.toConstraint(); + + visual_tools.publishAxis(constraint->referenceFrame()); + + // Plot each screw + const auto viz_screws = constraint->getVisualScrews(); + for (const auto &screw : viz_screws) { + // Create points for plotting + Eigen::Vector3d origin, axis; + tf2::fromMsg(screw.origin, origin); + tf2::fromMsg(screw.axis, axis); + Eigen::Vector3d end = origin + 0.2 * axis.normalized(); + geometry_msgs::Point end_point = tf2::toMsg(end); + geometry_msgs::Point origin_point = tf2::toMsg(origin); + + // Plot + auto color = screw.is_pure_translation ? rviz_visual_tools::PURPLE + : rviz_visual_tools::ORANGE; + visual_tools.publishArrow(origin_point, end_point, color, + rviz_visual_tools::LARGE); + } + visual_tools.trigger(); +} + +void show_trajectory(const trajectory_msgs::JointTrajectory &traj, + moveit_visual_tools::MoveItVisualTools &visual_tools) { + moveit_msgs::DisplayTrajectory joint_traj; + joint_traj.model_id = "panda"; + joint_traj.trajectory.push_back(moveit_msgs::RobotTrajectory()); + joint_traj.trajectory.at(0).joint_trajectory = traj; + + moveit_msgs::RobotState start_msg; + start_msg.joint_state.name = traj.joint_names; + auto first_waypoint = traj.points.at(0); + start_msg.joint_state.position = first_waypoint.positions; + joint_traj.trajectory_start = start_msg; + + joint_traj.trajectory.at(0).joint_trajectory.header.frame_id = "panda_link0"; + + int time = 0; + + for (auto &wp : joint_traj.trajectory.at(0).joint_trajectory.points) { + wp.time_from_start.sec = time; + ++time; + } + + visual_tools.publishTrajectoryPath(joint_traj); +} + +std::queue +get_planning_queue(const std::vector &default_joint_state) { + std::queue planning_queue; + ap_planning::APPlanningRequest single_request; + ap_planning::ScrewSegment screw_0, screw_1, screw_2, screw_3; + screw_0.screw_msg.header.frame_id = "panda_link0"; + screw_1.screw_msg.header.frame_id = "panda_link0"; + screw_2.screw_msg.header.frame_id = "panda_link0"; + + single_request.ee_frame_name = "panda_link8"; + single_request.planning_time = 10; + single_request.screw_path_type = ap_planning::ScrewPathType::UNCHAINED; + + // For now, all requests start at same point + single_request.start_pose.pose.position.x = 0.5; + single_request.start_pose.pose.position.z = 0.4; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + + // Add some test cases + // Case 1: Linear screws spanning x-y plane + screw_0.start_theta = 0.0; + screw_0.end_theta = 0.2; + screw_0.screw_msg.is_pure_translation = true; + screw_0.screw_msg.origin = single_request.start_pose.pose.position; + screw_0.screw_msg.axis.x = 1; + + screw_1.start_theta = 0.0; + screw_1.end_theta = 0.2; + screw_1.screw_msg.axis.y = 1; + screw_1.screw_msg.is_pure_translation = true; + screw_1.screw_msg.origin = single_request.start_pose.pose.position; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + planning_queue.push(single_request); + + // Case 2: Linear screws spanning x-z plane + single_request.screw_path.clear(); + single_request.start_pose.pose.position.x = 0.5; + single_request.start_pose.pose.position.z = 0.3; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + + screw_0.start_theta = 0.0; + screw_0.end_theta = 0.2; + screw_0.screw_msg.is_pure_translation = true; + screw_0.screw_msg.origin = single_request.start_pose.pose.position; + screw_0.screw_msg.axis.x = 1; + + single_request.start_pose.pose.position.x = 0.5; + single_request.start_pose.pose.position.z = 0.3; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + + screw_1.start_theta = 0.0; + screw_1.end_theta = 0.2; + screw_1.screw_msg.axis.y = 0; // reset the setting from Case 1 + screw_1.screw_msg.axis.z = 1; + screw_1.screw_msg.is_pure_translation = true; + screw_1.screw_msg.origin = single_request.start_pose.pose.position; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + planning_queue.push(single_request); + + // Case 3: Concentric circular screws + single_request.start_pose.pose.position.x = 0.3; + single_request.start_pose.pose.position.z = 0.4; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + single_request.screw_path.clear(); + + screw_0.start_theta = 0.0; + screw_0.end_theta = 0.5 * M_PI; + screw_0.screw_msg.is_pure_translation = false; + screw_0.screw_msg.axis.x = 0; + screw_0.screw_msg.axis.z = -1; + screw_0.screw_msg.origin = single_request.start_pose.pose.position; + screw_0.screw_msg.origin.y += 0.1; + + screw_1.start_theta = 0.0; + screw_1.end_theta = 0.5 * M_PI; + screw_1.screw_msg.is_pure_translation = false; + screw_1.screw_msg.axis.z = -1; + screw_1.screw_msg.origin = single_request.start_pose.pose.position; + screw_1.screw_msg.origin.y += 0.1; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + planning_queue.push(single_request); + + // Case 4: Circular parallel screws separated by a distance + single_request.start_pose.pose.position.x = 0.4; + single_request.start_pose.pose.position.z = 0.4; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + single_request.screw_path.clear(); + + screw_0.start_theta = 0.0; + screw_0.end_theta = 0.5 * M_PI; + screw_0.screw_msg.is_pure_translation = false; + screw_0.screw_msg.axis.x = 0; + screw_0.screw_msg.axis.z = -1; + screw_0.screw_msg.origin = single_request.start_pose.pose.position; + screw_0.screw_msg.origin.y += 0.1; + + single_request.start_pose.pose.position.x = 0.35; + single_request.start_pose.pose.position.z = 0.4; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + + screw_1.start_theta = 0.0; + screw_1.end_theta = 0.5 * M_PI; + screw_1.screw_msg.is_pure_translation = false; + screw_1.screw_msg.axis.z = -1; + screw_1.screw_msg.origin = single_request.start_pose.pose.position; + screw_1.screw_msg.origin.y += 0.1; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + planning_queue.push(single_request); + + // Case 5: Orthogonal circular screws similar to the unchained door-knob + // door-hinge case + single_request.start_pose.pose.position.x = 0.3; + single_request.start_pose.pose.position.z = 0.4; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + single_request.screw_path.clear(); + + screw_0.start_theta = 0.0; + screw_0.end_theta = 0.5 * M_PI; + screw_0.screw_msg.is_pure_translation = false; + screw_0.screw_msg.axis.x = 0; + screw_0.screw_msg.axis.y = 1; + screw_0.screw_msg.axis.z = 0; + screw_0.screw_msg.origin = single_request.start_pose.pose.position; + screw_0.screw_msg.origin.y += 0.0; + + single_request.start_pose.pose.position.x = 0.2; + single_request.start_pose.pose.position.z = 0.4; + single_request.start_pose.pose.orientation.x = 1.0; + single_request.start_pose.pose.orientation.w = 0; + + screw_1.start_theta = 0.0; + screw_1.end_theta = 0.5 * M_PI; + screw_1.screw_msg.is_pure_translation = false; + screw_1.screw_msg.axis.z = 1; + screw_1.screw_msg.origin = single_request.start_pose.pose.position; + screw_1.screw_msg.origin.y += 0.1; + + single_request.screw_path.push_back(screw_0); + single_request.screw_path.push_back(screw_1); + planning_queue.push(single_request); + + return planning_queue; +} + +std::queue get_collision_objects() { + // Set up stuff same across all test cases + std::queue output; + moveit_msgs::CollisionObject collision_object; + collision_object.header.frame_id = "panda_link0"; + collision_object.operation = collision_object.ADD; + + geometry_msgs::Pose box_pose; + box_pose.orientation.w = 1.0; + shape_msgs::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + + // Case 1 obstacle + collision_object.id = "screw1_box1"; + primitive.dimensions[primitive.BOX_X] = 0.1; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 0.4; + box_pose.position.x = 0.25; + box_pose.position.y = 0.0; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + // Case 2 obstacle + collision_object.id = "screw1_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 0.4; + box_pose.position.x = 0.25; + box_pose.position.y = 0.25; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + // Case 3 obstacle + collision_object.id = "screw1_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 0.4; + box_pose.position.x = 0.25; + box_pose.position.y = 0.45; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + // Case 4 obstacle + collision_object.id = "screw1_box1"; + primitive.dimensions[primitive.BOX_X] = 1.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 0.4; + box_pose.position.x = 0.25; + box_pose.position.y = 0.45; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + // Case 5 obstacle + collision_object.id = "screw1_box1"; + primitive.dimensions[primitive.BOX_X] = 0.1; + primitive.dimensions[primitive.BOX_Y] = 1.5; + primitive.dimensions[primitive.BOX_Z] = 0.2; + box_pose.position.x = 0.5; + box_pose.position.y = 0.0; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + output.push(collision_object); + collision_object.primitives.clear(); + collision_object.primitive_poses.clear(); + + return output; +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "ap_planning"); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(2); + spinner.start(); + + robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); + const moveit::core::RobotModelPtr &kinematic_model = + robot_model_loader.getModel(); + + moveit::core::RobotStatePtr kinematic_state( + new moveit::core::RobotState(kinematic_model)); + std::vector default_joint_state{0, -0.785, 0, -2.356, + 0, 1.571, 0.785}; + kinematic_state->setJointGroupPositions("panda_arm", default_joint_state); + + int num_sample; + nh.param(ros::this_node::getName() + "/num_sampling", num_sample, 2); + + bool show_trajectories; + nh.param(ros::this_node::getName() + "/show_trajectories", + show_trajectories, true); + + bool use_obstacles; + nh.param(ros::this_node::getName() + "/add_collision_objects", + use_obstacles, true); + + // Add collision objects + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + auto collision_objects = get_collision_objects(); + + ros::Duration(2.0).sleep(); + + moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); + visual_tools.deleteAllMarkers(); + visual_tools.loadRemoteControl(); + visual_tools.setRobotStateTopic("/display_robot_state"); + visual_tools.trigger(); + + auto planning_queue = get_planning_queue(default_joint_state); + + ap_planning::DSSPlanner ap_planner("panda_arm"); + + std::stringstream ss_dssp; + ss_dssp << "Start of output\n"; + size_t sample = 0; + ap_planning::APPlanningResponse last_plan; + bool collision_obj_exists = false; + + // Plan each screw request + while (planning_queue.size() > 0 && ros::ok()) { + sample++; + auto req = planning_queue.front(); + planning_queue.pop(); + + if (show_trajectories) { + visual_tools.prompt( + "Press 'next' in the RvizVisualToolsGui window to plan next screw"); + } + + if (use_obstacles && !collision_objects.empty()) { + if (collision_obj_exists) { + std::vector remove_objs; + remove_objs.push_back(collision_objects.front().id); + planning_scene_interface.removeCollisionObjects(remove_objs); + collision_objects.pop(); + } + std::vector collision_obj_vec; + collision_obj_vec.push_back(collision_objects.front()); + planning_scene_interface.addCollisionObjects(collision_obj_vec); + collision_obj_exists = true; + } + + show_multi_screw(req, visual_tools); + + for (size_t i = 0; i < num_sample; ++i) { + std::cout << "Starting i = " << i << "\n"; + ap_planning::APPlanningResponse result; + auto start = std::chrono::high_resolution_clock::now(); + ap_planning::Result success = ap_planner.plan(req, result); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + if (success == ap_planning::SUCCESS) { + std::cout << "\n\n\nDSS planning: Success!!\n\n"; + std::cout << "Trajectory is: " << result.percentage_complete * 100 + << "% complete, and has length: " << result.path_length + << "\n"; + + if (show_trajectories) { + show_trajectory(result.joint_trajectory, visual_tools); + } + last_plan = result; + } else { + std::cout << "\n\n\nDSS planning: Fail (" << ap_planning::toStr(success) + << ")\n\n"; + } + + ss_dssp << sample << ", DSS, " << ap_planning::toStr(success) << ", " + << result.percentage_complete * 100 << ", " << duration.count() + << ", " << result.path_length << ",\n"; + } + } + + if (last_plan.joint_trajectory.points.size() > 1) { + show_trajectory(last_plan.joint_trajectory, visual_tools); + } + + ss_dssp << "End output\n"; + std::cout << ss_dssp.str(); + + ros::shutdown(); + return 0; +}