Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/roboticstoolbox/ets/ETS2.py
Original file line number Diff line number Diff line change
Expand Up @@ -338,7 +338,7 @@ def eval(
# append
if l > 1:
T[k, :, :] = Tk
# ret.append(SE2(Tk, check=False)) # type: ignore
# ret.append(SE2(Tk, check=False))
else:
T = Tk
# ret = SE2(Tk, check=False)
Expand Down
14 changes: 7 additions & 7 deletions src/roboticstoolbox/models/DH/TwoLink.py
Original file line number Diff line number Diff line change
Expand Up @@ -73,12 +73,12 @@ def __init__(

zero = sym.zero()
pi = sym.pi()
a1, a2 = sym.symbol("a1 a2") # link lengths # type: ignore
m1, m2 = sym.symbol("m1 m2") # link masses # type: ignore
c1, c2 = sym.symbol("c1 c2") # link CoMs location relative to link frames# type: ignore
a1, a2 = sym.symbol("a1 a2") # link lengths
m1, m2 = sym.symbol("m1 m2") # link masses
c1, c2 = sym.symbol("c1 c2") # link CoMs location relative to link frames
if inertia:
r = sym.symbol("r") # link radius, assumed tubular, for inertia calculation # type: ignore
I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM # type: ignore
r = sym.symbol("r") # link radius, assumed tubular, for inertia calculation
I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM
else:
I1, I2 = None, None
g = sym.symbol("g")
Expand All @@ -98,7 +98,7 @@ def __init__(
c2 = 0.5 # CoM location of second link relative to second link frame
r = 0.1 # radius of links, assumed tubular, for inertia calculation
if inertia:
I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM # type: ignore
I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM
else:
I1, I2 = None, None
g = 9.8
Expand All @@ -120,7 +120,7 @@ def __init__(
c2 = -0.5 # CoM location of second link relative to second link frame
r = 0.1 # radius of links, assumed tubular, for inertia calculation
if inertia:
I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM # type: ignore
I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM
else:
I1, I2 = None, None
g = 9.8
Expand Down
11 changes: 6 additions & 5 deletions src/roboticstoolbox/robot/BaseRobot.py
Original file line number Diff line number Diff line change
Expand Up @@ -1937,15 +1937,16 @@ def recurse(link: Link) -> Any:
segs = [link.parent]
while True:
segs.append(link)
if link.nchildren == 0:
children = link.children or []
if len(children) == 0:
return [segs]
elif link.nchildren == 1:
link = link.children[0] # type: ignore
elif len(children) == 1:
link = children[0]
continue
elif link.nchildren > 1:
elif len(children) > 1:
segs = [segs] # type: ignore[list-item]

for child in link.children: # type: ignore
for child in children:
segs.extend(recurse(child))

return segs
Expand Down
21 changes: 17 additions & 4 deletions src/roboticstoolbox/robot/Link.py
Original file line number Diff line number Diff line change
Expand Up @@ -721,14 +721,15 @@ def I(self) -> NDArray:
@I.setter
@_dirties_frne
def I(self, I_new: ArrayLike) -> None:
I_new = np.asarray(I_new)
if ismatrix(I_new, (3, 3)):
# 3x3 matrix passed
if np.any(np.abs(I_new - I_new.T) > 1e-8): # type: ignore
if np.any(np.abs(I_new - I_new.T) > 1e-8):
raise ValueError("3x3 matrix is not symmetric")

elif isvector(I_new, 9):
# 3x3 matrix passed as a 1d vector
I_new = I_new.reshape(3, 3) # type: ignore
I_new = I_new.reshape(3, 3)
if np.any(np.abs(I_new - I_new.T) > 1e-8):
raise ValueError("3x3 matrix is not symmetric")

Expand Down Expand Up @@ -1100,7 +1101,13 @@ def closest_point(
"""

if not skip:
self.robot._update_link_tf(self.robot.q) # type: ignore
robot = self.robot
if robot is None:
raise ValueError(
"the link does not belong to a robot, use skip=True to use "
"its current transform"
)
robot._update_link_tf(robot.q)
self.update()
shape.update()

Expand Down Expand Up @@ -1133,7 +1140,13 @@ def iscollided(self, shape: Shape, skip: bool = False) -> bool:
"""

if not skip:
self.robot._update_link_tf(self.robot.q) # type: ignore
robot = self.robot
if robot is None:
raise ValueError(
"the link does not belong to a robot, use skip=True to use "
"its current transform"
)
robot._update_link_tf(robot.q)
self.update()
shape.update()

Expand Down
25 changes: 25 additions & 0 deletions tests/test_ELink.py
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,31 @@ def test_I(self):
nt.assert_array_almost_equal(l1.I, I1)
nt.assert_array_almost_equal(l2.I, I2)

def test_I_from_lists(self):
# a 3x3 matrix, and a 9-vector, given as lists rather than arrays
I0 = np.array([[1, 0, 0], [0, 2, 0], [0, 0, 3]])

l0 = rtb.Link(I=[[1, 0, 0], [0, 2, 0], [0, 0, 3]])
l1 = rtb.Link(I=[1, 0, 0, 0, 2, 0, 0, 0, 3])

nt.assert_array_almost_equal(l0.I, I0)
nt.assert_array_almost_equal(l1.I, I0)

with self.assertRaises(ValueError):
rtb.Link(I=[[1, 2, 0], [0, 2, 0], [0, 0, 3]]) # not symmetric

@skip_no_collision_checking
def test_collision_checks_need_a_robot(self):
# a link that is not part of a robot cannot update its transform
link = rtb.Link()
shape = gm.Cuboid([1, 1, 1])

with self.assertRaises(ValueError):
link.iscollided(shape)

with self.assertRaises(ValueError):
link.closest_point(shape)

def test_friction(self):
l0 = rtb.Link(Tc=[2, -1], B=3, G=2)

Expand Down
Loading