hiroishida / tinyfk

cache-utilized fast forward kinematics solver (pose & jacobian) for c++ and python.

tinyfk

A tiny fast forward-kinematics solver written in c++ and its python wrapper

Installation by downloading wheel from PyPI (only linux):

pip install tinyfk

or, building locally from source (for developer):

sudo apt-get install libeigen3-dev
git clone https://github.com/HiroIshida/tinyfk.git
cd tinyfk
git submodule update --init
pip install . 

Usage

import numpy as np

import tinyfk
from tinyfk import BaseType, KinematicModel, RotationType

urdf_model_path = tinyfk.pr2_urdfpath()
kin = KinematicModel(urdf_model_path)
joint_names = [
    "r_shoulder_pan_joint",
    "r_shoulder_lift_joint",
    "r_upper_arm_roll_joint",
    "r_elbow_flex_joint",
    "r_forearm_roll_joint",
    "r_wrist_flex_joint",
    "r_wrist_roll_joint",
]

joint_ids = kin.get_joint_ids(joint_names)
end_link_id = kin.get_link_ids(["r_gripper_tool_frame"])[0]

# first 7 elements are for joints and the last 3 elements are for x, y, yaw of base.
q = np.array([0.564, 0.35, -0.74, -0.7, -0.7, -0.17, -0.63, 0.1, 0.2, 0.3])

poses, jacobians = kin.solve_fk(
    q,
    end_link_id,
    joint_ids,
    rot_type=RotationType.RPY,
    base_type=BaseType.PLANER,
    with_jacobian=True,
)

For debugging

For debugging or developing, it's better using cmake directly rather than using pip. In this case, please use

sudo apt-get install libeigen3-dev
git clone https://github.com/HiroIshida/tinyfk.git
cd tinyfk
git submodule update --init
mkdir build
cd build
cmake --DCMAKE_BUILD_TYPE=Debug --DINSTALL_VIA_PIP=OFF ..
make -j4
# make install # please read the CMakeLists.txt before doing this

Note that when you build the python wrapper using cmake (not using pip), the package name for the wrapper will be _tinyfk. So, to make it compatible with the pip-installed one, please insert import _tinyfk as tinyfk in the beginning of the python script.

tinyfk's Issues

accessable from both string and id

special urdf::Pose type for fixed transform

remove tinyxml dependency

this

tinyfk/urdf_parser/CMakeLists.txt

Line 17 in 404888f

include_directories("external/tinyxml")

does not work.

invoke cmake from setup.py

use sckit build
see
https://github.com/scikit-build/scikit-build-sample-projects/tree/master/projects/hello-pybind11/src/hello

remove fixed joint from DOF count.

push_back ing joint_angles when we add another fixed joint is so strange!

avoid segmentation fault when urdf path is invalid

readability

link_ids -> link_name_id_map
joint_ids -> joint_name_id_map
joints -> movable_joints

lower level control

clear cache manually.

An example of scikit robot wrapper

