Added merge function
Improved handling of dummy links
Merge takes pointers now
Added routines for calculating constrained torques
Allow true/false edge planners to be called in incremental planners like SBL