Code for getting and merging plans for keyframe demonstrations is duplicated: https://github.com/HLP-R/hlpr_manipulation/tree/master/hlpr_manipulation_utils/src/hlpr_manipulation_utils (in both of the arm moveit files) and https://github.com/HLP-R/hlpr_kinesthetic_teaching/blob/master/hlpr_record_demonstration/src/hlpr_record_demonstration/playback_demo_action_server_refactor.py