Skip to content

Commit bae653b

Browse files
jhavljhavl
committed
fix petercorke#355 add qlim setters
1 parent 5506dca commit bae653b

File tree

4 files changed

+74
-18
lines changed

4 files changed

+74
-18
lines changed

‎roboticstoolbox/robot/BaseRobot.py

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,6 @@ def __init__(
8585
configs: Union[Dict[str, NDArray], None] = None,
8686
check_jindex: bool = True,
8787
):
88-
8988
# Initialise the scene node
9089
SceneNode.__init__(self)
9190

@@ -140,7 +139,6 @@ def __init__(
140139

141140
# Time to checkout the links for geometry information
142141
for link in self.links:
143-
144142
# Add link back to robot object
145143
link._robot = self
146144

@@ -226,7 +224,6 @@ def _sort_links(
226224
# Make sure each link has a name
227225
# ------------------------------
228226
for k, link in enumerate(links):
229-
230227
if not isinstance(link, BaseLink):
231228
raise TypeError("links should all be Link subclass")
232229

@@ -253,7 +250,6 @@ def _sort_links(
253250
link.parent = self._linkdict[link.parent_name]
254251

255252
if all([link.parent is None for link in links]):
256-
257253
# No parent links were given, assume they are sequential
258254
for i in range(len(links) - 1):
259255
# li = links[i]
@@ -336,7 +332,6 @@ def _sort_links(
336332
# Assign the joint indices and sort the links
337333
# -------------------------------------------
338334
if all([link.jindex is None or link.ets._auto_jindex for link in links]):
339-
340335
# No joints have an index
341336
jindex = [0] # "mutable integer" hack
342337

@@ -1017,7 +1012,6 @@ def qlim(self) -> NDArray:
10171012
j = 0
10181013

10191014
for link in self.links:
1020-
10211015
if link.isrevolute:
10221016
if link.qlim is None or np.any(np.isnan(link.qlim)):
10231017
v = [-np.pi, np.pi]
@@ -1037,6 +1031,19 @@ def qlim(self) -> NDArray:
10371031

10381032
return limits
10391033

1034+
@qlim.setter
1035+
def qlim(self, new_qlim: ArrayLike):
1036+
new_qlim = np.array(new_qlim)
1037+
1038+
if new_qlim.shape != (2, self.n):
1039+
raise ValueError("new_qlim must be of shape (2, n)")
1040+
1041+
j = 0
1042+
for link in self.links:
1043+
if link.isjoint:
1044+
link.qlim = new_qlim[:, j]
1045+
j += 1
1046+
10401047
@property
10411048
def structure(self) -> str:
10421049
"""
@@ -1244,7 +1251,6 @@ def base(self) -> SE3:
12441251

12451252
@base.setter
12461253
def base(self, T: Union[NDArray, SE3]):
1247-
12481254
if isinstance(self, rtb.Robot):
12491255
# All 3D robots
12501256
# Set the SceneNode T
@@ -1293,7 +1299,6 @@ def search(
12931299
explored: Set[Union[LinkType, Link]],
12941300
path: List[Union[LinkType, Link]],
12951301
) -> Union[List[Union[LinkType, Link]], None]:
1296-
12971302
link = self._getlink(start, self.base_link)
12981303
end = self._getlink(end, self.ee_links[0])
12991304

@@ -1499,7 +1504,6 @@ def _get_limit_links(
14991504

15001505
tool = None
15011506
if end is None:
1502-
15031507
if len(self.grippers) > 1:
15041508
end_ret = self.grippers[0].links[0]
15051509
tool = self.grippers[0].tool
@@ -1645,7 +1649,6 @@ def copy(self):
16451649
return deepcopy(self)
16461650

16471651
def __deepcopy__(self, memo):
1648-
16491652
links = []
16501653

16511654
# if isinstance(self, rtb.DHRobot):
@@ -1945,7 +1948,6 @@ def configurations_str(self, border="thin"):
19451948

19461949
# TODO: factor this out of DHRobot
19471950
def angle(theta, fmt=None):
1948-
19491951
if fmt is not None:
19501952
try:
19511953
return fmt.format(theta * deg) + "\u00b0"
@@ -2120,7 +2122,6 @@ def segments(self) -> List[List[Union[LinkType, None]]]:
21202122
"""
21212123

21222124
def recurse(link: Link):
2123-
21242125
segs = [link.parent]
21252126
while True:
21262127
segs.append(link)
@@ -2168,7 +2169,6 @@ def get_link_scene_node():
21682169
def _get_graphical_backend(
21692170
self, backend: Union[L["swift", "pyplot", "pyplot2"], None] = None
21702171
) -> Union[Swift, PyPlot, PyPlot2]:
2171-
21722172
default = self.default_backend
21732173

21742174
# figure out the right default

‎roboticstoolbox/robot/ETS.py

Lines changed: 25 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -284,17 +284,22 @@ def jindices(self) -> NDArray:
284284

285285
return np.array([j.jindex for j in self.joints()]) # type: ignore
286286

287-
@c_property
287+
@property
288288
def qlim(self):
289289
r"""
290-
Joint limits
290+
Get/Set Joint limits
291291
292292
Limits are extracted from the link objects. If joints limits are
293293
not set for:
294294
295295
- a revolute joint [-𝜋. 𝜋] is returned
296296
- a prismatic joint an exception is raised
297297
298+
Parameters
299+
----------
300+
new_qlim
301+
An ndarray(2, n) of the new joint limits to set
302+
298303
Returns
299304
-------
300305
:return: Array of joint limit values
@@ -333,6 +338,24 @@ def qlim(self):
333338

334339
return limits
335340

341+
@qlim.setter
342+
def qlim(self, new_qlim: ArrayLike):
343+
new_qlim = np.array(new_qlim)
344+
345+
if new_qlim.shape == (2,) and self.n == 1:
346+
new_qlim = new_qlim.reshape(2, 1)
347+
348+
if new_qlim.shape != (2, self.n):
349+
raise ValueError("new_qlim must be of shape (2, n)")
350+
351+
for j, i in enumerate(self.joint_idx()):
352+
et = self[i]
353+
et.qlim = new_qlim[:, j]
354+
355+
self[i] = et
356+
357+
self._update_internals()
358+
336359
@property
337360
def structure(self) -> str:
338361
"""

‎roboticstoolbox/robot/Link.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -578,7 +578,7 @@ def qlim(self) -> Union[NDArray, None]:
578578
@qlim.setter
579579
def qlim(self, qlim_new: ArrayLike):
580580
if self.v:
581-
self.v.qlim = qlim_new
581+
self.ets.qlim = qlim_new
582582
else:
583583
raise ValueError("Can not set qlim on a static joint")
584584

‎tests/test_Robot.py

Lines changed: 35 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -615,13 +615,11 @@ def test_fk_dict(self):
615615
self.assertTrue(len(rdict) > len(rdict2))
616616

617617
def test_URDF(self):
618-
619618
r = rtb.Robot.URDF("fetch_description/robots/fetch.urdf", gripper=6)
620619

621620
self.assertEqual(r.n, 5)
622621

623622
def test_URDF2(self):
624-
625623
r = rtb.Robot.URDF(
626624
"fetch_description/robots/fetch.urdf", gripper="forearm_roll_link"
627625
)
@@ -1422,6 +1420,41 @@ def test_erobot2(self):
14221420
self.assertEqual(robot.manufacturer, "I made it")
14231421
self.assertEqual(robot.comment, "other stuff")
14241422

1423+
def test_qlim_setters(self):
1424+
et = rtb.ET.Rz(qlim=[-1, 1])
1425+
ets = rtb.ETS([et])
1426+
l = rtb.Link(ets)
1427+
r = rtb.Robot([l])
1428+
1429+
nt.assert_almost_equal(et.qlim, np.array([-1, 1]))
1430+
nt.assert_almost_equal(ets.qlim, np.array([[-1, 1]]).T)
1431+
nt.assert_almost_equal(l.qlim, np.array([-1, 1]))
1432+
nt.assert_almost_equal(r.qlim, np.array([[-1, 1]]).T)
1433+
1434+
et.qlim = [-2, 2]
1435+
nt.assert_almost_equal(et.qlim, np.array([-2, 2]))
1436+
nt.assert_almost_equal(ets.qlim, np.array([[-1, 1]]).T)
1437+
nt.assert_almost_equal(l.qlim, np.array([-1, 1]))
1438+
nt.assert_almost_equal(r.qlim, np.array([[-1, 1]]).T)
1439+
1440+
ets.qlim = np.array([[-2, 2]]).T
1441+
nt.assert_almost_equal(et.qlim, np.array([-2, 2]))
1442+
nt.assert_almost_equal(ets.qlim, np.array([[-2, 2]]).T)
1443+
nt.assert_almost_equal(l.qlim, np.array([-2, 2]))
1444+
nt.assert_almost_equal(r.qlim, np.array([[-2, 2]]).T)
1445+
1446+
l.qlim = [-3, 3]
1447+
nt.assert_almost_equal(et.qlim, np.array([-2, 2]))
1448+
nt.assert_almost_equal(ets.qlim, np.array([[-3, 3]]).T)
1449+
nt.assert_almost_equal(l.qlim, np.array([-3, 3]))
1450+
nt.assert_almost_equal(r.qlim, np.array([[-3, 3]]).T)
1451+
1452+
r.qlim = np.array([[-4, 4]]).T
1453+
nt.assert_almost_equal(et.qlim, np.array([-2, 2]))
1454+
nt.assert_almost_equal(ets.qlim, np.array([[-4, 4]]).T)
1455+
nt.assert_almost_equal(l.qlim, np.array([-4, 4]))
1456+
nt.assert_almost_equal(r.qlim, np.array([[-4, 4]]).T)
1457+
14251458

14261459
if __name__ == "__main__": # pragma nocover
14271460
unittest.main()

0 commit comments

Comments
 (0)