├── skrobot ├── apps │ ├── __init__.py │ ├── urdf_hash.py │ ├── modularize_urdf.py │ ├── visualize_urdf.py │ ├── visualize_mesh.py │ ├── cli.py │ ├── scale_urdf.py │ └── convert_wheel_collision.py ├── data │ ├── kuka_description │ │ └── meshes │ │ │ ├── link_0.stl │ │ │ ├── link_1.stl │ │ │ ├── link_2.stl │ │ │ ├── link_3.stl │ │ │ ├── link_4.stl │ │ │ ├── link_5.stl │ │ │ ├── link_6.stl │ │ │ ├── link_7.stl │ │ │ ├── finger_base_left.stl │ │ │ ├── finger_tip_left.stl │ │ │ ├── finger_tip_right.stl │ │ │ ├── finger_base_right.stl │ │ │ ├── link_0.mtl │ │ │ ├── link_1.mtl │ │ │ ├── link_2.mtl │ │ │ ├── link_3.mtl │ │ │ ├── link_4.mtl │ │ │ ├── link_5.mtl │ │ │ ├── link_6.mtl │ │ │ └── link_7.mtl │ └── __init__.py ├── optimizers │ ├── __init__.py │ ├── cvxopt_solver.py │ └── quadprog_solver.py ├── utils │ ├── listify.py │ ├── package.py │ ├── checksum.py │ ├── __init__.py │ └── archive.py ├── planner │ └── __init__.py ├── interfaces │ ├── ros2 │ │ ├── __init__.py │ │ └── panda.py │ ├── ros │ │ ├── __init__.py │ │ ├── panda.py │ │ ├── tf_utils.py │ │ └── transform_listener.py │ └── __init__.py ├── pycompat.py ├── models │ ├── __init__.py │ ├── urdf.py │ ├── panda.py │ ├── kuka.py │ ├── fetch.py │ └── nextage.py ├── sdf │ └── __init__.py ├── coordinates │ └── __init__.py ├── urdf │ ├── __init__.py │ └── transform_urdf.py ├── model │ └── __init__.py ├── optimizer.py ├── _lazy_imports.py ├── viewers │ ├── __init__.py │ └── _viser.py └── __init__.py ├── tests └── skrobot_tests │ ├── __init__.py │ ├── model_tests │ └── __init__.py │ ├── urdf_tests │ ├── __init__.py │ └── test_hash.py │ ├── viewers_tests │ └── __init__.py │ ├── interfaces_tests │ ├── ros │ │ ├── __init__.py │ │ └── test_tf_utils.py │ ├── ros2 │ │ └── __init__.py │ └── test__pybullet.py │ ├── models_tests │ ├── test_pr2.py │ ├── test_kuka.py │ └── test_fetch.py │ ├── test_urdf.py │ ├── data │ └── primitives.urdf │ ├── planner_tests │ ├── test_swept_sphere.py │ ├── test_sqp_based_planner.py │ ├── test_collision_checker.py │ └── test_utils.py │ ├── test_interpolator.py │ ├── coordinates_tests │ ├── test_geo.py │ └── test_quaternion.py │ └── test_examples.py ├── requirements_opt.txt ├── requirements_docs.txt ├── docs ├── image │ ├── fetch.png │ ├── kuka.png │ ├── pr2.gif │ ├── pr2.png │ ├── viewer.jpg │ ├── robot_models.jpg │ ├── change-link-color.jpg │ ├── reset-link-color.jpg │ ├── pybullet_interface.png │ └── urdf-from-solidworks.png ├── source │ ├── _static │ │ ├── robots.txt │ │ ├── ik_basic_full_6dof.png │ │ ├── ik_mixed_planar_tilt.png │ │ ├── ik_mixed_planar_yaw.png │ │ ├── ik_basic_position_only.png │ │ ├── ik_mixed_frontal_plane.png │ │ ├── ik_mixed_vertical_tilt.png │ │ ├── ik_basic_orientation_only.png │ │ ├── ik_mixed_planar_position.png │ │ ├── visual-collision-comparison.jpg │ │ ├── ik_double_rot_rot_xy_trans_full.png │ │ ├── ik_double_rot_rot_yz_trans_full.png │ │ ├── ik_double_rot_rot_zx_trans_full.png │ │ ├── ik_minus_rot_rot_xm_trans_full.png │ │ ├── ik_minus_rot_rot_ym_trans_full.png │ │ ├── ik_minus_rot_rot_zm_trans_full.png │ │ ├── ik_mixed_vertical_full_rotation.png │ │ ├── ik_single_rot_rot_x_trans_full.png │ │ ├── ik_single_rot_rot_y_trans_full.png │ │ ├── ik_single_rot_rot_z_trans_full.png │ │ └── css │ │ │ └── modified_theme.css │ ├── overview │ │ └── index.rst │ ├── reference │ │ ├── index.rst │ │ ├── sdfs.rst │ │ ├── interfaces.rst │ │ ├── coordinates.rst │ │ ├── planner.rst │ │ ├── models.rst │ │ ├── functions.rst │ │ └── how_to_create_urdf_from_cad.rst │ ├── tutorials │ │ ├── index.rst │ │ ├── motion_planning.rst │ │ ├── inverse_kinematics.rst │ │ ├── visualization.rst │ │ ├── urdf_manipulation.rst │ │ └── real_robot_control.rst │ ├── _templates │ │ ├── search.html │ │ └── autosummary │ │ │ └── class.rst │ ├── install │ │ └── index.rst │ ├── examples_guide │ │ ├── index.rst │ │ ├── trimesh_scene_viewer.rst │ │ ├── grasp_and_pull.rst │ │ ├── batch_ik.rst │ │ ├── trajectory_interpolation.rst │ │ ├── pybullet_interface.rst │ │ ├── swept_sphere.rst │ │ ├── signed_distance_functions.rst │ │ ├── collision_free_trajectory.rst │ │ └── pr2_inverse_kinematics.rst │ ├── index.rst │ ├── _docstring_check.py │ ├── development │ │ └── index.rst │ ├── examples │ │ └── index.rst │ └── cli.rst ├── Makefile └── make.bat ├── .flake8 ├── MANIFEST.in ├── .gitignore ├── .readthedocs.yml ├── .github └── workflows │ ├── actionlint.yml │ ├── bump-version.yml │ └── release.yml ├── requirements.txt ├── LICENSE ├── pyproject.toml ├── examples ├── signed_distance_functions.py ├── pybullet_robot_interface.py ├── robot_models.py ├── trimesh_scene_viewer.py ├── swept_sphere.py └── grasp_and_pull_box.py └── setup.py /skrobot/apps/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tests/skrobot_tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tests/skrobot_tests/model_tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tests/skrobot_tests/urdf_tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /requirements_opt.txt: -------------------------------------------------------------------------------- 1 | cvxopt 2 | quadprog 3 | -------------------------------------------------------------------------------- /tests/skrobot_tests/viewers_tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tests/skrobot_tests/interfaces_tests/ros/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /requirements_docs.txt: -------------------------------------------------------------------------------- 1 | sphinx>=1.8.2 2 | sphinx_rtd_theme 3 | -------------------------------------------------------------------------------- /tests/skrobot_tests/interfaces_tests/ros2/__init__.py: -------------------------------------------------------------------------------- 1 | # ROS2 interface tests 2 | -------------------------------------------------------------------------------- /docs/image/fetch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/fetch.png -------------------------------------------------------------------------------- /docs/image/kuka.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/kuka.png -------------------------------------------------------------------------------- /docs/image/pr2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/pr2.gif -------------------------------------------------------------------------------- /docs/image/pr2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/pr2.png -------------------------------------------------------------------------------- /docs/image/viewer.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/viewer.jpg -------------------------------------------------------------------------------- /docs/image/robot_models.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/robot_models.jpg -------------------------------------------------------------------------------- /docs/image/change-link-color.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/change-link-color.jpg -------------------------------------------------------------------------------- /docs/image/reset-link-color.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/reset-link-color.jpg -------------------------------------------------------------------------------- /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | # Only check E128 (continuation line indentation) that ruff doesn't handle well 3 | select = E128 4 | -------------------------------------------------------------------------------- /docs/image/pybullet_interface.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/pybullet_interface.png -------------------------------------------------------------------------------- /docs/image/urdf-from-solidworks.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/image/urdf-from-solidworks.png -------------------------------------------------------------------------------- /docs/source/_static/robots.txt: -------------------------------------------------------------------------------- 1 | User-Agent: * 2 | Allow: / 3 | Allow: /en/stable/ 4 | Allow: /en/latest/ 5 | Disallow: /en/ 6 | -------------------------------------------------------------------------------- /docs/source/_static/ik_basic_full_6dof.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_basic_full_6dof.png -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include README.md 2 | include requirements.txt requirements_docs.txt requirements_opt.txt 3 | recursive-include skrobot/data * 4 | -------------------------------------------------------------------------------- /docs/source/_static/ik_mixed_planar_tilt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_mixed_planar_tilt.png -------------------------------------------------------------------------------- /docs/source/_static/ik_mixed_planar_yaw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_mixed_planar_yaw.png -------------------------------------------------------------------------------- /docs/source/_static/ik_basic_position_only.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_basic_position_only.png -------------------------------------------------------------------------------- /docs/source/_static/ik_mixed_frontal_plane.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_mixed_frontal_plane.png -------------------------------------------------------------------------------- /docs/source/_static/ik_mixed_vertical_tilt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_mixed_vertical_tilt.png -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/link_0.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/link_1.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/link_2.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/link_3.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/link_4.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/link_5.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/link_6.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/link_7.stl -------------------------------------------------------------------------------- /skrobot/optimizers/__init__.py: -------------------------------------------------------------------------------- 1 | from skrobot.optimizers import cvxopt_solver # NOQA 2 | from skrobot.optimizers import quadprog_solver # NOQA 3 | -------------------------------------------------------------------------------- /docs/source/_static/ik_basic_orientation_only.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_basic_orientation_only.png -------------------------------------------------------------------------------- /docs/source/_static/ik_mixed_planar_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_mixed_planar_position.png -------------------------------------------------------------------------------- /docs/source/_static/visual-collision-comparison.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/visual-collision-comparison.jpg -------------------------------------------------------------------------------- /docs/source/_static/ik_double_rot_rot_xy_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_double_rot_rot_xy_trans_full.png -------------------------------------------------------------------------------- /docs/source/_static/ik_double_rot_rot_yz_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_double_rot_rot_yz_trans_full.png -------------------------------------------------------------------------------- /docs/source/_static/ik_double_rot_rot_zx_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_double_rot_rot_zx_trans_full.png -------------------------------------------------------------------------------- /docs/source/_static/ik_minus_rot_rot_xm_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_minus_rot_rot_xm_trans_full.png -------------------------------------------------------------------------------- /docs/source/_static/ik_minus_rot_rot_ym_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_minus_rot_rot_ym_trans_full.png -------------------------------------------------------------------------------- /docs/source/_static/ik_minus_rot_rot_zm_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_minus_rot_rot_zm_trans_full.png -------------------------------------------------------------------------------- /docs/source/_static/ik_mixed_vertical_full_rotation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_mixed_vertical_full_rotation.png -------------------------------------------------------------------------------- /docs/source/_static/ik_single_rot_rot_x_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_single_rot_rot_x_trans_full.png -------------------------------------------------------------------------------- /docs/source/_static/ik_single_rot_rot_y_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_single_rot_rot_y_trans_full.png -------------------------------------------------------------------------------- /docs/source/_static/ik_single_rot_rot_z_trans_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/docs/source/_static/ik_single_rot_rot_z_trans_full.png -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/finger_base_left.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/finger_base_left.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/finger_tip_left.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/finger_tip_left.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/finger_tip_right.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/finger_tip_right.stl -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/finger_base_right.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iory/scikit-robot/HEAD/skrobot/data/kuka_description/meshes/finger_base_right.stl -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.egg/ 3 | *.pyc 4 | \#*\# 5 | .\#* 6 | /scikit_robot.egg-info/ 7 | /build/ 8 | /dist/ 9 | /src/ 10 | build 11 | /docs/source/**/reference/**/generated 12 | -------------------------------------------------------------------------------- /skrobot/utils/listify.py: -------------------------------------------------------------------------------- 1 | def listify(x, n=1): 2 | ret = None 3 | if isinstance(x, list): 4 | ret = x 5 | else: 6 | ret = [x] * n 7 | return ret 8 | -------------------------------------------------------------------------------- /skrobot/planner/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | from skrobot.planner.collision_checker import SweptSphereSdfCollisionChecker 4 | from skrobot.planner.sqp_based import sqp_plan_trajectory 5 | -------------------------------------------------------------------------------- /skrobot/interfaces/ros2/__init__.py: -------------------------------------------------------------------------------- 1 | from .base import ROS2RobotInterfaceBase 2 | from .panda import PandaROS2RobotInterface 3 | 4 | 5 | __all__ = ['ROS2RobotInterfaceBase', 'PandaROS2RobotInterface'] 6 | -------------------------------------------------------------------------------- /skrobot/pycompat.py: -------------------------------------------------------------------------------- 1 | import functools 2 | 3 | 4 | if hasattr(functools, 'lru_cache'): 5 | lru_cache = functools.lru_cache 6 | else: 7 | import repoze.lru 8 | lru_cache = repoze.lru.lru_cache 9 | -------------------------------------------------------------------------------- /tests/skrobot_tests/models_tests/test_pr2.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import skrobot 4 | 5 | 6 | class TestPR2(unittest.TestCase): 7 | 8 | def test_init(self): 9 | skrobot.models.PR2() 10 | -------------------------------------------------------------------------------- /tests/skrobot_tests/models_tests/test_kuka.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import skrobot 4 | 5 | 6 | class TestKuka(unittest.TestCase): 7 | 8 | def test_init(self): 9 | skrobot.models.Kuka() 10 | -------------------------------------------------------------------------------- /tests/skrobot_tests/models_tests/test_fetch.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import skrobot 4 | 5 | 6 | class TestFetch(unittest.TestCase): 7 | 8 | def test_init(self): 9 | skrobot.models.Fetch() 10 | -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_0.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_1.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_2.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_3.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_4.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_5.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_6.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /skrobot/data/kuka_description/meshes/link_7.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /docs/source/overview/index.rst: -------------------------------------------------------------------------------- 1 | ======== 2 | Overview 3 | ======== 4 | 5 | Scikit-Robot is a lightweight pure-Python library for robotic kinematics, motion planning, visualization and control. 6 | 7 | .. toctree:: 8 | :maxdepth: 2 9 | 10 | philosophy 11 | architecture 12 | -------------------------------------------------------------------------------- /skrobot/models/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | from skrobot.models.fetch import Fetch 4 | from skrobot.models.kuka import Kuka 5 | from skrobot.models.nextage import Nextage 6 | from skrobot.models.panda import Panda 7 | from skrobot.models.pr2 import PR2 8 | from skrobot.models.r8_6 import R8_6 9 | -------------------------------------------------------------------------------- /.readthedocs.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | build: 3 | os: "ubuntu-22.04" 4 | tools: 5 | python: "3.11" 6 | 7 | sphinx: 8 | configuration: docs/source/conf.py 9 | fail_on_warning: true 10 | 11 | python: 12 | install: 13 | - method: pip 14 | path: . 15 | extra_requirements: 16 | - all 17 | -------------------------------------------------------------------------------- /docs/source/reference/index.rst: -------------------------------------------------------------------------------- 1 | .. _reference: 2 | 3 | API Reference 4 | ============= 5 | 6 | .. module:: skrobot 7 | 8 | .. toctree:: 9 | :maxdepth: 2 10 | 11 | coordinates 12 | models 13 | functions 14 | interfaces 15 | sdfs 16 | planner 17 | viewers 18 | robot_model_tips 19 | how_to_create_urdf_from_cad.rst -------------------------------------------------------------------------------- /skrobot/sdf/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | from .signed_distance_function import BoxSDF 4 | from .signed_distance_function import CylinderSDF 5 | from .signed_distance_function import GridSDF 6 | from .signed_distance_function import SignedDistanceFunction 7 | from .signed_distance_function import SphereSDF 8 | from .signed_distance_function import UnionSDF 9 | 10 | from .signed_distance_function import trimesh2sdf 11 | -------------------------------------------------------------------------------- /docs/source/tutorials/index.rst: -------------------------------------------------------------------------------- 1 | ========= 2 | Tutorials 3 | ========= 4 | 5 | Step-by-step guides for common robotics tasks with scikit-robot. 6 | 7 | .. toctree:: 8 | :maxdepth: 2 9 | 10 | quickstart 11 | inverse_kinematics 12 | motion_planning 13 | visualization 14 | urdf_manipulation 15 | real_robot_control 16 | 17 | For detailed explanations of example scripts, see :doc:`../examples_guide/index`. 18 | -------------------------------------------------------------------------------- /docs/source/_templates/search.html: -------------------------------------------------------------------------------- 1 | {% extends "sphinx_rtd_theme/search.html" %} 2 | 3 | {% block footer %} 4 | 5 | {{ super() }} 6 | 7 | 15 | 16 | {% endblock %} 17 | -------------------------------------------------------------------------------- /.github/workflows/actionlint.yml: -------------------------------------------------------------------------------- 1 | name: Lint GitHub Actions workflows 2 | 3 | on: 4 | push: 5 | paths: 6 | - '.github/workflows/**' 7 | pull_request: 8 | paths: 9 | - '.github/workflows/**' 10 | 11 | jobs: 12 | actionlint: 13 | name: Actionlint 14 | runs-on: ubuntu-latest 15 | steps: 16 | - name: Checkout Repository 17 | uses: actions/checkout@v4 18 | 19 | - name: Run actionlint 20 | uses: reviewdog/action-actionlint@v1 21 | -------------------------------------------------------------------------------- /docs/source/install/index.rst: -------------------------------------------------------------------------------- 1 | .. _install: 2 | 3 | Installation and Setup Guide 4 | ============================ 5 | 6 | Python Installation 7 | ------------------- 8 | This package is pip-installable for any Python version. Simply run the 9 | following command: 10 | 11 | .. code-block:: bash 12 | 13 | pip install scikit-robot 14 | 15 | Installing in Development Mode 16 | ------------------------------ 17 | If you're planning on contributing to this repository, 18 | please see the :ref:`development`. 19 | -------------------------------------------------------------------------------- /skrobot/interfaces/ros/__init__.py: -------------------------------------------------------------------------------- 1 | from skrobot._lazy_imports import LazyImportClass 2 | 3 | 4 | NextageROSRobotInterface = LazyImportClass( 5 | ".nextage", "NextageROSRobotInterface", "skrobot.interfaces.ros") 6 | PandaROSRobotInterface = LazyImportClass( 7 | ".panda", "PandaROSRobotInterface", "skrobot.interfaces.ros") 8 | PR2ROSRobotInterface = LazyImportClass( 9 | ".pr2", "PR2ROSRobotInterface", "skrobot.interfaces.ros") 10 | 11 | __all__ = ["NextageROSRobotInterface", "PandaROSRobotInterface", "PR2ROSRobotInterface"] 12 | -------------------------------------------------------------------------------- /skrobot/utils/package.py: -------------------------------------------------------------------------------- 1 | def is_package_installed(package_name): 2 | try: 3 | from importlib.metadata import distribution 4 | from importlib.metadata import PackageNotFoundError 5 | try: 6 | distribution(package_name) 7 | return True 8 | except PackageNotFoundError: 9 | return False 10 | except ImportError: 11 | import imp 12 | try: 13 | imp.find_module(package_name) 14 | return True 15 | except ImportError: 16 | return False 17 | -------------------------------------------------------------------------------- /docs/source/tutorials/motion_planning.rst: -------------------------------------------------------------------------------- 1 | =============== 2 | Motion Planning 3 | =============== 4 | 5 | For collision-free trajectory planning and motion planning features, see: 6 | 7 | - :doc:`../reference/planner` - Motion planning API reference 8 | - :doc:`../examples_guide/collision_free_trajectory` - Collision-free trajectory example 9 | 10 | The motion planning module provides: 11 | 12 | - SQP-based trajectory optimization 13 | - Swept sphere collision detection 14 | - Signed Distance Field (SDF) integration 15 | - Smooth, collision-free path generation 16 | -------------------------------------------------------------------------------- /tests/skrobot_tests/interfaces_tests/test__pybullet.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import pytest 4 | 5 | import skrobot 6 | from skrobot.interfaces._pybullet import _check_available 7 | from skrobot.interfaces._pybullet import PybulletRobotInterface 8 | 9 | 10 | class TestPybulletRobotInterface(unittest.TestCase): 11 | 12 | @pytest.mark.skipif(_check_available(silent=True) is False, 13 | reason="Pybullet is not available") 14 | def test_init(self): 15 | fetch = skrobot.models.Fetch() 16 | PybulletRobotInterface(fetch, connect=2) 17 | -------------------------------------------------------------------------------- /docs/source/tutorials/inverse_kinematics.rst: -------------------------------------------------------------------------------- 1 | ==================== 2 | Inverse Kinematics 3 | ==================== 4 | 5 | For comprehensive inverse kinematics documentation including: 6 | 7 | - Basic inverse kinematics usage 8 | - Axis constraints (rotation_axis and translation_axis parameters) 9 | - Batch inverse kinematics for multiple poses 10 | - Visual examples of different constraint types 11 | - Advanced features and performance optimization 12 | - Common usage patterns 13 | 14 | See: 15 | 16 | - :doc:`../reference/robot_model_tips` - Comprehensive IK documentation 17 | - :doc:`../examples/index` - Basic IK examples 18 | -------------------------------------------------------------------------------- /docs/source/reference/sdfs.rst: -------------------------------------------------------------------------------- 1 | Signed distance function (SDF) 2 | ============================== 3 | 4 | SDF classes 5 | ----------- 6 | .. autosummary:: 7 | :toctree: generated/ 8 | :nosignatures: 9 | 10 | skrobot.sdf.SignedDistanceFunction 11 | skrobot.sdf.UnionSDF 12 | skrobot.sdf.BoxSDF 13 | skrobot.sdf.GridSDF 14 | skrobot.sdf.CylinderSDF 15 | skrobot.sdf.SphereSDF 16 | 17 | SDF utilities 18 | ------------- 19 | 20 | .. autosummary:: 21 | :toctree: generated/ 22 | :nosignatures: 23 | 24 | skrobot.sdf.signed_distance_function.trimesh2sdf 25 | skrobot.sdf.signed_distance_function.link2sdf 26 | -------------------------------------------------------------------------------- /skrobot/utils/checksum.py: -------------------------------------------------------------------------------- 1 | import hashlib 2 | 3 | 4 | def checksum_md5(filename, blocksize=8192): 5 | """Calculate md5sum. 6 | 7 | Parameters 8 | ---------- 9 | filename : str or pathlib.Path 10 | input filename. 11 | blocksize : int 12 | MD5 has 128-byte digest blocks (default: 8192 is 128x64). 13 | 14 | Returns 15 | ------- 16 | md5 : str 17 | calculated md5sum. 18 | """ 19 | filename = str(filename) 20 | hash_factory = hashlib.md5() 21 | with open(filename, 'rb') as f: 22 | for chunk in iter(lambda: f.read(blocksize), b''): 23 | hash_factory.update(chunk) 24 | return hash_factory.hexdigest() 25 | -------------------------------------------------------------------------------- /skrobot/coordinates/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | from .base import CascadedCoords 4 | from .base import Coordinates 5 | from .base import Transform 6 | from .base import make_coords 7 | from .base import make_cascoords 8 | 9 | from .geo import convert_to_axis_vector 10 | from .geo import midcoords 11 | from .geo import midpoint 12 | from .geo import orient_coords_to_axis 13 | from .geo import rotate_points 14 | 15 | from .math import matrix2quaternion 16 | from .math import quaternion2rpy 17 | from .math import make_matrix 18 | from .math import manipulability 19 | from .math import normalize_vector 20 | from .math import rpy_angle 21 | from .math import rpy_matrix 22 | from .math import sr_inverse 23 | -------------------------------------------------------------------------------- /skrobot/models/urdf.py: -------------------------------------------------------------------------------- 1 | from ..model import RobotModel 2 | 3 | 4 | class RobotModelFromURDF(RobotModel): 5 | 6 | def __init__(self, urdf=None, urdf_file=None): 7 | super(RobotModelFromURDF, self).__init__() 8 | 9 | if urdf and urdf_file: 10 | raise ValueError( 11 | "'urdf' and 'urdf_file' cannot be given at the same time" 12 | ) 13 | if urdf: 14 | self.load_urdf(urdf=urdf) 15 | elif urdf_file: 16 | self.load_urdf_file(file_obj=urdf_file) 17 | else: 18 | self.load_urdf_file(file_obj=self.default_urdf_path) 19 | 20 | @property 21 | def default_urdf_path(self): 22 | raise NotImplementedError 23 | -------------------------------------------------------------------------------- /docs/source/examples_guide/index.rst: -------------------------------------------------------------------------------- 1 | ================== 2 | Examples Explained 3 | ================== 4 | 5 | This section provides detailed explanations of the example scripts included with scikit-robot. 6 | Each example demonstrates specific features and best practices. 7 | 8 | All examples are located in the ``examples/`` directory of the scikit-robot repository: 9 | https://github.com/iory/scikit-robot/tree/main/examples 10 | 11 | .. toctree:: 12 | :maxdepth: 2 13 | 14 | robot_models 15 | pr2_inverse_kinematics 16 | collision_free_trajectory 17 | trimesh_scene_viewer 18 | pybullet_interface 19 | signed_distance_functions 20 | swept_sphere 21 | batch_ik 22 | grasp_and_pull 23 | trajectory_interpolation 24 | -------------------------------------------------------------------------------- /docs/source/examples_guide/trimesh_scene_viewer.rst: -------------------------------------------------------------------------------- 1 | ==================== 2 | Trimesh Scene Viewer 3 | ==================== 4 | 5 | **Example script**: ``examples/trimesh_scene_viewer.py`` 6 | 7 | This example demonstrates basic scene setup and robot visualization. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/trimesh_scene_viewer.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - Creating a 3D viewer 18 | - Adding robots to the scene 19 | - Basic camera control 20 | - Scene manipulation 21 | 22 | Related Documentation 23 | ===================== 24 | 25 | - :doc:`../reference/viewers` - Viewer API reference 26 | - :doc:`robot_models` - Displaying multiple robots 27 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SOURCEDIR = source 8 | BUILDDIR = build 9 | 10 | # Put it first so that "make" without argument is like "make help". 11 | help: 12 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 13 | 14 | .PHONY: help Makefile 15 | 16 | clean: 17 | rm -rf $(BUILDDIR)/* 18 | find source -type d -name generated -print0 | xargs -0 rm -rf 19 | 20 | # Catch-all target: route all unknown targets to Sphinx using the new 21 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 22 | %: Makefile 23 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 24 | -------------------------------------------------------------------------------- /docs/source/reference/interfaces.rst: -------------------------------------------------------------------------------- 1 | Interfaces 2 | ========== 3 | 4 | 5 | Pybullet Interface 6 | ------------------ 7 | 8 | You can use a Pybullet interface using skrobot. 9 | 10 | .. module:: skrobot.interfaces._pybullet 11 | 12 | .. autosummary:: 13 | :toctree: generated/ 14 | :nosignatures: 15 | 16 | PybulletRobotInterface 17 | 18 | >>> from skrobot.models import PR2 19 | >>> from skrobot.interfaces import PybulletRobotInterface 20 | >>> import pybullet 21 | >>> client_id = pybullet.connect(pybullet.GUI) 22 | >>> robot_model = PR2() 23 | >>> interface = PybulletRobotInterface(robot_model, connect=client_id) 24 | >>> interface.angle_vector(robot_model.reset_pose()) 25 | 26 | .. figure:: ../../image/pybullet_interface.png 27 | :scale: 60% 28 | :align: center 29 | -------------------------------------------------------------------------------- /docs/source/tutorials/visualization.rst: -------------------------------------------------------------------------------- 1 | ============== 2 | Visualization 3 | ============== 4 | 5 | For visualization and viewer documentation, see: 6 | 7 | - :doc:`../reference/viewers` - Viewer API reference 8 | - :doc:`../examples/index` - Basic visualization examples 9 | 10 | Scikit-robot supports multiple visualization backends: 11 | 12 | - **TrimeshSceneViewer**: Lightweight, fast rendering 13 | - **PyrenderViewer**: OpenGL-based, smoother rendering 14 | - **JupyterNotebookViewer**: Browser-based, works in Jupyter and Google Colab 15 | 16 | Basic usage: 17 | 18 | .. code-block:: python 19 | 20 | from skrobot.models import PR2 21 | from skrobot.viewers import TrimeshSceneViewer 22 | 23 | robot = PR2() 24 | viewer = TrimeshSceneViewer() 25 | viewer.add(robot) 26 | viewer.show() 27 | -------------------------------------------------------------------------------- /skrobot/utils/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | from skrobot.utils.urdf import URDF 4 | 5 | from skrobot.utils.archive import make_tarfile 6 | from skrobot.utils.checksum import checksum_md5 7 | 8 | from skrobot.utils.visualization import create_ik_visualization_hook 9 | from skrobot.utils.visualization import create_custom_ik_hook 10 | from skrobot.utils.visualization import ik_visualization 11 | from skrobot.utils.visualization import trajectory_visualization 12 | from skrobot.utils.visualization import auto_ik_hook 13 | from skrobot.utils.visualization import get_trajectory_optimization_callback 14 | from skrobot.utils.visualization import set_ik_visualization_enabled 15 | from skrobot.utils.visualization import get_ik_visualization_enabled 16 | 17 | from skrobot.utils.blender_mesh import remesh_with_blender 18 | -------------------------------------------------------------------------------- /skrobot/optimizers/cvxopt_solver.py: -------------------------------------------------------------------------------- 1 | import cvxopt 2 | from cvxopt import matrix as cvxmat 3 | from cvxopt.solvers import qp 4 | import numpy as np 5 | 6 | 7 | cvxopt.solvers.options['show_progress'] = False # disable cvxopt output 8 | 9 | 10 | def solve_qp(P, q, G, h, 11 | A=None, b=None, sym_proj=False, 12 | solver='cvxopt'): 13 | if sym_proj: 14 | P = .5 * (P + P.T) 15 | cvxmat(P) 16 | cvxmat(q) 17 | cvxmat(G) 18 | cvxmat(h) 19 | args = [cvxmat(P), cvxmat(q), cvxmat(G), cvxmat(h)] 20 | if A is not None: 21 | args.extend([cvxmat(A), cvxmat(b)]) 22 | sol = qp(*args, solver=solver) 23 | if 'optimal' not in sol['status']: 24 | raise ValueError('QP optimum not found: %s' % sol['status']) 25 | return np.array(sol['x']).reshape((P.shape[1],)) 26 | -------------------------------------------------------------------------------- /docs/source/examples_guide/grasp_and_pull.rst: -------------------------------------------------------------------------------- 1 | =============== 2 | Grasp and Pull 3 | =============== 4 | 5 | **Example script**: ``examples/grasp_and_pull_box.py`` 6 | 7 | This example demonstrates a complete manipulation task: grasping and pulling a box. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/grasp_and_pull_box.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - Inverse kinematics for reaching target 18 | - Gripper control 19 | - Sequential manipulation primitives 20 | - Coordinated motion planning 21 | 22 | This demonstrates a complete pick-and-place style task combining multiple robot capabilities. 23 | 24 | Related Documentation 25 | ===================== 26 | 27 | - :doc:`../reference/robot_model_tips` - IK for manipulation 28 | - :doc:`pr2_inverse_kinematics` - IK examples 29 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=source 11 | set BUILDDIR=build 12 | 13 | if "%1" == "" goto help 14 | 15 | %SPHINXBUILD% >NUL 2>NUL 16 | if errorlevel 9009 ( 17 | echo. 18 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 19 | echo.installed, then set the SPHINXBUILD environment variable to point 20 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 21 | echo.may add the Sphinx directory to PATH. 22 | echo. 23 | echo.If you don't have Sphinx installed, grab it from 24 | echo.http://sphinx-doc.org/ 25 | exit /b 1 26 | ) 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /docs/source/reference/coordinates.rst: -------------------------------------------------------------------------------- 1 | Coordinates 2 | =========== 3 | 4 | Coordinates classes 5 | ------------------- 6 | 7 | .. autosummary:: 8 | :toctree: generated/ 9 | :nosignatures: 10 | 11 | skrobot.coordinates.Coordinates 12 | skrobot.coordinates.CascadedCoords 13 | 14 | 15 | Coordinates utilities 16 | --------------------- 17 | 18 | .. autosummary:: 19 | :toctree: generated/ 20 | :nosignatures: 21 | 22 | skrobot.coordinates.geo.midcoords 23 | skrobot.coordinates.geo.orient_coords_to_axis 24 | skrobot.coordinates.base.random_coords 25 | skrobot.coordinates.base.coordinates_distance 26 | skrobot.coordinates.base.transform_coords 27 | 28 | Quaternion Classes 29 | ------------------ 30 | 31 | .. autosummary:: 32 | :toctree: generated/ 33 | :nosignatures: 34 | 35 | skrobot.coordinates.quaternion.Quaternion 36 | skrobot.coordinates.dual_quaternion.DualQuaternion 37 | -------------------------------------------------------------------------------- /docs/source/reference/planner.rst: -------------------------------------------------------------------------------- 1 | Planning 2 | ======== 3 | 4 | Sdf-swept-sphere-based collision checker 5 | ---------------------------------------- 6 | 7 | .. autosummary:: 8 | :toctree: generated/ 9 | :nosignatures: 10 | 11 | skrobot.planner.SweptSphereSdfCollisionChecker 12 | 13 | SQP-based trajectory planner 14 | ---------------------------- 15 | 16 | .. autosummary:: 17 | :toctree: generated/ 18 | :nosignatures: 19 | 20 | skrobot.planner.sqp_plan_trajectory 21 | 22 | Swept sphere generator 23 | ---------------------- 24 | 25 | .. autosummary:: 26 | :toctree: generated/ 27 | :nosignatures: 28 | 29 | skrobot.planner.swept_sphere.compute_swept_sphere 30 | 31 | Planner utils 32 | ------------- 33 | .. autosummary:: 34 | :toctree: generated/ 35 | :nosignatures: 36 | 37 | skrobot.planner.utils.scipinize 38 | skrobot.planner.utils.set_robot_config 39 | skrobot.planner.utils.get_robot_config 40 | skrobot.planner.utils.forward_kinematics_multi 41 | -------------------------------------------------------------------------------- /docs/source/examples_guide/batch_ik.rst: -------------------------------------------------------------------------------- 1 | ======================== 2 | Batch Inverse Kinematics 3 | ======================== 4 | 5 | **Example script**: ``examples/batch_ik_demo.py`` 6 | 7 | This example demonstrates batch inverse kinematics for generating multiple IK solutions. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/batch_ik_demo.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - Generating multiple IK solutions for the same target 18 | - Comparing different solutions 19 | - Selecting optimal solutions based on criteria 20 | 21 | Use Cases 22 | ========= 23 | 24 | Batch IK is useful for: 25 | 26 | - Finding collision-free configurations 27 | - Selecting configurations closer to current pose 28 | - Exploring the solution space 29 | - Path planning with multiple waypoints 30 | 31 | Related Documentation 32 | ===================== 33 | 34 | - :doc:`../reference/robot_model_tips` - Batch IK documentation 35 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | cached-property;python_version>="3.0" 2 | cached-property<=1.5.2;python_version<"3.0" 3 | filelock 4 | future 5 | gdown 6 | lxml 7 | networkx>=2.2.0;python_version<"3.8" 8 | networkx>=2.5;python_version>="3.8" 9 | numpy>=1.16.0;python_version<"3.8" 10 | numpy>=1.21.0;python_version>="3.8" 11 | ordered_set 12 | packaging 13 | pillow 14 | pycollada<0.9;python_version<"3.2" # required for robot model using collada 15 | pycollada>=0.8,<=0.9;python_version>="3.2" and python_version<"3.9" 16 | pycollada>=0.8;python_version>="3.9" # required for robot model using collada 17 | pyglet<2.0 18 | pysdfgen>=0.2.0 19 | repoze.lru;python_version<"3.2" 20 | rtree # need rtree for simplify_quadric_decimation 21 | scikit-learn;python_version<"3.8" 22 | scikit-learn>=0.24.0;python_version>="3.8" 23 | scikit-robot-pyrender>=0.1.47 24 | scipy;python_version>="3.0" and python_version<"3.8" 25 | scipy<=1.2.3;python_version<"3.0" 26 | scipy>=1.6.3;python_version>="3.8" 27 | six 28 | trimesh>=3.9.0,!=3.23.0 29 | viser 30 | -------------------------------------------------------------------------------- /docs/source/examples_guide/trajectory_interpolation.rst: -------------------------------------------------------------------------------- 1 | ======================== 2 | Trajectory Interpolation 3 | ======================== 4 | 5 | **Example script**: ``examples/trajectory_interpolation_demo.py`` 6 | 7 | This example demonstrates smooth trajectory interpolation between robot configurations. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/trajectory_interpolation_demo.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - Interpolating between joint configurations 18 | - Smooth motion generation 19 | - Trajectory visualization 20 | 21 | Key Concepts 22 | ============ 23 | 24 | Trajectory interpolation creates smooth transitions between discrete robot poses, essential for: 25 | 26 | - Smooth robot motion 27 | - Avoiding jerky movements 28 | - Velocity and acceleration control 29 | 30 | Related Documentation 31 | ===================== 32 | 33 | - :doc:`collision_free_trajectory` - Planning with collision avoidance 34 | -------------------------------------------------------------------------------- /docs/source/tutorials/urdf_manipulation.rst: -------------------------------------------------------------------------------- 1 | ================== 2 | URDF Manipulation 3 | ================== 4 | 5 | For URDF manipulation tools and techniques, see: 6 | 7 | - :doc:`../reference/how_to_create_urdf_from_cad` - Creating URDF from CAD software 8 | - :doc:`../cli` - Command-line URDF tools 9 | 10 | Scikit-robot provides comprehensive URDF manipulation tools: 11 | 12 | **modularize-urdf**: Convert monolithic URDF to reusable xacro macros 13 | 14 | .. code-block:: bash 15 | 16 | skr modularize-urdf robot.urdf --output robot_module.xacro 17 | 18 | **change-urdf-root**: Dynamically reconfigure kinematic hierarchy 19 | 20 | .. code-block:: bash 21 | 22 | skr change-urdf-root robot.urdf new_root_link output.urdf 23 | 24 | **convert-urdf-mesh**: Optimize 3D meshes 25 | 26 | .. code-block:: bash 27 | 28 | skr convert-urdf-mesh robot.urdf --output optimized.urdf --quality 0.5 29 | 30 | **visualize-urdf**: Interactive 3D preview 31 | 32 | .. code-block:: bash 33 | 34 | skr visualize-urdf robot.urdf --viewer trimesh 35 | -------------------------------------------------------------------------------- /docs/source/reference/models.rst: -------------------------------------------------------------------------------- 1 | Models 2 | ====== 3 | 4 | 5 | Robot Model class 6 | ----------------- 7 | 8 | .. module:: skrobot.model 9 | 10 | .. autosummary:: 11 | :toctree: generated/ 12 | :nosignatures: 13 | 14 | RobotModel 15 | 16 | 17 | Robot Model classes 18 | ------------------- 19 | 20 | .. module:: skrobot.models 21 | 22 | You can create use robot model classes. Here is a example of robot models. 23 | 24 | Fetch 25 | ~~~~~ 26 | 27 | .. autosummary:: 28 | :toctree: generated/ 29 | :nosignatures: 30 | 31 | fetch.Fetch 32 | 33 | .. figure:: ../../image/fetch.png 34 | :scale: 40% 35 | :align: center 36 | 37 | 38 | Kuka 39 | ~~~~ 40 | 41 | .. autosummary:: 42 | :toctree: generated/ 43 | :nosignatures: 44 | 45 | kuka.Kuka 46 | 47 | .. figure:: ../../image/kuka.png 48 | :scale: 40% 49 | :align: center 50 | 51 | PR2 52 | ~~~ 53 | 54 | .. autosummary:: 55 | :toctree: generated/ 56 | :nosignatures: 57 | 58 | pr2.PR2 59 | 60 | .. figure:: ../../image/pr2.png 61 | :scale: 40% 62 | :align: center 63 | -------------------------------------------------------------------------------- /skrobot/urdf/__init__.py: -------------------------------------------------------------------------------- 1 | from .aggregate import aggregate_urdf_mesh_files 2 | from .extract_sub_urdf import extract_sub_urdf 3 | from .modularize_urdf import find_root_link 4 | from .modularize_urdf import transform_urdf_to_macro 5 | from .primitives_converter import convert_meshes_to_primitives 6 | from .scale_urdf import scale_urdf 7 | from .transform_urdf import transform_urdf_with_world_link 8 | from .wheel_collision_converter import convert_wheel_collisions_to_cylinders 9 | from .wheel_collision_converter import get_mesh_dimensions 10 | from .xml_root_link_changer import change_urdf_root_link 11 | from .xml_root_link_changer import URDFXMLRootLinkChanger 12 | 13 | 14 | __all__ = [ 15 | 'change_urdf_root_link', 16 | 'URDFXMLRootLinkChanger', 17 | 'find_root_link', 18 | 'transform_urdf_to_macro', 19 | 'transform_urdf_with_world_link', 20 | 'aggregate_urdf_mesh_files', 21 | 'convert_wheel_collisions_to_cylinders', 22 | 'convert_meshes_to_primitives', 23 | 'get_mesh_dimensions', 24 | 'extract_sub_urdf', 25 | 'scale_urdf', 26 | ] 27 | -------------------------------------------------------------------------------- /docs/source/examples_guide/pybullet_interface.rst: -------------------------------------------------------------------------------- 1 | ================== 2 | PyBullet Interface 3 | ================== 4 | 5 | **Example script**: ``examples/pybullet_robot_interface.py`` 6 | 7 | This example demonstrates simulating robots in PyBullet physics engine. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/pybullet_robot_interface.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - PyBullet simulation setup 18 | - Sending angle vectors to simulated robots 19 | - Physics simulation integration 20 | - Same API as real robot control 21 | 22 | Key Features 23 | ============ 24 | 25 | The PyBullet interface provides the same API as the ROS interface, making it easy to: 26 | 27 | - Test robot motions in simulation before deploying to hardware 28 | - Develop and debug control algorithms safely 29 | - Visualize robot behavior with physics 30 | 31 | Related Documentation 32 | ===================== 33 | 34 | - :doc:`../reference/interfaces` - Robot interfaces API 35 | - :doc:`../tutorials/real_robot_control` - Real robot control 36 | -------------------------------------------------------------------------------- /docs/source/examples_guide/swept_sphere.rst: -------------------------------------------------------------------------------- 1 | ============ 2 | Swept Sphere 3 | ============ 4 | 5 | **Example script**: ``examples/swept_sphere.py`` 6 | 7 | This example demonstrates swept sphere collision detection for robot links. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/swept_sphere.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - Computing swept sphere representations of robot links 18 | - Efficient collision detection using spheres 19 | - Visualizing collision geometry 20 | 21 | Key Concepts 22 | ============ 23 | 24 | Swept spheres approximate robot links as a series of spheres, enabling: 25 | 26 | - Fast collision checking 27 | - Smooth distance calculations 28 | - Efficient motion planning 29 | 30 | This representation is particularly useful for real-time collision detection and trajectory optimization. 31 | 32 | Related Documentation 33 | ===================== 34 | 35 | - :doc:`../reference/planner` - Motion planning with swept spheres 36 | - :doc:`collision_free_trajectory` - Swept sphere usage example 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2020 Iori Yanokura. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | THE SOFTWARE. 20 | -------------------------------------------------------------------------------- /skrobot/interfaces/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | import importlib 4 | 5 | 6 | class LazyPybulletModule(object): 7 | def __init__(self): 8 | self._module = None 9 | 10 | def __getattr__(self, attr): 11 | if self._module is None: 12 | self._module = importlib.import_module('skrobot.interfaces._pybullet') 13 | return getattr(self._module, attr) 14 | 15 | def __dir__(self): 16 | if self._module is None: 17 | return [] 18 | return dir(self._module) 19 | 20 | 21 | # Lazy import for pybullet module 22 | pybullet = LazyPybulletModule() 23 | 24 | 25 | def __getattr__(name): 26 | if name == 'PybulletRobotInterface': 27 | return pybullet.PybulletRobotInterface 28 | elif name == '_pybullet': 29 | # Force lazy loading - only import when explicitly requested 30 | if pybullet._module is None: 31 | pybullet._module = importlib.import_module('skrobot.interfaces._pybullet') 32 | return pybullet._module 33 | raise AttributeError(f"module 'skrobot.interfaces' has no attribute '{name}'") 34 | 35 | 36 | from .ros import * 37 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.ruff] 2 | # Exclude directories and file types from linting 3 | extend-exclude = [".?*", "src/", "build", "docs", "*.ipynb"] 4 | line-length = 120 5 | 6 | [tool.ruff.lint] 7 | preview = true 8 | 9 | # Enable flake8-style rules 10 | select = [ 11 | "E", # style errors 12 | "F", # flakes 13 | "W", # style warnings 14 | "G", # flake8-logging-format 15 | "I", # import sorting 16 | ] 17 | # Ignore specific rules to match current flake8 config 18 | ignore = [ 19 | "E741", # ambiguous variable name 20 | ] 21 | 22 | [tool.ruff.lint.isort] 23 | # Match current isort configuration 24 | force-single-line = true 25 | force-sort-within-sections = true 26 | order-by-type = false 27 | lines-after-imports = 2 28 | 29 | [tool.typos] 30 | default.extend-ignore-re = [ 31 | "(?Rm)^.*(#|//)\\s*spellchecker:disable-line$", # spellchecker:disable-line 32 | "(?s)(#|//)\\s*spellchecker:off.*?\\n\\s*(#|//)\\s*spellchecker:on", # spellchecker: 33 | "thre", # TODO(iory) delete thre by using threshold 34 | ] 35 | 36 | default.extend-ignore-identifiers-re = [ 37 | # Add individual words here to ignore them 38 | ] 39 | -------------------------------------------------------------------------------- /skrobot/utils/archive.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | import tarfile 3 | 4 | 5 | def make_tarfile(source_dir, output_filename=None, arcname=None): 6 | """Create a tar.gz archive from a directory. 7 | 8 | Parameters 9 | ---------- 10 | source_dir : str or pathlib.Path 11 | The directory to archive 12 | output_filename : str or pathlib.Path, optional 13 | Output tar.gz filename. If None, uses source_dir.tar.gz 14 | arcname : str, optional 15 | Alternative name for the top-level directory in the archive 16 | 17 | Returns 18 | ------- 19 | str 20 | Path to created tar.gz file 21 | """ 22 | source_dir = Path(source_dir) 23 | if output_filename is None: 24 | output_filename = source_dir.with_suffix('.tar.gz') 25 | else: 26 | output_filename = Path(output_filename) 27 | if not str(output_filename).endswith('.tar.gz'): 28 | output_filename = output_filename.with_suffix('.tar.gz') 29 | 30 | with tarfile.open(output_filename, "w:gz") as tar: 31 | tar.add(source_dir, arcname=arcname or source_dir.name) 32 | 33 | return str(output_filename) 34 | -------------------------------------------------------------------------------- /.github/workflows/bump-version.yml: -------------------------------------------------------------------------------- 1 | name: Bump Version and Create PR 2 | 3 | on: 4 | workflow_dispatch: 5 | inputs: 6 | bump_type: 7 | description: 'Version bump type' 8 | type: choice 9 | required: true 10 | default: 'patch' 11 | options: 12 | - major 13 | - minor 14 | - patch 15 | 16 | permissions: 17 | contents: write 18 | pull-requests: write 19 | 20 | jobs: 21 | bump-version: 22 | runs-on: ubuntu-latest 23 | steps: 24 | - name: Checkout repository 25 | uses: actions/checkout@v4 26 | with: 27 | fetch-depth: 0 28 | 29 | - name: Bump Version 30 | id: bump 31 | uses: iory/github-action-bump-version@v1.0.0 32 | with: 33 | bump_type: ${{ github.event.inputs.bump_type }} 34 | github_token: ${{ secrets.AUTO_MERGE_PAT }} 35 | base_branch: 'main' 36 | labels: 'auto-merge-ok,release' 37 | 38 | - name: Print Versions 39 | run: | 40 | echo "Current Version: ${{ steps.bump.outputs.current_version }}" 41 | echo "New Version: ${{ steps.bump.outputs.new_version }}" 42 | -------------------------------------------------------------------------------- /docs/source/tutorials/real_robot_control.rst: -------------------------------------------------------------------------------- 1 | ================== 2 | Real Robot Control 3 | ================== 4 | 5 | For controlling real robots and simulation interfaces, see: 6 | 7 | - :doc:`../reference/interfaces` - Robot interfaces API reference 8 | 9 | Scikit-robot provides interfaces for: 10 | 11 | **ROS Interface** 12 | 13 | Control ROS-enabled robots: 14 | 15 | .. code-block:: python 16 | 17 | from skrobot.interfaces.ros import ROSRobotInterfaceBase 18 | 19 | class MyRobotInterface(ROSRobotInterfaceBase): 20 | def __init__(self, *args, **kwargs): 21 | super().__init__(*args, **kwargs) 22 | 23 | ri = MyRobotInterface(robot) 24 | ri.angle_vector(target_av, time=3.0) 25 | ri.wait_interpolation() 26 | 27 | **PyBullet Interface** 28 | 29 | Simulate robots in PyBullet: 30 | 31 | .. code-block:: python 32 | 33 | from skrobot.interfaces._pybullet import PybulletRobotInterface 34 | 35 | ri = PybulletRobotInterface(robot) 36 | ri.angle_vector(target_av, time=2.0) 37 | ri.wait_interpolation() 38 | 39 | The same code works for both simulation and real robots, making it easy to develop and test in simulation before deploying to hardware. 40 | -------------------------------------------------------------------------------- /skrobot/apps/urdf_hash.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import sys 5 | 6 | from skrobot.urdf.hash import get_urdf_hash 7 | 8 | 9 | def main(): 10 | """Calculate hash of URDF file including all referenced assets.""" 11 | parser = argparse.ArgumentParser( 12 | description='Calculate comprehensive hash of URDF including all ' 13 | 'referenced meshes and textures.', 14 | formatter_class=argparse.RawTextHelpFormatter) 15 | parser.add_argument( 16 | 'urdf_file', 17 | type=str, 18 | help='Path to the URDF file') 19 | parser.add_argument( 20 | '-v', '--verbose', 21 | action='store_true', 22 | help='Print verbose output') 23 | 24 | args = parser.parse_args() 25 | 26 | try: 27 | hash_value = get_urdf_hash(args.urdf_file) 28 | if args.verbose: 29 | print(f"URDF file: {args.urdf_file}") 30 | print(f"Hash: {hash_value}") 31 | else: 32 | print(hash_value) 33 | except Exception as e: 34 | print(f"Error: {e}", file=sys.stderr) 35 | sys.exit(1) 36 | 37 | 38 | if __name__ == '__main__': 39 | main() 40 | -------------------------------------------------------------------------------- /docs/source/examples_guide/signed_distance_functions.rst: -------------------------------------------------------------------------------- 1 | ========================== 2 | Signed Distance Functions 3 | ========================== 4 | 5 | **Example script**: ``examples/signed_distance_functions.py`` 6 | 7 | This example demonstrates creating and using Signed Distance Functions (SDFs) for collision detection. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/signed_distance_functions.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - Creating primitive SDFs (Box, Sphere, Cylinder) 18 | - Combining multiple SDFs with UnionSDF 19 | - Converting meshes to SDFs 20 | - Using SDFs for collision checking 21 | 22 | Key Concepts 23 | ============ 24 | 25 | SDFs represent the distance from any point in space to the nearest surface: 26 | 27 | - Negative values: inside the object 28 | - Positive values: outside the object 29 | - Zero: on the surface 30 | 31 | This makes them efficient for collision detection and motion planning. 32 | 33 | Related Documentation 34 | ===================== 35 | 36 | - :doc:`../reference/sdfs` - SDF API reference 37 | - :doc:`collision_free_trajectory` - Using SDFs for planning 38 | -------------------------------------------------------------------------------- /tests/skrobot_tests/test_urdf.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import unittest 4 | 5 | from skrobot.data import fetch_urdfpath 6 | from skrobot.models.urdf import RobotModelFromURDF 7 | from skrobot.utils.urdf import mesh_simplify_factor 8 | 9 | 10 | class TestURDF(unittest.TestCase): 11 | 12 | def test_load_urdfmodel(self): 13 | urdfpath = fetch_urdfpath() 14 | # Absolute path 15 | RobotModelFromURDF(urdf_file=urdfpath) 16 | # Relative path 17 | os.chdir(os.path.dirname(urdfpath)) 18 | RobotModelFromURDF( 19 | urdf_file=os.path.basename(urdfpath)) 20 | # String 21 | with open(urdfpath, 'r') as f: 22 | RobotModelFromURDF(urdf=f.read()) 23 | 24 | def test_load_urdfmodel_with_simplification(self): 25 | if sys.version_info.major < 3: 26 | return # this feature is supported only for python3.x 27 | 28 | # create cache and load 29 | with mesh_simplify_factor(0.1): 30 | RobotModelFromURDF(urdf_file=fetch_urdfpath()) 31 | 32 | # load using existing cache 33 | with mesh_simplify_factor(0.1): 34 | RobotModelFromURDF(urdf_file=fetch_urdfpath()) 35 | -------------------------------------------------------------------------------- /docs/source/examples_guide/collision_free_trajectory.rst: -------------------------------------------------------------------------------- 1 | ========================== 2 | Collision-Free Trajectory 3 | ========================== 4 | 5 | **Example script**: ``examples/collision_free_trajectory.py`` 6 | 7 | This example demonstrates collision-free motion planning using SQP-based trajectory optimization. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/collision_free_trajectory.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - SQP-based trajectory planning 18 | - Swept sphere collision detection 19 | - Box obstacle avoidance 20 | - Smooth trajectory generation from start to goal pose 21 | 22 | Running the Example 23 | =================== 24 | 25 | .. code-block:: bash 26 | 27 | python examples/collision_free_trajectory.py 28 | 29 | With trajectory visualization: 30 | 31 | .. code-block:: bash 32 | 33 | python examples/collision_free_trajectory.py --trajectory-visualization 34 | 35 | Key Features 36 | ============ 37 | 38 | - Creates a box obstacle using SDF (Signed Distance Function) 39 | - Plans collision-free trajectory for PR2 right arm 40 | - Optimizes for smoothness while avoiding obstacles 41 | - Visualizes the planned trajectory 42 | 43 | Related Documentation 44 | ===================== 45 | 46 | - :doc:`../reference/planner` - Motion planning API 47 | - :doc:`../reference/sdfs` - Signed Distance Functions 48 | -------------------------------------------------------------------------------- /tests/skrobot_tests/data/primitives.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | ====================================================================================== 2 | Scikit-Robot -- A Flexible Framework for Robot visualization and programming in Python 3 | ====================================================================================== 4 | 5 | Scikit-Robot is a simple pure-Python library for loading, manipulating, and visualizing 6 | URDF files and robot specifications. For example, here's a rendering of a PR2 7 | robot moving after being loaded by this library. 8 | 9 | .. only:: html 10 | 11 | .. image:: ../image/pr2.gif 12 | :align: center 13 | 14 | .. only:: latex 15 | 16 | .. image:: ../image/pr2.png 17 | :align: center 18 | 19 | .. toctree:: 20 | :maxdepth: 2 21 | :caption: Getting Started 22 | 23 | install/index.rst 24 | tutorials/quickstart.rst 25 | 26 | .. toctree:: 27 | :maxdepth: 2 28 | :caption: Understanding Scikit-Robot 29 | 30 | overview/index.rst 31 | 32 | .. toctree:: 33 | :maxdepth: 2 34 | :caption: User Guide 35 | 36 | tutorials/index.rst 37 | examples/index.rst 38 | examples_guide/index.rst 39 | cli.rst 40 | 41 | .. toctree:: 42 | :maxdepth: 2 43 | :caption: Reference 44 | 45 | reference/index.rst 46 | 47 | .. toctree:: 48 | :maxdepth: 2 49 | :caption: Development 50 | 51 | development/index.rst 52 | 53 | 54 | Indices and tables 55 | ================== 56 | 57 | * :ref:`genindex` 58 | * :ref:`modindex` 59 | * :ref:`search` 60 | -------------------------------------------------------------------------------- /tests/skrobot_tests/planner_tests/test_swept_sphere.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import numpy as np 4 | import trimesh 5 | 6 | import skrobot 7 | from skrobot.planner.swept_sphere import compute_swept_sphere 8 | 9 | 10 | class TestSweptSphere(unittest.TestCase): 11 | 12 | @classmethod 13 | def setup_class(cls): 14 | robot_model = skrobot.models.PR2() 15 | cls.mesh1 = robot_model.r_gripper_palm_link._visual_mesh[0] 16 | 17 | objfile_path = skrobot.data.bunny_objpath() 18 | bunnymesh = trimesh.load_mesh(objfile_path) 19 | cls.mesh2 = bunnymesh 20 | 21 | def test_copmute_swept_sphere(self): 22 | 23 | def approximation_accuracy_test(mesh, tol): 24 | center_pts, radius = compute_swept_sphere(mesh, tol=tol) 25 | 26 | def swept_sphere_sdf(pts): 27 | dists_list = [] 28 | for c in center_pts: 29 | dists = np.sqrt(np.sum((pts - c[None, :])**2, axis=1)) 30 | dists_list.append(dists) 31 | dists_arr = np.array(dists_list) 32 | # union sdf of all spheres 33 | signed_dists = np.min(dists_arr, axis=0) - radius 34 | return signed_dists 35 | 36 | jut_arr = swept_sphere_sdf(mesh.vertices) 37 | max_jut = np.max(jut_arr) 38 | self.assertLess(max_jut / radius, tol) 39 | 40 | tol = 1e-3 41 | approximation_accuracy_test(self.mesh1, tol) 42 | approximation_accuracy_test(self.mesh2, tol) 43 | -------------------------------------------------------------------------------- /skrobot/apps/modularize_urdf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import os 5 | 6 | from lxml import etree 7 | 8 | from skrobot.urdf.modularize_urdf import find_root_link 9 | from skrobot.urdf.modularize_urdf import print_xacro_usage_instructions 10 | from skrobot.urdf.modularize_urdf import transform_urdf_to_macro 11 | 12 | 13 | def main(): 14 | parser = argparse.ArgumentParser(description="Modularize URDF to xacro macro") 15 | parser.add_argument("input_urdf", help="Input URDF file path") 16 | parser.add_argument("--no-prefix", action="store_true", 17 | help="Remove 'prefix' parameter and do not use ${prefix} in names") 18 | parser.add_argument("--output", help="Output file path") 19 | parser.add_argument("--macro-name", help="Name for the xacro macro. Defaults to the robot name in the URDF.") 20 | args = parser.parse_args() 21 | 22 | root_link = find_root_link(args.input_urdf) 23 | xacro_root, final_macro_name = transform_urdf_to_macro( 24 | args.input_urdf, root_link, args.no_prefix, macro_name=args.macro_name) 25 | 26 | if args.output: 27 | output_path = args.output 28 | else: 29 | base_name = os.path.splitext(args.input_urdf)[0] 30 | output_path = base_name + "_modularized.xacro" 31 | 32 | etree.ElementTree(xacro_root).write(output_path, pretty_print=True, xml_declaration=True, encoding="utf-8") 33 | 34 | print_xacro_usage_instructions(output_path, final_macro_name, args.no_prefix) 35 | 36 | 37 | if __name__ == "__main__": 38 | main() 39 | -------------------------------------------------------------------------------- /skrobot/models/panda.py: -------------------------------------------------------------------------------- 1 | from cached_property import cached_property 2 | 3 | from ..data import panda_urdfpath 4 | from ..model import RobotModel 5 | from .urdf import RobotModelFromURDF 6 | 7 | 8 | class Panda(RobotModelFromURDF): 9 | 10 | """Panda Robot Model. 11 | 12 | https://frankaemika.github.io/docs/control_parameters.html 13 | """ 14 | 15 | def __init__(self, *args, **kwargs): 16 | super(Panda, self).__init__(*args, **kwargs) 17 | self.reset_pose() 18 | 19 | @cached_property 20 | def default_urdf_path(self): 21 | return panda_urdfpath() 22 | 23 | def reset_pose(self): 24 | angle_vector = [ 25 | 0.03942226991057396, 26 | -0.9558116793632507, 27 | -0.014800949953496456, 28 | -2.130282163619995, 29 | -0.013104429468512535, 30 | 1.1745525598526, 31 | 0.8112226724624634, 32 | ] 33 | for link, angle in zip(self.rarm.link_list, angle_vector): 34 | link.joint.joint_angle(angle) 35 | return self.angle_vector() 36 | 37 | def reset_manip_pose(self): 38 | """Reset robot to manipulation pose (same as reset_pose for Panda)""" 39 | return self.reset_pose() 40 | 41 | @cached_property 42 | def rarm(self): 43 | link_names = ['panda_link{}'.format(i) for i in range(1, 8)] 44 | links = [getattr(self, n) for n in link_names] 45 | joints = [l.joint for l in links] 46 | model = RobotModel(link_list=links, joint_list=joints) 47 | model.end_coords = self.panda_hand 48 | return model 49 | -------------------------------------------------------------------------------- /skrobot/model/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | from skrobot.model.link import Link 4 | 5 | from skrobot.model.primitives import Annulus 6 | from skrobot.model.primitives import Axis 7 | from skrobot.model.primitives import Box 8 | from skrobot.model.primitives import CameraMarker 9 | from skrobot.model.primitives import Cone 10 | from skrobot.model.primitives import Cylinder 11 | from skrobot.model.primitives import LineString 12 | from skrobot.model.primitives import MeshLink 13 | from skrobot.model.primitives import PointCloudLink 14 | from skrobot.model.primitives import Sphere 15 | 16 | from skrobot.model.joint import FixedJoint 17 | from skrobot.model.joint import Joint 18 | from skrobot.model.joint import LinearJoint 19 | from skrobot.model.joint import OmniWheelJoint 20 | from skrobot.model.joint import RotationalJoint 21 | 22 | from skrobot.model.joint import calc_angle_speed_gain_scalar 23 | from skrobot.model.joint import calc_angle_speed_gain_vector 24 | from skrobot.model.joint import calc_dif_with_axis 25 | from skrobot.model.joint import calc_jacobian_default_rotate_vector 26 | from skrobot.model.joint import calc_jacobian_linear 27 | from skrobot.model.joint import calc_jacobian_rotational 28 | from skrobot.model.joint import calc_joint_angle_min_max_for_limit_calculation 29 | from skrobot.model.joint import calc_target_joint_dimension 30 | from skrobot.model.joint import calc_target_joint_dimension_from_link_list 31 | from skrobot.model.joint import joint_angle_limit_nspace 32 | from skrobot.model.joint import joint_angle_limit_weight 33 | 34 | from skrobot.model.robot_model import CascadedLink 35 | from skrobot.model.robot_model import RobotModel 36 | -------------------------------------------------------------------------------- /docs/source/_templates/autosummary/class.rst: -------------------------------------------------------------------------------- 1 | {{ fullname }} 2 | {{ underline }} 3 | 4 | .. currentmodule:: {{ module }} 5 | 6 | .. autoclass:: {{ objname }} 7 | 8 | .. 9 | Methods 10 | 11 | {% block methods %} 12 | 13 | .. rubric:: Methods 14 | 15 | .. 16 | Special methods 17 | 18 | {% for item in ('__call__', '__enter__', '__exit__', '__getitem__', '__setitem__', '__len__', '__next__', '__iter__', '__copy__') %} 19 | {% if item in all_methods %} 20 | .. automethod:: {{ item }} 21 | {% endif %} 22 | {%- endfor %} 23 | 24 | .. 25 | Ordinary methods 26 | 27 | {% for item in methods %} 28 | {% if item not in ('__init__',) %} 29 | .. automethod:: {{ item }} 30 | {% endif %} 31 | {%- endfor %} 32 | 33 | .. 34 | Special methods 35 | 36 | {% for item in ('__eq__', '__ne__', '__lt__', '__le__', '__gt__', '__ge__', '__nonzero__', '__bool__') %} 37 | {% if item in all_methods %} 38 | .. automethod:: {{ item }} 39 | {% endif %} 40 | {%- endfor %} 41 | 42 | .. 43 | Special (arithmetic) methods 44 | 45 | {% for item in ('__neg__', '__abs__', '__add__', '__radd__', '__sub__', '__rsub__', '__mul__', '__rmul__', '__div__', '__truediv__', '__rdiv__', '__rtruediv__', '__floordiv__', '__rfloordiv__', '__pow__', '__rpow__', '__matmul__', '__rmatmul__') %} 46 | {% if item in all_methods %} 47 | .. automethod:: {{ item }} 48 | {% endif %} 49 | {%- endfor %} 50 | 51 | {% endblock %} 52 | 53 | .. 54 | Attributes 55 | 56 | {% block attributes %} {% if attributes %} 57 | 58 | .. rubric:: Attributes 59 | 60 | {% for item in attributes %} 61 | .. autoattribute:: {{ item }} 62 | {%- endfor %} 63 | {% endif %} {% endblock %} 64 | -------------------------------------------------------------------------------- /docs/source/examples_guide/pr2_inverse_kinematics.rst: -------------------------------------------------------------------------------- 1 | ======================= 2 | PR2 Inverse Kinematics 3 | ======================= 4 | 5 | **Example script**: ``examples/pr2_inverse_kinematics.py`` 6 | 7 | This example demonstrates inverse kinematics solving with the PR2 robot, including the ``revert_if_fail`` parameter. 8 | 9 | Source Code 10 | =========== 11 | 12 | https://github.com/iory/scikit-robot/blob/main/examples/pr2_inverse_kinematics.py 13 | 14 | What This Example Shows 15 | ======================== 16 | 17 | - Basic IK solving for PR2 right arm 18 | - IK visualization during solving 19 | - Comparison of ``revert_if_fail=True`` vs ``False`` 20 | - Unreachable target handling 21 | 22 | Running the Example 23 | =================== 24 | 25 | .. code-block:: bash 26 | 27 | python examples/pr2_inverse_kinematics.py 28 | 29 | With different options: 30 | 31 | .. code-block:: bash 32 | 33 | # Use pyrender viewer 34 | python examples/pr2_inverse_kinematics.py --viewer pyrender 35 | 36 | # Disable IK visualization 37 | python examples/pr2_inverse_kinematics.py --no-ik-visualization 38 | 39 | # Skip revert_if_fail demonstration 40 | python examples/pr2_inverse_kinematics.py --skip-revert-demo 41 | 42 | Key Concepts 43 | ============ 44 | 45 | **revert_if_fail Parameter** 46 | 47 | - ``True`` (default): Reverts to original joint angles if IK fails 48 | - ``False``: Keeps partial progress even if target not fully reached 49 | 50 | This is useful when you want to get as close as possible to an unreachable target. 51 | 52 | Related Documentation 53 | ===================== 54 | 55 | - :doc:`../reference/robot_model_tips` - Comprehensive IK documentation 56 | -------------------------------------------------------------------------------- /skrobot/optimizer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | from skrobot.optimizers.cvxopt_solver import solve_qp as cvxopt_solve_qp 5 | from skrobot.optimizers.quadprog_solver import solve_qp as quadprog_solve_qp 6 | 7 | 8 | def solve_qp(P, q, G, h, A=None, b=None, solver='cvxopt', sym_proj=False): 9 | """n Solve a Quadratic Program defined as: 10 | 11 | .. math:: 12 | \\begin{eqnarray} 13 | \\mathrm{minimize} & & (1/2) x^T P x + q^T x \\\\ 14 | \\mathrm{subject\\ to} & & G x \\leq h \\\\ 15 | & & A x = b 16 | \\end{eqnarray} 17 | Parameters 18 | ---------- 19 | P : array, shape=(n, n) 20 | Primal quadratic cost matrix. 21 | q : array, shape=(n,) 22 | Primal quadratic cost vector. 23 | G : array, shape=(m, n) 24 | Linear inequality constraint matrix. 25 | h : array, shape=(m,) 26 | Linear inequality constraint vector. 27 | A : array, shape=(meq, n), optional 28 | Linear equality constraint matrix. 29 | b : array, shape=(meq,), optional 30 | Linear equality constraint vector. 31 | solver : string, optional 32 | Name of the QP solver to use (default is 'quadprog'). 33 | sym_proj : bool, optional 34 | Set to `True` when the `P` matrix provided is not symmetric. 35 | Returns 36 | ------- 37 | x : array, shape=(n,) 38 | Optimal solution to the QP, if found. 39 | Raises 40 | ------ 41 | ValueError 42 | If the QP is not feasible. 43 | """ 44 | if solver == 'cvxopt': 45 | return cvxopt_solve_qp(P, q, G, h, A, b, sym_proj=sym_proj) 46 | elif solver == 'quadprog': 47 | return quadprog_solve_qp(P, q, G, h, A, b, sym_proj=sym_proj) 48 | raise ValueError('QP solver {} not supported'.format(solver)) 49 | -------------------------------------------------------------------------------- /examples/signed_distance_functions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import time 5 | 6 | import numpy as np 7 | 8 | import skrobot 9 | from skrobot.coordinates.math import rotation_matrix_from_axis 10 | from skrobot.model import Axis 11 | from skrobot.model import Box 12 | from skrobot.model import MeshLink 13 | from skrobot.sdf import UnionSDF 14 | 15 | 16 | b = Box(extents=[0.05, 0.1, 0.05], with_sdf=True) 17 | m = MeshLink(visual_mesh=skrobot.data.bunny_objpath(), with_sdf=True) 18 | b.translate([0, 0.1, 0]) 19 | u = UnionSDF([b.sdf, m.sdf]) 20 | axis = Axis(axis_radius=0.001, axis_length=0.3) 21 | parser = argparse.ArgumentParser( 22 | description='Visualization signed distance function.') 23 | parser.add_argument( 24 | '--viewer', type=str, 25 | choices=['trimesh', 'pyrender'], default='trimesh', 26 | help='Choose the viewer type: trimesh or pyrender') 27 | parser.add_argument( 28 | '--no-interactive', 29 | action='store_true', 30 | help="Run in non-interactive mode (do not wait for user input)" 31 | ) 32 | args = parser.parse_args() 33 | 34 | if args.viewer == 'trimesh': 35 | viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480)) 36 | elif args.viewer == 'pyrender': 37 | viewer = skrobot.viewers.PyrenderViewer(resolution=(640, 480)) 38 | 39 | viewer.add(b) 40 | viewer.add(m) 41 | viewer.show() 42 | pts, sd_vals = u.surface_points() 43 | 44 | for _ in range(100): 45 | idx = np.random.randint(len(pts)) 46 | rot = rotation_matrix_from_axis(np.random.random(3), np.random.random(3)) 47 | ax = Axis(axis_radius=0.001, axis_length=0.01, pos=pts[idx], rot=rot) 48 | viewer.add(ax) 49 | 50 | print('==> Press [q] to close window') 51 | if not args.no_interactive: 52 | while viewer.is_active: 53 | time.sleep(0.1) 54 | viewer.redraw() 55 | viewer.close() 56 | time.sleep(1.0) 57 | -------------------------------------------------------------------------------- /tests/skrobot_tests/test_interpolator.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import numpy as np 4 | from numpy import testing 5 | 6 | from skrobot.coordinates.math import midpoint 7 | from skrobot.interpolator import LinearInterpolator 8 | from skrobot.interpolator import MinjerkInterpolator 9 | 10 | 11 | class TestInterpolator(unittest.TestCase): 12 | 13 | def test_linear_interpolator(self): 14 | ip = LinearInterpolator() 15 | p0 = np.array([1, 2, 3]) 16 | t0 = 0.10 17 | p1 = np.array([3, 4, 5]) 18 | t1 = 0.18 19 | ip.reset(position_list=[p0, p1, p0], 20 | time_list=[t0, t1]) 21 | ip.start_interpolation() 22 | 23 | i = 0 24 | while t0 > i: 25 | ip.pass_time(0.02) 26 | testing.assert_almost_equal(ip.position, midpoint(i / t0, p0, p1)) 27 | i += 0.02 28 | 29 | i = t0 30 | while t1 >= i: 31 | ip.pass_time(0.02) 32 | testing.assert_almost_equal( 33 | ip.position, 34 | midpoint((i - t0) / (t1 - t0), p1, p0)) 35 | i += 0.02 36 | 37 | assert ip.is_interpolating is False 38 | 39 | def test_minjerk_interpolator(self): 40 | ip = MinjerkInterpolator() 41 | p0 = np.array([1, 2, 3]) 42 | t0 = 0.10 43 | p1 = np.array([3, 4, 5]) 44 | t1 = 0.18 45 | ip.reset(position_list=[p0, p1, p0], 46 | time_list=[t0, t1]) 47 | ip.start_interpolation() 48 | 49 | i = 0 50 | while t0 >= i: 51 | ip.pass_time(0.02) 52 | i += 0.02 53 | testing.assert_almost_equal(ip.position, p1) 54 | 55 | i = t0 56 | while t1 >= i: 57 | ip.pass_time(0.02) 58 | i += 0.02 59 | testing.assert_almost_equal(ip.position, p0) 60 | 61 | assert ip.is_interpolating is False 62 | -------------------------------------------------------------------------------- /skrobot/_lazy_imports.py: -------------------------------------------------------------------------------- 1 | import importlib 2 | 3 | 4 | _trimesh = None 5 | 6 | 7 | def _lazy_trimesh(): 8 | global _trimesh 9 | if _trimesh is None: 10 | import trimesh 11 | _trimesh = trimesh 12 | return _trimesh 13 | 14 | 15 | _scipy = None 16 | 17 | 18 | def _lazy_scipy(): 19 | global _scipy 20 | if _scipy is None: 21 | import scipy 22 | _scipy = scipy 23 | return _scipy 24 | 25 | 26 | class LazyImportClass(object): 27 | 28 | def __init__(self, module_name, class_name, package): 29 | self._module_name = module_name 30 | self._class_name = class_name 31 | self._class = None 32 | self._package = package 33 | 34 | def __getattr__(self, attr): 35 | if self._class is None: 36 | try: 37 | module = importlib.import_module(self._module_name, 38 | package=self._package) 39 | self._class = getattr(module, self._class_name) 40 | except ImportError: 41 | self._class = None 42 | if self._class is None: 43 | raise AttributeError("Failed to load {}".format(self._class_name)) 44 | return getattr(self._class, attr) 45 | 46 | def __call__(self, *args, **kwargs): 47 | if self._class is None: 48 | try: 49 | module = importlib.import_module(self._module_name, 50 | package=self._package) 51 | self._class = getattr(module, self._class_name) 52 | except ImportError as e: 53 | raise AttributeError( 54 | "Failed to load {}. Error log: {}".format( 55 | self._class_name, e)) 56 | return self._class(*args, **kwargs) 57 | 58 | def __dir__(self): 59 | if self._class is None: 60 | return [] 61 | return dir(self._class) 62 | -------------------------------------------------------------------------------- /skrobot/optimizers/quadprog_solver.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from quadprog import solve_qp as _solve_qp 3 | 4 | 5 | def solve_qp(P, q, G, h, A=None, b=None, sym_proj=False): 6 | """Solve a Quadratic Program defined as: 7 | 8 | .. math:: 9 | \\begin{eqnarray} 10 | \\mathrm{minimize} & & (1/2) x^T P x + q^T x \\\\ 11 | \\mathrm{subject\\ to} & & G x \\leq h \\\\ 12 | & & A x = b 13 | \\end{eqnarray} 14 | using the `quadprog `_ QP 15 | solver, which implements the Goldfarb-Idnani dual algorithm 16 | [Goldfarb83]_. 17 | Parameters 18 | ---------- 19 | P : array, shape=(n, n) 20 | Symmetric quadratic-cost matrix. 21 | q : array, shape=(n,) 22 | Quadratic-cost vector. 23 | G : array, shape=(m, n) 24 | Linear inequality matrix. 25 | h : array, shape=(m,) 26 | Linear inequality vector. 27 | A : array, shape=(meq, n), optional 28 | Linear equality matrix. 29 | b : array, shape=(meq,), optional 30 | Linear equality vector. 31 | sym_proj : bool, optional 32 | Set to `True` when the `P` matrix provided is not symmetric. 33 | Returns 34 | ------- 35 | x : array, shape=(n,) 36 | Optimal solution to the QP, if found. 37 | Raises 38 | ------ 39 | ValueError 40 | If the QP is not feasible. 41 | Note 42 | ---- 43 | The quadprog solver assumes `P` is symmetric. If that is not the case, set 44 | `sym_proj=True` to project it on its symmetric part beforehand. 45 | """ 46 | if sym_proj: 47 | qp_G = .5 * (P + P.T) 48 | else: 49 | qp_G = P 50 | qp_a = -q 51 | if A is not None: 52 | qp_C = - np.vstack([A, G]).T 53 | qp_b = - np.hstack([b, h]) 54 | meq = A.shape[0] 55 | else: # no equality constraint 56 | qp_C = - G.T 57 | qp_b = - h 58 | meq = 0 59 | return _solve_qp(qp_G, qp_a, qp_C, qp_b, meq)[0] 60 | -------------------------------------------------------------------------------- /skrobot/interfaces/ros/panda.py: -------------------------------------------------------------------------------- 1 | import actionlib 2 | import control_msgs.msg 3 | import franka_gripper.msg 4 | 5 | from .base import ROSRobotInterfaceBase 6 | 7 | 8 | WIDTH_MAX = 0.08 9 | 10 | 11 | class PandaROSRobotInterface(ROSRobotInterfaceBase): 12 | 13 | def __init__(self, *args, **kwargs): 14 | super(PandaROSRobotInterface, self).__init__(*args, **kwargs) 15 | 16 | self.gripper_move = actionlib.SimpleActionClient( 17 | 'franka_gripper/move', 18 | franka_gripper.msg.MoveAction) 19 | self.gripper_move.wait_for_server() 20 | 21 | self.gripper_stop = actionlib.SimpleActionClient( 22 | 'franka_gripper/stop', 23 | franka_gripper.msg.StopAction) 24 | self.gripper_stop.wait_for_server() 25 | 26 | @property 27 | def rarm_controller(self): 28 | return dict( 29 | controller_type='rarm_controller', 30 | controller_action='position_joint_trajectory_controller/follow_joint_trajectory', # NOQA 31 | controller_state='position_joint_trajectory_controller/state', 32 | action_type=control_msgs.msg.FollowJointTrajectoryAction, 33 | joint_names=[j.name for j in self.robot.rarm.joint_list], 34 | ) 35 | 36 | def default_controller(self): 37 | return [self.rarm_controller] 38 | 39 | def grasp(self, width=0, **kwargs): 40 | self.move_gripper(width=width, **kwargs) 41 | 42 | def ungrasp(self, **kwargs): 43 | self.move_gripper(width=WIDTH_MAX, **kwargs) 44 | 45 | def move_gripper(self, width, speed=WIDTH_MAX, wait=True): 46 | goal = franka_gripper.msg.MoveGoal(width=width, speed=speed) 47 | if wait: 48 | self.gripper_move.send_goal_and_wait(goal) 49 | else: 50 | self.gripper_move.send_goal(goal) 51 | 52 | def stop_gripper(self, wait=True): 53 | goal = franka_gripper.msg.StopGoal() 54 | if wait: 55 | self.gripper_move.send_goal_and_wait(goal) 56 | else: 57 | self.gripper_move.send_goal(goal) 58 | -------------------------------------------------------------------------------- /examples/pybullet_robot_interface.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import sys 5 | import time 6 | 7 | import numpy as np 8 | 9 | import skrobot 10 | from skrobot.interfaces._pybullet import _check_available 11 | 12 | 13 | def main(): 14 | if _check_available() is False: 15 | sys.exit(0) 16 | import pybullet 17 | parser = argparse.ArgumentParser( 18 | description='Scikit-robot pybullet interface example.') 19 | parser.add_argument( 20 | '--no-interactive', 21 | action='store_true', 22 | help="Run in non-interactive mode (do not wait for user input)" 23 | ) 24 | args = parser.parse_args() 25 | # initialize robot 26 | robot = skrobot.models.Kuka() 27 | interface = skrobot.interfaces.PybulletRobotInterface(robot) 28 | pybullet.resetDebugVisualizerCamera( 29 | cameraDistance=1.5, 30 | cameraYaw=45, 31 | cameraPitch=-45, 32 | cameraTargetPosition=(0, 0, 0.5), 33 | ) 34 | print('==> Initialized Kuka Robot on PyBullet') 35 | for _ in range(100): 36 | pybullet.stepSimulation() 37 | time.sleep(3) 38 | 39 | # reset pose 40 | print('==> Moving to Reset Pose') 41 | robot.reset_manip_pose() 42 | interface.angle_vector(robot.angle_vector(), realtime_simulation=True) 43 | interface.wait_interpolation() 44 | time.sleep(3) 45 | 46 | # ik 47 | print('==> Solving Inverse Kinematics') 48 | target_coords = skrobot.coordinates.Coordinates( 49 | pos=[0.5, 0, 0] 50 | ).rotate(np.pi / 2.0, 'y', 'local') 51 | skrobot.interfaces.pybullet.draw(target_coords) 52 | robot.inverse_kinematics( 53 | target_coords, 54 | link_list=robot.rarm.link_list, 55 | move_target=robot.rarm_end_coords, 56 | rotation_axis=True, 57 | stop=1000, 58 | ) 59 | interface.angle_vector(robot.angle_vector(), realtime_simulation=True) 60 | interface.wait_interpolation() 61 | 62 | if not args.no_interactive: 63 | # wait 64 | while pybullet.isConnected(): 65 | time.sleep(0.01) 66 | 67 | 68 | if __name__ == '__main__': 69 | main() 70 | -------------------------------------------------------------------------------- /examples/robot_models.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import time 5 | 6 | import numpy as np 7 | 8 | import skrobot 9 | 10 | 11 | def _get_tile_shape(num, hw_ratio=1): 12 | r_num = int(round(np.sqrt(num / hw_ratio))) # weighted by wh_ratio 13 | c_num = 0 14 | while r_num * c_num < num: 15 | c_num += 1 16 | while (r_num - 1) * c_num >= num: 17 | r_num -= 1 18 | return r_num, c_num 19 | 20 | 21 | def main(): 22 | parser = argparse.ArgumentParser( 23 | description='Set viewer for skrobot.') 24 | parser.add_argument( 25 | '--viewer', type=str, 26 | choices=['trimesh', 'pyrender'], default='pyrender', 27 | help='Choose the viewer type: trimesh or pyrender') 28 | parser.add_argument( 29 | '--no-interactive', 30 | action='store_true', 31 | help="Run in non-interactive mode (do not wait for user input)" 32 | ) 33 | args = parser.parse_args() 34 | 35 | if args.viewer == 'trimesh': 36 | viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480)) 37 | elif args.viewer == 'pyrender': 38 | viewer = skrobot.viewers.PyrenderViewer(resolution=(640, 480)) 39 | 40 | robots = [ 41 | skrobot.models.Kuka(), 42 | skrobot.models.Fetch(), 43 | skrobot.models.Nextage(), 44 | skrobot.models.PR2(), 45 | skrobot.models.Panda(), 46 | ] 47 | nrow, ncol = _get_tile_shape(len(robots)) 48 | row, col = 2, 2 49 | 50 | for i in range(nrow): 51 | for j in range(ncol): 52 | try: 53 | robot = robots[i * ncol + j] 54 | except IndexError: 55 | break 56 | plane = skrobot.model.Box(extents=(row - 0.01, col - 0.01, 0.01)) 57 | plane.translate((row * i, col * j, -0.01)) 58 | viewer.add(plane) 59 | robot.translate((row * i, col * j, 0)) 60 | viewer.add(robot) 61 | 62 | viewer.set_camera(angles=[np.deg2rad(30), 0, 0]) 63 | viewer.show() 64 | 65 | if not args.no_interactive: 66 | print('==> Press [q] to close window') 67 | while viewer.is_active: 68 | time.sleep(0.1) 69 | viewer.redraw() 70 | viewer.close() 71 | time.sleep(1.0) 72 | 73 | 74 | if __name__ == '__main__': 75 | main() 76 | -------------------------------------------------------------------------------- /docs/source/_static/css/modified_theme.css: -------------------------------------------------------------------------------- 1 | @import url('theme.css'); 2 | 3 | /* Main background color modification */ 4 | .btn-info, .wy-menu-vertical a:active, .wy-side-nav-search, .wy-side-nav-search img, .wy-nav .wy-menu-vertical a:hover, .wy-nav-top img, .wy-tray-item-info, .wy-side-nav-search, .wy-dropdown-menu > dd > a:hover, .wy-dropdown-menu a:hover, .wy-nav-top { 5 | background-color: #d17062 !important; 6 | } 7 | 8 | .wy-side-nav-search input[type=text] { 9 | border-color: #a47224 !important; 10 | } 11 | 12 | .rst-content .note .admonition-title { 13 | background-color: #c9c9c9 !important; 14 | } 15 | 16 | .rst-content .note { 17 | background-color: #e3e3e3 !important; 18 | } 19 | 20 | 21 | /* deprecation box */ 22 | .rst-content .deprecated { 23 | background-color: #ffedcc !important; 24 | padding: 0 1em 0.1em 1em !important; 25 | } 26 | 27 | .rst-content .deprecated .versionmodified { 28 | /* title text */ 29 | display: block !important; 30 | background-color: #f0b37e !important; 31 | color: #ffffff !important; 32 | font-weight: bold !important; 33 | 34 | padding: 0.1em 1em 0.1em 1em !important; 35 | margin: 0 -1em 0.5em -1em !important; 36 | } 37 | 38 | 39 | /* Avoid horizontal scroll of tables */ 40 | .rst-content table.docutils td { 41 | white-space: normal; 42 | } 43 | 44 | 45 | /* code style */ 46 | .rst-content table.longtable.docutils code { 47 | font-size: 100% !important; 48 | background-color: transparent !important; 49 | border: none !important; 50 | } 51 | 52 | .rst-content a.reference code { 53 | color: #3399cc !important; 54 | } 55 | 56 | .rst-content a.reference code:hover { 57 | text-decoration: underline !important; 58 | color: #3366cc !important; 59 | } 60 | 61 | 62 | /* rubric */ 63 | .rst-content .class .rubric { 64 | color: #333333; 65 | background-color: #aaeeaa; 66 | padding: 0.2em 0 0.2em 1em; 67 | border-top: solid 0.3em #66cc66; 68 | } 69 | 70 | /* External links in menus */ 71 | .wy-menu a.external:after { 72 | font-family: "FontAwesome"; 73 | font-size: 0.7em; 74 | vertical-align: top; 75 | content: " \f0a9"; 76 | } 77 | 78 | /* External links in content */ 79 | .rst-content a.external:after { 80 | font-family: "FontAwesome"; 81 | font-size: 0.7em; 82 | vertical-align: top; 83 | content: " \f0a9"; 84 | } 85 | -------------------------------------------------------------------------------- /skrobot/apps/visualize_urdf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import os.path as osp 5 | import time 6 | 7 | import skrobot 8 | from skrobot.model import RobotModel 9 | 10 | 11 | def main(): 12 | parser = argparse.ArgumentParser(description='Visualize URDF') 13 | parser.add_argument('input_urdfpath', type=str, nargs='?', help='Input URDF path') 14 | parser.add_argument( 15 | '--viewer', type=str, 16 | choices=['trimesh', 'pyrender'], default='pyrender', 17 | help='Choose the viewer type: trimesh or pyrender') 18 | parser.add_argument( 19 | '--interactive', '-i', 20 | action='store_true', 21 | help='enter interactive shell' 22 | ) 23 | parser.add_argument( 24 | '--ros', type=str, nargs='?', const='/robot_description', default=None, 25 | help='Load URDF from ROS parameter server (specify parameter name, default: /robot_description)' 26 | ) 27 | args = parser.parse_args() 28 | 29 | # Validate arguments 30 | if args.ros is not None and args.input_urdfpath is not None: 31 | parser.error("Cannot specify both URDF file path and --ros option") 32 | if args.ros is None and args.input_urdfpath is None: 33 | parser.error("Must specify either URDF file path or --ros option") 34 | 35 | if args.viewer == 'trimesh': 36 | viewer = skrobot.viewers.TrimeshSceneViewer(update_interval=0.1) 37 | elif args.viewer == 'pyrender': 38 | viewer = skrobot.viewers.PyrenderViewer(update_interval=0.1) 39 | 40 | # Load robot model from ROS parameter or file 41 | if args.ros is not None: 42 | robot_model = RobotModel.from_robot_description(args.ros) 43 | else: 44 | if not osp.exists(args.input_urdfpath): 45 | parser.error(f"URDF file not found: {args.input_urdfpath}") 46 | robot_model = RobotModel.from_urdf(osp.abspath(args.input_urdfpath)) 47 | 48 | viewer.add(robot_model) 49 | viewer.show() 50 | if args.interactive: 51 | try: 52 | import IPython 53 | except Exception as e: 54 | print("IPython is not installed. {}".format(e)) 55 | return 56 | IPython.embed() 57 | else: 58 | while viewer.is_active: 59 | viewer.redraw() 60 | time.sleep(0.1) 61 | viewer.close() 62 | 63 | 64 | if __name__ == '__main__': 65 | main() 66 | -------------------------------------------------------------------------------- /skrobot/viewers/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | import inspect 4 | 5 | 6 | def warn_gl(error_log): 7 | if 'Library "GL" not found.' in str(error_log): 8 | print( 9 | '\x1b[31m' # red 10 | + 'Library "GL" not found. Please install it by running:\n' 11 | + 'sudo apt-get install freeglut3-dev' 12 | + '\x1b[39m' # reset 13 | ) 14 | 15 | 16 | class DummyViewer(object): 17 | def __init__(self, *args, **kwargs): 18 | self.has_exit = True 19 | def redraw(self): 20 | pass 21 | def show(self): 22 | pass 23 | def add(self, *args, **kwargs): 24 | pass 25 | def delete(self, *args, **kwargs): 26 | pass 27 | def set_camera(self, *args, **kwargs): 28 | pass 29 | def save_image(self, file_obj): 30 | pass 31 | 32 | try: 33 | from ._trimesh import TrimeshSceneViewer 34 | except TypeError: 35 | # trimesh.viewer.SceneViewer can have function type. 36 | class TrimeshSceneViewer(object): 37 | def __init__(self, *args, **kwargs): 38 | raise RuntimeError('TrimeshSceneViewer cannot be initialized. ' 39 | 'This issue happens when the X window system ' 40 | 'is not running.') 41 | except ImportError as error_log: 42 | warn_gl(error_log) 43 | class TrimeshSceneViewer(DummyViewer): 44 | pass 45 | 46 | try: 47 | from ._pyrender import PyrenderViewer 48 | import pyrender 49 | args = inspect.getfullargspec(pyrender.Viewer.__init__).args 50 | if 'auto_start' not in args: 51 | class PyrenderViewer(object): 52 | def __init__(self, *args, **kwargs): 53 | raise RuntimeError( 54 | 'The correct version of pyrender is not installed.\n' 55 | 'To install the appropriate version of pyrender, ' 56 | 'please execute the following command:\n' 57 | 'pip uninstall -y pyrender && pip install scikit-robot-pyrender --no-cache-dir' 58 | ) 59 | except ImportError as error_log: 60 | warn_gl(error_log) 61 | class PyrenderViewer(DummyViewer): 62 | pass 63 | 64 | try: 65 | from ._notebook import JupyterNotebookViewer 66 | except ImportError: 67 | class JupyterNotebookViewer(DummyViewer): 68 | pass 69 | 70 | try: 71 | from ._viser import ViserVisualizer 72 | except ImportError: 73 | class ViserVisualizer(DummyViewer): 74 | pass 75 | -------------------------------------------------------------------------------- /skrobot/__init__.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | 3 | import sys 4 | 5 | 6 | if (sys.version_info[0] == 3 and sys.version_info[1] >= 7) \ 7 | or sys.version_info[0] > 3: 8 | import importlib 9 | import importlib.metadata 10 | _SUBMODULES = [ 11 | "coordinates", 12 | "data", 13 | "interpolator", 14 | "planner", 15 | "interfaces", 16 | "model", 17 | "models", 18 | "viewers", 19 | "utils", 20 | "sdf", 21 | "urdf" 22 | ] 23 | __all__ = _SUBMODULES 24 | _version = None 25 | 26 | def determine_version(module_name): 27 | return importlib.metadata.version(module_name) 28 | 29 | 30 | class LazyModule(object): 31 | def __init__(self, name): 32 | self.__name__ = "skrobot." + name 33 | self._name = name 34 | self._module = None 35 | 36 | def __getattr__(self, attr): 37 | if self._module is None: 38 | self._module = importlib.import_module("skrobot." + self._name) 39 | return getattr(self._module, attr) 40 | 41 | def __dir__(self): 42 | if self._module is None: 43 | return ["__all__"] 44 | return dir(self._module) 45 | 46 | 47 | _module_cache = {} 48 | for submodule in _SUBMODULES: 49 | _module_cache[submodule] = LazyModule(submodule) 50 | 51 | 52 | def __getattr__(name): 53 | global _version 54 | if name == "__version__": 55 | if _version is None: 56 | _version = determine_version('scikit-robot') 57 | return _version 58 | if name in _SUBMODULES: 59 | return _module_cache[name] 60 | raise AttributeError( 61 | "module {} has no attribute {}".format(__name__, name)) 62 | 63 | 64 | def __dir__(): 65 | return __all__ + ['__version__'] 66 | else: 67 | import pkg_resources 68 | 69 | 70 | def determine_version(module_name): 71 | return pkg_resources.get_distribution(module_name).version 72 | 73 | __version__ = determine_version('scikit-robot') 74 | 75 | 76 | from skrobot import coordinates 77 | from skrobot import data 78 | from skrobot import interpolator 79 | from skrobot import planner 80 | from skrobot import interfaces 81 | from skrobot import model 82 | from skrobot import models 83 | from skrobot import viewers 84 | from skrobot import utils 85 | from skrobot import sdf 86 | from skrobot import urdf 87 | -------------------------------------------------------------------------------- /skrobot/models/kuka.py: -------------------------------------------------------------------------------- 1 | from cached_property import cached_property 2 | import numpy as np 3 | 4 | from skrobot.coordinates import CascadedCoords 5 | from skrobot.data import kuka_urdfpath 6 | from skrobot.model import RobotModel 7 | 8 | from .urdf import RobotModelFromURDF 9 | 10 | 11 | class Kuka(RobotModelFromURDF): 12 | """Kuka Robot Model.""" 13 | 14 | def __init__(self, *args, **kwargs): 15 | super(Kuka, self).__init__(*args, **kwargs) 16 | self.rarm_end_coords = CascadedCoords( 17 | parent=self.lbr_iiwa_with_wsg50__lbr_iiwa_link_7, 18 | name='rarm_end_coords') 19 | self.rarm_end_coords.translate( 20 | np.array([0.0, 0.030, 0.250], dtype=np.float32)) 21 | self.rarm_end_coords.rotate(- np.pi / 2.0, axis='y') 22 | self.rarm_end_coords.rotate(- np.pi / 2.0, axis='x') 23 | self.end_coords = [self.rarm_end_coords] 24 | 25 | @cached_property 26 | def default_urdf_path(self): 27 | return kuka_urdfpath() 28 | 29 | def reset_manip_pose(self): 30 | return self.angle_vector([0, np.deg2rad(10), 0, 31 | np.deg2rad(-90), 0, np.deg2rad(90), 32 | 0, 0, 0, 0, 0, 0]) 33 | 34 | @cached_property 35 | def rarm(self): 36 | rarm_links = [self.lbr_iiwa_with_wsg50__lbr_iiwa_link_1, 37 | self.lbr_iiwa_with_wsg50__lbr_iiwa_link_2, 38 | self.lbr_iiwa_with_wsg50__lbr_iiwa_link_3, 39 | self.lbr_iiwa_with_wsg50__lbr_iiwa_link_4, 40 | self.lbr_iiwa_with_wsg50__lbr_iiwa_link_5, 41 | self.lbr_iiwa_with_wsg50__lbr_iiwa_link_6, 42 | self.lbr_iiwa_with_wsg50__lbr_iiwa_link_7] 43 | rarm_joints = [] 44 | for link in rarm_links: 45 | if hasattr(link, 'joint'): 46 | rarm_joints.append(link.joint) 47 | r = RobotModel(link_list=rarm_links, 48 | joint_list=rarm_joints) 49 | r.end_coords = self.rarm_end_coords 50 | return r 51 | 52 | def close_hand(self, av=None): 53 | if av is None: 54 | av = self.angle_vector() 55 | av[-2] = 0 56 | av[-4] = 0 57 | return self.angle_vector(av) 58 | 59 | def open_hand(self, default_angle=np.deg2rad(10), av=None): 60 | if av is None: 61 | av = self.angle_vector() 62 | av[-2] = default_angle 63 | av[-4] = -default_angle 64 | return self.angle_vector(av) 65 | -------------------------------------------------------------------------------- /tests/skrobot_tests/coordinates_tests/test_geo.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import numpy as np 4 | from numpy import pi 5 | from numpy import testing 6 | 7 | from skrobot.coordinates import make_coords 8 | from skrobot.coordinates.geo import midcoords 9 | from skrobot.coordinates.geo import orient_coords_to_axis 10 | from skrobot.coordinates.geo import rotate_points 11 | from skrobot.coordinates.math import matrix2quaternion 12 | 13 | 14 | class TestGeo(unittest.TestCase): 15 | 16 | def test_midcoords(self): 17 | a = make_coords(pos=[1.0, 1.0, 1.0]) 18 | b = make_coords() 19 | c = midcoords(0.5, a, b) 20 | testing.assert_array_equal(c.worldpos(), 21 | [0.5, 0.5, 0.5]) 22 | testing.assert_array_equal(matrix2quaternion(c.worldrot()), 23 | [1, 0, 0, 0]) 24 | 25 | def test_orient_coords_to_axis(self): 26 | target_coords = make_coords(pos=[1.0, 1.0, 1.0]) 27 | orient_coords_to_axis(target_coords, [0, 1, 0]) 28 | 29 | testing.assert_array_equal(target_coords.worldpos(), 30 | [1, 1, 1]) 31 | from skrobot.coordinates.math import matrix2ypr 32 | testing.assert_array_almost_equal( 33 | matrix2ypr(target_coords.rotation), 34 | [0, 0, -1.57079633]) 35 | 36 | # case of rot_angle_cos == 1.0 37 | target_coords = make_coords(pos=[1.0, 1.0, 1.0]) 38 | orient_coords_to_axis(target_coords, [0, 0, 1]) 39 | 40 | testing.assert_array_equal(target_coords.worldpos(), 41 | [1, 1, 1]) 42 | testing.assert_array_almost_equal( 43 | matrix2ypr(target_coords.rotation), 44 | [0, 0, 0]) 45 | 46 | # case of rot_angle_cos == -1.0 47 | target_coords = make_coords() 48 | orient_coords_to_axis(target_coords, [0, 0, -1]) 49 | 50 | testing.assert_array_equal(target_coords.worldpos(), 51 | [0, 0, 0]) 52 | testing.assert_array_almost_equal( 53 | matrix2ypr(target_coords.rotation), 54 | [0, 0, pi]) 55 | 56 | def test_rotate_points(self): 57 | points = np.array([1, 0, 0]) 58 | rot_points = rotate_points(points, [1, 0, 0], [0, 0, 1]) 59 | testing.assert_almost_equal(rot_points, [[0, 0, 1]]) 60 | 61 | points = np.array([[1, 0, 0]]) 62 | rot_points = rotate_points(points, [1, 0, 0], [0, 0, 1]) 63 | testing.assert_almost_equal(rot_points, [[0, 0, 1]]) 64 | 65 | points = np.array([[1, 0, 0]]) 66 | rot_points = rotate_points(points, [0, 0, 1], [0, 0, 1]) 67 | testing.assert_almost_equal(rot_points, [[1, 0, 0]]) 68 | -------------------------------------------------------------------------------- /examples/trimesh_scene_viewer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import time 5 | 6 | import numpy as np 7 | 8 | import skrobot 9 | 10 | 11 | def main(): 12 | parser = argparse.ArgumentParser( 13 | formatter_class=argparse.ArgumentDefaultsHelpFormatter, 14 | ) 15 | parser.add_argument( 16 | '--interactive', 17 | action='store_true', 18 | help='enter interactive shell' 19 | ) 20 | parser.add_argument( 21 | '--no-interactive', 22 | action='store_true', 23 | help="Run in non-interactive mode (do not wait for user input)" 24 | ) 25 | args = parser.parse_args() 26 | 27 | robot = skrobot.models.Kuka() 28 | 29 | viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480)) 30 | 31 | # base plane 32 | plane = skrobot.model.Box( 33 | extents=(2, 2, 0.01), face_colors=(0.75, 0.75, 0.75) 34 | ) 35 | viewer.add(plane) 36 | 37 | viewer.add(robot) 38 | 39 | viewer.set_camera(angles=[np.deg2rad(45), 0, 0], distance=4) 40 | 41 | viewer.show() 42 | 43 | box = skrobot.model.Box( 44 | extents=(0.05, 0.05, 0.05), face_colors=(1., 0, 0) 45 | ) 46 | box.translate((0.5, 0, 0.3)) 47 | viewer.add(box) 48 | 49 | if args.interactive: 50 | print('''\ 51 | >>> # Usage 52 | 53 | >>> robot.reset_manip_pose() 54 | >>> viewer.redraw() 55 | >>> robot.init_pose() 56 | >>> robot.inverse_kinematics(box, rotation_axis='y') 57 | ''') 58 | 59 | import IPython 60 | 61 | IPython.embed() 62 | else: 63 | print('==> Waiting 3 seconds') 64 | time.sleep(3) 65 | 66 | print('==> Moving to reset_manip_pose') 67 | robot.reset_manip_pose() 68 | print(robot.angle_vector()) 69 | time.sleep(1) 70 | viewer.redraw() 71 | 72 | print('==> Waiting 3 seconds') 73 | time.sleep(3) 74 | 75 | print('==> Moving to init_pose') 76 | robot.init_pose() 77 | print(robot.angle_vector()) 78 | time.sleep(1) 79 | viewer.redraw() 80 | 81 | print('==> Waiting 3 seconds') 82 | time.sleep(3) 83 | 84 | print('==> IK to box') 85 | robot.reset_manip_pose() 86 | robot.inverse_kinematics(box, rotation_axis='y') 87 | print(robot.angle_vector()) 88 | time.sleep(1) 89 | viewer.redraw() 90 | 91 | if not args.no_interactive: 92 | print('==> Press [q] to close window') 93 | while viewer.is_active: 94 | time.sleep(0.1) 95 | viewer.redraw() 96 | viewer.close() 97 | time.sleep(1.0) 98 | 99 | 100 | if __name__ == '__main__': 101 | main() 102 | -------------------------------------------------------------------------------- /skrobot/apps/visualize_mesh.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import os.path as osp 5 | import time 6 | 7 | import numpy as np 8 | 9 | import skrobot 10 | from skrobot.model import Axis 11 | from skrobot.utils.urdf import load_meshes 12 | 13 | 14 | def main(): 15 | parser = argparse.ArgumentParser(description='Visualize Meshes and Origin Coordinate System') 16 | parser.add_argument('input_mesh_paths', type=str, nargs='+', 17 | help='Input mesh paths (e.g., .stl, .3dxml, .dae, ...)') 18 | parser.add_argument( 19 | '--axis_length', type=float, default=1, 20 | help='Length of the coordinate system axes. Default is 1.') 21 | parser.add_argument( 22 | '--axis_radius', type=float, default=0.01, 23 | help='Radius (thickness) of the coordinate system axes. Default is 0.01.') 24 | parser.add_argument( 25 | '--viewer', type=str, 26 | choices=['trimesh', 'pyrender'], default='pyrender', 27 | help='Choose the viewer type: trimesh or pyrender') 28 | parser.add_argument( 29 | '--interactive', '-i', 30 | action='store_true', 31 | help='enter interactive shell' 32 | ) 33 | args = parser.parse_args() 34 | 35 | if args.viewer == 'trimesh': 36 | viewer = skrobot.viewers.TrimeshSceneViewer() 37 | elif args.viewer == 'pyrender': 38 | viewer = skrobot.viewers.PyrenderViewer() 39 | loaded_count = 0 40 | for mesh_path in args.input_mesh_paths: 41 | if not osp.exists(mesh_path): 42 | print(f"Warning: Mesh file not found at {mesh_path}. Skipping.") 43 | continue 44 | try: 45 | print(mesh_path) 46 | mesh = load_meshes(osp.abspath(mesh_path)) 47 | print(mesh) 48 | link = skrobot.model.Link(collision_mesh=mesh, visual_mesh=mesh) 49 | viewer.add(link) 50 | loaded_count += 1 51 | except Exception as e: 52 | print(f"Error loading mesh {mesh_path}: {e}. Skipping.") 53 | 54 | if loaded_count == 0: 55 | print("Error: No mesh files could be loaded. Exiting.") 56 | return 57 | 58 | axis = Axis(pos=np.array([0, 0, 0]), 59 | rot=np.eye(3), 60 | axis_length=args.axis_length, 61 | axis_radius=args.axis_radius) 62 | viewer.add(axis) 63 | viewer.show() 64 | if args.interactive: 65 | try: 66 | import IPython 67 | except Exception as e: 68 | print("IPython is not installed. {}".format(e)) 69 | return 70 | IPython.embed() 71 | else: 72 | while viewer.is_active: 73 | viewer.redraw() 74 | time.sleep(0.1) 75 | viewer.close() 76 | 77 | 78 | if __name__ == '__main__': 79 | main() 80 | -------------------------------------------------------------------------------- /examples/swept_sphere.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import time 4 | 5 | import skrobot 6 | from skrobot.model import Box 7 | from skrobot.planner import SweptSphereSdfCollisionChecker 8 | from skrobot.planner.utils import set_robot_config 9 | 10 | 11 | try: 12 | robot_model # noqa 13 | except: # noqa 14 | robot_model = skrobot.models.PR2() 15 | table = Box(extents=[0.7, 1.0, 0.05], with_sdf=True) 16 | table.translate([0.8, 0.0, 0.655]) 17 | 18 | parser = argparse.ArgumentParser( 19 | description='Swept spheres visualization.') 20 | parser.add_argument( 21 | '--viewer', type=str, 22 | choices=['trimesh', 'pyrender'], default='trimesh', 23 | help='Choose the viewer type: trimesh or pyrender') 24 | parser.add_argument( 25 | '--no-interactive', 26 | action='store_true', 27 | help="Run in non-interactive mode (do not wait for user input)" 28 | ) 29 | args = parser.parse_args() 30 | 31 | if args.viewer == 'trimesh': 32 | viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480)) 33 | elif args.viewer == 'pyrender': 34 | viewer = skrobot.viewers.PyrenderViewer(resolution=(640, 480)) 35 | 36 | link_idx_table = {} 37 | for link_idx in range(len(robot_model.link_list)): 38 | name = robot_model.link_list[link_idx].name 39 | link_idx_table[name] = link_idx 40 | 41 | coll_link_names = [ 42 | "r_upper_arm_link", 43 | "r_forearm_link", 44 | "r_gripper_palm_link", 45 | "r_gripper_r_finger_link", 46 | "r_gripper_l_finger_link"] 47 | 48 | coll_link_list = [robot_model.link_list[link_idx_table[lname]] 49 | for lname in coll_link_names] 50 | 51 | move_link_names = [ 52 | "r_shoulder_pan_link", 53 | "r_shoulder_lift_link", 54 | "r_upper_arm_roll_link", 55 | "r_elbow_flex_link", 56 | "r_forearm_roll_link", 57 | "r_wrist_flex_link", 58 | "r_wrist_roll_link"] 59 | link_list = [robot_model.link_list[link_idx_table[lname]] 60 | for lname in move_link_names] 61 | joint_list = [link.joint for link in link_list] 62 | 63 | sscc = SweptSphereSdfCollisionChecker(table.sdf, robot_model) 64 | for lname in coll_link_names: 65 | link = robot_model.link_list[link_idx_table[lname]] 66 | sscc.add_collision_link(link) 67 | 68 | with_base = True 69 | av = [0.4, 0.6] + [-0.7] * 5 + [0.1, 0.0, 0.3] 70 | set_robot_config(robot_model, joint_list, av, with_base=with_base) 71 | dists = sscc.update_color() 72 | sscc.add_coll_spheres_to_viewer(viewer) 73 | viewer.add(robot_model) 74 | viewer.add(table) 75 | viewer.show() 76 | 77 | print('==> Press [q] to close window') 78 | if not args.no_interactive: 79 | while viewer.is_active: 80 | time.sleep(0.1) 81 | viewer.redraw() 82 | viewer.close() 83 | time.sleep(1.0) 84 | -------------------------------------------------------------------------------- /tests/skrobot_tests/interfaces_tests/ros/test_tf_utils.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from numpy import testing 4 | 5 | from skrobot.coordinates import Coordinates 6 | 7 | 8 | try: 9 | ModuleNotFoundError 10 | except NameError: 11 | ModuleNotFoundError = ImportError 12 | try: 13 | import geometry_msgs.msg 14 | 15 | from skrobot.interfaces.ros.tf_utils import coords_to_geometry_pose 16 | from skrobot.interfaces.ros.tf_utils import coords_to_tf_pose 17 | from skrobot.interfaces.ros.tf_utils import geometry_pose_to_coords 18 | from skrobot.interfaces.ros.tf_utils import tf_pose_to_coords 19 | _ros_available = True 20 | except ModuleNotFoundError: 21 | _ros_available = False 22 | 23 | 24 | class TestTFUtils(unittest.TestCase): 25 | 26 | @unittest.skipUnless(_ros_available, 'ROS is not available.') 27 | def test_coords_to_geometry_pose(self): 28 | c = Coordinates(pos=(1, 2, 3)) 29 | pose = coords_to_geometry_pose(c) 30 | testing.assert_equal( 31 | [pose.position.x, pose.position.y, pose.position.z], (1, 2, 3)) 32 | testing.assert_equal( 33 | [pose.orientation.w, pose.orientation.x, 34 | pose.orientation.y, pose.orientation.z], (1, 0, 0, 0)) 35 | 36 | @unittest.skipUnless(_ros_available, 'ROS is not available.') 37 | def test_coords_to_tf_pose(self): 38 | c = Coordinates(pos=(1, 2, 3)) 39 | pose = coords_to_tf_pose(c) 40 | testing.assert_equal( 41 | [pose.translation.x, pose.translation.y, pose.translation.z], 42 | (1, 2, 3)) 43 | testing.assert_equal( 44 | [pose.rotation.w, pose.rotation.x, 45 | pose.rotation.y, pose.rotation.z], (1, 0, 0, 0)) 46 | 47 | @unittest.skipUnless(_ros_available, 'ROS is not available.') 48 | def test_tf_pose_to_coords(self): 49 | pose = geometry_msgs.msg.Transform() 50 | c = tf_pose_to_coords(pose) 51 | testing.assert_equal(c.translation, (0, 0, 0)) 52 | testing.assert_equal(c.quaternion, (1, 0, 0, 0)) 53 | 54 | pose_stamped = geometry_msgs.msg.TransformStamped() 55 | c = tf_pose_to_coords(pose_stamped) 56 | testing.assert_equal(c.translation, (0, 0, 0)) 57 | testing.assert_equal(c.quaternion, (1, 0, 0, 0)) 58 | 59 | @unittest.skipUnless(_ros_available, 'ROS is not available.') 60 | def test_geometry_pose_to_coords(self): 61 | pose = geometry_msgs.msg.Pose() 62 | c = geometry_pose_to_coords(pose) 63 | testing.assert_equal(c.translation, (0, 0, 0)) 64 | testing.assert_equal(c.quaternion, (1, 0, 0, 0)) 65 | 66 | pose_stamped = geometry_msgs.msg.Pose() 67 | c = geometry_pose_to_coords(pose_stamped) 68 | testing.assert_equal(c.translation, (0, 0, 0)) 69 | testing.assert_equal(c.quaternion, (1, 0, 0, 0)) 70 | -------------------------------------------------------------------------------- /docs/source/reference/functions.rst: -------------------------------------------------------------------------------- 1 | Functions 2 | ========= 3 | 4 | .. module:: skrobot.coordinates.math 5 | 6 | Utilities functions 7 | ------------------- 8 | 9 | .. autosummary:: 10 | :toctree: generated/ 11 | :nosignatures: 12 | 13 | convert_to_axis_vector 14 | _check_valid_rotation 15 | _check_valid_translation 16 | triple_product 17 | inverse_rodrigues 18 | rotation_angle 19 | make_matrix 20 | random_rotation 21 | random_translation 22 | midpoint 23 | interpolate_rotation_matrices 24 | transform 25 | rotation_matrix 26 | rotate_vector 27 | rotate_matrix 28 | rpy_matrix 29 | rpy_angle 30 | normalize_vector 31 | rotation_matrix_to_axis_angle_vector 32 | axis_angle_vector_to_rotation_matrix 33 | skew_symmetric_matrix 34 | rotation_matrix_from_rpy 35 | rotation_matrix_from_axis 36 | rodrigues 37 | rotation_angle 38 | rotation_distance 39 | axis_angle_from_matrix 40 | angle_between_vectors 41 | matrix2ypr 42 | matrix2rpy 43 | ypr2matrix 44 | rpy2matrix 45 | 46 | 47 | Jacobian Functions 48 | ------------------ 49 | 50 | .. autosummary:: 51 | :toctree: generated/ 52 | :nosignatures: 53 | 54 | sr_inverse 55 | sr_inverse_org 56 | manipulability 57 | 58 | 59 | Quaternion Functions 60 | -------------------- 61 | 62 | .. autosummary:: 63 | :toctree: generated/ 64 | :nosignatures: 65 | 66 | xyzw2wxyz 67 | wxyz2xyzw 68 | random_quaternion 69 | quaternion_multiply 70 | quaternion_conjugate 71 | quaternion_inverse 72 | quaternion_slerp 73 | quaternion_distance 74 | quaternion_absolute_distance 75 | quaternion_norm 76 | quaternion_normalize 77 | matrix2quaternion 78 | quaternion2matrix 79 | quaternion2rpy 80 | rpy2quaternion 81 | rpy_from_quat 82 | quat_from_rotation_matrix 83 | quat_from_rpy 84 | rotation_matrix_from_quat 85 | quaternion_from_axis_angle 86 | axis_angle_from_quaternion 87 | 88 | 89 | .. module:: skrobot.coordinates.geo 90 | 91 | Geometry functions 92 | ------------------ 93 | 94 | .. autosummary:: 95 | :toctree: generated/ 96 | :nosignatures: 97 | 98 | rotate_points 99 | 100 | 101 | Deprecated Functions 102 | -------------------- 103 | 104 | These functions are deprecated and will be removed in future versions. 105 | Please use their replacements instead. 106 | 107 | **Deprecated Functions:** 108 | 109 | - :func:`~skrobot.coordinates.math.matrix_log` → Use :func:`~skrobot.coordinates.math.rotation_matrix_to_axis_angle_vector` instead 110 | - :func:`~skrobot.coordinates.math.matrix_exponent` → Use :func:`~skrobot.coordinates.math.axis_angle_vector_to_rotation_matrix` instead 111 | - :func:`~skrobot.coordinates.math.midrot` → Use :func:`~skrobot.coordinates.math.interpolate_rotation_matrices` instead 112 | - :func:`~skrobot.coordinates.math.outer_product_matrix` → Use :func:`~skrobot.coordinates.math.skew_symmetric_matrix` instead 113 | -------------------------------------------------------------------------------- /docs/source/_docstring_check.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | 4 | def check(app, what, name, obj, options, lines): 5 | ctx = DocstringCheckContext(app, what, name, obj, options, lines) 6 | 7 | if what in ('function', 'method'): 8 | _docstring_check_returns_indent(ctx) 9 | 10 | 11 | class DocstringCheckContext(object): 12 | def __init__(self, app, what, name, obj, options, lines): 13 | self.app = app 14 | self.what = what 15 | self.name = name 16 | self.obj = obj 17 | self.options = options 18 | self.lines = lines 19 | 20 | self.iline = 0 21 | 22 | def nextline(self): 23 | if self.iline >= len(self.lines): 24 | raise StopIteration 25 | line = self.lines[self.iline] 26 | self.iline += 1 27 | return line 28 | 29 | def error(self, msg, include_line=True, include_source=True): 30 | lines = self.lines 31 | iline = self.iline - 1 32 | msg = ('{}\n\n' 33 | 'on {}'.format(msg, self.name)) 34 | 35 | if include_line and 0 <= iline < len(lines): 36 | line = lines[iline] 37 | msg += '\n' + 'at line {}: "{}"\n'.format(iline, line) 38 | 39 | if include_source: 40 | msg += '\n' 41 | msg += 'docstring:\n' 42 | digits = int(math.floor(math.log10(len(lines)))) + 1 43 | linum_fmt = '{{:0{}d}} '.format(digits) 44 | for i, line in enumerate(lines): 45 | msg += linum_fmt.format(i) + line + '\n' 46 | raise InvalidDocstringError(msg, self, iline) 47 | 48 | 49 | class InvalidDocstringError(Exception): 50 | def __init__(self, msg, ctx, iline): 51 | super(InvalidDocstringError, self).__init__(self, msg) 52 | self.msg = msg 53 | self.ctx = ctx 54 | self.iline = iline 55 | 56 | def __str__(self): 57 | return self.msg 58 | 59 | 60 | def _docstring_check_returns_indent(ctx): 61 | # Seek the :returns: header 62 | try: 63 | line = ctx.nextline() 64 | while line != ':returns:': 65 | line = ctx.nextline() 66 | except StopIteration: 67 | return # No `Returns` section 68 | 69 | # Skip empty lines and seek the first line of the content 70 | try: 71 | line = ctx.nextline() 72 | while not line: 73 | line = ctx.nextline() 74 | except StopIteration: 75 | ctx.error('`Returns` section has no content') 76 | 77 | # Find the indentation of the first line 78 | # (note: line should have at least one non-space character) 79 | nindent = next(i for i, c in enumerate(line) if c != ' ') 80 | 81 | # Check the indentation of the following lines 82 | try: 83 | line = ctx.nextline() 84 | while line.startswith(' '): 85 | if (not line.startswith(' ' * nindent) or 86 | line[nindent:].startswith(' ')): 87 | ctx.error('Invalid indentation of `Returns` section') 88 | line = ctx.nextline() 89 | except StopIteration: 90 | pass 91 | -------------------------------------------------------------------------------- /tests/skrobot_tests/test_examples.py: -------------------------------------------------------------------------------- 1 | import os 2 | import os.path as osp 3 | import subprocess 4 | import sys 5 | import tempfile 6 | import unittest 7 | 8 | import pytest 9 | 10 | 11 | run_examples = ( 12 | os.environ.get("RUN_EXAMPLE_TESTS", "false").lower() == "true" 13 | or os.environ.get("GITHUB_ACTIONS", "false").lower() == "true" 14 | ) 15 | 16 | pytestmark = pytest.mark.skipif( 17 | not run_examples, 18 | reason="Skipping example tests" 19 | + "unless RUN_EXAMPLE_TESTS is set or running in GitHub Actions" 20 | ) 21 | 22 | 23 | class TestExampleScripts(unittest.TestCase): 24 | 25 | @pytest.mark.skipif(sys.version_info[0] == 2, reason="Skip in Python 2") 26 | def test_all_examples(self): 27 | examples_dir = osp.join(osp.dirname(__file__), "..", "..", "examples") 28 | self.assertTrue(osp.exists(examples_dir), 29 | "Examples directory not found: {}" 30 | .format(examples_dir)) 31 | 32 | example_scripts = [ 33 | osp.join(examples_dir, f) 34 | for f in os.listdir(examples_dir) 35 | if f.endswith(".py") 36 | ] 37 | self.assertTrue(len(example_scripts) > 0, 38 | "No example scripts found in examples/ directory") 39 | 40 | failures = [] 41 | 42 | for script in example_scripts: 43 | max_attempts = 3 44 | attempt = 0 45 | success = False 46 | 47 | while attempt < max_attempts and not success: 48 | attempt += 1 49 | with tempfile.TemporaryDirectory() as tmp_dir: 50 | env = os.environ.copy() 51 | env["TMPDIR"] = tmp_dir 52 | 53 | cmd = "{} {} --no-interactive".format(sys.executable, script) 54 | print("Executing: {} (attempt {}/{})".format(cmd, attempt, max_attempts)) 55 | result = subprocess.run( 56 | cmd, 57 | shell=True, 58 | stdout=subprocess.PIPE, 59 | stderr=subprocess.PIPE, 60 | env=env, 61 | ) 62 | 63 | if result.returncode == 0: 64 | success = True 65 | print("Success on attempt {}".format(attempt)) 66 | elif attempt < max_attempts: 67 | print("Failed on attempt {}, retrying...".format(attempt)) 68 | else: 69 | failures.append( 70 | ( 71 | script, 72 | result.returncode, 73 | result.stdout.decode(), 74 | result.stderr.decode(), 75 | ) 76 | ) 77 | 78 | if failures: 79 | messages = [] 80 | for script, code, stdout, stderr in failures: 81 | messages.append( 82 | "Script {} failed with exit ".format(script) 83 | + "code {}\nstdout:\n{}\nstderr:\n{}".format( 84 | code, stdout, stderr) 85 | ) 86 | self.fail("\n\n".join(messages)) 87 | -------------------------------------------------------------------------------- /skrobot/urdf/transform_urdf.py: -------------------------------------------------------------------------------- 1 | import math 2 | import os 3 | import xml.etree.ElementTree as ET 4 | 5 | from .modularize_urdf import find_root_link 6 | 7 | 8 | def transform_urdf_with_world_link(input_file, output_file, 9 | x=0.0, y=0.0, z=0.0, 10 | roll=0.0, pitch=0.0, yaw=0.0, 11 | world_link_name="world"): 12 | """Add a transformed world link to a URDF file. 13 | 14 | Parameters 15 | ---------- 16 | input_file : str 17 | Path to the input URDF file 18 | output_file : str 19 | Path for the output URDF file 20 | x : float, optional 21 | Translation in X (meters). Default: 0.0 22 | y : float, optional 23 | Translation in Y (meters). Default: 0.0 24 | z : float, optional 25 | Translation in Z (meters). Default: 0.0 26 | roll : float, optional 27 | Rotation around X-axis (degrees). Default: 0.0 28 | pitch : float, optional 29 | Rotation around Y-axis (degrees). Default: 0.0 30 | yaw : float, optional 31 | Rotation around Z-axis (degrees). Default: 0.0 32 | world_link_name : str, optional 33 | Name for the new world link. Default: 'world' 34 | 35 | Raises 36 | ------ 37 | FileNotFoundError 38 | If the input file does not exist 39 | ValueError 40 | If the world link name already exists in the URDF or if root link cannot be determined 41 | """ 42 | if not os.path.exists(input_file): 43 | raise FileNotFoundError(f"URDF file not found: {input_file}") 44 | 45 | # Register xacro namespace if present 46 | ET.register_namespace('xacro', "http://ros.org/wiki/xacro") 47 | 48 | tree = ET.parse(input_file) 49 | root = tree.getroot() 50 | 51 | # Find the original root link 52 | original_root_link = find_root_link(input_file) 53 | 54 | # Check if the new world link name already exists 55 | if root.find(f"./link[@name='{world_link_name}']") is not None: 56 | raise ValueError( 57 | f"Link '{world_link_name}' already exists in the URDF. " 58 | f"Choose a different world link name.") 59 | 60 | # Create the new world link element 61 | world_link = ET.Element('link', name=world_link_name) 62 | 63 | # Create the new fixed joint to connect world to the original root 64 | joint_name = f"{world_link_name}_to_{original_root_link}" 65 | new_joint = ET.Element('joint', name=joint_name, type='fixed') 66 | 67 | # Set parent (world) and child (original root) 68 | ET.SubElement(new_joint, 'parent', link=world_link_name) 69 | ET.SubElement(new_joint, 'child', link=original_root_link) 70 | 71 | # Create the origin element with the specified transform 72 | # Convert degrees to radians for RPY 73 | roll_rad = math.radians(roll) 74 | pitch_rad = math.radians(pitch) 75 | yaw_rad = math.radians(yaw) 76 | 77 | xyz_str = f"{x} {y} {z}" 78 | rpy_str = f"{roll_rad} {pitch_rad} {yaw_rad}" 79 | 80 | ET.SubElement(new_joint, 'origin', xyz=xyz_str, rpy=rpy_str) 81 | 82 | # Insert the new elements into the XML tree (at the beginning) 83 | root.insert(0, new_joint) 84 | root.insert(0, world_link) 85 | 86 | # Try to use pretty printing if available (Python 3.9+) 87 | try: 88 | ET.indent(tree, space=" ") 89 | except AttributeError: 90 | pass 91 | 92 | tree.write(output_file, encoding='utf-8', xml_declaration=True) 93 | -------------------------------------------------------------------------------- /docs/source/development/index.rst: -------------------------------------------------------------------------------- 1 | .. _development: 2 | 3 | Development Guide 4 | ================= 5 | 6 | Read this guide before doing development in ``skrobot``. 7 | 8 | Setting Up 9 | ---------- 10 | 11 | To set up the tools you'll need for developing, you'll need to install 12 | ``skrobot`` in development mode. Start by installing the development 13 | dependencies: 14 | 15 | .. code-block:: bash 16 | 17 | git clone https://github.com/iory/scikit-robot.git 18 | cd scikit-robot 19 | pip install -e . 20 | 21 | Running Code Style Checks 22 | ------------------------- 23 | 24 | We follow `PEP 8 `_ and partially `OpenStack Style Guidelines `_ as basic style guidelines. 25 | Any contributions in terms of code are expected to follow these guidelines. 26 | 27 | You can use ``ruff`` to check and automatically fix code style issues, including import ordering. 28 | ``ruff`` is a fast Python linter and formatter that replaces ``flake8``, ``isort``, and ``autopep8``. 29 | Install it with the following command:: 30 | 31 | $ pip install ruff pytest 32 | 33 | Check your code with:: 34 | 35 | $ ruff check path/to/your/code.py 36 | 37 | ``ruff`` can automatically fix many style issues:: 38 | 39 | $ ruff check --fix path/to/your/code.py 40 | 41 | To check the entire project:: 42 | 43 | $ ruff check . 44 | 45 | For more information, please see `the ruff documentation`_. 46 | 47 | .. _the ruff documentation: https://docs.astral.sh/ruff/ 48 | 49 | Running Tests 50 | ------------- 51 | 52 | This project uses `pytest`_, the standard Python testing framework. 53 | Their website has tons of useful details, but here are the basics. 54 | 55 | .. _pytest: https://docs.pytest.org/en/latest/ 56 | 57 | To run the testing suite, simply navigate to the top-level folder 58 | in ``scikit-robot`` and run the following command: 59 | 60 | .. code-block:: bash 61 | 62 | pytest -v tests 63 | 64 | You should see the testing suite run. There are a few useful command line 65 | options that are good to know: 66 | 67 | - ``-s`` - Shows the output of ``stdout``. By default, this output is masked. 68 | - ``--pdb`` - Instead of crashing, opens a debugger at the first fault. 69 | - ``--lf`` - Instead of running all tests, just run the ones that failed last. 70 | - ``--trace`` - Open a debugger at the start of each test. 71 | 72 | You can see all of the other command-line options `here`_. 73 | 74 | .. _here: https://docs.pytest.org/en/latest/usage.html 75 | 76 | By default, ``pytest`` will look in the ``tests`` folder recursively. 77 | It will run any function that starts with ``test_`` in any file that starts 78 | with ``test_``. You can run ``pytest`` on a directory or on a particular file 79 | by specifying the file path: 80 | 81 | .. code-block:: bash 82 | 83 | pytest -v tests/skrobot_tests/coordinates_tests/test_math.py 84 | 85 | 86 | Building Documentation 87 | ---------------------- 88 | 89 | To build ``scikit-robot``'s documentation, go to the ``docs`` directory and run 90 | ``make`` with the appropriate target. 91 | For example, 92 | 93 | .. code-block:: bash 94 | 95 | cd docs/ 96 | make html 97 | 98 | will generate HTML-based docs, which are probably the easiest to read. 99 | The resulting index page is at ``docs/build/html/index.html``. 100 | If the docs get stale, just run ``make clean`` to remove all build files. 101 | -------------------------------------------------------------------------------- /skrobot/interfaces/ros2/panda.py: -------------------------------------------------------------------------------- 1 | import control_msgs.action 2 | import rclpy 3 | from rclpy.action import ActionClient 4 | 5 | from .base import ROS2RobotInterfaceBase 6 | 7 | 8 | try: 9 | import franka_gripper.action 10 | FRANKA_GRIPPER_AVAILABLE = True 11 | except ImportError: 12 | FRANKA_GRIPPER_AVAILABLE = False 13 | 14 | 15 | WIDTH_MAX = 0.08 16 | 17 | 18 | class PandaROS2RobotInterface(ROS2RobotInterfaceBase): 19 | 20 | def __init__(self, *args, **kwargs): 21 | super(PandaROS2RobotInterface, self).__init__(*args, **kwargs) 22 | 23 | if FRANKA_GRIPPER_AVAILABLE: 24 | self.gripper_move = ActionClient( 25 | self, 26 | franka_gripper.action.Move, 27 | 'franka_gripper/move') 28 | self.gripper_move.wait_for_server() 29 | 30 | self.gripper_stop = ActionClient( 31 | self, 32 | franka_gripper.action.Stop, 33 | 'franka_gripper/stop') 34 | self.gripper_stop.wait_for_server() 35 | else: 36 | self.get_logger().warn("franka_gripper package not available. Gripper functions disabled.") 37 | 38 | @property 39 | def rarm_controller(self): 40 | return dict( 41 | controller_type='rarm_controller', 42 | controller_action='/panda_arm_controller/follow_joint_trajectory', 43 | controller_state='/panda_arm_controller/state', 44 | action_type=control_msgs.action.FollowJointTrajectory, 45 | joint_names=[j.name for j in self.robot.rarm.joint_list], 46 | ) 47 | 48 | def default_controller(self): 49 | return [self.rarm_controller] 50 | 51 | def grasp(self, width=0, **kwargs): 52 | self.move_gripper(width=width, **kwargs) 53 | 54 | def ungrasp(self, **kwargs): 55 | self.move_gripper(width=WIDTH_MAX, **kwargs) 56 | 57 | def move_gripper(self, width, speed=WIDTH_MAX, wait=True): 58 | if not FRANKA_GRIPPER_AVAILABLE: 59 | self.get_logger().warn("franka_gripper package not available. Cannot move gripper.") 60 | return 61 | 62 | goal = franka_gripper.action.Move.Goal() 63 | goal.width = width 64 | goal.speed = speed 65 | 66 | if wait: 67 | future = self.gripper_move.send_goal_async(goal) 68 | rclpy.spin_until_future_complete(self, future) 69 | goal_handle = future.result() 70 | if goal_handle.accepted: 71 | result_future = goal_handle.get_result_async() 72 | rclpy.spin_until_future_complete(self, result_future) 73 | return result_future.result() 74 | else: 75 | self.gripper_move.send_goal_async(goal) 76 | 77 | def stop_gripper(self, wait=True): 78 | if not FRANKA_GRIPPER_AVAILABLE: 79 | self.get_logger().warn("franka_gripper package not available. Cannot stop gripper.") 80 | return 81 | 82 | goal = franka_gripper.action.Stop.Goal() 83 | 84 | if wait: 85 | future = self.gripper_stop.send_goal_async(goal) 86 | rclpy.spin_until_future_complete(self, future) 87 | goal_handle = future.result() 88 | if goal_handle.accepted: 89 | result_future = goal_handle.get_result_async() 90 | rclpy.spin_until_future_complete(self, result_future) 91 | return result_future.result() 92 | else: 93 | self.gripper_stop.send_goal_async(goal) 94 | -------------------------------------------------------------------------------- /tests/skrobot_tests/coordinates_tests/test_quaternion.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import numpy as np 4 | from numpy import testing 5 | 6 | from skrobot.coordinates.quaternion import Quaternion 7 | 8 | 9 | class TestQuaternion(unittest.TestCase): 10 | 11 | def test_init(self): 12 | q = [1, 0, 0, 0] 13 | numpy_q = np.array(q) 14 | Quaternion(q) 15 | Quaternion(q=q) 16 | Quaternion(q[0], q[1], q[2], q[3]) 17 | Quaternion(numpy_q) 18 | Quaternion(q=numpy_q) 19 | 20 | def test_rotation(self): 21 | q = Quaternion([1, 0, 0, 0]) 22 | testing.assert_almost_equal( 23 | q.rotation, np.eye(3)) 24 | 25 | def test_axis(self): 26 | q = Quaternion(w=0.7071, x=0.7071) 27 | testing.assert_almost_equal( 28 | q.axis, [1.0, 0.0, 0.0]) 29 | 30 | q = Quaternion(w=0.7071, y=0.7071) 31 | testing.assert_almost_equal( 32 | q.axis, [0.0, 1.0, 0.0]) 33 | 34 | q = Quaternion(w=0.7071, z=0.7071) 35 | testing.assert_almost_equal( 36 | q.axis, [0.0, 0.0, 1.0]) 37 | 38 | q = Quaternion([1, 1, 1, 1]) 39 | testing.assert_almost_equal( 40 | q.axis, [0.5773503, 0.5773503, 0.5773503]) 41 | 42 | def test_angle(self): 43 | q = Quaternion(w=0.7071, x=0.7071) 44 | testing.assert_almost_equal( 45 | q.angle, np.pi / 2.0) 46 | 47 | q = Quaternion(w=0.7071, y=0.7071) 48 | testing.assert_almost_equal( 49 | q.angle, np.pi / 2.0) 50 | 51 | q = Quaternion(w=0.7071, z=0.7071) 52 | testing.assert_almost_equal( 53 | q.angle, np.pi / 2.0) 54 | 55 | q = Quaternion([1, 1, 1, 1]) 56 | testing.assert_almost_equal( 57 | q.angle, 2.0943951) 58 | 59 | def test_add(self): 60 | q1 = Quaternion() 61 | q2 = Quaternion() 62 | 63 | q = q1 + q2 64 | testing.assert_almost_equal( 65 | q.q, [2.0, 0.0, 0.0, 0.0]) 66 | 67 | def test_mul(self): 68 | q1 = Quaternion() 69 | q2 = Quaternion() 70 | 71 | q = q1 * q2 72 | testing.assert_almost_equal( 73 | q.q, [1.0, 0.0, 0.0, 0.0]) 74 | 75 | q = 3.0 * q1 76 | testing.assert_almost_equal( 77 | q.q, [3.0, 0.0, 0.0, 0.0]) 78 | 79 | def test_div(self): 80 | q1 = Quaternion() 81 | q2 = Quaternion() 82 | 83 | q = q1 / q2 84 | testing.assert_almost_equal( 85 | q.q, [1.0, 0.0, 0.0, 0.0]) 86 | 87 | q = q1 / 2.0 88 | testing.assert_almost_equal( 89 | q.q, [0.5, 0.0, 0.0, 0.0]) 90 | 91 | def test_norm(self): 92 | q = Quaternion() 93 | 94 | testing.assert_almost_equal( 95 | q.norm, 1.0) 96 | 97 | def test_conjugate(self): 98 | q = Quaternion() 99 | testing.assert_almost_equal( 100 | q.conjugate.q, 101 | [1.0, 0.0, 0.0, 0.0]) 102 | 103 | q = Quaternion(q=[1, -1, -2, -3]) 104 | testing.assert_almost_equal( 105 | q.conjugate.q, 106 | [1, 1, 2, 3]) 107 | 108 | def test_inverse(self): 109 | q = Quaternion() 110 | testing.assert_almost_equal( 111 | q.inverse.q, 112 | [1.0, 0.0, 0.0, 0.0]) 113 | 114 | def test_T(self): 115 | q = Quaternion() 116 | testing.assert_almost_equal( 117 | q.T(), 118 | np.eye(4)) 119 | -------------------------------------------------------------------------------- /skrobot/apps/cli.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import os 5 | import sys 6 | 7 | 8 | def get_available_apps(): 9 | """Dynamically discover available apps in the apps directory.""" 10 | apps_dir = os.path.dirname(__file__) 11 | apps = {} 12 | 13 | # App metadata: (help_text, version_requirement) 14 | app_metadata = { 15 | 'visualize_urdf': ('Visualize URDF model', None), 16 | 'convert_urdf_mesh': ('Convert URDF mesh files', (3, 6)), 17 | 'convert_urdf_to_primitives': ('Convert URDF meshes to primitive shapes', None), 18 | 'modularize_urdf': ('Modularize URDF files', None), 19 | 'change_urdf_root': ('Change URDF root link', None), 20 | 'transform_urdf': ('Add world link with transform to URDF', None), 21 | 'visualize_mesh': ('Visualize mesh file', None), 22 | 'urdf_hash': ('Calculate URDF hash', None), 23 | 'convert_wheel_collision': ('Convert wheel collision model', None), 24 | 'extract_sub_urdf': ('Extract sub-URDF from a root link', None), 25 | } 26 | 27 | for filename in os.listdir(apps_dir): 28 | if filename.endswith('.py') and filename != '__init__.py' and filename != 'cli.py': 29 | app_name = filename[:-3] # Remove .py extension 30 | 31 | # Check if app exists and has main function 32 | try: 33 | module_path = f'skrobot.apps.{app_name}' 34 | module = __import__(module_path, fromlist=['main']) 35 | if hasattr(module, 'main'): 36 | # Convert underscore to hyphen for command name 37 | command_name = app_name.replace('_', '-') 38 | 39 | # Get metadata 40 | help_text, version_req = app_metadata.get(app_name, (f'Run {command_name}', None)) 41 | 42 | # Check version requirement 43 | if version_req is None or (sys.version_info.major, sys.version_info.minor) >= version_req: 44 | apps[command_name] = { 45 | 'module': module_path, 46 | 'help': help_text 47 | } 48 | except ImportError: 49 | continue 50 | 51 | return apps 52 | 53 | 54 | def main(): 55 | parser = argparse.ArgumentParser( 56 | prog='skr', 57 | description='scikit-robot CLI tool' 58 | ) 59 | 60 | subparsers = parser.add_subparsers( 61 | dest='command', 62 | help='Available commands' 63 | ) 64 | 65 | # Dynamically add subcommands 66 | available_apps = get_available_apps() 67 | for command_name, app_info in available_apps.items(): 68 | app_parser = subparsers.add_parser( 69 | command_name, 70 | help=app_info['help'], 71 | add_help=False 72 | ) 73 | app_parser.set_defaults( 74 | func=lambda args, module=app_info['module']: run_app(f'{module}:main', args) 75 | ) 76 | 77 | # Parse arguments 78 | args, unknown = parser.parse_known_args() 79 | 80 | if args.command is None: 81 | parser.print_help() 82 | sys.exit(0) 83 | 84 | # Pass remaining arguments to the subcommand 85 | sys.argv = [args.command] + unknown 86 | args.func(args) 87 | 88 | 89 | def run_app(module_path, args): 90 | module_name, func_name = module_path.split(':') 91 | module = __import__(module_name, fromlist=[func_name]) 92 | func = getattr(module, func_name) 93 | func() 94 | 95 | 96 | if __name__ == '__main__': 97 | main() 98 | -------------------------------------------------------------------------------- /docs/source/reference/how_to_create_urdf_from_cad.rst: -------------------------------------------------------------------------------- 1 | How to Create URDF from CAD Software 2 | ==================================== 3 | 4 | Overview 5 | -------- 6 | 7 | .. figure:: ../../image/urdf-from-solidworks.png 8 | :scale: 30% 9 | :align: center 10 | 11 | This document explains how to create a URDF file from SolidWorks models by using: 12 | 13 | 1. SolidWorks to URDF Exporter 14 | 2. scikit-robot's ``convert-urdf-mesh`` command 15 | 16 | - **SolidWorks to URDF Exporter** 17 | This exporter helps convert SolidWorks assemblies into a format (3dxml) which is more easily processed to produce URDF-compatible mesh files. 18 | 19 | - **scikit-robot ``convert-urdf-mesh``** 20 | A tool within the `scikit-robot `_ library that can convert 3D model files (like ``.3dxml``, ``.obj``, ``.stl``, etc.) into a URDF or mesh files (e.g., ``.dae``) suitable for ROS. 21 | 22 | Coordinate Systems 23 | ------------------ 24 | 25 | - **.3dxml export**: uses the overall assembly coordinate system from SolidWorks. 26 | - **.stl export**: uses each part's or link's local coordinate system. 27 | 28 | ``convert-urdf-mesh`` provides an option for adjusting coordinates: 29 | 30 | - ``--force-zero-origin``: Forces the origin to be the link's local coordinate in the converted mesh files (e.g., ``.dae``). 31 | - Useful when your source is in the assembly coordinate system (like ``.3dxml``). 32 | - Highly recommended if you plan to use these meshes in **MuJoCo**. 33 | 34 | Installation 35 | ------------ 36 | 37 | 1. **Install scikit-robot** 38 | 39 | .. code-block:: bash 40 | 41 | pip install scikit-robot 42 | 43 | or clone directly from GitHub if you need the latest updates: 44 | 45 | .. code-block:: bash 46 | 47 | git clone https://github.com/iory/scikit-robot.git 48 | cd scikit-robot 49 | pip install -e . 50 | 51 | 2. **Install SolidWorks URDF Exporter** 52 | 53 | - Obtain the plugin from the following link: 54 | 55 | `SolidWorks URDF Exporter Plugin `_ 56 | 57 | - Follow the official instructions to install it into your SolidWorks environment. 58 | 59 | `Installation Instructions `_ 60 | 61 | Workflow 62 | -------- 63 | 64 | 1. **Export from SolidWorks** 65 | 66 | - In SolidWorks, open your assembly. 67 | - Use the "SolidWorks to URDF Exporter" plugin to generate a ``.3dxml`` file. 68 | 69 | 2. **Convert to DAE (or STL) and Generate URDF** 70 | 71 | - Run the scikit-robot command to convert ``.3dxml`` (or other mesh formats) to ``.dae`` and generate a URDF automatically. 72 | - Example usage: 73 | 74 | .. code-block:: bash 75 | 76 | convert-urdf-mesh --output 77 | 78 | - This command outputs: 79 | - A set of mesh files (e.g., ``.dae``) 80 | - A URDF file referencing those meshes 81 | 82 | 3. **Verify URDF in ROS** 83 | 84 | - Copy the generated URDF and mesh files into your ROS package. 85 | - Test in Rviz or another ROS-compatible viewer: 86 | 87 | .. code-block:: bash 88 | 89 | roslaunch urdf_tutorial display.launch model:=path/to/generated.urdf 90 | 91 | - Confirm the model loads and displays properly. 92 | 93 | 4. **Usage with MuJoCo** 94 | 95 | - MuJoCo typically requires meshes to be centered at their local origin. 96 | - Therefore, always use the ``--force-zero-origin`` option when converting to ensure proper alignment. 97 | 98 | .. code-block:: bash 99 | 100 | convert-urdf-mesh --output --force-zero-origin 101 | -------------------------------------------------------------------------------- /tests/skrobot_tests/planner_tests/test_sqp_based_planner.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import numpy as np 4 | from numpy import testing 5 | 6 | import skrobot 7 | from skrobot.model.primitives import Box 8 | from skrobot.planner import sqp_plan_trajectory 9 | from skrobot.planner import SweptSphereSdfCollisionChecker 10 | from skrobot.planner.utils import get_robot_config 11 | from skrobot.planner.utils import set_robot_config 12 | 13 | 14 | class Test_sqp_based_planner(unittest.TestCase): 15 | 16 | @classmethod 17 | def setup_class(cls): 18 | robot_model = skrobot.models.PR2() 19 | robot_model.init_pose() 20 | 21 | # setting up box sdf 22 | box_center = np.array([0.9, -0.2, 0.9]) 23 | box = Box(extents=[0.7, 0.5, 0.6], with_sdf=True) 24 | box.translate(box_center) 25 | 26 | # defining control joints 27 | link_list = [ 28 | robot_model.r_shoulder_pan_link, robot_model.r_shoulder_lift_link, 29 | robot_model.r_upper_arm_roll_link, robot_model.r_elbow_flex_link, 30 | robot_model.r_forearm_roll_link, robot_model.r_wrist_flex_link, 31 | robot_model.r_wrist_roll_link] 32 | joint_list = [link.joint for link in link_list] 33 | 34 | # defining collision links 35 | coll_link_list = [ 36 | robot_model.r_upper_arm_link, robot_model.r_forearm_link, 37 | robot_model.r_gripper_palm_link, 38 | robot_model.r_gripper_r_finger_link, 39 | robot_model.r_gripper_l_finger_link] 40 | 41 | # collision checker setup 42 | sscc = SweptSphereSdfCollisionChecker( 43 | lambda X: box.sdf(X), robot_model) 44 | for link in coll_link_list: 45 | sscc.add_collision_link(link) 46 | 47 | # by solving inverse kinematics, setting up av_goal 48 | coef = 3.1415 / 180.0 49 | joint_angles = [coef * e for e in [-60, 74, -70, -120, -20, -30, 180]] 50 | set_robot_config(robot_model, joint_list, joint_angles) 51 | 52 | robot_model.inverse_kinematics( 53 | target_coords=skrobot.coordinates.Coordinates( 54 | [0.8, -0.6, 0.8], [0, 0, 0]), 55 | link_list=link_list, 56 | move_target=robot_model.rarm_end_coords, rotation_axis=True) 57 | av_goal = get_robot_config(robot_model, joint_list, with_base=False) 58 | 59 | cls.joint_list = joint_list 60 | cls.av_start = np.array([0.564, 0.35, -0.74, -0.7, -0.7, -0.17, -0.63]) 61 | cls.av_goal = av_goal 62 | cls.sscc = sscc 63 | 64 | def test_sqp_plan_trajectory(self): 65 | _av_start = self.av_start 66 | _av_goal = self.av_goal 67 | joint_list = self.joint_list 68 | sscc = self.sscc 69 | n_wp = 10 70 | 71 | for with_base in [False, True]: 72 | if with_base: 73 | av_start = np.hstack((_av_start, [0, 0, 0])) 74 | av_goal = np.hstack((_av_goal, [0, 0, 0])) 75 | else: 76 | av_start, av_goal = _av_start, _av_goal 77 | 78 | av_seq = sqp_plan_trajectory( 79 | sscc, av_start, av_goal, joint_list, n_wp, 80 | safety_margin=1e-2, with_base=with_base) 81 | # check equality (terminal) constraint 82 | testing.assert_almost_equal(av_seq[0], av_start) 83 | testing.assert_almost_equal(av_seq[-1], av_goal) 84 | 85 | # check inequality constraint, 86 | # meaning that in any waypoint, rarm does not collide 87 | batch_dists, _ = sscc.compute_batch_sd_vals( 88 | joint_list, av_seq, with_base=with_base, with_jacobian=False) 89 | self.assertTrue(np.all(batch_dists > -1e-4)) 90 | -------------------------------------------------------------------------------- /skrobot/models/fetch.py: -------------------------------------------------------------------------------- 1 | from cached_property import cached_property 2 | import numpy as np 3 | 4 | from skrobot.coordinates import CascadedCoords 5 | from skrobot.data import fetch_urdfpath 6 | from skrobot.model import RobotModel 7 | 8 | from .urdf import RobotModelFromURDF 9 | 10 | 11 | class Fetch(RobotModelFromURDF): 12 | """Fetch Robot Model. 13 | 14 | http://docs.fetchrobotics.com/robot_hardware.html 15 | """ 16 | 17 | def __init__(self, *args, **kwargs): 18 | super(Fetch, self).__init__(*args, **kwargs) 19 | self.rarm_end_coords = CascadedCoords(parent=self.gripper_link, 20 | name='rarm_end_coords') 21 | self.rarm_end_coords.translate([0, 0, 0]) 22 | self.rarm_end_coords.rotate(0, axis='z') 23 | self.end_coords = [self.rarm_end_coords] 24 | 25 | @cached_property 26 | def default_urdf_path(self): 27 | return fetch_urdfpath() 28 | 29 | def reset_pose(self): 30 | self.torso_lift_joint.joint_angle(0) 31 | self.shoulder_pan_joint.joint_angle(np.deg2rad(75.6304)) 32 | self.shoulder_lift_joint.joint_angle(np.deg2rad(80.2141)) 33 | self.upperarm_roll_joint.joint_angle(np.deg2rad(-11.4592)) 34 | self.elbow_flex_joint.joint_angle(np.deg2rad(98.5487)) 35 | self.forearm_roll_joint.joint_angle(0.0) 36 | self.wrist_flex_joint.joint_angle(np.deg2rad(95.111)) 37 | self.wrist_roll_joint.joint_angle(0.0) 38 | self.head_pan_joint.joint_angle(0.0) 39 | self.head_tilt_joint.joint_angle(0.0) 40 | return self.angle_vector() 41 | 42 | def reset_manip_pose(self): 43 | self.torso_lift_joint.joint_angle(0) 44 | self.shoulder_pan_joint.joint_angle(0) 45 | self.shoulder_lift_joint.joint_angle(0) 46 | self.upperarm_roll_joint.joint_angle(0) 47 | self.elbow_flex_joint.joint_angle(np.pi / 2.0) 48 | self.forearm_roll_joint.joint_angle(0) 49 | self.wrist_flex_joint.joint_angle(- np.pi / 2.0) 50 | self.wrist_roll_joint.joint_angle(0) 51 | self.head_pan_joint.joint_angle(0) 52 | self.head_tilt_joint.joint_angle(0) 53 | return self.angle_vector() 54 | 55 | @cached_property 56 | def rarm(self): 57 | rarm_links = [self.shoulder_pan_link, 58 | self.shoulder_lift_link, 59 | self.upperarm_roll_link, 60 | self.elbow_flex_link, 61 | self.forearm_roll_link, 62 | self.wrist_flex_link, 63 | self.wrist_roll_link] 64 | rarm_joints = [] 65 | for link in rarm_links: 66 | rarm_joints.append(link.joint) 67 | r = RobotModel(link_list=rarm_links, 68 | joint_list=rarm_joints) 69 | r.end_coords = self.rarm_end_coords 70 | return r 71 | 72 | @cached_property 73 | def rarm_with_torso(self): 74 | rarm_with_torso_links = [self.torso_lift_link, 75 | self.shoulder_pan_link, 76 | self.shoulder_lift_link, 77 | self.upperarm_roll_link, 78 | self.elbow_flex_link, 79 | self.forearm_roll_link, 80 | self.wrist_flex_link, 81 | self.wrist_roll_link] 82 | rarm_with_torso_joints = [] 83 | for link in rarm_with_torso_links: 84 | rarm_with_torso_joints.append(link.joint) 85 | r = RobotModel(link_list=rarm_with_torso_links, 86 | joint_list=rarm_with_torso_joints) 87 | r.end_coords = self.rarm_end_coords 88 | return r 89 | -------------------------------------------------------------------------------- /skrobot/models/nextage.py: -------------------------------------------------------------------------------- 1 | from cached_property import cached_property 2 | import numpy as np 3 | 4 | from ..coordinates import CascadedCoords 5 | from ..data import nextage_urdfpath 6 | from ..model import RobotModel 7 | from .urdf import RobotModelFromURDF 8 | 9 | 10 | class Nextage(RobotModelFromURDF): 11 | """ 12 | - Nextage Open Official Information. 13 | 14 | https://nextage.kawadarobot.co.jp/open 15 | 16 | - Nextage Open Robot Description 17 | 18 | https://github.com/tork-a/rtmros_nextage/tree/indigo-devel/nextage_description/urdf 19 | """ 20 | 21 | def __init__(self, *args, **kwargs): 22 | super(Nextage, self).__init__(*args, **kwargs) 23 | 24 | # End effector coordinates 25 | self.rarm_end_coords = CascadedCoords( 26 | pos=[-0.185, 0.0, -0.01], 27 | parent=self.RARM_JOINT5_Link, 28 | name='rarm_end_coords') 29 | self.rarm_end_coords.rotate(-np.pi / 2.0, 'y') 30 | 31 | self.larm_end_coords = CascadedCoords( 32 | pos=[-0.185, 0.0, -0.01], 33 | parent=self.LARM_JOINT5_Link, 34 | name='larm_end_coords') 35 | self.larm_end_coords.rotate(-np.pi / 2.0, 'y') 36 | 37 | self.head_end_coords = CascadedCoords( 38 | pos=[0.06, 0, 0.025], 39 | parent=self.HEAD_JOINT1_Link, 40 | name='head_end_coords') 41 | self.head_end_coords.rotate(np.deg2rad(90), 'y') 42 | 43 | self.reset_pose() 44 | 45 | @cached_property 46 | def default_urdf_path(self): 47 | return nextage_urdfpath() 48 | 49 | def reset_pose(self): 50 | angle_vector = [ 51 | 0.0, 52 | 0.0, 53 | 0.0, 54 | np.deg2rad(0.6), 55 | 0.0, 56 | np.deg2rad(-100), 57 | np.deg2rad(-15.2), 58 | np.deg2rad(9.4), 59 | np.deg2rad(-3.2), 60 | np.deg2rad(-0.6), 61 | 0.0, 62 | np.deg2rad(-100), 63 | np.deg2rad(15.2), 64 | np.deg2rad(9.4), 65 | np.deg2rad(3.2), 66 | ] 67 | self.angle_vector(angle_vector) 68 | return self.angle_vector() 69 | 70 | def reset_manip_pose(self): 71 | """Reset robot to manipulation pose (same as reset_pose for Nextage)""" 72 | return self.reset_pose() 73 | 74 | @cached_property 75 | def rarm(self): 76 | link_names = ["RARM_JOINT{}_Link".format(i) for i in range(6)] 77 | links = [getattr(self, n) for n in link_names] 78 | joints = [l.joint for l in links] 79 | model = RobotModel(link_list=links, joint_list=joints) 80 | model.end_coords = self.rarm_end_coords 81 | return model 82 | 83 | @cached_property 84 | def larm(self): 85 | link_names = ["LARM_JOINT{}_Link".format(i) for i in range(6)] 86 | links = [getattr(self, n) for n in link_names] 87 | joints = [l.joint for l in links] 88 | model = RobotModel(link_list=links, joint_list=joints) 89 | model.end_coords = self.larm_end_coords 90 | return model 91 | 92 | @cached_property 93 | def head(self): 94 | link_names = ["HEAD_JOINT{}_Link".format(i) for i in range(2)] 95 | links = [getattr(self, n) for n in link_names] 96 | joints = [l.joint for l in links] 97 | model = RobotModel(link_list=links, joint_list=joints) 98 | model.end_coords = self.head_end_coords 99 | return model 100 | 101 | @cached_property 102 | def torso(self): 103 | link_names = ["CHEST_JOINT0_Link"] 104 | links = [getattr(self, n) for n in link_names] 105 | joints = [l.joint for l in links] 106 | model = RobotModel(link_list=links, joint_list=joints) 107 | return model 108 | -------------------------------------------------------------------------------- /skrobot/interfaces/ros/tf_utils.py: -------------------------------------------------------------------------------- 1 | import geometry_msgs.msg 2 | 3 | from skrobot.coordinates import Coordinates 4 | 5 | 6 | def coords_to_geometry_pose(coords): 7 | """Convert Coordinates to geometry_msgs.msg.Pose 8 | 9 | Parameters 10 | ---------- 11 | coords : skrobot.coordinates.Coordinates 12 | coordinates pose. 13 | 14 | Returns 15 | ------- 16 | pose : geometry_msgs.msg.Pose 17 | converted geometry_msgs pose. 18 | """ 19 | pose = geometry_msgs.msg.Pose() 20 | pose.position.x = coords.translation[0] 21 | pose.position.y = coords.translation[1] 22 | pose.position.z = coords.translation[2] 23 | q = coords.quaternion_wxyz 24 | pose.orientation.w = q[0] 25 | pose.orientation.x = q[1] 26 | pose.orientation.y = q[2] 27 | pose.orientation.z = q[3] 28 | return pose 29 | 30 | 31 | def coords_to_tf_pose(coords): 32 | """Convert Coordinates to geometry_msgs.msg.Transform 33 | 34 | Parameters 35 | ---------- 36 | coords : skrobot.coordinates.Coordinates 37 | coordinates pose. 38 | 39 | Returns 40 | ------- 41 | pose : geometry_msgs.msg.Transform 42 | converted transform pose. 43 | """ 44 | pose = geometry_msgs.msg.Transform() 45 | pose.translation.x = coords.translation[0] 46 | pose.translation.y = coords.translation[1] 47 | pose.translation.z = coords.translation[2] 48 | q = coords.quaternion_wxyz 49 | pose.rotation.w = q[0] 50 | pose.rotation.x = q[1] 51 | pose.rotation.y = q[2] 52 | pose.rotation.z = q[3] 53 | return pose 54 | 55 | 56 | def tf_pose_to_coords(tf_pose): 57 | """Convert TransformStamped to Coordinates 58 | 59 | Parameters 60 | ---------- 61 | tf_pose : geometry_msgs.msg.Transform or geometry_msgs.msg.TransformStamped 62 | transform pose. 63 | 64 | Returns 65 | ------- 66 | ret : skrobot.coordinates.Coordinates 67 | converted coordinates. 68 | """ 69 | if isinstance(tf_pose, geometry_msgs.msg.Transform): 70 | transform = tf_pose 71 | elif isinstance(tf_pose, geometry_msgs.msg.TransformStamped): 72 | transform = tf_pose.transform 73 | else: 74 | raise TypeError('{} not supported'.format(type(tf_pose))) 75 | if transform.rotation.w == 0.0 and \ 76 | transform.rotation.x == 0.0 and \ 77 | transform.rotation.y == 0.0 and \ 78 | transform.rotation.z == 0.0: 79 | transform.rotation.w = 1.0 80 | return Coordinates(pos=[transform.translation.x, 81 | transform.translation.y, 82 | transform.translation.z], 83 | rot=[transform.rotation.w, transform.rotation.x, 84 | transform.rotation.y, transform.rotation.z]) 85 | 86 | 87 | def geometry_pose_to_coords(tf_pose): 88 | """Convert geometry_msgs.msg.Pose to Coordinates 89 | 90 | Parameters 91 | ---------- 92 | tf_pose : geometry_msgs.msg.Pose or geometry_msgs.msg.PoseStamped 93 | pose. 94 | 95 | Returns 96 | ------- 97 | ret : skrobot.coordinates.Coordinates 98 | converted coordinates. 99 | """ 100 | if isinstance(tf_pose, geometry_msgs.msg.Pose): 101 | pose = tf_pose 102 | elif isinstance(tf_pose, geometry_msgs.msg.PoseStamped): 103 | pose = tf_pose.pose 104 | else: 105 | raise TypeError('{} not supported'.format(type(tf_pose))) 106 | if pose.orientation.w == 0.0 and \ 107 | pose.orientation.x == 0.0 and \ 108 | pose.orientation.y == 0.0 and \ 109 | pose.orientation.z == 0.0: 110 | pose.orientation.w = 1.0 111 | return Coordinates(pos=[pose.position.x, pose.position.y, pose.position.z], 112 | rot=[pose.orientation.w, pose.orientation.x, 113 | pose.orientation.y, pose.orientation.z]) 114 | -------------------------------------------------------------------------------- /tests/skrobot_tests/urdf_tests/test_hash.py: -------------------------------------------------------------------------------- 1 | import hashlib 2 | import os 3 | import tempfile 4 | import unittest 5 | 6 | from skrobot.data import fetch_urdfpath 7 | from skrobot.data import kuka_urdfpath 8 | from skrobot.data import panda_urdfpath 9 | from skrobot.data import pr2_urdfpath 10 | from skrobot.urdf.hash import get_file_hash 11 | from skrobot.urdf.hash import get_urdf_hash 12 | 13 | 14 | class TestHashFunctions(unittest.TestCase): 15 | """Test cases for URDF hash functions.""" 16 | 17 | def test_get_file_hash(self): 18 | """Test basic file hash calculation.""" 19 | with tempfile.NamedTemporaryFile(mode='w', delete=False) as f: 20 | f.write('test content') 21 | temp_path = f.name 22 | 23 | try: 24 | hash_value = get_file_hash(temp_path) 25 | expected = hashlib.sha256(b'test content').hexdigest() 26 | self.assertEqual(hash_value, expected) 27 | finally: 28 | os.unlink(temp_path) 29 | 30 | # Test non-existent file 31 | hash_value = get_file_hash('/nonexistent/file/path') 32 | self.assertEqual(hash_value, 'file_not_found') 33 | 34 | def test_get_urdf_hash_simple(self): 35 | """Test URDF hash calculation with simple URDF.""" 36 | urdf_content = ''' 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | ''' 46 | 47 | with tempfile.NamedTemporaryFile(mode='w', suffix='.urdf', 48 | delete=False) as f: 49 | f.write(urdf_content) 50 | urdf_path = f.name 51 | 52 | try: 53 | hash_value = get_urdf_hash(urdf_path) 54 | self.assertIsInstance(hash_value, str) 55 | self.assertEqual(len(hash_value), 64) # SHA-256 hash length 56 | finally: 57 | os.unlink(urdf_path) 58 | 59 | def test_get_urdf_hash_kuka(self): 60 | """Test URDF hash calculation with KUKA robot.""" 61 | urdf_path = kuka_urdfpath() 62 | hash_value = get_urdf_hash(urdf_path) 63 | 64 | # Basic checks 65 | self.assertIsInstance(hash_value, str) 66 | self.assertEqual(len(hash_value), 64) 67 | 68 | # Hash should be consistent for the same file 69 | hash_value2 = get_urdf_hash(urdf_path) 70 | self.assertEqual(hash_value, hash_value2) 71 | 72 | def test_get_urdf_hash_pr2(self): 73 | """Test URDF hash calculation with PR2 robot.""" 74 | urdf_path = pr2_urdfpath() 75 | hash_value = get_urdf_hash(urdf_path) 76 | 77 | # Basic checks 78 | self.assertIsInstance(hash_value, str) 79 | self.assertEqual(len(hash_value), 64) 80 | 81 | # Hash should be consistent 82 | hash_value2 = get_urdf_hash(urdf_path) 83 | self.assertEqual(hash_value, hash_value2) 84 | 85 | def test_get_urdf_hash_fetch(self): 86 | """Test URDF hash calculation with Fetch robot.""" 87 | urdf_path = fetch_urdfpath() 88 | hash_value = get_urdf_hash(urdf_path) 89 | 90 | # Basic checks 91 | self.assertIsInstance(hash_value, str) 92 | self.assertEqual(len(hash_value), 64) 93 | 94 | # Hash should be consistent 95 | hash_value2 = get_urdf_hash(urdf_path) 96 | self.assertEqual(hash_value, hash_value2) 97 | 98 | def test_get_urdf_hash_panda(self): 99 | """Test URDF hash calculation with Panda robot.""" 100 | urdf_path = panda_urdfpath() 101 | hash_value = get_urdf_hash(urdf_path) 102 | 103 | # Basic checks 104 | self.assertIsInstance(hash_value, str) 105 | self.assertEqual(len(hash_value), 64) 106 | 107 | # Hash should be consistent 108 | hash_value2 = get_urdf_hash(urdf_path) 109 | self.assertEqual(hash_value, hash_value2) 110 | 111 | 112 | if __name__ == '__main__': 113 | unittest.main() 114 | -------------------------------------------------------------------------------- /skrobot/data/__init__.py: -------------------------------------------------------------------------------- 1 | import os 2 | import os.path as osp 3 | 4 | from packaging.version import Version 5 | 6 | 7 | data_dir = osp.abspath(osp.dirname(__file__)) 8 | _default_cache_dir = osp.expanduser('~/.skrobot') 9 | 10 | _gdown = None 11 | _gdown_version = None 12 | 13 | 14 | def _lazy_gdown(): 15 | global _gdown 16 | if _gdown is None: 17 | import gdown 18 | _gdown = gdown 19 | return _gdown 20 | 21 | 22 | def _lazy_gdown_version(): 23 | global _gdown_version 24 | if _gdown_version is None: 25 | from skrobot import determine_version 26 | _gdown_version = determine_version('gdown') 27 | 28 | return _gdown_version 29 | 30 | 31 | def get_cache_dir(): 32 | return os.environ.get('SKROBOT_CACHE_DIR', _default_cache_dir) 33 | 34 | 35 | def _download(url, path, md5, postprocess=None, quiet=False): 36 | gdown = _lazy_gdown() 37 | if postprocess == 'extractall': 38 | postprocess = gdown.extractall 39 | if Version(_lazy_gdown_version()) < Version("5.1.0"): 40 | gdown.cached_download( 41 | url=url, path=path, md5=md5, quiet=quiet, 42 | postprocess=postprocess, 43 | ) 44 | else: 45 | gdown.cached_download( 46 | url=url, 47 | path=path, 48 | hash="md5:{}".format(md5), 49 | quiet=quiet, 50 | postprocess=postprocess, 51 | ) 52 | 53 | 54 | def bunny_objpath(): 55 | path = osp.join(get_cache_dir(), 'mesh', 'bunny.obj') 56 | if osp.exists(path): 57 | return path 58 | _download( 59 | url='https://raw.githubusercontent.com/iory/scikit-robot-models/master/data/bunny.obj', # NOQA 60 | path=path, 61 | md5='19bd31bde1fcf5242a8a82ed4ac03c72', 62 | quiet=True, 63 | ) 64 | return path 65 | 66 | 67 | def fetch_urdfpath(): 68 | path = osp.join(get_cache_dir(), 69 | 'fetch_description', 'fetch.urdf') 70 | if osp.exists(path): 71 | return path 72 | _download( 73 | url='https://github.com/iory/scikit-robot-models/raw/master/fetch_description.tar.gz', # NOQA 74 | path=osp.join(get_cache_dir(), 'fetch_description.tar.gz'), 75 | md5='fbe29ab5f3d029d165a625175b43a265', 76 | postprocess='extractall', 77 | quiet=True, 78 | ) 79 | return path 80 | 81 | 82 | def kuka_urdfpath(): 83 | return osp.join(data_dir, 'kuka_description', 'kuka.urdf') 84 | 85 | 86 | def nextage_urdfpath(): 87 | path = osp.join(get_cache_dir(), 'nextage_description', 'urdf', 'NextageOpen.urdf') 88 | if osp.exists(path): 89 | return path 90 | _download( 91 | url='https://github.com/iory/scikit-robot-models/raw/refs/heads/master/nextage_description.tar.gz', # NOQA 92 | path=osp.join(get_cache_dir(), 'nextage_description.tar.gz'), 93 | md5='9805ac9cd97b67056dde31aa88762ec7', 94 | postprocess='extractall', 95 | quiet=True, 96 | ) 97 | return path 98 | 99 | 100 | def panda_urdfpath(): 101 | path = osp.join(get_cache_dir(), 102 | 'franka_description', 'panda.urdf') 103 | _download( 104 | url='https://github.com/iory/scikit-robot-models/raw/master/franka_description.tar.gz', # NOQA 105 | path=osp.join(get_cache_dir(), 'franka_description.tar.gz'), 106 | md5='3de5bd15262b519e3beb88f1422032ac', 107 | postprocess='extractall', 108 | quiet=True, 109 | ) 110 | return path 111 | 112 | 113 | def pr2_urdfpath(): 114 | path = osp.join(get_cache_dir(), 'pr2_description', 'pr2.urdf') 115 | if osp.exists(path): 116 | return path 117 | _download( 118 | url='https://github.com/iory/scikit-robot-models/raw/master/pr2_description.tar.gz', # NOQA 119 | path=osp.join(get_cache_dir(), 'pr2_description.tar.gz'), 120 | md5='6e6e2d4f38e2c5c0a93f44b67962b98a', 121 | postprocess='extractall', 122 | quiet=True, 123 | ) 124 | return path 125 | 126 | 127 | def r8_6_urdfpath(): 128 | return osp.join(data_dir, 'robot_descriptions', 'urdf', 'r8_6.urdf') 129 | -------------------------------------------------------------------------------- /docs/source/examples/index.rst: -------------------------------------------------------------------------------- 1 | .. _examples: 2 | 3 | Usage Examples 4 | ============== 5 | 6 | This page documents several simple use cases for you to try out. 7 | For full details, see the :ref:`reference`, and check out the full 8 | class reference for :class:`.RobotModel`. 9 | 10 | Loading from a File 11 | ------------------- 12 | 13 | You can load a URDF from any ``.urdf`` file, as long as you fix the links 14 | to be relative or absolute links rather than ROS resource URLs. 15 | 16 | >>> import skrobot 17 | >>> robot_model = skrobot.models.urdf.RobotModelFromURDF(urdf_file=skrobot.data.pr2_urdfpath()) 18 | 19 | 20 | Visualization 21 | ------------- 22 | 23 | >>> viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480)) 24 | >>> viewer.add(robot_model) 25 | >>> viewer.show() 26 | 27 | If you would like to update renderer: 28 | 29 | >>> viewer.redraw() 30 | 31 | Accessing Links and Joints 32 | -------------------------- 33 | 34 | You have direct access to link and joint information. 35 | 36 | >>> for link in robot_model.link_list: 37 | ... print(link.name) 38 | 39 | 40 | >>> for joint in robot_model.joint_list: 41 | ... print(joint.name) 42 | 43 | 44 | >>> robot_model.l_elbow_flex_joint.joint_angle() 45 | 0.0 46 | 47 | >>> robot_model.l_elbow_flex_joint.joint_angle(-0.5) 48 | -0.5 49 | 50 | >>> robot_model.l_elbow_flex_joint.joint_angle() 51 | -0.5 52 | 53 | Inverse Kinematics 54 | ------------------ 55 | 56 | This section provides a basic example of inverse kinematics. For comprehensive documentation 57 | including axis constraints, batch processing, and advanced features, see :doc:`../reference/robot_model_tips`. 58 | 59 | First, set the initial pose. Note that the position of the prismatic joint is in [m] and angles of rotational joints are in [rad]. 60 | 61 | >>> robot_model.torso_lift_joint.joint_angle(0.05) 62 | >>> robot_model.l_shoulder_pan_joint.joint_angle(60 * 3.14/180.0) 63 | >>> robot_model.l_shoulder_lift_joint.joint_angle(74 * 3.14/180.0) 64 | >>> robot_model.l_upper_arm_roll_joint.joint_angle(70* 3.14/180.0) 65 | >>> robot_model.l_elbow_flex_joint.joint_angle(-120 * 3.14/180.0) 66 | >>> robot_model.l_forearm_roll_joint.joint_angle(20 * 3.14/180.0) 67 | >>> robot_model.l_wrist_flex_joint.joint_angle(-30 * 3.14/180.0) 68 | >>> robot_model.l_wrist_roll_joint.joint_angle(180 * 3.14/180.0) 69 | >>> robot_model.r_shoulder_pan_joint.joint_angle(-60 * 3.14/180.0) 70 | >>> robot_model.r_shoulder_lift_joint.joint_angle(74 * 3.14/180.0) 71 | >>> robot_model.r_upper_arm_roll_joint.joint_angle(-70 * 3.14/180.0) 72 | >>> robot_model.r_elbow_flex_joint.joint_angle(-120 * 3.14/180.0) 73 | >>> robot_model.r_forearm_roll_joint.joint_angle(-20 * 3.14/180.0) 74 | >>> robot_model.r_wrist_flex_joint.joint_angle(-30 * 3.14/180.0) 75 | >>> robot_model.r_wrist_roll_joint.joint_angle(180 * 3.14/180.0) 76 | >>> robot_model.head_pan_joint.joint_angle(0) 77 | >>> robot_model.head_tilt_joint.joint_angle(0) 78 | 79 | Next, set move_target and link_list 80 | 81 | >>> rarm_end_coords = skrobot.coordinates.CascadedCoords( 82 | ... parent=robot_model.r_gripper_tool_frame, 83 | ... name='rarm_end_coords') 84 | >>> move_target = rarm_end_coords 85 | >>> link_list = [ 86 | ... robot_model.r_shoulder_pan_link, 87 | ... robot_model.r_shoulder_lift_link, 88 | ... robot_model.r_upper_arm_roll_link, 89 | ... robot_model.r_elbow_flex_link, 90 | ... robot_model.r_forearm_roll_link, 91 | ... robot_model.r_wrist_flex_link, 92 | ... robot_model.r_wrist_roll_link] 93 | 94 | Set target_coords. 95 | 96 | >>> target_coords = skrobot.coordinates.Coordinates([0.5, -0.3, 0.7], [0, 0, 0]) 97 | >>> robot_model.inverse_kinematics( 98 | ... target_coords, 99 | ... link_list=link_list, 100 | ... move_target=move_target) 101 | 102 | For detailed information about inverse kinematics including: 103 | 104 | - Axis constraints (``rotation_axis`` and ``translation_axis`` parameters) 105 | - Batch inverse kinematics for multiple poses 106 | - Visual examples of different constraint types 107 | - Advanced features and performance optimization 108 | - Common usage patterns 109 | 110 | See the comprehensive :doc:`../reference/robot_model_tips` documentation. 111 | -------------------------------------------------------------------------------- /examples/grasp_and_pull_box.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import time 5 | 6 | import numpy as np 7 | 8 | import skrobot 9 | from skrobot.coordinates.base import Coordinates 10 | from skrobot.coordinates.geo import midcoords 11 | from skrobot.model.primitives import Box 12 | 13 | 14 | def init_pose(): 15 | robot_model.reset_pose() 16 | 17 | target_coords = skrobot.coordinates.Coordinates( 18 | [0.5, -0.3, 0.7], [0, 0, 0]) 19 | robot_model.inverse_kinematics( 20 | target_coords, 21 | link_list=link_list, 22 | move_target=move_target) 23 | 24 | open_right_gripper() 25 | 26 | 27 | def open_right_gripper(): 28 | robot_model.gripper_distance(0.093, arm='rarm') 29 | viewer.redraw() 30 | 31 | 32 | def close_right_gripper(): 33 | robot_model.gripper_distance(0.0, arm='rarm') 34 | viewer.redraw() 35 | 36 | 37 | def open_left_gripper(): 38 | robot_model.gripper_distance(0.093, arm='larm') 39 | viewer.redraw() 40 | 41 | 42 | def close_left_gripper(): 43 | robot_model.gripper_distance(0.0, arm='larm') 44 | viewer.redraw() 45 | 46 | 47 | def add_box(box_center): 48 | box = Box(extents=[0.1, 0.1, 0.2], with_sdf=True) 49 | box.translate(box_center) 50 | viewer.add(box) 51 | return box 52 | 53 | 54 | def move_to_box(box): 55 | robot_model.look_at(box) 56 | start_coords = move_target.copy_worldcoords() 57 | target_coords = box.copy_worldcoords() 58 | 59 | for i in range(20): 60 | robot_model.inverse_kinematics( 61 | midcoords(i / 20.0, start_coords, target_coords), 62 | link_list=link_list, 63 | move_target=move_target) 64 | viewer.redraw() 65 | time.sleep(0.1) 66 | 67 | 68 | def grasp_box(box): 69 | robot_model.gripper_distance(0.036, arm='rarm') 70 | move_target.assoc(box) 71 | viewer.redraw() 72 | 73 | 74 | def pull_box(box): 75 | target_coords = Coordinates([0.5, -0.3, 0.7]) 76 | start_coords = box 77 | 78 | for i in range(20): 79 | robot_model.inverse_kinematics( 80 | midcoords(i / 20.0, start_coords, target_coords), 81 | link_list=link_list, 82 | move_target=move_target) 83 | robot_model.look_at(box) 84 | viewer.redraw() 85 | time.sleep(0.1) 86 | 87 | 88 | parser = argparse.ArgumentParser( 89 | description='Scikit-robot grasp and pull box example.') 90 | parser.add_argument( 91 | '--no-interactive', 92 | action='store_true', 93 | help="Run in non-interactive mode (do not wait for user input)" 94 | ) 95 | parser.add_argument( 96 | '--viewer', type=str, 97 | choices=['trimesh', 'pyrender'], default='trimesh', 98 | help='Choose the viewer type: trimesh or pyrender') 99 | args = parser.parse_args() 100 | 101 | 102 | # Create robot model 103 | robot_model = skrobot.models.PR2() 104 | link_list = [ 105 | robot_model.r_shoulder_pan_link, 106 | robot_model.r_shoulder_lift_link, 107 | robot_model.r_upper_arm_roll_link, 108 | robot_model.r_elbow_flex_link, 109 | robot_model.r_forearm_roll_link, 110 | robot_model.r_wrist_flex_link, 111 | robot_model.r_wrist_roll_link] 112 | rarm_end_coords = skrobot.coordinates.CascadedCoords( 113 | parent=robot_model.r_gripper_tool_frame, 114 | name='rarm_end_coords') 115 | move_target = rarm_end_coords 116 | 117 | # Create viewer 118 | if args.viewer == 'trimesh': 119 | viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480), 120 | update_interval=0.1) 121 | elif args.viewer == 'pyrender': 122 | viewer = skrobot.viewers.PyrenderViewer(resolution=(640, 480), 123 | update_interval=0.1) 124 | viewer.add(robot_model) 125 | viewer.show() 126 | viewer.set_camera([np.deg2rad(45), -np.deg2rad(0), 127 | np.deg2rad(135)], distance=2.5) 128 | 129 | # Move robot 130 | init_pose() 131 | box_center = np.array([0.9, -0.2, 0.9]) 132 | box = add_box(box_center) 133 | move_to_box(box) 134 | grasp_box(box) 135 | pull_box(box) 136 | 137 | if not args.no_interactive: 138 | print('==> Press [q] to close window') 139 | while viewer.is_active: 140 | time.sleep(0.1) 141 | viewer.redraw() 142 | viewer.close() 143 | time.sleep(1.0) 144 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: Release 2 | 3 | on: 4 | workflow_run: 5 | workflows: ["Run Tests"] 6 | types: 7 | - completed 8 | 9 | jobs: 10 | auto-merge: 11 | if: github.event.workflow_run.conclusion == 'success' 12 | runs-on: ubuntu-latest 13 | outputs: 14 | mergeResult: ${{ steps.merge.outputs.mergeResult }} 15 | prLabels: ${{ steps.get_labels.outputs.labels }} 16 | permissions: 17 | pull-requests: write 18 | contents: write 19 | actions: read 20 | steps: 21 | - name: Checkout repository 22 | uses: actions/checkout@v4 23 | 24 | - name: Get PR labels 25 | id: get_labels 26 | uses: actions/github-script@v7 27 | with: 28 | script: | 29 | const prs = context.payload.workflow_run.pull_requests; 30 | if (!prs || prs.length === 0) { 31 | core.info("Could not find any pull requests in the workflow run."); 32 | core.setOutput("labels", "[]"); 33 | return; 34 | } 35 | const prNumber = prs[0].number; 36 | core.info(`Found PR number: ${prNumber}`); 37 | const { data: pr } = await github.rest.pulls.get({ 38 | owner: context.repo.owner, 39 | repo: context.repo.repo, 40 | pull_number: prNumber, 41 | }); 42 | const labels = pr.labels.map(label => label.name); 43 | core.info(`Retrieved labels: ${labels}`); 44 | core.setOutput("labels", JSON.stringify(labels)); 45 | 46 | - name: Debug labels output 47 | run: | 48 | echo "Labels output: ${{ steps.get_labels.outputs.labels }}" 49 | 50 | - name: Auto merge if auto-merge-ok label exists 51 | id: merge 52 | if: contains(fromJson(steps.get_labels.outputs.labels || '[]'), 'auto-merge-ok') 53 | uses: pascalgn/automerge-action@v0.16.3 54 | env: 55 | GITHUB_TOKEN: "${{ secrets.GITHUB_TOKEN }}" 56 | MERGE_LABELS: "auto-merge-ok" 57 | MERGE_METHOD: squash 58 | MERGE_DELETE_BRANCH: true 59 | 60 | tag-release: 61 | runs-on: ubuntu-latest 62 | needs: auto-merge 63 | if: ${{ needs.auto-merge.outputs.mergeResult == 'merged' && contains(fromJson(needs.auto-merge.outputs.prLabels || '[]'), 'release') }} 64 | steps: 65 | - uses: actions/checkout@v4 66 | with: 67 | ref: main 68 | fetch-depth: 0 69 | token: ${{ secrets.GITHUB_TOKEN }} 70 | 71 | - name: Set up Python 72 | uses: actions/setup-python@v5 73 | with: 74 | python-version: '3.9' 75 | 76 | - name: Wait for apt-get lock 77 | run: | 78 | while sudo fuser /var/lib/dpkg/lock-frontend >/dev/null 2>&1; do 79 | echo "Waiting for apt lock release..." 80 | sleep 1 81 | done 82 | 83 | - name: Install APT On Linux 84 | run: | 85 | sudo apt-get update -qq -y 86 | sudo apt-get install -qq -y libspatialindex-dev freeglut3-dev libsuitesparse-dev libblas-dev liblapack-dev 87 | sudo apt-get install -qq -y xvfb # for headless testing 88 | 89 | - name: Install package 90 | run: | 91 | python -m pip install --upgrade pip setuptools wheel 92 | pip install .\[all\] 93 | 94 | - name: Get version from package 95 | id: get_version 96 | run: | 97 | VERSION=$(python -c "import skrobot; print(skrobot.__version__)") 98 | echo "Detected version: $VERSION" 99 | echo "version=$VERSION" >> "$GITHUB_OUTPUT" 100 | 101 | - name: Tag and push release 102 | run: | 103 | git tag "v${{ steps.get_version.outputs.version }}" 104 | git push origin "v${{ steps.get_version.outputs.version }}" 105 | 106 | pypi: 107 | name: Release To PyPi 108 | needs: tag-release 109 | runs-on: ubuntu-latest 110 | steps: 111 | - uses: actions/checkout@v4 112 | with: 113 | ref: main 114 | fetch-depth: 0 115 | token: ${{ secrets.GITHUB_TOKEN }} 116 | - name: Set up Python 117 | uses: actions/setup-python@v5 118 | with: 119 | python-version: '3.x' 120 | - name: Install publishing dependencies 121 | run: | 122 | python -m pip install --upgrade pip 123 | pip install setuptools wheel 124 | - name: Build 125 | run: | 126 | python setup.py sdist 127 | python setup.py bdist_wheel 128 | - name: Upload to PyPI 129 | uses: pypa/gh-action-pypi-publish@master 130 | with: 131 | password: ${{ secrets.PYPI_TOKEN }} 132 | -------------------------------------------------------------------------------- /skrobot/apps/scale_urdf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import os 5 | import sys 6 | 7 | from skrobot.urdf.scale_urdf import scale_urdf 8 | 9 | 10 | def main(): 11 | """Main entry point for the scale_urdf command. 12 | 13 | This command line tool allows users to scale a URDF model by a given 14 | factor, creating a miniature (scale < 1.0) or enlarged (scale > 1.0) 15 | version of the robot model. 16 | """ 17 | parser = argparse.ArgumentParser( 18 | description='Scale a URDF file by a given factor', 19 | formatter_class=argparse.RawDescriptionHelpFormatter, 20 | epilog=""" 21 | Examples: 22 | # Create a half-scale miniature model 23 | scale-urdf robot.urdf robot_half.urdf --scale 0.5 24 | 25 | # Create a double-size model 26 | scale-urdf robot.urdf robot_2x.urdf --scale 2.0 27 | 28 | # Create a 10cm miniature (10% of original size) 29 | scale-urdf robot.urdf robot_mini.urdf --scale 0.1 30 | 31 | # Modify file in place 32 | scale-urdf robot.urdf --inplace --scale 0.5 33 | 34 | Notes: 35 | This tool scales all geometric and physical properties: 36 | - Joint and link positions (xyz coordinates) 37 | - Mesh geometries (via scale attribute) 38 | - Primitive geometries (box, cylinder, sphere dimensions) 39 | - Mass (scaled by scale^3, assuming constant density) 40 | - Inertia tensors (scaled by scale^5) 41 | """) 42 | 43 | parser.add_argument( 44 | 'input_urdf', 45 | help='Path to the input URDF file' 46 | ) 47 | 48 | parser.add_argument( 49 | 'output_urdf', 50 | nargs='?', 51 | help='Path for the output URDF file (default: _scaled.urdf)' 52 | ) 53 | 54 | parser.add_argument( 55 | '--scale', 56 | type=float, 57 | required=True, 58 | help='Scale factor (e.g., 0.5 for half-size, 2.0 for double-size)' 59 | ) 60 | 61 | parser.add_argument( 62 | '--verbose', '-v', 63 | action='store_true', 64 | help='Enable verbose output' 65 | ) 66 | 67 | parser.add_argument( 68 | '--inplace', '-i', 69 | action='store_true', 70 | help='Modify the input file in place (ignores output_urdf argument)' 71 | ) 72 | 73 | args = parser.parse_args() 74 | 75 | # Check if input file exists 76 | if not os.path.exists(args.input_urdf): 77 | print(f"Error: Input file '{args.input_urdf}' not found.", file=sys.stderr) 78 | sys.exit(1) 79 | 80 | # Validate scale 81 | if args.scale <= 0: 82 | print(f"Error: Scale must be positive, got: {args.scale}", file=sys.stderr) 83 | sys.exit(1) 84 | 85 | # Determine output filename 86 | if args.inplace: 87 | output_urdf = args.input_urdf 88 | if args.verbose: 89 | print(f"Using inplace mode: will modify '{output_urdf}' directly") 90 | else: 91 | output_urdf = args.output_urdf 92 | if not output_urdf: 93 | base, ext = os.path.splitext(args.input_urdf) 94 | output_urdf = f"{base}_scaled{ext}" 95 | 96 | # Check if output file already exists 97 | if os.path.exists(output_urdf): 98 | print(f"Error: Output file '{output_urdf}' already exists.", file=sys.stderr) 99 | print("Please specify a different output file or remove the existing file.", file=sys.stderr) 100 | sys.exit(1) 101 | 102 | if args.verbose: 103 | print(f"Input URDF: {args.input_urdf}") 104 | print(f"Output URDF: {output_urdf}") 105 | print(f"Scale factor: {args.scale}") 106 | if args.scale < 1.0: 107 | percentage = args.scale * 100 108 | print(f"Creating miniature model at {percentage}% of original size") 109 | elif args.scale > 1.0: 110 | print(f"Creating enlarged model at {args.scale}x original size") 111 | else: 112 | print("Scale is 1.0 - creating identical copy") 113 | 114 | try: 115 | scale_urdf( 116 | input_file=args.input_urdf, 117 | output_file=output_urdf, 118 | scale=args.scale 119 | ) 120 | 121 | if args.verbose: 122 | print(f"Successfully created scaled URDF: {output_urdf}") 123 | else: 124 | print(f"Scaled URDF saved to: {output_urdf}") 125 | 126 | except FileNotFoundError as e: 127 | print(f"Error: {e}", file=sys.stderr) 128 | sys.exit(1) 129 | except ValueError as e: 130 | print(f"Error: {e}", file=sys.stderr) 131 | sys.exit(1) 132 | except Exception as e: 133 | print(f"Unexpected error: {e}", file=sys.stderr) 134 | sys.exit(1) 135 | 136 | 137 | if __name__ == '__main__': 138 | main() 139 | -------------------------------------------------------------------------------- /skrobot/apps/convert_wheel_collision.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Convert continuous joint child link collision meshes to cylinders. 4 | 5 | This tool identifies continuous joints in a URDF file and converts 6 | their child links' collision meshes to cylinder primitives. This is 7 | particularly useful for wheel representations where cylinder collisions 8 | are more efficient for physics simulations. 9 | """ 10 | 11 | import argparse 12 | from pathlib import Path 13 | import sys 14 | 15 | from skrobot.urdf import convert_wheel_collisions_to_cylinders 16 | 17 | 18 | def main(): 19 | parser = argparse.ArgumentParser( 20 | description='Convert continuous joint child link collision meshes to cylinders', 21 | formatter_class=argparse.RawDescriptionHelpFormatter, 22 | epilog=""" 23 | Examples: 24 | # Convert wheel collisions in a URDF file 25 | convert-wheel-collision robot.urdf 26 | 27 | # Specify output file 28 | convert-wheel-collision robot.urdf -o robot_cylinder.urdf 29 | 30 | # Modify the input file in place 31 | convert-wheel-collision robot.urdf --inplace 32 | 33 | # Process with verbose output 34 | convert-wheel-collision robot.urdf -v 35 | """ 36 | ) 37 | 38 | parser.add_argument( 39 | 'urdf_file', 40 | type=str, 41 | help='Path to the input URDF file' 42 | ) 43 | 44 | parser.add_argument( 45 | '-o', '--output', 46 | type=str, 47 | default=None, 48 | help='Path to the output URDF file (default: input_cylinder_collision.urdf)' 49 | ) 50 | 51 | parser.add_argument( 52 | '--inplace', 53 | action='store_true', 54 | help='Modify the input URDF file in place' 55 | ) 56 | 57 | parser.add_argument( 58 | '-v', '--verbose', 59 | action='store_true', 60 | help='Enable verbose output' 61 | ) 62 | 63 | parser.add_argument( 64 | '--force', 65 | action='store_true', 66 | help='Overwrite output file if it exists' 67 | ) 68 | 69 | args = parser.parse_args() 70 | 71 | # Configure logging 72 | import logging 73 | if args.verbose: 74 | logging.basicConfig(level=logging.INFO, format='%(message)s') 75 | else: 76 | logging.basicConfig(level=logging.WARNING, format='%(message)s') 77 | 78 | # Check input file 79 | input_path = Path(args.urdf_file) 80 | if not input_path.exists(): 81 | print(f"Error: Input file '{args.urdf_file}' does not exist", file=sys.stderr) 82 | return 1 83 | 84 | # Check for conflicting options 85 | if args.output and args.inplace: 86 | print("Error: Cannot use both --output and --inplace options", file=sys.stderr) 87 | return 1 88 | 89 | # Determine output file 90 | if args.inplace: 91 | output_path = None # Will modify in place 92 | elif args.output: 93 | output_path = Path(args.output) 94 | # Check if output exists 95 | if output_path.exists() and not args.force: 96 | print(f"Error: Output file '{output_path}' already exists. Use --force to overwrite.", file=sys.stderr) 97 | return 1 98 | else: 99 | output_path = input_path.parent / f"{input_path.stem}_cylinder_collision.urdf" 100 | # Check if output exists 101 | if output_path.exists() and not args.force: 102 | print(f"Error: Output file '{output_path}' already exists. Use --force to overwrite.", file=sys.stderr) 103 | return 1 104 | 105 | try: 106 | # Load and convert URDF 107 | print(f"Loading URDF from: {input_path}") 108 | print("Converting wheel collisions to cylinders...") 109 | 110 | # Call the conversion function 111 | modified_links = convert_wheel_collisions_to_cylinders( 112 | str(input_path), 113 | str(output_path) if output_path else None 114 | ) 115 | 116 | if modified_links: 117 | print(f"Modified {len(modified_links)} links:") 118 | for link_name in modified_links: 119 | print(f" - {link_name}") 120 | else: 121 | print("No continuous joint child links found to convert") 122 | 123 | if args.inplace: 124 | print(f"Modified URDF saved in place: {input_path}") 125 | elif output_path: 126 | print(f"Modified URDF saved to: {output_path}") 127 | 128 | print("Conversion completed successfully!") 129 | return 0 130 | 131 | except Exception as e: 132 | print(f"Error: {e}", file=sys.stderr) 133 | if args.verbose: 134 | import traceback 135 | traceback.print_exc() 136 | return 1 137 | 138 | 139 | if __name__ == '__main__': 140 | sys.exit(main()) 141 | -------------------------------------------------------------------------------- /tests/skrobot_tests/planner_tests/test_collision_checker.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import unittest 3 | 4 | import numpy as np 5 | from numpy import testing 6 | 7 | import skrobot 8 | from skrobot.model.primitives import Box 9 | from skrobot.planner import SweptSphereSdfCollisionChecker 10 | from skrobot.planner.utils import set_robot_config 11 | 12 | 13 | def jacobian_test_util(func, x0, decimal=5): 14 | f0, jac = func(x0) 15 | n_dim = len(x0) 16 | 17 | eps = 1e-7 18 | jac_numerical = np.zeros(jac.shape) 19 | for idx in range(n_dim): 20 | x1 = copy.copy(x0) 21 | x1[idx] += eps 22 | f1, _ = func(x1) 23 | jac_numerical[:, idx] = (f1 - f0) / eps 24 | 25 | testing.assert_almost_equal(jac, jac_numerical, decimal=decimal) 26 | 27 | 28 | class TestCollisionChecker(unittest.TestCase): 29 | 30 | @classmethod 31 | def setup_class(cls): 32 | robot_model = skrobot.models.PR2() 33 | robot_model.init_pose() 34 | 35 | table = Box(extents=[0.7, 1.0, 0.05], with_sdf=True) 36 | table.translate([0.8, 0.0, 0.655]) 37 | 38 | link_idx_table = {} 39 | for link_idx in range(len(robot_model.link_list)): 40 | name = robot_model.link_list[link_idx].name 41 | link_idx_table[name] = link_idx 42 | 43 | coll_link_names = ["r_upper_arm_link", "r_forearm_link", 44 | "r_gripper_palm_link", "r_gripper_r_finger_link", 45 | "r_gripper_l_finger_link"] 46 | 47 | coll_link_list = [robot_model.link_list[link_idx_table[lname]] 48 | for lname in coll_link_names] 49 | 50 | link_names = ["r_shoulder_pan_link", "r_shoulder_lift_link", 51 | "r_upper_arm_roll_link", "r_elbow_flex_link", 52 | "r_forearm_roll_link", "r_wrist_flex_link", 53 | "r_wrist_roll_link"] 54 | 55 | link_list = [robot_model.link_list[link_idx_table[lname]] 56 | for lname in link_names] 57 | joint_list = [l.joint for l in link_list] 58 | 59 | coll_checker = SweptSphereSdfCollisionChecker(table.sdf, robot_model) 60 | for link in coll_link_list: 61 | coll_checker.add_collision_link(link) 62 | 63 | cls.robot_model = robot_model 64 | cls.coll_checker = coll_checker 65 | cls.joint_list = joint_list 66 | 67 | def test_update_color(self): 68 | robot_model = self.robot_model 69 | av = np.array([0.4, 0.6] + [-0.7] * 5) 70 | av_with_base = np.hstack((av, [0.1, 0.0, 0.3])) 71 | joint_list = self.joint_list 72 | set_robot_config(robot_model, joint_list, av_with_base, with_base=True) 73 | 74 | dists_ground_truth = np.array([ 75 | 0.15608007, 0.06519901, -0.01225516, -0.04206324, -0.01748433, 76 | -0.01476751, -0.01205069, -0.00933387, -0.00681298, -0.03081892, 77 | -0.02670986, 0.02851278, 0.01134911]) 78 | dists = self.coll_checker.update_color() 79 | testing.assert_almost_equal(dists, dists_ground_truth) 80 | 81 | idxes_colliding = np.where(dists < 0)[0] 82 | color_collide = self.coll_checker.color_collision_sphere 83 | for idx in idxes_colliding: 84 | sphere = self.coll_checker.coll_sphere_list[idx] 85 | color_actual = sphere.colors[0] 86 | testing.assert_equal(color_actual, color_collide) 87 | 88 | def test_coll_batch_forward_kinematics(self): 89 | robot_model = self.robot_model 90 | av = np.array([0.4, 0.6] + [-0.7] * 5) 91 | av_with_base = np.hstack((av, [0.1, 0.0, 0.3])) 92 | joint_list = self.joint_list 93 | set_robot_config(robot_model, joint_list, av_with_base, with_base=True) 94 | 95 | n_wp = 1 96 | n_feature = self.coll_checker.n_feature 97 | 98 | def collision_fk(av, with_base): 99 | # this function wraps _coll_batch_forward_kinematics so that it's 100 | # output will be scipy-style (f, jac) 101 | n_dof = len(av) 102 | f_tmp, jac_tmp = self.coll_checker._coll_batch_forward_kinematics( 103 | joint_list, [av], with_base=with_base, with_jacobian=True) 104 | f = f_tmp.flatten() 105 | jac = jac_tmp.reshape((n_wp * n_feature * 3, n_dof)) 106 | return f, jac 107 | 108 | jacobian_test_util(lambda av: collision_fk(av, False), av) 109 | jacobian_test_util(lambda av: collision_fk(av, True), av_with_base) 110 | 111 | def test_compute_batch_sd_vals(self): 112 | robot_model = self.robot_model 113 | joint_list = self.joint_list 114 | av = np.array([0.4, 0.6] + [-0.7] * 5) 115 | set_robot_config(robot_model, joint_list, av) 116 | 117 | def func(av): 118 | return self.coll_checker.compute_batch_sd_vals( 119 | joint_list, [av], with_jacobian=True) 120 | jacobian_test_util(func, av) 121 | -------------------------------------------------------------------------------- /skrobot/interfaces/ros/transform_listener.py: -------------------------------------------------------------------------------- 1 | import geometry_msgs.msg 2 | import rospy 3 | import tf 4 | import tf2_ros 5 | 6 | 7 | class TransformListener(object): 8 | 9 | """ROS TF Listener class 10 | 11 | """ 12 | 13 | def __init__(self, use_tf2=True): 14 | if use_tf2: 15 | try: 16 | self.tf_listener = tf2_ros.BufferClient("/tf2_buffer_server") 17 | ok = self.tf_listener.wait_for_server(rospy.Duration(10)) 18 | if not ok: 19 | raise Exception( 20 | "timed out: wait_for_server for 10.0 seconds") 21 | except Exception as e: 22 | rospy.logerr("Failed to initialize tf2 client: %s" % str(e)) 23 | rospy.logwarn("Fallback to tf client") 24 | use_tf2 = False 25 | if not use_tf2: 26 | self.tf_listener = tf.TransformListener() 27 | self.use_tf2 = use_tf2 28 | 29 | def _wait_for_transform_tf1(self, 30 | target_frame, source_frame, 31 | time, timeout): 32 | try: 33 | self.tf_listener.waitForTransform( 34 | target_frame, source_frame, time, timeout) 35 | return True 36 | except (tf.LookupException, 37 | tf.ConnectivityException, 38 | tf.ExtrapolationException, 39 | rospy.exceptions.ROSTimeMovedBackwardsException): 40 | return False 41 | 42 | def _wait_for_transform_tf2(self, 43 | target_frame, source_frame, 44 | time, timeout): 45 | try: 46 | ret = self.tf_listener.can_transform( 47 | target_frame, source_frame, time, timeout, True) 48 | if ret[0] > 0: 49 | return True 50 | else: 51 | raise Exception(ret[1]) 52 | except (tf2_ros.LookupException, 53 | tf2_ros.ConnectivityException, 54 | tf2_ros.ExtrapolationException, 55 | tf2_ros.TransformException, 56 | rospy.exceptions.ROSTimeMovedBackwardsException): 57 | return False 58 | 59 | def wait_for_transform(self, 60 | target_frame, source_frame, 61 | time, timeout=rospy.Duration(0)): 62 | if self.use_tf2: 63 | ret = self._wait_for_transform_tf2( 64 | target_frame, source_frame, time, timeout) 65 | else: 66 | ret = self._wait_for_transform_tf1( 67 | target_frame, source_frame, time, timeout) 68 | return ret 69 | 70 | def _lookup_transform_tf1(self, target_frame, source_frame, time, timeout): 71 | self._wait_for_transform_tf1(target_frame, source_frame, time, timeout) 72 | try: 73 | res = self.tf_listener.lookupTransform( 74 | target_frame, source_frame, time) 75 | if time.is_zero(): 76 | time = self.tf_listener.getLatestCommonTime( 77 | target_frame, source_frame) 78 | except (tf.LookupException, 79 | tf.ConnectivityException, 80 | tf.ExtrapolationException, 81 | rospy.exceptions.ROSTimeMovedBackwardsException): 82 | return False 83 | ret = geometry_msgs.msg.TransformStamped() 84 | ret.header.frame_id = target_frame 85 | ret.header.stamp = time 86 | ret.child_frame_id = source_frame 87 | ret.transform.translation.x = res[0][0] 88 | ret.transform.translation.y = res[0][1] 89 | ret.transform.translation.z = res[0][2] 90 | ret.transform.rotation.x = res[1][0] 91 | ret.transform.rotation.y = res[1][1] 92 | ret.transform.rotation.z = res[1][2] 93 | ret.transform.rotation.w = res[1][3] 94 | return ret 95 | 96 | def _lookup_transform_tf2(self, target_frame, source_frame, time, timeout): 97 | try: 98 | return self.tf_listener.lookup_transform( 99 | target_frame, source_frame, time, timeout) 100 | except (tf2_ros.LookupException, 101 | tf2_ros.ConnectivityException, 102 | tf2_ros.ExtrapolationException, 103 | tf2_ros.TransformException, 104 | rospy.exceptions.ROSTimeMovedBackwardsException): 105 | return False 106 | 107 | def lookup_transform(self, 108 | target_frame, 109 | source_frame, 110 | time=rospy.Time(0), 111 | timeout=rospy.Duration(0)): 112 | if self.use_tf2: 113 | ret = self._lookup_transform_tf2( 114 | target_frame, source_frame, time, timeout) 115 | else: 116 | ret = self._lookup_transform_tf1( 117 | target_frame, source_frame, time, timeout) 118 | return ret 119 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import os 4 | import re 5 | import sys 6 | 7 | from setuptools import find_packages 8 | from setuptools import setup 9 | 10 | 11 | version = '0.2.37' 12 | 13 | 14 | def listup_package_data(): 15 | data_files = [] 16 | for root, _, files in os.walk('skrobot/data'): 17 | for filename in files: 18 | data_files.append( 19 | os.path.join( 20 | root[len('skrobot/'):], 21 | filename)) 22 | return data_files 23 | 24 | 25 | setup_requires = [] 26 | 27 | with open('requirements.txt') as f: 28 | install_requires = [] 29 | for line in f: 30 | req = line.split('#')[0].strip() 31 | install_requires.append(req) 32 | 33 | docs_install_requires = [] 34 | with open('requirements_docs.txt') as f: 35 | for line in f: 36 | req = line.split('#')[0].strip() 37 | docs_install_requires.append(req) 38 | 39 | opt_install_requires = [] 40 | with open('requirements_opt.txt') as f: 41 | for line in f: 42 | req = line.split('#')[0].strip() 43 | opt_install_requires.append(req) 44 | 45 | 46 | def remove_from_requirements(install_requires, remove_req): 47 | # If remove_req is "pyglet", requirement name with or without 48 | # version specification will be removed. 49 | # For example, either "pyglet" or "pyglet<2.0", "pyglet==1.4" will 50 | # be removed. 51 | delete_requirement = [] 52 | for req in install_requires: 53 | req_without_version = re.split("[<>=]", req)[0] 54 | if req_without_version == remove_req: 55 | delete_requirement.append(req) 56 | assert len(delete_requirement) == 1, "expect only one match" 57 | install_requires.remove(delete_requirement.pop()) 58 | 59 | 60 | extra_all_requires = ['pybullet>=2.1.9'] 61 | if (sys.version_info.major > 2): 62 | # open3d doesn't support Python 3.13 yet 63 | if (sys.version_info.major, sys.version_info.minor) < (3, 13): 64 | extra_all_requires.append('open3d') 65 | extra_all_requires.append('fast-simplification') 66 | 67 | # Python 2.7 and 3.4 support has been dropped from packages 68 | # version lock those packages here so install succeeds 69 | if (sys.version_info.major, sys.version_info.minor) <= (3, 7): 70 | # packages that no longer support old Python 71 | lock = [('pyglet', '1.4.10', install_requires), 72 | ('cvxopt', '1.2.7', opt_install_requires)] 73 | for name, version, requires in lock: 74 | remove_from_requirements(requires, name) 75 | 76 | # add working version locked requirements 77 | requires.append('{}=={}'.format(name, version)) 78 | 79 | extra_all_requires += docs_install_requires + opt_install_requires 80 | 81 | 82 | console_scripts = ["skr=skrobot.apps.cli:main", 83 | "visualize-urdf=skrobot.apps.visualize_urdf:main"] 84 | if (sys.version_info.major, sys.version_info.minor) >= (3, 6): 85 | console_scripts.append( 86 | "convert-urdf-mesh=skrobot.apps.convert_urdf_mesh:main") 87 | console_scripts.append("modularize-urdf=skrobot.apps.modularize_urdf:main") 88 | console_scripts.append("change-urdf-root=skrobot.apps.change_urdf_root:main") 89 | console_scripts.append("visualize-mesh=skrobot.apps.visualize_mesh:main") 90 | console_scripts.append("urdf-hash=skrobot.apps.urdf_hash:main") 91 | console_scripts.append("convert-wheel-collision=skrobot.apps.convert_wheel_collision:main") 92 | 93 | 94 | setup( 95 | name='scikit-robot', 96 | version=version, 97 | description='A Flexible Framework for Robot Control in Python', 98 | author='iory', 99 | author_email='ab.ioryz@gmail.com', 100 | url='https://github.com/iory/scikit-robot', 101 | long_description=open('README.md').read(), 102 | long_description_content_type='text/markdown', 103 | license='MIT', 104 | classifiers=[ 105 | 'Development Status :: 5 - Production/Stable', 106 | 'Intended Audience :: Developers', 107 | 'Natural Language :: English', 108 | 'License :: OSI Approved :: MIT License', 109 | 'Programming Language :: Python', 110 | 'Programming Language :: Python :: 3.8', 111 | 'Programming Language :: Python :: 3.9', 112 | 'Programming Language :: Python :: 3.10', 113 | 'Programming Language :: Python :: 3.11', 114 | 'Programming Language :: Python :: 3.12', 115 | 'Programming Language :: Python :: 3.13', 116 | 'Programming Language :: Python :: Implementation :: CPython', 117 | ], 118 | packages=find_packages(), 119 | package_data={'skrobot': listup_package_data()}, 120 | zip_safe=False, 121 | setup_requires=setup_requires, 122 | install_requires=install_requires, 123 | entry_points={ 124 | "console_scripts": console_scripts, 125 | }, 126 | extras_require={ 127 | 'opt': opt_install_requires, 128 | 'docs': docs_install_requires, 129 | 'all': extra_all_requires, 130 | }, 131 | ) 132 | -------------------------------------------------------------------------------- /skrobot/viewers/_viser.py: -------------------------------------------------------------------------------- 1 | from typing import Union 2 | import webbrowser 3 | 4 | import numpy as np 5 | import trimesh 6 | import viser 7 | 8 | from skrobot.coordinates.math import matrix2quaternion 9 | from skrobot.model.link import Link 10 | from skrobot.model.primitives import Axis 11 | from skrobot.model.primitives import LineString 12 | from skrobot.model.primitives import PointCloudLink 13 | from skrobot.model.primitives import Sphere 14 | from skrobot.model.robot_model import CascadedLink 15 | 16 | 17 | class ViserVisualizer: 18 | def __init__(self, draw_grid: bool = True): 19 | self._server = viser.ViserServer() 20 | self._linkid_to_handle = dict() 21 | self._linkid_to_link = dict() 22 | 23 | def draw_grid(self, width: float = 20.0, height: float = -0.001): 24 | self._server.scene.add_grid( 25 | "/grid", 26 | width=20.0, 27 | height=20.0, 28 | position=np.array([0.0, 0.0, -0.01]), 29 | ) 30 | 31 | def _add_link(self, link: Link): 32 | assert isinstance(link, Link) 33 | link_id = str(id(link)) 34 | if link_id in self._linkid_to_handle: 35 | return 36 | 37 | handle = None 38 | if isinstance(link, Sphere): 39 | # Although sphere can be treated as trimesh, naively rendering 40 | # it requires high cost. Therefore, we use an analytic sphere. 41 | color = link.visual_mesh.visual.face_colors[0, :3] 42 | alpha = link.visual_mesh.visual.face_colors[0, 3] 43 | if alpha > 1.0: 44 | alpha = alpha / 255.0 45 | handle = self._server.scene.add_icosphere( 46 | link.name, 47 | radius=link.radius, 48 | position=link.worldpos(), 49 | color=color, 50 | opacity=alpha) 51 | elif isinstance(link, Axis): 52 | handle = self._server.scene.add_frame( 53 | link.name, 54 | axes_length=link.axis_length, 55 | axes_radius=link.axis_radius, 56 | wxyz=matrix2quaternion(link.worldrot()), 57 | position=link.worldpos(), 58 | ) 59 | elif isinstance(link, PointCloudLink): 60 | mesh = link.visual_mesh 61 | assert isinstance(mesh, trimesh.PointCloud) 62 | if len(mesh.colors) > 0: 63 | colors = mesh.colors[:, :3] 64 | else: 65 | colors = np.zeros(3) 66 | self._server.scene.add_point_cloud( 67 | link.name, 68 | points=mesh.vertices, 69 | colors=colors, 70 | point_size=0.002, # TODO(HiroIshida): configurable 71 | ) 72 | elif isinstance(link, LineString): 73 | raise NotImplementedError("not implemented yet") 74 | else: 75 | mesh = link.concatenated_visual_mesh 76 | if mesh is not None: 77 | handle = self._server.scene.add_mesh_trimesh( 78 | link.name, 79 | mesh=mesh, 80 | wxyz=matrix2quaternion(link.worldrot()), 81 | position=link.worldpos(), 82 | ) 83 | 84 | if handle is not None: 85 | self._linkid_to_link[link_id] = link 86 | self._linkid_to_handle[link_id] = handle 87 | 88 | def add(self, geometry: Union[Link, CascadedLink]): 89 | if isinstance(geometry, Link): 90 | self._add_link(geometry) 91 | elif isinstance(geometry, CascadedLink): 92 | for link in geometry.link_list: 93 | self._add_link(link) 94 | else: 95 | raise TypeError("geometry must be Link or CascadedLink") 96 | 97 | def show(self): 98 | host = self._server.get_host() 99 | port = self._server.get_port() 100 | url = f"http://{host}:{port}" 101 | webbrowser.open(url) 102 | 103 | def redraw(self): 104 | for link_id, handle in self._linkid_to_handle.items(): 105 | link = self._linkid_to_link[link_id] 106 | handle.position = link.worldpos() 107 | handle.wxyz = matrix2quaternion(link.worldrot()) 108 | 109 | def delete(self, geometry: Union[Link, CascadedLink]): 110 | if isinstance(geometry, Link): 111 | links = [geometry] 112 | elif isinstance(geometry, CascadedLink): 113 | links = geometry.link_list 114 | else: 115 | raise TypeError("geometry must be Link or CascadedLink") 116 | 117 | for link in links: 118 | link_id = str(id(link)) 119 | if link_id not in self._linkid_to_handle: 120 | continue 121 | handle = self._linkid_to_handle[link_id] 122 | handle.remove() 123 | self._linkid_to_link.pop(link_id) 124 | self._linkid_to_handle.pop(link_id) 125 | -------------------------------------------------------------------------------- /tests/skrobot_tests/planner_tests/test_utils.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import unittest 3 | 4 | import numpy as np 5 | from numpy import testing 6 | 7 | import skrobot 8 | from skrobot.planner.utils import forward_kinematics_multi 9 | from skrobot.planner.utils import get_robot_config 10 | from skrobot.planner.utils import set_robot_config 11 | 12 | 13 | def jacobian_test_util(func, x0, decimal=5): 14 | # test jacobian by comparing the resulting and numerical jacobian 15 | f0, jac = func(x0) 16 | n_dim = len(x0) 17 | 18 | eps = 1e-7 19 | jac_numerical = np.zeros(jac.shape) 20 | for idx in range(n_dim): 21 | x1 = copy.copy(x0) 22 | x1[idx] += eps 23 | f1, _ = func(x1) 24 | jac_numerical[:, idx] = (f1 - f0) / eps 25 | testing.assert_almost_equal(jac, jac_numerical, decimal=decimal) 26 | 27 | 28 | class TestPlannerUtils(unittest.TestCase): 29 | 30 | @classmethod 31 | def setup_class(cls): 32 | robot_model = skrobot.models.PR2() 33 | 34 | link_idx_table = {} 35 | for link_idx in range(len(robot_model.link_list)): 36 | name = robot_model.link_list[link_idx].name 37 | link_idx_table[name] = link_idx 38 | 39 | link_names = ["r_shoulder_pan_link", "r_shoulder_lift_link", 40 | "r_upper_arm_roll_link", "r_elbow_flex_link", 41 | "r_forearm_roll_link", "r_wrist_flex_link", 42 | "r_wrist_roll_link"] 43 | 44 | link_list = [robot_model.link_list[link_idx_table[lname]] 45 | for lname in link_names] 46 | joint_list = [l.joint for l in link_list] 47 | 48 | cls.robot_model = robot_model 49 | cls.link_list = link_list 50 | cls.joint_list = joint_list 51 | 52 | av = np.array([0.4, 0.6] + [-0.7] * 5) 53 | cls.av = np.array([0.4, 0.6] + [-0.7] * 5) 54 | cls.av_with_base = np.hstack((av, [0.1, 0.0, 0.3])) 55 | 56 | def test_set_and_get_robot_config(self): 57 | robot_model = self.robot_model 58 | joint_list = self.joint_list 59 | av = self.av 60 | av_with_base = self.av_with_base 61 | 62 | with self.assertRaises(AssertionError): 63 | set_robot_config(robot_model, joint_list, av, with_base=True) 64 | set_robot_config(robot_model, joint_list, av, with_base=False) 65 | 66 | with self.assertRaises(AssertionError): 67 | set_robot_config(robot_model, joint_list, 68 | av_with_base, with_base=False) 69 | set_robot_config(robot_model, joint_list, av_with_base, with_base=True) 70 | 71 | testing.assert_almost_equal( 72 | av, 73 | get_robot_config(robot_model, joint_list, with_base=False) 74 | ) 75 | testing.assert_almost_equal( 76 | av_with_base, 77 | get_robot_config(robot_model, joint_list, with_base=True) 78 | ) 79 | 80 | def test_forward_kinematics_multi(self): 81 | robot_model = self.robot_model 82 | link_list = self.link_list 83 | joint_list = self.joint_list 84 | av_init = self.av 85 | av_with_base_init = self.av_with_base 86 | 87 | move_target_list = link_list 88 | n_feature = len(move_target_list) 89 | 90 | def fk_fun_simple(av, with_base, with_rot, with_jacobian): 91 | pose_arr, jac_arr = forward_kinematics_multi( 92 | robot_model, joint_list, av, move_target_list, 93 | with_rot, with_base, with_jacobian) 94 | return pose_arr, jac_arr 95 | 96 | # checking returning types and shapes: 97 | for with_base, av in [(False, av_init), (True, av_with_base_init)]: 98 | for with_rot, n_pose in [(False, 3), (True, 3 + 4)]: 99 | for with_jacobian in [False, True]: 100 | p, jac = fk_fun_simple( 101 | av, with_base, with_rot, with_jacobian) 102 | self.assertEqual(p.shape, (n_feature, n_pose)) 103 | n_dof = len(av) 104 | if with_jacobian: 105 | self.assertEqual(jac.shape, (n_feature, n_pose, n_dof)) 106 | else: 107 | self.assertEqual(jac, None) 108 | 109 | def fk_jac_test(av, with_base, with_rot): 110 | n_dof = len(av) 111 | pose_arr, jac_arr = fk_fun_simple(av, with_base, with_rot, True) 112 | pose_flatten = pose_arr.flatten() # (pose_dim * n_feature) 113 | pose_dim = 7 if with_rot else 3 114 | jac_flatten = jac_arr.reshape(n_feature * pose_dim, n_dof) 115 | return pose_flatten, jac_flatten 116 | 117 | # checking jacobian 118 | jacobian_test_util( 119 | lambda av: fk_jac_test(av, False, False), av_init) 120 | jacobian_test_util( 121 | lambda av: fk_jac_test(av, False, True), av_init) 122 | jacobian_test_util( 123 | lambda av: fk_jac_test(av, True, False), av_with_base_init) 124 | jacobian_test_util( 125 | lambda av: fk_jac_test(av, True, True), av_with_base_init) 126 | -------------------------------------------------------------------------------- /docs/source/cli.rst: -------------------------------------------------------------------------------- 1 | Command Line Interface (CLI) 2 | ============================ 3 | 4 | Scikit-robot provides a unified command-line interface through the ``skr`` command, which consolidates all robot-related tools into a single entry point. This design makes it easy to discover and use various robot manipulation tools. 5 | 6 | Installation 7 | ------------ 8 | 9 | The CLI tools are automatically installed when you install scikit-robot: 10 | 11 | .. code-block:: bash 12 | 13 | pip install scikit-robot 14 | 15 | Getting Started 16 | --------------- 17 | 18 | To see all available commands: 19 | 20 | .. code-block:: bash 21 | 22 | skr --help 23 | 24 | This will display a list of all available subcommands along with their descriptions. 25 | 26 | Available Commands 27 | ------------------ 28 | 29 | visualize-urdf 30 | ~~~~~~~~~~~~~~ 31 | 32 | Visualize URDF robot models in an interactive 3D viewer. 33 | 34 | .. code-block:: bash 35 | 36 | # Basic usage 37 | skr visualize-urdf robot.urdf 38 | 39 | # With specific viewer 40 | skr visualize-urdf robot.urdf --viewer trimesh 41 | skr visualize-urdf robot.urdf --viewer pyrender 42 | 43 | convert-urdf-mesh 44 | ~~~~~~~~~~~~~~~~~ 45 | 46 | Convert mesh files referenced in URDF to different formats or simplify them. 47 | 48 | .. code-block:: bash 49 | 50 | # Convert meshes 51 | skr convert-urdf-mesh robot.urdf --output converted_robot.urdf 52 | 53 | # Simplify meshes with voxel size 54 | skr convert-urdf-mesh robot.urdf --voxel-size 0.001 55 | 56 | # Convert to STL format 57 | skr convert-urdf-mesh robot.urdf --output robot_stl.urdf -f stl 58 | 59 | change-urdf-root 60 | ~~~~~~~~~~~~~~~~ 61 | 62 | Change the root link of a URDF file to a different link. 63 | 64 | .. code-block:: bash 65 | 66 | # Change root link 67 | skr change-urdf-root robot.urdf new_root_link output.urdf 68 | 69 | # List available links 70 | skr change-urdf-root robot.urdf --list 71 | 72 | # Verbose output 73 | skr change-urdf-root robot.urdf new_root output.urdf --verbose 74 | 75 | modularize-urdf 76 | ~~~~~~~~~~~~~~~ 77 | 78 | Modularize URDF files by breaking them into reusable components. 79 | 80 | .. code-block:: bash 81 | 82 | skr modularize-urdf robot.urdf --output modular_robot.urdf 83 | 84 | urdf-hash 85 | ~~~~~~~~~ 86 | 87 | Calculate a hash value for URDF files to track changes and versions. 88 | 89 | .. code-block:: bash 90 | 91 | skr urdf-hash robot.urdf 92 | 93 | visualize-mesh 94 | ~~~~~~~~~~~~~~ 95 | 96 | Visualize individual mesh files in 3D. 97 | 98 | .. code-block:: bash 99 | 100 | skr visualize-mesh mesh_file.stl 101 | skr visualize-mesh mesh_file.obj 102 | 103 | convert-wheel-collision 104 | ~~~~~~~~~~~~~~~~~~~~~~~ 105 | 106 | Convert wheel collision models in URDF files. 107 | 108 | .. code-block:: bash 109 | 110 | skr convert-wheel-collision robot.urdf --output converted.urdf 111 | 112 | Backward Compatibility 113 | ---------------------- 114 | 115 | For backward compatibility, all original individual commands are still available: 116 | 117 | .. code-block:: bash 118 | 119 | # These commands work the same as their skr equivalents 120 | visualize-urdf robot.urdf 121 | convert-urdf-mesh robot.urdf --output converted.urdf 122 | change-urdf-root robot.urdf new_root output.urdf 123 | modularize-urdf robot.urdf --output modular.urdf 124 | urdf-hash robot.urdf 125 | visualize-mesh mesh_file.stl 126 | convert-wheel-collision robot.urdf --output converted.urdf 127 | 128 | Getting Help 129 | ------------ 130 | 131 | Each subcommand provides its own help information: 132 | 133 | .. code-block:: bash 134 | 135 | # General help 136 | skr --help 137 | 138 | # Help for specific commands 139 | skr visualize-urdf --help 140 | skr convert-urdf-mesh --help 141 | skr change-urdf-root --help 142 | 143 | Examples 144 | -------- 145 | 146 | Here are some common usage examples: 147 | 148 | Visualizing a Robot Model 149 | ~~~~~~~~~~~~~~~~~~~~~~~~~ 150 | 151 | .. code-block:: bash 152 | 153 | # Download a sample robot model and visualize it 154 | skr visualize-urdf ~/.skrobot/pr2_description/pr2.urdf --viewer trimesh 155 | 156 | Converting Mesh Formats 157 | ~~~~~~~~~~~~~~~~~~~~~~~~ 158 | 159 | .. code-block:: bash 160 | 161 | # Convert all meshes in a URDF to STL format 162 | skr convert-urdf-mesh robot.urdf --output robot_stl.urdf -f stl 163 | 164 | # Simplify meshes by decimation 165 | skr convert-urdf-mesh robot.urdf -d 0.98 --output simplified.urdf 166 | 167 | Changing Robot Structure 168 | ~~~~~~~~~~~~~~~~~~~~~~~~ 169 | 170 | .. code-block:: bash 171 | 172 | # First, see what links are available 173 | skr change-urdf-root robot.urdf --list 174 | 175 | # Then change the root to a specific link 176 | skr change-urdf-root robot.urdf base_link new_robot.urdf 177 | 178 | Architecture 179 | ------------ 180 | 181 | The CLI system is designed to be extensible. New commands can be added by: 182 | 183 | 1. Creating a new Python module in ``skrobot/apps/`` with a ``main()`` function 184 | 2. The CLI will automatically discover and register the new command 185 | 3. Command names are derived from the module filename (underscores become hyphens) 186 | 187 | This modular design makes it easy to add new functionality while maintaining a consistent interface. --------------------------------------------------------------------------------