Merge pull request #105 from open-dynamic-robot-initiative/fkloss/rob…

…ot_properties_fingers

Remove mirrored model files, depend on robot_properties_fingers

.github/workflows/lint.yml

@@ -10,15 +10,12 @@ jobs:
       - uses: actions/setup-python@v4
         with:
           python-version: '3.10'
-      - name: Install mypy
-        run: |
-          python3 -m pip install mypy types-PyYAML types-setuptools
       - name: Add matcher
         run: |
           echo "::add-matcher::.github/workflows/mypy-problem-matcher.json"
       - name: Install package
         run: |
-          pip install .
+          pip install ".[mypy]"
       - name: Run mypy
         run: |
           python3 -m mypy --exclude=build .

CHANGELOG

@@ -23,6 +23,11 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Fixed included URDF files of (Tri)FingerPro (see [robot_properties_fingers #15
   ](https://github.com/open-dynamic-robot-initiative/robot_properties_fingers/pull/15)).
 
+### Removed
+- trifinger_simulation does no longer maintain a copy of the robot model files for the
+  pure-Python installation.  Instead it depends on robot_properties_fingers now for both
+  installation types (colcon and pure Python).
+
 
 ## 1.4.1 - 2022-06-24
 

setup.cfg

@@ -36,6 +36,7 @@ install_requires =
     gym >=0.23.1
     opencv-python >=4.2.0.34
     pyyaml >=5.3.1
+    robot_properties_fingers>=2.0.2
 
 scripts =
     demos/demo_cameras.py
@@ -50,7 +51,11 @@ scripts =
 
 [options.extras_require]
 test = pytest
-doc = 
+mypy =
+    mypy
+    types-PyYAML
+    types-setuptools
+doc =
     sphinx
     sphinx_rtd_theme
 
@@ -63,8 +68,6 @@ trifinger_simulation =
     data/cube_v2/*.png
     data/cube_v2/*.mtl
     data/cube_v2/*.obj
-    robot_properties_fingers/**/*.stl
-    robot_properties_fingers/**/*.urdf
 
 [options.data_files]
 share/ament_index/resource_index/packages = resource/trifinger_simulation

trifinger_simulation/pinocchio_utils.py

@@ -1,10 +1,8 @@
 """Wrappers around Pinocchio for easy forward and inverse kinematics."""
 
-import typing
 import warnings
 
-import numpy as np
-import pinocchio
+from robot_properties_fingers import Kinematics
 
 
 warnings.warn(
@@ -15,169 +13,4 @@
     stacklevel=1,
 )
 
-
-# === IMPORTANT: This class is deprecated!  See warning above.
-# The implementation here is kept for now, as robot_properties_fingers is not yet
-# pip-installable but should be removed as soon as possible.
-# Please do not make any changes here anymore but instead add new features/fixes/etc. in
-# robot_properties_fingers.
-
-
-class Kinematics:
-    """Forward and inverse kinematics for arbitrary Finger robots.
-
-    Provides forward and inverse kinematics functions for a Finger robot with
-    arbitrarily many independent fingers.
-    """
-
-    def __init__(
-        self, finger_urdf_path: str, tip_link_names: typing.Iterable[str]
-    ):
-        """
-        Args:
-            finger_urdf_path:  Path to the URDF file describing the robot.
-            tip_link_names:  Names of the finger tip frames, one per finger.
-        """
-        self.robot_model = pinocchio.buildModelFromUrdf(finger_urdf_path)
-        self.data = self.robot_model.createData()
-        self.tip_link_ids = [
-            self.robot_model.getFrameId(link_name)
-            for link_name in tip_link_names
-        ]
-
-    def forward_kinematics(
-        self,
-        joint_positions: typing.List[np.ndarray],
-        joint_velocities: typing.Optional[np.ndarray] = None,
-    ) -> typing.Union[
-        typing.List[np.ndarray],
-        typing.Tuple[typing.List[np.ndarray], typing.List[np.ndarray]],
-    ]:
-        """Compute end-effector positions (and velocities) for the given joint configuration.
-
-        Args:
-            joint_positions:  Flat list of angular joint positions.
-            joint_velocities: Optional. Flat list of angular joint
-                velocities.
-
-        Returns:
-            If only joint positions are given: List of end-effector
-            positions. Each position is given as an np.array with x,y,z
-            positions.
-            If joint positions and velocities are given: Tuple with
-            (i) list of end-effector positions and (ii) list of
-            end-effector velocities. Each position and velocity is given
-            as an np.array with x,y,z components.
-        """
-        pinocchio.framesForwardKinematics(
-            self.robot_model, self.data, joint_positions
-        )
-        positions = [
-            np.asarray(self.data.oMf[link_id].translation).reshape(-1).tolist()
-            for link_id in self.tip_link_ids
-        ]
-        if joint_velocities is None:
-            return positions
-        else:
-            pinocchio.forwardKinematics(
-                self.robot_model, self.data, joint_positions, joint_velocities
-            )
-            velocities = []
-            for link_id in self.tip_link_ids:
-                local_to_world_transform = pinocchio.SE3.Identity()
-                local_to_world_transform.rotation = self.data.oMf[
-                    link_id
-                ].rotation
-                v_local = pinocchio.getFrameVelocity(
-                    self.robot_model, self.data, link_id
-                )
-                v_world = local_to_world_transform.act(v_local)
-                velocities.append(v_world.linear)
-            return positions, velocities
-
-    def _inverse_kinematics_step(
-        self, frame_id: int, xdes: np.ndarray, q0: np.ndarray
-    ) -> typing.Tuple[np.ndarray, np.ndarray]:
-        """Compute one IK iteration for a single finger."""
-        dt = 1.0e-1
-        pinocchio.computeJointJacobians(self.robot_model, self.data, q0)
-        pinocchio.framesForwardKinematics(self.robot_model, self.data, q0)
-        Ji = pinocchio.getFrameJacobian(
-            self.robot_model,
-            self.data,
-            frame_id,
-            pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED,
-        )[:3, :]
-        xcurrent = self.data.oMf[frame_id].translation
-        try:
-            Jinv = np.linalg.inv(Ji)
-        except Exception:
-            Jinv = np.linalg.pinv(Ji)
-        err = xdes - xcurrent
-        dq = Jinv.dot(xdes - xcurrent)
-        qnext = pinocchio.integrate(self.robot_model, q0, dt * dq)
-        return qnext, err
-
-    def inverse_kinematics_one_finger(
-        self,
-        finger_idx: int,
-        tip_target_position: np.ndarray,
-        joint_angles_guess: np.ndarray,
-        tolerance: float = 0.005,
-        max_iterations: int = 1000,
-    ) -> typing.Tuple[np.ndarray, np.ndarray]:
-        """Inverse kinematics for a single finger.
-
-        Args:
-            finger_idx: Index of the finger.
-            tip_target_positions: Target position for the finger tip in world
-                frame.
-            joint_angles_guess: Initial guess for the joint angles.
-            tolerance: Position error tolerance.  Stop if the error is less
-                then that.
-            max_iterations: Max. number of iterations.
-
-        Returns:
-            First element is the joint configuration (for joints that are not
-            part of the specified finger, the values from the initial guess are
-            kept).  Second element is (x,y,z)-error of the tip position.
-        """
-        q = joint_angles_guess
-        for i in range(max_iterations):
-            q, err = self._inverse_kinematics_step(
-                self.tip_link_ids[finger_idx], tip_target_position, q
-            )
-
-            if np.linalg.norm(err) < tolerance:
-                break
-        return q, err
-
-    def inverse_kinematics(
-        self,
-        tip_target_positions: typing.Iterable[np.ndarray],
-        joint_angles_guess: np.ndarray,
-        tolerance: float = 0.005,
-        max_iterations: int = 1000,
-    ) -> typing.Tuple[np.ndarray, typing.List[np.ndarray]]:
-        """Inverse kinematics for the whole manipulator.
-
-        Args:
-            tip_target_positions: List of finger tip target positions, one for
-                each finger.
-            joint_angles_guess: See :meth:`inverse_kinematics_one_finger`.
-            tolerance: See :meth:`inverse_kinematics_one_finger`.
-            max_iterations: See :meth:`inverse_kinematics_one_finger`.
-
-        Returns:
-            First element is the joint configuration, second element is a list
-            of (x,y,z)-errors of the tip positions.
-        """
-        q = joint_angles_guess
-        errors = []
-        for i, pos in enumerate(tip_target_positions):
-            q, err = self.inverse_kinematics_one_finger(
-                i, pos, q, tolerance, max_iterations
-            )
-            errors.append(err)
-
-        return q, errors
+__all__ = ["Kinematics"]