fix(robot-model): improve test coverage for ik

.devcontainer/devcontainer.json

@@ -1,11 +1,14 @@
 {
     "name": "control libraries",
     "containerUser": "ubuntu",
-    "build": {
-        "dockerfile": "../Dockerfile",
-        "context": "..",
-        "target": "development"
-    },
+    "initializeCommand": [
+        "docker",
+        "build",
+        "--target=development",
+        "--tag=control-libraries:development",
+        "."
+    ],
+    "image": "control-libraries:development",
     "workspaceMount": "source=${localWorkspaceFolder},target=/src,type=bind,consistency=cached",
     "workspaceFolder": "/src",
     "customizations": {

CHANGELOG.md

@@ -16,6 +16,10 @@ Release Versions
 - [6.3.0](#630)
 - [6.2.0](#620)
 
+## Upcoming changes
+
+- fix(robot-model): improve ik performance (#205)
+
 ## 9.0.1
 
 Version 9.0.1 is a minor version update that exposes Cartesian and Joint state utility functions from the 

Dockerfile

@@ -1,5 +1,5 @@
 ARG BASE_TAG=24.04
-FROM ubuntu:${BASE_TAG} as base
+FROM ubuntu:${BASE_TAG} AS base
 ENV DEBIAN_FRONTEND=noninteractive
 
 RUN apt-get update && apt-get install -y \
@@ -20,7 +20,7 @@ RUN mkdir -p /root/.ssh/ && ssh-keyscan github.com | tee -a /root/.ssh/known_hos
 
 ARG CMAKE_BUILD_TYPE=Release
 
-FROM base as apt-dependencies
+FROM base AS apt-dependencies
 COPY apt-packages.tx[t] /
 
 RUN <<HEREDOC
@@ -77,14 +77,14 @@ xargs -a /tmp/new-packages.txt dpkg-query -L \
 # this root can then be copied to / to install everything globally or use LD_LIBRARY_PATH to use it locally
 HEREDOC
 
-FROM base as base-dependencies
+FROM base AS base-dependencies
 ARG TARGETPLATFORM
 ARG CACHEID
 COPY dependencies/base_dependencies.cmake CMakeLists.txt
 RUN --mount=type=cache,target=/build,id=cmake-base-deps-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   cmake -B build -DBUILD_TESTING=OFF -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} && cmake --build build && cmake --install build --prefix /tmp/deps
 
-FROM base as pinocchio-dependencies
+FROM base AS pinocchio-dependencies
 COPY --from=apt-dependencies /tmp/apt /
 COPY --from=base-dependencies /tmp/deps /usr
 ARG TARGETPLATFORM
@@ -95,8 +95,8 @@ ARG HPP_FCL_TAG=v2.4.4
 #  * `pinocchio` doesn't provide an include directory we can easily plug into `target_include_directories` and thus needs to be installed first
 #  * `pinocchio` uses hacks relying on undocumented CMake quirks which break if you use `FetchContent`
 # FIXME: it needs `CMAKE_INSTALL_PREFIX` and `--prefix` because it doesn't install to the right place otherwise
-RUN --mount=type=cache,target=/pinocchio,id=cmake-pinocchio-src-${PINOCCHIO_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
-  --mount=type=cache,target=/hpp-fcl,id=cmake-hpp-fcl-src-${HPP_FCL_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
+RUN --mount=type=cache,target=/hpp-fcl,id=cmake-hpp-fcl-src-${HPP_FCL_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
+  --mount=type=cache,target=/pinocchio,id=cmake-pinocchio-src-${PINOCCHIO_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   --mount=type=cache,target=/build,id=cmake-pinocchio-${PINOCCHIO_TAG}-${HPP_FCL_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
 <<EOF
 set -e
@@ -121,7 +121,7 @@ cmake --build build/pinocchio --target all install
 find /tmp/deps -type f -exec sed -i 's#/tmp/deps#/usr#g' '{}' \;
 EOF
 
-FROM base as dependencies
+FROM base AS dependencies
 ARG TARGETPLATFORM
 ARG CACHEID
 # Needed to build `osqp-eigen`
@@ -134,11 +134,11 @@ RUN --mount=type=cache,target=/build,id=cmake-deps-${TARGETPLATFORM}-${CACHEID},
 COPY --from=base-dependencies /tmp/deps /tmp/deps
 COPY --from=pinocchio-dependencies /tmp/deps /tmp/deps
 
-FROM base as code
+FROM base AS code
 COPY --from=apt-dependencies /tmp/apt /
 COPY --from=dependencies /tmp/deps /usr
 
-FROM code as development
+FROM code AS development
 
 RUN usermod -a -G dialout ubuntu
 RUN echo "ubuntu ALL=(ALL) NOPASSWD: ALL" > /etc/sudoers.d/99_aptget
@@ -164,7 +164,7 @@ USER ${USER}
 WORKDIR /src
 COPY --chown=${USER}:${USER} . .
 
-FROM code as build
+FROM code AS build
 ARG TARGETPLATFORM
 ARG CACHEID
 COPY licenses licenses
@@ -174,19 +174,19 @@ COPY CMakeLists.txt CMakeLists.txt
 RUN --mount=type=cache,target=/build,id=cmake-build-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   cmake -B build -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} && cmake --build build
 
-FROM build as cpp-test
+FROM build AS cpp-test
 ARG TARGETPLATFORM
 ARG CACHEID
 RUN --mount=type=cache,target=/build,id=cmake-build-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   cmake -B build -DBUILD_TESTING=ON && make -C build && CTEST_OUTPUT_ON_FAILURE=1 make -C build test
 
-FROM build as install
+FROM build AS install
 ARG TARGETPLATFORM
 ARG CACHEID
 RUN --mount=type=cache,target=/build,id=cmake-build-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   cmake --install build --prefix /tmp/cl
 
-FROM code as python
+FROM code AS python
 ARG TARGETPLATFORM
 ARG CACHEID
 COPY --from=install /tmp/cl /usr
@@ -197,14 +197,14 @@ RUN --mount=type=cache,target=/.cache,id=pip-${TARGETPLATFORM}-${CACHEID},uid=10
   python3 -m pip install --prefix=/tmp/python /python
 RUN mv /tmp/python/local /tmp/python-usr
 
-FROM cpp-test as python-test
+FROM cpp-test AS python-test
 RUN pip install pytest --break-system-packages
 COPY --from=install /tmp/cl /usr
 COPY --from=python /tmp/python-usr /usr
 COPY ./python/test /test
 RUN pytest /test
 
-FROM code as python-stubs
+FROM code AS python-stubs
 ARG TARGETPLATFORM
 ARG CACHEID
 COPY --from=install /tmp/cl /usr
@@ -239,7 +239,7 @@ done
 HEREDOC
 RUN mv /tmp/python/local /tmp/python-usr
 
-FROM scratch as production
+FROM scratch AS production
 COPY --from=apt-dependencies /tmp/apt /
 COPY --from=dependencies /tmp/deps /usr
 COPY --from=install /tmp/cl /usr

source/robot_model/test/fixtures/ur5e.urdf

@@ -1,63 +1,5 @@
 <?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from /ws/install/ur_robot_driver/share/ur_robot_driver/urdf/ur.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
 <robot name="ur5e">
-  <!--
-    Base UR robot series xacro macro.
-
-    NOTE this is NOT a URDF. It cannot directly be loaded by consumers
-    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
-    (but note that .xacro must still be processed by the xacro command).
-
-    This file models the base kinematic chain of a UR robot, which then gets
-    parameterised by various configuration files to convert it into a UR3(e),
-    UR5(e), UR10(e) or UR16e.
-
-    NOTE the default kinematic parameters (i.e., link lengths, frame locations,
-    offsets, etc) do not correspond to any particular robot. They are defaults
-    only. There WILL be non-zero offsets between the Forward Kinematics results
-    in TF (i.e., robot_state_publisher) and the values reported by the Teach
-    Pendant.
-
-    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
-    parameter MUST point to a .yaml file containing the appropriate values for
-    the targeted robot.
-
-    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
-    described in the readme of that repository to extract the kinematic
-    calibration from the controller and generate the required .yaml file.
-
-    Main author of the migration to yaml configs Ludovic Delval.
-
-    Contributors to previous versions (in no particular order)
-
-     - Denis Stogl
-     - Lovro Ivanov
-     - Felix Messmer
-     - Kelsey Hawkins
-     - Wim Meeussen
-     - Shaun Edwards
-     - Nadia Hammoudeh Garcia
-     - Dave Hershberger
-     - G. vd. Hoorn
-     - Philip Long
-     - Dave Coleman
-     - Miguel Prada
-     - Mathias Luedtke
-     - Marcel Schnirring
-     - Felix von Drigalski
-     - Felix Exner
-     - Jimmy Da Silva
-     - Ajit Krisshna N L
-     - Muhammad Asif Rana
-  -->
-  <!--
-    NOTE the macro defined in this file is NOT part of the public API of this
-          package. Users CANNOT rely on this file being available, or stored in
-          this location. Nor can they rely on the existence of the macro.
-  -->
   <!-- create link fixed to the "world" -->
   <link name="world"/>
   <!-- links -  main serial chain -->