diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs
index 30efb6c3..c0c35d52 100644
--- a/.git-blame-ignore-revs
+++ b/.git-blame-ignore-revs
@@ -6,3 +6,9 @@ b2be1d0b5a2ef7a5de758bca053161fb652aba56
# pre-commit run -a (Guilhem Saurel, 2022-09-05)
1d932e2d389a6205e6d3a7490b5c826fe01fea47
+
+# ruff (Guilhem Saurel, 2024-08-26)
+1ae998e5aef7dacc13bad7aa457575a513d78864
+
+# pre-commit run -a (Guilhem Saurel, 2024-08-26)
+e928496128b53fb15b1aad3b04fedd844ee86500
diff --git a/.github/workflows/ci-linux-ros.yml b/.github/workflows/ci-linux-ros.yml
index 3608395d..8b976305 100644
--- a/.github/workflows/ci-linux-ros.yml
+++ b/.github/workflows/ci-linux-ros.yml
@@ -6,22 +6,25 @@ jobs:
strategy:
matrix:
env:
- - {ROS_DISTRO: noetic, CMAKE_BUILD_TYPE: Debug}
- - {ROS_DISTRO: noetic, CMAKE_BUILD_TYPE: Release}
+ #- {ROS_DISTRO: rolling} Unable to locate package ros-rolling-pinocchio
+ - {ROS_DISTRO: iron}
+ - {ROS_DISTRO: humble}
env:
CCACHE_DIR: /github/home/.ccache # Enable ccache
UPSTREAM_WORKSPACE: dependencies.rosinstall
CMAKE_ARGS: -DBUILD_WITH_OSQP=ON -DBUILD_WITH_PROXQP=ON -DBUILD_WITH_VECTORIZATION_SUPPORT=OFF # Simde is not available yet
runs-on: ubuntu-latest
steps:
- - uses: actions/checkout@v3
+ - uses: actions/checkout@v4
with:
submodules: recursive
+ # eiquadprog is not yet available in ROS2
+ - run: sed -i "/eiquadprog/d" package.xml
# This step will fetch/store the directory used by ccache before/after the ci run
- uses: actions/cache@v3
with:
path: ${{ env.CCACHE_DIR }}
key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
# Run industrial_ci
- - uses: 'ros-industrial/industrial_ci@6a8f546cbd31fbd5c9f77e3409265c8b39abc3d6'
+ - uses: 'ros-industrial/industrial_ci@d23b9ad2c63bfad638a2b1fe3df34b8df9a2f17b'
env: ${{ matrix.env }}
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index a873326a..e357e4d4 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -1,31 +1,32 @@
ci:
- autoupdate_branch: 'devel'
+ autoupdate_branch: devel
repos:
-- repo: https://github.com/pre-commit/pre-commit-hooks
- rev: v4.4.0
- hooks:
- - id: check-json
- - id: check-symlinks
- - id: check-toml
- - id: check-yaml
-- repo: https://github.com/psf/black
- rev: 23.3.0
- hooks:
- - id: black
-- repo: https://github.com/PyCQA/isort
- rev: 5.12.0
- hooks:
- - id: isort
-- repo: https://github.com/cheshirekow/cmake-format-precommit
- rev: v0.6.13
- hooks:
- - id: cmake-format
-- repo: https://github.com/pre-commit/mirrors-clang-format
- rev: v16.0.3
- hooks:
- - id: clang-format
- args: ['--style={BasedOnStyle: Google,SortIncludes: false}']
-- repo: https://github.com/pappasam/toml-sort
- rev: v0.23.1
- hooks:
- - id: toml-sort-fix
+- repo: https://github.com/astral-sh/ruff-pre-commit
+ rev: v0.6.2
+ hooks:
+ - id: ruff
+ args:
+ - --fix
+ - --exit-non-zero-on-fix
+ - id: ruff-format
+- repo: https://github.com/cheshirekow/cmake-format-precommit
+ rev: v0.6.13
+ hooks:
+ - id: cmake-format
+- repo: https://github.com/pappasam/toml-sort
+ rev: v0.23.1
+ hooks:
+ - id: toml-sort-fix
+- repo: https://github.com/pre-commit/mirrors-clang-format
+ rev: v18.1.8
+ hooks:
+ - id: clang-format
+ args:
+ - '--style={BasedOnStyle: Google, SortIncludes: false, WhitespaceSensitiveMacros: ["TSID_DISABLE_WARNING"]}'
+- repo: https://github.com/pre-commit/pre-commit-hooks
+ rev: v4.6.0
+ hooks:
+ - id: check-json
+ - id: check-symlinks
+ - id: check-toml
+ - id: check-yaml
diff --git a/CHANGELOG.md b/CHANGELOG.md
new file mode 100644
index 00000000..32d494fa
--- /dev/null
+++ b/CHANGELOG.md
@@ -0,0 +1,185 @@
+# Changelog
+
+All notable changes to this project will be documented in this file.
+
+The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
+and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
+
+## [Unreleased]
+
+- Fix a typo in ex_4_walking
+- check solver status, set eps_abs to 1e-6, fix seed
+- CMake: require >= 3.10
+- add changelog
+- setup ruff & fix isort pre-commit config
+- update ROS CI
+
+## [1.7.0] - 2023-05-13
+
+- expose SE3ToVector and vectorToSE3
+- remove warnings
+- Enhance Python target packaging
+- Add CI with GitHub Action for conda
+- Add support for proxqp and osqp solver
+- Clean and update contributors list
+- pre-commit autoupdate
+- Expose TaskJointPosVelAccBounds
+- Expose AddMotionTask for TaskJointPosVelAccBounds
+- Fix qpmad
+- Add clang-format Google style
+- Add Measured force as an external force task with moving objects
+- update CMake: fetch submodule, set default build type, fix RPATH
+- fix for eigenpy v3
+- np.matrix → np.array
+- tooling: setup black isort & toml-sort
+
+## [1.6.3] - 2022-11-02
+
+- Require C++17
+- fix tests in 18.04
+- update pinocchio use
+
+## [1.6.2] - 2022-09-05
+
+- update python tests for np array
+- add task actuation equality
+- use generated headers
+- Choose floating-base (or not) when creating a robot wrapper
+- Add optional support of qpmad solver
+- update packaging for eigenpy 2.7.12
+
+## [1.6.1] - 2021-10-19
+
+This new release is mostly a maintenance release, including fixes for the recent version of Eigen 3.4.
+
+## [1.6.0] - 2021-03-18
+
+- Add bindings for method Contact6d::getMotionTask
+- Add Center of Pressure Taks
+- fix warnings
+
+## [1.5.0] - 2021-03-03
+
+- [py] Add config variable to specify whether end-effector task should
+ be formulated in local frame or world frame.
+- [C++] Standardize names of methods to get and set mask in motion tasks.
+- [py] Add accessors masks in TaskCOM and TaskSE3Equality.
+- bugfixes
+
+## [1.4.2] - 2020-11-26
+
+- Add a mask to task-com-equality
+- Fix bug in Contact6d::setRegularizationTaskWeightVector
+- CMake: fix configure without tests
+
+## [1.4.1] - 2020-09-25
+
+- fix memory leaks thanks to shared_ptr
+- fix warnings
+- fix package.xml for ROS
+
+## [1.4.0] - 2020-09-09
+
+- add setGravity
+- stop using StdVec in python to make code more user friendly
+- add 6d contact with motion constraint at priority level 1 in python
+- use example-robot-data in notebooks
+
+## [1.3.1] - 2020-06-05
+
+- fix license
+- fix generated tsid.pc
+
+## [1.3.0] - 2020-05-26
+
+- reactive test
+- add package.xml
+- add CheatSheet
+- updates for numpy.array & python 3
+- updateRigidContactWeights: fix and add to python API
+- use eiquadprog
+- fix compatibility with pinocchio v2.4.5
+
+## [1.2.3] - 2020-03-30
+
+- renamed tests dir
+- fix python tests
+- CMake: export project and use exports from dependencies
+- CMake: keep minimal required instructions
+
+## [1.2.2] - 2020-03-01
+
+- add angular momentum equality task
+- update to pinocchio changes
+- Python 3 compatibility
+- add some documentation
+- fix python issue
+
+## [1.2.1] - 2019-09-19
+
+- fix compatibility with recent pinocchio versions
+
+## [1.2.0] - 2019-03-06
+
+- Pinocchio v2, fix #31
+- Fix demo_romeo for pinocchio v2
+- Pull request for use TSID in python.
+- Add missing includes, fix #18
+
+## [1.1.0] - 2018-10-10
+
+This release updates to non backward-compatibles changes in pinocchio v1.3.0
+
+## [1.0.2] - 2018-06-12
+
+This release is mostly a maintenance release.
+It fixes some bug with respect to **Pinocchio**.
+It also fixes some issues with respect to the packaging.
+
+## [1.0.1] - 2018-01-12
+
+- [joint-posture-task] Fix bug in computation of task matrix from mask
+- [task-se3-equality] Add method to get frame-id
+- [Robot] Add missing evaluation of the center of mass acceleration provided zero joint acceleration
+- [Math] Fix bug related to Eigen undefined function set_is_malloc_allowed
+- [CMake] Correct minimal version of Eigen3
+- [inv-dyn-form-acc-force] Fix potential bug in removal of contact constraint
+- [task-se3-equality] Fix small bug in computaiton of acceleration (just used for debugging)
+- [contact-6d] Add methods to set reference force and weight vector
+- [inv-dyn-form-acc-force] Fix bug: weight of force regularization task was not updated
+- [robot-wrapper] Fix bug in mass matrix: copy upper triangular part to lower triangular part (before this it was set to zero)
+- [formulations] Remove debug prints
+- [formulations] Fix bug in update of task weights
+- [robot-wrapper] BUG FIX: compute center of mass acceleration in computeAllTerms (before it was not computed so we were introducing random number in the CoM task)
+- [tsid-formulations] Add method to change the weight of a task.
+- [eigquadprog-fast] In DEBUG, in case a constraint is not verified, set the status flag to ERROR (even if the solver said the problem has been solved)
+- [task-com-equality] Fix little bugs in methods to get pos/vel references and errors, and in method to get desired acceleration
+- [math-utils] Add function to check if matrix/vector contains NaN
+- [math-utils] Pass JacobiSVD by reference in function solveWithDampingFromSvd
+- [math-utils] Add function to solve linear system of equations from svd decomposition
+
+## [1.0.0] - 2017-06-16
+
+This is the first release of TSID.
+This release includes minimal features for the torque control of humanoid robots such as HRP-2.
+
+[unreleased]: https://github.com/stack-of-tasks/tsid/compare/v1.7.0...HEAD
+[1.7.0]: https://github.com/stack-of-tasks/tsid/compare/v1.6.3...v1.7.0
+[1.6.3]: https://github.com/stack-of-tasks/tsid/compare/v1.6.2...v1.6.3
+[1.6.2]: https://github.com/stack-of-tasks/tsid/compare/v1.6.1...v1.6.2
+[1.6.1]: https://github.com/stack-of-tasks/tsid/compare/v1.6.0...v1.6.1
+[1.6.0]: https://github.com/stack-of-tasks/tsid/compare/v1.5.0...v1.6.0
+[1.5.0]: https://github.com/stack-of-tasks/tsid/compare/v1.4.2...v1.5.0
+[1.4.2]: https://github.com/stack-of-tasks/tsid/compare/v1.4.1...v1.4.2
+[1.4.1]: https://github.com/stack-of-tasks/tsid/compare/v1.4.0...v1.4.1
+[1.4.0]: https://github.com/stack-of-tasks/tsid/compare/v1.3.1...v1.4.0
+[1.3.1]: https://github.com/stack-of-tasks/tsid/compare/v1.3.0...v1.3.1
+[1.3.0]: https://github.com/stack-of-tasks/tsid/compare/v1.2.3...v1.3.0
+[1.2.3]: https://github.com/stack-of-tasks/tsid/compare/v1.2.2...v1.2.3
+[1.2.2]: https://github.com/stack-of-tasks/tsid/compare/v1.2.1...v1.2.2
+[1.2.1]: https://github.com/stack-of-tasks/tsid/compare/v1.2.0...v1.2.1
+[1.2.0]: https://github.com/stack-of-tasks/tsid/compare/v1.1.0...v1.2.0
+[1.1.0]: https://github.com/stack-of-tasks/tsid/compare/v1.0.2...v1.1.0
+[1.0.2]: https://github.com/stack-of-tasks/tsid/compare/v1.0.1...v1.0.2
+[1.0.1]: https://github.com/stack-of-tasks/tsid/compare/v1.0.0...v1.0.1
+[1.0.0]: https://github.com/stack-of-tasks/tsid/releases/tag/v1.0.0
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 11a6356d..3403dc88 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,7 +11,7 @@
# GNU Lesser General Public License along with tsid If not, see
# .
-cmake_minimum_required(VERSION 3.1)
+cmake_minimum_required(VERSION 3.10)
# Project properties
set(PROJECT_ORG stack-of-tasks)
@@ -52,13 +52,25 @@ set(CXX_DISABLE_WERROR TRUE)
set(DOXYGEN_USE_MATHJAX YES)
set(CMAKE_VERBOSE_MAKEFILE TRUE)
-# JRL-cmakemodule setup
+# Check if the submodule cmake have been initialized
set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake")
-if(NOT EXISTS "${CMAKE_SOURCE_DIR}/cmake/base.cmake")
- if(${CMAKE_VERSION} VERSION_LESS "3.14.0")
+if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake")
+ message(STATUS "JRL cmakemodules found in 'cmake/' git submodule")
+else()
+ find_package(jrl-cmakemodules QUIET CONFIG)
+ if(jrl-cmakemodules_FOUND)
+ get_property(
+ JRL_CMAKE_MODULES
+ TARGET jrl-cmakemodules::jrl-cmakemodules
+ PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
+ message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}")
+ elseif(${CMAKE_VERSION} VERSION_LESS "3.14.0")
message(
FATAL_ERROR
- "\nPlease run the following command first:\ngit submodule update --init\n"
+ "\nCan't find jrl-cmakemodules. Please either:\n"
+ " - use git submodule: 'git submodule update --init'\n"
+ " - or install https://github.com/jrl-umi3218/jrl-cmakemodules\n"
+ " - or upgrade your CMake version to >= 3.14 to allow automatic fetching\n"
)
else()
message(STATUS "JRL cmakemodules not found. Let's fetch it.")
@@ -217,20 +229,6 @@ set(${PROJECT_NAME}_FORMULATIONS_HEADERS
include/tsid/formulations/inverse-dynamics-formulation-base.hpp
include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp)
-file(
- GLOB
- ${PYWRAP}_HEADERS
- include/tsid/bindings/python/fwd.hpp
- include/tsid/bindings/python/constraint/*.hpp
- include/tsid/bindings/python/contacts/*.hpp
- include/tsid/bindings/python/formulations/*.hpp
- include/tsid/bindings/python/robots/*.hpp
- include/tsid/bindings/python/solvers/*.hpp
- include/tsid/bindings/python/tasks/*.hpp
- include/tsid/bindings/python/trajectories/*.hpp
- include/tsid/bindings/python/utils/*.hpp
- include/tsid/bindings/python/math/*.hpp)
-
set(${PROJECT_NAME}_HEADERS
include/tsid/macros.hpp
include/tsid/utils/statistics.hpp
@@ -325,13 +323,14 @@ add_source_group(${PROJECT_NAME}_SOURCES)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}
${${PROJECT_NAME}_HEADERS})
-target_include_directories(${PROJECT_NAME} PUBLIC $)
+target_include_directories(
+ ${PROJECT_NAME} PUBLIC $)
target_link_libraries(${PROJECT_NAME} PUBLIC pinocchio::pinocchio
eiquadprog::eiquadprog)
if(BUILD_WITH_PROXQP)
target_compile_definitions(${PROJECT_NAME} PUBLIC -DTSID_WITH_PROXSUITE)
- target_link_libraries(${PROJECT_NAME} PUBLIC ${proxsuite_INTERFACE})
+ target_link_libraries(${PROJECT_NAME} PUBLIC proxsuite::proxsuite)
endif()
if(BUILD_WITH_OSQP)
diff --git a/README.md b/README.md
index a990f37c..13ef22e8 100644
--- a/README.md
+++ b/README.md
@@ -1,9 +1,11 @@
# TSID - Task Space Inverse Dynamics
-[](https://opensource.org/licenses/BSD-2-Clause)
[](https://gitlab.laas.fr/stack-of-tasks/tsid/commits/master)
[](http://projects.laas.fr/gepetto/doc/stack-of-tasks/tsid/master/coverage/)
[](https://badge.fury.io/py/tsid)
+
+[](https://opensource.org/licenses/BSD-2-Clause)
[](https://github.com/psf/black)
+[](https://github.com/astral-sh/ruff)
TSID is a C++ library for optimization-based inverse-dynamics control based on the rigid multi-body dynamics library [Pinocchio](https://github.com/stack-of-tasks/pinocchio).
diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt
index b6abdfec..b73ea18c 100644
--- a/bindings/python/CMakeLists.txt
+++ b/bindings/python/CMakeLists.txt
@@ -12,19 +12,72 @@
# .
# --- LIBRARY --- #
-file(
- GLOB
- ${PYWRAP}_SOURCES
- *.cpp
- constraint/*.cpp
- contacts/*.cpp
- formulations/*.cpp
- robots/*.cpp
- solvers/*.cpp
- tasks/*.cpp
- trajectories/*.cpp
- math/*.cpp
- utils/*.cpp)
+set(${PYWRAP}_SOURCES
+ constraint/constraint-bound.cpp
+ constraint/constraint-equality.cpp
+ constraint/constraint-inequality.cpp
+ contacts/contact-6d.cpp
+ contacts/contact-point.cpp
+ contacts/contact-two-frame-positions.cpp
+ formulations/formulation.cpp
+ math/utils.cpp
+ module.cpp
+ robots/robot-wrapper.cpp
+ solvers/HQPData.cpp
+ solvers/HQPOutput.cpp
+ solvers/solver-HQP-eiquadprog.cpp
+ tasks/task-actuation-bounds.cpp
+ tasks/task-am-equality.cpp
+ tasks/task-com-equality.cpp
+ tasks/task-contact-force-equality.cpp
+ tasks/task-cop-equality.cpp
+ tasks/task-joint-bounds.cpp
+ tasks/task-joint-posture.cpp
+ tasks/task-joint-posVelAcc-bounds.cpp
+ tasks/task-se3-equality.cpp
+ tasks/task-two-frames-equality.cpp
+ trajectories/trajectory-base.cpp
+ trajectories/trajectory-euclidian.cpp
+ trajectories/trajectory-se3.cpp)
+
+set(${PYWRAP}_HEADERS
+ ../../include/tsid/bindings/python/constraint/constraint-bound.hpp
+ ../../include/tsid/bindings/python/constraint/constraint-equality.hpp
+ ../../include/tsid/bindings/python/constraint/constraint-inequality.hpp
+ ../../include/tsid/bindings/python/constraint/expose-constraints.hpp
+ ../../include/tsid/bindings/python/contacts/contact-6d.hpp
+ ../../include/tsid/bindings/python/contacts/contact-point.hpp
+ ../../include/tsid/bindings/python/contacts/contact-two-frame-positions.hpp
+ ../../include/tsid/bindings/python/contacts/contact-two-frames.hpp
+ ../../include/tsid/bindings/python/contacts/expose-contact.hpp
+ ../../include/tsid/bindings/python/formulations/expose-formulations.hpp
+ ../../include/tsid/bindings/python/formulations/formulation.hpp
+ ../../include/tsid/bindings/python/fwd.hpp
+ ../../include/tsid/bindings/python/math/utils.hpp
+ ../../include/tsid/bindings/python/robots/expose-robots.hpp
+ ../../include/tsid/bindings/python/robots/robot-wrapper.hpp
+ ../../include/tsid/bindings/python/solvers/expose-solvers.hpp
+ ../../include/tsid/bindings/python/solvers/HQPData.hpp
+ ../../include/tsid/bindings/python/solvers/HQPOutput.hpp
+ ../../include/tsid/bindings/python/solvers/solver-HQP-eiquadprog.hpp
+ ../../include/tsid/bindings/python/solvers/solver-osqp.hpp
+ ../../include/tsid/bindings/python/solvers/solver-proxqp.hpp
+ ../../include/tsid/bindings/python/tasks/expose-tasks.hpp
+ ../../include/tsid/bindings/python/tasks/task-actuation-bounds.hpp
+ ../../include/tsid/bindings/python/tasks/task-am-equality.hpp
+ ../../include/tsid/bindings/python/tasks/task-com-equality.hpp
+ ../../include/tsid/bindings/python/tasks/task-contact-force-equality.hpp
+ ../../include/tsid/bindings/python/tasks/task-cop-equality.hpp
+ ../../include/tsid/bindings/python/tasks/task-joint-bounds.hpp
+ ../../include/tsid/bindings/python/tasks/task-joint-posture.hpp
+ ../../include/tsid/bindings/python/tasks/task-joint-posVelAcc-bounds.hpp
+ ../../include/tsid/bindings/python/tasks/task-se3-equality.hpp
+ ../../include/tsid/bindings/python/tasks/task-two-frames-equality.hpp
+ ../../include/tsid/bindings/python/trajectories/expose-trajectories.hpp
+ ../../include/tsid/bindings/python/trajectories/trajectory-base.hpp
+ ../../include/tsid/bindings/python/trajectories/trajectory-euclidian.hpp
+ ../../include/tsid/bindings/python/trajectories/trajectory-se3.hpp
+ ../../include/tsid/bindings/python/utils/container.hpp)
add_library(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
target_link_libraries(${PYWRAP} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy)
@@ -64,7 +117,5 @@ install(
set(PYTHON_FILES __init__.py)
foreach(python ${PYTHON_FILES})
- python_build(${PROJECT_NAME} ${python})
- install(FILES "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/tsid/${python}"
- DESTINATION ${${PYWRAP}_INSTALL_DIR})
+ python_install(${PROJECT_NAME} ${python} ${${PYWRAP}_INSTALL_DIR})
endforeach(python)
diff --git a/bindings/python/tsid/__init__.py b/bindings/python/tsid/__init__.py
index 55ada2b0..b2b526d7 100644
--- a/bindings/python/tsid/__init__.py
+++ b/bindings/python/tsid/__init__.py
@@ -1 +1 @@
-from .tsid_pywrap import *
+from .tsid_pywrap import * # noqa: F403
diff --git a/cmake b/cmake
index 17cbc7f7..b3c2af1b 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 17cbc7f73afa4471ecf320d9ec8a2b07c4494f3a
+Subproject commit b3c2af1b68686dc9d5f459fb617647e37a15a76d
diff --git a/demo/demo_quadruped.py b/demo/demo_quadruped.py
index b708671f..24b3747f 100644
--- a/demo/demo_quadruped.py
+++ b/demo/demo_quadruped.py
@@ -2,17 +2,18 @@
import subprocess
import sys
import time
+from pathlib import Path
import gepetto.corbaserver
+import matplotlib.pyplot as plt
import numpy as np
import pinocchio as pin
+import plot_utils as plut
import tsid
from numpy import nan
from numpy.linalg import norm as norm
-sys.path += [os.getcwd() + "/../exercizes"]
-import matplotlib.pyplot as plt
-import plot_utils as plut
+sys.path += [Path.cwd().parent / "exercizes"]
np.set_printoptions(precision=3, linewidth=200, suppress=True)
@@ -42,7 +43,7 @@
DISPLAY_N = 25 # update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION = 6000 # number of time steps simulated
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../models"
urdf = path + "/quadruped/urdf/quadruped.urdf"
vector = pin.StdVec_StdString()
@@ -57,8 +58,8 @@
],
pin.JointModelFreeFlyer(),
)
-l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
-if int(l[1]) == 0:
+n = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
+if int(n[1]) == 0:
os.system("gepetto-gui &")
time.sleep(1)
cl = gepetto.corbaserver.Client()
@@ -182,18 +183,19 @@
com_acc_des[:, i] = comTask.getDesiredAcceleration
if i % PRINT_N == 0:
- print("Time %.3f" % (t))
+ print(f"Time {t:.3f}")
print("\tNormal forces: ", end=" ")
for contact in contacts:
if invdyn.checkContact(contact.name, sol):
f = invdyn.getContactForce(contact.name, sol)
- print("%4.1f" % (contact.getNormalForce(f)), end=" ")
+ print(f"{contact.getNormalForce(f):4.1f}", end=" ")
print(
- "\n\ttracking err %s: %.3f"
- % (comTask.name.ljust(20, "."), norm(comTask.position_error, 2))
+ "\n\ttracking err {}: {:.3f}".format(
+ comTask.name.ljust(20, "."), norm(comTask.position_error, 2)
+ )
)
- print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
+ print(f"\t||v||: {norm(v, 2):.3f}\t ||dv||: {norm(dv):.3f}")
v_mean = v + 0.5 * dt * dv
v += dt * dv
diff --git a/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
index e48918d2..bc01e7c7 100644
--- a/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
+++ b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
@@ -1,15 +1,14 @@
-"""
-Simple demo of usage of ContactTwoFramePositions in TSID
+"""Simple demo of usage of ContactTwoFramePositions in TSID
Make the Talos gripper model works with closed kinematic chains
(c) MIPT
"""
-import os
+
import time
+from pathlib import Path
import numpy as np
import pinocchio as pin
import tsid
-from numpy import nan
from numpy.linalg import norm as norm
np.set_printoptions(precision=3, linewidth=200, suppress=True)
@@ -38,7 +37,7 @@
# Talos gripepr model (c) 2016, PAL Robotics, S.L.
# Please use https://github.com/egordv/tsid_demo_closed_kinematic_chain repo for the model files
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "../../tsid_demo_closed_kinematic_chain/models/talos_gripper"
urdf = path + "../../tsid_demo_closed_kinematic_chain/urdf/talos_gripper_half.urdf"
vector = pin.StdVec_StdString()
@@ -111,12 +110,10 @@
# Setting actuation to zero for passive joints in kinematic chain via TaskActuationBounds
tau_max = model.effortLimit[-robot.na :]
-tau_max[
- 0
-] = 0.0 # setting gripper_left_inner_single_joint to passive contstrainig it's actuation bounds to zero
-tau_max[
- 1
-] = 0.0 # setting gripper_left_fingertip_3_joint to passive contstrainig it's actuation bounds to zero
+# setting gripper_left_inner_single_joint to passive contstrainig it's actuation bounds to zero
+tau_max[0] = 0.0
+# setting gripper_left_fingertip_3_joint to passive contstrainig it's actuation bounds to zero
+tau_max[1] = 0.0
tau_min = -tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
actuationBoundsTask.setBounds(tau_min, tau_max)
@@ -179,7 +176,7 @@
dv = invdyn.getAccelerations(sol)
if i % PRINT_N == 0:
- print("Time %.3f" % (t))
+ print(f"Time {t:.3f}")
v_mean = v + 0.5 * dt * dv
v += dt * dv
diff --git a/dependencies.rosinstall b/dependencies.rosinstall
index 49cd1eb0..29fca791 100644
--- a/dependencies.rosinstall
+++ b/dependencies.rosinstall
@@ -4,7 +4,7 @@
local-name: qpmad
- git:
uri: https://github.com/Simple-Robotics/proxsuite.git
- version: devel
+ version: main
local-name: proxsuite
- git:
uri: https://github.com/ori-drs/osqp.git # fork for packaging
@@ -14,3 +14,7 @@
uri: https://github.com/robotology/osqp-eigen.git
version: master
local-name: osqp-eigen
+- git:
+ uri: https://github.com/stack-of-tasks/eiquadprog.git
+ version: master
+ local-name: eiquadprog
diff --git a/exercizes/ex_0_ur5_joint_space_control.py b/exercizes/ex_0_ur5_joint_space_control.py
index bee28a8e..22ea8470 100644
--- a/exercizes/ex_0_ur5_joint_space_control.py
+++ b/exercizes/ex_0_ur5_joint_space_control.py
@@ -7,11 +7,12 @@
import numpy as np
import pinocchio as pin
import plot_utils as plut
-import tsid
import ur5_conf as conf
from numpy import nan
from numpy.linalg import norm as norm
+import tsid
+
print("".center(conf.LINE_WIDTH, "#"))
print(" Joint Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, "#"))
print("".center(conf.LINE_WIDTH, "#"), "\n")
@@ -54,8 +55,8 @@
conf.path,
],
)
- l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
- if int(l[1]) == 0:
+ n = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
+ if int(n[1]) == 0:
os.system("gepetto-gui &")
time.sleep(1)
gepetto.corbaserver.Client()
@@ -101,7 +102,7 @@
HQPData = formulation.computeProblemData(t, q[:, i], v[:, i])
sol = solver.solve(HQPData)
if sol.status != 0:
- print("Time %.3f QP problem could not be solved! Error code:" % t, sol.status)
+ print(f"Time {t:.3f} QP problem could not be solved! Error code:", sol.status)
break
tau[:, i] = formulation.getActuatorForces(sol)
@@ -109,10 +110,11 @@
dv_des[:, i] = postureTask.getDesiredAcceleration
if i % conf.PRINT_N == 0:
- print("Time %.3f" % (t))
+ print(f"Time {t:.3f}")
print(
- "\ttracking err %s: %.3f"
- % (postureTask.name.ljust(20, "."), norm(postureTask.position_error, 2))
+ "\ttracking err {}: {:.3f}".format(
+ postureTask.name.ljust(20, "."), norm(postureTask.position_error, 2)
+ )
)
# numerical integration
diff --git a/exercizes/ex_1.py b/exercizes/ex_1.py
index ee779c3b..90fd4b6f 100644
--- a/exercizes/ex_1.py
+++ b/exercizes/ex_1.py
@@ -55,26 +55,29 @@
com_acc_des[:, i] = tsid.comTask.getDesiredAcceleration
if i % conf.PRINT_N == 0:
- print("Time %.3f" % (t))
+ print(f"Time {t:.3f}")
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactRF.name, sol)
print(
- "\tnormal force %s: %.1f"
- % (tsid.contactRF.name.ljust(20, "."), tsid.contactRF.getNormalForce(f))
+ "\tnormal force {}: {:.1f}".format(
+ tsid.contactRF.name.ljust(20, "."), tsid.contactRF.getNormalForce(f)
+ )
)
if tsid.formulation.checkContact(tsid.contactLF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactLF.name, sol)
print(
- "\tnormal force %s: %.1f"
- % (tsid.contactLF.name.ljust(20, "."), tsid.contactLF.getNormalForce(f))
+ "\tnormal force {}: {:.1f}".format(
+ tsid.contactLF.name.ljust(20, "."), tsid.contactLF.getNormalForce(f)
+ )
)
print(
- "\ttracking err %s: %.3f"
- % (tsid.comTask.name.ljust(20, "."), norm(tsid.comTask.position_error, 2))
+ "\ttracking err {}: {:.3f}".format(
+ tsid.comTask.name.ljust(20, "."), norm(tsid.comTask.position_error, 2)
+ )
)
- print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
+ print(f"\t||v||: {norm(v, 2):.3f}\t ||dv||: {norm(dv):.3f}")
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
diff --git a/exercizes/ex_1_ur5.py b/exercizes/ex_1_ur5.py
index ea6a5491..8ab385f1 100644
--- a/exercizes/ex_1_ur5.py
+++ b/exercizes/ex_1_ur5.py
@@ -10,9 +10,9 @@
from numpy.linalg import norm as norm
from tsid_manipulator import TsidManipulator
-print(("".center(conf.LINE_WIDTH, "#")))
-print((" TSID - Manipulator End-Effector Sin Tracking ".center(conf.LINE_WIDTH, "#")))
-print(("".center(conf.LINE_WIDTH, "#")))
+print("".center(conf.LINE_WIDTH, "#"))
+print(" TSID - Manipulator End-Effector Sin Tracking ".center(conf.LINE_WIDTH, "#"))
+print("".center(conf.LINE_WIDTH, "#"))
print("")
PLOT_EE_POS = 1
@@ -70,7 +70,7 @@
sol = tsid.solver.solve(HQPData)
if sol.status != 0:
- print(("Time %.3f QP problem could not be solved! Error code:" % t, sol.status))
+ print((f"Time {t:.3f} QP problem could not be solved! Error code:", sol.status))
break
tau[:, i] = tsid.formulation.getActuatorForces(sol)
@@ -89,11 +89,10 @@
ee_acc_des[:, i] = tsid.eeTask.getDesiredAcceleration[:3]
if i % conf.PRINT_N == 0:
- print(("Time %.3f" % (t)))
+ print(f"Time {t:.3f}")
print(
- (
- "\ttracking err %s: %.3f"
- % (tsid.eeTask.name.ljust(20, "."), norm(tsid.eeTask.position_error, 2))
+ "\ttracking err {}: {:.3f}".format(
+ tsid.eeTask.name.ljust(20, "."), norm(tsid.eeTask.position_error, 2)
)
)
@@ -102,9 +101,9 @@
if i % conf.DISPLAY_N == 0:
tsid.robot_display.display(q[:, i])
- tsid.gui.applyConfiguration("world/ee", ee_pos[:, i].tolist() + [0, 0, 0, 1.0])
+ tsid.gui.applyConfiguration("world/ee", [*ee_pos[:, i].tolist(), 0, 0, 0, 1.0])
tsid.gui.applyConfiguration(
- "world/ee_ref", ee_pos_ref[:, i].tolist() + [0, 0, 0, 1.0]
+ "world/ee_ref", [*ee_pos_ref[:, i].tolist(), 0, 0, 0, 1.0]
)
time_spent = time.time() - time_start
diff --git a/exercizes/ex_2.py b/exercizes/ex_2.py
index 4d6e1841..81baec22 100644
--- a/exercizes/ex_2.py
+++ b/exercizes/ex_2.py
@@ -2,7 +2,6 @@
import matplotlib.pyplot as plt
import numpy as np
-import pinocchio as pin
import plot_utils as plut
import romeo_conf as conf
from numpy import nan
@@ -67,26 +66,29 @@
com_acc_des[:, i] = tsid.comTask.getDesiredAcceleration
if i % conf.PRINT_N == 0:
- print("Time %.3f" % (t))
+ print(f"Time {t:.3f}")
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactRF.name, sol)
print(
- "\tnormal force %s: %.1f"
- % (tsid.contactRF.name.ljust(20, "."), tsid.contactRF.getNormalForce(f))
+ "\tnormal force {}: {:.1f}".format(
+ tsid.contactRF.name.ljust(20, "."), tsid.contactRF.getNormalForce(f)
+ )
)
if tsid.formulation.checkContact(tsid.contactLF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactLF.name, sol)
print(
- "\tnormal force %s: %.1f"
- % (tsid.contactLF.name.ljust(20, "."), tsid.contactLF.getNormalForce(f))
+ "\tnormal force {}: {:.1f}".format(
+ tsid.contactLF.name.ljust(20, "."), tsid.contactLF.getNormalForce(f)
+ )
)
print(
- "\ttracking err %s: %.3f"
- % (tsid.comTask.name.ljust(20, "."), norm(tsid.comTask.position_error, 2))
+ "\ttracking err {}: {:.3f}".format(
+ tsid.comTask.name.ljust(20, "."), norm(tsid.comTask.position_error, 2)
+ )
)
- print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
+ print(f"\t||v||: {norm(v, 2):.3f}\t ||dv||: {norm(dv):.3f}")
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
diff --git a/exercizes/ex_3_biped_balance_with_gui.py b/exercizes/ex_3_biped_balance_with_gui.py
index ffd7ad8b..678d051b 100644
--- a/exercizes/ex_3_biped_balance_with_gui.py
+++ b/exercizes/ex_3_biped_balance_with_gui.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Wed Apr 17 22:31:22 2019
+"""Created on Wed Apr 17 22:31:22 2019
@author: student
"""
@@ -27,7 +25,7 @@ def __init__(self, master, name, from_, to, tickinterval, length, orient, comman
for i in range(3):
self.s[i] = Scale(
master,
- label="%s %s" % (name, AXES[i]),
+ label=f"{name} {AXES[i]}",
from_=from_[i],
to=to[i],
tickinterval=tickinterval[i],
@@ -47,7 +45,7 @@ class Entry3d:
def __init__(self, master, name):
self.s = 3 * [None]
for i in range(3):
- Label(master, text="%s %s" % (name, AXES[i])).pack() # side=tk.TOP)
+ Label(master, text=f"{name} {AXES[i]}").pack() # side=tk.TOP)
self.s[i] = Entry(master, width=5)
self.s[i].pack() # side=tk.BOTTOM)
separator = Frame(height=1, bd=1, relief=tk.SUNKEN)
@@ -56,7 +54,7 @@ def __init__(self, master, name):
def get(self):
try:
return [float(self.s[i].get()) for i in range(3)]
- except:
+ except ValueError:
print(
"could not convert string to float", [self.s[i].get() for i in range(3)]
)
@@ -116,8 +114,14 @@ def push_robot():
def create_gui():
- """thread worker function"""
- global scale_com, scale_RF, scale_LF, button_contact_RF, button_contact_LF, com_vel_entry
+ """Thread worker function"""
+ global \
+ scale_com, \
+ scale_RF, \
+ scale_LF, \
+ button_contact_RF, \
+ button_contact_LF, \
+ com_vel_entry
master = Tk(className="TSID GUI")
scale_com = Scale3d(
master,
@@ -218,10 +222,10 @@ def run_simu():
x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]
x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]
vizutils.applyViewerConfiguration(
- tsid.viz, "world/com", x_com.tolist() + [0, 0, 0, 1.0]
+ tsid.viz, "world/com", [*x_com.tolist(), 0, 0, 0, 1.0]
)
vizutils.applyViewerConfiguration(
- tsid.viz, "world/com_ref", x_com_ref.tolist() + [0, 0, 0, 1.0]
+ tsid.viz, "world/com_ref", [*x_com_ref.tolist(), 0, 0, 0, 1.0]
)
vizutils.applyViewerConfiguration(
tsid.viz, "world/rf", pin.SE3ToXYZQUATtuple(H_rf)
@@ -230,16 +234,15 @@ def run_simu():
tsid.viz, "world/lf", pin.SE3ToXYZQUATtuple(H_lf)
)
vizutils.applyViewerConfiguration(
- tsid.viz, "world/rf_ref", x_rf_ref.tolist() + [0, 0, 0, 1.0]
+ tsid.viz, "world/rf_ref", [*x_rf_ref.tolist(), 0, 0, 0, 1.0]
)
vizutils.applyViewerConfiguration(
- tsid.viz, "world/lf_ref", x_lf_ref.tolist() + [0, 0, 0, 1.0]
+ tsid.viz, "world/lf_ref", [*x_lf_ref.tolist(), 0, 0, 0, 1.0]
)
if i % 1000 == 0:
print(
- "Average loop time: %.1f (expected is %.1f)"
- % (1e3 * time_avg, 1e3 * conf.dt)
+ f"Average loop time: {1e3 * time_avg:.1f} (expected is {1e3 * conf.dt:.1f})"
)
time_spent = time.time() - time_start
diff --git a/exercizes/ex_4_conf.py b/exercizes/ex_4_conf.py
index 74f7a860..8eaa0a2b 100644
--- a/exercizes/ex_4_conf.py
+++ b/exercizes/ex_4_conf.py
@@ -1,11 +1,9 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
-import os
+from pathlib import Path
import numpy as np
import pinocchio as pin
@@ -18,7 +16,7 @@
# robot parameters
# ----------------------------------------------
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../models/romeo"
urdf = path + "/urdf/romeo.urdf"
srdf = path + "/srdf/romeo_collision.srdf"
diff --git a/exercizes/ex_4_long_conf.py b/exercizes/ex_4_long_conf.py
index c6cacd4a..603d1ff2 100644
--- a/exercizes/ex_4_long_conf.py
+++ b/exercizes/ex_4_long_conf.py
@@ -1,11 +1,9 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
-import os
+from pathlib import Path
import numpy as np
@@ -17,7 +15,7 @@
# robot parameters
# ----------------------------------------------
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../models/romeo"
urdf = path + "/urdf/romeo.urdf"
srdf = path + "/srdf/romeo_collision.srdf"
diff --git a/exercizes/ex_4_plan_LIPM_romeo.py b/exercizes/ex_4_plan_LIPM_romeo.py
index 4823256e..1b326fee 100644
--- a/exercizes/ex_4_plan_LIPM_romeo.py
+++ b/exercizes/ex_4_plan_LIPM_romeo.py
@@ -35,7 +35,8 @@
import ex_4_conf as conf
import matplotlib.pyplot as plt
-from plot_utils import *
+
+# from plot_utils import *
# import ex_4_long_conf as conf
@@ -155,7 +156,6 @@
plot_utils.plot_xy(
time, N, foot_length, foot_width, cop_ref, cop_x, cop_y, com_state_x, com_state_y
)
-import matplotlib.pyplot as plt
plt.gca().set_xlim([cop_ref[0, 0] - 0.2, cop_ref[-1, 0] + 0.2])
plt.gca().set_ylim([cop_ref[0, 1] - 0.2, cop_ref[-1, 1] + 0.2])
diff --git a/exercizes/ex_4_talos_conf.py b/exercizes/ex_4_talos_conf.py
index 0f76b111..f2c0662e 100644
--- a/exercizes/ex_4_talos_conf.py
+++ b/exercizes/ex_4_talos_conf.py
@@ -1,11 +1,9 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
-import os
+from pathlib import Path
import numpy as np
import pinocchio as pin
@@ -19,12 +17,12 @@
# robot parameters
# ----------------------------------------------
-filename = str(os.path.dirname(os.path.abspath(__file__)))
-urdf = "/talos_data/robots/talos_reduced.urdf"
-modelPath = getModelPath(urdf)
-urdf = modelPath + urdf
-srdf = modelPath + "/talos_data/srdf/talos.srdf"
-path = os.path.join(modelPath, "../..")
+filename = str(Path(__file__).resolve().parent)
+urdf = "talos_data/robots/talos_reduced.urdf"
+modelPath = Path(getModelPath(urdf))
+urdf = modelPath / urdf
+srdf = modelPath / "talos_data/srdf/talos.srdf"
+path = modelPath / "../.."
nv = 38
foot_scaling = 1.0
diff --git a/exercizes/ex_4_walking.py b/exercizes/ex_4_walking.py
index 18d0120c..3bbb82ec 100644
--- a/exercizes/ex_4_walking.py
+++ b/exercizes/ex_4_walking.py
@@ -4,11 +4,12 @@
import matplotlib.pyplot as plt
import numpy as np
import plot_utils as plut
-import tsid
from numpy import nan
from numpy.linalg import norm as norm
from tsid_biped import TsidBiped
+import tsid
+
print("".center(conf.LINE_WIDTH, "#"))
print(" Test Walking ".center(conf.LINE_WIDTH, "#"))
print("".center(conf.LINE_WIDTH, "#"), "\n")
@@ -124,8 +125,7 @@
elif i > 0 and i < N - 1:
if contact_phase[i] != contact_phase[i - 1]:
print(
- "Time %.3f Changing contact phase from %s to %s"
- % (t, contact_phase[i - 1], contact_phase[i])
+ f"Time {t:.3f} Changing contact phase from {contact_phase[i - 1]} to {contact_phase[i]}"
)
if contact_phase[i] == "left":
tsid_biped.add_contact_LF()
@@ -150,7 +150,7 @@
print("QP problem could not be solved! Error code:", sol.status)
break
if norm(v, 2) > 40.0:
- print("Time %.3f Velocities are too high, stop everything!" % (t), norm(v))
+ print(f"Time {t:.3f} Velocities are too high, stop everything!", norm(v))
break
if i > 0:
@@ -180,7 +180,7 @@
cop_RF[0, i] = f_RF[4, i] / f_RF[2, i]
cop_RF[1, i] = -f_RF[3, i] / f_RF[2, i]
if tsid_biped.formulation.checkContact(tsid_biped.contactLF.name, sol):
- T_LF = tsid_biped.contactRF.getForceGeneratorMatrix
+ T_LF = tsid_biped.contactLF.getForceGeneratorMatrix
f_LF[:, i] = T_LF.dot(
tsid_biped.formulation.getContactForce(tsid_biped.contactLF.name, sol)
)
@@ -189,14 +189,15 @@
cop_LF[1, i] = -f_LF[3, i] / f_LF[2, i]
if i % conf.PRINT_N == 0:
- print("Time %.3f" % (t))
+ print(f"Time {t:.3f}")
if (
tsid_biped.formulation.checkContact(tsid_biped.contactRF.name, sol)
and i >= 0
):
print(
- "\tnormal force %s: %.1f"
- % (tsid_biped.contactRF.name.ljust(20, "."), f_RF[2, i])
+ "\tnormal force {}: {:.1f}".format(
+ tsid_biped.contactRF.name.ljust(20, "."), f_RF[2, i]
+ )
)
if (
@@ -204,18 +205,18 @@
and i >= 0
):
print(
- "\tnormal force %s: %.1f"
- % (tsid_biped.contactLF.name.ljust(20, "."), f_LF[2, i])
+ "\tnormal force {}: {:.1f}".format(
+ tsid_biped.contactLF.name.ljust(20, "."), f_LF[2, i]
+ )
)
print(
- "\ttracking err %s: %.3f"
- % (
+ "\ttracking err {}: {:.3f}".format(
tsid_biped.comTask.name.ljust(20, "."),
norm(tsid_biped.comTask.position_error, 2),
)
)
- print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
+ print(f"\t||v||: {norm(v, 2):.3f}\t ||dv||: {norm(dv):.3f}")
q, v = tsid_biped.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
diff --git a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
index 04eee69d..7c17f757 100644
--- a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
+++ b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
@@ -16,7 +16,9 @@
"outputs": [],
"source": [
"import sys\n",
- "sys.path.append('..')\n",
+ "from pathlib import Path\n",
+ "\n",
+ "sys.path.append(\"..\")\n",
"\n",
"import numpy as np\n",
"from numpy.linalg import norm\n",
@@ -25,9 +27,8 @@
"import time\n",
"import pinocchio as pin\n",
"import tsid\n",
- "#import gepetto.corbaserver\n",
- "import subprocess\n",
- "import os\n",
+ "\n",
+ "# import gepetto.corbaserver\n",
"\n",
"import talos_arm_conf as conf"
]
@@ -124,7 +125,9 @@
"jointBoundsTask = tsid.TaskJointBounds(\"task-joint-bounds\", robot, conf.dt)\n",
"jointBoundsTask.setVelocityBounds(v_min, v_max)\n",
"priorityLevel = 0\n",
- "formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, priorityLevel, transitionTime)"
+ "formulation.addMotionTask(\n",
+ " jointBoundsTask, conf.w_joint_bounds, priorityLevel, transitionTime\n",
+ ")"
]
},
{
@@ -157,10 +160,14 @@
"metadata": {},
"outputs": [],
"source": [
- "robot_display = pin.RobotWrapper.BuildFromURDF(conf.urdf, [os.path.join(conf.path, '../..')])\n",
- "#Viewer = pin.visualize.GepettoVisualizer\n",
+ "robot_display = pin.RobotWrapper.BuildFromURDF(\n",
+ " conf.urdf, [str(Path(conf.path) / \"../..\")]\n",
+ ")\n",
+ "# Viewer = pin.visualize.GepettoVisualizer\n",
"Viewer = pin.visualize.MeshcatVisualizer\n",
- "viz = Viewer(robot_display.model, robot_display.collision_model, robot_display.visual_model)\n",
+ "viz = Viewer(\n",
+ " robot_display.model, robot_display.collision_model, robot_display.visual_model\n",
+ ")\n",
"viz.initViewer(loadModel=True)\n",
"viz.display(q0)"
]
@@ -171,7 +178,7 @@
"metadata": {},
"outputs": [],
"source": [
- "hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
+ "hasattr(viz.viewer, \"jupyter_cell\") and viz.viewer.jupyter_cell()"
]
},
{
@@ -188,12 +195,12 @@
"outputs": [],
"source": [
"N = conf.N_SIMULATION\n",
- "tau = np.full((robot.na, N), np.nan)\n",
- "q = np.full((robot.nq, N + 1), np.nan)\n",
- "v = np.full((robot.nv, N + 1), np.nan)\n",
- "dv = np.full((robot.nv, N + 1), np.nan)\n",
- "q_ref = np.full((robot.nq, N), np.nan)\n",
- "v_ref = np.full((robot.nv, N), np.nan)\n",
+ "tau = np.full((robot.na, N), np.nan)\n",
+ "q = np.full((robot.nq, N + 1), np.nan)\n",
+ "v = np.full((robot.nv, N + 1), np.nan)\n",
+ "dv = np.full((robot.nv, N + 1), np.nan)\n",
+ "q_ref = np.full((robot.nq, N), np.nan)\n",
+ "v_ref = np.full((robot.nv, N), np.nan)\n",
"dv_ref = np.full((robot.nv, N), np.nan)\n",
"dv_des = np.full((robot.nv, N), np.nan)\n",
"samplePosture = trajPosture.computeNext()"
@@ -212,10 +219,12 @@
"metadata": {},
"outputs": [],
"source": [
- "amp = np.array([0.2, 0.3, 0.4, 0.0, 0.0, 0.0, 0.0]) # amplitude\n",
- "phi = np.array([0.0, 0.5 * np.pi, 0.0, 0.0, 0.0, 0.0, 0.0]) # phase\n",
- "two_pi_f = 2 * np.pi * np.array([1.0, 0.5, 0.3, 0.0, 0.0, 0.0, 0.0]) # frequency (time 2 PI)\n",
- "two_pi_f_amp = np.multiply(two_pi_f, amp)\n",
+ "amp = np.array([0.2, 0.3, 0.4, 0.0, 0.0, 0.0, 0.0]) # amplitude\n",
+ "phi = np.array([0.0, 0.5 * np.pi, 0.0, 0.0, 0.0, 0.0, 0.0]) # phase\n",
+ "two_pi_f = (\n",
+ " 2 * np.pi * np.array([1.0, 0.5, 0.3, 0.0, 0.0, 0.0, 0.0])\n",
+ ") # frequency (time 2 PI)\n",
+ "two_pi_f_amp = np.multiply(two_pi_f, amp)\n",
"two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)"
]
},
@@ -242,38 +251,42 @@
"\n",
"for i in range(N):\n",
" time_start = time.time()\n",
- " \n",
+ "\n",
" # set reference trajectory\n",
- " q_ref[:,i] = q0 + amp * np.sin(two_pi_f * t + phi)\n",
- " v_ref[:,i] = two_pi_f_amp * np.cos(two_pi_f * t + phi)\n",
- " dv_ref[:,i] = -two_pi_f_squared_amp * np.sin(two_pi_f * t + phi)\n",
+ " q_ref[:, i] = q0 + amp * np.sin(two_pi_f * t + phi)\n",
+ " v_ref[:, i] = two_pi_f_amp * np.cos(two_pi_f * t + phi)\n",
+ " dv_ref[:, i] = -two_pi_f_squared_amp * np.sin(two_pi_f * t + phi)\n",
" samplePosture.pos(q_ref[:, i])\n",
" samplePosture.vel(v_ref[:, i])\n",
" samplePosture.acc(dv_ref[:, i])\n",
" postureTask.setReference(samplePosture)\n",
"\n",
- " HQPData = formulation.computeProblemData(t, q[:,i], v[:,i])\n",
+ " HQPData = formulation.computeProblemData(t, q[:, i], v[:, i])\n",
" sol = solver.solve(HQPData)\n",
" if sol.status != 0:\n",
- " print(\"Time %.3f QP problem could not be solved! Error code:\" % t, sol.status)\n",
+ " print(f\"Time {t:.3f} QP problem could not be solved! Error code:\", sol.status)\n",
" break\n",
- " \n",
- " tau[:,i] = formulation.getActuatorForces(sol)\n",
- " dv[:,i] = formulation.getAccelerations(sol)\n",
- " dv_des[:,i] = postureTask.getDesiredAcceleration\n",
+ "\n",
+ " tau[:, i] = formulation.getActuatorForces(sol)\n",
+ " dv[:, i] = formulation.getAccelerations(sol)\n",
+ " dv_des[:, i] = postureTask.getDesiredAcceleration\n",
"\n",
" if i % conf.PRINT_N == 0:\n",
- " print(\"Time %.3f\" % (t))\n",
- " print(\"\\ttracking err %s: %.3f\" % (postureTask.name.ljust(20, '.'), norm(postureTask.position_error, 2)))\n",
+ " print(f\"Time {t:.3f}\")\n",
+ " print(\n",
+ " \"\\ttracking err {}: {:.3f}\".format(\n",
+ " postureTask.name.ljust(20, \".\"), norm(postureTask.position_error, 2)\n",
+ " )\n",
+ " )\n",
"\n",
" # numerical integration\n",
- " v_mean = v[:,i] + 0.5*dt*dv[:,i]\n",
- " v[:,i + 1] = v[:,i] + dt * dv[:,i]\n",
- " q[:,i + 1] = pin.integrate(model, q[:,i], dt * v_mean)\n",
+ " v_mean = v[:, i] + 0.5 * dt * dv[:, i]\n",
+ " v[:, i + 1] = v[:, i] + dt * dv[:, i]\n",
+ " q[:, i + 1] = pin.integrate(model, q[:, i], dt * v_mean)\n",
" t += conf.dt\n",
- " \n",
+ "\n",
" if i % conf.DISPLAY_N == 0:\n",
- " viz.display(q[:,i])\n",
+ " viz.display(q[:, i])\n",
"\n",
" time_spent = time.time() - time_start\n",
" if time_spent < conf.dt:\n",
@@ -291,10 +304,10 @@
"source": [
"(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
"for i in range(robot.nv):\n",
- " ax[i].plot(time, q[i,:-1], label='q %i' % i)\n",
- " ax[i].plot(time, q_ref[i,:], '--', label='q ref %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('q [rad]')\n",
+ " ax[i].plot(time, q[i, :-1], label=\"q %i\" % i)\n",
+ " ax[i].plot(time, q_ref[i, :], \"--\", label=\"q ref %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"q [rad]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"plt.show()"
@@ -308,12 +321,12 @@
"source": [
"(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
"for i in range(robot.nv):\n",
- " ax[i].plot(time, v[i,:-1], label='v %i ' % i)\n",
- " ax[i].plot(time, v_ref[i,:], '--', label='v ref %i' % i)\n",
- " ax[i].plot([time[0], time[-1]], 2 * [v_min[i]], ':')\n",
- " ax[i].plot([time[0], time[-1]], 2 * [v_max[i]], ':')\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('v [rad/s]')\n",
+ " ax[i].plot(time, v[i, :-1], label=\"v %i \" % i)\n",
+ " ax[i].plot(time, v_ref[i, :], \"--\", label=\"v ref %i\" % i)\n",
+ " ax[i].plot([time[0], time[-1]], 2 * [v_min[i]], \":\")\n",
+ " ax[i].plot([time[0], time[-1]], 2 * [v_max[i]], \":\")\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"v [rad/s]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"plt.show()"
@@ -327,11 +340,11 @@
"source": [
"(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
"for i in range(robot.nv):\n",
- " ax[i].plot(time, dv[i,:-1], label='dv '+str(i))\n",
- " ax[i].plot(time, dv_ref[i,:], '--', label='dv ref %i' % i)\n",
- " ax[i].plot(time, dv_des[i,:], ':', label='dv des %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('dv [rad/s^2]')\n",
+ " ax[i].plot(time, dv[i, :-1], label=\"dv \" + str(i))\n",
+ " ax[i].plot(time, dv_ref[i, :], \"--\", label=\"dv ref %i\" % i)\n",
+ " ax[i].plot(time, dv_des[i, :], \":\", label=\"dv des %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"dv [rad/s^2]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"plt.show()"
@@ -345,9 +358,9 @@
"source": [
"(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
"for i in range(robot.nv):\n",
- " ax[i].plot(time, tau[i,:], label='tau %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('tau [Nm]')\n",
+ " ax[i].plot(time, tau[i, :], label=\"tau %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"tau [Nm]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"plt.show()"
diff --git a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
index 1d707a61..97834dcd 100644
--- a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
+++ b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
@@ -236,18 +236,21 @@
"import matplotlib.pyplot as plt\n",
"import numpy as np\n",
"from numpy.linalg import norm\n",
- "import os\n",
+ "from pathlib import Path\n",
"import time as tmp\n",
"\n",
"# import the library TSID for the Whole-Body Controller\n",
"import tsid\n",
+ "\n",
"# import the pinocchio library for the mathematical methods (Lie algebra) and multi-body dynamics computations.\n",
"import pinocchio as pin\n",
+ "\n",
"# import example_example_robot_data to get the current path for the robot models\n",
"from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR\n",
"\n",
"import sys\n",
- "sys.path.append('..')\n",
+ "\n",
+ "sys.path.append(\"..\")\n",
"\n",
"# import graphical tools\n",
"import plot_utils as plut"
@@ -261,40 +264,70 @@
"source": [
"# Definition of the tasks gains, weights and the foot geometry (for contact task)\n",
"\n",
- "lxp = 0.1 # foot length in positive x direction\n",
- "lxn = 0.11 # foot length in negative x direction\n",
- "lyp = 0.069 # foot length in positive y direction\n",
- "lyn = 0.069 # foot length in negative y direction\n",
- "lz = 0.107 # foot sole height with respect to ankle joint\n",
- "mu = 0.3 # friction coefficient\n",
- "fMin = 1.0 # minimum normal force\n",
+ "lxp = 0.1 # foot length in positive x direction\n",
+ "lxn = 0.11 # foot length in negative x direction\n",
+ "lyp = 0.069 # foot length in positive y direction\n",
+ "lyn = 0.069 # foot length in negative y direction\n",
+ "lz = 0.107 # foot sole height with respect to ankle joint\n",
+ "mu = 0.3 # friction coefficient\n",
+ "fMin = 1.0 # minimum normal force\n",
"fMax = 1000.0 # maximum normal force\n",
"\n",
- "rf_frame_name = \"leg_right_6_joint\" # right foot joint name\n",
- "lf_frame_name = \"leg_left_6_joint\" # left foot joint name\n",
- "contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contact surface\n",
+ "rf_frame_name = \"leg_right_6_joint\" # right foot joint name\n",
+ "lf_frame_name = \"leg_left_6_joint\" # left foot joint name\n",
+ "contactNormal = np.array(\n",
+ " [0.0, 0.0, 1.0]\n",
+ ") # direction of the normal to the contact surface\n",
"\n",
- "w_com = 1.0 # weight of center of mass task\n",
- "w_posture = 0.1 # weight of joint posture task\n",
+ "w_com = 1.0 # weight of center of mass task\n",
+ "w_posture = 0.1 # weight of joint posture task\n",
"w_forceRef = 1e-3 # weight of force regularization task\n",
- "w_waist = 1.0 # weight of waist task\n",
+ "w_waist = 1.0 # weight of waist task\n",
"\n",
"kp_contact = 30.0 # proportional gain of contact constraint\n",
- "kp_com = 20.0 # proportional gain of center of mass task\n",
- "kp_waist = 500.0 # proportional gain of waist task\n",
- "\n",
- "kp_posture = np.array( # proportional gain of joint posture task\n",
- " [10., 5., 5., 1., 1., 10., # lleg, low gain on axis along y and knee\n",
- " 10., 5., 5., 1., 1., 10., # rleg\n",
- " 500., 500., # chest\n",
- " 50., 10., 10., 10., 10., 10., 10., 10., # larm\n",
- " 50., 10., 10., 10., 10., 10., 10., 10., # rarm\n",
- " 100., 100.] # head\n",
+ "kp_com = 20.0 # proportional gain of center of mass task\n",
+ "kp_waist = 500.0 # proportional gain of waist task\n",
+ "\n",
+ "kp_posture = np.array( # proportional gain of joint posture task\n",
+ " [\n",
+ " 10.0,\n",
+ " 5.0,\n",
+ " 5.0,\n",
+ " 1.0,\n",
+ " 1.0,\n",
+ " 10.0, # lleg, low gain on axis along y and knee\n",
+ " 10.0,\n",
+ " 5.0,\n",
+ " 5.0,\n",
+ " 1.0,\n",
+ " 1.0,\n",
+ " 10.0, # rleg\n",
+ " 500.0,\n",
+ " 500.0, # chest\n",
+ " 50.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0, # larm\n",
+ " 50.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0, # rarm\n",
+ " 100.0,\n",
+ " 100.0,\n",
+ " ] # head\n",
")\n",
"\n",
- "dt = 0.001 # controller time step\n",
- "PRINT_N = 500 # print every PRINT_N time steps\n",
- "DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps\n",
+ "dt = 0.001 # controller time step\n",
+ "PRINT_N = 500 # print every PRINT_N time steps\n",
+ "DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps\n",
"N_SIMULATION = 10000 # number of time steps simulated"
]
},
@@ -306,10 +339,10 @@
"source": [
"# Set the path where the urdf file of the robot is registered\n",
"path = EXAMPLE_ROBOT_DATA_MODEL_DIR\n",
- "urdf = path + '/talos_data/robots/talos_reduced.urdf'\n",
+ "urdf = path + \"/talos_data/robots/talos_reduced.urdf\"\n",
"# Create the robot wrapper from the urdf, it will give the model of the robot and its data\n",
"robot = tsid.RobotWrapper(urdf, [path], pin.JointModelFreeFlyer(), False)\n",
- "srdf = path + '/talos_data/srdf/talos.srdf'"
+ "srdf = path + \"/talos_data/srdf/talos.srdf\""
]
},
{
@@ -319,10 +352,14 @@
"outputs": [],
"source": [
"# Creation of the robot wrapper for gepetto viewer (graphical interface)\n",
- "robot_display = pin.RobotWrapper.BuildFromURDF(urdf, [os.path.join(path, '../..')], pin.JointModelFreeFlyer())\n",
- "#Viewer = pin.visualize.GepettoVisualizer\n",
+ "robot_display = pin.RobotWrapper.BuildFromURDF(\n",
+ " urdf, [str(Path(path) / \"../..\")], pin.JointModelFreeFlyer()\n",
+ ")\n",
+ "# Viewer = pin.visualize.GepettoVisualizer\n",
"Viewer = pin.visualize.MeshcatVisualizer\n",
- "viz = Viewer(robot_display.model, robot_display.collision_model, robot_display.visual_model)\n",
+ "viz = Viewer(\n",
+ " robot_display.model, robot_display.collision_model, robot_display.visual_model\n",
+ ")\n",
"viz.initViewer(loadModel=True)"
]
},
@@ -336,7 +373,7 @@
"model = robot.model()\n",
"pin.loadReferenceConfigurations(model, srdf, False)\n",
"# Set the current configuration q to the robot configuration half_sitting\n",
- "q = model.referenceConfigurations['half_sitting']\n",
+ "q = model.referenceConfigurations[\"half_sitting\"]\n",
"# Set the current velocity to zero\n",
"v = np.zeros(robot.nv)"
]
@@ -436,21 +473,23 @@
"source": [
"# COM Task\n",
"comTask = tsid.TaskComEquality(\"task-com\", robot)\n",
- "comTask.setKp(kp_com * np.ones(3)) # Proportional gain defined before = 20\n",
- "comTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(3)) # Derivative gain = 2 * sqrt(20)\n",
+ "comTask.setKp(kp_com * np.ones(3)) # Proportional gain defined before = 20\n",
+ "comTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(3)) # Derivative gain = 2 * sqrt(20)\n",
"# Add the task to the HQP with weight = 1.0, priority level = 0 (as constraint) and a transition duration = 0.0\n",
"invdyn.addMotionTask(comTask, w_com, 0, 0.0)\n",
"\n",
"# WAIST Task\n",
- "waistTask = tsid.TaskSE3Equality(\"task-waist\", robot, 'root_joint') # waist -> root_joint\n",
- "waistTask.setKp(kp_waist * np.ones(6)) # Proportional gain defined before = 500\n",
- "waistTask.setKd(2.0 * np.sqrt(kp_waist) * np.ones(6)) # Derivative gain = 2 * sqrt(500)\n",
+ "waistTask = tsid.TaskSE3Equality(\n",
+ " \"task-waist\", robot, \"root_joint\"\n",
+ ") # waist -> root_joint\n",
+ "waistTask.setKp(kp_waist * np.ones(6)) # Proportional gain defined before = 500\n",
+ "waistTask.setKd(2.0 * np.sqrt(kp_waist) * np.ones(6)) # Derivative gain = 2 * sqrt(500)\n",
"\n",
"# Add a Mask to the task which will select the vector dimensions on which the task will act.\n",
"# In this case the waist configuration is a vector 6d (position and orientation -> SE3)\n",
"# Here we set a mask = [0 0 0 1 1 1] so the task on the waist will act on the orientation of the robot\n",
"mask = np.ones(6)\n",
- "mask[:3] = 0.\n",
+ "mask[:3] = 0.0\n",
"waistTask.setMask(mask)\n",
"# Add the task to the HQP with weight = 1.0, priority level = 1 (in the cost function) and a transition duration = 0.0\n",
"invdyn.addMotionTask(waistTask, w_waist, 1, 0.0)\n",
@@ -458,8 +497,10 @@
"\n",
"# POSTURE Task\n",
"postureTask = tsid.TaskJointPosture(\"task-posture\", robot)\n",
- "postureTask.setKp(kp_posture) # Proportional gain defined before (different for each joints)\n",
- "postureTask.setKd(2.0 * kp_posture) # Derivative gain = 2 * kp\n",
+ "postureTask.setKp(\n",
+ " kp_posture\n",
+ ") # Proportional gain defined before (different for each joints)\n",
+ "postureTask.setKd(2.0 * kp_posture) # Derivative gain = 2 * kp\n",
"# Add the task with weight = 0.1, priority level = 1 (in cost function) and a transition duration = 0.0\n",
"invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)"
]
@@ -549,9 +590,13 @@
"# the friction parameter with the ground (mu = 0.3), the normal force bounds (fMin =1.0, fMax=1000.0)\n",
"\n",
"# Right Foot\n",
- "contactRF = tsid.Contact6d(\"contact_rfoot\", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)\n",
- "contactRF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30\n",
- "contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.ones(6)) # Derivative gain = 2 * sqrt(30)\n",
+ "contactRF = tsid.Contact6d(\n",
+ " \"contact_rfoot\", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax\n",
+ ")\n",
+ "contactRF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30\n",
+ "contactRF.setKd(\n",
+ " 2.0 * np.sqrt(kp_contact) * np.ones(6)\n",
+ ") # Derivative gain = 2 * sqrt(30)\n",
"# Reference position of the right ankle -> initial position\n",
"H_rf_ref = robot.position(data, model.getJointId(rf_frame_name))\n",
"contactRF.setReference(H_rf_ref)\n",
@@ -560,9 +605,13 @@
"invdyn.addRigidContact(contactRF, w_forceRef)\n",
"\n",
"# Left Foot\n",
- "contactLF = tsid.Contact6d(\"contact_lfoot\", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)\n",
- "contactLF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30\n",
- "contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.ones(6)) # Derivative gain = 2 * sqrt(30)\n",
+ "contactLF = tsid.Contact6d(\n",
+ " \"contact_lfoot\", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax\n",
+ ")\n",
+ "contactLF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30\n",
+ "contactLF.setKd(\n",
+ " 2.0 * np.sqrt(kp_contact) * np.ones(6)\n",
+ ") # Derivative gain = 2 * sqrt(30)\n",
"# Reference position of the left ankle -> initial position\n",
"H_lf_ref = robot.position(data, model.getJointId(lf_frame_name))\n",
"contactLF.setReference(H_lf_ref)\n",
@@ -594,14 +643,20 @@
"source": [
"# Set the reference trajectory of the tasks\n",
"\n",
- "com_ref = data.com[0] # Initial value of the CoM\n",
+ "com_ref = data.com[0] # Initial value of the CoM\n",
"trajCom = tsid.TrajectoryEuclidianConstant(\"traj_com\", com_ref)\n",
- "sampleCom = trajCom.computeNext() # Compute the first step of the trajectory from the initial value\n",
+ "sampleCom = (\n",
+ " trajCom.computeNext()\n",
+ ") # Compute the first step of the trajectory from the initial value\n",
"\n",
- "q_ref = q[7:] # Initial value of the joints of the robot (in halfSitting position without the freeFlyer (6 first values))\n",
+ "q_ref = q[\n",
+ " 7:\n",
+ "] # Initial value of the joints of the robot (in halfSitting position without the freeFlyer (6 first values))\n",
"trajPosture = tsid.TrajectoryEuclidianConstant(\"traj_joint\", q_ref)\n",
"\n",
- "waist_ref = robot.position(data, model.getJointId('root_joint')) # Initial value of the waist (root_joint)\n",
+ "waist_ref = robot.position(\n",
+ " data, model.getJointId(\"root_joint\")\n",
+ ") # Initial value of the waist (root_joint)\n",
"# Here the waist is defined as a 6d vector (position + orientation) so it is in the SE3 group (Lie group)\n",
"# Thus, the trajectory is not Euclidian but remains in the SE3 domain -> TrajectorySE3Constant\n",
"trajWaist = tsid.TrajectorySE3Constant(\"traj_waist\", waist_ref)"
@@ -617,7 +672,7 @@
"# Use EiquadprogFast: dynamic matrix sizes (memory allocation performed only when resizing)\n",
"solver = tsid.SolverHQuadProgFast(\"qp solver\")\n",
"# Resize the solver to fit the number of variables, equality and inequality constraints\n",
- "solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn) "
+ "solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)"
]
},
{
@@ -635,7 +690,7 @@
"com_pos_ref = np.full((3, N_SIMULATION), np.nan)\n",
"com_vel_ref = np.full((3, N_SIMULATION), np.nan)\n",
"com_acc_ref = np.full((3, N_SIMULATION), np.nan)\n",
- "com_acc_des = np.full((3, N_SIMULATION), np.nan) "
+ "com_acc_des = np.full((3, N_SIMULATION), np.nan)"
]
},
{
@@ -646,11 +701,15 @@
"source": [
"# Parametes of the CoM sinusoid\n",
"\n",
- "offset = robot.com(data) # offset of the measured CoM\n",
+ "offset = robot.com(data) # offset of the measured CoM\n",
"amp = np.array([0.0, 0.05, 0.0]) # amplitude function of 0.05 along the y axis\n",
- "two_pi_f = 2 * np.pi * np.array([0.0, 0.5, 0.0]) # 2π function along the y axis with 0.5 amplitude\n",
+ "two_pi_f = (\n",
+ " 2 * np.pi * np.array([0.0, 0.5, 0.0])\n",
+ ") # 2π function along the y axis with 0.5 amplitude\n",
"two_pi_f_amp = two_pi_f * amp # 2π function times amplitude function\n",
- "two_pi_f_squared_amp = two_pi_f * two_pi_f_amp # 2π function times squared amplitude function"
+ "two_pi_f_squared_amp = (\n",
+ " two_pi_f * two_pi_f_amp\n",
+ ") # 2π function times squared amplitude function"
]
},
{
@@ -707,26 +766,38 @@
" tau = invdyn.getActuatorForces(sol)\n",
" dv = invdyn.getAccelerations(sol)\n",
"\n",
- " com_pos[:,i] = robot.com(invdyn.data())\n",
- " com_vel[:,i] = robot.com_vel(invdyn.data())\n",
- " com_acc[:,i] = comTask.getAcceleration(dv)\n",
- " com_pos_ref[:,i] = sampleCom.pos()\n",
- " com_vel_ref[:,i] = sampleCom.vel()\n",
- " com_acc_ref[:,i] = sampleCom.acc()\n",
- " com_acc_des[:,i] = comTask.getDesiredAcceleration\n",
+ " com_pos[:, i] = robot.com(invdyn.data())\n",
+ " com_vel[:, i] = robot.com_vel(invdyn.data())\n",
+ " com_acc[:, i] = comTask.getAcceleration(dv)\n",
+ " com_pos_ref[:, i] = sampleCom.pos()\n",
+ " com_vel_ref[:, i] = sampleCom.vel()\n",
+ " com_acc_ref[:, i] = sampleCom.acc()\n",
+ " com_acc_des[:, i] = comTask.getDesiredAcceleration\n",
"\n",
" if i % PRINT_N == 0:\n",
- " print(\"Time %.3f\" % t)\n",
+ " print(f\"Time {t:.3f}\")\n",
" if invdyn.checkContact(contactRF.name, sol):\n",
" f = invdyn.getContactForce(contactRF.name, sol)\n",
- " print(\"\\tnormal force %s: %.1f\" % (contactRF.name.ljust(20, '.'), contactRF.getNormalForce(f)))\n",
+ " print(\n",
+ " \"\\tnormal force {}: {:.1f}\".format(\n",
+ " contactRF.name.ljust(20, \".\"), contactRF.getNormalForce(f)\n",
+ " )\n",
+ " )\n",
"\n",
" if invdyn.checkContact(contactLF.name, sol):\n",
" f = invdyn.getContactForce(contactLF.name, sol)\n",
- " print(\"\\tnormal force %s: %.1f\" % (contactLF.name.ljust(20, '.'), contactLF.getNormalForce(f)))\n",
- "\n",
- " print(\"\\ttracking err %s: %.3f\" % (comTask.name.ljust(20, '.'), norm(comTask.position_error, 2)))\n",
- " print(\"\\t||v||: %.3f\\t ||dv||: %.3f\" % (norm(v, 2), norm(dv)))\n",
+ " print(\n",
+ " \"\\tnormal force {}: {:.1f}\".format(\n",
+ " contactLF.name.ljust(20, \".\"), contactLF.getNormalForce(f)\n",
+ " )\n",
+ " )\n",
+ "\n",
+ " print(\n",
+ " \"\\ttracking err {}: {:.3f}\".format(\n",
+ " comTask.name.ljust(20, \".\"), norm(comTask.position_error, 2)\n",
+ " )\n",
+ " )\n",
+ " print(f\"\\t||v||: {norm(v, 2):.3f}\\t ||dv||: {norm(dv):.3f}\")\n",
"\n",
" v_mean = v + 0.5 * dt * dv\n",
" v += dt * dv\n",
@@ -737,7 +808,7 @@
" viz.display(q)\n",
"\n",
" time_spent = tmp.time() - time_start\n",
- " if time_spent < dt: \n",
+ " if time_spent < dt:\n",
" tmp.sleep(dt - time_spent)"
]
},
@@ -761,10 +832,10 @@
"\n",
"(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
"for i in range(3):\n",
- " ax[i].plot(time, com_pos[i,:], label='CoM %i' % i)\n",
- " ax[i].plot(time, com_pos_ref[i,:], 'r:', label='CoM Ref %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('CoM [m]')\n",
+ " ax[i].plot(time, com_pos[i, :], label=\"CoM %i\" % i)\n",
+ " ax[i].plot(time, com_pos_ref[i, :], \"r:\", label=\"CoM Ref %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"CoM [m]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"\n",
@@ -781,10 +852,10 @@
"\n",
"(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
"for i in range(3):\n",
- " ax[i].plot(time, com_vel[i,:], label='CoM Vel %i' % i)\n",
- " ax[i].plot(time, com_vel_ref[i,:], 'r:', label='CoM Vel Ref %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('CoM Vel [m/s]')\n",
+ " ax[i].plot(time, com_vel[i, :], label=\"CoM Vel %i\" % i)\n",
+ " ax[i].plot(time, com_vel_ref[i, :], \"r:\", label=\"CoM Vel Ref %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"CoM Vel [m/s]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"\n",
@@ -803,11 +874,11 @@
"\n",
"(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
"for i in range(3):\n",
- " ax[i].plot(time, com_acc[i,:], label='CoM Acc %i' % i)\n",
- " ax[i].plot(time, com_acc_ref[i,:], 'r:', label='CoM Acc Ref %i' % i)\n",
- " ax[i].plot(time, com_acc_des[i,:], 'g--', label='CoM Acc Des %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('CoM Acc [m/s^2]')\n",
+ " ax[i].plot(time, com_acc[i, :], label=\"CoM Acc %i\" % i)\n",
+ " ax[i].plot(time, com_acc_ref[i, :], \"r:\", label=\"CoM Acc Ref %i\" % i)\n",
+ " ax[i].plot(time, com_acc_des[i, :], \"g--\", label=\"CoM Acc Des %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"CoM Acc [m/s^2]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"\n",
diff --git a/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb b/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
index c63fad6d..882a2415 100644
--- a/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
+++ b/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
@@ -10,13 +10,12 @@
"import time\n",
"import sys\n",
"\n",
- "from IPython.display import display\n",
"from ipywidgets import interact\n",
"import ipywidgets as widgets\n",
"import numpy as np\n",
"import pinocchio as pin\n",
"\n",
- "sys.path.append('..')\n",
+ "sys.path.append(\"..\")\n",
"import talos_conf as conf\n",
"import vizutils\n",
"from tsid_biped import TsidBiped"
@@ -32,15 +31,27 @@
"tsid.q0[2] = 1.02127\n",
"\n",
"com_0 = tsid.robot.com(tsid.formulation.data())\n",
- "H_rf_0 = tsid.robot.framePosition(tsid.formulation.data(), tsid.model.getFrameId(conf.rf_frame_name))\n",
- "H_lf_0 = tsid.robot.framePosition(tsid.formulation.data(), tsid.model.getFrameId(conf.lf_frame_name))\n",
- "\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/com', conf.SPHERE_RADIUS, conf.COM_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/com_ref', conf.REF_SPHERE_RADIUS, conf.COM_REF_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/rf', conf.SPHERE_RADIUS, conf.RF_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/rf_ref', conf.REF_SPHERE_RADIUS, conf.RF_REF_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/lf', conf.SPHERE_RADIUS, conf.LF_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/lf_ref', conf.REF_SPHERE_RADIUS, conf.LF_REF_SPHERE_COLOR)"
+ "H_rf_0 = tsid.robot.framePosition(\n",
+ " tsid.formulation.data(), tsid.model.getFrameId(conf.rf_frame_name)\n",
+ ")\n",
+ "H_lf_0 = tsid.robot.framePosition(\n",
+ " tsid.formulation.data(), tsid.model.getFrameId(conf.lf_frame_name)\n",
+ ")\n",
+ "\n",
+ "vizutils.addViewerSphere(\n",
+ " tsid.viz, \"world/com\", conf.SPHERE_RADIUS, conf.COM_SPHERE_COLOR\n",
+ ")\n",
+ "vizutils.addViewerSphere(\n",
+ " tsid.viz, \"world/com_ref\", conf.REF_SPHERE_RADIUS, conf.COM_REF_SPHERE_COLOR\n",
+ ")\n",
+ "vizutils.addViewerSphere(tsid.viz, \"world/rf\", conf.SPHERE_RADIUS, conf.RF_SPHERE_COLOR)\n",
+ "vizutils.addViewerSphere(\n",
+ " tsid.viz, \"world/rf_ref\", conf.REF_SPHERE_RADIUS, conf.RF_REF_SPHERE_COLOR\n",
+ ")\n",
+ "vizutils.addViewerSphere(tsid.viz, \"world/lf\", conf.SPHERE_RADIUS, conf.LF_SPHERE_COLOR)\n",
+ "vizutils.addViewerSphere(\n",
+ " tsid.viz, \"world/lf_ref\", conf.REF_SPHERE_RADIUS, conf.LF_REF_SPHERE_COLOR\n",
+ ")"
]
},
{
@@ -109,19 +120,31 @@
" H_rf = tsid.robot.framePosition(tsid.formulation.data(), tsid.RF)\n",
" x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]\n",
" x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/com', x_com.tolist() + [0, 0, 0, 1.])\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/com_ref', x_com_ref.tolist() + [0, 0, 0, 1.])\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/rf', pin.SE3ToXYZQUATtuple(H_rf))\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/lf', pin.SE3ToXYZQUATtuple(H_lf))\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/rf_ref', x_rf_ref.tolist() + [0, 0, 0, 1.])\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/lf_ref', x_lf_ref.tolist() + [0, 0, 0, 1.])\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/com\", [*x_com.tolist(), 0, 0, 0, 1.0]\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/com_ref\", [*x_com_ref.tolist(), 0, 0, 0, 1.0]\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/rf\", pin.SE3ToXYZQUATtuple(H_rf)\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/lf\", pin.SE3ToXYZQUATtuple(H_lf)\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/rf_ref\", [*x_rf_ref.tolist(), 0, 0, 0, 1.0]\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/lf_ref\", [*x_lf_ref.tolist(), 0, 0, 0, 1.0]\n",
+ " )\n",
"\n",
" time_spent = time.time() - time_start\n",
" time_avg = (i * time_avg + time_spent) / (i + 1)\n",
"\n",
" if time_avg < 0.9 * conf.dt:\n",
" time.sleep(conf.dt - time_avg)\n",
- " print('end of simulation')"
+ " print(\"end of simulation\")"
]
},
{
@@ -136,41 +159,44 @@
"th_simu.start()\n",
"\n",
"\n",
- "@interact(com_x=(-10., 10.), com_y=(-15., 15.), com_z=(-40., 40.))\n",
+ "@interact(com_x=(-10.0, 10.0), com_y=(-15.0, 15.0), com_z=(-40.0, 40.0))\n",
"def sliders_com(com_x, com_y, com_z):\n",
- " tsid.trajCom.setReference(com_0 + np.array([1e-2 * com_x, 1e-2 * com_y, 1e-2 * com_z]).T)\n",
+ " tsid.trajCom.setReference(\n",
+ " com_0 + np.array([1e-2 * com_x, 1e-2 * com_y, 1e-2 * com_z]).T\n",
+ " )\n",
"\n",
"\n",
- "@interact(rf_x=(-30., 30.), rf_y=(-30., 30.), rf_z=(-30., 30.))\n",
+ "@interact(rf_x=(-30.0, 30.0), rf_y=(-30.0, 30.0), rf_z=(-30.0, 30.0))\n",
"def sliders_rf(rf_x, rf_y, rf_z):\n",
" H_rf_ref = H_rf_0.copy()\n",
" H_rf_ref.translation += np.array([1e-2 * rf_x, 1e-2 * rf_y, 1e-2 * rf_z]).T\n",
" tsid.trajRF.setReference(H_rf_ref)\n",
"\n",
"\n",
- "@interact(lf_x=(-30., 30.), lf_y=(-30., 30.), lf_z=(-30., 30.))\n",
+ "@interact(lf_x=(-30.0, 30.0), lf_y=(-30.0, 30.0), lf_z=(-30.0, 30.0))\n",
"def sliders_lf(lf_x, lf_y, lf_z):\n",
" H_lf_ref = H_lf_0.copy()\n",
- " H_lf_ref.translation += + np.array([1e-2 * lf_x, 1e-2 * lf_y, 1e-2 * lf_z]).T\n",
+ " H_lf_ref.translation += +np.array([1e-2 * lf_x, 1e-2 * lf_y, 1e-2 * lf_z]).T\n",
" tsid.trajLF.setReference(H_lf_ref)\n",
"\n",
"\n",
"def buttons(b):\n",
- " if b.description == 'Stop':\n",
+ " if b.description == \"Stop\":\n",
" run.clear()\n",
" th_simu.join()\n",
- " elif b.description == 'Switch Right Foot':\n",
+ " elif b.description == \"Switch Right Foot\":\n",
" if tsid.contact_RF_active:\n",
" tsid.remove_contact_RF()\n",
" else:\n",
" tsid.add_contact_RF()\n",
- " elif b.description == 'Switch Left Foot':\n",
+ " elif b.description == \"Switch Left Foot\":\n",
" if tsid.contact_LF_active:\n",
" tsid.remove_contact_LF()\n",
" else:\n",
- " tsid.add_contact_LF() \n",
+ " tsid.add_contact_LF()\n",
"\n",
- "for action in ['Stop', 'Switch Right Foot', 'Switch Left Foot']:\n",
+ "\n",
+ "for action in [\"Stop\", \"Switch Right Foot\", \"Switch Left Foot\"]:\n",
" button = widgets.Button(description=action)\n",
" button.on_click(buttons)\n",
" output.append_display_data(button)"
@@ -182,7 +208,7 @@
"metadata": {},
"outputs": [],
"source": [
- "hasattr(tsid.viz.viewer, 'jupyter_cell') and tsid.viz.viewer.jupyter_cell()"
+ "hasattr(tsid.viz.viewer, \"jupyter_cell\") and tsid.viz.viewer.jupyter_cell()"
]
},
{
diff --git a/exercizes/plot_utils.py b/exercizes/plot_utils.py
index f33c63ef..81a136e5 100644
--- a/exercizes/plot_utils.py
+++ b/exercizes/plot_utils.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Fri Jan 16 09:16:56 2015
+"""Created on Fri Jan 16 09:16:56 2015
@author: adelpret
"""
@@ -66,7 +64,7 @@
def create_empty_figure(nRows=1, nCols=1, figsize=(7, 7), spinesPos=None, sharex=True):
f, ax = plt.subplots(nRows, nCols, figsize=figsize, sharex=sharex)
- mngr = plt.get_current_fig_manager()
+ _mngr = plt.get_current_fig_manager()
# mngr.window.setGeometry(50,50,1080,720)
if spinesPos is not None:
@@ -90,7 +88,7 @@ def movePlotSpines(ax, spinesPos):
def setAxisFontSize(ax, size):
for label in ax.get_xticklabels() + ax.get_yticklabels():
label.set_fontsize(size)
- label.set_bbox(dict(facecolor="white", edgecolor="None", alpha=0.65))
+ label.set_bbox({"facecolor": "white", "edgecolor": "None", "alpha": 0.65})
mpl.rcdefaults()
@@ -160,7 +158,7 @@ def plotNdQuantity(
)
-def plotNdQuantityPerSolver(
+def plotNdQuantityPerSolver( # noqa: C901
nRows,
nCols,
quantity,
@@ -307,7 +305,7 @@ def plotNdQuantityPerSolver(
ax[nRows // 2, 0].set_ylabel(title)
if SHOW_LEGENDS:
# leg = ax[0,0].legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=2, mode="expand", borderaxespad=0.)
- leg = ax[0, 0].legend(loc="best")
+ _leg = ax[0, 0].legend(loc="best")
# leg.get_frame().set_alpha(LEGEND_ALPHA)
return ax
diff --git a/exercizes/romeo_conf.py b/exercizes/romeo_conf.py
index a2040151..20cb8c01 100644
--- a/exercizes/romeo_conf.py
+++ b/exercizes/romeo_conf.py
@@ -1,11 +1,9 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
-import os
+from pathlib import Path
import numpy as np
import pinocchio as pin
@@ -74,7 +72,7 @@
LF_SPHERE_COLOR = (0, 0, 1, 0.5)
LF_REF_SPHERE_COLOR = (0.5, 0, 1, 0.5)
-filename = str(os.path.dirname(os.path.abspath(__file__)))
-path = filename + "/../models/romeo"
-urdf = path + "/urdf/romeo.urdf"
-srdf = path + "/srdf/romeo_collision.srdf"
+filename = Path(__file__).resolve().parent
+path = filename / "../models/romeo"
+urdf = path / "urdf/romeo.urdf"
+srdf = path / "srdf/romeo_collision.srdf"
diff --git a/exercizes/talos_arm_conf.py b/exercizes/talos_arm_conf.py
index a8a198ef..dd8f00d4 100644
--- a/exercizes/talos_arm_conf.py
+++ b/exercizes/talos_arm_conf.py
@@ -1,3 +1,5 @@
+from pathlib import Path
+
import numpy as np
from example_robot_data.robots_loader import getModelPath
@@ -45,5 +47,5 @@
EE_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
urdf = "/talos_data/robots/talos_left_arm.urdf"
-path = getModelPath(urdf)
-urdf = path + urdf
+path = Path(getModelPath(urdf))
+urdf = path / urdf
diff --git a/exercizes/talos_conf.py b/exercizes/talos_conf.py
index 4e8d9333..40098f98 100644
--- a/exercizes/talos_conf.py
+++ b/exercizes/talos_conf.py
@@ -1,4 +1,4 @@
-import os
+from pathlib import Path
import numpy as np
import pinocchio as pin
@@ -104,7 +104,7 @@
LF_REF_SPHERE_COLOR = (0.5, 0, 1, 0.5)
urdf = "/talos_data/robots/talos_reduced.urdf"
-path = getModelPath(urdf)
-urdf = path + urdf
-srdf = path + "/talos_data/srdf/talos.srdf"
-path = os.path.join(path, "../..")
+path = Path(getModelPath(urdf))
+urdf = path / urdf
+srdf = path / "talos_data/srdf/talos.srdf"
+path = path / "../.."
diff --git a/exercizes/test_cop_task.py b/exercizes/test_cop_task.py
index 6300c45f..c9a6e256 100644
--- a/exercizes/test_cop_task.py
+++ b/exercizes/test_cop_task.py
@@ -1,10 +1,10 @@
-""" This script is a slight variation of ex_4_walking.py introduced to test the new
- center of pressure (CoP) task. In this script, besides tracking a reference
- center of mass (CoM), the TSID controller also tries to track a reference CoP.
- The resulting motion doesn't look great because the reference CoP is an open-loop
- reference trajectory, so it is not stabilizing, and it conflicts with the CoM task.
- However, the results show that the CoP is tracked reasonably well during the motion,
- which was the goal of the test, validating the CoP task.
+"""This script is a slight variation of ex_4_walking.py introduced to test the new
+center of pressure (CoP) task. In this script, besides tracking a reference
+center of mass (CoM), the TSID controller also tries to track a reference CoP.
+The resulting motion doesn't look great because the reference CoP is an open-loop
+reference trajectory, so it is not stabilizing, and it conflicts with the CoM task.
+However, the results show that the CoP is tracked reasonably well during the motion,
+which was the goal of the test, validating the CoP task.
"""
import time
@@ -96,8 +96,7 @@
elif i > 0 and i < N - 1:
if contact_phase[i] != contact_phase[i - 1]:
print(
- "Time %.3f Changing contact phase from %s to %s"
- % (t, contact_phase[i - 1], contact_phase[i])
+ f"Time {t:.3f} Changing contact phase from {contact_phase[i - 1]} to {contact_phase[i]}"
)
if contact_phase[i] == "left":
tsid.add_contact_LF()
@@ -129,7 +128,7 @@
print("QP problem could not be solved! Error code:", sol.status)
break
if norm(v, 2) > 40.0:
- print("Time %.3f Velocities are too high, stop everything!" % (t), norm(v))
+ print(f"Time {t:.3f} Velocities are too high, stop everything!", norm(v))
break
if i > 0:
@@ -177,24 +176,27 @@
)
if i % conf.PRINT_N == 0:
- print("Time %.3f" % (t))
+ print(f"Time {t:.3f}")
if tsid.formulation.checkContact(tsid.contactRF.name, sol) and i >= 0:
print(
- "\tnormal force %s: %.1f"
- % (tsid.contactRF.name.ljust(20, "."), f_RF[2, i])
+ "\tnormal force {}: {:.1f}".format(
+ tsid.contactRF.name.ljust(20, "."), f_RF[2, i]
+ )
)
if tsid.formulation.checkContact(tsid.contactLF.name, sol) and i >= 0:
print(
- "\tnormal force %s: %.1f"
- % (tsid.contactLF.name.ljust(20, "."), f_LF[2, i])
+ "\tnormal force {}: {:.1f}".format(
+ tsid.contactLF.name.ljust(20, "."), f_LF[2, i]
+ )
)
print(
- "\ttracking err %s: %.3f"
- % (tsid.comTask.name.ljust(20, "."), norm(tsid.comTask.position_error, 2))
+ "\ttracking err {}: {:.3f}".format(
+ tsid.comTask.name.ljust(20, "."), norm(tsid.comTask.position_error, 2)
+ )
)
- print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
+ print(f"\t||v||: {norm(v, 2):.3f}\t ||dv||: {norm(dv):.3f}")
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
diff --git a/exercizes/tsid_biped.py b/exercizes/tsid_biped.py
index 58a229c6..a391d1fb 100644
--- a/exercizes/tsid_biped.py
+++ b/exercizes/tsid_biped.py
@@ -4,6 +4,7 @@
import numpy as np
import pinocchio as pin
+
import tsid
@@ -176,8 +177,6 @@ def __init__(self, conf, viewer=pin.visualize.MeshcatVisualizer):
conf.urdf, [conf.path], pin.JointModelFreeFlyer()
)
if viewer == pin.visualize.GepettoVisualizer:
- import gepetto.corbaserver
-
launched = subprocess.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l"
)
diff --git a/exercizes/tsid_manipulator.py b/exercizes/tsid_manipulator.py
index c8001270..0c1bb228 100644
--- a/exercizes/tsid_manipulator.py
+++ b/exercizes/tsid_manipulator.py
@@ -4,8 +4,8 @@
import gepetto.corbaserver
import numpy as np
-import numpy.matlib as matlib
import pinocchio as se3
+
import tsid
@@ -23,13 +23,13 @@ def __init__(self, conf, viewer=True):
robot = self.robot
self.model = model = robot.model()
try:
- # q = se3.getNeutralConfiguration(model, conf.srdf, False)
+ # q = se3.getNeutralConfiguration(model, conf.srdf, False)
se3.loadReferenceConfigurations(model, conf.srdf, False)
q = model.referenceConfigurations["default"]
- # q = model.referenceConfigurations["half_sitting"]
- except:
+ # q = model.referenceConfigurations["half_sitting"]
+ except Exception:
q = conf.q0
- # q = np.array(np.zeros(robot.nv)).T
+ # q = np.array(np.zeros(robot.nv)).T
v = np.zeros(robot.nv)
assert model.existFrame(conf.ee_frame_name)
@@ -93,10 +93,10 @@ def __init__(self, conf, viewer=True):
conf.path,
],
)
- l = subprocess.getstatusoutput(
+ n = subprocess.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l"
)
- if int(l[1]) == 0:
+ if int(n[1]) == 0:
os.system("gepetto-gui &")
time.sleep(1)
gepetto.corbaserver.Client()
diff --git a/exercizes/ur5_conf.py b/exercizes/ur5_conf.py
index 97f4f815..3ca6c139 100644
--- a/exercizes/ur5_conf.py
+++ b/exercizes/ur5_conf.py
@@ -1,13 +1,12 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
-import os
+from pathlib import Path
import numpy as np
+from example_robot_data.robots_loader import getModelPath
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
@@ -53,11 +52,8 @@
EE_SPHERE_COLOR = (1, 0.5, 0, 0.5)
EE_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
-from os.path import join
-
-from example_robot_data.robots_loader import getModelPath
urdf = "ur_description/urdf/ur5_robot.urdf"
-path = getModelPath(urdf)
-urdf = join(path, urdf)
-path = join(path, "../..")
+path = Path(getModelPath(urdf))
+urdf = path / urdf
+path = path / "../.."
diff --git a/exercizes/ur5_reaching_conf.py b/exercizes/ur5_reaching_conf.py
index cfa9f626..f2501167 100644
--- a/exercizes/ur5_reaching_conf.py
+++ b/exercizes/ur5_reaching_conf.py
@@ -1,13 +1,12 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
-import os
+from pathlib import Path
import numpy as np
+from example_robot_data.robots_loader import getModelPath
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
@@ -53,11 +52,8 @@
EE_SPHERE_COLOR = (1, 0.5, 0, 0.5)
EE_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
-from os.path import join
-
-from example_robot_data.robots_loader import getModelPath
urdf = "ur_description/urdf/ur5_robot.urdf"
-path = getModelPath(urdf)
-urdf = join(path, urdf)
-path = join(path, "../..")
+path = Path(getModelPath(urdf))
+urdf = path / urdf
+path = path / "../.."
diff --git a/exercizes/vizutils.py b/exercizes/vizutils.py
index c6b20b6e..0a67098d 100644
--- a/exercizes/vizutils.py
+++ b/exercizes/vizutils.py
@@ -30,7 +30,8 @@ def addViewerBox(viz, name, sizex, sizey, sizez, rgba):
elif isinstance(viz, pin.visualize.GepettoVisualizer):
viz.viewer.gui.addBox(name, sizex, sizey, sizez, rgba)
else:
- raise AttributeError("Viewer %s is not supported." % viz.__class__)
+ msg = f"Viewer {viz.__class__} is not supported."
+ raise AttributeError(msg)
def addViewerSphere(viz, name, size, rgba):
@@ -43,7 +44,8 @@ def addViewerSphere(viz, name, size, rgba):
elif isinstance(viz, pin.visualize.GepettoVisualizer):
viz.viewer.gui.addSphere(name, size, rgba)
else:
- raise AttributeError("Viewer %s is not supported." % viz.__class__)
+ msg = f"Viewer {viz.__class__} is not supported."
+ raise AttributeError(msg)
def applyViewerConfiguration(viz, name, xyzquat):
@@ -53,7 +55,8 @@ def applyViewerConfiguration(viz, name, xyzquat):
viz.viewer.gui.applyConfiguration(name, xyzquat)
viz.viewer.gui.refresh()
else:
- raise AttributeError("Viewer %s is not supported." % viz.__class__)
+ msg = f"Viewer {viz.__class__} is not supported."
+ raise AttributeError(msg)
"""
diff --git a/include/tsid/macros.hpp b/include/tsid/macros.hpp
index 0dcc4254..db76f7cc 100644
--- a/include/tsid/macros.hpp
+++ b/include/tsid/macros.hpp
@@ -17,8 +17,10 @@
#define TSID_DISABLE_WARNING_POP TSID_DO_PRAGMA(GCC diagnostic pop)
#define TSID_DISABLE_WARNING(warningName) \
TSID_DO_PRAGMA(GCC diagnostic ignored #warningName)
+// clang-format off
#define TSID_DISABLE_WARNING_DEPRECATED \
- TSID_DISABLE_WARNING(-Wdeprecated - declarations)
+ TSID_DISABLE_WARNING(-Wdeprecated-declarations)
+// clang-format on
#else
diff --git a/include/tsid/solvers/solver-HQP-base.hpp b/include/tsid/solvers/solver-HQP-base.hpp
index 1c668d6c..0211b9d3 100644
--- a/include/tsid/solvers/solver-HQP-base.hpp
+++ b/include/tsid/solvers/solver-HQP-base.hpp
@@ -42,7 +42,7 @@ class TSID_DLLAPI SolverHQPBase {
typedef math::ConstRefMatrix ConstRefMatrix;
SolverHQPBase(const std::string& name);
- virtual ~SolverHQPBase(){};
+ virtual ~SolverHQPBase() {};
virtual const std::string& name() const { return m_name; }
diff --git a/include/tsid/solvers/solver-HQP-eiquadprog-rt.hpp b/include/tsid/solvers/solver-HQP-eiquadprog-rt.hpp
index b7f5624b..e819c6d7 100644
--- a/include/tsid/solvers/solver-HQP-eiquadprog-rt.hpp
+++ b/include/tsid/solvers/solver-HQP-eiquadprog-rt.hpp
@@ -51,7 +51,7 @@ class TSID_DLLAPI SolverHQuadProgRT : public SolverHQPBase {
// TODO: change eiquadprog-rt to new API
/** Retrieve the matrices describing a QP problem from the problem data. */
void retrieveQPData(const HQPData& /*problemData*/,
- const bool /*hessianRegularization = true*/){};
+ const bool /*hessianRegularization = true*/) {};
// /** Return the QP data object. */
// const QPDataQuadProg getQPData() const { return m_qpData; }
diff --git a/include/tsid/tasks/task-contact-force.hpp b/include/tsid/tasks/task-contact-force.hpp
index 72b85fc1..f7b3ce64 100644
--- a/include/tsid/tasks/task-contact-force.hpp
+++ b/include/tsid/tasks/task-contact-force.hpp
@@ -39,7 +39,9 @@ class TaskContactForce : public TaskBase {
* tasks that involve all contacts, such as the CoP task.
*/
TSID_DISABLE_WARNING_PUSH
- TSID_DISABLE_WARNING(-Woverloaded - virtual)
+ // clang-format off
+ TSID_DISABLE_WARNING(-Woverloaded-virtual)
+ // clang-format on
virtual const ConstraintBase& compute(
const double t, ConstRefVector q, ConstRefVector v, Data& data,
const std::vector >* contacts) = 0;
diff --git a/pyproject.toml b/pyproject.toml
index 4441909f..56ec9123 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -1,5 +1,9 @@
-[tool.isort]
-profile = "black"
+[tool.ruff]
+extend-exclude = ["cmake"]
+
+[tool.ruff.lint]
+extend-ignore = ["COM812", "D203", "D213"]
+extend-select = ["A", "B", "C", "COM", "EM", "EXE", "G", "NPY", "PTH", "RET", "RUF", "UP", "W", "YTT"]
[tool.tomlsort]
all = true
diff --git a/tests/python/generator.py b/tests/python/generator.py
index 78b19335..b162d001 100644
--- a/tests/python/generator.py
+++ b/tests/python/generator.py
@@ -6,15 +6,18 @@
def create_7dof_arm(revoluteOnly=False):
- """
- Create a 7 DoF robot arm (with spherical joint for shoulder and wrist and revolute joint for elbow)
+ """Create a 7 DoF robot arm (with spherical joint for shoulder and wrist and revolute
+ joint for elbow)
Optionnal parameters:
- revoluteOnly (default=False): if True, the arm is created with only revolute joints. (Spherical joints are replaced by 3 revolute joints)
+ revoluteOnly (default=False): if True, the arm is created with only revolute
+ joints. (Spherical joints are replaced by 3 revolute joints)
Return:
+ ------
model: pinocchio model of the robot
geom_model: pinocchio geometric model of the robot
+
"""
# Some useful values
geom_radius = 0.1 # radius of the arm geometries
@@ -255,7 +258,7 @@ def set_limit(model, joint_id, pos_absmax, vel_absmax, eff_absmax):
id_hand,
fcl.Cylinder(0.02, 0.1),
pin.XYZQUATToSE3(
- np.array([geom_radius, 0, 2 * geom_radius] + [0, 0.707, 0, 0.707])
+ np.array([geom_radius, 0, 2 * geom_radius, 0, 0.707, 0, 0.707])
),
"",
np.ones(3),
diff --git a/tests/python/test_Constraint.py b/tests/python/test_Constraint.py
index 7110b517..f6280b1b 100644
--- a/tests/python/test_Constraint.py
+++ b/tests/python/test_Constraint.py
@@ -1,5 +1,5 @@
import numpy as np
-import pinocchio as se3
+
import tsid
print("")
diff --git a/tests/python/test_Contact.py b/tests/python/test_Contact.py
index 4872e919..3e5eee5e 100644
--- a/tests/python/test_Contact.py
+++ b/tests/python/test_Contact.py
@@ -1,7 +1,8 @@
-import copy
+from pathlib import Path
import numpy as np
import pinocchio as se3
+
import tsid
print("")
@@ -9,9 +10,8 @@
print("")
tol = 1e-5
-import os
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = se3.StdVec_StdString()
diff --git a/tests/python/test_ContactPoint.py b/tests/python/test_ContactPoint.py
index 00e5f24b..4f4aad3f 100644
--- a/tests/python/test_ContactPoint.py
+++ b/tests/python/test_ContactPoint.py
@@ -1,5 +1,4 @@
-import copy
-import os
+from pathlib import Path
import numpy as np
import pinocchio as se3
@@ -10,7 +9,8 @@
print("")
tol = 1e-5
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
+
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = se3.StdVec_StdString()
diff --git a/tests/python/test_Deprecations.py b/tests/python/test_Deprecations.py
index 588ce382..dc1b9e76 100644
--- a/tests/python/test_Deprecations.py
+++ b/tests/python/test_Deprecations.py
@@ -1,8 +1,7 @@
-import sys
import unittest
-import warnings
import numpy as np
+
import tsid
@@ -11,28 +10,12 @@ def test_trajectory(self):
q_ref = np.ones(5)
traj_euclidian = tsid.TrajectoryEuclidianConstant("traj_eucl", q_ref)
- if sys.version_info >= (3, 2):
- with self.assertWarns(UserWarning):
- traj_euclidian.computeNext().pos()
- with self.assertWarns(UserWarning):
- traj_euclidian.computeNext().vel()
- with self.assertWarns(UserWarning):
- traj_euclidian.computeNext().acc()
- else:
- with warnings.catch_warnings(record=True) as w:
- self.assertEqual(len(w), 0)
-
- traj_euclidian.computeNext().pos()
- self.assertEqual(len(w), 1)
- self.assertEqual(w[-1].category, UserWarning)
-
- traj_euclidian.computeNext().vel()
- self.assertEqual(len(w), 2)
- self.assertEqual(w[-1].category, UserWarning)
-
- traj_euclidian.computeNext().acc()
- self.assertEqual(len(w), 3)
- self.assertEqual(w[-1].category, UserWarning)
+ with self.assertWarns(UserWarning):
+ traj_euclidian.computeNext().pos()
+ with self.assertWarns(UserWarning):
+ traj_euclidian.computeNext().vel()
+ with self.assertWarns(UserWarning):
+ traj_euclidian.computeNext().acc()
if __name__ == "__main__":
diff --git a/tests/python/test_Formulation.py b/tests/python/test_Formulation.py
index 07d3af06..3fa9fc88 100644
--- a/tests/python/test_Formulation.py
+++ b/tests/python/test_Formulation.py
@@ -1,4 +1,4 @@
-import os
+from pathlib import Path
import numpy as np
import pinocchio as se3
@@ -9,7 +9,7 @@
print("Test InvDyn")
print("")
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = se3.StdVec_StdString()
@@ -98,7 +98,7 @@
postureTask.setKd(2.0 * np.sqrt(kp_posture) * np.ones(robot.nv - 6))
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
-########### Test 1 ##################3
+# ########## Test 1 ##################3
dt = 0.01
PRINT_N = 100
REMOVE_CONTACT_N = 100
diff --git a/tests/python/test_Gravity.py b/tests/python/test_Gravity.py
index c85d4fb8..1b0e4f8b 100644
--- a/tests/python/test_Gravity.py
+++ b/tests/python/test_Gravity.py
@@ -1,6 +1,5 @@
-import os
+from pathlib import Path
-import numpy as np
import pinocchio as se3
import tsid
@@ -9,7 +8,7 @@
print("")
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = se3.StdVec_StdString()
diff --git a/tests/python/test_RobotWrapper.py b/tests/python/test_RobotWrapper.py
index 6e7bb073..9bf240c5 100644
--- a/tests/python/test_RobotWrapper.py
+++ b/tests/python/test_RobotWrapper.py
@@ -1,7 +1,8 @@
-import os
+from pathlib import Path
import numpy as np
import pinocchio as se3
+
import tsid
print("")
@@ -9,7 +10,7 @@
print("")
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = se3.StdVec_StdString()
diff --git a/tests/python/test_Solvers.py b/tests/python/test_Solvers.py
index eb8e2896..d295f2df 100644
--- a/tests/python/test_Solvers.py
+++ b/tests/python/test_Solvers.py
@@ -5,6 +5,8 @@
print("Test Solvers")
print("")
+rng = np.random.default_rng(seed=0)
+
EPS = 1e-3
nTest = 100
n = 60
@@ -38,6 +40,7 @@
try:
solver_proxqp = tsid.SolverProxQP("proxqp solver")
+ solver_proxqp.set_epsilon_absolute(1e-6)
solver_proxqp.resize(n, neq, nin)
solver_list.append(("proxqp", solver_proxqp))
print("Adding proxqp to list of solvers to test")
@@ -47,6 +50,8 @@
try:
solver_osqp = tsid.SolverOSQP("osqp solver")
+ solver_osqp.set_epsilon_absolute(1e-6)
+ solver_osqp.set_maximum_iterations(10_000)
solver_osqp.resize(n, neq, nin)
solver_list.append(("osqp", solver_osqp))
print("Adding osqp to list of solvers to test")
@@ -55,14 +60,15 @@
pass
HQPData = tsid.HQPData()
-A1 = np.random.rand(n, n) + 0.001 * np.eye(n)
-b1 = np.random.rand(n)
+A1 = rng.random((n, n)) + 0.001 * np.eye(n)
+b1 = rng.random(n)
cost = tsid.ConstraintEquality("c1", A1, b1)
-x = np.linalg.inv(A1).dot(b1)
-A_in = np.random.rand(nin, n)
-A_lb = np.random.rand(nin) * NORMAL_DISTR_VAR
-A_ub = np.random.rand(nin) * NORMAL_DISTR_VAR
+x = np.linalg.solve(A1, b1)
+
+A_in = rng.random((nin, n))
+A_lb = rng.random(nin) * NORMAL_DISTR_VAR
+A_ub = rng.random(nin) * NORMAL_DISTR_VAR
constrVal = A_in.dot(x)
for i in range(0, nin):
@@ -76,7 +82,7 @@
A_lb[i] = constrVal[i] - MARGIN_PERC * np.abs(constrVal[i])
in_const = tsid.ConstraintInequality("ini1", A_in, A_lb, A_ub)
-A_eq = np.random.rand(neq, n)
+A_eq = rng.random((neq, n))
b_eq = A_eq.dot(x)
eq_const = tsid.ConstraintEquality("eq1", A_eq, b_eq)
@@ -98,9 +104,9 @@
gradientPerturbations = []
hessianPerturbations = []
-for i in range(0, nTest):
- gradientPerturbations.append(np.random.rand(n) * GRADIENT_PERTURBATION_VARIANCE)
- hessianPerturbations.append(np.random.rand(n, n) * HESSIAN_PERTURBATION_VARIANCE)
+for _ in range(0, nTest):
+ gradientPerturbations.append(rng.random(n) * GRADIENT_PERTURBATION_VARIANCE)
+ hessianPerturbations.append(rng.random((n, n)) * HESSIAN_PERTURBATION_VARIANCE)
for name, solver in solver_list:
print(f"Using {name}")
@@ -110,6 +116,7 @@
HQPoutput = solver.solve(HQPData)
+ assert HQPoutput.status == 0 # HQP_STATUS_OPTIMAL = 0
assert np.linalg.norm(A_eq.dot(HQPoutput.x) - b_eq, 2) < EPS
assert (A_in.dot(HQPoutput.x) <= A_ub + EPS).all()
assert (A_in.dot(HQPoutput.x) > A_lb - EPS).all()
diff --git a/tests/python/test_Tasks.py b/tests/python/test_Tasks.py
index 59711dc1..d4122cf5 100644
--- a/tests/python/test_Tasks.py
+++ b/tests/python/test_Tasks.py
@@ -1,17 +1,20 @@
-import os
+from pathlib import Path
import numpy as np
import pinocchio as pin
import tsid
from numpy.linalg import norm
+# Get robot model generator module
+from generator import create_7dof_arm
+
print("")
print("Test Task COM")
print("")
tol = 1e-5
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = pin.StdVec_StdString()
@@ -98,7 +101,8 @@
assert np.linalg.norm(Kp - task_joint.Kp, 2) < tol
assert np.linalg.norm(Kd - task_joint.Kd, 2) < tol
-q_ref = np.random.randn(na)
+rng = np.random.default_rng()
+q_ref = rng.standard_normal(na)
traj = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
sample = tsid.TrajectorySample(0)
@@ -200,7 +204,7 @@
print("")
tol = 1e-5
-filename = str(os.path.dirname(os.path.abspath(__file__)))
+filename = str(Path(__file__).resolve().parent)
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = pin.StdVec_StdString()
@@ -235,7 +239,7 @@
max_it = 1000
Jpinv = np.zeros((robot.nv, 3))
error_past = 1e100
-v = np.random.randn(robot.nv)
+v = rng.standard_normal(robot.nv)
for i in range(0, max_it):
robot.computeAllTerms(data, q, v)
@@ -278,8 +282,6 @@
print("Test Task Joint Posture (Uncommon joints)")
print("")
-# Get robot model generator module
-from generator import create_7dof_arm
# Get robot model
(
diff --git a/tests/python/test_Trajectories.py b/tests/python/test_Trajectories.py
index 8c0acdd5..0674e989 100644
--- a/tests/python/test_Trajectories.py
+++ b/tests/python/test_Trajectories.py
@@ -1,5 +1,6 @@
import numpy as np
import pinocchio as se3
+
import tsid
print("")