add trajectory node. gripper joint not working yet