8000 cleanup np · Zyszu200/robotics-toolbox-python@61e1331 · GitHub
[go: up one dir, main page]
Skip to content

Commit 61e1331

Browse files
jhavljhavl
committed
cleanup np
1 parent dc97c56 commit 61e1331

File tree

1 file changed

+33
-23
lines changed

1 file changed

+33
-23
lines changed

‎roboticstoolbox/robot/ERobot.py

Lines changed: 33 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,20 @@
88
import tempfile
99
import subprocess
1010
import webbrowser
11-
from numpy import array, ndarray, isnan, zeros, eye, expand_dims, empty, concatenate
11+
from numpy import (
12+
array,
13+
ndarray,
14+
isnan,
15+
zeros,
16+
eye,
17+
expand_dims,
18+
empty,
19+
concatenate,
20+
cross,
21+
arccos,
22+
dot,
23+
)
24+
from numpy.linalg import norm as npnorm, inv
1225
from spatialmath import SE3, SE2
1326
from spatialgeometry import Cylinder
1427
from spatialmath.base.argcheck import getvector, islistof
@@ -2019,7 +2032,6 @@ def vision_collision_damper(
20192032
:returns: Ain, Bin as the inequality contraints for an omptimisor
20202033
:rtype: ndarray(6), ndarray(6)
20212034
"""
2022-
import numpy as np
20232035

20242036
if start is None:
20252037
start = self.base_link
@@ -2034,11 +2046,11 @@ def vision_collision_damper(
20342046
bin = None
20352047

20362048
def rotation_between_vectors(a, b):
2037-
a = a / np.linalg.norm(a)
2038-
b = b / np.linalg.norm(b)
2049+
a = a / npnorm(a)
2050+
b = b / npnorm(b)
20392051

2040-
angle = np.arccos(np.dot(a, b))
2041-
axis = np.cross(a, b)
2052+
angle = arccos(dot(a, b))
2053+
axis = cross(a, b)
20422054

20432055
return SE3.AngleAxis(angle, axis)
20442056

@@ -2051,13 +2063,11 @@ def rotation_between_vectors(a, b):
20512063

20522064
# Create line of sight object
20532065
los_mid = SE3((wTcp + wTtp) / 2)
2054-
los_orientation = rotation_between_vectors(
2055-
np.array([0.0, 0.0, 1.0]), wTcp - wTtp
2056-
)
2066+
los_orientation = rotation_between_vectors(array([0.0, 0.0, 1.0]), wTcp - wTtp)
20572067

20582068
los = Cylinder(
20592069
radius=0.001,
2060-
length=np.linalg.norm(wTcp - wTtp),
2070+
length=npnorm(wTcp - wTtp),
20612071
base=(los_mid * los_orientation),
20622072
)
20632073

@@ -2068,11 +2078,9 @@ def indiv_calculation(link, link_col, q):
20682078
lpTvp = -wTlp + wTvp
20692079

20702080
norm = lpTvp / d
2071-
norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)
2081+
norm_h = expand_dims(concatenate((norm, [0, 0, 0])), axis=0)
20722082

2073-
tool = SE3(
2074-
(np.linalg.inv(self.fkine(q, end=link).A) @ SE3(wTlp).A)[:3, 3]
2075-
)
2083+
tool = SE3((inv(self.fkine(q, end=link).A) @ SE3(wTlp).A)[:3, 3])
20762084

20772085
Je = self.jacob0(q, end=link, tool=tool.A)
20782086
Je[:3, :] = self._T[:3, :3] @ Je[:3, :]
@@ -2082,21 +2090,23 @@ def indiv_calculation(link, link_col, q):
20822090
Jv = camera.jacob0(camera.q)
20832091
Jv[:3, :] = self._T[:3, :3] @ Jv[:3, :]
20842092

2085-
Jv *= np.linalg.norm(wTvp - shape.T[:3, -1]) / los.length
2093+
Jv *= npnorm(wTvp - shape.T[:3, -1]) / los.length
20862094

20872095
dpc = norm_h @ Jv
2088-
dpc = np.r_[
2089-
dpc[0, :-camera_n],
2090-
np.zeros(self.n - (camera.n - camera_n)),
2091-
dpc[0, -camera_n:],
2092-
]
2096+
dpc = concatenate(
2097+
(
2098+
dpc[0, :-camera_n],
2099+
zeros(self.n - (camera.n - camera_n)),
2100+
dpc[0, -camera_n:],
2101+
)
2102+
)
20932103
else:
2094-
dpc = np.zeros((1, self.n + camera_n))
2104+
dpc = zeros((1, self.n + camera_n))
20952105

20962106
dpt = norm_h @ shape.v
2097-
dpt *= np.linalg.norm(wTvp - wTcp) / los.length
2107+
dpt *= npnorm(wTvp - wTcp) / los.length
20982108

2099-
l_Ain = np.zeros((1, self.n + camera_n))
2109+
l_Ain = zeros((1, self.n + camera_n))
21002110
l_Ain[0, :n_dim] = norm_h @ Je
21012111
l_Ain -= dpc
21022112
l_bin = (xi * (d - ds) / (di - ds)) + dpt

0 commit comments

Comments
 (0)
0