fix #355 add qlim setters

Author: jhavl

roboticstoolbox/robot/BaseRobot.py

@@ -85,7 +85,6 @@ def __init__(
         configs: Union[Dict[str, NDArray], None] = None,
         check_jindex: bool = True,
     ):
-
         # Initialise the scene node
         SceneNode.__init__(self)
 
@@ -140,7 +139,6 @@ def __init__(
 
         # Time to checkout the links for geometry information
         for link in self.links:
-
             # Add link back to robot object
             link._robot = self
 
@@ -226,7 +224,6 @@ def _sort_links(
         # Make sure each link has a name
         # ------------------------------
         for k, link in enumerate(links):
-
             if not isinstance(link, BaseLink):
                 raise TypeError("links should all be Link subclass")
 
@@ -253,7 +250,6 @@ def _sort_links(
                 link.parent = self._linkdict[link.parent_name]
 
         if all([link.parent is None for link in links]):
-
             # No parent links were given, assume they are sequential
             for i in range(len(links) - 1):
                 # li = links[i]
@@ -336,7 +332,6 @@ def _sort_links(
         # Assign the joint indices and sort the links
         # -------------------------------------------
         if all([link.jindex is None or link.ets._auto_jindex for link in links]):
-
             # No joints have an index
             jindex = [0]  # "mutable integer" hack
 
@@ -1017,7 +1012,6 @@ def qlim(self) -> NDArray:
         j = 0
 
         for link in self.links:
-
             if link.isrevolute:
                 if link.qlim is None or np.any(np.isnan(link.qlim)):
                     v = [-np.pi, np.pi]
@@ -1037,6 +1031,19 @@ def qlim(self) -> NDArray:
 
         return limits
 
+    @qlim.setter
+    def qlim(self, new_qlim: ArrayLike):
+        new_qlim = np.array(new_qlim)
+
+        if new_qlim.shape != (2, self.n):
+            raise ValueError("new_qlim must be of shape (2, n)")
+
+        j = 0
+        for link in self.links:
+            if link.isjoint:
+                link.qlim = new_qlim[:, j]
+                j += 1
+
     @property
     def structure(self) -> str:
         """
@@ -1244,7 +1251,6 @@ def base(self) -> SE3:
 
     @base.setter
     def base(self, T: Union[NDArray, SE3]):
-
         if isinstance(self, rtb.Robot):
             # All 3D robots
             # Set the SceneNode T
@@ -1293,7 +1299,6 @@ def search(
             explored: Set[Union[LinkType, Link]],
             path: List[Union[LinkType, Link]],
         ) -> Union[List[Union[LinkType, Link]], None]:
-
             link = self._getlink(start, self.base_link)
             end = self._getlink(end, self.ee_links[0])
 
@@ -1499,7 +1504,6 @@ def _get_limit_links(
 
         tool = None
         if end is None:
-
             if len(self.grippers) > 1:
                 end_ret = self.grippers[0].links[0]
                 tool = self.grippers[0].tool
@@ -1645,7 +1649,6 @@ def copy(self):
         return deepcopy(self)
 
     def __deepcopy__(self, memo):
-
         links = []
 
         # if isinstance(self, rtb.DHRobot):
@@ -1945,7 +1948,6 @@ def configurations_str(self, border="thin"):
 
         # TODO: factor this out of DHRobot
         def angle(theta, fmt=None):
-
             if fmt is not None:
                 try:
                     return fmt.format(theta * deg) + "\u00b0"
@@ -2120,7 +2122,6 @@ def segments(self) -> List[List[Union[LinkType, None]]]:
         """
 
         def recurse(link: Link):
-
             segs = [link.parent]
             while True:
                 segs.append(link)
@@ -2168,7 +2169,6 @@ def get_link_scene_node():
     def _get_graphical_backend(
         self, backend: Union[L["swift", "pyplot", "pyplot2"], None] = None
     ) -> Union[Swift, PyPlot, PyPlot2]:
-
         default = self.default_backend
 
         # figure out the right default

roboticstoolbox/robot/ETS.py

@@ -284,17 +284,22 @@ def jindices(self) -> NDArray:
 
         return np.array([j.jindex for j in self.joints()])  # type: ignore
 
-    @c_property
+    @property
     def qlim(self):
         r"""
-        Joint limits
+        Get/Set Joint limits
 
         Limits are extracted from the link objects.  If joints limits are
         not set for:
 
         - a revolute joint [-𝜋. 𝜋] is returned
         - a prismatic joint an exception is raised
 
+        Parameters
+        ----------
+        new_qlim
+            An ndarray(2, n) of the new joint limits to set
+
         Returns
         -------
         :return: Array of joint limit values
@@ -333,6 +338,24 @@ def qlim(self):
 
         return limits
 
+    @qlim.setter
+    def qlim(self, new_qlim: ArrayLike):
+        new_qlim = np.array(new_qlim)
+
+        if new_qlim.shape == (2,) and self.n == 1:
+            new_qlim = new_qlim.reshape(2, 1)
+
+        if new_qlim.shape != (2, self.n):
+            raise ValueError("new_qlim must be of shape (2, n)")
+
+        for j, i in enumerate(self.joint_idx()):
+            et = self[i]
+            et.qlim = new_qlim[:, j]
+
+            self[i] = et
+
+        self._update_internals()
+
     @property
     def structure(self) -> str:
         """

roboticstoolbox/robot/Link.py

@@ -578,7 +578,7 @@ def qlim(self) -> Union[NDArray, None]:
     @qlim.setter
     def qlim(self, qlim_new: ArrayLike):
         if self.v:
-            self.v.qlim = qlim_new
+            self.ets.qlim = qlim_new
         else:
             raise ValueError("Can not set qlim on a static joint")
 

tests/test_Robot.py

@@ -615,13 +615,11 @@ def test_fk_dict(self):
         self.assertTrue(len(rdict) > len(rdict2))
 
     def test_URDF(self):
-
         r = rtb.Robot.URDF("fetch_description/robots/fetch.urdf", gripper=6)
 
         self.assertEqual(r.n, 5)
 
     def test_URDF2(self):
-
         r = rtb.Robot.URDF(
             "fetch_description/robots/fetch.urdf", gripper="forearm_roll_link"
         )
@@ -1422,6 +1420,41 @@ def test_erobot2(self):
         self.assertEqual(robot.manufacturer, "I made it")
         self.assertEqual(robot.comment, "other stuff")
 
+    def test_qlim_setters(self):
+        et = rtb.ET.Rz(qlim=[-1, 1])
+        ets = rtb.ETS([et])
+        l = rtb.Link(ets)
+        r = rtb.Robot([l])
+
+        nt.assert_almost_equal(et.qlim, np.array([-1, 1]))
+        nt.assert_almost_equal(ets.qlim, np.array([[-1, 1]]).T)
+        nt.assert_almost_equal(l.qlim, np.array([-1, 1]))
+        nt.assert_almost_equal(r.qlim, np.array([[-1, 1]]).T)
+
+        et.qlim = [-2, 2]
+        nt.assert_almost_equal(et.qlim, np.array([-2, 2]))
+        nt.assert_almost_equal(ets.qlim, np.array([[-1, 1]]).T)
+        nt.assert_almost_equal(l.qlim, np.array([-1, 1]))
+        nt.assert_almost_equal(r.qlim, np.array([[-1, 1]]).T)
+
+        ets.qlim = np.array([[-2, 2]]).T
+        nt.assert_almost_equal(et.qlim, np.array([-2, 2]))
+        nt.assert_almost_equal(ets.qlim, np.array([[-2, 2]]).T)
+        nt.assert_almost_equal(l.qlim, np.array([-2, 2]))
+        nt.assert_almost_equal(r.qlim, np.array([[-2, 2]]).T)
+
+        l.qlim = [-3, 3]
+        nt.assert_almost_equal(et.qlim, np.array([-2, 2]))
+        nt.assert_almost_equal(ets.qlim, np.array([[-3, 3]]).T)
+        nt.assert_almost_equal(l.qlim, np.array([-3, 3]))
+        nt.assert_almost_equal(r.qlim, np.array([[-3, 3]]).T)
+
+        r.qlim = np.array([[-4, 4]]).T
+        nt.assert_almost_equal(et.qlim, np.array([-2, 2]))
+        nt.assert_almost_equal(ets.qlim, np.array([[-4, 4]]).T)
+        nt.assert_almost_equal(l.qlim, np.array([-4, 4]))
+        nt.assert_almost_equal(r.qlim, np.array([[-4, 4]]).T)
+
 
 if __name__ == "__main__":  # pragma nocover
     unittest.main()