add addLink function to add_sensor_to_urdf.py