removed duplicate function

Author: rhys-newbury1

environment.yml

@@ -0,0 +1,198 @@
+name: mobile
+channels:
+  - conda-forge
+  - defaults
+dependencies:
+  - aiohttp=3.8.1=py38h294d835_0
+  - aiosignal=1.2.0=pyhd8ed1ab_0
+  - ansitable=0.9.6=pyhd8ed1ab_0
+  - anyio=3.4.0=py38haa244fe_0
+  - argon2-cffi=21.1.0=py38h294d835_2
+  - async-timeout=4.0.2=pyhd8ed1ab_0
+  - async_generator=1.10=py_0
+  - attrs=21.2.0=pyhd8ed1ab_0
+  - autobahn=21.11.1=pyhd8ed1ab_0
+  - backcall=0.2.0=pyh9f0ad1d_0
+  - backports=1.0=py_2
+  - backports.functools_lru_cache=1.6.4=pyhd8ed1ab_0
+  - bleach=4.1.0=pyhd8ed1ab_0
+  - brotli=1.0.9=h8ffe710_6
+  - brotli-bin=1.0.9=h8ffe710_6
+  - bullet=3.17=h57928b3_1
+  - bullet-cpp=3.17=h5d928e2_1
+  - ca-certificates=2021.10.8=h5b45459_0
+  - certifi=2021.10.8=py38haa244fe_1
+  - cffi=1.15.0=py38hd8c33c5_0
+  - charset-normalizer=2.0.9=pyhd8ed1ab_0
+  - colorama=0.4.4=pyh9f0ad1d_0
+  - colored=1.4.2=py_0
+  - cryptography=36.0.1=py38hb7941b4_0
+  - cvxopt=1.2.7=py38he924ae6_1
+  - cycler=0.11.0=pyhd8ed1ab_0
+  - cython=0.29.26=py38h885f38d_0
+  - debugpy=1.5.1=py38h885f38d_0
+  - decorator=5.1.0=pyhd8ed1ab_0
+  - defusedxml=0.7.1=pyhd8ed1ab_0
+  - dsdp=5.8=h6e01ec9_1203
+  - ecos=2.0.8=py38h6f4d8f0_2
+  - entrypoints=0.3=pyhd8ed1ab_1003
+  - ffmpeg=4.3.1=ha925a31_0
+  - fftw=3.3.10=nompi_h77347bd_102
+  - fonttools=4.28.5=py38h294d835_0
+  - freeglut=3.2.1=h0e60522_2
+  - freetype=2.10.4=h546665d_1
+  - frozenlist=1.2.0=py38h294d835_1
+  - glpk=4.65=h8ffe710_1004
+  - gsl=2.7=hdfb1a43_0
+  - hyperlink=21.0.0=pyhd3deb0d_0
+  - icu=68.2=h0e60522_0
+  - idna=3.1=pyhd3deb0d_0
+  - imageio=2.13.4=pyh239f2a4_0
+  - imageio-ffmpeg=0.4.5=pyhd8ed1ab_0
+  - importlib-metadata=4.10.0=py38haa244fe_0
+  - importlib_resources=5.4.0=pyhd8ed1ab_0
+  - intel-openmp=2021.4.0=h57928b3_3556
+  - ipykernel=6.5.1=py38h595d716_0
+  - ipython=7.30.1=py38haa244fe_0
+  - ipython_genutils=0.2.0=py_1
+  - ipywidgets=7.6.5=pyhd8ed1ab_0
+  - jasper=2.0.33=h77af90b_0
+  - jbig=2.1=h8d14728_2003
+  - jedi=0.18.1=py38haa244fe_0
+  - jinja2=3.0.3=pyhd8ed1ab_0
+  - jpeg=9d=h8ffe710_0
+  - jsonschema=4.3.2=pyhd8ed1ab_0
+  - jupyter=1.0.0=py38haa244fe_7
+  - jupyter-server-proxy=3.2.0=pyhd8ed1ab_0
+  - jupyter_client=6.1.12=pyhd8ed1ab_0
+  - jupyter_console=6.4.0=pyhd8ed1ab_1
+  - jupyter_core=4.9.1=py38haa244fe_1
+  - jupyter_server=1.13.1=pyhd8ed1ab_0
+  - jupyterlab_pygments=0.1.2=pyh9f0ad1d_0
+  - jupyterlab_widgets=1.0.2=pyhd8ed1ab_0
+  - kiwisolver=1.3.2=py38hbd9d945_1
+  - lcms2=2.12=h2a16943_0
+  - lerc=3.0=h0e60522_0
+  - libblas=3.9.0=12_win64_mkl
+  - libbrotlicommon=1.0.9=h8ffe710_6
+  - libbrotlidec=1.0.9=h8ffe710_6
+  - libbrotlienc=1.0.9=h8ffe710_6
+  - libcblas=3.9.0=12_win64_mkl
+  - libclang=11.1.0=default_h5c34c98_1
+  - libdeflate=1.8=h8ffe710_0
+  - liblapack=3.9.0=12_win64_mkl
+  - liblapacke=3.9.0=12_win64_mkl
+  - libopencv=4.5.5=py38hd5548a8_0
+  - libpng=1.6.37=h1d00b33_2
+  - libprotobuf=3.19.3=h7755175_0
+  - libsodium=1.0.18=h8d14728_1
+  - libtiff=4.3.0=hd413186_2
+  - libwebp-base=1.2.1=h8ffe710_0
+  - libzlib=1.2.11=h8ffe710_1013
+  - lz4-c=1.9.3=h8ffe710_1
+  - m2w64-gcc-libgfortran=5.3.0=6
+  - m2w64-gcc-libs=5.3.0=7
+  - m2w64-gcc-libs-core=5.3.0=7
+  - m2w64-gmp=6.1.0=2
+  - m2w64-libwinpthread-git=5.0.0.4634.697f757=2
+  - markupsafe=2.0.1=py38h294d835_1
+  - matplotlib-base=3.5.1=py38h1f000d6_0
+  - matplotlib-inline=0.1.3=pyhd8ed1ab_0
+  - mistune=0.8.4=py38h294d835_1005
+  - mkl=2021.4.0=h0e2418a_729
+  - mpmath=1.2.1=pyhd8ed1ab_0
+  - msys2-conda-epoch=20160418=1
+  - multidict=5.2.0=py38h294d835_1
+  - munkres=1.1.4=pyh9f0ad1d_0
+  - nbclient=0.5.9=pyhd8ed1ab_0
+  - nbconvert=6.3.0=py38haa244fe_1
+  - nbformat=5.1.3=pyhd8ed1ab_0
+  - nest-asyncio=1.5.4=pyhd8ed1ab_0
+  - notebook=6.4.6=pyha770c72_0
+  - numpy=1.21.5=py38hcf66579_0
+  - numpy-stl=2.16.3=py38h6f4d8f0_1
+  - olefile=0.46=pyh9f0ad1d_1
+  - opencv=4.5.5=py38haa244fe_0
+  - openjpeg=2.4.0=hb211442_1
+  - openssl=1.1.1l=h8ffe710_0
+  - osqp=0.6.2.post0=py38h5d928e2_3
+  - packaging=21.3=pyhd8ed1ab_0
+  - pandoc=2.16.2=h8ffe710_0
+  - pandocfilters=1.5.0=pyhd8ed1ab_0
+  - parso=0.8.3=pyhd8ed1ab_0
+  - pgraph-python=0.6.1=pyhd8ed1ab_0
+  - pickleshare=0.7.5=py_1003
+  - pillow=8.4.0=py38h794f750_0
+  - pip=21.3.1=pyhd8ed1ab_0
+  - progress=1.6=pyhd8ed1ab_0
+  - prometheus_client=0.12.0=pyhd8ed1ab_0
+  - prompt-toolkit=3.0.24=pyha770c72_0
+  - prompt_toolkit=3.0.24=hd8ed1ab_0
+  - py-opencv=4.5.5=py38h595d716_0
+  - pybullet=3.17=py38h5d928e2_1
+  - pycparser=2.21=pyhd8ed1ab_0
+  - pygments=2.10.0=pyhd8ed1ab_0
+  - pyparsing=3.0.6=pyhd8ed1ab_0
+  - pyqt=5.12.3=py38haa244fe_8
+  - pyqt-impl=5.12.3=py38h885f38d_8
+  - pyqt5-sip=4.19.18=py38h885f38d_8
+  - pyqtchart=5.12=py38h885f38d_8
+  - pyqtwebengine=5.12.1=py38h885f38d_8
+  - pyrsistent=0.18.0=py38h294d835_0
+  - python=3.8.12=h7840368_2_cpython
+  - python-dateutil=2.8.2=pyhd8ed1ab_0
+  - python-utils=2.6.3=pyhd8ed1ab_0
+  - python_abi=3.8=2_cp38
+  - pywin32=302=py38h294d835_2
+  - pywinpty=1.1.6=py38hd3f51b4_0
+  - pyyaml=6.0=py38h294d835_3
+  - pyzmq=22.3.0=py38h09162b1_1
+  - qdldl-python=0.1.5=py38h5d928e2_2
+  - qpsolvers=1.7.2=pyhd8ed1ab_0
+  - qt=5.12.9=h5909a2a_4
+  - qtconsole=5.2.2=pyhd8ed1ab_0
+  - qtpy=2.0.0=pyhd8ed1ab_0
+  - quadprog=0.1.11=py38hbd9d945_0
+  - roboticstoolbox-python=0.11.0=py38h6f4d8f0_0
+  - rtb-data=0.10.0=pyhd8ed1ab_0
+  - scipy=1.7.3=py38ha1292f7_0
+  - send2trash=1.8.0=pyhd8ed1ab_0
+  - setuptools=60.0.4=py38haa244fe_0
+  - simpervisor=0.4=pyhd8ed1ab_0
+  - six=1.16.0=pyh6c4a22f_0
+  - sniffio=1.2.0=py38haa244fe_2
+  - spatialgeometry=0.2.0=pyhd8ed1ab_0
+  - spatialmath-python=0.11=pyhd8ed1ab_0
+  - sphinxcontrib-jsmath=1.0.1=py_0
+  - sqlite=3.37.0=h8ffe710_0
+  - swift-sim=0.10.0=py38h6f4d8f0_1
+  - sympy=1.9=py38haa244fe_1
+  - tbb=2021.4.0=h2d74725_1
+  - terminado=0.12.1=py38haa244fe_1
+  - testpath=0.5.0=pyhd8ed1ab_0
+  - tk=8.6.11=h8ffe710_1
+  - tornado=6.1=py38h294d835_2
+  - traitlets=5.1.1=pyhd8ed1ab_0
+  - txaio=21.2.1=pyhd8ed1ab_0
+  - typing-extensions=4.0.1=hd8ed1ab_0
+  - typing_extensions=4.0.1=pyha770c72_0
+  - ucrt=10.0.20348.0=h57928b3_0
+  - unicodedata2=14.0.0=py38h294d835_0
+  - vc=14.2=hb210afc_5
+  - vpython=7.6.2=py38h294d835_0
+  - vs2015_runtime=14.29.30037=h902a5da_5
+  - wcwidth=0.2.5=pyh9f0ad1d_2
+  - webencodings=0.5.1=py_1
+  - websocket-client=1.2.3=pyhd8ed1ab_0
+  - websockets=10.1=py38h294d835_0
+  - wheel=0.37.1=pyhd8ed1ab_0
+  - widgetsnbextension=3.5.2=py38haa244fe_1
+  - winpty=0.4.3=4
+  - xz=5.2.5=h62dcd97_1
+  - yaml=0.2.5=he774522_0
+  - yarl=1.7.2=py38h294d835_1
+  - zeromq=4.3.4=h0e60522_1
+  - zipp=3.6.0=pyhd8ed1ab_0
+  - zlib=1.2.11=h8ffe710_1013
+  - zstd=1.5.0=h6255e5f_0
+prefix: D:\conda\envs\mobile

