From 4b821d00248cfd3174ce3d6578f5f706a3452f76 Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Thu, 5 Dec 2024 15:56:24 +0100 Subject: [PATCH] fix(robot-model): improve test coverage for ik (#206) --- .devcontainer/devcontainer.json | 13 +- CHANGELOG.md | 4 + Dockerfile | 32 +-- source/robot_model/test/fixtures/ur5e.urdf | 58 ----- source/robot_model/test/fixtures/xarm.urdf | 217 ++++++++++++++++++ .../test/tests/test_kinematics.cpp | 46 +++- 6 files changed, 286 insertions(+), 84 deletions(-) create mode 100644 source/robot_model/test/fixtures/xarm.urdf diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 512230a95..37276a178 100644 --- a/.devcontainer/devcontainer.json +++ b/.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": { diff --git a/CHANGELOG.md b/CHANGELOG.md index ce5e0d9b4..656e2006a 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/Dockerfile b/Dockerfile index a2ef9d1e7..dbd1950ab 100644 --- a/Dockerfile +++ b/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 < /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 diff --git a/source/robot_model/test/fixtures/ur5e.urdf b/source/robot_model/test/fixtures/ur5e.urdf index 35377d74f..92b15d063 100644 --- a/source/robot_model/test/fixtures/ur5e.urdf +++ b/source/robot_model/test/fixtures/ur5e.urdf @@ -1,63 +1,5 @@ - - - - - - diff --git a/source/robot_model/test/fixtures/xarm.urdf b/source/robot_model/test/fixtures/xarm.urdf new file mode 100644 index 000000000..1b7dbc4b9 --- /dev/null +++ b/source/robot_model/test/fixtures/xarm.urdf @@ -0,0 +1,217 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/source/robot_model/test/tests/test_kinematics.cpp b/source/robot_model/test/tests/test_kinematics.cpp index 3edb67118..50ca3633f 100644 --- a/source/robot_model/test/tests/test_kinematics.cpp +++ b/source/robot_model/test/tests/test_kinematics.cpp @@ -4,6 +4,8 @@ #include #include +#include + #include "robot_model/exceptions/InvalidJointStateSizeException.hpp" #include "robot_model/exceptions/FrameNotFoundException.hpp" #include "robot_model/exceptions/InverseKinematicsNotConvergingException.hpp" @@ -280,16 +282,16 @@ TEST_F(RobotModelKinematicsTest, TestClamp) { EXPECT_TRUE(franka->in_range(franka->clamp_in_range(joint_state))); } -TEST_F(RobotModelKinematicsTest, TestInverseKinematics) { - state_representation::JointState config1("robot", franka->get_joint_frames()); - state_representation::JointState config2("robot", franka->get_joint_frames()); - state_representation::JointState config3("robot", franka->get_joint_frames()); +TEST_F(RobotModelKinematicsTest, TestInverseKinematicsPanda) { + state_representation::JointPositions config1("robot", franka->get_joint_frames()); + state_representation::JointPositions config2("robot", franka->get_joint_frames()); + state_representation::JointPositions config3("robot", franka->get_joint_frames()); // Random test configurations config1.set_positions(std::vector{-0.059943, 1.667088, 1.439900, -1.367141, -1.164922, 0.948034, 2.239983}); config2.set_positions(std::vector{2.648782, -0.553976, 0.801067, -2.042097, -1.642935, 2.946476, 1.292717}); config3.set_positions(std::vector{-0.329909, -0.235174, -1.881858, -2.491807, 0.674615, 0.996670, 0.345810}); - std::vector test_configs = {config1, config2, config3}; + std::vector test_configs = {config1, config2, config3}; double tol = 1e-3; std::chrono::nanoseconds dt(static_cast(1e9)); InverseKinematicsParameters param = InverseKinematicsParameters(); @@ -303,6 +305,40 @@ TEST_F(RobotModelKinematicsTest, TestInverseKinematics) { } } +TEST_F(RobotModelKinematicsTest, TestInverseKinematics) { + std::chrono::nanoseconds dt(static_cast(1e9)); + double tol = 1e-3; + InverseKinematicsParameters param = InverseKinematicsParameters(); + param.tolerance = tol; + + std::size_t num_samples = 10000; + for (const auto& urdf : std::vector{"panda_arm.urdf", "ur5e.urdf", "xarm.urdf"}) { + auto robot = std::make_unique("robot", std::string(TEST_FIXTURES) + "panda_arm.urdf"); + state_representation::JointPositions config("robot", robot->get_joint_frames()); + + int success = 0; + std::chrono::time_point> start_time; + std::chrono::duration diff; + double total_time = 0; + for (std::size_t i = 0; i < num_samples; ++i) { + config.set_positions(pinocchio::randomConfiguration(robot->get_pinocchio_model())); + auto reference = franka->forward_kinematics(config); + try { + start_time = std::chrono::system_clock::now(); + robot->inverse_kinematics(reference, param); + diff = std::chrono::system_clock::now() - start_time; + total_time += diff.count(); + ++success; + } catch (const std::exception&) { + continue; + } + } + std::cout << urdf << ": found " << success << " solutions (" << 100.0 * success / num_samples + << "%) with an average of " << total_time / num_samples << " secs per sample" << std::endl; + EXPECT_GT(success, 0.95 * num_samples); + } +} + TEST_F(RobotModelKinematicsTest, TestInverseKinematicsIKDoesNotConverge) { state_representation::JointState config("robot", franka->get_joint_frames()); // Random test configuration