From 036c0d890ed74434a25c84fa33935913bf022c65 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Sat, 10 Oct 2026 15:58:47 +0800 Subject: [PATCH 1/4] fix(robot): relink parent graph when copying robots --- src/roboticstoolbox/robot/BaseRobot.py | 9 +++++++- tests/test_BaseRobot.py | 31 ++++++++++++++++++++++++++ 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index b732f5c50..c0be91dfc 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -1528,7 +1528,14 @@ def __deepcopy__(self, memo): cls = rtb.Robot for link in self.links: - links.append(deepcopy(link)) + cloned = deepcopy(link) + # Rebuild parent/child references within the copied robot, rather + # than retaining links from the source robot. + cloned._children = [] + if cloned.parent is not None: + cloned._parent_name = cloned.parent.name + cloned._parent = None + links.append(cloned) name = deepcopy(self.name) manufacturer = deepcopy(self.manufacturer) diff --git a/tests/test_BaseRobot.py b/tests/test_BaseRobot.py index 192b5dc9a..e9108309b 100644 --- a/tests/test_BaseRobot.py +++ b/tests/test_BaseRobot.py @@ -589,6 +589,37 @@ def test_copy3(self): self.assertTrue(r2.links[0] != r.links[0]) + def test_copy_preserves_parent_graph_and_dynamics(self): + # A fixed link between two joints exercises the parent references + # used by both the forward and backward RNE recursions. + base = Link(ET.Rz(), name="joint0", m=1.0, r=[0.1, 0, 0], I=[0.02] * 3) + fixed = Link( + ET.tx(0.25), name="fixed", parent=base, + m=0.3, r=[0.05, 0, 0], I=[0.01] * 3, + ) + tip = Link( + ET.Ry(), name="joint1", parent=fixed, + m=0.6, r=[0.1, 0, 0], I=[0.02] * 3, + ) + robot = Robot([base, fixed, tip]) + q = np.array([0.2, -0.3]) + qd = np.array([0.1, 0.2]) + qdd = np.array([0.3, -0.1]) + + expected_tau = robot.rne(q, qd, qdd) + clone = robot.copy() + + self.assertIsNot(clone.links[0], robot.links[0]) + self.assertIsNone(clone.links[0].parent) + self.assertIs(clone.links[1].parent, clone.links[0]) + self.assertIs(clone.links[2].parent, clone.links[1]) + self.assertEqual(clone.links[0]._children, [clone.links[1]]) + self.assertEqual(clone.links[1]._children, [clone.links[2]]) + self.assertEqual(robot.links[0]._children, [robot.links[1]]) + self.assertEqual(robot.links[1]._children, [robot.links[2]]) + nt.assert_allclose(clone.rne(q, qd, qdd), expected_tau) + nt.assert_allclose(clone.coriolis(q, qd), robot.coriolis(q, qd)) + def test_toradians(self): r = rtb.models.Panda() From fbc9474d8664bc646f8f4a54ecf6df83a1e08297 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Sat, 10 Oct 2026 16:02:35 +0800 Subject: [PATCH 2/4] fix(dynamics): preserve copied robot links when removing friction --- src/roboticstoolbox/robot/Dynamics.py | 9 +++++++-- tests/test_BaseRobot.py | 5 +++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/roboticstoolbox/robot/Dynamics.py b/src/roboticstoolbox/robot/Dynamics.py index 169dfecd3..2708cf681 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/src/roboticstoolbox/robot/Dynamics.py @@ -179,8 +179,13 @@ def nofriction(self: RobotProto, coulomb: bool = True, viscous: bool = False): nf = self.copy() nf.name = "NF/" + self.name - # add the modified links (copies) - nf._links = [link.nofriction(coulomb, viscous) for link in self.links] + # Modify the already-copied links in place. Replacing _links with + # independent copies would break their parent/child relationships. + for link in nf.links: + if viscous: + link.B = 0.0 + if coulomb: + link.Tc = [0.0, 0.0] return nf diff --git a/tests/test_BaseRobot.py b/tests/test_BaseRobot.py index e9108309b..3d4753e7f 100644 --- a/tests/test_BaseRobot.py +++ b/tests/test_BaseRobot.py @@ -618,6 +618,11 @@ def test_copy_preserves_parent_graph_and_dynamics(self): self.assertEqual(robot.links[0]._children, [robot.links[1]]) self.assertEqual(robot.links[1]._children, [robot.links[2]]) nt.assert_allclose(clone.rne(q, qd, qdd), expected_tau) + + frictionless = robot.nofriction(coulomb=True, viscous=True) + self.assertIs(frictionless.links[1].parent, frictionless.links[0]) + self.assertIs(frictionless.links[2].parent, frictionless.links[1]) + nt.assert_allclose(frictionless.rne(q, qd, qdd), expected_tau) nt.assert_allclose(clone.coriolis(q, qd), robot.coriolis(q, qd)) def test_toradians(self): From 328a616b2bbbd3f244cd003ab8875cba636e83dc Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 22 Oct 2026 06:24:36 -0400 Subject: [PATCH 3/4] fix(robot): deepcopy linked graphs and retain grippers Use one deepcopy memo for all arm and gripper links. Register each link before copying its parent, leaving the child graph to be reconstructed by the robot constructor. Restore the gripper tool and joint state. Preserve nofriction topology while delegating coefficient changes to Link.nofriction. Add a Panda gripper and FK regression test. Co-Authored-By: ChatGPT (GPT-6) --- src/roboticstoolbox/robot/BaseRobot.py | 26 +++++++++--------- src/roboticstoolbox/robot/Dynamics.py | 9 +++--- src/roboticstoolbox/robot/Link.py | 38 +++++++++++++------------- tests/test_BaseRobot.py | 20 ++++++++++++++ 4 files changed, 57 insertions(+), 36 deletions(-) diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index c0be91dfc..d998cf928 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -1518,25 +1518,18 @@ def copy(self): return deepcopy(self) def __deepcopy__(self, memo): - links = [] + # All links share one memo so parent references point to the same copies. + links = [deepcopy(link, memo) for link in self.links] + gripper_links = [] + for gripper in self.grippers: + gripper_links.append(deepcopy(gripper.links[0], memo)) + links.extend(deepcopy(link, memo) for link in gripper.links) - # if isinstance(self, rtb.DHRobot): - # cls = rtb.DHRobot if isinstance(self, rtb.Robot2): cls = rtb.Robot2 else: cls = rtb.Robot - for link in self.links: - cloned = deepcopy(link) - # Rebuild parent/child references within the copied robot, rather - # than retaining links from the source robot. - cloned._children = [] - if cloned.parent is not None: - cloned._parent_name = cloned.parent.name - cloned._parent = None - links.append(cloned) - name = deepcopy(self.name) manufacturer = deepcopy(self.manufacturer) comment = deepcopy(self.comment) @@ -1549,6 +1542,7 @@ def __deepcopy__(self, memo): result = cls( links, + gripper_links=gripper_links, name=name, manufacturer=manufacturer, comment=comment, @@ -1560,6 +1554,12 @@ def __deepcopy__(self, memo): configs=configs, ) + # Gripper constructor recreates joint indices and defaults; restore + # its original tool and configuration without sharing mutable state. + for original, cloned in zip(self.grippers, result.grippers): + cloned.tool = deepcopy(original.tool, memo) + cloned.q = deepcopy(original.q, memo) + # if a configuration was an attribute of original robot, make it an # attribute of the deep copy for config in configs: diff --git a/src/roboticstoolbox/robot/Dynamics.py b/src/roboticstoolbox/robot/Dynamics.py index 2708cf681..81bd2c5ef 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/src/roboticstoolbox/robot/Dynamics.py @@ -179,13 +179,14 @@ def nofriction(self: RobotProto, coulomb: bool = True, viscous: bool = False): nf = self.copy() nf.name = "NF/" + self.name - # Modify the already-copied links in place. Replacing _links with - # independent copies would break their parent/child relationships. + # Reuse Link.nofriction, but preserve the copies' graph identities. + # Replacing _links with independently cloned links breaks parent links. for link in nf.links: + frictionless = link.nofriction(coulomb, viscous) if viscous: - link.B = 0.0 + link.B = frictionless.B if coulomb: - link.Tc = [0.0, 0.0] + link.Tc = frictionless.Tc return nf diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index ccae7b6be..022a49bd9 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -394,20 +394,21 @@ def _copy(self): raise DeprecationWarning("Use copy method of Link class") def __deepcopy__(self, memo): - ets = deepcopy(self.ets) - name = deepcopy(self.name) - parent = self.parent - joint_name = deepcopy(self._joint_name) - m = deepcopy(self.m) - r = deepcopy(self.r) - I = deepcopy(self.I) - Jm = deepcopy(self.Jm) - B = deepcopy(self.B) - Tc = deepcopy(self.Tc) - G = deepcopy(self.G) - qlim = deepcopy(self.qlim) - qdlim = deepcopy(self.qdlim) - tlim = deepcopy(self.tlim) + ets = deepcopy(self.ets, memo) + name = deepcopy(self.name, memo) + # A detached link can still refer to its parent by name. + parent = self._parent_name if self.parent is None else None + joint_name = deepcopy(self._joint_name, memo) + m = deepcopy(self.m, memo) + r = deepcopy(self.r, memo) + I = deepcopy(self.I, memo) + Jm = deepcopy(self.Jm, memo) + B = deepcopy(self.B, memo) + Tc = deepcopy(self.Tc, memo) + G = deepcopy(self.G, memo) + qlim = deepcopy(self.qlim, memo) + qdlim = deepcopy(self.qdlim, memo) + tlim = deepcopy(self.tlim, memo) geometry = _copy_shapes(self._geometry, self.name) collision = _copy_shapes(self._collision, self.name) @@ -431,12 +432,11 @@ def __deepcopy__(self, memo): collision=collision, # type: ignore ) - if self._children: - result._children = self._children.copy() - - result._robot = self.robot - + # Register first: a link's parent and children form a graph, not a tree. memo[id(self)] = result + if self.parent is not None: + result.parent = deepcopy(self.parent, memo) + # Robot construction rebuilds the children and owning robot references. return result # -------------------------------------------------------------------------- # diff --git a/tests/test_BaseRobot.py b/tests/test_BaseRobot.py index 3d4753e7f..b02510406 100644 --- a/tests/test_BaseRobot.py +++ b/tests/test_BaseRobot.py @@ -625,6 +625,26 @@ def test_copy_preserves_parent_graph_and_dynamics(self): nt.assert_allclose(frictionless.rne(q, qd, qdd), expected_tau) nt.assert_allclose(clone.coriolis(q, qd), robot.coriolis(q, qd)) + def test_copy_preserves_gripper_links_and_fkine(self): + robot = rtb.models.Panda() + self.assertGreater(len(robot.grippers), 0) + gripper = robot.grippers[0] + gripper.q = np.full(gripper.n, 0.01) + gripper.tool = SE3(0, 0, 0.12) + + copied = robot.copy() + self.assertEqual(len(copied.grippers), len(robot.grippers)) + self.assertEqual(copied.grippers[0].name, gripper.name) + self.assertIsNot(copied.grippers[0], gripper) + self.assertIsNot(copied.grippers[0].links[0], gripper.links[0]) + self.assertIs(copied.grippers[0].links[0].parent, copied.ee_links[0]) + nt.assert_allclose(copied.grippers[0].q, gripper.q) + nt.assert_allclose(copied.grippers[0].tool.A, gripper.tool.A) + nt.assert_allclose(copied.fkine(robot.qr).A, robot.fkine(robot.qr).A) + + copied.grippers[0].q[:] = 0 + self.assertTrue(np.all(gripper.q == 0.01)) + def test_toradians(self): r = rtb.models.Panda() From b5fa99bca16f511368f1818c4e695923407df2c2 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 22 Oct 2026 18:41:37 +0800 Subject: [PATCH 4/4] fix(robot): remove redundant type ignores in deepcopy mypy reports three unused-ignore errors after the graph-copy update. Remove only these obsolete annotations; there is no runtime change. Co-Authored-By: ChatGPT (GPT-6) --- src/roboticstoolbox/robot/BaseRobot.py | 2 +- src/roboticstoolbox/robot/Link.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index 86d45ad22..354fa436b 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -1607,7 +1607,7 @@ def __deepcopy__(self, memo: Any) -> Any: name=name, manufacturer=manufacturer, comment=comment, - base=base, # type: ignore + base=base, tool=tool, gravity=gravity, keywords=keywords, diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index 1155bbc24..243acc164 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -452,8 +452,8 @@ def __deepcopy__(self, memo: Any) -> Any: qlim=qlim, qdlim=qdlim, tlim=tlim, - geometry=geometry, # type: ignore - collision=collision, # type: ignore + geometry=geometry, + collision=collision ) # Register first: a link's parent and children form a graph, not a tree.