diff --git a/.github/actions/build-push/action.yml b/.github/actions/build-push/action.yml new file mode 100644 index 000000000..9a4fe49da --- /dev/null +++ b/.github/actions/build-push/action.yml @@ -0,0 +1,43 @@ +name: 'Build and Push' +description: 'Build the Docker image and push to GitHub Container Registry' +inputs: + image: + description: 'The image to build.' + required: true + options: + - 'Development dependencies' + - 'Protocol dependencies' + secret: + description: 'GitHub Container Registry secret' + required: true + +runs: + using: "composite" + steps: + - name: Set up QEMU + uses: docker/setup-qemu-action@v1 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v1 + + - name: Checkout repository + uses: actions/checkout@v2 + + - name: Login to GitHub Package Registry + run: echo "${{ inputs.secret }}" | docker login ghcr.io -u ${{ github.actor }} --password-stdin + shell: bash + + - name: Build and push image + run: | + if [ "${{ inputs.image }}" = "Development dependencies" ]; then + DOCKERFILE=Dockerfile.base + IMAGE_NAME=${{ github.repository }}/development-dependencies:latest + else + DOCKERFILE=Dockerfile.proto + IMAGE_NAME=${{ github.repository }}/proto-dependencies + fi + docker buildx build --file "${DOCKERFILE}" \ + --platform=linux/arm64,linux/amd64 \ + --push --tag ghcr.io/${IMAGE_NAME} \ + . + shell: bash diff --git a/.github/actions/build-test-python/entrypoint.sh b/.github/actions/build-test-python/entrypoint.sh index 07e427105..fdd182aa4 100755 --- a/.github/actions/build-test-python/entrypoint.sh +++ b/.github/actions/build-test-python/entrypoint.sh @@ -7,7 +7,6 @@ bash /github/workspace/protocol/install.sh --auto || exit 1 echo ">>> Building Python bindings..." export OSQP_INCLUDE_DIR='/usr/local/include/osqp' -export OPENROBOTS_INCLUDE_DIR='/opt/openrobots/include' pip3 install /github/workspace/python || (echo ">>> [ERROR] Build stage failed!" && exit 2) || exit $? echo ">>> Running all test stages..." diff --git a/.github/workflows/build-push.yml b/.github/workflows/build-push.yml index 19eb42335..d416c697f 100644 --- a/.github/workflows/build-push.yml +++ b/.github/workflows/build-push.yml @@ -8,7 +8,6 @@ on: workflow_dispatch: jobs: - build-publish-proto-dependencies: runs-on: ubuntu-latest name: Build and publish proto dependencies image @@ -16,19 +15,11 @@ jobs: - name: Checkout Repository uses: actions/checkout@v2 - - name: Build image - run: | - docker build . --file ./Dockerfile.proto --tag proto-dependencies - - - name: Login to GitHub Container Registry - run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.actor }} --password-stdin - - - name: Push image - run: | - IMAGE_NAME=${{ github.repository }}/proto-dependencies:latest - IMAGE_NAME=${IMAGE_NAME/_/-} - docker tag proto-dependencies ghcr.io/${IMAGE_NAME} - docker push ghcr.io/${IMAGE_NAME} + - name: Build and Push + uses: ./.github/actions/build-push + with: + image: "Protocol dependencies" + secret: ${{ secrets.GITHUB_TOKEN }} build-publish-development-dependencies: needs: build-publish-proto-dependencies @@ -38,16 +29,8 @@ jobs: - name: Checkout Repository uses: actions/checkout@v2 - - name: Build image - run: | - docker build . --file ./Dockerfile.base --tag development-dependencies - - - name: Login to GitHub Container Registry - run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.actor }} --password-stdin - - - name: Push image - run: | - IMAGE_NAME=${{ github.repository }}/development-dependencies:latest - IMAGE_NAME=${IMAGE_NAME/_/-} - docker tag development-dependencies ghcr.io/${IMAGE_NAME} - docker push ghcr.io/${IMAGE_NAME} \ No newline at end of file + - name: Build and Push + uses: ./.github/actions/build-push + with: + image: "Development dependencies" + secret: ${{ secrets.GITHUB_TOKEN }} diff --git a/CHANGELOG.md b/CHANGELOG.md index 08e0656cc..d71a4a1ae 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,7 @@ # CHANGELOG Release Versions: +- [6.2.0](#620) - [6.1.0](#610) - [6.0.0](#600) - [5.2.0](#520) @@ -13,6 +14,34 @@ Release Versions: - [2.0.0](#200) - [1.0.0](#100) +## 6.2.0 + +Version 6.2.0 contains minor changes to the Python bindings and robot model installation dependencies. + +### Features and improvements + +- Refactor frame_name parameters (#309) +- Support ARM64 (#310) +- Remove openrobots directives (#312) + +An inconsistency in nomenclature was resolved; see [issue #308](https://github.com/epfl-lasa/control-libraries/issues/308) +and [PR #309](https://github.com/epfl-lasa/control-libraries/pull/309) for more details. + +Behind the scenes, the docker images for +[development dependencies](https://github.com/orgs/epfl-lasa/packages/container/package/control-libraries%2Fdevelopment-dependencies) +and [proto dependencies](https://github.com/epfl-lasa/control-libraries/pkgs/container/control-libraries%2Fproto-dependencies) +are now built for multiple architectures: `linux/amd64` and `linux/arm64`. + +At the time of release, no pre-built arm64 binaries existed for Pinocchio, a dependency of the **robot_model** library. +For this reason, the installation script is revised to install Pinocchio from source, which has the side effect of +simplifying the CMake configuration; the path inclusion of `/opt/openrobots` is no longer required. + +### Upgrade notes + +The core C++ API is unchanged, and therefore this release is not marked by a major version change. However, some +unintended usages of the Python bindings may be affected as noted in issue #308. Additionally, it may be necessary +to redo the installation steps for the revised dependencies if using the robot model library on a local installation. + ## 6.1.0 Version 6.1.0 contains a few small new features as well as several fixes, mainly in diff --git a/Dockerfile.base b/Dockerfile.base index 2e0efc4b2..e309f99bf 100644 --- a/Dockerfile.base +++ b/Dockerfile.base @@ -10,69 +10,90 @@ RUN apt-get update && apt-get install -y \ curl \ g++ \ gcc \ - gnupg2 \ - make \ git \ - wget \ + gnupg2 \ libtool \ lsb-release \ + make \ + pkg-config \ + wget \ + && apt-get clean \ + && rm -rf /var/lib/apt/lists/* + + +FROM core-build-dependencies as google-dependencies + +RUN apt-get update && apt-get install -y \ + libgtest-dev \ && apt-get clean \ && rm -rf /var/lib/apt/lists/* +# install gtest +WORKDIR /tmp +RUN mkdir gtest_build && cd gtest_build && cmake /usr/src/gtest && make -j \ + && cp lib/* /usr/local/lib || cp *.a /usr/local/lib -FROM core-build-dependencies as project-dependencies -ARG OSQP_TAG=v0.6.2 -ARG OSQP_EIGEN_TAG=v0.6.4 +RUN rm -rf /tmp/* && ldconfig -# add pinocchio to package list -RUN echo "deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \ - | tee /etc/apt/sources.list.d/robotpkg.list \ - && curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key \ - | apt-key add - -RUN wget -c https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz -O - | tar -xz -RUN cd eigen-3.4.0 && mkdir build && cd build && cmake .. && make install -RUN rm -r eigen-3.4.0 +FROM core-build-dependencies as robot-model-dependencies -# install dependencies for building the libraries RUN apt-get update && apt-get install -y \ - robotpkg-pinocchio \ + libboost-all-dev \ + liburdfdom-dev \ && apt-get clean \ && rm -rf /var/lib/apt/lists/* -# install osqp and eigen wrapper -WORKDIR /tmp/osqp_build -RUN git clone --depth 1 -b ${OSQP_TAG} --recursive https://github.com/oxfordcontrol/osqp \ - && cd osqp && mkdir build && cd build && cmake -G "Unix Makefiles" .. && cmake --build . --target install +WORKDIR /tmp +ARG EIGEN_TAG=3.4.0 +RUN wget -c https://gitlab.com/libeigen/eigen/-/archive/${EIGEN_TAG}/eigen-${EIGEN_TAG}.tar.gz -O - | tar -xz \ + && cd eigen-${EIGEN_TAG} && mkdir build && cd build && cmake .. && make install \ + && cd ../.. && rm -r eigen-${EIGEN_TAG} || exit 1 -RUN git clone --depth 1 -b ${OSQP_EIGEN_TAG} https://github.com/robotology/osqp-eigen.git \ - && cd osqp-eigen && mkdir build && cd build && cmake .. && make -j && make install +ARG OSQP_TAG=0.6.2 +RUN git clone --depth 1 -b v${OSQP_TAG} --recursive https://github.com/oxfordcontrol/osqp \ + && cd osqp && mkdir build && cd build && cmake -G "Unix Makefiles" .. && cmake --build . --target install \ + && cd ../.. && rm -r osqp || exit 1 -ENV LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib:/opt/openrobots/lib/ +ARG OSQP_EIGEN_TAG=0.6.4 +RUN git clone --depth 1 -b v${OSQP_EIGEN_TAG} https://github.com/robotology/osqp-eigen.git \ + && cd osqp-eigen && mkdir build && cd build && cmake .. && make -j && make install \ + && cd ../.. && rm -r osqp-eigen || exit 1 -WORKDIR /home -RUN rm -rf /tmp/* +ARG PINOCCHIO_TAG=2.6.9 +RUN git clone --depth 1 -b v${PINOCCHIO_TAG} --recursive https://github.com/stack-of-tasks/pinocchio \ + && cd pinocchio && mkdir build && cd build \ + && cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local -DBUILD_PYTHON_INTERFACE=OFF \ + && make -j1 && make install && cd ../.. && rm -r pinocchio || exit 1 +RUN ldconfig -FROM core-build-dependencies as google-dependencies +FROM robot-model-dependencies as development-dependencies RUN apt-get update && apt-get install -y \ - libgtest-dev \ + clang \ + gdb \ + python3 \ + python3-dev \ + python3-pip \ + tar \ + unzip \ && apt-get clean \ && rm -rf /var/lib/apt/lists/* -# install gtest -WORKDIR /tmp/gtest_build -RUN cmake /usr/src/gtest \ - && make \ - && cp lib/* /usr/local/lib || cp *.a /usr/local/lib - -WORKDIR /home -RUN rm -rf /tmp/* +# install python requirements +RUN pip3 install numpy setuptools pybind11 +# install google dependencies +COPY --from=google-dependencies /usr/include/gtest /usr/include/gtest +COPY --from=google-dependencies /usr/local/lib/libgtest* /usr/local/lib/ +COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/include/google /usr/local/include/google +COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/lib/libproto* /usr/local/lib/ +COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/bin/protoc /usr/local/bin +RUN ldconfig -FROM project-dependencies as ssh-configuration +FROM development-dependencies as ssh-configuration RUN apt-get update && apt-get install -y \ sudo \ libssl-dev \ @@ -93,7 +114,7 @@ RUN ( \ ENV USER developer ENV HOME /home/${USER} -# create amd configure a new user +# create and configure a new user ARG UID=1000 ARG GID=1000 RUN addgroup --gid ${GID} ${USER} @@ -112,27 +133,4 @@ RUN mkdir /root/.ssh/ && ssh-keyscan github.com | tee -a /root/.ssh/known_hosts RUN echo "session required pam_limits.so" | tee --append /etc/pam.d/common-session > /dev/null - -FROM ssh-configuration as development-dependencies - -RUN apt-get update && apt-get install -y \ - clang \ - gdb \ - python3 \ - python3-dev \ - python3-pip \ - tar \ - unzip \ - && apt-get clean \ - && rm -rf /var/lib/apt/lists/* - -# install python requirements -RUN pip3 install numpy setuptools pybind11 - -# install google dependencies -COPY --from=google-dependencies /usr/include/gtest /usr/include/gtest -COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/include/google /usr/local/include/google -COPY --from=google-dependencies /usr/local/lib/libgtest* /usr/local/lib/ -COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/lib/libproto* /usr/local/lib/ -COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/bin/protoc /usr/local/bin -RUN ldconfig +WORKDIR ${HOME} diff --git a/Dockerfile.proto b/Dockerfile.proto index 60aeee26c..6936ae380 100644 --- a/Dockerfile.proto +++ b/Dockerfile.proto @@ -1,5 +1,4 @@ FROM ubuntu:20.04 as build-stage -ARG PROTOBUF_VERSION=3.17.0 ENV DEBIAN_FRONTEND=noninteractive RUN apt-get update && apt-get install -y \ @@ -14,19 +13,20 @@ RUN apt-get update && apt-get install -y \ && rm -rf /var/lib/apt/lists/* WORKDIR /tmp +ARG PROTOBUF_VERSION=21.0 RUN wget -O protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \ - https://github.com/protocolbuffers/protobuf/releases/download/v"${PROTOBUF_VERSION}"/protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \ + https://github.com/protocolbuffers/protobuf/releases/download/v${PROTOBUF_VERSION}/protobuf-cpp-3.${PROTOBUF_VERSION}.tar.gz \ && tar -xzf protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \ && rm protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz -WORKDIR /tmp/protobuf-"${PROTOBUF_VERSION}" +WORKDIR /tmp/protobuf-3."${PROTOBUF_VERSION}" RUN ./autogen.sh \ && ./configure \ && make \ && make install -FROM ubuntu:20.04 as google-dependencies +FROM ubuntu:22.04 as google-dependencies COPY --from=build-stage /usr/local/include/google /usr/local/include/google COPY --from=build-stage /usr/local/lib/libproto* /usr/local/lib/ COPY --from=build-stage /usr/local/bin/protoc /usr/local/bin diff --git a/VERSION b/VERSION index dfda3e0b4..6abaeb2f9 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -6.1.0 +6.2.0 diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index ba4edcc21..c1dccec78 100644 --- a/demos/CMakeLists.txt +++ b/demos/CMakeLists.txt @@ -15,10 +15,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -list(APPEND CMAKE_PREFIX_PATH /opt/openrobots) -include_directories(/opt/openrobots/include) - -find_package(control_libraries 6.1.0 CONFIG REQUIRED) +find_package(control_libraries 6.2.0 CONFIG REQUIRED) set(DEMOS_SCRIPTS task_space_control_loop diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 48736b024..b10983ef6 100644 --- a/doxygen/doxygen.conf +++ b/doxygen/doxygen.conf @@ -38,7 +38,7 @@ PROJECT_NAME = "Control Libraries" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 6.1.0 +PROJECT_NUMBER = 6.2.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt index 56980e2a0..fa29d70fa 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/protocol/clproto_cpp/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(clproto VERSION 6.1.0) +project(clproto VERSION 6.2.0) # Default to C99 if(NOT CMAKE_C_STANDARD) diff --git a/python/Dockerfile.python b/python/Dockerfile.python index d64e60d40..21ab57833 100644 --- a/python/Dockerfile.python +++ b/python/Dockerfile.python @@ -11,7 +11,6 @@ COPY include control-libraries/python/include COPY source control-libraries/python/source COPY pyproject.toml setup.py control-libraries/python/ ENV OSQP_INCLUDE_DIR /usr/local/include/osqp -ENV OPENROBOTS_INCLUDE_DIR /opt/openrobots/include RUN pip3 install control-libraries/python diff --git a/python/README.md b/python/README.md index 16d84d729..188c19b8e 100644 --- a/python/README.md +++ b/python/README.md @@ -26,10 +26,9 @@ pip3 install control-libraries/python ``` If the installation fails, it may be because of non-default installation directories for some dependencies. -In this case, the include path for OSQP and OpenRobots can be set through environment variables before the pip install. +In this case, the include path for OSQP can be set through environment variables before the pip install. ```shell export OSQP_INCLUDE_DIR='/path/to/include/osqp' # default /usr/local/include/osqp -export OPENROBOTS_INCLUDE_DIR='/path/to/openrobots/include' # default /opt/openrobots/include pip3 install control-libraries/python ``` diff --git a/python/setup.py b/python/setup.py index 8feb7e593..fb793314e 100644 --- a/python/setup.py +++ b/python/setup.py @@ -8,9 +8,8 @@ # names of the environment variables that define osqp and openrobots include directories osqp_path_var = 'OSQP_INCLUDE_DIR' -openrobots_path_var = 'OPENROBOTS_INCLUDE_DIR' -__version__ = "6.1.0" +__version__ = "6.2.0" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] @@ -50,9 +49,6 @@ if __install_robot_model_module__: osqp_path = os.environ[osqp_path_var] if osqp_path_var in os.environ.keys() else '/usr/local/include/osqp' __include_dirs__.append(osqp_path) - openrobots_path = os.environ[ - openrobots_path_var] if openrobots_path_var in os.environ.keys() else '/opt/openrobots/include' - __include_dirs__.append('/opt/openrobots/include') if __install_controllers_module__ and not __install_robot_model_module__: warnings.warn( diff --git a/python/source/controllers/bind_cartesian_controllers.cpp b/python/source/controllers/bind_cartesian_controllers.cpp index 74b5583c3..1f4c8196a 100644 --- a/python/source/controllers/bind_cartesian_controllers.cpp +++ b/python/source/controllers/bind_cartesian_controllers.cpp @@ -25,7 +25,7 @@ void cartesian_controller(py::module_& m) { "Compute the command output in joint space from command and feedback states in task space.", "command_state"_a, "feedback_state"_a, "jacobian"_a); c.def( "compute_command", py::overload_cast(&IController::compute_command), - "Compute the command output in joint space from command and feedback states in task space.", "command_state"_a, "feedback_state"_a, "joint_positions"_a, "frame_name"_a = std::string("")); + "Compute the command output in joint space from command and feedback states in task space.", "command_state"_a, "feedback_state"_a, "joint_positions"_a, "frame"_a = std::string("")); c.def("get_robot_model", &IController::get_robot_model, "Get the robot model associated with the controller."); c.def("set_robot_model", &IController::set_robot_model, "Set the robot model associated with the controller.", "robot_model"_a); diff --git a/python/source/robot_model/bind_model.cpp b/python/source/robot_model/bind_model.cpp index b20b0b214..0e0bc72fb 100644 --- a/python/source/robot_model/bind_model.cpp +++ b/python/source/robot_model/bind_model.cpp @@ -46,10 +46,10 @@ void model(py::module_& m) { c.def( "compute_jacobian", py::overload_cast(&Model::compute_jacobian), - "Compute the Jacobian from a given joint state at the frame given in parameter.", "joint_positions"_a, "frame_name"_a = std::string("")); + "Compute the Jacobian from a given joint state at the frame given in parameter.", "joint_positions"_a, "frame"_a = std::string("")); c.def( "compute_jacobian_time_derivative", py::overload_cast(&Model::compute_jacobian_time_derivative), - "Compute the time derivative of the Jacobian from given joint positions and velocities at the frame in parameter.", "joint_positions"_a, "joint_velocities"_a, "frame_name"_a = std::string("")); + "Compute the time derivative of the Jacobian from given joint positions and velocities at the frame in parameter.", "joint_positions"_a, "joint_velocities"_a, "frame"_a = std::string("")); c.def("compute_inertia_matrix", py::overload_cast(&Model::compute_inertia_matrix), "Compute the Inertia matrix from given joint positions.", "joint_positions"_a); c.def( "compute_inertia_torques", py::overload_cast(&Model::compute_inertia_torques), @@ -61,28 +61,28 @@ void model(py::module_& m) { c.def("compute_gravity_torques", py::overload_cast(&Model::compute_gravity_torques), "Compute the gravity torques.", "joint_positions"_a); c.def("forward_kinematics", py::overload_cast&>(&Model::forward_kinematics), - "Compute the forward kinematics, i.e. the pose of certain frames from the joint positions", "joint_positions"_a, "frame_names"_a); + "Compute the forward kinematics, i.e. the pose of certain frames from the joint positions", "joint_positions"_a, "frames"_a); c.def("forward_kinematics", py::overload_cast(&Model::forward_kinematics), - "Compute the forward kinematics, i.e. the pose of the frame from the joint positions", "joint_positions"_a, "frame_name"_a = std::string("")); + "Compute the forward kinematics, i.e. the pose of the frame from the joint positions", "joint_positions"_a, "frame"_a = std::string("")); c.def("inverse_kinematics", py::overload_cast(&Model::inverse_kinematics), - "Compute the inverse kinematics, i.e. joint positions from the pose of the end-effector in an iterative manner", "cartesian_pose"_a, "parameters"_a = InverseKinematicsParameters(), "frame_name"_a = std::string("")); + "Compute the inverse kinematics, i.e. joint positions from the pose of the end-effector in an iterative manner", "cartesian_pose"_a, "parameters"_a = InverseKinematicsParameters(), "frame"_a = std::string("")); c.def("inverse_kinematics", py::overload_cast(&Model::inverse_kinematics), - " Compute the inverse kinematics, i.e. joint positions from the pose of the end-effector", "cartesian_pose"_a, "joint_positions"_a, "parameters"_a = InverseKinematicsParameters(), "frame_name"_a = std::string("")); + " Compute the inverse kinematics, i.e. joint positions from the pose of the end-effector", "cartesian_pose"_a, "joint_positions"_a, "parameters"_a = InverseKinematicsParameters(), "frame"_a = std::string("")); c.def("forward_velocity", py::overload_cast&>(&Model::forward_velocity), - "Compute the forward velocity kinematics, i.e. the twist of certain frames from the joint states", "joint_state"_a, "frame_names"_a); + "Compute the forward velocity kinematics, i.e. the twist of certain frames from the joint states", "joint_state"_a, "frames"_a); c.def("forward_velocity", py::overload_cast(&Model::forward_velocity), - "Compute the forward velocity kinematics, i.e. the twist of the end-effector from the joint velocities", "joint_state"_a, "frame_name"_a = std::string("")); + "Compute the forward velocity kinematics, i.e. the twist of the end-effector from the joint velocities", "joint_state"_a, "frame"_a = std::string("")); c.def("inverse_velocity", py::overload_cast&, const JointPositions&, const std::vector&>(&Model::inverse_velocity), - "Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter the Jacobian", "cartesian_twists"_a, "joint_positions"_a, "frame_names"_a); + "Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter the Jacobian", "cartesian_twists"_a, "joint_positions"_a, "frames"_a); c.def("inverse_velocity", py::overload_cast(&Model::inverse_velocity), - "Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the Jacobian", "cartesian_twist"_a, "joint_positions"_a, "frame_name"_a = std::string("")); + "Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the Jacobian", "cartesian_twist"_a, "joint_positions"_a, "frame"_a = std::string("")); c.def("inverse_velocity", py::overload_cast&, const JointPositions&, const QPInverseVelocityParameters&, const std::vector&>(&Model::inverse_velocity), - "Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter using the QP optimization method", "cartesian_twists"_a, "joint_positions"_a, "parameters"_a, "frame_names"_a); + "Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter using the QP optimization method", "cartesian_twists"_a, "joint_positions"_a, "parameters"_a, "frames"_a); c.def("inverse_velocity", py::overload_cast(&Model::inverse_velocity), - "Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the QP optimization method", "cartesian_twist"_a, "joint_positions"_a, "parameters"_a, "frame_name"_a = std::string("")); + "Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the QP optimization method", "cartesian_twist"_a, "joint_positions"_a, "parameters"_a, "frame"_a = std::string("")); c.def("print_qp_problem", &Model::print_qp_problem, "Helper function to print the qp problem (for debugging)."); c.def("in_range", [](Model& self, const JointPositions& joint_positions) -> bool { return self.in_range(joint_positions); }, diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp index 6d33c98a8..fb3da9a7e 100644 --- a/python/source/state_representation/bind_cartesian_space.cpp +++ b/python/source/state_representation/bind_cartesian_space.cpp @@ -57,11 +57,11 @@ void cartesian_state(py::module_& m) { py::class_, SpatialState> c(m, "CartesianState"); c.def(py::init(), "Empty constructor"); - c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference_frame"_a=std::string("world")); c.def(py::init(), "Copy constructor of a CartesianState", "state"_a); - c.def_static("Identity", &CartesianState::Identity, "Constructor for the identity CartesianState (identity pose and 0 for the rest)", "name"_a, "reference"_a=std::string("world")); - c.def_static("Random", &CartesianState::Random, "Constructor for a random state", "name"_a, "reference"_a=std::string("world")); + c.def_static("Identity", &CartesianState::Identity, "Constructor for the identity CartesianState (identity pose and 0 for the rest)", "name"_a, "reference_frame"_a=std::string("world")); + c.def_static("Random", &CartesianState::Random, "Constructor for a random state", "name"_a, "reference_frame"_a=std::string("world")); c.def("get_position", &CartesianState::get_position, "Getter of the position attribute"); c.def("get_orientation", [](const CartesianState &state) -> py::object { @@ -175,24 +175,24 @@ void cartesian_pose(py::module_& m) { py::class_, CartesianState> c(m, "CartesianPose"); c.def(py::init(), "Empty constructor"); - c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference_frame"_a=std::string("world")); c.def(py::init(), "Copy constructor of a CartesianPose", "pose"_a); c.def(py::init(), "Copy constructor from a CartesianState", "state"_a); c.def(py::init(), "Copy constructor from a CartesianTwist by considering that it is a displacement over 1 second", "twist"_a); - c.def(py::init(), "Constructor of a CartesianPose from a position given as a vector of coordinates", "name"_a, "position"_a, "reference"_a=std::string("world")); - c.def(py::init(), "Constructor of a CartesianPose from a position given as three scalar coordinates", "name"_a, "x"_a, "y"_a, "z"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Constructor of a CartesianPose from a position given as a vector of coordinates", "name"_a, "position"_a, "reference_frame"_a=std::string("world")); + c.def(py::init(), "Constructor of a CartesianPose from a position given as three scalar coordinates", "name"_a, "x"_a, "y"_a, "z"_a, "reference_frame"_a=std::string("world")); c.def(py::init([](const std::string& name, const Eigen::Vector4d& orientation, const std::string& reference) { Eigen::Quaterniond q(orientation(0), orientation(1), orientation(2), orientation(3)); return new CartesianPose(name, q, reference); - }), "Constructor of a CartesianPose from a quaternion", "name"_a, "orientation"_a, "reference"_a=std::string("world")); + }), "Constructor of a CartesianPose from a quaternion", "name"_a, "orientation"_a, "reference_frame"_a=std::string("world")); c.def(py::init([](const std::string& name, const Eigen::Vector3d& position, const Eigen::Vector4d& orientation, const std::string& reference) { Eigen::Quaterniond q(orientation(0), orientation(1), orientation(2), orientation(3)); return new CartesianPose(name, position, q, reference); - }), "Constructor of a CartesianPose from a position given as a vector of coordinates and a quaternion", "name"_a, "position"_a, "orientation"_a, "reference"_a=std::string("world")); + }), "Constructor of a CartesianPose from a position given as a vector of coordinates and a quaternion", "name"_a, "position"_a, "orientation"_a, "reference_frame"_a=std::string("world")); - c.def_static("Identity", &CartesianPose::Identity, "Constructor for the identity pose", "name"_a, "reference"_a=std::string("world")); - c.def_static("Random", &CartesianPose::Random, "Constructor for a random pose", "name"_a, "reference"_a=std::string("world")); + c.def_static("Identity", &CartesianPose::Identity, "Constructor for the identity pose", "name"_a, "reference_frame"_a=std::string("world")); + c.def_static("Random", &CartesianPose::Random, "Constructor for a random pose", "name"_a, "reference_frame"_a=std::string("world")); std::vector deleted_attributes = { "linear_velocity", @@ -254,17 +254,17 @@ void cartesian_twist(py::module_& m) { py::class_, CartesianState> c(m, "CartesianTwist"); c.def(py::init(), "Empty constructor"); - c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference_frame"_a=std::string("world")); c.def(py::init(), "Copy constructor", "twist"_a); c.def(py::init(), "Copy constructor from a CartesianState", "state"_a); c.def(py::init(), "Copy constructor from a CartesianPose by considering that it is equivalent to dividing the pose by 1 second", "pose"_a); - c.def(py::init(), "Construct a CartesianTwist from a linear velocity given as a vector.", "name"_a, "linear_velocity"_a, "reference"_a=std::string("world")); - c.def(py::init(), "Construct a CartesianTwist from a linear velocity and angular velocity given as vectors.", "name"_a, "linear_velocity"_a, "angular_velocity"_a, "reference"_a=std::string("world")); - c.def(py::init&, const std::string&>(), "Construct a CartesianTwist from a single 6d twist vector", "name"_a, "twist"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Construct a CartesianTwist from a linear velocity given as a vector.", "name"_a, "linear_velocity"_a, "reference_frame"_a=std::string("world")); + c.def(py::init(), "Construct a CartesianTwist from a linear velocity and angular velocity given as vectors.", "name"_a, "linear_velocity"_a, "angular_velocity"_a, "reference_frame"_a=std::string("world")); + c.def(py::init&, const std::string&>(), "Construct a CartesianTwist from a single 6d twist vector", "name"_a, "twist"_a, "reference_frame"_a=std::string("world")); - c.def_static("Zero", &CartesianTwist::Zero, "Constructor for the zero twist", "name"_a, "reference"_a=std::string("world")); - c.def_static("Random", &CartesianTwist::Random, "Constructor for a random twist", "name"_a, "reference"_a=std::string("world")); + c.def_static("Zero", &CartesianTwist::Zero, "Constructor for the zero twist", "name"_a, "reference_frame"_a=std::string("world")); + c.def_static("Random", &CartesianTwist::Random, "Constructor for a random twist", "name"_a, "reference_frame"_a=std::string("world")); std::vector deleted_attributes = { "position", @@ -328,17 +328,17 @@ void cartesian_acceleration(py::module_& m) { py::class_, CartesianState> c(m, "CartesianAcceleration"); c.def(py::init(), "Empty constructor"); - c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference_frame"_a=std::string("world")); c.def(py::init(), "Copy constructor", "acceleration"_a); c.def(py::init(), "Copy constructor from a CartesianState", "state"_a); c.def(py::init(), "Copy constructor from a CartesianTwist by considering that it is equivalent to dividing the twist by 1 second", "twist"_a); - c.def(py::init(), "Construct a CartesianAcceleration from a linear acceleration given as a vector.", "name"_a, "linear_acceleration"_a, "reference"_a=std::string("world")); - c.def(py::init(), "Construct a CartesianAcceleration from a linear acceleration and angular acceleration given as vectors.", "name"_a, "linear_acceleration"_a, "angular_acceleration"_a, "reference"_a=std::string("world")); - c.def(py::init&, const std::string&>(), "Construct a CartesianAcceleration from a single 6d acceleration vector", "name"_a, "acceleration"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Construct a CartesianAcceleration from a linear acceleration given as a vector.", "name"_a, "linear_acceleration"_a, "reference_frame"_a=std::string("world")); + c.def(py::init(), "Construct a CartesianAcceleration from a linear acceleration and angular acceleration given as vectors.", "name"_a, "linear_acceleration"_a, "angular_acceleration"_a, "reference_frame"_a=std::string("world")); + c.def(py::init&, const std::string&>(), "Construct a CartesianAcceleration from a single 6d acceleration vector", "name"_a, "acceleration"_a, "reference_frame"_a=std::string("world")); - c.def_static("Zero", &CartesianAcceleration::Zero, "Constructor for the zero acceleration", "name"_a, "reference"_a=std::string("world")); - c.def_static("Random", &CartesianAcceleration::Random, "Constructor for a random acceleration", "name"_a, "reference"_a=std::string("world")); + c.def_static("Zero", &CartesianAcceleration::Zero, "Constructor for the zero acceleration", "name"_a, "reference_frame"_a=std::string("world")); + c.def_static("Random", &CartesianAcceleration::Random, "Constructor for a random acceleration", "name"_a, "reference_frame"_a=std::string("world")); std::vector deleted_attributes = { "position", @@ -401,16 +401,16 @@ void cartesian_wrench(py::module_& m) { py::class_, CartesianState> c(m, "CartesianWrench"); c.def(py::init(), "Empty constructor"); - c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Constructor with name and reference frame provided", "name"_a, "reference_frame"_a=std::string("world")); c.def(py::init(), "Copy constructor", "twist"_a); c.def(py::init(), "Copy constructor from a CartesianState", "state"_a); - c.def(py::init(), "Construct a CartesianWrench from a force given as a vector.", "name"_a, "force"_a, "reference"_a=std::string("world")); - c.def(py::init(), "Construct a CartesianWrench from a force and torque given as vectors.", "name"_a, "force"_a, "torque"_a, "reference"_a=std::string("world")); - c.def(py::init&, const std::string&>(), "Construct a CartesianTwist from a single 6d wrench vector", "name"_a, "wrench"_a, "reference"_a=std::string("world")); + c.def(py::init(), "Construct a CartesianWrench from a force given as a vector.", "name"_a, "force"_a, "reference_frame"_a=std::string("world")); + c.def(py::init(), "Construct a CartesianWrench from a force and torque given as vectors.", "name"_a, "force"_a, "torque"_a, "reference_frame"_a=std::string("world")); + c.def(py::init&, const std::string&>(), "Construct a CartesianTwist from a single 6d wrench vector", "name"_a, "wrench"_a, "reference_frame"_a=std::string("world")); - c.def_static("Zero", &CartesianWrench::Zero, "Constructor for the zero wrench", "name"_a, "reference"_a=std::string("world")); - c.def_static("Random", &CartesianWrench::Random, "Constructor for a random wrench", "name"_a, "reference"_a=std::string("world")); + c.def_static("Zero", &CartesianWrench::Zero, "Constructor for the zero wrench", "name"_a, "reference_frame"_a=std::string("world")); + c.def_static("Random", &CartesianWrench::Random, "Constructor for a random wrench", "name"_a, "reference_frame"_a=std::string("world")); std::vector deleted_attributes = { "position", diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index a22f78f2e..3cbca89cc 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 6.1.0) +project(control_libraries VERSION 6.2.0) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/README.md b/source/README.md index 690fed564..e2d7f9f29 100644 --- a/source/README.md +++ b/source/README.md @@ -176,15 +176,3 @@ find_package(control_libraries 5.0.0 CONFIG REQUIRED dynamical_systems robot_mod # ensure that the `controllers` library is available, and also check for dynamical_systems in the background find_package(control_libraries 5.0.0 CONFIG REQUIRED controllers OPTIONAL_COMPONENTS dynamical_systems) ``` - -### robot_model and Pinocchio - -If the `robot_model` library is used and `pinocchio` has been installed to a location that is -not on the default include and link paths, that location must also be provided. -The default installation of `pinocchio` is at `/opt/openrobots`. For this reason it is normal to add the following -lines to the CMakeLists file: - -```cmake -list(APPEND CMAKE_PREFIX_PATH /opt/openrobots) -include_directories(/opt/openrobots/include) -``` diff --git a/source/controllers/include/controllers/IController.hpp b/source/controllers/include/controllers/IController.hpp index a3c5b325e..5afaf8c9c 100644 --- a/source/controllers/include/controllers/IController.hpp +++ b/source/controllers/include/controllers/IController.hpp @@ -20,7 +20,7 @@ namespace controllers { /** * @class IController * @brief Abstract class to define a controller in a desired state type, such as joint or Cartesian spaces - * @tparam S the state type of the controller + * @tparam S The state type of the controller */ template class IController : public state_representation::ParameterMap { @@ -38,18 +38,18 @@ class IController : public state_representation::ParameterMap { /** * @brief Compute the command output based on the commanded state and a feedback state * To be redefined based on the actual controller implementation. - * @param command_state the input state to the controller - * @param feedback_state the current state of the system given as feedback - * @return the output command at the input state + * @param command_state The input state to the controller + * @param feedback_state The current state of the system given as feedback + * @return The output command in the same state space as the input */ [[nodiscard]] virtual S compute_command(const S& command_state, const S& feedback_state) = 0; /** * @brief Compute the command output in joint space - * @param command_state - * @param feedback_state - * @param jacobian the Jacobian matrix relating Cartesian forces to joint space - * @return + * @param command_state The input state to the controller + * @param feedback_state The current state of the system given as feedback + * @param jacobian The Jacobian matrix relating Cartesian forces to joint space + * @return The output command in joint space */ [[nodiscard]] state_representation::JointState compute_command( const S& command_state, const S& feedback_state, const state_representation::Jacobian& jacobian @@ -58,15 +58,15 @@ class IController : public state_representation::ParameterMap { /** * @brief Compute the command output in joint space from command and feedback states in task space * @details This method requires the controller to have an associated robot model to compute the Jacobian. - * @param command_state - * @param feedback_state + * @param command_state The input state to the controller + * @param feedback_state The current state of the system given as feedback * @param joint_positions The current joint positions, which are required for computing the Jacobian - * @param frame_name - * @return + * @param frame The name of the robot frame at which to compute the Jacobian + * @return The output command in joint space */ [[nodiscard]] state_representation::JointState compute_command( const S& command_state, const S& feedback_state, const state_representation::JointPositions& joint_positions, - const std::string& frame_name = "" + const std::string& frame = "" ); /** diff --git a/source/controllers/src/IController.cpp b/source/controllers/src/IController.cpp index fbe28ccdc..2648421f7 100644 --- a/source/controllers/src/IController.cpp +++ b/source/controllers/src/IController.cpp @@ -29,14 +29,14 @@ JointState IController::compute_command( template<> JointState IController::compute_command( const CartesianState& command_state, const CartesianState& feedback_state, const JointPositions& joint_positions, - const std::string& frame_name + const std::string& frame ) { if (this->robot_model_ == nullptr) { throw exceptions::NoRobotModelException( "A robot model is required to convert the control command from Cartesian to joint space"); } - auto jacobian = this->robot_model_->compute_jacobian(joint_positions, frame_name); + auto jacobian = this->robot_model_->compute_jacobian(joint_positions, frame); return this->compute_command(command_state, feedback_state, jacobian); } diff --git a/source/dev-server.sh b/source/dev-server.sh index 0aed7e7c6..9312a7f1f 100755 --- a/source/dev-server.sh +++ b/source/dev-server.sh @@ -1,7 +1,7 @@ #!/usr/bin/env bash IMAGE_NAME=ghcr.io/epfl-lasa/control-libraries/development-dependencies -CONTAINER_NAME=epfl-lasa-control-libraries-development-dependencies-ssh +CONTAINER_NAME=control-libraries-development-dependencies-ssh SSH_PORT=2222 SSH_KEY_FILE="${HOME}/.ssh/id_rsa.pub" diff --git a/source/install.sh b/source/install.sh index 86aaeeefa..0d0c425df 100755 --- a/source/install.sh +++ b/source/install.sh @@ -10,8 +10,9 @@ INSTALL_DESTINATION="/usr/local" AUTO_INSTALL="" EIGEN_VERSION=3.4.0 -OSQP_TAG=v0.6.2 -OSQP_EIGEN_TAG=v0.6.4 +OSQP_TAG=0.6.2 +OSQP_EIGEN_TAG=0.6.4 +PINOCCHIO_TAG=2.6.9 FAIL_MESSAGE="The provided input arguments are not valid. Run the script with the '--help' argument." @@ -84,8 +85,15 @@ rm -rf "${SOURCE_PATH}"/tmp # install base dependencies echo ">>> INSTALLING BASE DEPENDENCIES" -INSTALLED_EIGEN=$(pkg-config --modversion eigen3) + +if [ -z $(which pkg-config) ]; then + echo ">>> INSTALLING pkg-config tool" + apt-get update && apt-get install "${AUTO_INSTALL}" pkg-config || exit 1 +fi + +INSTALLED_EIGEN=$(pkg-config --modversion eigen3) || exit 0 if [ "${INSTALLED_EIGEN::4}" != "${EIGEN_VERSION::4}" ]; then + echo ">>> INSTALLING EIGEN" mkdir -p "${SOURCE_PATH}"/tmp/lib && cd "${SOURCE_PATH}"/tmp/lib || exit 1 wget -c "https://gitlab.com/libeigen/eigen/-/archive/${EIGEN_VERSION}/eigen-${EIGEN_VERSION}.tar.gz" -O - | tar -xz || exit 1 cd "eigen-${EIGEN_VERSION}" && mkdir -p build && cd build && cmake .. && make install || exit 1 @@ -98,32 +106,28 @@ fi # install module-specific dependencies if [ "${BUILD_ROBOT_MODEL}" == "ON" ]; then echo ">>> INSTALLING ROBOT MODEL DEPENDENCIES" - apt-get update && apt-get install "${AUTO_INSTALL}" lsb-release gnupg2 curl || exit 1 - - # install pinocchio - echo "deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \ - | tee /etc/apt/sources.list.d/robotpkg.list - curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | apt-key add - - - apt-get update && apt-get install "${AUTO_INSTALL}" robotpkg-pinocchio || exit 1 - - export PATH=/opt/openrobots/bin:$PATH - export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH - export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH - export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH - - # install osqp - mkdir -p "${SOURCE_PATH}"/tmp/lib && cd "${SOURCE_PATH}"/tmp/lib || exit 1 - git clone --recursive https://github.com/oxfordcontrol/osqp - cd "${SOURCE_PATH}"/tmp/lib/osqp/ && git checkout "${OSQP_TAG}" && mkdir -p build && cd build || exit 1 - cmake -G "Unix Makefiles" .. && cmake --build . --target install || exit 1 - # install osqp eigen wrapper - cd "${SOURCE_PATH}"/tmp/lib || exit 1 - git clone https://github.com/robotology/osqp-eigen.git - cd "${SOURCE_PATH}"/tmp/lib/osqp-eigen && git checkout "${OSQP_EIGEN_TAG}" && mkdir -p build && cd build || exit 1 - cmake .. && make -j && make install || exit 1 - - ln -s /opt/openrobots/lib/libpinocchio.so* "${INSTALL_DESTINATION}"/lib/ + apt-get update && apt-get install "${AUTO_INSTALL}" libboost-all-dev liburdfdom-dev || exit 1 + + INSTALLED_PINOCCHIO=$(pkg-config --modversion pinocchio) + if [ "${INSTALLED_PINOCCHIO}" != "${PINOCCHIO_TAG}" ]; then + mkdir -p "${SOURCE_PATH}"/tmp/lib && cd "${SOURCE_PATH}"/tmp/lib || exit 1 + + echo ">>> INSTALLING OSQP [1/3]" + git clone --depth 1 -b v${OSQP_TAG} --recursive https://github.com/oxfordcontrol/osqp \ + && cd osqp && mkdir build && cd build && cmake -G "Unix Makefiles" .. && cmake --build . --target install \ + && cd ../.. && rm -r osqp || exit 1 + + echo ">>> INSTALLING OSQP_EIGEN [2/3]" + git clone --depth 1 -b v${OSQP_EIGEN_TAG} https://github.com/robotology/osqp-eigen.git \ + && cd osqp-eigen && mkdir build && cd build && cmake .. && make -j && make install \ + && cd ../.. && rm -r osqp-eigen || exit 1 + + echo ">>> INSTALLING PINOCCHIO [3/3]" + git clone --depth 1 -b v${PINOCCHIO_TAG} --recursive https://github.com/stack-of-tasks/pinocchio \ + && cd pinocchio && mkdir build && cd build \ + && cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local -DBUILD_PYTHON_INTERFACE=OFF \ + && make -j1 && make install && cd ../.. && rm -r pinocchio || exit 1 + fi ldconfig fi diff --git a/source/robot_model/CMakeLists.txt b/source/robot_model/CMakeLists.txt index 0d983b9cb..181abce69 100644 --- a/source/robot_model/CMakeLists.txt +++ b/source/robot_model/CMakeLists.txt @@ -1,6 +1,5 @@ set(LIBRARY_NAME robot_model) -list(APPEND CMAKE_PREFIX_PATH /opt/openrobots) find_package(pinocchio REQUIRED) find_package(OsqpEigen REQUIRED) @@ -14,8 +13,8 @@ add_library(${PROJECT_NAME}::${LIBRARY_NAME} ALIAS ${LIBRARY_NAME}) # add include directories target_include_directories(${LIBRARY_NAME} PUBLIC - "$" - "$" + $ + $ ) target_link_libraries(${LIBRARY_NAME} diff --git a/source/robot_model/include/robot_model/Model.hpp b/source/robot_model/include/robot_model/Model.hpp index f8f9f6555..b351e220e 100644 --- a/source/robot_model/include/robot_model/Model.hpp +++ b/source/robot_model/include/robot_model/Model.hpp @@ -64,7 +64,7 @@ class Model { // @format:off std::shared_ptr> robot_name_;///< name of the robot std::shared_ptr> urdf_path_; ///< path to the urdf file - std::vector frame_names_; ///< name of the frames + std::vector frames_; ///< name of the frames pinocchio::Model robot_model_; ///< the robot model with pinocchio pinocchio::Data robot_data_; ///< the robot data with pinocchio OsqpEigen::Solver solver_; ///< osqp solver for the quadratic programming based inverse kinematics @@ -86,17 +86,17 @@ class Model { /** * @brief Check if frames exist in robot model and return its ids - * @param frame_names containing the frame names to check + * @param frames containing the frame names to check * @return the ids of the frames */ - std::vector get_frame_ids(const std::vector& frame_names); + std::vector get_frame_ids(const std::vector& frames); /** * @brief Check if a frame exist in robot model and return its id - * @param frame_name containing the frame name to check + * @param frame containing the frame name to check * @return the id of the frame if it exists */ - unsigned int get_frame_id(const std::string& frame_name); + unsigned int get_frame_id(const std::string& frame); /** * @brief Compute the Jacobian from given joint positions at the frame in parameter @@ -179,11 +179,11 @@ class Model { * @brief Check the arguments of the inverse_velocity function and throw exceptions if they are not correct * @param cartesian_twists vector of twist * @param joint_positions current joint positions, used to compute the Jacobian matrix - * @param frame_names names of the frames at which to compute the twists + * @param frames names of the frames at which to compute the twists */ void check_inverse_velocity_arguments(const std::vector& cartesian_twists, const state_representation::JointPositions& joint_positions, - const std::vector& frame_names); + const std::vector& frames); public: /** @@ -283,22 +283,22 @@ class Model { /** * @brief Compute the Jacobian from a given joint state at the frame given in parameter * @param joint_positions containing the joint positions of the robot - * @param frame_name name of the frame at which to compute the Jacobian, if empty computed for the last frame + * @param frame name of the frame at which to compute the Jacobian, if empty computed for the last frame * @return the Jacobian matrix */ state_representation::Jacobian compute_jacobian(const state_representation::JointPositions& joint_positions, - const std::string& frame_name = ""); + const std::string& frame = ""); /** * @brief Compute the time derivative of the Jacobian from given joint positions and velocities at the frame in parameter * @param joint_positions containing the joint positions of the robot * @param joint_velocities containing the joint positions of the robot - * @param frame_name name of the frame at which to compute the Jacobian, if empty computed for the last frame + * @param frame name of the frame at which to compute the Jacobian, if empty computed for the last frame * @return the time derivative of Jacobian matrix */ Eigen::MatrixXd compute_jacobian_time_derivative(const state_representation::JointPositions& joint_positions, const state_representation::JointVelocities& joint_velocities, - const std::string& frame_name = ""); + const std::string& frame = ""); /** * @brief Compute the Inertia matrix from a given joint positions @@ -340,32 +340,32 @@ class Model { /** * @brief Compute the forward kinematics, i.e. the pose of certain frames from the joint positions * @param joint_positions the joint state of the robot - * @param frame_names names of the frames at which to extract the poses + * @param frames names of the frames at which to extract the poses * @return the pose of desired frames */ std::vector forward_kinematics(const state_representation::JointPositions& joint_positions, - const std::vector& frame_names); + const std::vector& frames); /** * @brief Compute the forward kinematics, i.e. the pose of the frame from the joint positions * @param joint_positions the joint state of the robot - * @param frame_name name of the frame at which to extract the pose + * @param frame name of the frame at which to extract the pose * @return the pose of the desired frame */ state_representation::CartesianPose forward_kinematics(const state_representation::JointPositions& joint_positions, - const std::string& frame_name = ""); + const std::string& frame = ""); /** * @brief Compute the inverse kinematics, i.e. joint positions from the pose of the end-effector in an iterative manner * @param cartesian_pose containing the desired pose of the end-effector * @param parameters parameters of the inverse kinematics algorithm (default is default values of the * InverseKinematicsParameters structure) - * @param frame_name name of the frame at which to extract the pose + * @param frame name of the frame at which to extract the pose * @return the joint positions of the robot */ state_representation::JointPositions inverse_kinematics(const state_representation::CartesianPose& cartesian_pose, const InverseKinematicsParameters& parameters = InverseKinematicsParameters(), - const std::string& frame_name = ""); + const std::string& frame = ""); /** * @brief Compute the inverse kinematics, i.e. joint positions from the pose of the end-effector @@ -373,57 +373,57 @@ class Model { * @param joint_positions current state of the robot containing the generalized position * @param parameters parameters of the inverse kinematics algorithm (default is default values of the * InverseKinematicsParameters structure) - * @param frame_name name of the frame at which to extract the pose + * @param frame name of the frame at which to extract the pose * @return the joint positions of the robot */ state_representation::JointPositions inverse_kinematics(const state_representation::CartesianPose& cartesian_pose, const state_representation::JointPositions& joint_positions, const InverseKinematicsParameters& parameters = InverseKinematicsParameters(), - const std::string& frame_name = ""); + const std::string& frame = ""); /** * @brief Compute the forward velocity kinematics, i.e. the twist of certain frames from the joint states * @param joint_state the joint state of the robot with positions to compute the Jacobian and velocities for the twist - * @param frame_names name of the frames at which to compute the twist + * @param frames name of the frames at which to compute the twist * @return the twists of the frames in parameter */ std::vector forward_velocity(const state_representation::JointState& joint_state, - const std::vector& frame_names); + const std::vector& frames); /** * @brief Compute the forward velocity kinematics, i.e. the twist of the end-effector from the joint velocities * @param joint_state the joint state of the robot with positions to compute the Jacobian and velocities for the twist - * @param frame_name name of the frame at which to compute the twist + * @param frame name of the frame at which to compute the twist * @return the twist of the frame in parameter */ state_representation::CartesianTwist forward_velocity(const state_representation::JointState& joint_state, - const std::string& frame_name = ""); + const std::string& frame = ""); /** * @brief Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter * using the Jacobian * @param cartesian_twists vector of twist * @param joint_positions current joint positions, used to compute the Jacobian matrix - * @param frame_names names of the frames at which to compute the twists + * @param frames names of the frames at which to compute the twists * @return the joint velocities of the robot */ state_representation::JointVelocities inverse_velocity(const std::vector& cartesian_twists, const state_representation::JointPositions& joint_positions, - const std::vector& frame_names); + const std::vector& frames); /** * @brief Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the * Jacobian * @param cartesian_twist containing the twist of the end-effector * @param joint_positions current joint positions, used to compute the Jacobian matrix - * @param frame_name name of the frame at which to compute the twist + * @param frame name of the frame at which to compute the twist * @param parameters parameters of the inverse velocity kinematics algorithm (default is default values of the * QPInverseVelocityParameters structure) * @return the joint velocities of the robot */ state_representation::JointVelocities inverse_velocity(const state_representation::CartesianTwist& cartesian_twist, const state_representation::JointPositions& joint_positions, - const std::string& frame_name = ""); + const std::string& frame = ""); /** * @brief Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter @@ -432,13 +432,13 @@ class Model { * @param joint_positions current joint positions, used to compute the jacobian matrix * @param parameters parameters of the inverse velocity kinematics algorithm (default is default values of the * QPInverseVelocityParameters structure) - * @param frame_names names of the frames at which to compute the twists + * @param frames names of the frames at which to compute the twists * @return the joint velocities of the robot */ state_representation::JointVelocities inverse_velocity(const std::vector& cartesian_twists, const state_representation::JointPositions& joint_positions, const QPInverseVelocityParameters& parameters, - const std::vector& frame_names); + const std::vector& frames); /** * @brief Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the @@ -448,13 +448,13 @@ class Model { * @param joint_positions current joint positions, used to compute the Jacobian matrix * @param parameters parameters of the inverse velocity kinematics algorithm (default is default values of the * QPInverseVelocityParameters structure) - * @param frame_name name of the frame at which to compute the twist + * @param frame name of the frame at which to compute the twist * @return the joint velocities of the robot */ state_representation::JointVelocities inverse_velocity(const state_representation::CartesianTwist& cartesian_twist, const state_representation::JointPositions& joint_positions, const QPInverseVelocityParameters& parameters, - const std::string& frame_name = ""); + const std::string& frame = ""); /** * @brief Helper function to print the qp_problem (for debugging) @@ -537,11 +537,11 @@ inline std::vector Model::get_joint_frames() const { } inline const std::string& Model::get_base_frame() const { - return this->frame_names_.front(); + return this->frames_.front(); } inline std::vector Model::get_frames() const { - return this->frame_names_; + return this->frames_; } inline Eigen::Vector3d Model::get_gravity_vector() const { diff --git a/source/robot_model/include/robot_model/exceptions/FrameNotFoundException.hpp b/source/robot_model/include/robot_model/exceptions/FrameNotFoundException.hpp index fcd182fc6..d0fb2cb3d 100644 --- a/source/robot_model/include/robot_model/exceptions/FrameNotFoundException.hpp +++ b/source/robot_model/include/robot_model/exceptions/FrameNotFoundException.hpp @@ -5,7 +5,7 @@ namespace robot_model::exceptions { class FrameNotFoundException : public std::invalid_argument { public: - explicit FrameNotFoundException(const std::string& frame_name) : - invalid_argument("Frame with name or ID " + frame_name + " is not in the robot model") {}; + explicit FrameNotFoundException(const std::string& frame) : + invalid_argument("Frame with name or ID " + frame + " is not in the robot model") {}; }; }// namespace robot_model::exceptions \ No newline at end of file diff --git a/source/robot_model/src/Model.cpp b/source/robot_model/src/Model.cpp index f449dd990..7dd88258e 100644 --- a/source/robot_model/src/Model.cpp +++ b/source/robot_model/src/Model.cpp @@ -32,13 +32,13 @@ bool Model::create_urdf_from_string(const std::string& urdf_string, const std::s void Model::init_model() { pinocchio::urdf::buildModel(this->get_urdf_path(), this->robot_model_); this->robot_data_ = pinocchio::Data(this->robot_model_); - // get the frame names + // get the frames std::vector frames; for (auto& f : this->robot_model_.frames) { frames.push_back(f.name); } // remove universe and root_joint frame added by Pinocchio - this->frame_names_ = std::vector(frames.begin() + 2, frames.end()); + this->frames_ = std::vector(frames.begin() + 2, frames.end()); this->init_qp_solver(); } @@ -100,27 +100,27 @@ bool Model::init_qp_solver() { return this->solver_.initSolver(); } -std::vector Model::get_frame_ids(const std::vector& frame_names) { +std::vector Model::get_frame_ids(const std::vector& frames) { std::vector frame_ids; - frame_ids.reserve(frame_names.size()); + frame_ids.reserve(frames.size()); - for (auto& frame_name : frame_names) { - if (frame_name.empty()) { + for (auto& frame : frames) { + if (frame.empty()) { // get last frame if none specified frame_ids.push_back(this->robot_model_.frames.size() - 1); } else { // throw error if specified frame does not exist - if (!this->robot_model_.existFrame(frame_name)) { - throw (exceptions::FrameNotFoundException(frame_name)); + if (!this->robot_model_.existFrame(frame)) { + throw (exceptions::FrameNotFoundException(frame)); } - frame_ids.push_back(this->robot_model_.getFrameId(frame_name)); + frame_ids.push_back(this->robot_model_.getFrameId(frame)); } } return frame_ids; } -unsigned int Model::get_frame_id(const std::string& frame_name) { - return get_frame_ids(std::vector{frame_name}).back(); +unsigned int Model::get_frame_id(const std::string& frame) { + return get_frame_ids(std::vector{frame}).back(); } state_representation::Jacobian Model::compute_jacobian(const state_representation::JointPositions& joint_positions, @@ -146,8 +146,8 @@ state_representation::Jacobian Model::compute_jacobian(const state_representatio } state_representation::Jacobian Model::compute_jacobian(const state_representation::JointPositions& joint_positions, - const std::string& frame_name) { - auto frame_id = get_frame_id(frame_name); + const std::string& frame) { + auto frame_id = get_frame_id(frame); return this->compute_jacobian(joint_positions, frame_id); } @@ -177,8 +177,8 @@ Eigen::MatrixXd Model::compute_jacobian_time_derivative(const state_representati Eigen::MatrixXd Model::compute_jacobian_time_derivative(const state_representation::JointPositions& joint_positions, const state_representation::JointVelocities& joint_velocities, - const std::string& frame_name) { - auto frame_id = get_frame_id(frame_name); + const std::string& frame) { + auto frame_id = get_frame_id(frame); return this->compute_jacobian_time_derivative(joint_positions, joint_velocities, frame_id); } @@ -251,14 +251,14 @@ std::vector Model::forward_kinematics(const } state_representation::CartesianPose Model::forward_kinematics(const state_representation::JointPositions& joint_positions, - const std::string& frame_name) { - std::string actual_frame_name = frame_name.empty() ? this->robot_model_.frames.back().name : frame_name; - return this->forward_kinematics(joint_positions, std::vector{actual_frame_name}).front(); + const std::string& frame) { + std::string actual_frame = frame.empty() ? this->robot_model_.frames.back().name : frame; + return this->forward_kinematics(joint_positions, std::vector{actual_frame}).front(); } std::vector Model::forward_kinematics(const state_representation::JointPositions& joint_positions, - const std::vector& frame_names) { - auto frame_ids = get_frame_ids(frame_names); + const std::vector& frames) { + auto frame_ids = get_frame_ids(frames); return this->forward_kinematics(joint_positions, frame_ids); } @@ -308,10 +308,10 @@ state_representation::JointPositions Model::inverse_kinematics(const state_representation::CartesianPose& cartesian_pose, const state_representation::JointPositions& joint_positions, const InverseKinematicsParameters& parameters, - const std::string& frame_name) { - std::string actual_frame_name = frame_name.empty() ? this->robot_model_.frames.back().name : frame_name; - if (!this->robot_model_.existFrame(actual_frame_name)) { - throw (exceptions::FrameNotFoundException(actual_frame_name)); + const std::string& frame) { + std::string actual_frame = frame.empty() ? this->robot_model_.frames.back().name : frame; + if (!this->robot_model_.existFrame(actual_frame)) { + throw (exceptions::FrameNotFoundException(actual_frame)); } // 1 second for the Newton-Raphson method const std::chrono::nanoseconds dt(static_cast(1e9)); @@ -320,7 +320,7 @@ Model::inverse_kinematics(const state_representation::CartesianPose& cartesian_p state_representation::JointVelocities dq(this->get_robot_name(), joint_positions.get_names()); state_representation::Jacobian J(this->get_robot_name(), this->get_joint_frames(), - actual_frame_name, + actual_frame, this->get_base_frame()); Eigen::MatrixXd J_b = Eigen::MatrixXd::Zero(6, this->get_number_of_joints()); Eigen::MatrixXd JJt(6, 6); @@ -329,12 +329,12 @@ Model::inverse_kinematics(const state_representation::CartesianPose& cartesian_p Eigen::VectorXd psi(this->get_number_of_joints()); Eigen::VectorXd err(6); for (unsigned int i = 0; i < parameters.max_number_of_iterations; ++i) { - err = ((cartesian_pose - this->forward_kinematics(q, actual_frame_name)) / dt).data(); + err = ((cartesian_pose - this->forward_kinematics(q, actual_frame)) / dt).data(); // break in case of convergence if (err.cwiseAbs().maxCoeff() < parameters.tolerance) { return q; } - J = this->compute_jacobian(q, actual_frame_name); + J = this->compute_jacobian(q, actual_frame); W_b = this->cwln_weighted_matrix(q, parameters.margin); W_c = Eigen::MatrixXd::Identity(this->get_number_of_joints(), this->get_number_of_joints()) - W_b; psi = parameters.gamma * this->cwln_repulsive_potential_field(q, parameters.margin); @@ -352,40 +352,40 @@ Model::inverse_kinematics(const state_representation::CartesianPose& cartesian_p state_representation::JointPositions Model::inverse_kinematics(const state_representation::CartesianPose& cartesian_pose, const InverseKinematicsParameters& parameters, - const std::string& frame_name) { + const std::string& frame) { Eigen::VectorXd q(pinocchio::neutral(this->robot_model_)); state_representation::JointPositions positions(this->get_robot_name(), this->get_joint_frames(), q); - return this->inverse_kinematics(cartesian_pose, positions, parameters, frame_name); + return this->inverse_kinematics(cartesian_pose, positions, parameters, frame); } std::vector Model::forward_velocity(const state_representation::JointState& joint_state, - const std::vector& frame_names) { - std::vector cartesian_twists(frame_names.size()); - for (std::size_t i = 0; i < frame_names.size(); ++i) { - cartesian_twists.at(i) = this->compute_jacobian(joint_state, frame_names.at(i)) + const std::vector& frames) { + std::vector cartesian_twists(frames.size()); + for (std::size_t i = 0; i < frames.size(); ++i) { + cartesian_twists.at(i) = this->compute_jacobian(joint_state, frames.at(i)) * static_cast(joint_state); } return cartesian_twists; } state_representation::CartesianTwist Model::forward_velocity(const state_representation::JointState& joint_state, - const std::string& frame_name) { - return this->forward_velocity(joint_state, std::vector{frame_name}).front(); + const std::string& frame) { + return this->forward_velocity(joint_state, std::vector{frame}).front(); } void Model::check_inverse_velocity_arguments(const std::vector& cartesian_twists, const state_representation::JointPositions& joint_positions, - const std::vector& frame_names) { - if (cartesian_twists.size() != frame_names.size()) { - throw (std::invalid_argument("The number of provided twists and frame names does not match")); + const std::vector& frames) { + if (cartesian_twists.size() != frames.size()) { + throw (std::invalid_argument("The number of provided twists and frames does not match")); } if (joint_positions.get_size() != this->get_number_of_joints()) { throw (exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints())); } - for (auto& frame_name : frame_names) { - if (!this->robot_model_.existFrame(frame_name)) { - throw (exceptions::FrameNotFoundException(frame_name)); + for (auto& frame : frames) { + if (!this->robot_model_.existFrame(frame)) { + throw (exceptions::FrameNotFoundException(frame)); } } } @@ -393,9 +393,9 @@ void Model::check_inverse_velocity_arguments(const std::vector& cartesian_twists, const state_representation::JointPositions& joint_positions, - const std::vector& frame_names) { + const std::vector& frames) { // sanity check - this->check_inverse_velocity_arguments(cartesian_twists, joint_positions, frame_names); + this->check_inverse_velocity_arguments(cartesian_twists, joint_positions, frames); const unsigned int nb_joints = this->get_number_of_joints(); // the velocity vector contains position of the intermediate frame and full pose of the end-effector @@ -405,11 +405,11 @@ Model::inverse_velocity(const std::vector& // extract only the linear velocity for intermediate points dX.segment<3>(3 * i) = cartesian_twists[i].get_linear_velocity(); jacobian.block(3 * i, 0, 3 * i + 3, nb_joints) = - this->compute_jacobian(joint_positions, frame_names.at(i)).data().block(0, 0, 3, nb_joints); + this->compute_jacobian(joint_positions, frames.at(i)).data().block(0, 0, 3, nb_joints); } // full twist for the last provided frame dX.tail(6) = cartesian_twists.back().data(); - jacobian.bottomRows(6) = this->compute_jacobian(joint_positions, frame_names.back()).data(); + jacobian.bottomRows(6) = this->compute_jacobian(joint_positions, frames.back()).data(); // solve a linear system return state_representation::JointVelocities(joint_positions.get_name(), @@ -419,22 +419,22 @@ Model::inverse_velocity(const std::vector& state_representation::JointVelocities Model::inverse_velocity(const state_representation::CartesianTwist& cartesian_twist, const state_representation::JointPositions& joint_positions, - const std::string& frame_name) { - std::string actual_frame_name = frame_name.empty() ? this->robot_model_.frames.back().name : frame_name; + const std::string& frame) { + std::string actual_frame = frame.empty() ? this->robot_model_.frames.back().name : frame; return this->inverse_velocity(std::vector({cartesian_twist}), joint_positions, - std::vector({actual_frame_name})); + std::vector({actual_frame})); } state_representation::JointVelocities Model::inverse_velocity(const std::vector& cartesian_twists, const state_representation::JointPositions& joint_positions, const QPInverseVelocityParameters& parameters, - const std::vector& frame_names) { + const std::vector& frames) { using namespace state_representation; using namespace std::chrono; // sanity check - this->check_inverse_velocity_arguments(cartesian_twists, joint_positions, frame_names); + this->check_inverse_velocity_arguments(cartesian_twists, joint_positions, frames); const unsigned int nb_joints = this->get_number_of_joints(); // the velocity vector contains position of the intermediate frame and full pose of the end-effector @@ -446,13 +446,13 @@ Model::inverse_velocity(const std::vector& // extract only the position for intermediate points delta_r.segment<3>(3 * i) = displacement.get_position(); jacobian.block(3 * i, 0, 3 * i + 3, nb_joints) = - this->compute_jacobian(joint_positions, frame_names.at(i)).data().block(0, 0, 3, nb_joints); + this->compute_jacobian(joint_positions, frames.at(i)).data().block(0, 0, 3, nb_joints); } // extract pose for the last provided frame CartesianPose full_displacement = cartesian_twists.back(); delta_r.segment<3>(3 * (cartesian_twists.size() - 1)) = full_displacement.get_position(); delta_r.tail(3) = full_displacement.get_orientation().vec(); - jacobian.bottomRows(6) = this->compute_jacobian(joint_positions, frame_names.back()).data(); + jacobian.bottomRows(6) = this->compute_jacobian(joint_positions, frames.back()).data(); // compute the Jacobian Eigen::MatrixXd hessian_matrix = jacobian.transpose() * jacobian; // set the hessian sparse matrix @@ -499,12 +499,12 @@ Model::inverse_velocity(const std::vector& state_representation::JointVelocities Model::inverse_velocity(const state_representation::CartesianTwist& cartesian_twist, const state_representation::JointPositions& joint_positions, const QPInverseVelocityParameters& parameters, - const std::string& frame_name) { - std::string actual_frame_name = frame_name.empty() ? this->robot_model_.frames.back().name : frame_name; + const std::string& frame) { + std::string actual_frame = frame.empty() ? this->robot_model_.frames.back().name : frame; return this->inverse_velocity(std::vector({cartesian_twist}), joint_positions, parameters, - std::vector({actual_frame_name})); + std::vector({actual_frame})); } void Model::print_qp_problem() { diff --git a/source/state_representation/test/tests/test_parameter.cpp b/source/state_representation/test/tests/test_parameter.cpp index 9aea7e16c..801738755 100644 --- a/source/state_representation/test/tests/test_parameter.cpp +++ b/source/state_representation/test/tests/test_parameter.cpp @@ -368,4 +368,4 @@ using ParameterTestTypes = testing::Types; -INSTANTIATE_TYPED_TEST_SUITE_P(, ParameterTest, ParameterTestTypes); \ No newline at end of file +INSTANTIATE_TYPED_TEST_SUITE_P(Type, ParameterTest, ParameterTestTypes); \ No newline at end of file