diff --git a/pyproject.toml b/pyproject.toml index 63a8c8887..c8b4b4c47 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -13,7 +13,7 @@ authors = [ dependencies = [ "numpy", - "spatialmath-python>=1.1.16", + "spatialmath-python>=1.1.18", "spatialgeometry>=1.4.0", "pgraph-python", "scipy", diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 35e35303e..c5f12411d 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1928,7 +1928,15 @@ def rne( i for i, group in enumerate(link_groups) if parent_idx in group ][0] - f[group_idx] = f[group_idx] + Xup[j] * f[j] + # Xup[j] is the child<-parent motion transform (v_child = + # Xup[j] * v_parent); propagating a force the other way, + # child->parent, needs the transform in the other + # direction too. spatialmath's SE3 * SpatialForce applies + # the coadjoint of its left operand (since + # spatialmath-python 1.1.18, rai-opensource/ + # spatialmath-python#207), so the un-inverted transform + # (Xup[j].inv(), i.e. parent<-child) is what's needed here. + f[group_idx] = f[group_idx] + Xup[j].inv() * f[j] # The current Q has the length equal to the number of links within the robot # rather than the number of joints. We need to remove the static links diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index 166acb003..0c7eed17b 100644 --- a/tests/test_ERobot.py +++ b/tests/test_ERobot.py @@ -329,7 +329,7 @@ def test_symdyn(self): self.assertEqual( str(Q[0]), - "a1**2*m2*qd0**2*sin(q1)*cos(q1) + a1*qd0*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1))*sin(q1) - a1*(m2*(a1*qd0*qd1*cos(q1) - a1*qdd0*sin(q1) - g*sin(q0)*cos(q1) - g*sin(q1)*cos(q0)) + (qd0 + qd1)*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1)))*sin(q1) - a1*(-a1*m2*qd0*(-qd0 - qd1)*sin(q1) - m2*r2*(qdd0 + qdd1) + m2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1)))*cos(q1) + g*m1*r1*cos(q0) + m1*qdd0*r1**2 + m2*r2**2*(qdd0 + qdd1) - m2*r2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1))", + "a1**2*m2*qd0**2*sin(q1)*cos(q1) + a1*qd0*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1))*sin(q1) + g*m1*r1*cos(q0) + m1*qdd0*r1**2 + m2*r2**2*(qdd0 + qdd1) - m2*r2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1)) - (a1*sin(q1)**2 + a1*cos(q1)**2)*(m2*(a1*qd0*qd1*cos(q1) - a1*qdd0*sin(q1) - g*sin(q0)*cos(q1) - g*sin(q1)*cos(q0)) + (qd0 + qd1)*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1)))*sin(q1) - (a1*sin(q1)**2 + a1*cos(q1)**2)*(-a1*m2*qd0*(-qd0 - qd1)*sin(q1) - m2*r2*(qdd0 + qdd1) + m2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1)))*cos(q1)", ) self.assertEqual( str(Q[1]),