roboticstoolbox/robot/ERobot.py

@@ -2130,123 +2130,6 @@ def indiv_calculation(link, link_col, q):
         return Ain, bin
 
     def vision_collision_damper(
-        self,
-        shape,
-        q=None,
-        di=0.3,
-        ds=0.05,
-        xi=1.0,
-        end=None,
-        start=None,
-        collision_list=None,
-        camera=None
-    ):
-        """
-        Formulates an inequality contraint which, when optimised for will
-        make it impossible for the robot to run into a collision. Requires
-        See examples/neo.py for use case
-        :param ds: The minimum distance in which a joint is allowed to
-            approach the collision object shape
-        :type ds: float
-        :param di: The influence distance in which the velocity
-            damper becomes active
-        :type di: float
-        :param xi: The gain for the velocity damper
-        :type xi: float
-        :param from_link: The first link to consider, defaults to the base
-            link
-        :type from_link: ELink
-        :param to_link: The last link to consider, will consider all links
-            between from_link and to_link in the robot, defaults to the
-            end-effector link
-        :type to_link: ELink
-        :returns: Ain, Bin as the inequality contraints for an omptimisor
-        :rtype: ndarray(6), ndarray(6)
-        """
-
-        if start is None:
-            start = self.base_link
-
-        if end is None:
-            end = self.ee_link
-
-        links, n, _ = self.get_path(start=start, end=end)
-
-        # if q is None:
-        #     q = np.copy(self.q)
-        # else:
-        #     q = getvector(q, n)
-
-        j = 0
-        Ain = None
-        bin = None
-        din = None
-
-        def indiv_calculation(link, link_col, q):
-            d, wTlp, wTcp = link_col.closest_point(shape, di)
-
-            if d is not None:
-                lpTcp = -wTlp + wTcp
-
-                norm = lpTcp / d
-                norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)
-
-                tool = SE3((np.linalg.inv(self.fkine(q, end=link, fast=True)) @ SE3(wTlp).A)[:3, 3])
-
-                Je = self.jacob0(
-                    q, end=link, tool=tool.A, fast=True
-                )
-                Je[:3, :] = self._base.A[:3, :3] @ Je[:3, :]
-
-                wTc = camera.fkine(camera.q, fast=True)
-                Jv = camera.jacob0(
-                    camera.q, tool=SE3(np.linalg.inv(wTc[:3, :3]) @ (wTcp - wTc[:3, -1])).A, fast=True
-                )
-                Jv[:3, :] = self._base.A[:3, :3] @ Jv[:3, :]
-
-                n_dim = Je.shape[1]
-                dp = norm_h @ Jv
-                l_Ain = np.zeros((1, self.n + 2 + 10))
-                l_Ain[0, :n_dim] = norm_h @ Je
-                l_Ain -= np.r_[dp[0, :3], np.zeros(7), dp[0, 3:], np.zeros(9), 1]
-                l_bin = (xi * (d - ds) / (di - ds))
-            else:
-                l_Ain = None
-                l_bin = None
-
-            return l_Ain, l_bin, d, wTcp
-        
-        for link in links:
-            if link.isjoint:
-                j += 1
-
-            if collision_list is None:
-                col_list = link.collision
-            else:
-                col_list = collision_list[j - 1]
-
-            for link_col in col_list:
-                l_Ain, l_bin, d, wTcp = indiv_calculation(link, link_col, q)
-
-                if l_Ain is not None and l_bin is not None:
-                    if Ain is None:
-                        Ain = l_Ain
-                    else:
-                        Ain = np.r_[Ain, l_Ain]
-
-                    if bin is None:
-                        bin = np.array(l_bin)
-                    else:
-                        bin = np.r_[bin, l_bin]
-
-                    if din is None:
-                        din = d
-                    else:
-                        din = np.r_[din, d]
-
-        return Ain, bin, din
-
-    def new_vision_collision_damper(
         self,
         shape,
         q=None,
@@ -2261,8 +2144,8 @@ def new_vision_collision_damper(
     ):
         """
         Formulates an inequality contraint which, when optimised for will
-        make it impossible for the robot to run into a collision. Requires
-        See examples/neo.py for use case
+        make it impossible for the robot to self-occlude
+        :param shape: The line of sight object. 
         :param ds: The minimum distance in which a joint is allowed to
             approach the collision object shape
         :type ds: float
@@ -2290,11 +2173,6 @@ def new_vision_collision_damper(
 
         links, n, _ = self.get_path(start=start, end=end)
 
-        # if q is None:
-        #     q = np.copy(self.q)
-        # else:
-        #     q = getvector(q, n)
-
         j = 0
         Ain = None
         bin = None
@@ -2371,7 +2249,6 @@ def indiv_calculation(link, link_col, q):
 
         return Ain, bin, din
 
-
     # inverse dynamics (recursive Newton-Euler) using spatial vector notation
     def rne(robot, q, qd, qdd, symbolic=False, gravity=None):