simple_heap Node for Motoman SDA10F control. Both direct Joint Trajectory Action and MoveIt Library approach are used