Add debug, add new robot, goal joint_constraints added