From b6e412bb2b754ef317c761a4a22e8a36bf93e8ac Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Sat, 14 Sep 2024 10:56:20 +0200 Subject: [PATCH 01/11] ruff: select E, F, I, RUF, UP & W --- CHANGELOG.md | 4 ++ bindings/python/pinocchio/deprecated.py | 5 +- bindings/python/pinocchio/deprecation.py | 4 +- bindings/python/pinocchio/derivative/dcrba.py | 29 ++++---- .../python/pinocchio/derivative/lambdas.py | 20 +++--- bindings/python/pinocchio/derivative/xm.py | 3 +- bindings/python/pinocchio/robot_wrapper.py | 31 +++++--- bindings/python/pinocchio/romeo_wrapper.py | 4 +- bindings/python/pinocchio/shortcuts.py | 18 +++-- bindings/python/pinocchio/utils.py | 2 +- .../pinocchio/visualize/base_visualizer.py | 44 ++++++++---- .../pinocchio/visualize/gepetto_visualizer.py | 20 +++--- .../pinocchio/visualize/meshcat_visualizer.py | 71 ++++++++++--------- .../pinocchio/visualize/panda3d_visualizer.py | 13 ++-- .../pinocchio/visualize/rviz_visualizer.py | 14 ++-- .../python/pinocchio/windows_dll_manager.py | 8 +-- doc/d-practical-exercises/src/continuous.py | 9 ++- doc/d-practical-exercises/src/display.py | 20 +++--- doc/d-practical-exercises/src/dpendulum.py | 1 - doc/d-practical-exercises/src/factor.py | 27 +++---- doc/d-practical-exercises/src/foot_steps.py | 29 ++++---- doc/d-practical-exercises/src/graph.py | 6 +- doc/d-practical-exercises/src/mobilerobot.py | 16 +++-- doc/d-practical-exercises/src/ocp.py | 7 +- doc/d-practical-exercises/src/pendulum.py | 13 ++-- doc/d-practical-exercises/src/prm_display.py | 9 +-- doc/d-practical-exercises/src/qnet.py | 4 +- doc/d-practical-exercises/src/qtable.py | 1 - doc/d-practical-exercises/src/robot_hand.py | 29 ++++---- doc/d-practical-exercises/src/ur5x4.py | 11 +-- examples/anymal-simulation.py | 6 +- .../append-urdf-model-with-another-model.py | 14 ++-- examples/build-reduced-model.py | 9 ++- examples/capsule-approximation.py | 14 ++-- examples/casadi/cartpole.py | 12 ++-- examples/casadi/quadrotor-ocp.py | 3 +- .../simulation-pendulum-variational.ipynb | 5 +- examples/cassie-simulation.py | 2 +- examples/codegen/codegen-rnea.py | 10 +-- examples/collision-with-point-clouds.py | 21 +++--- examples/collisions.py | 5 +- examples/contact-cholesky.py | 4 +- examples/cppad/autodiff-rnea.py | 6 +- examples/display-shapes-meshcat.py | 3 +- examples/display-shapes.py | 9 ++- examples/floating-base-velocity-viewer.py | 8 +-- examples/forward-dynamics-derivatives.py | 6 +- examples/geometry-models.py | 13 ++-- examples/gepetto-viewer.py | 10 +-- examples/inverse-dynamics-derivatives.py | 6 +- examples/inverse-kinematics.py | 13 ++-- examples/kinematics-derivatives.py | 8 ++- examples/meshcat-viewer-dae.py | 16 +++-- examples/meshcat-viewer-octree.py | 12 ++-- examples/meshcat-viewer-solo.py | 6 +- examples/meshcat-viewer.py | 14 ++-- examples/overview-simple.py | 2 - examples/overview-urdf.py | 12 ++-- .../reachable-workspace-with-collisions.py | 9 ++- examples/reachable-workspace.py | 9 ++- examples/robot-wrapper-viewer.py | 5 +- examples/run-algo-in-parallel.py | 6 +- examples/rviz-viewer.py | 4 +- examples/sample-model-viewer.py | 11 +-- .../simulation-closed-kinematic-chains.py | 10 ++- examples/simulation-contact-dynamics.py | 21 +++--- examples/simulation-inverted-pendulum.py | 12 ++-- examples/simulation-pendulum.py | 18 ++--- examples/static-contact-dynamics.py | 52 ++++++++------ examples/talos-simulation.py | 4 +- examples/update-model-after-urdf.py | 3 +- models/simple_model.py | 12 ++-- pyproject.toml | 5 ++ unittest/python/bindings.py | 5 +- unittest/python/bindings_Symmetric3.py | 4 +- unittest/python/bindings_aba.py | 4 +- ...indings_centroidal_dynamics_derivatives.py | 8 +-- unittest/python/bindings_com.py | 5 +- .../bindings_contact_inverse_dynamics.py | 6 +- unittest/python/bindings_data.py | 2 +- unittest/python/bindings_dynamics.py | 1 - unittest/python/bindings_fcl_transform.py | 1 + unittest/python/bindings_force.py | 5 +- .../bindings_forward_dynamics_derivatives.py | 8 +-- unittest/python/bindings_frame.py | 6 +- unittest/python/bindings_frame_derivatives.py | 8 +-- unittest/python/bindings_geometry_model.py | 1 + .../python/bindings_geometry_model_urdf.py | 3 +- unittest/python/bindings_geometry_object.py | 3 +- unittest/python/bindings_inertia.py | 1 - .../bindings_inverse_dynamics_derivatives.py | 8 +-- unittest/python/bindings_joint_algorithms.py | 16 ++--- unittest/python/bindings_joint_composite.py | 1 + unittest/python/bindings_joints.py | 16 ++--- .../python/bindings_kinematic_regressor.py | 3 +- unittest/python/bindings_kinematics.py | 4 +- .../python/bindings_kinematics_derivatives.py | 8 +-- unittest/python/bindings_liegroups.py | 1 - unittest/python/bindings_model.py | 2 +- unittest/python/bindings_motion.py | 5 +- unittest/python/bindings_regressor.py | 1 - unittest/python/bindings_rnea.py | 4 +- unittest/python/bindings_sample_models.py | 1 - unittest/python/bindings_spatial.py | 5 +- unittest/python/bindings_std_map.py | 9 ++- unittest/python/bindings_std_vector.py | 9 ++- unittest/python/bindings_urdf.py | 3 +- unittest/python/casadi/bindings_explog.py | 5 +- unittest/python/casadi/bindings_main_algo.py | 14 ++-- unittest/python/explog.py | 10 ++- unittest/python/pybind11/test-cpp2pybind11.py | 5 +- unittest/python/robot_wrapper.py | 1 - unittest/python/rpy.py | 16 ++--- unittest/python/serialization.py | 1 - unittest/python/test_case.py | 2 +- unittest/python/utils.py | 1 - unittest/python/version.py | 4 +- 117 files changed, 608 insertions(+), 520 deletions(-) create mode 100644 pyproject.toml diff --git a/CHANGELOG.md b/CHANGELOG.md index ff13e0681f..96ebc1f476 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Fix mjcf parser appending of inertias at root joint ([#2403](https://github.com/stack-of-tasks/pinocchio/pull/2403)) - Fix unit tests with GCC 13.3 ([#2406](https://github.com/stack-of-tasks/pinocchio/pull/2416) +### Changed + +- Modernize python code base with ruff ([#2418](https://github.com/stack-of-tasks/pinocchio/pull/2418)) + ## [3.2.0] - 2024-08-27 ### Fixed diff --git a/bindings/python/pinocchio/deprecated.py b/bindings/python/pinocchio/deprecated.py index 62726f7fbe..437ab70f0b 100644 --- a/bindings/python/pinocchio/deprecated.py +++ b/bindings/python/pinocchio/deprecated.py @@ -2,6 +2,5 @@ # Copyright (c) 2018-2023 CNRS INRIA # -## In this file, are reported some deprecated functions that are still maintained until the next important future releases ## - -from __future__ import print_function +## In this file, are reported some deprecated functions +# that are still maintained until the next important future releases diff --git a/bindings/python/pinocchio/deprecation.py b/bindings/python/pinocchio/deprecation.py index 1b4686043f..aaee02e2f4 100644 --- a/bindings/python/pinocchio/deprecation.py +++ b/bindings/python/pinocchio/deprecation.py @@ -20,9 +20,7 @@ def decorator(func): @functools.wraps(func) def wrapper(*args, **kwargs): - message = "Call to deprecated function {}. {}".format( - func.__name__, instructions - ) + message = f"Call to deprecated function {func.__name__}. {instructions}" warnings.warn(message, category=DeprecatedWarning, stacklevel=2) diff --git a/bindings/python/pinocchio/derivative/dcrba.py b/bindings/python/pinocchio/derivative/dcrba.py index a0d75cc23e..250097f377 100644 --- a/bindings/python/pinocchio/derivative/dcrba.py +++ b/bindings/python/pinocchio/derivative/dcrba.py @@ -2,19 +2,14 @@ # Copyright (c) 2016 CNRS # -from __future__ import print_function +import lambdas import numpy as np import pinocchio as pin -from numpy.linalg import norm -from pinocchio.robot_wrapper import RobotWrapper -from pinocchio.utils import rand - -import lambdas from lambdas import ( FCross, - Mcross, MCross, + Mcross, adj, adjdual, ancestors, @@ -23,6 +18,9 @@ quad, td, ) +from numpy.linalg import norm +from pinocchio.robot_wrapper import RobotWrapper +from pinocchio.utils import rand def hessian(robot, q, crossterms=False): @@ -111,9 +109,12 @@ def __call__(self): T_jSd = np.array(H[:, d, j0:j1]) # this is 0 is d<=j """ - assert( norm(T_iSd)<1e-6 or not joint_diff TiSd=0 - assert( norm(T_jSd)<1e-6 or not joint_diff TjSd=0 - assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 ) # TiSd=0 => TjSd=0 + # d TiSd=0 + assert( norm(T_iSd)<1e-6 or not joint_diff TjSd=0 + assert( norm(T_jSd)<1e-6 or not joint_diff TjSd=0 + assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 ) assert( norm(T_iSd)>1e-6 ) """ @@ -123,7 +124,8 @@ def __call__(self): if j < joint_diff: dM[i0:i1, j0:j1, d] += Si.T * Yd * T_jSd - # Make dM triangular by copying strict-upper triangle to lower one. + # Make dM triangular by copying strict-upper triangle to lower + # one. if i != j: dM[j0:j1, i0:i1, d] = dM[i0:i1, j0:j1, d].T else: @@ -203,7 +205,8 @@ def __call__(self, q): H[:, k0:k1, j0:j1], YJ[:, i0:i1], [0, 0] ) - # Fill the border elements of levels below k Q[kk,k,:] and Q[kk,:,k] with kk eps else " 0. " + mformat = lambda x, sarg=sarg, eps=eps: ( + sarg.format(x) if abs(x) > eps else " 0. " ) np.set_printoptions(formatter={"float": mformat}) diff --git a/bindings/python/pinocchio/derivative/xm.py b/bindings/python/pinocchio/derivative/xm.py index 9e0df9b254..f9374d1d13 100644 --- a/bindings/python/pinocchio/derivative/xm.py +++ b/bindings/python/pinocchio/derivative/xm.py @@ -4,12 +4,11 @@ import numpy as np import pinocchio as pin +from dcrba import DCRBA, DRNEA, Coriolis from numpy.linalg import norm from pinocchio.robot_wrapper import RobotWrapper from pinocchio.utils import rand -from dcrba import DCRBA, DRNEA, Coriolis - np.random.seed(0) robot = RobotWrapper( diff --git a/bindings/python/pinocchio/robot_wrapper.py b/bindings/python/pinocchio/robot_wrapper.py index 4749049ee5..1c4ac21887 100644 --- a/bindings/python/pinocchio/robot_wrapper.py +++ b/bindings/python/pinocchio/robot_wrapper.py @@ -12,7 +12,7 @@ ) -class RobotWrapper(object): +class RobotWrapper: @staticmethod def BuildFromURDF( filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None @@ -160,13 +160,16 @@ def centroidalMomentum(self, q, v): def centroidalMap(self, q): """ - Computes the centroidal momentum matrix which maps from the joint velocity vector to the centroidal momentum expressed around the center of mass. + Computes the centroidal momentum matrix which maps from the joint velocity + vector to the centroidal momentum expressed around the center of mass. """ return pin.computeCentroidalMap(self.model, self.data, q) def centroidal(self, q, v): """ - Computes all the quantities related to the centroidal dynamics (hg, Ag and Ig), corresponding to the centroidal momentum, the centroidal map and the centroidal rigid inertia. + Computes all the quantities related to the centroidal dynamics (hg, Ag and Ig), + corresponding to the centroidal momentum, the centroidal map and the centroidal + rigid inertia. """ pin.ccrba(self.model, self.data, q, v) return (self.data.hg, self.data.Ag, self.data.Ig) @@ -350,15 +353,16 @@ def buildReducedRobot(self, list_of_joints_to_lock, reference_configuration=None def getFrameJacobian(self, frame_id, rf_frame=pin.ReferenceFrame.LOCAL): """ - It computes the Jacobian of frame given by its id (frame_id) either expressed in the - local coordinate frame or in the world coordinate frame. + It computes the Jacobian of frame given by its id (frame_id) either expressed in + the local coordinate frame or in the world coordinate frame. """ return pin.getFrameJacobian(self.model, self.data, frame_id, rf_frame) def computeFrameJacobian(self, q, frame_id): """ Similar to getFrameJacobian but does not need pin.computeJointJacobians and - pin.updateFramePlacements to update internal value of self.data related to frames. + pin.updateFramePlacements to update internal value of self.data related to + frames. """ return pin.computeFrameJacobian(self.model, self.data, q, frame_id) @@ -396,8 +400,10 @@ def viewer(self): return self.viz.viewer def setVisualizer(self, visualizer, init=True, copy_models=False): - """Set the visualizer. If init is True, the visualizer is initialized with this wrapper's models. - If copy_models is also True, the models are copied. Otherwise, they are simply kept as a reference. + """ + Set the visualizer. If init is True, the visualizer is initialized with this + wrapper's models. If copy_models is also True, the models are copied. + Otherwise, they are simply kept as a reference. """ if init: visualizer.__init__( @@ -406,7 +412,10 @@ def setVisualizer(self, visualizer, init=True, copy_models=False): self.viz = visualizer def getViewerNodeName(self, geometry_object, geometry_type): - """For each geometry object, returns the corresponding name of the node in the display.""" + """ + For each geometry object, returns the corresponding name of the node in the + display. + """ return self.viz.getViewerNodeName(geometry_object, geometry_type) def initViewer(self, share_data=True, *args, **kwargs): @@ -437,7 +446,9 @@ def loadViewerModel(self, *args, **kwargs): self.viz.loadViewerModel(*args, **kwargs) def display(self, q): - """Display the robot at configuration q in the viewer by placing all the bodies.""" + """ + Display the robot at configuration q in the viewer by placing all the bodies. + """ self.viz.display(q) def displayCollisions(self, visibility): diff --git a/bindings/python/pinocchio/romeo_wrapper.py b/bindings/python/pinocchio/romeo_wrapper.py index f371a84252..c886ddbea7 100644 --- a/bindings/python/pinocchio/romeo_wrapper.py +++ b/bindings/python/pinocchio/romeo_wrapper.py @@ -68,7 +68,9 @@ def __init__(self, filename, package_dirs=None, verbose=False): for op, name in self.opCorrespondances.items(): self.__dict__[op] = self.index(name) - # self.__dict__['_M'+op] = types.MethodType(lambda s, q: s.position(q,idx),self) + # self.__dict__["_M" + op] = types.MethodType( + # lambda s, q: s.position(q, idx), self + # ) # --- SHORTCUTS --- def Mrh(self, q): diff --git a/bindings/python/pinocchio/shortcuts.py b/bindings/python/pinocchio/shortcuts.py index dfec0ccd88..8cba963979 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -1,13 +1,15 @@ +# ruff: noqa: E501 # # Copyright (c) 2018-2020 CNRS INRIA # ## In this file, some shortcuts are provided ## -from . import pinocchio_pywrap_default as pin -from . import WITH_HPP_FCL, WITH_HPP_FCL_BINDINGS from typing import Tuple +from . import WITH_HPP_FCL, WITH_HPP_FCL_BINDINGS +from . import pinocchio_pywrap_default as pin + nle = pin.nonLinearEffects @@ -77,8 +79,10 @@ def buildModelsFromUrdf( def createDatas(*models): - """Call createData() on each Model or GeometryModel in input and return the results in a tuple. - If one of the models is None, the corresponding data object in the result is also None. + """ + Call createData() on each Model or GeometryModel in input and return the results in + a tuple. If one of the models is None, the corresponding data object in the result + is also None. """ return tuple([None if model is None else model.createData() for model in models]) @@ -120,7 +124,8 @@ def buildModelsFromSdf( ) if verbose and not WITH_HPP_FCL_BINDINGS and meshLoader is not None: print( - "Info: MeshLoader is ignored. The HPP-FCL Python bindings have not been installed." + "Info: MeshLoader is ignored. " + "The HPP-FCL Python bindings have not been installed." ) if package_dirs is None: package_dirs = [] @@ -173,7 +178,8 @@ def buildModelsFromMJCF( ) if verbose and not WITH_HPP_FCL_BINDINGS and meshLoader is not None: print( - "Info: MeshLoader is ignored. The HPP-FCL Python bindings have not been installed." + "Info: MeshLoader is ignored. " + "The HPP-FCL Python bindings have not been installed." ) lst = [model] diff --git a/bindings/python/pinocchio/utils.py b/bindings/python/pinocchio/utils.py index 261f809bf9..2401d142f1 100644 --- a/bindings/python/pinocchio/utils.py +++ b/bindings/python/pinocchio/utils.py @@ -76,7 +76,7 @@ def mprint(M, name="ans", eps=1e-15): cmin = i * 6 cmax = (i + 1) * 6 cmax = ncol if ncol < cmax else cmax - print("Columns %s through %s" % (cmin, cmax - 1)) + print(f"Columns {cmin} through {cmax - 1}") print() for r in range(M.shape[0]): sys.stdout.write(" ") diff --git a/bindings/python/pinocchio/visualize/base_visualizer.py b/bindings/python/pinocchio/visualize/base_visualizer.py index fa2717605f..1864653ea8 100644 --- a/bindings/python/pinocchio/visualize/base_visualizer.py +++ b/bindings/python/pinocchio/visualize/base_visualizer.py @@ -1,10 +1,11 @@ -from .. import pinocchio_pywrap_default as pin -from ..shortcuts import createDatas - +import abc +import os.path as osp import time + import numpy as np -import os.path as osp -import abc + +from .. import pinocchio_pywrap_default as pin +from ..shortcuts import createDatas try: import imageio @@ -15,8 +16,11 @@ class BaseVisualizer(abc.ABC): - """Pinocchio visualizers are employed to easily display a model at a given configuration. - BaseVisualizer is not meant to be directly employed, but only to provide a uniform interface and a few common methods. + """ + Pinocchio visualizers are employed to easily display a model at a given + configuration. + BaseVisualizer is not meant to be directly employed, but only to provide a uniform + interface and a few common methods. New visualizers should extend this class and override its methods as neeeded. """ @@ -34,8 +38,11 @@ def __init__( collision_data=None, visual_data=None, ): - """Construct a display from the given model, collision model, and visual model. - If copy_models is True, the models are copied. Otherwise, they are simply kept as a reference.""" + """ + Construct a display from the given model, collision model, and visual model. + If copy_models is True, the models are copied. Otherwise, they are simply kept + as a reference. + """ if copy_models: self.model = model.copy() @@ -90,8 +97,11 @@ def clean(self): @abc.abstractmethod def display(self, q=None): - """Display the robot at configuration q or refresh the rendering - from the current placements contained in data by placing all the bodies in the viewer.""" + """ + Display the robot at configuration q or refresh the rendering + from the current placements contained in data by placing all the bodies in the + viewer. + """ @abc.abstractmethod def displayCollisions(self, visibility): @@ -144,7 +154,10 @@ def has_video_writer(self): return self._video_writer is not None def play(self, q_trajectory, dt=None, callback=None, capture=False, **kwargs): - """Play a trajectory with given time step. Optionally capture RGB images and returns them.""" + """ + Play a trajectory with given time step. Optionally capture RGB images and + returns them. + """ nsteps = len(q_trajectory) if not capture: capture = self.has_video_writer() @@ -169,13 +182,14 @@ def play(self, q_trajectory, dt=None, callback=None, capture=False, **kwargs): return imgs def create_video_ctx(self, filename=None, fps=30, directory=None, **kwargs): - """Create a video recording context, generating the output filename if necessary. + """ + Create a video recording context, generating the output filename if necessary. Code inspired from https://github.com/petrikvladimir/RoboMeshCat. """ if not IMAGEIO_SUPPORT: - import warnings import contextlib + import warnings warnings.warn( "Video context cannot be created because imageio is not available.", @@ -189,7 +203,7 @@ def create_video_ctx(self, filename=None, fps=30, directory=None, **kwargs): directory = gettempdir() f_fmt = "%Y%m%d_%H%M%S" ext = "mp4" - filename = time.strftime("{}.{}".format(f_fmt, ext)) + filename = time.strftime(f"{f_fmt}.{ext}") filename = osp.join(directory, filename) return VideoContext(self, fps, filename) diff --git a/bindings/python/pinocchio/visualize/gepetto_visualizer.py b/bindings/python/pinocchio/visualize/gepetto_visualizer.py index 1b862e1c6e..b21ac1bf75 100644 --- a/bindings/python/pinocchio/visualize/gepetto_visualizer.py +++ b/bindings/python/pinocchio/visualize/gepetto_visualizer.py @@ -129,10 +129,7 @@ def loadPrimitive(self, meshName, geometry_object): gui.setLightingMode(meshName, "OFF") return True else: - msg = "Unsupported geometry type for %s (%s)" % ( - geometry_object.name, - type(geom), - ) + msg = f"Unsupported geometry type for {geometry_object.name} ({type(geom)})" warnings.warn(msg, category=UserWarning, stacklevel=2) return False @@ -154,16 +151,19 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type): success = self.loadPrimitive(meshName, geometry_object) else: if meshName == "": - msg = "Display of geometric primitives is supported only if pinocchio is build with HPP-FCL bindings." + msg = ( + "Display of geometric primitives is supported only if " + "pinocchio is build with HPP-FCL bindings." + ) warnings.warn(msg, category=UserWarning, stacklevel=2) return success = gui.addMesh(meshName, meshPath) if not success: return except Exception as e: - msg = "Error while loading geometry object: %s\nError message:\n%s" % ( - geometry_object.name, - e, + msg = ( + "Error while loading geometry object: " + f"{geometry_object.name}\nError message:\n{e}" ) warnings.warn(msg, category=UserWarning, stacklevel=2) return @@ -210,7 +210,9 @@ def loadViewerModel(self, rootNodeName="pinocchio"): gui.refresh() def display(self, q=None): - """Display the robot at configuration q in the viewer by placing all the bodies.""" + """ + Display the robot at configuration q in the viewer by placing all the bodies. + """ if "viewer" not in self.__dict__: return diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 353e1f2c98..dc88012f07 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -1,14 +1,14 @@ +import os +import warnings +from typing import ClassVar, List + +import numpy as np + from .. import pinocchio_pywrap_default as pin from ..deprecation import DeprecatedWarning from ..utils import npToTuple - from . import BaseVisualizer -import os -import warnings -import numpy as np -from typing import List - try: import meshcat import meshcat.geometry as mg @@ -17,11 +17,11 @@ else: import_meshcat_succeed = True -# DaeMeshGeometry -import xml.etree.ElementTree as Et import base64 -from typing import Optional, Any, Dict, Union, Set +# DaeMeshGeometry +import xml.etree.ElementTree as Et +from typing import Any, Dict, Optional, Set, Union MsgType = Dict[str, Union[str, bytes, bool, float, "MsgType"]] @@ -115,7 +115,7 @@ def __init__(self, dae_path: str, cache: Optional[Set[str]] = None) -> None: # Raw file content dae_dir = os.path.dirname(dae_path) - with open(dae_path, "r") as text_file: + with open(dae_path) as text_file: self.dae_raw = text_file.read() # Parse the image resource in Collada file @@ -273,6 +273,7 @@ def loadOctree(octree: hppfcl.OcTree): colors[:] = np.ones(3) mesh = mg.TriangularMeshGeometry(all_points, all_faces, colors) return mesh + else: def loadOctree(octree): @@ -414,6 +415,7 @@ def loadMesh(mesh): ) return mesh + else: def loadMesh(mesh): @@ -456,10 +458,7 @@ def loadPrimitive(geometry_object): obj = loadMesh(geom) if obj is None: - msg = "Unsupported geometry type for %s (%s)" % ( - geometry_object.name, - type(geom), - ) + msg = f"Unsupported geometry type for {geometry_object.name} ({type(geom)})" warnings.warn(msg, category=UserWarning, stacklevel=2) return obj @@ -540,7 +539,7 @@ class MeshcatVisualizer(BaseVisualizer): FORCE_SCALE = 0.06 FRAME_VEL_COLOR = 0x00FF00 - CAMERA_PRESETS = { + CAMERA_PRESETS: ClassVar = { "preset0": [ np.zeros(3), # target [3.0, 0.0, 1.0], # anchor point (x, z, -y) lhs coords @@ -570,11 +569,12 @@ def __init__( if not import_meshcat_succeed: msg = ( "Error while importing the viewer client.\n" - "Check whether meshcat is properly installed (pip install --user meshcat)." + "Check whether meshcat is properly installed " + "(pip install --user meshcat)." ) raise ImportError(msg) - super(MeshcatVisualizer, self).__init__( + super().__init__( model, collision_model, visual_model, @@ -594,7 +594,8 @@ def getViewerNodeName(self, geometry_object, geometry_type): def initViewer(self, viewer=None, open=False, loadModel=False, zmq_url=None): """Start a new MeshCat server and client. - Note: the server can also be started separately using the "meshcat-server" command in a terminal: + Note: the server can also be started separately using the "meshcat-server" + command in a terminal: this enables the server to remain active after the current script ends. """ @@ -726,10 +727,7 @@ def loadPrimitive(self, geometry_object: pin.GeometryObject): obj = loadMesh(geom) if obj is None: - msg = "Unsupported geometry type for %s (%s)" % ( - geometry_object.name, - type(geom), - ) + msg = f"Unsupported geometry type for {geometry_object.name} ({type(geom)})" warnings.warn(msg, category=UserWarning, stacklevel=2) obj = None @@ -738,7 +736,10 @@ def loadPrimitive(self, geometry_object: pin.GeometryObject): def loadMeshFromFile(self, geometry_object): # Mesh path is empty if Pinocchio is built without HPP-FCL bindings if geometry_object.meshPath == "": - msg = "Display of geometric primitives is supported only if pinocchio is build with HPP-FCL bindings." + msg = ( + "Display of geometric primitives is supported only if " + "pinocchio is build with HPP-FCL bindings." + ) warnings.warn(msg, category=UserWarning, stacklevel=2) return None @@ -751,7 +752,7 @@ def loadMeshFromFile(self, geometry_object): elif file_extension.lower() == ".stl": obj = mg.StlMeshGeometry.from_file(geometry_object.meshPath) else: - msg = "Unknown mesh file format: {}.".format(geometry_object.meshPath) + msg = f"Unknown mesh file format: {geometry_object.meshPath}." warnings.warn(msg, category=UserWarning, stacklevel=2) obj = None @@ -798,9 +799,9 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None): warnings.warn(msg, category=UserWarning, stacklevel=2) return except Exception as e: - msg = "Error while loading geometry object: %s\nError message:\n%s" % ( - geometry_object.name, - e, + msg = ( + "Error while loading geometry object: " + f"{geometry_object.name}\nError message:\n{e}" ) warnings.warn(msg, category=UserWarning, stacklevel=2) return @@ -809,7 +810,8 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None): meshcat_node.set_object(obj) elif isinstance(obj, (mg.Geometry, mg.ReferenceSceneElement)): material = mg.MeshPhongMaterial() - # Set material color from URDF, converting for triplet of doubles to a single int. + # Set material color from URDF, converting for triplet of doubles to a + # single int. def to_material_color(rgba) -> int: """Convert rgba color as list into rgba color as int""" @@ -933,7 +935,9 @@ def delete(self, geometry_object, geometry_type): self.viewer[viewer_name].delete() def display(self, q=None): - """Display the robot at configuration q in the viewer by placing all the bodies.""" + """ + Display the robot at configuration q in the viewer by placing all the bodies + """ if q is not None: pin.forwardKinematics(self.model, self.data, q) @@ -959,7 +963,8 @@ def updatePlacements(self, geometry_type): visual_name = self.getViewerNodeName(visual, geometry_type) # Get mesh pose. M = geom_data.oMg[geom_model.getGeometryId(visual.name)] - # Manage scaling: force scaling even if this should be normally handled by MeshCat (but there is a bug here) + # Manage scaling: force scaling even if this should be normally handled by + # MeshCat (but there is a bug here) geom = visual.geometry if WITH_HPP_FCL_BINDINGS and isinstance( geom, (hppfcl.Plane, hppfcl.Halfspace) @@ -1039,7 +1044,7 @@ def initializeFrames(self, frame_ids=None, axis_length=0.2, axis_width=2): for fid, frame in enumerate(self.model.frames): if frame_ids is None or fid in frame_ids: - frame_viz_name = "%s/%s" % (self.viewerFramesGroupName, frame.name) + frame_viz_name = f"{self.viewerFramesGroupName}/{frame.name}" self.viewer[frame_viz_name].set_object( mg.LineSegments( mg.PointsGeometry( @@ -1061,7 +1066,7 @@ def updateFrames(self): pin.updateFramePlacements(self.model, self.data) for fid in self.frame_ids: frame_name = self.model.frames[fid].name - frame_viz_name = "%s/%s" % (self.viewerFramesGroupName, frame_name) + frame_viz_name = f"{self.viewerFramesGroupName}/{frame_name}" self.viewer[frame_viz_name].set_transform(self.data.oMf[fid].homogeneous) def drawFrameVelocities(self, frame_id: int, v_scale=0.2, color=FRAME_VEL_COLOR): @@ -1069,7 +1074,7 @@ def drawFrameVelocities(self, frame_id: int, v_scale=0.2, color=FRAME_VEL_COLOR) vFr = pin.getFrameVelocity( self.model, self.data, frame_id, pin.LOCAL_WORLD_ALIGNED ) - line_group_name = "ee_vel/{}".format(frame_id) + line_group_name = f"ee_vel/{frame_id}" self._draw_vectors_from_frame( [v_scale * vFr.linear], [frame_id], [line_group_name], [color] ) diff --git a/bindings/python/pinocchio/visualize/panda3d_visualizer.py b/bindings/python/pinocchio/visualize/panda3d_visualizer.py index c4f1afd31e..dd2b5b314a 100644 --- a/bindings/python/pinocchio/visualize/panda3d_visualizer.py +++ b/bindings/python/pinocchio/visualize/panda3d_visualizer.py @@ -1,11 +1,11 @@ import warnings +import numpy as np + from .. import pinocchio_pywrap_default as pin from ..utils import npToTuple from .base_visualizer import BaseVisualizer -import numpy as np - try: import hppfcl @@ -59,10 +59,7 @@ def append(root, obj): elif isinstance(geom, hppfcl.Sphere): self.viewer.append_sphere(root, obj.name, geom.radius) else: - msg = "Unsupported geometry type for %s (%s)" % ( - obj.name, - type(geom), - ) + msg = f"Unsupported geometry type for {obj.name} ({type(geom)})" warnings.warn(msg, category=UserWarning, stacklevel=2) return else: @@ -96,7 +93,9 @@ def getViewerNodeName(self, geometry_object, geometry_type): return self.collision_group + "/" + geometry_object.name def display(self, q=None): - """Display the robot at configuration q in the viewer by placing all the bodies.""" + """ + Display the robot at configuration q in the viewer by placing all the bodies. + """ if q is not None: pin.forwardKinematics(self.model, self.data, q) diff --git a/bindings/python/pinocchio/visualize/rviz_visualizer.py b/bindings/python/pinocchio/visualize/rviz_visualizer.py index ab71345bc1..8a84d0dccf 100644 --- a/bindings/python/pinocchio/visualize/rviz_visualizer.py +++ b/bindings/python/pinocchio/visualize/rviz_visualizer.py @@ -69,7 +69,9 @@ def initViewer( loadModel=False, initRosNode=True, ): - """Init RVizViewer by starting a ros node (or not) and creating an RViz window.""" + """ + Init RVizViewer by starting a ros node (or not) and creating an RViz window. + """ from python_qt_binding.QtWidgets import QApplication from rosgraph import is_master_online from rospy import WARN, init_node @@ -186,7 +188,10 @@ def display(self, q=None): ) def _plot(self, publisher, model, data, previous_ids=()): - """Create markers for each object of the model and publish it as MarkerArray (also delete unused previously created markers)""" + """ + Create markers for each object of the model and publish it as MarkerArray + (also delete unused previously created markers) + """ from geometry_msgs.msg import Point from rospy import get_rostime from std_msgs.msg import ColorRGBA, Header @@ -243,10 +248,7 @@ def _plot(self, publisher, model, data, previous_ids=()): create_capsule_markers(marker, data.oMg[obj_id], d, fl) ) else: - msg = "Unsupported geometry type for %s (%s)" % ( - obj.name, - type(geom), - ) + msg = f"Unsupported geometry type for {obj.name} ({type(geom)})" warnings.warn(msg, category=UserWarning, stacklevel=2) continue else: diff --git a/bindings/python/pinocchio/windows_dll_manager.py b/bindings/python/pinocchio/windows_dll_manager.py index 21a6de9448..c414d1bb4f 100644 --- a/bindings/python/pinocchio/windows_dll_manager.py +++ b/bindings/python/pinocchio/windows_dll_manager.py @@ -1,6 +1,5 @@ -import os -import sys import contextlib +import os def get_dll_paths(): @@ -50,7 +49,4 @@ def __exit__(self, *exc_details): def build_directory_manager(): - if sys.version_info >= (3, 8): - return DllDirectoryManager() - else: - return PathManager() + return DllDirectoryManager() diff --git a/doc/d-practical-exercises/src/continuous.py b/doc/d-practical-exercises/src/continuous.py index 23cc24ab00..3b1e5ed7ff 100644 --- a/doc/d-practical-exercises/src/continuous.py +++ b/doc/d-practical-exercises/src/continuous.py @@ -1,6 +1,7 @@ """ Deep actor-critic network, -From "Continuous control with deep reinforcement learning", by Lillicrap et al, arXiv:1509.02971 +From "Continuous control with deep reinforcement learning", +by Lillicrap et al, arXiv:1509.02971 """ import random @@ -12,7 +13,6 @@ import numpy as np import tensorflow as tf import tflearn - from pendulum import Pendulum ### --- Random seed @@ -249,9 +249,8 @@ def rendertrial(maxiter=NSTEPS, verbose=True): else 0 ) print( - "Ep#{:3d}: lasted {:d} steps, reward={:3.0f}, max qvalue={:2.3f}".format( - episode, step, rsum, maxq - ) + f"Ep#{episode:3d}: lasted {step:d} steps, " + f"reward={rsum:3.0f}, max qvalue={maxq:2.3f}" ) h_rwd.append(rsum) h_qva.append(maxq) diff --git a/doc/d-practical-exercises/src/display.py b/doc/d-practical-exercises/src/display.py index 920b5346ac..7eff5ca45d 100644 --- a/doc/d-practical-exercises/src/display.py +++ b/doc/d-practical-exercises/src/display.py @@ -7,15 +7,16 @@ # 'place' method to set the position/rotation of a 3D visual object in a scene. class Display: """ - Class Display: Example of a class implementing a client for the Gepetto-viewer server. The main - method of the class is 'place', that sets the position/rotation of a 3D visual object in a scene. + Class Display: Example of a class implementing a client for the Gepetto-viewer + server. The main method of the class is 'place', that sets the position/rotation of + a 3D visual object in a scene. """ def __init__(self, windowName="pinocchio"): """ - This function connect with the Gepetto-viewer server and open a window with the given name. - If the window already exists, it is kept in the current state. Otherwise, the newly-created - window is set up with a scene named 'world'. + This function connect with the Gepetto-viewer server and open a window with the + given name. If the window already exists, it is kept in the current state. + Otherwise, the newly-created window is set up with a scene named 'world'. """ # Create the client and connect it with the display server. @@ -31,7 +32,8 @@ def __init__(self, windowName="pinocchio"): windowID = self.viewer.gui.getWindowID(windowName) print("Warning: window '" + windowName + "' already created.") print( - "The previously created objects will not be destroyed and do not have to be created again." + "The previously created objects will not be destroyed " + "and do not have to be created again." ) except: # noqa: E722 # Otherwise, create the empty window. @@ -46,9 +48,9 @@ def __init__(self, windowName="pinocchio"): def place(self, objName, M, refresh=True): """ This function places (ie changes both translation and rotation) of the object - names "objName" in place given by the SE3 object "M". By default, immediately refresh - the layout. If multiple objects have to be placed at the same time, do the refresh - only at the end of the list. + names "objName" in place given by the SE3 object "M". By default, immediately + refresh the layout. If multiple objects have to be placed at the same time, do + the refresh only at the end of the list. """ self.viewer.gui.applyConfiguration(objName, pin.se3ToXYZQUATtuple(M)) if refresh: diff --git a/doc/d-practical-exercises/src/dpendulum.py b/doc/d-practical-exercises/src/dpendulum.py index 8f10d69e1b..3ef3aa7c8a 100644 --- a/doc/d-practical-exercises/src/dpendulum.py +++ b/doc/d-practical-exercises/src/dpendulum.py @@ -2,7 +2,6 @@ import numpy as np from numpy import pi - from pendulum import Pendulum p = Pendulum(1) diff --git a/doc/d-practical-exercises/src/factor.py b/doc/d-practical-exercises/src/factor.py index 5a0155d873..469b582038 100644 --- a/doc/d-practical-exercises/src/factor.py +++ b/doc/d-practical-exercises/src/factor.py @@ -1,31 +1,33 @@ -from pinocchio.utils import zero, eye import numpy as np import numpy.linalg as npl +from pinocchio.utils import eye, zero """ This file implements a sparse linear problem (quadric cost, linear constraints -- LCQP) -where the decision variables are denoted by x=(x1 ... xn), n being the number of factors. +where the decision variables are denoted by x=(x1 ... xn), n being the number of +factors. The problem can be written: min Sum_i=1^p || A_i x - b_i ||^2 x1...xn so that forall j=1:q C_j x = d_i -Matrices A_i and C_j are block sparse, i.e. they are acting only on some (few) of the variables -x1 .. xn. +Matrices A_i and C_j are block sparse, i.e. they are acting only on some (few) of the +variables x1 .. xn. -The file implements the main class FactorGraph, which stores the LCQP problem and solve it. +The file implements the main class FactorGraph, which stores the LCQP problem and solve +it. It also provides a secondary class Factor, used to set up FactorGraph """ -class Factor(object): +class Factor: """ A factor is a part of a linear constraint corresponding either a cost ||A x - b|| or a constraint Cx = d. - In both cases, we have Ax = sum A_i x_i, where some A_i are null. One object of class - Factor stores one of the A_i, along with the correspond index. It is simply a pair - (index, matrix). + In both cases, we have Ax = sum A_i x_i, where some A_i are null. One object of + class Factor stores one of the A_i, along with the correspond index. It is + simply a pair (index, matrix). This class is used as a arguments of some of the setup functions of FactorGraph. """ @@ -35,10 +37,11 @@ def __init__(self, index, matrix): self.matrix = matrix -class FactorGraph(object): +class FactorGraph: """ - The class FactorGraph stores a block-sparse linear-constrained quadratic program (LCQP) - of variable x=(x1...xn). The size of the problem is set up at construction of the object. + The class FactorGraph stores a block-sparse linear-constrained quadratic program + (LCQP) of variable x=(x1...xn). The size of the problem is set up at construction of + the object. Methods add_factor() and add_factor_constraint() are used to set up the problem. Method solve() is used to compute the solution to the problem. """ diff --git a/doc/d-practical-exercises/src/foot_steps.py b/doc/d-practical-exercises/src/foot_steps.py index 82b8f349dd..47e6d989cb 100644 --- a/doc/d-practical-exercises/src/foot_steps.py +++ b/doc/d-practical-exercises/src/foot_steps.py @@ -1,21 +1,22 @@ -class FootSteps(object): +class FootSteps: """ The class stores three functions of time: left, right and flying_foot. Each function is piecewise constant. For each function, the user can ask what is the value of this function at time t. - The storage is composed of three lists for left, right and flying_foot, and a list for time. + The storage is composed of three lists for left, right and flying_foot, and a list + for time. The list of times stores the time intervals, i.e. each element of the list is the start of a time interval. The first element of the list is 0. - The value of the functions left, right, flying_foot one this time interval is stored at - the same position is their respective list (i.e. value of left on interval + The value of the functions left, right, flying_foot one this time interval is stored + at the same position is their respective list (i.e. value of left on interval [time[i], time[i+1]] is stored in left[i]. The 4 lists are set up using function add_phase(). - The values of functions left, right, flying_foot can be accessed through the function - get_phase_type(t), get_left_position(t), get_right_position(t). - phase_type are 'left' (meaning left foot is flying, right foot is fixed), 'right' (ie the opposite) - or 'none' (meaning no foot is flying, both are fixed on the ground). + The values of functions left, right, flying_foot can be accessed through the + function get_phase_type(t), get_left_position(t), get_right_position(t). phase_type + are 'left' (meaning left foot is flying, right foot is fixed), 'right' (ie the + opposite) or 'none' (meaning no foot is flying, both are fixed on the ground). Additionnally, functions get_left_next_position(t), get_right_next_position(t) can be used to get the next position of the @@ -39,10 +40,10 @@ def __init__(self, right, left): def add_phase(self, duration, foot, position=None): """ - Add a phase lasting where the flyhing foot (either 'left' or 'right') - moves to (being a vector or a SE3 placement). - Alternatively, might be set to 'none' (i.e double support). In that case, - is not specified (or is set to None, default). + Add a phase lasting where the flyhing foot (either 'left' or + 'right') moves to (being a vector or a SE3 placement). + Alternatively, might be set to 'none' (i.e double support). In that case, + is not specified (or is set to None, default). """ assert foot == "left" or foot == "right" or foot == "none" self.time.append(self.time[-1] + duration) @@ -55,7 +56,9 @@ def add_phase(self, duration, foot, position=None): self.right[-1] = position def get_index_from_time(self, t): - """Return the index i of the interval containing t, i.e. t in time[i], time[i+1]""" + """ + Return the index i of the interval containing t, i.e. t in time[i], time[i+1] + """ if t > self.time[-1]: return len(self.time) - 1 return next(i for i, ti in enumerate(self.time) if ti > t) - 1 diff --git a/doc/d-practical-exercises/src/graph.py b/doc/d-practical-exercises/src/graph.py index 0d0cd898a4..eb3d745637 100644 --- a/doc/d-practical-exercises/src/graph.py +++ b/doc/d-practical-exercises/src/graph.py @@ -1,4 +1,4 @@ -class Graph(object): +class Graph: def __init__(self): self.children = {} # dictionnary giving the list of childrens for each node. self.q = [] # configuration associated to each node. @@ -50,5 +50,7 @@ def rename_connex(self, past, future): pass def connexIndexes(self, connex): - """Return the list of all node indexes belonging to connex component .""" + """ + Return the list of all node indexes belonging to connex component . + """ return [i for i, c in enumerate(self.connex) if c == connex] diff --git a/doc/d-practical-exercises/src/mobilerobot.py b/doc/d-practical-exercises/src/mobilerobot.py index 111ce9f5ce..f8d6d3394a 100644 --- a/doc/d-practical-exercises/src/mobilerobot.py +++ b/doc/d-practical-exercises/src/mobilerobot.py @@ -7,12 +7,13 @@ class MobileRobotWrapper(RobotWrapper): """ The class models a mobile robot with UR5 arm, feature 3+6 DOF. - The configuration of the basis is represented by [X, Y, cos, sin], with the additionnal constraint - that (cos, sin) should have norm 1. Hence take care when sampling and integrating configurations. - The robot is depicted as an orange box with two black cylinders (wheels) atop of which is the - classical (realistic) UR5 model. - The model also features to OPERATIONAL frames, named "mobile" (on the front of the mobile basis, 30cm - above the ground) and "tool" (at the very end of the effector). + The configuration of the basis is represented by [X, Y, cos, sin], with the + additionnal constraint that (cos, sin) should have norm 1. Hence take care when + sampling and integrating configurations. + The robot is depicted as an orange box with two black cylinders (wheels) atop of + which is the classical (realistic) UR5 model. + The model also features to OPERATIONAL frames, named "mobile" (on the front of the + mobile basis, 30cm above the ground) and "tool" (at the very end of the effector). """ def __init__(self, urdf, pkgs): @@ -29,7 +30,8 @@ def __init__(self, urdf, pkgs): basisMop = pin.SE3(eye(3), np.array([0.3, 0.0, 0.1])) self.model.addFrame(pin.Frame("mobile", 1, 1, basisMop, pin.FrameType.OP_FRAME)) - # Placement of the tool frame wrt end effector frame (located at the center of the wrist) + # Placement of the tool frame wrt end effector frame (located at the center of + # the wrist) effMop = pin.SE3(eye(3), np.maarraytrix([0.0, 0.08, 0.095])) self.model.addFrame(pin.Frame("tool", 6, 6, effMop, pin.FrameType.OP_FRAME)) diff --git a/doc/d-practical-exercises/src/ocp.py b/doc/d-practical-exercises/src/ocp.py index f8b3e6b171..c16beb9ce2 100644 --- a/doc/d-practical-exercises/src/ocp.py +++ b/doc/d-practical-exercises/src/ocp.py @@ -7,9 +7,8 @@ import matplotlib.pyplot as plt import numpy as np -from scipy.optimize import fmin_l_bfgs_b - from pendulum import Pendulum +from scipy.optimize import fmin_l_bfgs_b env = Pendulum(1) NSTEPS = 50 @@ -31,7 +30,7 @@ def display(U, verbose=False): """Display the trajectory on Gepetto viewer.""" x = x0.copy() if verbose: - print("U = ", " ".join(map(lambda u: "%.1f" % u, np.asarray(U).flatten()))) + print("U = ", " ".join(map(lambda u: f"{u:.1f}", np.asarray(U).flatten()))) for i in range(len(U) / env.nu): env.dynamics(x, U[env.nu * i : env.nu * (i + 1)], True) env.display(x) @@ -52,7 +51,7 @@ def __call__(self, U): print( "Iteration ", self.iter, - " ".join(map(lambda u: "%.1f" % u, np.asarray(U).flatten())), + " ".join(map(lambda u: f"{u:.1f}", np.asarray(U).flatten())), ) self.iter += 1 self.U = U.copy() diff --git a/doc/d-practical-exercises/src/pendulum.py b/doc/d-practical-exercises/src/pendulum.py index 113e9cf01f..4412656ba8 100644 --- a/doc/d-practical-exercises/src/pendulum.py +++ b/doc/d-practical-exercises/src/pendulum.py @@ -15,15 +15,15 @@ import numpy as np import pinocchio as pin +from display import Display from numpy.linalg import inv from pinocchio.utils import rand -from display import Display - class Visual: """ - Class representing one 3D mesh of the robot, to be attached to a joint. The class contains: + Class representing one 3D mesh of the robot, to be attached to a joint. The class + contains: * the name of the 3D objects inside Gepetto viewer. * the ID of the joint in the kinematic tree to which the body is attached. * the placement of the body with respect to the joint frame. @@ -45,11 +45,12 @@ class Pendulum: Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3). The configuration is nq=7. The velocity is the same. The members of the class are: - * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them. + * viewer: a display encapsulating a gepetto viewer client to create 3D objects and + place them. * model: the kinematic tree of the robot. * data: the temporary variables to be used by the kinematic algorithms. - * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being - an object Visual (see above). + * visuals: the list of all the 'visual' 3D objects to render the robot, each element + of the list being an object Visual (see above). See tp1.py for an example of use. """ diff --git a/doc/d-practical-exercises/src/prm_display.py b/doc/d-practical-exercises/src/prm_display.py index 0d08fcb31a..9145d0d6bc 100644 --- a/doc/d-practical-exercises/src/prm_display.py +++ b/doc/d-practical-exercises/src/prm_display.py @@ -4,10 +4,11 @@ def display_prm(robot, graph): - """Take a graph object containing a list of configurations q and - a dictionnary of graph relations edge. Display the configurations by the correspond - placement of the robot end effector. Display the graph relation by vertices connecting - the robot end effector positions. + """ + Take a graph object containing a list of configurations q and a dictionnary of graph + relations edge. Display the configurations by the correspond placement of the robot + end effector. Display the graph relation by vertices connecting the robot end + effector positions. """ gui = robot.viewer.gui diff --git a/doc/d-practical-exercises/src/qnet.py b/doc/d-practical-exercises/src/qnet.py index cddbc3b01e..4e1983b9b2 100644 --- a/doc/d-practical-exercises/src/qnet.py +++ b/doc/d-practical-exercises/src/qnet.py @@ -1,5 +1,6 @@ """ -Example of Q-table learning with a simple discretized 1-pendulum environment using a linear Q network. +Example of Q-table learning with a simple discretized 1-pendulum environment using a +linear Q network. """ import signal @@ -8,7 +9,6 @@ import matplotlib.pyplot as plt import numpy as np import tensorflow as tf - from dpendulum import DPendulum ### --- Random seed diff --git a/doc/d-practical-exercises/src/qtable.py b/doc/d-practical-exercises/src/qtable.py index 61075c44ef..9593bab3a4 100644 --- a/doc/d-practical-exercises/src/qtable.py +++ b/doc/d-practical-exercises/src/qtable.py @@ -7,7 +7,6 @@ import matplotlib.pyplot as plt import numpy as np - from dpendulum import DPendulum ### --- Random seed diff --git a/doc/d-practical-exercises/src/robot_hand.py b/doc/d-practical-exercises/src/robot_hand.py index 4035306345..89eae808bd 100644 --- a/doc/d-practical-exercises/src/robot_hand.py +++ b/doc/d-practical-exercises/src/robot_hand.py @@ -2,25 +2,25 @@ import numpy as np import pinocchio as pin +from display import Display from numpy.linalg import norm, pinv from pinocchio.utils import cross, eye, rotate, zero -from display import Display - -class Visual(object): +class Visual: """ - Class representing one 3D mesh of the robot, to be attached to a joint. The class contains: + Class representing one 3D mesh of the robot, to be attached to a joint. The class + contains: * the name of the 3D objects inside Gepetto viewer. * the ID of the joint in the kinematic tree to which the body is attached. * the placement of the body with respect to the joint frame. This class is only used in the list Robot.visuals (see below). - The visual are supposed mostly to be capsules. In that case, the object also contains - radius and length of the capsule. + The visual are supposed mostly to be capsules. In that case, the object also + contains radius and length of the capsule. The collision checking computes collision test, distance, and witness points. - Using the supporting robot, the collision Jacobian returns a 1xN matrix corresponding - to the normal direction. + Using the supporting robot, the collision Jacobian returns a 1xN matrix + corresponding to the normal direction. """ def __init__(self, name, jointParent, placement, radius=0.1, length=None): @@ -131,20 +131,21 @@ def displayCollision(self, viewer, name="world/wa"): viewer.place(name, pin.SE3(self.R, self.w)) -class Robot(object): +class Robot: """ Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3). The configuration is nq=7. The velocity is the same. The members of the class are: - * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them. + * viewer: a display encapsulating a gepetto viewer client to create 3D objects and + place them. * model: the kinematic tree of the robot. * data: the temporary variables to be used by the kinematic algorithms. - * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being - an object Visual (see above). + * visuals: the list of all the 'visual' 3D objects to render the robot, each element + of the list being an object Visual (see above). CollisionPairs is a list of visual indexes. - Reference to the collision pair is used in the collision test and jacobian of the collision - (which are simply proxy method to methods of the visual class). + Reference to the collision pair is used in the collision test and jacobian of the + collision (which are simply proxy method to methods of the visual class). """ def __init__(self): diff --git a/doc/d-practical-exercises/src/ur5x4.py b/doc/d-practical-exercises/src/ur5x4.py index 4350fde4a9..2a8459be11 100644 --- a/doc/d-practical-exercises/src/ur5x4.py +++ b/doc/d-practical-exercises/src/ur5x4.py @@ -1,15 +1,15 @@ """ -Load 4 times the UR5 model, plus a plate object on top of them, to feature a simple parallel robot. +Load 4 times the UR5 model, plus a plate object on top of them, to feature a simple +parallel robot. No optimization, this file is just an example of how to load the models. """ from os.path import join +import numpy as np from pinocchio import SE3 from pinocchio.robot_wrapper import RobotWrapper -from pinocchio.utils import rotate, zero, eye, se3ToXYZQUATtuple, urdf - -import numpy as np +from pinocchio.utils import eye, rotate, se3ToXYZQUATtuple, urdf, zero PKG = "/opt/openrobots/share" URDF = join(PKG, "ur5_description/urdf/ur5_gripper.urdf") @@ -18,7 +18,8 @@ def loadRobot(M0, name): """ This function load a UR5 robot n a new model, move the basis to placement - and add the corresponding visuals in gepetto viewer with name prefix given by string . + and add the corresponding visuals in gepetto viewer with name prefix given by string + . It returns the robot wrapper (model,data). """ robot = RobotWrapper(urdf, [PKG]) diff --git a/examples/anymal-simulation.py b/examples/anymal-simulation.py index 3a839c3e4b..c5f28bc910 100644 --- a/examples/anymal-simulation.py +++ b/examples/anymal-simulation.py @@ -64,7 +64,7 @@ def squashing(model, data, q_in): q = q_in.copy() - y = np.ones((constraint_dim)) + y = np.ones(constraint_dim) N_full = 200 @@ -94,7 +94,9 @@ def com_des(k): constraint_value = np.concatenate( [cd.c1Mc2.translation for cd in constraint_datas] ) - # constraint_value = np.concatenate([pinocchio.log6(cd.c1Mc2) for cd in constraint_datas]) + # constraint_value = np.concatenate( + # [pinocchio.log6(cd.c1Mc2) for cd in constraint_datas] + # ) J = np.vstack( [ pinocchio.getFrameJacobian( diff --git a/examples/append-urdf-model-with-another-model.py b/examples/append-urdf-model-with-another-model.py index cff4d82079..389a83f0f1 100644 --- a/examples/append-urdf-model-with-another-model.py +++ b/examples/append-urdf-model-with-another-model.py @@ -1,12 +1,11 @@ -from os.path import dirname, join, abspath import math import sys +from os.path import abspath, dirname, join +import hppfcl as fcl import numpy as np - import pinocchio as pin from pinocchio.visualize import MeshcatVisualizer as Visualizer -import hppfcl as fcl # load model from example-robot urdf pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models/") @@ -43,7 +42,7 @@ geom1_name = "ball" shape1 = fcl.Sphere(body_radius) geom1_obj = pin.GeometryObject(geom1_name, joint_id, body_placement, shape1) -geom1_obj.meshColor = np.ones((4)) +geom1_obj.meshColor = np.ones(4) geom_model.addGeometryObject(geom1_obj) geom2_name = "bar" @@ -69,8 +68,8 @@ ) print( - "Check the joints of the appended model:\n %s \n ->Notice the spherical joint at the end." - % model + f"Check the joints of the appended model:\n {model} \n " + "->Notice the spherical joint at the end." ) try: @@ -78,7 +77,8 @@ viz.initViewer(open=True) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) diff --git a/examples/build-reduced-model.py b/examples/build-reduced-model.py index 723dc24312..144e764283 100644 --- a/examples/build-reduced-model.py +++ b/examples/build-reduced-model.py @@ -3,7 +3,8 @@ import numpy as np import pinocchio as pin -# Goal: Build a reduced model from an existing URDF model by fixing the desired joints at a specified position. +# Goal: Build a reduced model from an existing URDF model by fixing the desired joints +# at a specified position. # Load UR robot arm # This path refers to Pinocchio source code but you can define your own directory here. @@ -46,12 +47,14 @@ # Option 1: Only build the reduced model in case no display needed: model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig) -# Option 2: Build the reduced model including the geometric model for proper displaying of the robot +# Option 2: Build the reduced model including the geometric model for proper displaying +# of the robot. model_reduced, visual_model_reduced = pin.buildReducedModel( model, visual_model, jointsToLockIDs, initialJointConfig ) -# Option 3: Build the reduced model including multiple geometric models (for example: visuals, collision) +# Option 3: Build the reduced model including multiple geometric models (for example: +# visuals, collision). geom_models = [visual_model, collision_model] model_reduced, geometric_models_reduced = pin.buildReducedModel( model, diff --git a/examples/capsule-approximation.py b/examples/capsule-approximation.py index 0dde81a430..0b02037d70 100644 --- a/examples/capsule-approximation.py +++ b/examples/capsule-approximation.py @@ -1,6 +1,7 @@ """ Copyright (c) 2020 INRIA -Inspired from Antonio El Khoury PhD: https://tel.archives-ouvertes.fr/file/index/docid/833019/filename/thesis.pdf +Inspired from Antonio El Khoury PhD: +https://tel.archives-ouvertes.fr/file/index/docid/833019/filename/thesis.pdf Section 3.8.1 Computing minimum bounding capsules """ @@ -62,11 +63,11 @@ def capsule_vol(x): ) res = optimize.minimize(capsule_vol, x0, constraints=constraint) res_constraint = constraint_cap(res.x) - assert ( - res_constraint <= 1e-4 - ), "The computed solution is invalid, a vertex is at a distance {:.5f} of the capsule.".format( - res_constraint + err = ( + "The computed solution is invalid, " + "a vertex is at a distance {:.5f} of the capsule." ) + assert res_constraint <= 1e-4, err.format(res_constraint) a, b, r = res.x[:3], res.x[3:6], res.x[6] return a, b, r @@ -162,7 +163,8 @@ def set_transform(origin, a, b): # a, b, r = capsule_approximation(vertices) # Example for a whole URDF model - # This path refers to Pinocchio source code but you can define your own directory here. + # This path refers to Pinocchio source code but you can define your own directory + # here. from os.path import abspath, dirname, join pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") diff --git a/examples/casadi/cartpole.py b/examples/casadi/cartpole.py index 812399b00b..31e468fe49 100644 --- a/examples/casadi/cartpole.py +++ b/examples/casadi/cartpole.py @@ -1,10 +1,11 @@ +import sys + import casadi import hppfcl as fcl import numpy as np import pinocchio as pin -from pinocchio.visualize import MeshcatVisualizer import pinocchio.casadi as cpin -import sys +from pinocchio.visualize import MeshcatVisualizer def make_cartpole(ub=True): @@ -90,7 +91,9 @@ def create_dynamics(self): self.acc_func = casadi.Function("acc", [q, v, u], [a], ["q", "v", "u"], ["a"]) def create_discrete_dynamics(self): - """Create the map `(q,v) -> (qnext, vnext)` using semi-implicit Euler integration.""" + """ + Create the map `(q,v) -> (qnext, vnext)` using semi-implicit Euler integration. + """ q = self.q_node v = self.v_node u = self.u_node @@ -183,7 +186,8 @@ def integrate_no_control(x0, nsteps): viz.play(q_trajectory=qs_, dt=dt) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) diff --git a/examples/casadi/quadrotor-ocp.py b/examples/casadi/quadrotor-ocp.py index a5d0636c60..b8cd6c0da0 100644 --- a/examples/casadi/quadrotor-ocp.py +++ b/examples/casadi/quadrotor-ocp.py @@ -292,7 +292,8 @@ def main(): plt.show(block=False) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) diff --git a/examples/casadi/simulation-pendulum-variational.ipynb b/examples/casadi/simulation-pendulum-variational.ipynb index 9a2c0654e7..066caf69b7 100644 --- a/examples/casadi/simulation-pendulum-variational.ipynb +++ b/examples/casadi/simulation-pendulum-variational.ipynb @@ -91,12 +91,11 @@ "metadata": {}, "outputs": [], "source": [ + "import casadi\n", + "import hppfcl as fcl\n", "import numpy as np\n", "import pinocchio as pin\n", "import pinocchio.casadi as cpin\n", - "import hppfcl as fcl\n", - "\n", - "import casadi\n", "from casadi import SX" ] }, diff --git a/examples/cassie-simulation.py b/examples/cassie-simulation.py index 06ba1554a3..f973c932ab 100644 --- a/examples/cassie-simulation.py +++ b/examples/cassie-simulation.py @@ -199,7 +199,7 @@ def com_des(k): def squashing(model, data, q_in, Nin=N, epsin=eps, verbose=True): q = q_in.copy() - y = np.ones((constraint_dim)) + y = np.ones(constraint_dim) _N_full = 200 diff --git a/examples/codegen/codegen-rnea.py b/examples/codegen/codegen-rnea.py index d7cebf22df..e3cf2ce1b5 100644 --- a/examples/codegen/codegen-rnea.py +++ b/examples/codegen/codegen-rnea.py @@ -1,15 +1,15 @@ +import numpy as np +import pinocchio as pin +import pinocchio.cppadcg as cgpin from pycppad import ( ADCG, CG, - Independent, ADCGFun, CodeHandler, - LanguageC, + Independent, LangCDefaultVariableNameGenerator, + LanguageC, ) -import pinocchio.cppadcg as cgpin -import pinocchio as pin -import numpy as np pinmodel = pin.buildSampleModelHumanoidRandom() model = cgpin.Model(pinmodel) diff --git a/examples/collision-with-point-clouds.py b/examples/collision-with-point-clouds.py index e37314d5d4..427b57c865 100644 --- a/examples/collision-with-point-clouds.py +++ b/examples/collision-with-point-clouds.py @@ -1,13 +1,14 @@ -# This examples shows how to perform collision detection between the end-effector of a robot and a point cloud depicted as a Height Field +# This examples shows how to perform collision detection between the end-effector of a +# robot and a point cloud depicted as a Height Field # Note: this feature requires Meshcat to be installed, this can be done using # pip install --user meshcat -import pinocchio as pin -import hppfcl as fcl -import numpy as np import sys -from os.path import dirname, join, abspath +from os.path import abspath, dirname, join +import hppfcl as fcl +import numpy as np +import pinocchio as pin from pinocchio.visualize import MeshcatVisualizer # Load the URDF model. @@ -63,14 +64,14 @@ go_point_cloud = pin.GeometryObject( "point_cloud", 0, point_cloud_placement, point_cloud ) -go_point_cloud.meshColor = np.ones((4)) +go_point_cloud.meshColor = np.ones(4) collision_model.addGeometryObject(go_point_cloud) visual_model.addGeometryObject(go_point_cloud) go_height_field = pin.GeometryObject( "height_field", 0, height_field_placement, height_field ) -go_height_field.meshColor = np.ones((4)) +go_height_field.meshColor = np.ones(4) height_field_collision_id = collision_model.addGeometryObject(go_height_field) visual_model.addGeometryObject(go_height_field) @@ -86,7 +87,8 @@ collision_model.addCollisionPair(collision_pair) # Start a new MeshCat server and client. -# Note: the server can also be started separately using the "meshcat-server" command in a terminal: +# Note: the server can also be started separately using the "meshcat-server" command in +# a terminal: # this enables the server to remain active after the current script ends. # # Option open=True pens the visualizer. @@ -96,7 +98,8 @@ viz.initViewer(open=True) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) diff --git a/examples/collisions.py b/examples/collisions.py index 3c88db9ed6..449ef05ef1 100644 --- a/examples/collisions.py +++ b/examples/collisions.py @@ -1,7 +1,6 @@ -from __future__ import print_function -import pinocchio as pin +from os.path import abspath, dirname, join -from os.path import dirname, join, abspath +import pinocchio as pin pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") diff --git a/examples/contact-cholesky.py b/examples/contact-cholesky.py index 986aa38439..09c135bed6 100644 --- a/examples/contact-cholesky.py +++ b/examples/contact-cholesky.py @@ -1,6 +1,6 @@ -import pinocchio as pin +from os.path import abspath, dirname, join -from os.path import join, dirname, abspath +import pinocchio as pin pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") urdf_filename = ( diff --git a/examples/cppad/autodiff-rnea.py b/examples/cppad/autodiff-rnea.py index 4bfe2bfb02..7b4466efc3 100644 --- a/examples/cppad/autodiff-rnea.py +++ b/examples/cppad/autodiff-rnea.py @@ -1,8 +1,8 @@ -from pycppad import AD, Independent, ADFun -import pinocchio.cppad as ADpin -import pinocchio as pin import numpy as np +import pinocchio as pin +import pinocchio.cppad as ADpin from pinocchio.utils import isapprox +from pycppad import AD, ADFun, Independent pinmodel = pin.buildSampleModelHumanoidRandom() model = ADpin.Model(pinmodel) diff --git a/examples/display-shapes-meshcat.py b/examples/display-shapes-meshcat.py index ec538eb612..5ff1be1f99 100644 --- a/examples/display-shapes-meshcat.py +++ b/examples/display-shapes-meshcat.py @@ -1,4 +1,5 @@ import sys + import numpy as np import pinocchio as pin @@ -21,7 +22,7 @@ ] for i, geom in enumerate(geometries): placement = pin.SE3(np.eye(3), np.array([i, 0, 0])) - geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, geom, placement) + geom_obj = pin.GeometryObject(f"obj{i}", 0, 0, geom, placement) color = np.random.uniform(0, 1, 4) color[3] = 1 geom_obj.meshColor = color diff --git a/examples/display-shapes.py b/examples/display-shapes.py index cd8e5e081d..7c8fde33fb 100644 --- a/examples/display-shapes.py +++ b/examples/display-shapes.py @@ -1,4 +1,5 @@ import sys + import numpy as np import pinocchio as pin @@ -21,7 +22,7 @@ ] for i, geom in enumerate(geometries): placement = pin.SE3(np.eye(3), np.array([i, 0, 0])) - geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, placement, geom) + geom_obj = pin.GeometryObject(f"obj{i}", 0, 0, placement, geom) color = np.random.uniform(0, 1, 4) color[3] = 1 geom_obj.meshColor = color @@ -38,7 +39,8 @@ viz.initViewer() except ImportError as error: print( - "Error while initializing the viewer. It seems you should install gepetto-viewer" + "Error while initializing the viewer. " + "It seems you should install gepetto-viewer" ) print(error) sys.exit(0) @@ -47,7 +49,8 @@ viz.loadViewerModel("shapes") except AttributeError as error: print( - "Error while loading the viewer model. It seems you should start gepetto-viewer" + "Error while loading the viewer model. " + "It seems you should start gepetto-viewer" ) print(error) sys.exit(0) diff --git a/examples/floating-base-velocity-viewer.py b/examples/floating-base-velocity-viewer.py index f380abbf15..c3f1bfe2b3 100644 --- a/examples/floating-base-velocity-viewer.py +++ b/examples/floating-base-velocity-viewer.py @@ -1,9 +1,9 @@ -import pinocchio as pin -import numpy as np -import hppfcl +from time import sleep +import hppfcl +import numpy as np +import pinocchio as pin from pinocchio.visualize import MeshcatVisualizer -from time import sleep def create_pin_cube_model(j0="freeflyer"): diff --git a/examples/forward-dynamics-derivatives.py b/examples/forward-dynamics-derivatives.py index 833f3b3bd2..fbbae869d0 100644 --- a/examples/forward-dynamics-derivatives.py +++ b/examples/forward-dynamics-derivatives.py @@ -1,11 +1,13 @@ -import pinocchio as pin import numpy as np +import pinocchio as pin ## ## In this short script, we show how to compute the derivatives of the ## forward dynamics, using the algorithms explained in: ## -## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 +## Analytical Derivatives of Rigid Body Dynamics Algorithms, +## Justin Carpentier and Nicolas Mansard, +## Robotics: Science and Systems, 2018 ## # Create model and data diff --git a/examples/geometry-models.py b/examples/geometry-models.py index 2c83a260d9..a5848a93b2 100644 --- a/examples/geometry-models.py +++ b/examples/geometry-models.py @@ -1,6 +1,7 @@ -import pinocchio +from os.path import abspath, dirname, join from sys import argv -from os.path import dirname, join, abspath + +import pinocchio # This path refers to Pinocchio source code but you can define your own directory here. pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") @@ -24,7 +25,7 @@ # Sample a random configuration q = pinocchio.randomConfiguration(model) -print("q: %s" % q.T) +print(f"q: {q.T}") # Perform the forward kinematics over the kinematic tree pinocchio.forwardKinematics(model, data, q) @@ -36,14 +37,14 @@ # Print out the placement of each joint of the kinematic tree print("\nJoint placements:") for name, oMi in zip(model.names, data.oMi): - print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))) + print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)) # Print out the placement of each collision geometry object print("\nCollision object placements:") for k, oMg in enumerate(collision_data.oMg): - print(("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat))) + print("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat)) # Print out the placement of each visual geometry object print("\nVisual object placements:") for k, oMg in enumerate(visual_data.oMg): - print(("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat))) + print("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat)) diff --git a/examples/gepetto-viewer.py b/examples/gepetto-viewer.py index 3a81f080fd..3fb86e4c8c 100644 --- a/examples/gepetto-viewer.py +++ b/examples/gepetto-viewer.py @@ -1,10 +1,10 @@ # NOTE: this example needs gepetto-gui to be installed # usage: launch gepetto-gui and then run this test -import pinocchio as pin import sys -from os.path import dirname, join, abspath +from os.path import abspath, dirname, join +import pinocchio as pin from pinocchio.visualize import GepettoVisualizer # Load the URDF model. @@ -26,7 +26,8 @@ viz.initViewer() except ImportError as err: print( - "Error while initializing the viewer. It seems you should install gepetto-viewer" + "Error while initializing the viewer. " + "It seems you should install gepetto-viewer" ) print(err) sys.exit(0) @@ -35,7 +36,8 @@ viz.loadViewerModel("pinocchio") except AttributeError as err: print( - "Error while loading the viewer model. It seems you should start gepetto-viewer" + "Error while loading the viewer model. " + "It seems you should start gepetto-viewer" ) print(err) sys.exit(0) diff --git a/examples/inverse-dynamics-derivatives.py b/examples/inverse-dynamics-derivatives.py index 0543b89a6d..693e864977 100644 --- a/examples/inverse-dynamics-derivatives.py +++ b/examples/inverse-dynamics-derivatives.py @@ -1,11 +1,13 @@ -import pinocchio as pin import numpy as np +import pinocchio as pin ## ## In this short script, we show how to compute the derivatives of the ## inverse dynamics (RNEA), using the algorithms proposed in: ## -## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 +## Analytical Derivatives of Rigid Body Dynamics Algorithms, +## Justin Carpentier and Nicolas Mansard, +## Robotics: Science and Systems, 2018 ## # Create model and data diff --git a/examples/inverse-kinematics.py b/examples/inverse-kinematics.py index 9aa25cff16..693126b47b 100644 --- a/examples/inverse-kinematics.py +++ b/examples/inverse-kinematics.py @@ -1,9 +1,6 @@ -from __future__ import print_function - import numpy as np -from numpy.linalg import norm, solve - import pinocchio +from numpy.linalg import norm, solve model = pinocchio.buildSampleModelManipulator() data = model.createData() @@ -40,8 +37,10 @@ print("Convergence achieved!") else: print( - "\nWarning: the iterative algorithm has not reached convergence to the desired precision" + "\n" + "Warning: the iterative algorithm has not reached convergence " + "to the desired precision" ) -print("\nresult: %s" % q.flatten().tolist()) -print("\nfinal error: %s" % err.T) +print(f"\nresult: {q.flatten().tolist()}") +print(f"\nfinal error: {err.T}") diff --git a/examples/kinematics-derivatives.py b/examples/kinematics-derivatives.py index 6742d46cc6..1d75a805f6 100644 --- a/examples/kinematics-derivatives.py +++ b/examples/kinematics-derivatives.py @@ -1,5 +1,5 @@ -import pinocchio as pin import numpy as np +import pinocchio as pin # Create model and data @@ -24,7 +24,8 @@ joint_name = "rleg6_joint" joint_id = model.getJointId(joint_name) -# Derivatives of the spatial velocity with respect to the joint configuration and velocity vectors +# Derivatives of the spatial velocity with respect to the joint configuration and +# velocity vectors. (dv_dq, dv_dv) = pin.getJointVelocityDerivatives( model, data, joint_id, pin.ReferenceFrame.WORLD @@ -34,7 +35,8 @@ model, data, joint_id, pin.ReferenceFrame.LOCAL ) -# Derivatives of the spatial acceleration of the joint with respect to the joint configuration, velocity and acceleration vectors +# Derivatives of the spatial acceleration of the joint with respect to the joint +# configuration, velocity and acceleration vectors. (dv_dq, da_dq, da_dv, da_da) = pin.getJointAccelerationDerivatives( model, data, joint_id, pin.ReferenceFrame.WORLD diff --git a/examples/meshcat-viewer-dae.py b/examples/meshcat-viewer-dae.py index 752238b50b..50412c76cd 100644 --- a/examples/meshcat-viewer-dae.py +++ b/examples/meshcat-viewer-dae.py @@ -2,11 +2,11 @@ # Note: this feature requires Meshcat to be installed, this can be done using # pip install --user meshcat -import pinocchio as pin -import numpy as np import sys -from os.path import dirname, join, abspath +from os.path import abspath, dirname, join +import numpy as np +import pinocchio as pin from pinocchio.visualize import MeshcatVisualizer # Load the URDF model. @@ -23,7 +23,8 @@ ) # Start a new MeshCat server and client. -# Note: the server can also be started separately using the "meshcat-server" command in a terminal: +# Note: the server can also be started separately using the "meshcat-server" command in +# a terminal: # this enables the server to remain active after the current script ends. # # Option open=True pens the visualizer. @@ -33,14 +34,15 @@ viz.initViewer(open=True) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) # Load the robot in the viewer. -# Color is needed here because the Romeo URDF doesn't contain any color, so the default color results in an -# invisible robot (alpha value set to 0). +# Color is needed here because the Romeo URDF doesn't contain any color, so the default +# color results in an invisible robot (alpha value set to 0). viz.loadViewerModel(color=[0.0, 0.0, 0.0, 1.0]) # Display a robot configuration. diff --git a/examples/meshcat-viewer-octree.py b/examples/meshcat-viewer-octree.py index d131f823bb..b683c06b9a 100644 --- a/examples/meshcat-viewer-octree.py +++ b/examples/meshcat-viewer-octree.py @@ -2,12 +2,12 @@ # Note: this feature requires Meshcat to be installed, this can be done using # pip install --user meshcat -import pinocchio as pin -import numpy as np import sys -from pinocchio.visualize import MeshcatVisualizer import hppfcl as fcl +import numpy as np +import pinocchio as pin +from pinocchio.visualize import MeshcatVisualizer if tuple(map(int, fcl.__version__.split("."))) >= (3, 0, 0): with_octomap = fcl.WITH_OCTOMAP @@ -30,7 +30,8 @@ viz = MeshcatVisualizer(model, collision_model, visual_model) # Start a new MeshCat server and client. -# Note: the server can also be started separately using the "meshcat-server" command in a terminal: +# Note: the server can also be started separately using the "meshcat-server" command in +# a terminal: # this enables the server to remain active after the current script ends. # # Option open=True pens the visualizer. @@ -39,7 +40,8 @@ viz.initViewer(open=True) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) diff --git a/examples/meshcat-viewer-solo.py b/examples/meshcat-viewer-solo.py index ec4eaf4cde..c70eb5560e 100644 --- a/examples/meshcat-viewer-solo.py +++ b/examples/meshcat-viewer-solo.py @@ -1,12 +1,12 @@ """ -Pose a Solo-12 robot on a surface defined through a function and displayed through an hppfcl.HeightField. +Pose a Solo-12 robot on a surface defined through a function and displayed through an +hppfcl.HeightField. """ import numpy as np import pinocchio as pin - -from pinocchio.visualize import MeshcatVisualizer from example_robot_data import load +from pinocchio.visualize import MeshcatVisualizer robot = load("solo12") diff --git a/examples/meshcat-viewer.py b/examples/meshcat-viewer.py index 7da5540930..5729e5f8f9 100644 --- a/examples/meshcat-viewer.py +++ b/examples/meshcat-viewer.py @@ -2,11 +2,11 @@ # Note: this feature requires Meshcat to be installed, this can be done using # pip install --user meshcat -import pinocchio as pin -import numpy as np import sys -from os.path import dirname, join, abspath +from os.path import abspath, dirname, join +import numpy as np +import pinocchio as pin from pinocchio.visualize import MeshcatVisualizer # Load the URDF model. @@ -25,7 +25,8 @@ ) # Start a new MeshCat server and client. -# Note: the server can also be started separately using the "meshcat-server" command in a terminal: +# Note: the server can also be started separately using the "meshcat-server" command in +# a terminal: # this enables the server to remain active after the current script ends. # # Option open=True pens the visualizer. @@ -35,7 +36,8 @@ viz.initViewer(open=True) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) @@ -58,7 +60,7 @@ placement = pin.SE3.Identity() placement.translation[0] = 2.0 geometry = pin.GeometryObject("convex", 0, placement, convex) - geometry.meshColor = np.ones((4)) + geometry.meshColor = np.ones(4) # Add a PhongMaterial to the convex object geometry.overrideMaterial = True geometry.meshMaterial = pin.GeometryPhongMaterial() diff --git a/examples/overview-simple.py b/examples/overview-simple.py index 3c40bd9622..f2aa07384f 100644 --- a/examples/overview-simple.py +++ b/examples/overview-simple.py @@ -1,5 +1,3 @@ -from __future__ import print_function - import pinocchio model = pinocchio.buildSampleModelManipulator() diff --git a/examples/overview-urdf.py b/examples/overview-urdf.py index 568966a55c..b57e4a0254 100644 --- a/examples/overview-urdf.py +++ b/examples/overview-urdf.py @@ -1,11 +1,13 @@ -import pinocchio +from os.path import abspath, dirname, join from sys import argv -from os.path import dirname, join, abspath + +import pinocchio # This path refers to Pinocchio source code but you can define your own directory here. pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") -# You should change here to set up your own URDF file or just pass it as an argument of this example. +# You should change here to set up your own URDF file or just pass it as an argument of +# this example. urdf_filename = ( pinocchio_model_dir + "/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf" @@ -22,11 +24,11 @@ # Sample a random configuration q = pinocchio.randomConfiguration(model) -print("q: %s" % q.T) +print(f"q: {q.T}") # Perform the forward kinematics over the kinematic tree pinocchio.forwardKinematics(model, data, q) # Print out the placement of each joint of the kinematic tree for name, oMi in zip(model.names, data.oMi): - print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))) + print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)) diff --git a/examples/reachable-workspace-with-collisions.py b/examples/reachable-workspace-with-collisions.py index 87e890db62..54c386778d 100644 --- a/examples/reachable-workspace-with-collisions.py +++ b/examples/reachable-workspace-with-collisions.py @@ -73,7 +73,8 @@ def XYZRPYtoSE3(xyzrpy): viz.initViewer(open=True) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) @@ -89,7 +90,8 @@ def XYZRPYtoSE3(xyzrpy): n_samples = 5 facet_dims = 2 -# To have convex hull computation or just the points of the reachable workspace and then compute it with cgal +# To have convex hull computation or just the points of the reachable workspace and then +# compute it with cgal. convex = False if convex: @@ -127,7 +129,8 @@ def alpha_shape_with_cgal(coords, alpha=None): Retrieved from http://blog.thehumangeo.com/2014/05/12/drawing-boundaries-in-python/ :param coords : Coordinates of points - :param alpha: List of alpha values to influence the gooeyness of the border. Smaller numbers don't fall inward as much as larger numbers. + :param alpha: List of alpha values to influence the gooeyness of the border. + Smaller numbers don't fall inward as much as larger numbers. Too large, and you lose everything! :return: Shapely.MultiPolygons which is the hull of the input set of points """ diff --git a/examples/reachable-workspace.py b/examples/reachable-workspace.py index 1f3123455a..717d9ba2dc 100644 --- a/examples/reachable-workspace.py +++ b/examples/reachable-workspace.py @@ -27,7 +27,8 @@ viz.initViewer(open=True) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) @@ -43,7 +44,8 @@ n_samples = 5 facet_dims = 2 -# To have convex hull computation or just the points of the reachable workspace and then compute it with cgal +# To have convex hull computation or just the points of the reachable workspace and then +# compute it with cgal. convex = True if convex: @@ -81,7 +83,8 @@ def alpha_shape_with_cgal(coords, alpha=None): Retrieved from http://blog.thehumangeo.com/2014/05/12/drawing-boundaries-in-python/ :param coords : Coordinates of points - :param alpha: List of alpha values to influence the gooeyness of the border. Smaller numbers don't fall inward as much as larger numbers. + :param alpha: List of alpha values to influence the gooeyness of the border. + Smaller numbers don't fall inward as much as larger numbers. Too large, and you lose everything! :return: Shapely.MultiPolygons which is the hull of the input set of points """ diff --git a/examples/robot-wrapper-viewer.py b/examples/robot-wrapper-viewer.py index 50e677ab92..5eb0a076e8 100644 --- a/examples/robot-wrapper-viewer.py +++ b/examples/robot-wrapper-viewer.py @@ -3,11 +3,12 @@ # integrating different kinds of viewers # +from os.path import abspath, dirname, join +from sys import argv + import pinocchio as pin from pinocchio.robot_wrapper import RobotWrapper from pinocchio.visualize import GepettoVisualizer, MeshcatVisualizer -from sys import argv -from os.path import dirname, join, abspath # If you want to visualize the robot in this example, # you can choose which visualizer to employ diff --git a/examples/run-algo-in-parallel.py b/examples/run-algo-in-parallel.py index 8f74bbdde0..a931f67b6b 100644 --- a/examples/run-algo-in-parallel.py +++ b/examples/run-algo-in-parallel.py @@ -1,5 +1,5 @@ -import pinocchio as pin import numpy as np +import pinocchio as pin model = pin.buildSampleModelHumanoid() model.lowerPositionLimit[:7] = -np.ones(7) @@ -17,8 +17,8 @@ a = np.zeros((model.nv, batch_size)) tau = np.zeros((model.nv, batch_size)) -print("num_threads: {}".format(num_threads)) -print("batch_size: {}".format(batch_size)) +print(f"num_threads: {num_threads}") +print(f"batch_size: {batch_size}") # Call RNEA res_rnea = np.empty((model.nv, batch_size)) diff --git a/examples/rviz-viewer.py b/examples/rviz-viewer.py index 22b16e12f2..284108bf4b 100644 --- a/examples/rviz-viewer.py +++ b/examples/rviz-viewer.py @@ -1,9 +1,9 @@ # NOTE: this example needs RViz to be installed # usage: start ROS master (roscore) and then run this test -import pinocchio as pin -from os.path import dirname, join, abspath +from os.path import abspath, dirname, join +import pinocchio as pin from pinocchio.visualize import RVizVisualizer # Load the URDF model. diff --git a/examples/sample-model-viewer.py b/examples/sample-model-viewer.py index 590dc5f6c4..c895bf7ba2 100644 --- a/examples/sample-model-viewer.py +++ b/examples/sample-model-viewer.py @@ -1,13 +1,8 @@ -import pinocchio as pin -from pinocchio.visualize import MeshcatVisualizer, GepettoVisualizer, RVizVisualizer from sys import argv -from numpy import pi -try: - # Python 2 - input = raw_input # noqa -except NameError: - pass +import pinocchio as pin +from numpy import pi +from pinocchio.visualize import GepettoVisualizer, MeshcatVisualizer, RVizVisualizer # GepettoVisualizer: -g # MeshcatVisualizer: -m diff --git a/examples/simulation-closed-kinematic-chains.py b/examples/simulation-closed-kinematic-chains.py index 8f59e9b952..4997c6f7cd 100644 --- a/examples/simulation-closed-kinematic-chains.py +++ b/examples/simulation-closed-kinematic-chains.py @@ -45,9 +45,7 @@ geom_obj0 = pin.GeometryObject( "link_A1", base_joint_id, - pin.SE3( - pin.Quaternion.FromTwoVectors(pin.ZAxis, pin.XAxis).matrix(), np.zeros((3)) - ), + pin.SE3(pin.Quaternion.FromTwoVectors(pin.ZAxis, pin.XAxis).matrix(), np.zeros(3)), shape_link_A, ) geom_obj0.meshColor = WHITE_COLOR @@ -114,7 +112,7 @@ q = q0.copy() -y = np.ones((constraint_dim)) +y = np.ones(constraint_dim) data.M = np.eye(model.nv) * rho kkt_constraint = pin.ContactCholeskyDecomposition(model, [constraint_model]) eps = 1e-10 @@ -152,8 +150,8 @@ # Perform the simulation q = q_sol.copy() -v = np.zeros((model.nv)) -tau = np.zeros((model.nv)) +v = np.zeros(model.nv) +tau = np.zeros(model.nv) dt = 5e-3 T_sim = 100000 diff --git a/examples/simulation-contact-dynamics.py b/examples/simulation-contact-dynamics.py index 0ee104f497..c84e070cf2 100644 --- a/examples/simulation-contact-dynamics.py +++ b/examples/simulation-contact-dynamics.py @@ -1,11 +1,10 @@ -import pinocchio as pin - import math -import numpy as np import sys -from os.path import dirname, join, abspath import time +from os.path import abspath, dirname, join +import numpy as np +import pinocchio as pin from pinocchio.visualize import MeshcatVisualizer # Load the URDF model. @@ -24,7 +23,8 @@ ) # Start a new MeshCat server and client. -# Note: the server can also be started separately using the "meshcat-server" command in a terminal: +# Note: the server can also be started separately using the "meshcat-server" command in +# a terminal: # this enables the server to remain active after the current script ends. # # Option open=True pens the visualizer. @@ -34,7 +34,8 @@ viz.initViewer(open=False) except ImportError as err: print( - "Error while initializing the viewer. It seems you should install Python meshcat" + "Error while initializing the viewer. " + "It seems you should install Python meshcat" ) print(err) sys.exit(0) @@ -45,13 +46,13 @@ # Display a robot configuration. pin.loadReferenceConfigurations(model, srdf_full_path) q0 = model.referenceConfigurations["half_sitting"] -q_ref = pin.integrate(model, q0, 0.1 * np.random.rand((model.nv))) +q_ref = pin.integrate(model, q0, 0.1 * np.random.rand(model.nv)) viz.display(q0) feet_name = ["left_sole_link", "right_sole_link"] frame_ids = [model.getFrameId(frame_name) for frame_name in feet_name] -v0 = np.zeros((model.nv)) +v0 = np.zeros(model.nv) v_ref = v0.copy() data_sim = model.createData() data_control = model.createData() @@ -84,7 +85,7 @@ q = q0.copy() v = v0.copy() -tau = np.zeros((model.nv)) +tau = np.zeros(model.nv) T = 5 @@ -108,7 +109,7 @@ constraint_index += 6 A = np.vstack((S, J_constraint)) - b = pin.rnea(model, data_control, q, v, np.zeros((model.nv))) + b = pin.rnea(model, data_control, q, v, np.zeros(model.nv)) sol = np.linalg.lstsq(A.T, b, rcond=None)[0] tau = np.concatenate((np.zeros((6)), sol[: model.nv - 6])) diff --git a/examples/simulation-inverted-pendulum.py b/examples/simulation-inverted-pendulum.py index ee7a32ad21..a89376c5a0 100644 --- a/examples/simulation-inverted-pendulum.py +++ b/examples/simulation-inverted-pendulum.py @@ -35,7 +35,7 @@ geom1_name = "ball_" + str(k + 1) shape1 = fcl.Sphere(body_radius) geom1_obj = pin.GeometryObject(geom1_name, joint_id, body_placement, shape1) - geom1_obj.meshColor = np.ones((4)) + geom1_obj.meshColor = np.ones(4) geom_model.addGeometryObject(geom1_obj) geom2_name = "bar_" + str(k + 1) @@ -59,7 +59,8 @@ viz.initViewer() except ImportError as err: print( - "Error while initializing the viewer. It seems you should install gepetto-viewer" + "Error while initializing the viewer. " + "It seems you should install gepetto-viewer" ) print(err) sys.exit(0) @@ -68,7 +69,8 @@ viz.loadViewerModel("pinocchio") except AttributeError as err: print( - "Error while loading the viewer model. It seems you should start gepetto-viewer" + "Error while loading the viewer model. " + "It seems you should start gepetto-viewer" ) print(err) sys.exit(0) @@ -86,12 +88,12 @@ model.lowerPositionLimit.fill(-math.pi) model.upperPositionLimit.fill(+math.pi) q = pin.randomConfiguration(model) -v = np.zeros((model.nv)) +v = np.zeros(model.nv) t = 0.0 data_sim = model.createData() for k in range(N): - tau_control = np.zeros((model.nv)) + tau_control = np.zeros(model.nv) a = pin.aba(model, data_sim, q, v, tau_control) # Forward dynamics v += a * dt diff --git a/examples/simulation-pendulum.py b/examples/simulation-pendulum.py index 37b42f49e0..77cfd2bd94 100644 --- a/examples/simulation-pendulum.py +++ b/examples/simulation-pendulum.py @@ -47,9 +47,9 @@ body_inertia = pin.Inertia.FromCylinder(cart_mass, cart_radius, cart_length) body_placement = geometry_placement - model.appendBodyToJoint( - joint_id, body_inertia, body_placement - ) # We need to rotate the inertia as it is expressed in the LOCAL frame of the geometry + # We need to rotate the inertia as it is expressed in the LOCAL frame of the + # geometry. + model.appendBodyToJoint(joint_id, body_inertia, body_placement) shape_cart = fcl.Cylinder(cart_radius, cart_length) @@ -85,7 +85,7 @@ geom1_name = "ball_" + str(k + 1) shape1 = fcl.Sphere(body_radius) geom1_obj = pin.GeometryObject(geom1_name, joint_id, body_placement, shape1) - geom1_obj.meshColor = np.ones((4)) + geom1_obj.meshColor = np.ones(4) geom_model.addGeometryObject(geom1_obj) geom2_name = "bar_" + str(k + 1) @@ -108,7 +108,8 @@ viz.initViewer() except ImportError as err: print( - "Error while initializing the viewer. It seems you should install gepetto-viewer" + "Error while initializing the viewer. " + "It seems you should install gepetto-viewer" ) print(err) sys.exit(0) @@ -117,7 +118,8 @@ viz.loadViewerModel("pinocchio") except AttributeError as err: print( - "Error while loading the viewer model. It seems you should start gepetto-viewer" + "Error while loading the viewer model. " + "It seems you should start gepetto-viewer" ) print(err) sys.exit(0) @@ -142,8 +144,8 @@ t = 0.0 q = pin.randomConfiguration(model) -v = np.zeros((model.nv)) -tau_control = np.zeros((model.nv)) +v = np.zeros(model.nv) +tau_control = np.zeros(model.nv) damping_value = 0.1 for k in range(N): tic = time.time() diff --git a/examples/static-contact-dynamics.py b/examples/static-contact-dynamics.py index 3b60fddc04..29c0109bea 100644 --- a/examples/static-contact-dynamics.py +++ b/examples/static-contact-dynamics.py @@ -1,29 +1,33 @@ +from os.path import abspath, dirname, join + import numpy as np import pinocchio as pin -from os.path import dirname, join, abspath - np.set_printoptions(linewidth=np.inf) # ----- PROBLEM STATEMENT ------ # -# We want to find the contact forces and torques required to stand still at a configuration 'q0'. +# We want to find the contact forces and torques required to stand still at a +# configuration 'q0'. # We assume 3D contacts at each of the feet # # The dynamic equation would look like: # -# M*q_ddot + g(q) + C(q, q_dot) = tau + J^T*lambda --> (for the static case) --> g(q) = tau + Jc^T*lambda (1) +# M*q_ddot + g(q) + C(q, q_dot) = tau + J^T*lambda --> (for the static case) --> g(q) = +# tau + Jc^T*lambda (1). # ----- SOLVING STRATEGY ------ -# Split the equation between the base link (_bl) joint and the rest of the joints (_j). That is, +# Split the equation between the base link (_bl) joint and the rest of the joints (_j). +# That is, # # | g_bl | | 0 | | Jc__feet_bl.T | | l1 | # | g_j | = | tau | + | Jc__feet_j.T | * | l2 | (2) # | l3 | # | l4 | -# First, find the contact forces l1, l2, l3, l4 (these are 3 dimensional) by solving for the first 6 rows of (2). +# First, find the contact forces l1, l2, l3, l4 (these are 3 dimensional) by solving for +# the first 6 rows of (2). # That is, # # g_bl = Jc__feet_bl.T * | l1 | @@ -88,7 +92,8 @@ # 1. GRAVITY TERM -# We compute the gravity terms by using the ID at desired configuration q0, with velocity and acceleration being 0. I.e., ID with a = v = 0 +# We compute the gravity terms by using the ID at desired configuration q0, with +# velocity and acceleration being 0. I.e., ID with a = v = 0. g_grav = pin.rnea(model, data, q0, v0, a0) g_bl = g_grav[:6] @@ -96,7 +101,8 @@ # 2. FIND CONTACTS -# First, we set the frame for our contacts. We assume the contacts are placed at the following 4 frames and they are 3D +# First, we set the frame for our contacts. We assume the contacts are placed at the +# following 4 frames and they are 3D. feet_names = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"] feet_ids = [model.getFrameId(n) for n in feet_names] bl_id = model.getFrameId("base_link") @@ -110,7 +116,8 @@ Js__feet_bl = [np.copy(J[:3, :6]) for J in Js__feet_q] -# Notice that we can write the equation above as an horizontal stack of Jacobians transposed and vertical stack of contact forces +# Notice that we can write the equation above as an horizontal stack of Jacobians +# transposed and vertical stack of contact forces. Jc__feet_bl_T = np.zeros([6, 3 * ncontact]) Jc__feet_bl_T[:, :] = np.vstack(Js__feet_bl).T @@ -132,13 +139,12 @@ print("\n--- CONTACT FORCES ---") for l__f, foot_id, name in zip(ls__bl, feet_ids, feet_names): - print("Contact force at foot {} expressed at the BL is: {}".format(name, l__f)) + print(f"Contact force at foot {name} expressed at the BL is: {l__f}") # Notice that if we add all the contact forces are equal to the g_grav print( - "Error between contact forces and gravity at base link: {}".format( - np.linalg.norm(g_bl - sum(ls__bl)) - ) + "Error between contact forces and gravity at base link: " + f"{np.linalg.norm(g_bl - sum(ls__bl))}" ) # 3. FIND TAU @@ -154,7 +160,8 @@ # 4. CROSS CHECKS # INVERSE DYNAMICS -# We can compare this torques with the ones one would obtain when computing the ID considering the external forces in ls +# We can compare this torques with the ones one would obtain when computing the ID +# considering the external forces in ls. pin.framesForwardKinematics(model, data, q0) joint_names = ["FL_KFE", "FR_KFE", "HL_KFE", "HR_KFE"] @@ -169,12 +176,13 @@ tau_rnea = pin.rnea(model, data, q0, v0, a0, fs_ext) print("\n--- ID: JOINT TORQUES ---") -print("Tau from RNEA: {}".format(tau_rnea)) -print("Tau computed manually: {}".format(np.append(np.zeros(6), tau))) -print("Tau error: {}".format(np.linalg.norm(np.append(np.zeros(6), tau) - tau_rnea))) +print(f"Tau from RNEA: {tau_rnea}") +print(f"Tau computed manually: {np.append(np.zeros(6), tau)}") +print(f"Tau error: {np.linalg.norm(np.append(np.zeros(6), tau) - tau_rnea)}") # FORWARD DYNAMICS -# We can also check the results using FD. FD with the tau we got, q0 and v0, should give 0 acceleration and the contact forces +# We can also check the results using FD. FD with the tau we got, q0 and v0, should give +# 0 acceleration and the contact forces. Js_feet3d_q = [np.copy(J[:3, :]) for J in Js__feet_q] acc = pin.forwardDynamics( model, @@ -187,7 +195,7 @@ ) print("\n--- FD: ACC. & CONTACT FORCES ---") -print("Norm of the FD acceleration: {}".format(np.linalg.norm(acc))) -print("Contact forces manually: {}".format(ls)) -print("Contact forces FD: {}".format(data.lambda_c)) -print("Contact forces error: {}".format(np.linalg.norm(data.lambda_c - ls))) +print(f"Norm of the FD acceleration: {np.linalg.norm(acc)}") +print(f"Contact forces manually: {ls}") +print(f"Contact forces FD: {data.lambda_c}") +print(f"Contact forces error: {np.linalg.norm(data.lambda_c - ls)}") diff --git a/examples/talos-simulation.py b/examples/talos-simulation.py index a3442a9f70..1d8f0b1703 100644 --- a/examples/talos-simulation.py +++ b/examples/talos-simulation.py @@ -1,8 +1,8 @@ from time import sleep +import example_robot_data import numpy as np import pinocchio -import example_robot_data robot = example_robot_data.load("talos") model = robot.model @@ -90,7 +90,7 @@ def squashing(model, data, q_in): q = q_in.copy() - y = np.ones((constraint_dim)) + y = np.ones(constraint_dim) N_full = 200 diff --git a/examples/update-model-after-urdf.py b/examples/update-model-after-urdf.py index 91d927aa78..926bf25d4c 100644 --- a/examples/update-model-after-urdf.py +++ b/examples/update-model-after-urdf.py @@ -1,5 +1,4 @@ #!/usr/bin/env python -# -*- coding: utf-8 -*- # # Copyright (C) 2023 Inria @@ -23,7 +22,7 @@ def check_limb_lengths(limb_length: float) -> bool: - print("Checking that limbs are %s m long... " % limb_length, end="") + print(f"Checking that limbs are {limb_length} m long... ", end="") for side in ("left", "right"): for joint in ("knee", "wheel"): joint_id = model.getJointId(f"{side}_{joint}") diff --git a/models/simple_model.py b/models/simple_model.py index 2e446d6f25..dac4168e35 100644 --- a/models/simple_model.py +++ b/models/simple_model.py @@ -2,8 +2,10 @@ # Copyright (c) 2016-2023 CNRS INRIA # -import pinocchio as pin from math import pi +from typing import ClassVar + +import pinocchio as pin from pinocchio.utils import np, rotate DENSITY = 1 @@ -22,7 +24,7 @@ def color(body_number=1): return [int(i) for i in "%03d" % int(bin(body_number % 8)[2:])] + [1] -class ModelWrapper(object): +class ModelWrapper: def __init__(self, name=None, display=False): self.visuals = [ ("universe", pin.SE3.Identity(), pin.SE3.Identity().translation) @@ -61,7 +63,7 @@ def add_joint( elif isinstance(lever, dict): lever = placement(**lever) joint_name, body_name = ( - "world/%s_%s_%s" % (self.name, joint_name, i) for i in ("joint", "body") + f"world/{self.name}_{joint_name}_{i}" for i in ("joint", "body") ) body_inertia = pin.Inertia.Random() if shape == "box": @@ -113,7 +115,7 @@ def place(self): self.display.viewer.gui.refresh() -class RobotDisplay(object): +class RobotDisplay: def __init__(self, window_name="pinocchio"): from gepetto import corbaserver @@ -133,7 +135,7 @@ def place(self, obj_name, m): class SimplestWalker(ModelWrapper): - joints = [ + joints: ClassVar = [ { "joint_name": "pelvis", "dimensions": (0.1, 0.2, 0.1), diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000000..b70c5e0372 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,5 @@ +[tool.ruff] +exclude = [ "cmake/*", "models/example-robot-data/*" ] + +[tool.ruff.lint] +select = [ "E", "F", "I", "RUF", "UP", "W" ] diff --git a/unittest/python/bindings.py b/unittest/python/bindings.py index 1218e2f8ba..9651b9c20e 100644 --- a/unittest/python/bindings.py +++ b/unittest/python/bindings.py @@ -1,7 +1,7 @@ import unittest + import pinocchio as pin from pinocchio.utils import np, npl, rand, zero - from test_case import PinocchioTestCase as TestCase # This whole file seems to be outdated and superseded by more recent tests @@ -37,7 +37,8 @@ def test_se3(self): # Currently, the different cases do not throw the same exception type. # To have a more robust test, only Exception is checked. - # In the comments, the most specific actual exception class at the time of writing + # In the comments, the most specific actual exception class at the time of + # writing p = rand(5) with self.assertRaises(Exception): # RuntimeError m * p diff --git a/unittest/python/bindings_Symmetric3.py b/unittest/python/bindings_Symmetric3.py index 70b668c14e..43136aefa4 100644 --- a/unittest/python/bindings_Symmetric3.py +++ b/unittest/python/bindings_Symmetric3.py @@ -1,8 +1,8 @@ import unittest -import pinocchio as pin + import numpy as np +import pinocchio as pin from pinocchio.utils import eye, zero - from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/bindings_aba.py b/unittest/python/bindings_aba.py index d6f475242b..47c75f63ad 100644 --- a/unittest/python/bindings_aba.py +++ b/unittest/python/bindings_aba.py @@ -1,8 +1,8 @@ import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestABA(TestCase): diff --git a/unittest/python/bindings_centroidal_dynamics_derivatives.py b/unittest/python/bindings_centroidal_dynamics_derivatives.py index 836ee579fb..9626a99c9c 100644 --- a/unittest/python/bindings_centroidal_dynamics_derivatives.py +++ b/unittest/python/bindings_centroidal_dynamics_derivatives.py @@ -1,8 +1,8 @@ import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestDeriavtives(TestCase): @@ -12,8 +12,8 @@ def setUp(self): qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) - self.v = np.random.rand((self.model.nv)) - self.a = np.random.rand((self.model.nv)) + self.v = np.random.rand(self.model.nv) + self.a = np.random.rand(self.model.nv) def test_centroidal_derivatives(self): res = pin.computeCentroidalDynamicsDerivatives( diff --git a/unittest/python/bindings_com.py b/unittest/python/bindings_com.py index a5a53a1772..1714bd9087 100644 --- a/unittest/python/bindings_com.py +++ b/unittest/python/bindings_com.py @@ -1,8 +1,9 @@ import unittest -from test_case import PinocchioTestCase as TestCase + +import numpy as np import pinocchio as pin from pinocchio.utils import rand -import numpy as np +from test_case import PinocchioTestCase as TestCase class TestComBindings(TestCase): diff --git a/unittest/python/bindings_contact_inverse_dynamics.py b/unittest/python/bindings_contact_inverse_dynamics.py index c5d2dfc057..c610b392de 100644 --- a/unittest/python/bindings_contact_inverse_dynamics.py +++ b/unittest/python/bindings_contact_inverse_dynamics.py @@ -1,7 +1,7 @@ import os import unittest -import numpy as np +import numpy as np import pinocchio as pin from test_case import PinocchioTestCase as TestCase @@ -46,8 +46,8 @@ def test_call_to_contact_inverse_dynamics(self): frame_ids = [model.getFrameId(frame_name) for frame_name in feet_name] q = self.q0 - v = np.zeros((model.nv)) - a = np.zeros((model.nv)) + v = np.zeros(model.nv) + a = np.zeros(model.nv) data = model.createData() contact_models_list = [] diff --git a/unittest/python/bindings_data.py b/unittest/python/bindings_data.py index 60bbde27dc..a93372e97c 100644 --- a/unittest/python/bindings_data.py +++ b/unittest/python/bindings_data.py @@ -1,6 +1,6 @@ import unittest -import pinocchio as pin +import pinocchio as pin from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/bindings_dynamics.py b/unittest/python/bindings_dynamics.py index bff0226480..36203a504e 100644 --- a/unittest/python/bindings_dynamics.py +++ b/unittest/python/bindings_dynamics.py @@ -3,7 +3,6 @@ import numpy as np import pinocchio as pin from pinocchio.utils import rand, zero - from test_case import PinocchioTestCase as TestCase # common quantities for all tests. diff --git a/unittest/python/bindings_fcl_transform.py b/unittest/python/bindings_fcl_transform.py index 777af355d6..f2c5490e0e 100644 --- a/unittest/python/bindings_fcl_transform.py +++ b/unittest/python/bindings_fcl_transform.py @@ -1,4 +1,5 @@ import unittest + import pinocchio as pin diff --git a/unittest/python/bindings_force.py b/unittest/python/bindings_force.py index 554f46c2a6..626ce6ed99 100644 --- a/unittest/python/bindings_force.py +++ b/unittest/python/bindings_force.py @@ -1,7 +1,8 @@ import unittest -import pinocchio as pin + import numpy as np -from pinocchio.utils import zero, rand +import pinocchio as pin +from pinocchio.utils import rand, zero class TestForceBindings(unittest.TestCase): diff --git a/unittest/python/bindings_forward_dynamics_derivatives.py b/unittest/python/bindings_forward_dynamics_derivatives.py index d5d35e5e9f..691d62aaa1 100644 --- a/unittest/python/bindings_forward_dynamics_derivatives.py +++ b/unittest/python/bindings_forward_dynamics_derivatives.py @@ -1,8 +1,8 @@ import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestDeriavtives(TestCase): @@ -12,8 +12,8 @@ def setUp(self): qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) - self.v = np.random.rand((self.model.nv)) - self.tau = np.random.rand((self.model.nv)) + self.v = np.random.rand(self.model.nv) + self.tau = np.random.rand(self.model.nv) self.fext = [] for _ in range(self.model.njoints): diff --git a/unittest/python/bindings_frame.py b/unittest/python/bindings_frame.py index a7d3df597f..e5c80f975c 100644 --- a/unittest/python/bindings_frame.py +++ b/unittest/python/bindings_frame.py @@ -1,7 +1,7 @@ import unittest -import pinocchio as pin -import numpy as np +import numpy as np +import pinocchio as pin from test_case import PinocchioTestCase @@ -148,7 +148,7 @@ def test_frame_algo(self): data = model.createData() q = pin.neutral(model) - v = np.random.rand((model.nv)) + v = np.random.rand(model.nv) frame_id = self.frame_idx J1 = pin.computeFrameJacobian(model, data, q, frame_id) diff --git a/unittest/python/bindings_frame_derivatives.py b/unittest/python/bindings_frame_derivatives.py index 30a1154224..8ca4da4ae2 100644 --- a/unittest/python/bindings_frame_derivatives.py +++ b/unittest/python/bindings_frame_derivatives.py @@ -1,7 +1,7 @@ import unittest -import pinocchio as pin -import numpy as np +import numpy as np +import pinocchio as pin from test_case import PinocchioTestCase @@ -29,8 +29,8 @@ def setUp(self): self.data = self.model.createData() self.q = pin.randomConfiguration(self.model) - self.v = np.random.rand((self.model.nv)) - self.a = np.random.rand((self.model.nv)) + self.v = np.random.rand(self.model.nv) + self.a = np.random.rand(self.model.nv) def tearDown(self): del self.model diff --git a/unittest/python/bindings_geometry_model.py b/unittest/python/bindings_geometry_model.py index b0d5ad8522..2e46dec564 100644 --- a/unittest/python/bindings_geometry_model.py +++ b/unittest/python/bindings_geometry_model.py @@ -1,4 +1,5 @@ import unittest + import pinocchio as pin diff --git a/unittest/python/bindings_geometry_model_urdf.py b/unittest/python/bindings_geometry_model_urdf.py index 25879e9fc0..c8a08f3856 100644 --- a/unittest/python/bindings_geometry_model_urdf.py +++ b/unittest/python/bindings_geometry_model_urdf.py @@ -1,6 +1,7 @@ +import os import unittest + import pinocchio as pin -import os def checkGeom(geom1, geom2): diff --git a/unittest/python/bindings_geometry_object.py b/unittest/python/bindings_geometry_object.py index 1ffa4b9745..799499172b 100644 --- a/unittest/python/bindings_geometry_object.py +++ b/unittest/python/bindings_geometry_object.py @@ -1,6 +1,7 @@ import unittest -import pinocchio as pin + import numpy as np +import pinocchio as pin @unittest.skipUnless(pin.WITH_HPP_FCL, "Needs HPP-FCL") diff --git a/unittest/python/bindings_inertia.py b/unittest/python/bindings_inertia.py index 5d64bcbaa3..3fb9148767 100644 --- a/unittest/python/bindings_inertia.py +++ b/unittest/python/bindings_inertia.py @@ -3,7 +3,6 @@ import numpy as np import pinocchio as pin from pinocchio.utils import eye, rand, zero - from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/bindings_inverse_dynamics_derivatives.py b/unittest/python/bindings_inverse_dynamics_derivatives.py index 5cc0f5f01a..ed96b0867f 100644 --- a/unittest/python/bindings_inverse_dynamics_derivatives.py +++ b/unittest/python/bindings_inverse_dynamics_derivatives.py @@ -1,8 +1,8 @@ import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestDeriavtives(TestCase): @@ -12,8 +12,8 @@ def setUp(self): qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) - self.v = np.random.rand((self.model.nv)) - self.a = np.random.rand((self.model.nv)) + self.v = np.random.rand(self.model.nv) + self.a = np.random.rand(self.model.nv) self.fext = [] for _ in range(self.model.njoints): diff --git a/unittest/python/bindings_joint_algorithms.py b/unittest/python/bindings_joint_algorithms.py index f375b75d38..2f1782099b 100644 --- a/unittest/python/bindings_joint_algorithms.py +++ b/unittest/python/bindings_joint_algorithms.py @@ -1,8 +1,8 @@ import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestJointsAlgo(TestCase): @@ -16,23 +16,23 @@ def setUp(self): def test_basic(self): model = self.model - q_ones = np.ones((model.nq)) + q_ones = np.ones(model.nq) self.assertFalse(pin.isNormalized(model, q_ones)) self.assertFalse(pin.isNormalized(model, q_ones, 1e-8)) self.assertTrue(pin.isNormalized(model, q_ones, 1e2)) - q_rand = np.random.rand((model.nq)) + q_rand = np.random.rand(model.nq) q_rand = pin.normalize(model, q_rand) self.assertTrue(pin.isNormalized(model, q_rand)) self.assertTrue(pin.isNormalized(model, q_rand, 1e-8)) self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.0) <= 1e-8) - q_next = pin.integrate(model, self.q, np.zeros((model.nv))) + q_next = pin.integrate(model, self.q, np.zeros(model.nv)) self.assertApprox(q_next, self.q) v_diff = pin.difference(model, self.q, q_next) - self.assertApprox(v_diff, np.zeros((model.nv))) + self.assertApprox(v_diff, np.zeros(model.nv)) q_next = pin.integrate(model, self.q, self.v) q_int = pin.interpolate(model, self.q, q_next, 0.5) @@ -49,9 +49,7 @@ def test_basic(self): self.assertApprox(q_neutral, q_neutral) q_rand1 = pin.randomConfiguration(model) - q_rand2 = pin.randomConfiguration( - model, -np.ones((model.nq)), np.ones((model.nq)) - ) + q_rand2 = pin.randomConfiguration(model, -np.ones(model.nq), np.ones(model.nq)) self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8)) diff --git a/unittest/python/bindings_joint_composite.py b/unittest/python/bindings_joint_composite.py index 62f601b957..b042dd0939 100644 --- a/unittest/python/bindings_joint_composite.py +++ b/unittest/python/bindings_joint_composite.py @@ -1,4 +1,5 @@ import unittest + import pinocchio as pin diff --git a/unittest/python/bindings_joints.py b/unittest/python/bindings_joints.py index 73181fe62f..f82778be9c 100644 --- a/unittest/python/bindings_joints.py +++ b/unittest/python/bindings_joints.py @@ -1,8 +1,8 @@ import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestJointsAlgo(TestCase): @@ -16,23 +16,23 @@ def setUp(self): def test_basic(self): model = self.model - q_ones = np.ones((model.nq)) + q_ones = np.ones(model.nq) self.assertFalse(pin.isNormalized(model, q_ones)) self.assertFalse(pin.isNormalized(model, q_ones, 1e-8)) self.assertTrue(pin.isNormalized(model, q_ones, 1e2)) - q_rand = np.random.rand((model.nq)) + q_rand = np.random.rand(model.nq) q_rand = pin.normalize(model, q_rand) self.assertTrue(pin.isNormalized(model, q_rand)) self.assertTrue(pin.isNormalized(model, q_rand, 1e-8)) self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.0) <= 1e-8) - q_next = pin.integrate(model, self.q, np.zeros((model.nv))) + q_next = pin.integrate(model, self.q, np.zeros(model.nv)) self.assertApprox(q_next, self.q) v_diff = pin.difference(model, self.q, q_next) - self.assertApprox(v_diff, np.zeros((model.nv))) + self.assertApprox(v_diff, np.zeros(model.nv)) q_next = pin.integrate(model, self.q, self.v) q_int = pin.interpolate(model, self.q, q_next, 0.5) @@ -49,9 +49,7 @@ def test_basic(self): self.assertApprox(q_neutral, q_neutral) q_rand1 = pin.randomConfiguration(model) - q_rand2 = pin.randomConfiguration( - model, -np.ones((model.nq)), np.ones((model.nq)) - ) + q_rand2 = pin.randomConfiguration(model, -np.ones(model.nq), np.ones(model.nq)) self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8)) diff --git a/unittest/python/bindings_kinematic_regressor.py b/unittest/python/bindings_kinematic_regressor.py index 299f3b1cac..7224a24ce6 100644 --- a/unittest/python/bindings_kinematic_regressor.py +++ b/unittest/python/bindings_kinematic_regressor.py @@ -1,6 +1,7 @@ import unittest -from test_case import PinocchioTestCase as TestCase + import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestKinematicRegressorBindings(TestCase): diff --git a/unittest/python/bindings_kinematics.py b/unittest/python/bindings_kinematics.py index 6802e61624..402e712f1b 100644 --- a/unittest/python/bindings_kinematics.py +++ b/unittest/python/bindings_kinematics.py @@ -1,7 +1,7 @@ import unittest -import pinocchio as pin -import numpy as np +import numpy as np +import pinocchio as pin from test_case import PinocchioTestCase diff --git a/unittest/python/bindings_kinematics_derivatives.py b/unittest/python/bindings_kinematics_derivatives.py index 8a3fd97620..6096454ec1 100644 --- a/unittest/python/bindings_kinematics_derivatives.py +++ b/unittest/python/bindings_kinematics_derivatives.py @@ -1,7 +1,7 @@ import unittest -import pinocchio as pin -import numpy as np +import numpy as np +import pinocchio as pin from test_case import PinocchioTestCase @@ -16,8 +16,8 @@ def setUp(self): self.data = self.model.createData() self.q = pin.randomConfiguration(self.model) - self.v = np.random.rand((self.model.nv)) - self.a = np.random.rand((self.model.nv)) + self.v = np.random.rand(self.model.nv) + self.a = np.random.rand(self.model.nv) def tearDown(self): del self.model diff --git a/unittest/python/bindings_liegroups.py b/unittest/python/bindings_liegroups.py index 5cf0293f71..28e7983438 100644 --- a/unittest/python/bindings_liegroups.py +++ b/unittest/python/bindings_liegroups.py @@ -2,7 +2,6 @@ import numpy as np import pinocchio as pin - from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/bindings_model.py b/unittest/python/bindings_model.py index 32eb52a80e..23067b7f45 100644 --- a/unittest/python/bindings_model.py +++ b/unittest/python/bindings_model.py @@ -1,7 +1,7 @@ import unittest + import pinocchio as pin from pinocchio.utils import np, zero - from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/bindings_motion.py b/unittest/python/bindings_motion.py index df3364c417..6592396e39 100644 --- a/unittest/python/bindings_motion.py +++ b/unittest/python/bindings_motion.py @@ -1,7 +1,8 @@ import unittest -import pinocchio as pin + import numpy as np -from pinocchio.utils import zero, rand +import pinocchio as pin +from pinocchio.utils import rand, zero class TestMotionBindings(unittest.TestCase): diff --git a/unittest/python/bindings_regressor.py b/unittest/python/bindings_regressor.py index 595d67f259..e99396331b 100644 --- a/unittest/python/bindings_regressor.py +++ b/unittest/python/bindings_regressor.py @@ -2,7 +2,6 @@ import pinocchio as pin from pinocchio.utils import zero - from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/bindings_rnea.py b/unittest/python/bindings_rnea.py index c2459bf602..88c54a0f8c 100644 --- a/unittest/python/bindings_rnea.py +++ b/unittest/python/bindings_rnea.py @@ -1,8 +1,8 @@ import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestRNEA(TestCase): diff --git a/unittest/python/bindings_sample_models.py b/unittest/python/bindings_sample_models.py index b13c483a70..159a5f6139 100644 --- a/unittest/python/bindings_sample_models.py +++ b/unittest/python/bindings_sample_models.py @@ -1,7 +1,6 @@ import unittest import pinocchio as pin - from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/bindings_spatial.py b/unittest/python/bindings_spatial.py index e8e9ee6ff8..f5d9e6d1e9 100644 --- a/unittest/python/bindings_spatial.py +++ b/unittest/python/bindings_spatial.py @@ -6,9 +6,8 @@ from pinocchio import skew, skewSquare, unSkew try: - from pinocchio import casadi as cpin - import casadi + from pinocchio import casadi as cpin WITH_CASADI = True except ImportError: @@ -74,7 +73,7 @@ def test_Jlog6(self): R = dM.rotation logR = pin.log3(R) Jrot = pin.Jlog3(R) - print(R, ":\nlog: {}".format(logR)) + print(R, f":\nlog: {logR}") print(Jrot) self.assertApprox(dM, pin.SE3.Identity()) diff --git a/unittest/python/bindings_std_map.py b/unittest/python/bindings_std_map.py index 6dcfc97edc..0165bed0dd 100644 --- a/unittest/python/bindings_std_map.py +++ b/unittest/python/bindings_std_map.py @@ -1,10 +1,9 @@ +import pickle import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np - -import pickle +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestStdMap(TestCase): @@ -17,7 +16,7 @@ def test_pickle(self): for k in range(100): key_name = "key_" + str(k + 1) keys.append(key_name) - map[key_name] = np.random.rand((10)) + map[key_name] = np.random.rand(10) pickle.dump(map, open("save_std_map.p", "wb")) diff --git a/unittest/python/bindings_std_vector.py b/unittest/python/bindings_std_vector.py index b648aa8a9f..e4acf30dcb 100644 --- a/unittest/python/bindings_std_vector.py +++ b/unittest/python/bindings_std_vector.py @@ -1,10 +1,9 @@ +import pickle import unittest -from test_case import PinocchioTestCase as TestCase -import pinocchio as pin import numpy as np - -import pickle +import pinocchio as pin +from test_case import PinocchioTestCase as TestCase class TestStdMap(TestCase): @@ -14,7 +13,7 @@ def setUp(self): def test_pickle(self): vec = pin.StdVec_Vector3() for k in range(100): - vec.append(np.random.rand((3))) + vec.append(np.random.rand(3)) pickle.dump(vec, open("save_std_vec.p", "wb")) diff --git a/unittest/python/bindings_urdf.py b/unittest/python/bindings_urdf.py index 8505fe2f1b..b61502cf30 100644 --- a/unittest/python/bindings_urdf.py +++ b/unittest/python/bindings_urdf.py @@ -1,6 +1,7 @@ +import os import unittest + import pinocchio as pin -import os @unittest.skipUnless(pin.WITH_URDFDOM, "Needs URDFDOM") diff --git a/unittest/python/casadi/bindings_explog.py b/unittest/python/casadi/bindings_explog.py index 7f3c00fb00..8e60389493 100644 --- a/unittest/python/casadi/bindings_explog.py +++ b/unittest/python/casadi/bindings_explog.py @@ -4,13 +4,12 @@ sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +import casadi import numpy as np import pinocchio as pin import pinocchio.casadi as cpin -from test_case import PinocchioTestCase as TestCase - -import casadi from casadi import SX +from test_case import PinocchioTestCase as TestCase class TestLogExpDerivatives(TestCase): diff --git a/unittest/python/casadi/bindings_main_algo.py b/unittest/python/casadi/bindings_main_algo.py index 4dfd47dc68..1eba2bb79e 100644 --- a/unittest/python/casadi/bindings_main_algo.py +++ b/unittest/python/casadi/bindings_main_algo.py @@ -1,16 +1,14 @@ -import unittest -import sys import os +import sys +import unittest sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from test_case import PinocchioTestCase as TestCase - -import pinocchio as pin -from pinocchio import casadi as cpin - import casadi -from casadi import SX import numpy as np +import pinocchio as pin +from casadi import SX +from pinocchio import casadi as cpin +from test_case import PinocchioTestCase as TestCase class TestMainAlgos(TestCase): diff --git a/unittest/python/explog.py b/unittest/python/explog.py index 3ed9b19492..14bf0ba6e5 100644 --- a/unittest/python/explog.py +++ b/unittest/python/explog.py @@ -1,12 +1,10 @@ -import unittest import math +import unittest import numpy as np import pinocchio as pin - -from pinocchio.utils import rand, zero, eye from pinocchio.explog import exp, log - +from pinocchio.utils import eye, rand, zero from test_case import PinocchioTestCase as TestCase @@ -83,8 +81,8 @@ def test_Jlog6(self): self.assertApprox(J, eye(6)) def test_skew(self): - u = np.random.rand((3)) - v = np.random.rand((3)) + u = np.random.rand(3) + v = np.random.rand(3) u_skew = pin.skew(u) u_unskew = pin.unSkew(u_skew) diff --git a/unittest/python/pybind11/test-cpp2pybind11.py b/unittest/python/pybind11/test-cpp2pybind11.py index d2b864aa08..c522cd9e9d 100644 --- a/unittest/python/pybind11/test-cpp2pybind11.py +++ b/unittest/python/pybind11/test-cpp2pybind11.py @@ -1,6 +1,7 @@ -import cpp2pybind11 -import sys import gc +import sys + +import cpp2pybind11 import pinocchio a = pinocchio.SE3.Random() diff --git a/unittest/python/robot_wrapper.py b/unittest/python/robot_wrapper.py index 09afab9369..3853d810bc 100644 --- a/unittest/python/robot_wrapper.py +++ b/unittest/python/robot_wrapper.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -# -*- coding: utf-8 -*- # # SPDX-License-Identifier: BSD-2-Clause diff --git a/unittest/python/rpy.py b/unittest/python/rpy.py index e05ba9926d..c3e3dd08b4 100644 --- a/unittest/python/rpy.py +++ b/unittest/python/rpy.py @@ -1,22 +1,20 @@ import unittest from math import pi - -import numpy as np -from numpy.linalg import inv from random import random -from eigenpy import AngleAxis +import numpy as np import pinocchio as pin -from pinocchio.utils import npToTuple +from eigenpy import AngleAxis +from numpy.linalg import inv from pinocchio.rpy import ( - matrixToRpy, - rpyToMatrix, - rotate, computeRpyJacobian, computeRpyJacobianInverse, computeRpyJacobianTimeDerivative, + matrixToRpy, + rotate, + rpyToMatrix, ) - +from pinocchio.utils import npToTuple from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/serialization.py b/unittest/python/serialization.py index ed7d68d9f9..66c7d37b34 100644 --- a/unittest/python/serialization.py +++ b/unittest/python/serialization.py @@ -1,7 +1,6 @@ import unittest import pinocchio as pin - from test_case import PinocchioTestCase as TestCase main_path = "./serialization-data" diff --git a/unittest/python/test_case.py b/unittest/python/test_case.py index 57fc3ffdb1..c6e08459d5 100644 --- a/unittest/python/test_case.py +++ b/unittest/python/test_case.py @@ -12,5 +12,5 @@ class PinocchioTestCase(unittest.TestCase): def assertApprox(self, a, b, eps=1e-6): return self.assertTrue( isapprox(a, b, eps), - "\n%s\nis not approximately equal to\n%s\nwith precision %f" % (a, b, eps), + f"\n{a}\nis not approximately equal to\n{b}\nwith precision {eps:f}", ) diff --git a/unittest/python/utils.py b/unittest/python/utils.py index 9c3f34001d..ae34ee8cf6 100644 --- a/unittest/python/utils.py +++ b/unittest/python/utils.py @@ -4,7 +4,6 @@ import numpy as np import pinocchio as pin from pinocchio.utils import isapprox - from test_case import PinocchioTestCase as TestCase diff --git a/unittest/python/version.py b/unittest/python/version.py index 1921a6f47c..d09c9a3bcb 100644 --- a/unittest/python/version.py +++ b/unittest/python/version.py @@ -1,8 +1,6 @@ -from __future__ import print_function - import unittest -import pinocchio as pin +import pinocchio as pin from test_case import PinocchioTestCase as TestCase From dc2257eb4b32a76fdb81599617d3115a18f3e1b9 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 19 Sep 2024 18:59:55 +0200 Subject: [PATCH 02/11] python/viewers: fix abstract class error --- .../pinocchio/visualize/rviz_visualizer.py | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/bindings/python/pinocchio/visualize/rviz_visualizer.py b/bindings/python/pinocchio/visualize/rviz_visualizer.py index ab71345bc1..360f3562f8 100644 --- a/bindings/python/pinocchio/visualize/rviz_visualizer.py +++ b/bindings/python/pinocchio/visualize/rviz_visualizer.py @@ -1,4 +1,5 @@ import warnings +import numpy as np from .. import pinocchio_pywrap_default as pin from ..utils import npToTuple @@ -305,5 +306,32 @@ def sleep(self, dt): QTest.qWait(1e3 * dt) + def setBackgroundColor(self): + raise NotImplementedError() + + def setCameraTarget(self, target): + raise NotImplementedError() + + def setCameraPosition(self, position: np.ndarray): + raise NotImplementedError() + + def setCameraZoom(self, zoom: float): + raise NotImplementedError() + + def setCameraPose(self, pose: np.ndarray): + raise NotImplementedError() + + def captureImage(self, w=None, h=None): + raise NotImplementedError() + + def disableCameraControl(self): + raise NotImplementedError() + + def enableCameraControl(self): + raise NotImplementedError() + + def drawFrameVelocities(self, *args, **kwargs): + raise NotImplementedError() + __all__ = ["RVizVisualizer"] From 30abd0213555d62f202bb735fc191eb0f8dca628 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 19 Sep 2024 19:01:37 +0200 Subject: [PATCH 03/11] changelog: update --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index ff13e0681f..58cb1f7f91 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400)) - Fix mjcf parser appending of inertias at root joint ([#2403](https://github.com/stack-of-tasks/pinocchio/pull/2403)) - Fix unit tests with GCC 13.3 ([#2406](https://github.com/stack-of-tasks/pinocchio/pull/2416) +- Fix class abtract error for Rviz viewer ([#2425](https://github.com/stack-of-tasks/pinocchio/pull/2425)) ## [3.2.0] - 2024-08-27 From 4c2513c0375235df4ae9e7fa6a649c85f6974cb7 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 20 Sep 2024 12:09:53 +0200 Subject: [PATCH 04/11] github/workflow: remove accelerate support for build_all --- .github/workflows/scripts/get_label.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/scripts/get_label.js b/.github/workflows/scripts/get_label.js index 2cf0e242a0..0cb9f86187 100644 --- a/.github/workflows/scripts/get_label.js +++ b/.github/workflows/scripts/get_label.js @@ -34,7 +34,7 @@ module.exports = async ({github, context, core}) => { ' -DBUILD_PYTHON_BINDINGS_WITH_BOOST_MPFR_SUPPORT=ON', ' -DINSTALL_DOCUMENTATION=ON', ' -DGENERATE_PYTHON_STUBS=ON', - ' -DBUILD_WITH_ACCELERATE_SUPPORT=ON' + ' -DBUILD_WITH_ACCELERATE_SUPPORT=OFF' ], build_collision: ' -DBUILD_WITH_COLLISION_SUPPORT=ON', build_casadi: ' -DBUILD_WITH_CASADI_SUPPORT=ON', From 9fdcfe4ac678cfe00d314cf8b8921e102f129a62 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 16:46:42 +0200 Subject: [PATCH 05/11] examples: add inverse-kinematics for translation error only --- examples/CMakeLists.txt | 8 ++-- examples/inverse-kinematics-3d.cpp | 68 ++++++++++++++++++++++++++++++ examples/inverse-kinematics-3d.py | 47 +++++++++++++++++++++ 3 files changed, 120 insertions(+), 3 deletions(-) create mode 100644 examples/inverse-kinematics-3d.cpp create mode 100644 examples/inverse-kinematics-3d.py diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index dc1629b45c..bbb2e5427a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (c) 2015-2022 CNRS INRIA +# Copyright (c) 2015-2024 CNRS INRIA # # Compute flags outside the macro to avoid recomputing it for each tests @@ -46,6 +46,7 @@ function(ADD_PINOCCHIO_CPP_EXAMPLE EXAMPLE) endfunction() add_pinocchio_cpp_example(inverse-kinematics) +add_pinocchio_cpp_example(inverse-kinematics-3d) add_pinocchio_cpp_example(overview-simple) add_pinocchio_cpp_example(overview-lie) add_pinocchio_cpp_example(overview-SE3) @@ -70,8 +71,9 @@ if(BUILD_WITH_COLLISION_SUPPORT) endif() if(BUILD_PYTHON_INTERFACE) - set(${PROJECT_NAME}_PYTHON_EXAMPLES inverse-kinematics overview-simple kinematics-derivatives - forward-dynamics-derivatives inverse-dynamics-derivatives) + set(${PROJECT_NAME}_PYTHON_EXAMPLES + inverse-kinematics inverse-kinematics-3d overview-simple kinematics-derivatives + forward-dynamics-derivatives inverse-dynamics-derivatives) if(BUILD_WITH_URDF_SUPPORT) list( diff --git a/examples/inverse-kinematics-3d.cpp b/examples/inverse-kinematics-3d.cpp new file mode 100644 index 0000000000..c1e19a8ab6 --- /dev/null +++ b/examples/inverse-kinematics-3d.cpp @@ -0,0 +1,68 @@ +#include + +#include "pinocchio/multibody/sample-models.hpp" +#include "pinocchio/spatial/explog.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" + +int main(int /* argc */, char ** /* argv */) +{ + pinocchio::Model model; + pinocchio::buildModels::manipulator(model); + pinocchio::Data data(model); + + const int JOINT_ID = 6; + const pinocchio::SE3 oMdes(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1., 0., 1.)); + + Eigen::VectorXd q = pinocchio::neutral(model); + const double eps = 1e-4; + const int IT_MAX = 1000; + const double DT = 1e-1; + const double damp = 1e-12; + + pinocchio::Data::Matrix6x joint_jacobian(6, model.nv); + joint_jacobian.setZero(); + + bool success = false; + Eigen::Vector3d err; + Eigen::VectorXd v(model.nv); + for (int i = 0;; i++) + { + pinocchio::forwardKinematics(model, data, q); + const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes); + err = iMd.translation(); // in joint frame + if (err.norm() < eps) + { + success = true; + break; + } + if (i >= IT_MAX) + { + success = false; + break; + } + pinocchio::computeJointJacobian( + model, data, q, JOINT_ID, joint_jacobian); // joint_jacobian expressed in the joint frame + const auto J = -joint_jacobian.topRows<3>(); // Jacobian associated with the error + const Eigen::Matrix3d JJt = J * J.transpose() + damp * Eigen::Matrix3d::Identity(); + v.noalias() = -J.transpose() * JJt.ldlt().solve(err); + q = pinocchio::integrate(model, q, v * DT); + if (!(i % 10)) + std::cout << i << ": error = " << err.transpose() << std::endl; + } + + if (success) + { + std::cout << "Convergence achieved!" << std::endl; + } + else + { + std::cout + << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" + << std::endl; + } + + std::cout << "\nresult: " << q.transpose() << std::endl; + std::cout << "\nfinal error: " << err.transpose() << std::endl; +} diff --git a/examples/inverse-kinematics-3d.py b/examples/inverse-kinematics-3d.py new file mode 100644 index 0000000000..17563e122e --- /dev/null +++ b/examples/inverse-kinematics-3d.py @@ -0,0 +1,47 @@ +from __future__ import print_function + +import numpy as np +from numpy.linalg import norm, solve + +import pinocchio + +model = pinocchio.buildSampleModelManipulator() +data = model.createData() + +JOINT_ID = 6 +oMdes = pinocchio.SE3(np.eye(3), np.array([1.0, 0.0, 1.0])) + +q = pinocchio.neutral(model) +eps = 1e-4 +IT_MAX = 1000 +DT = 1e-1 +damp = 1e-12 + +it = 0 +while True: + pinocchio.forwardKinematics(model, data, q) + iMd = data.oMi[JOINT_ID].actInv(oMdes) + err = iMd.translation + if norm(err) < eps: + success = True + break + if it >= IT_MAX: + success = False + break + J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID) # in joint frame + J = -J[:3, :] # linear part of the Jacobian + v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(3), err)) + q = pinocchio.integrate(model, q, v * DT) + if not it % 10: + print("%d: error = %s" % (it, err.T)) + it += 1 + +if success: + print("Convergence achieved!") +else: + print( + "\nWarning: the iterative algorithm has not reached convergence to the desired precision" + ) + +print("\nresult: %s" % q.flatten().tolist()) +print("\nfinal error: %s" % err.T) From c0284f39dfed0a8701dd88e438542025cd59e49c Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 16:58:02 +0200 Subject: [PATCH 06/11] changelog: sync --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 58cb1f7f91..7299be685e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Added - Default visualizer can be changed with `PINOCCHIO_VIEWER` environment variable ([#2419](https://github.com/stack-of-tasks/pinocchio/pull/2419)) +- Add more Python and C++ examples related to inverse kinematics with 3d tasks ([#2428](https://github.com/stack-of-tasks/pinocchio/pull/2428)) ### Fixed - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400)) From 066499126597f5951f9bc39b31fa15cc396b215c Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 16:58:54 +0200 Subject: [PATCH 07/11] cmake: sync submodule --- cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake b/cmake index b3c2af1b68..31e46019be 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit b3c2af1b68686dc9d5f459fb617647e37a15a76d +Subproject commit 31e46019beda968ba9e516ad645a951c64256eed From 14bbfe42f3f1ad14331ae21f52210cd9325c271d Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 20:47:33 +0200 Subject: [PATCH 08/11] ruff: fix --- .../python/pinocchio/visualize/rviz_visualizer.py | 1 + examples/inverse-kinematics-3d.py | 12 +++++------- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/bindings/python/pinocchio/visualize/rviz_visualizer.py b/bindings/python/pinocchio/visualize/rviz_visualizer.py index fcbcb9eb30..0feb4692e8 100644 --- a/bindings/python/pinocchio/visualize/rviz_visualizer.py +++ b/bindings/python/pinocchio/visualize/rviz_visualizer.py @@ -1,4 +1,5 @@ import warnings + import numpy as np from .. import pinocchio_pywrap_default as pin diff --git a/examples/inverse-kinematics-3d.py b/examples/inverse-kinematics-3d.py index 17563e122e..1b4dd256bf 100644 --- a/examples/inverse-kinematics-3d.py +++ b/examples/inverse-kinematics-3d.py @@ -1,9 +1,6 @@ -from __future__ import print_function - import numpy as np -from numpy.linalg import norm, solve - import pinocchio +from numpy.linalg import norm, solve model = pinocchio.buildSampleModelManipulator() data = model.createData() @@ -40,8 +37,9 @@ print("Convergence achieved!") else: print( - "\nWarning: the iterative algorithm has not reached convergence to the desired precision" + "\nWarning: the iterative algorithm has not reached convergence to " + "the desired precision" ) -print("\nresult: %s" % q.flatten().tolist()) -print("\nfinal error: %s" % err.T) +print(f"\nresult: {q.flatten().tolist()}") +print(f"\nfinal error: {err.T}") From ac9a540a871c504d572a3dca2213c5702be4e060 Mon Sep 17 00:00:00 2001 From: Cfather Date: Tue, 17 Sep 2024 13:27:08 +0000 Subject: [PATCH 09/11] add passivity rnea algorithm and the tests --- include/pinocchio/algorithm/rnea.hpp | 37 +++++ include/pinocchio/algorithm/rnea.hxx | 173 +++++++++++++++++++++++ include/pinocchio/algorithm/rnea.txx | 15 ++ include/pinocchio/multibody/data.hpp | 3 + include/pinocchio/multibody/data.hxx | 3 +- include/pinocchio/serialization/data.hpp | 1 + src/algorithm/rnea.cpp | 15 ++ unittest/rnea.cpp | 53 +++++++ 8 files changed, 299 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/rnea.hpp b/include/pinocchio/algorithm/rnea.hpp index 3650508b58..55249f5dca 100644 --- a/include/pinocchio/algorithm/rnea.hpp +++ b/include/pinocchio/algorithm/rnea.hpp @@ -80,6 +80,43 @@ namespace pinocchio const Eigen::MatrixBase & a, const container::aligned_vector & fext); + /// + /// \brief The passivity-based Recursive Newton-Euler algorithm. It computes a modified inverse dynamics, aka the joint + /// torques according to the current state of the system and the auxiliary joint velocities and accelerations. + /// To be more specific, it computes H(q) * a_r + C(q, v) * v_r + g(q) + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType1 Type of the joint velocity vector. + /// \tparam TangentVectorType2 Type of the auxiliary joint velocity vector. + /// \tparam TangentVectorType3 Type of the auxiliary joint acceleration vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] q The joint configuration vector (dim model.nq). + /// \param[in] v The joint velocity vector (dim model.nv). + /// \param[in] v_r The auxiliary joint velocity vector (dim model.nv). + /// \param[in] a_r The auxiliary joint acceleration vector (dim model.nv). + /// + /// \return The desired joint torques stored in data.tau. + /// + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r); + /// /// \brief Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), /// also called the bias terms \f$ b(q,\dot{q}) \f$ of the Lagrangian dynamics:
\f$ diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index 4d1b775a87..d75ddb6fff 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -215,6 +215,159 @@ namespace pinocchio return data.tau; } + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + struct PassivityRneaForwardStep + : public fusion::JointUnaryVisitorBase> + { + typedef ModelTpl Model; + typedef DataTpl Data; + typedef ForceTpl Force; + + typedef boost::fusion::vector< + const Model &, + Data &, + const ConfigVectorType &, + const TangentVectorType1 &, + const TangentVectorType2 &, + const TangentVectorType3 &> + ArgsType; + + template + static void algo( + const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r) + { + typedef typename Model::JointIndex JointIndex; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + + jmodel.calc(jdata.derived(), q.derived(), v.derived()); + data.v[i] = jdata.v(); + + jmodel.calc(jdata.derived(), q.derived(), v_r.derived()); + data.v_r[i] = jdata.v(); + + data.liMi[i] = model.jointPlacements[i] * jdata.M(); + + if (parent > 0) { + data.v[i] += data.liMi[i].actInv(data.v[parent]); + data.v_r[i] += data.liMi[i].actInv(data.v_r[parent]); + } + + data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); + data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a_r); + data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); + // + // data.f[i] = model.inertias[i]*data.a_gf[i];// + model.inertias[i].vxiv(data.v[i]); + // // -f_ext data.h[i] = model.inertias[i]*data.v[i]; + + // model.inertias[i].__mult__(data.v_r[i], data.h[i]); // option 1 + data.B[i] = model.inertias[i].variation(Scalar(0.5) * data.v[i]); + model.inertias[i].__mult__(data.v[i], data.h[i]); + addForceCrossMatrix(Scalar(0.5) * data.h[i], data.B[i]); // option 3 (Christoffel-consistent factorization) + + model.inertias[i].__mult__(data.a_gf[i], data.f[i]); + // data.f[i] += data.v[i].cross(data.h[i]); // option 1 + data.f[i] += Force(data.B[i] * data.v_r[i].toVector()); // option 3 (Christoffel-consistent factorization) + + // data.h[i].motionAction(data.v[i],data.f[i]); + // data.f[i] = model.inertias[i].vxiv(data.v[i]); + // data.f[i].setZero(); + } + + template + static void + addForceCrossMatrix(const ForceDense & f, const Eigen::MatrixBase & mout) + { + M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); + addSkew( + -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR)); + addSkew( + -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR)); + addSkew( + -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR)); + } + }; + + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r) + { + assert(model.check(data) && "data is not consistent with model."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + q.size(), model.nq, "The configuration vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + v.size(), model.nv, "The velocity vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + v_r.size(), model.nv, "The auxiliary velocity vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + a_r.size(), model.nv, "The auxiliary acceleration vector is not of right size"); + + typedef ModelTpl Model; + typedef typename Model::JointIndex JointIndex; + + data.v[0].setZero(); + data.v_r[0].setZero(); + data.a_gf[0] = -model.gravity; + + typedef PassivityRneaForwardStep< + Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, + TangentVectorType2, TangentVectorType3> + Pass1; + typename Pass1::ArgsType arg1(model, data, q.derived(), v.derived(), v_r.derived(), a_r.derived()); + for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) + { + Pass1::run(model.joints[i], data.joints[i], arg1); + } + + typedef RneaBackwardStep Pass2; + typename Pass2::ArgsType arg2(model, data); + for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) + { + Pass2::run(model.joints[i], data.joints[i], arg2); + } + + // Add rotorinertia contribution + data.tau.array() += model.armature.array() * a_r.array(); // Check if there is memory allocation + + return data.tau; + } + template< typename Scalar, int Options, @@ -786,6 +939,26 @@ namespace pinocchio return impl::rnea(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext); } + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r) + { + return impl::passivityRNEA(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(v_r), make_const_ref(a_r)); + } + template< typename Scalar, int Options, diff --git a/include/pinocchio/algorithm/rnea.txx b/include/pinocchio/algorithm/rnea.txx index 09331477f4..c14e122ecf 100644 --- a/include/pinocchio/algorithm/rnea.txx +++ b/include/pinocchio/algorithm/rnea.txx @@ -39,6 +39,21 @@ namespace pinocchio const Eigen::MatrixBase> &, const container::aligned_vector &); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI const context::VectorXs & + passivityRNEA< + context::Scalar, + context::Options, + JointCollectionDefaultTpl, + Eigen::Ref, + Eigen::Ref, + Eigen::Ref>( + const context::Model &, + context::Data &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI const context::VectorXs & nonLinearEffects< context::Scalar, diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index d19468315c..4f145b0b6f 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/include/pinocchio/multibody/data.hpp @@ -138,6 +138,9 @@ namespace pinocchio /// \brief Vector of joint velocities expressed in the local frame of the joint. PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v; + /// \brief Vector of auxiliary joint velocities expressed in the local frame of the joint. + PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v_r; + /// \brief Vector of joint velocities expressed at the origin of the world. PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov; diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index c0aed6741c..281c3ed842 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -26,6 +26,7 @@ namespace pinocchio , a_gf((std::size_t)model.njoints, Motion::Zero()) , oa_gf((std::size_t)model.njoints, Motion::Zero()) , v((std::size_t)model.njoints, Motion::Zero()) + , v_r((std::size_t)model.njoints, Motion::Zero()) , ov((std::size_t)model.njoints, Motion::Zero()) , f((std::size_t)model.njoints, Force::Zero()) , of((std::size_t)model.njoints, Force::Zero()) @@ -276,7 +277,7 @@ namespace pinocchio bool value = data1.joints == data2.joints && data1.a == data2.a && data1.oa == data2.oa && data1.oa_drift == data2.oa_drift && data1.oa_augmented == data2.oa_augmented - && data1.a_gf == data2.a_gf && data1.oa_gf == data2.oa_gf && data1.v == data2.v + && data1.a_gf == data2.a_gf && data1.oa_gf == data2.oa_gf && data1.v == data2.v && data1.v_r == data2.v_r && data1.ov == data2.ov && data1.f == data2.f && data1.of == data2.of && data1.of_augmented == data2.of_augmented && data1.h == data2.h && data1.oh == data2.oh && data1.oMi == data2.oMi && data1.liMi == data2.liMi && data1.tau == data2.tau diff --git a/include/pinocchio/serialization/data.hpp b/include/pinocchio/serialization/data.hpp index 6772dd8c8a..95fadf3f7f 100644 --- a/include/pinocchio/serialization/data.hpp +++ b/include/pinocchio/serialization/data.hpp @@ -39,6 +39,7 @@ namespace boost PINOCCHIO_MAKE_DATA_NVP(ar, data, a_gf); PINOCCHIO_MAKE_DATA_NVP(ar, data, oa_gf); PINOCCHIO_MAKE_DATA_NVP(ar, data, v); + PINOCCHIO_MAKE_DATA_NVP(ar, data, v_r); PINOCCHIO_MAKE_DATA_NVP(ar, data, ov); PINOCCHIO_MAKE_DATA_NVP(ar, data, f); PINOCCHIO_MAKE_DATA_NVP(ar, data, of); diff --git a/src/algorithm/rnea.cpp b/src/algorithm/rnea.cpp index 07c27057e1..e4211eb60a 100644 --- a/src/algorithm/rnea.cpp +++ b/src/algorithm/rnea.cpp @@ -36,6 +36,21 @@ namespace pinocchio const Eigen::MatrixBase> &, const container::aligned_vector &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & passivityRNEA< + context::Scalar, + context::Options, + JointCollectionDefaultTpl, + Eigen::Ref, + Eigen::Ref, + Eigen::Ref, + Eigen::Ref>( + const context::Model &, + context::Data &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & nonLinearEffects< context::Scalar, diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index 51b7842c63..60c562165f 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -344,4 +344,57 @@ BOOST_AUTO_TEST_CASE(test_compute_coriolis) } } +BOOST_AUTO_TEST_CASE(test_passivityrnea_vs_rnea) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + buildModels::humanoidRandom(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + + pinocchio::Data data_passivityrnea(model); + pinocchio::Data data_rnea(model); + + VectorXd q = randomConfiguration(model); + VectorXd v = VectorXd::Random(model.nv); + VectorXd a = VectorXd::Random(model.nv); + + VectorXd tau_passivityrnea = passivityRNEA(model, data_passivityrnea, q, v, v, a); + VectorXd tau_rnea = rnea(model, data_rnea, q, v, a); + + BOOST_CHECK(tau_passivityrnea.isApprox(tau_rnea, 1e-12)); +} + +BOOST_AUTO_TEST_CASE(test_passivityrnea_compute_coriolis) +{ + using namespace Eigen; + using namespace pinocchio; + + const double prec = Eigen::NumTraits::dummy_precision(); + + Model model; + buildModels::humanoidRandom(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + model.gravity.setZero(); + + Data data_ref(model); + Data data(model); + + VectorXd q = randomConfiguration(model); + + VectorXd v(VectorXd::Random(model.nv)); + computeCoriolisMatrix(model, data, q, v); + + VectorXd v_r(VectorXd::Random(model.nv)); + passivityRNEA(model, data_ref, q, v, v_r, VectorXd::Zero(model.nv)); + + VectorXd tau = data.C * v_r; + BOOST_CHECK(tau.isApprox(data_ref.tau, prec)); +} + BOOST_AUTO_TEST_SUITE_END() From 010d6c854de96057e585af2f2178c775e7550c67 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 18 Sep 2024 14:40:11 +0200 Subject: [PATCH 10/11] algo/rnea: remove useless lines in pasitivity RNEA --- include/pinocchio/algorithm/rnea.hxx | 7 ------- 1 file changed, 7 deletions(-) diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index d75ddb6fff..7a83fd2757 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -279,9 +279,6 @@ namespace pinocchio data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a_r); data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); - // - // data.f[i] = model.inertias[i]*data.a_gf[i];// + model.inertias[i].vxiv(data.v[i]); - // // -f_ext data.h[i] = model.inertias[i]*data.v[i]; // model.inertias[i].__mult__(data.v_r[i], data.h[i]); // option 1 data.B[i] = model.inertias[i].variation(Scalar(0.5) * data.v[i]); @@ -291,10 +288,6 @@ namespace pinocchio model.inertias[i].__mult__(data.a_gf[i], data.f[i]); // data.f[i] += data.v[i].cross(data.h[i]); // option 1 data.f[i] += Force(data.B[i] * data.v_r[i].toVector()); // option 3 (Christoffel-consistent factorization) - - // data.h[i].motionAction(data.v[i],data.f[i]); - // data.f[i] = model.inertias[i].vxiv(data.v[i]); - // data.f[i].setZero(); } template From 252effb8d818e3486ce342d86acbe92c90fffaf3 Mon Sep 17 00:00:00 2001 From: Cfather Date: Wed, 18 Sep 2024 16:13:12 +0000 Subject: [PATCH 11/11] add option 2 --- include/pinocchio/algorithm/rnea.hpp | 74 +++--- include/pinocchio/algorithm/rnea.hxx | 344 ++++++++++++++------------- src/algorithm/rnea.cpp | 30 +-- 3 files changed, 230 insertions(+), 218 deletions(-) diff --git a/include/pinocchio/algorithm/rnea.hpp b/include/pinocchio/algorithm/rnea.hpp index 55249f5dca..8c4aef13a1 100644 --- a/include/pinocchio/algorithm/rnea.hpp +++ b/include/pinocchio/algorithm/rnea.hpp @@ -80,43 +80,6 @@ namespace pinocchio const Eigen::MatrixBase & a, const container::aligned_vector & fext); - /// - /// \brief The passivity-based Recursive Newton-Euler algorithm. It computes a modified inverse dynamics, aka the joint - /// torques according to the current state of the system and the auxiliary joint velocities and accelerations. - /// To be more specific, it computes H(q) * a_r + C(q, v) * v_r + g(q) - /// - /// \tparam JointCollection Collection of Joint types. - /// \tparam ConfigVectorType Type of the joint configuration vector. - /// \tparam TangentVectorType1 Type of the joint velocity vector. - /// \tparam TangentVectorType2 Type of the auxiliary joint velocity vector. - /// \tparam TangentVectorType3 Type of the auxiliary joint acceleration vector. - /// - /// \param[in] model The model structure of the rigid body system. - /// \param[in] data The data structure of the rigid body system. - /// \param[in] q The joint configuration vector (dim model.nq). - /// \param[in] v The joint velocity vector (dim model.nv). - /// \param[in] v_r The auxiliary joint velocity vector (dim model.nv). - /// \param[in] a_r The auxiliary joint acceleration vector (dim model.nv). - /// - /// \return The desired joint torques stored in data.tau. - /// - template< - typename Scalar, - int Options, - template - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename TangentVectorType3> - const typename DataTpl::TangentVectorType & passivityRNEA( - const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & v_r, - const Eigen::MatrixBase & a_r); - /// /// \brief Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), /// also called the bias terms \f$ b(q,\dot{q}) \f$ of the Lagrangian dynamics:
\f$ @@ -258,6 +221,43 @@ namespace pinocchio const ModelTpl & model, DataTpl & data); + /// + /// \brief The passivity-based Recursive Newton-Euler algorithm. It computes a modified inverse dynamics, aka the joint + /// torques according to the current state of the system and the auxiliary joint velocities and accelerations. + /// To be more specific, it computes H(q) * a_r + C(q, v) * v_r + g(q) + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType1 Type of the joint velocity vector. + /// \tparam TangentVectorType2 Type of the auxiliary joint velocity vector. + /// \tparam TangentVectorType3 Type of the auxiliary joint acceleration vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] q The joint configuration vector (dim model.nq). + /// \param[in] v The joint velocity vector (dim model.nv). + /// \param[in] v_r The auxiliary joint velocity vector (dim model.nv). + /// \param[in] a_r The auxiliary joint acceleration vector (dim model.nv). + /// + /// \return The desired joint torques stored in data.tau. + /// + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r); + } // namespace pinocchio /* --- Details -------------------------------------------------------------------- */ diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index 7a83fd2757..9413970d78 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -215,152 +215,6 @@ namespace pinocchio return data.tau; } - template< - typename Scalar, - int Options, - template - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename TangentVectorType3> - struct PassivityRneaForwardStep - : public fusion::JointUnaryVisitorBase> - { - typedef ModelTpl Model; - typedef DataTpl Data; - typedef ForceTpl Force; - - typedef boost::fusion::vector< - const Model &, - Data &, - const ConfigVectorType &, - const TangentVectorType1 &, - const TangentVectorType2 &, - const TangentVectorType3 &> - ArgsType; - - template - static void algo( - const JointModelBase & jmodel, - JointDataBase & jdata, - const Model & model, - Data & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & v_r, - const Eigen::MatrixBase & a_r) - { - typedef typename Model::JointIndex JointIndex; - - const JointIndex i = jmodel.id(); - const JointIndex parent = model.parents[i]; - - jmodel.calc(jdata.derived(), q.derived(), v.derived()); - data.v[i] = jdata.v(); - - jmodel.calc(jdata.derived(), q.derived(), v_r.derived()); - data.v_r[i] = jdata.v(); - - data.liMi[i] = model.jointPlacements[i] * jdata.M(); - - if (parent > 0) { - data.v[i] += data.liMi[i].actInv(data.v[parent]); - data.v_r[i] += data.liMi[i].actInv(data.v_r[parent]); - } - - data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); - data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a_r); - data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); - - // model.inertias[i].__mult__(data.v_r[i], data.h[i]); // option 1 - data.B[i] = model.inertias[i].variation(Scalar(0.5) * data.v[i]); - model.inertias[i].__mult__(data.v[i], data.h[i]); - addForceCrossMatrix(Scalar(0.5) * data.h[i], data.B[i]); // option 3 (Christoffel-consistent factorization) - - model.inertias[i].__mult__(data.a_gf[i], data.f[i]); - // data.f[i] += data.v[i].cross(data.h[i]); // option 1 - data.f[i] += Force(data.B[i] * data.v_r[i].toVector()); // option 3 (Christoffel-consistent factorization) - } - - template - static void - addForceCrossMatrix(const ForceDense & f, const Eigen::MatrixBase & mout) - { - M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); - addSkew( - -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR)); - addSkew( - -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR)); - addSkew( - -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR)); - } - }; - - template< - typename Scalar, - int Options, - template - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename TangentVectorType3> - const typename DataTpl::TangentVectorType & passivityRNEA( - const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & v_r, - const Eigen::MatrixBase & a_r) - { - assert(model.check(data) && "data is not consistent with model."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - q.size(), model.nq, "The configuration vector is not of right size"); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - v.size(), model.nv, "The velocity vector is not of right size"); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - v_r.size(), model.nv, "The auxiliary velocity vector is not of right size"); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - a_r.size(), model.nv, "The auxiliary acceleration vector is not of right size"); - - typedef ModelTpl Model; - typedef typename Model::JointIndex JointIndex; - - data.v[0].setZero(); - data.v_r[0].setZero(); - data.a_gf[0] = -model.gravity; - - typedef PassivityRneaForwardStep< - Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, - TangentVectorType2, TangentVectorType3> - Pass1; - typename Pass1::ArgsType arg1(model, data, q.derived(), v.derived(), v_r.derived(), a_r.derived()); - for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) - { - Pass1::run(model.joints[i], data.joints[i], arg1); - } - - typedef RneaBackwardStep Pass2; - typename Pass2::ArgsType arg2(model, data); - for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) - { - Pass2::run(model.joints[i], data.joints[i], arg2); - } - - // Add rotorinertia contribution - data.tau.array() += model.armature.array() * a_r.array(); // Check if there is memory allocation - - return data.tau; - } - template< typename Scalar, int Options, @@ -805,6 +659,164 @@ namespace pinocchio return data.C; } + + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + struct PassivityRneaForwardStep + : public fusion::JointUnaryVisitorBase> + { + typedef ModelTpl Model; + typedef DataTpl Data; + typedef ForceTpl Force; + + typedef boost::fusion::vector< + const Model &, + Data &, + const ConfigVectorType &, + const TangentVectorType1 &, + const TangentVectorType2 &, + const TangentVectorType3 &> + ArgsType; + + typedef impl::CoriolisMatrixForwardStep< + Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1> + CoriolisPass1; + + template + static void algo( + const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r) + { + typedef typename Model::JointIndex JointIndex; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + + jmodel.calc(jdata.derived(), q.derived(), v.derived()); + data.v[i] = jdata.v(); + + jmodel.calc(jdata.derived(), q.derived(), v_r.derived()); + data.v_r[i] = jdata.v(); + + data.liMi[i] = model.jointPlacements[i] * jdata.M(); + + if (parent > 0) { + data.v[i] += data.liMi[i].actInv(data.v[parent]); + data.v_r[i] += data.liMi[i].actInv(data.v_r[parent]); + } + + data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); + data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a_r); + data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); + + // // option 1 + // model.inertias[i].__mult__(data.v_r[i], data.h[i]); + // // option 1 + + // // option 2 + // data.B[i].setZero(); + // model.inertias[i].__mult__(data.v[i], data.h[i]); + // CoriolisPass1::addForceCrossMatrix(data.h[i], data.B[i]); + // // option 2 + + // option 3 (Christoffel-consistent factorization) + data.B[i] = model.inertias[i].variation(Scalar(0.5) * data.v[i]); + model.inertias[i].__mult__(data.v[i], data.h[i]); + CoriolisPass1::addForceCrossMatrix(Scalar(0.5) * data.h[i], data.B[i]); + // option 3 (Christoffel-consistent factorization) + + model.inertias[i].__mult__(data.a_gf[i], data.f[i]); + + // // option 1 + // data.f[i] += data.v[i].cross(data.h[i]); + // // option 1 + + // // option 2 + // data.f[i] += Force(data.B[i] * data.v_r[i].toVector()); + // // option 2 + + // option 3 (Christoffel-consistent factorization) + data.f[i] += Force(data.B[i] * data.v_r[i].toVector()); + // option 3 (Christoffel-consistent factorization) + } + }; + + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r) + { + assert(model.check(data) && "data is not consistent with model."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + q.size(), model.nq, "The configuration vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + v.size(), model.nv, "The velocity vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + v_r.size(), model.nv, "The auxiliary velocity vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + a_r.size(), model.nv, "The auxiliary acceleration vector is not of right size"); + + typedef ModelTpl Model; + typedef typename Model::JointIndex JointIndex; + + data.v[0].setZero(); + data.v_r[0].setZero(); + data.a_gf[0] = -model.gravity; + + typedef PassivityRneaForwardStep< + Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, + TangentVectorType2, TangentVectorType3> + Pass1; + typename Pass1::ArgsType arg1(model, data, q.derived(), v.derived(), v_r.derived(), a_r.derived()); + for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) + { + Pass1::run(model.joints[i], data.joints[i], arg1); + } + + typedef RneaBackwardStep Pass2; + typename Pass2::ArgsType arg2(model, data); + for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) + { + Pass2::run(model.joints[i], data.joints[i], arg2); + } + + // Add rotorinertia contribution + data.tau.array() += model.armature.array() * a_r.array(); // Check if there is memory allocation + + return data.tau; + } } // namespace impl template class JointCollectionTpl> struct GetCoriolisMatrixBackwardStep @@ -932,26 +944,6 @@ namespace pinocchio return impl::rnea(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext); } - template< - typename Scalar, - int Options, - template - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename TangentVectorType3> - const typename DataTpl::TangentVectorType & passivityRNEA( - const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & v_r, - const Eigen::MatrixBase & a_r) - { - return impl::passivityRNEA(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(v_r), make_const_ref(a_r)); - } - template< typename Scalar, int Options, @@ -1014,6 +1006,26 @@ namespace pinocchio { return impl::computeCoriolisMatrix(model, data, make_const_ref(q), make_const_ref(v)); } + + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r) + { + return impl::passivityRNEA(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(v_r), make_const_ref(a_r)); + } } // namespace pinocchio /// @endcond diff --git a/src/algorithm/rnea.cpp b/src/algorithm/rnea.cpp index e4211eb60a..6ff35bd9ea 100644 --- a/src/algorithm/rnea.cpp +++ b/src/algorithm/rnea.cpp @@ -36,21 +36,6 @@ namespace pinocchio const Eigen::MatrixBase> &, const container::aligned_vector &); - template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & passivityRNEA< - context::Scalar, - context::Options, - JointCollectionDefaultTpl, - Eigen::Ref, - Eigen::Ref, - Eigen::Ref, - Eigen::Ref>( - const context::Model &, - context::Data &, - const Eigen::MatrixBase> &, - const Eigen::MatrixBase> &, - const Eigen::MatrixBase> &, - const Eigen::MatrixBase> &); - template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & nonLinearEffects< context::Scalar, @@ -95,6 +80,21 @@ namespace pinocchio context::Data &, const Eigen::MatrixBase> &, const Eigen::MatrixBase> &); + + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & passivityRNEA< + context::Scalar, + context::Options, + JointCollectionDefaultTpl, + Eigen::Ref, + Eigen::Ref, + Eigen::Ref, + Eigen::Ref>( + const context::Model &, + context::Data &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &); } // namespace impl template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::MatrixXs & getCoriolisMatrix(