import tinyfk
import skrobot
from skrobot.coordinates.math import rpy_angle
@dataclass(frozen=True)
class KinematicsSolver:  # a wrapper of tinyfk
    # assume using python 3.6 or higher. and use cpython.
    kinsol: tinyfk.RobotModel
    control_joint_names: List[str]
    endeffector_link_names: List[str]
    joint_name_id_table: Dict[str, int]
    link_name_id_table: Dict[str, int]

    @classmethod
    def construct(cls, robot_model: skrobot.model.RobotModel, control_joint_names: List[str], endeffector_names: List[str]) -> 'KinematicsSolver':
        kinsol = tinyfk.RobotModel(robot_model.urdf_path)

        joint_names = [j.name for j in robot_model.joint_list]
        joint_ids = kinsol.get_joint_ids(joint_names)
        joint_name_id_table = {name: idd for name, idd in zip(joint_names, joint_ids)}

        link_names = [j.name for j in robot_model.link_list]
        link_ids = kinsol.get_link_ids(link_names)
        link_name_id_table = {name: idd for name, idd in zip(link_names, link_ids)}
        return cls(kinsol, control_joint_names, endeffector_names, joint_name_id_table, link_name_id_table)

    def get_control_joint_ids(self) -> List[int]:
        return self.kinsol.get_joint_ids(self.control_joint_names)

    def get_endeffector_link_ids(self) -> List[int]:
        return self.kinsol.get_link_ids(self.endeffector_link_names)

    def solve_ik(self, coords: Coordinates, robot_model: skrobot.model.RobotModel) -> Optional[np.ndarray]:

        if robot_model is not None:
            av = robot_model.angle_vector()
            assert len(av) == len(self.joint_name_id_table)
            self.kinsol.set_joint_angles(list(self.joint_name_id_table.values()), av)

        init_angle = np.array([robot_model.__dict__[jname].joint_angle() for jname in self.control_joint_names])

        xyz = coords.worldpos()
        ypr = np.flip(rpy_angle(coords.worldrot()))[1]
        #from IPython import embed; embed()
        xyzrpy = np.hstack([xyz, ypr])
        elink_id = self.get_endeffector_link_ids()[0]
        joint_ids = self.get_control_joint_ids()
        angles = self.kinsol.solve_inverse_kinematics(xyzrpy, init_angle, elink_id, joint_ids)

        for joint_name, angle in zip(self.control_joint_names, angles):
            robot_model.__dict__[joint_name].joint_angle(angle)
        return robot_model.angle_vector()

test `l_gripper_finger_link` fetch

found an error that link points origin of l_gripper_finger_link does not match with the one of scikit-robot

write cache friendly code

1. a vector of a shared_pointer based data structure is not cache friendly. nstead we must make array of object.
2. use struct of array instead of array of struct. https://github.com/foonathan/array variant_bag<BlockStorage, T1, T2, ...>

avoid overhead of date passing between c++/python

try EigendREF

register pypi

make pypirc (don' need to fill password)
register to testpypi

https://qiita.com/icoxfog417/items/edba14600323df6bf5e0

https://qiita.com/kinpira/items/0a4e7c78fc5dd28bd695

remove console_bridge dependency

tinyfk/urdf_parser/CMakeLists.txt

Line 13 in dc1e95b

find_package(console_bridge REQUIRED)

-- The C compiler identification is GNU 7.5.0
-- The CXX compiler identification is GNU 7.5.0
-- Check for working C compiler: /usr/bin/cc
-- Check for working C compiler: /usr/bin/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
CMake Error at urdf_parser/CMakeLists.txt:13 (find_package):
  By not providing "Findconsole_bridge.cmake" in CMAKE_MODULE_PATH this
  project has asked CMake to find a package configuration file provided by
  "console_bridge", but CMake did not find one.

  Could not find a package configuration file provided by "console_bridge"
  with any of the following names:

    console_bridgeConfig.cmake
    console_bridge-config.cmake

  Add the installation prefix of "console_bridge" to CMAKE_PREFIX_PATH or set
  "console_bridge_DIR" to a directory containing one of the above files.  If
  "console_bridge" provides a separate development package or SDK, be sure it
  has been installed.

fix memory safety

some comments are incorret

e.g. rpy jacobain-> geometric jacobian

tinyfk/src/kinematics.cpp

Line 160 in dc1e95b

if(rotalso){ // (compute rpy jacobian)

GLIBCXX version is too new

on older system (ubuntu16.04):

root@a8e8891acba9:/# python
Python 2.7.12 (default, Mar  1 2021, 11:38:31) 
[GCC 5.4.0 20160609] on linux2
Type "help", "copyright", "credits" or "license" for more information.
>>> import tinyfk 
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
  File "/usr/local/lib/python2.7/dist-packages/tinyfk/__init__.py", line 4, in <module>
    from . import _tinyfk
ImportError: /usr/lib/x86_64-linux-gnu/libstdc++.so.6: version `GLIBCXX_3.4.26' not found (required by /usr/local/lib/python2.7/dist-packages/tinyfk/_tinyfk.so)