Added ToolCommandStamped, trajectory test scripts