Add demo with visualization

Signed-off-by: methylDragon <methylDragon@gmail.com>

CMakeLists.txt

@@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   moveit_msgs
   moveit_ros_planning
   moveit_ros_planning_interface
+  moveit_ros_trajectory_cache
   moveit_servo
   moveit_task_constructor_core
   moveit_visual_tools
@@ -69,6 +70,7 @@ add_subdirectory(doc/how_to_guides/parallel_planning)
 add_subdirectory(doc/how_to_guides/chomp_planner)
 add_subdirectory(doc/how_to_guides/persistent_scenes_and_states)
 add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner)
+add_subdirectory(doc/how_to_guides/trajectory_cache)
 add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning)
 add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor)
 add_subdirectory(doc/tutorials/quickstart_in_rviz)

doc/how_to_guides/trajectory_cache/CMakeLists.txt

@@ -0,0 +1,6 @@
+add_executable(trajectory_cache_demo src/trajectory_cache_demo.cpp)
+ament_target_dependencies(trajectory_cache_demo ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost moveit_ros_trajectory_cache)
+
+install(TARGETS trajectory_cache_demo DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})

doc/how_to_guides/trajectory_cache/config/trajectory_cache_joint_limits.yaml

@@ -0,0 +1,56 @@
+# These are faster joint limits to allow for faster execution.
+joint_limits:
+  panda_joint1:
+    has_velocity_limits: true
+    max_velocity: 50.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false
+  panda_joint2:
+    has_velocity_limits: true
+    max_velocity: 50.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false
+  panda_joint3:
+    has_velocity_limits: true
+    max_velocity: 50.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false
+  panda_joint4:
+    has_velocity_limits: true
+    max_velocity: 50.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false
+  panda_joint5:
+    has_velocity_limits: true
+    max_velocity: 50.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false
+  panda_joint6:
+    has_velocity_limits: true
+    max_velocity: 50.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false
+  panda_joint7:
+    has_velocity_limits: true
+    max_velocity: 50.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false
+  panda_finger_joint1:
+    has_velocity_limits: true
+    max_velocity: 25.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false
+  panda_finger_joint2:
+    has_velocity_limits: true
+    max_velocity: 25.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+    has_jerk_limits: false