Repository navigation
Commit bebede0
fix(rne): attribute a sandwiched static link's mass to its preceding joint, not the following one (#693)
Robot.rne()'s link-grouping attached a static (fixed) link to the first
joint encountered scanning *forward* through the link list -- correct for
a trailing run of static links after the last joint (#636), but wrong for
a static link sandwiched between two joints: it's rigidly welded to the
*preceding* joint's output, not the following one, so its mass was being
misattributed to the wrong joint's torque.
Restructures the grouping to walk each static link's .parent chain up to
its nearest joint ancestor (correct under branching too, not just a flat
list scan), which also lets group[0] be unconditionally the joint --
simplifying the forward/backward recursion's joint lookups and the
kinematic-transform construction that #636's fix had to special-case.
test_invdyn_static2's previous expected values were themselves wrong,
copied from test_invdyn's different topology (where the second link
*does* rotate with the second joint) rather than derived for its own
(where the fixed link's position is independent of the second joint's
angle, so that joint's torque, Coriolis terms, and inertia-matrix
row/column must all be exactly zero) -- so the bug was being actively
protected by a passing test rather than caught by one. Rederived and
verified against an independent virtual-work (finite-difference of CoM
height vs. q) ground truth, not just against rne() itself. Also adds a
dedicated regression test (test_invdyn_sandwiched_static_link, +inertia
variant) isolating the exact scenario from #483.
Fixes #483.
Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>1 parent a0ef01c commit bebede0
2 files changed
Lines changed: 149 additions & 106 deletions
| Original file line number | Diff line number | Diff line change | |
|---|---|---|---|
| |||
1792 | 1792 | | |
1793 | 1793 | | |
1794 | 1794 | | |
1795 | | - | |
1796 | | - | |
1797 | | - | |
| 1795 | + | |
| 1796 | + | |
| 1797 | + | |
| 1798 | + | |
| 1799 | + | |
| 1800 | + | |
| 1801 | + | |
| 1802 | + | |
| 1803 | + | |
| 1804 | + | |
| 1805 | + | |
| 1806 | + | |
| 1807 | + | |
| 1808 | + | |
| 1809 | + | |
| 1810 | + | |
| 1811 | + | |
| 1812 | + | |
| 1813 | + | |
| 1814 | + | |
| 1815 | + | |
1798 | 1816 | | |
1799 | | - | |
1800 | | - | |
1801 | | - | |
1802 | 1817 | | |
1803 | | - | |
1804 | | - | |
1805 | | - | |
1806 | | - | |
1807 | | - | |
1808 | | - | |
1809 | | - | |
1810 | | - | |
1811 | | - | |
1812 | | - | |
1813 | | - | |
1814 | | - | |
1815 | | - | |
1816 | | - | |
| 1818 | + | |
| 1819 | + | |
| 1820 | + | |
| 1821 | + | |
| 1822 | + | |
| 1823 | + | |
| 1824 | + | |
1817 | 1825 | | |
1818 | 1826 | | |
1819 | 1827 | | |
1820 | 1828 | | |
1821 | 1829 | | |
1822 | | - | |
1823 | | - | |
1824 | | - | |
1825 | | - | |
1826 | | - | |
1827 | | - | |
1828 | | - | |
1829 | | - | |
1830 | | - | |
1831 | | - | |
1832 | | - | |
1833 | | - | |
1834 | | - | |
| 1830 | + | |
| 1831 | + | |
| 1832 | + | |
| 1833 | + | |
| 1834 | + | |
| 1835 | + | |
| 1836 | + | |
| 1837 | + | |
| 1838 | + | |
| 1839 | + | |
1835 | 1840 | | |
1836 | 1841 | | |
1837 | 1842 | | |
| |||
1876 | 1881 | | |
1877 | 1882 | | |
1878 | 1883 | | |
1879 | | - | |
1880 | | - | |
| 1884 | + | |
| 1885 | + | |
1881 | 1886 | | |
1882 | 1887 | | |
1883 | 1888 | | |
| |||
1886 | 1891 | | |
1887 | 1892 | | |
1888 | 1893 | | |
1889 | | - | |
1890 | | - | |
1891 | | - | |
1892 | | - | |
1893 | | - | |
| 1894 | + | |
| 1895 | + | |
| 1896 | + | |
| 1897 | + | |
| 1898 | + | |
| 1899 | + | |
| 1900 | + | |
| 1901 | + | |
1894 | 1902 | | |
1895 | 1903 | | |
1896 | 1904 | | |
1897 | 1905 | | |
1898 | | - | |
1899 | | - | |
1900 | | - | |
1901 | | - | |
1902 | | - | |
1903 | | - | |
1904 | | - | |
1905 | | - | |
1906 | | - | |
1907 | | - | |
1908 | | - | |
1909 | | - | |
1910 | | - | |
1911 | | - | |
1912 | | - | |
1913 | | - | |
1914 | | - | |
1915 | | - | |
1916 | | - | |
1917 | | - | |
1918 | | - | |
1919 | | - | |
1920 | | - | |
1921 | | - | |
1922 | | - | |
| 1906 | + | |
| 1907 | + | |
| 1908 | + | |
| 1909 | + | |
| 1910 | + | |
| 1911 | + | |
1923 | 1912 | | |
1924 | 1913 | | |
1925 | | - | |
1926 | | - | |
1927 | | - | |
1928 | | - | |
| 1914 | + | |
1929 | 1915 | | |
1930 | 1916 | | |
1931 | 1917 | | |
1932 | | - | |
1933 | | - | |
| 1918 | + | |
| 1919 | + | |
1934 | 1920 | | |
1935 | 1921 | | |
1936 | 1922 | | |
| |||
1949 | 1935 | | |
1950 | 1936 | | |
1951 | 1937 | | |
1952 | | - | |
1953 | | - | |
1954 | | - | |
| 1938 | + | |
1955 | 1939 | | |
1956 | 1940 | | |
1957 | 1941 | | |
| |||
1966 | 1950 | | |
1967 | 1951 | | |
1968 | 1952 | | |
1969 | | - | |
1970 | | - | |
1971 | | - | |
| 1953 | + | |
| 1954 | + | |
| 1955 | + | |
1972 | 1956 | | |
1973 | 1957 | | |
1974 | 1958 | | |
| |||
| Original file line number | Diff line number | Diff line change | |
|---|---|---|---|
| |||
214 | 214 | | |
215 | 215 | | |
216 | 216 | | |
217 | | - | |
218 | | - | |
| 217 | + | |
| 218 | + | |
| 219 | + | |
| 220 | + | |
| 221 | + | |
| 222 | + | |
| 223 | + | |
| 224 | + | |
| 225 | + | |
| 226 | + | |
| 227 | + | |
| 228 | + | |
| 229 | + | |
| 230 | + | |
| 231 | + | |
| 232 | + | |
| 233 | + | |
219 | 234 | | |
220 | 235 | | |
221 | 236 | | |
222 | 237 | | |
223 | 238 | | |
224 | 239 | | |
225 | | - | |
| 240 | + | |
226 | 241 | | |
227 | | - | |
| 242 | + | |
228 | 243 | | |
229 | 244 | | |
230 | | - | |
| 245 | + | |
231 | 246 | | |
232 | 247 | | |
233 | | - | |
| 248 | + | |
234 | 249 | | |
235 | 250 | | |
236 | 251 | | |
237 | 252 | | |
238 | | - | |
| 253 | + | |
| 254 | + | |
| 255 | + | |
239 | 256 | | |
240 | 257 | | |
241 | | - | |
242 | | - | |
243 | | - | |
244 | | - | |
245 | 258 | | |
246 | | - | |
247 | | - | |
248 | | - | |
249 | | - | |
250 | | - | |
251 | | - | |
252 | | - | |
253 | | - | |
254 | | - | |
255 | | - | |
256 | | - | |
257 | | - | |
258 | | - | |
259 | | - | |
260 | | - | |
| 259 | + | |
| 260 | + | |
| 261 | + | |
261 | 262 | | |
| 263 | + | |
262 | 264 | | |
263 | 265 | | |
264 | 266 | | |
265 | 267 | | |
266 | | - | |
| 268 | + | |
267 | 269 | | |
268 | 270 | | |
269 | | - | |
| 271 | + | |
270 | 272 | | |
271 | 273 | | |
272 | | - | |
| 274 | + | |
| 275 | + | |
| 276 | + | |
| 277 | + | |
| 278 | + | |
| 279 | + | |
| 280 | + | |
| 281 | + | |
| 282 | + | |
| 283 | + | |
| 284 | + | |
| 285 | + | |
| 286 | + | |
| 287 | + | |
| 288 | + | |
| 289 | + | |
| 290 | + | |
| 291 | + | |
| 292 | + | |
| 293 | + | |
| 294 | + | |
| 295 | + | |
| 296 | + | |
| 297 | + | |
| 298 | + | |
| 299 | + | |
| 300 | + | |
| 301 | + | |
| 302 | + | |
| 303 | + | |
| 304 | + | |
| 305 | + | |
| 306 | + | |
| 307 | + | |
| 308 | + | |
| 309 | + | |
| 310 | + | |
| 311 | + | |
| 312 | + | |
| 313 | + | |
| 314 | + | |
| 315 | + | |
| 316 | + | |
| 317 | + | |
| 318 | + | |
| 319 | + | |
| 320 | + | |
| 321 | + | |
| 322 | + | |
| 323 | + | |
| 324 | + | |
| 325 | + | |
| 326 | + | |
| 327 | + | |
| 328 | + | |
| 329 | + | |
| 330 | + | |
| 331 | + | |
273 | 332 | | |
274 | 333 | | |
275 | 334 | | |
| |||
0 commit comments