hiroishida / tinyfk Goto Github PK

-- 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.

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()

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)