diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index 9c2913481..354fa436b 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -1578,19 +1578,19 @@ def copy(self) -> Any: return deepcopy(self) def __deepcopy__(self, memo: Any) -> Any: - 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 cls: type[BaseRobot[Any]] if isinstance(self, rtb.Robot2): cls = rtb.Robot2 else: cls = rtb.Robot - for link in self.links: - links.append(deepcopy(link)) - name = deepcopy(self.name) manufacturer = deepcopy(self.manufacturer) comment = deepcopy(self.comment) @@ -1603,6 +1603,7 @@ def __deepcopy__(self, memo: Any) -> Any: result = cls( links, + gripper_links=gripper_links, name=name, manufacturer=manufacturer, comment=comment, @@ -1614,6 +1615,12 @@ def __deepcopy__(self, memo: Any) -> Any: 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 dd6ca6950..98718caa4 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/src/roboticstoolbox/robot/Dynamics.py @@ -191,8 +191,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] + # Reuse Link.nofriction, but keep the copied robot's link graph intact. + for link in nf.links: + frictionless = link.nofriction(coulomb, viscous) + if viscous: + link.B = frictionless.B + if coulomb: + link.Tc = frictionless.Tc return nf diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index c052cb6b2..243acc164 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -418,20 +418,21 @@ def _copy(self) -> None: raise DeprecationWarning("Use copy method of Link class") def __deepcopy__(self, memo: Any) -> Any: - 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) @@ -452,15 +453,14 @@ def __deepcopy__(self, memo: Any) -> Any: qdlim=qdlim, tlim=tlim, geometry=geometry, - collision=collision, + collision=collision ) - 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 d610c12d2..8c6e706f8 100644 --- a/tests/test_BaseRobot.py +++ b/tests/test_BaseRobot.py @@ -601,6 +601,62 @@ 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) + + 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_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()