├── oscbf ├── core │ ├── __init__.py │ ├── franka_collision_model.py │ ├── oscbf_configs.py │ └── manipulation_env.py ├── utils │ ├── __init__.py │ ├── urdfpy │ │ ├── version.py │ │ ├── __init__.py │ │ └── utils.py │ ├── general_utils.py │ ├── trajectory.py │ └── visualization.py ├── __init__.py ├── assets │ ├── franka_panda │ │ ├── meshes │ │ │ ├── visual │ │ │ │ ├── colors.png │ │ │ │ ├── link1.mtl │ │ │ │ ├── link2.mtl │ │ │ │ ├── finger.mtl │ │ │ │ ├── link5.mtl │ │ │ │ ├── link4.mtl │ │ │ │ ├── link3.mtl │ │ │ │ ├── hand.mtl │ │ │ │ └── link6.mtl │ │ │ └── collision │ │ │ │ ├── link6.mtl │ │ │ │ ├── finger.obj │ │ │ │ ├── link0.obj │ │ │ │ └── hand.obj │ │ ├── README.md │ │ └── panda.urdf │ └── point_robot.urdf ├── scripts │ ├── visualize_franka_collision_model.py │ └── validate_urdf_parsing.py └── examples │ ├── self_collision.py │ ├── multiple_safety_conditions.py │ ├── dynamic_obstacle.py │ ├── dynamic_motion.py │ └── cluttered_tabletop.py ├── .gitignore ├── LICENSE ├── pyproject.toml ├── README.md ├── test ├── test_manipulator_dynamics.py └── test_speed.py └── pylintrc /oscbf/core/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /oscbf/utils/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /oscbf/utils/urdfpy/version.py: -------------------------------------------------------------------------------- 1 | __version__ = "0.0.22" 2 | -------------------------------------------------------------------------------- /oscbf/__init__.py: -------------------------------------------------------------------------------- 1 | import jax 2 | 3 | jax.config.update("jax_enable_x64", True) 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.egg-info/* 2 | __pycache__ 3 | .python-version 4 | site/ 5 | .vscode/ 6 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/colors.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/oscbf/HEAD/oscbf/assets/franka_panda/meshes/visual/colors.png -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/link1.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 1 3 | 4 | newmtl Part__Feature_001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.062500 0.062500 0.062500 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/link2.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link3.blend' 2 | # Material Count: 1 3 | 4 | newmtl Part__Feature024 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.125000 0.125000 0.125000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/README.md: -------------------------------------------------------------------------------- 1 | URDF is originally from https://github.com/erwincoumans/pybullet_robots/tree/master/data/franka_panda 2 | 3 | Modifications include: 4 | - Removing the `package://` linking to meshes 5 | - Set a fixed joint axis to `0, 0, 1` instead of `0, 0, 0` (which can have issues with certain parsers) 6 | - Locked the gripper joints (to ignore in the kinematic chain) 7 | - Removed "safety controller" entries 8 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/finger.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 2 3 | 4 | newmtl Part__Feature001_006.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.901961 0.921569 0.929412 8 | Ks 0.250000 0.250000 0.250000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature_007.001 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.250980 0.250980 0.250980 19 | Ks 0.250000 0.250000 0.250000 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | -------------------------------------------------------------------------------- /oscbf/assets/point_robot.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 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/link5.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 3 3 | 4 | newmtl Part__Feature_002_004_003.002 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.015625 0.015625 0.015625 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Shell001_001_001_003.002 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.250000 0.250000 0.250000 19 | Ks 0.015625 0.015625 0.015625 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | 26 | newmtl Shell_001_001_003.002 27 | Ns -1.960784 28 | Ka 1.000000 1.000000 1.000000 29 | Kd 1.000000 1.000000 1.000000 30 | Ks 0.015625 0.015625 0.015625 31 | Ke 0.000000 0.000000 0.000000 32 | Ni 1.000000 33 | d 1.000000 34 | illum 2 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Daniel Morton 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [project] 6 | name = "oscbf" 7 | version = "0.0.1" 8 | description = "Operational Space Control Barrier Functions" 9 | authors = [{ name = "Daniel Morton", email = "danielpmorton@gmail.com" }] 10 | readme = "README.md" 11 | keywords = ["control", "barrier", "function", "CBF", "Jax", "operational", "space", "safe", "robotics"] 12 | # TODO: Have a better way of managing these dependencies 13 | # There was a slowdown introduced in jax 0.4.32 14 | dependencies = [ 15 | "numpy", 16 | "jax==0.4.30", 17 | "jaxlib==0.4.30", 18 | "cbfpy==0.0.1", 19 | "qpax", 20 | "pybullet>=3.2.7", # Pybullet < 3.2.7 has trouble with numpy 2.0 21 | "matplotlib", 22 | # Dependencies from urdfpy... I should try to remove some of these 23 | "networkx", 24 | "lxml", 25 | "trimesh", 26 | ] 27 | 28 | [project.urls] 29 | Repository = "https://github.com/stanfordasl/oscbf/" 30 | 31 | [project.optional-dependencies] 32 | dev = ["pylint", "black"] 33 | 34 | [tool.setuptools.packages.find] 35 | exclude = ["artifacts", "images"] 36 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/link4.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link4.blend' 2 | # Material Count: 4 3 | 4 | newmtl Part__Feature001_001_003_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Feature002_001_003_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.250980 0.250980 0.250980 18 | Ks 0.007812 0.007812 0.007812 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | map_Kd colors.png 24 | 25 | newmtl Part__Feature003_001_003_001.001 26 | Ns -1.960784 27 | Ka 1.000000 1.000000 1.000000 28 | Kd 1.000000 1.000000 1.000000 29 | Ks 0.007812 0.007812 0.007812 30 | Ke 0.000000 0.000000 0.000000 31 | Ni 1.000000 32 | d 1.000000 33 | illum 2 34 | map_Kd colors.png 35 | 36 | newmtl Part__Feature_002_003_001.001 37 | Ns -1.960784 38 | Ka 1.000000 1.000000 1.000000 39 | Kd 1.000000 1.000000 1.000000 40 | Ks 0.007812 0.007812 0.007812 41 | Ke 0.000000 0.000000 0.000000 42 | Ni 1.000000 43 | d 1.000000 44 | illum 2 45 | map_Kd colors.png 46 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/link3.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link4.blend' 2 | # Material Count: 4 3 | 4 | newmtl Part__Feature001_010_001_002 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature002_007_001_002 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 1.000000 1.000000 1.000000 19 | Ks 0.007812 0.007812 0.007812 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | 26 | newmtl Part__Feature003_004_001_002 27 | Ns -1.960784 28 | Ka 1.000000 1.000000 1.000000 29 | Kd 1.000000 1.000000 1.000000 30 | Ks 0.007812 0.007812 0.007812 31 | Ke 0.000000 0.000000 0.000000 32 | Ni 1.000000 33 | d 1.000000 34 | illum 2 35 | map_Kd colors.png 36 | 37 | newmtl Part__Feature_001_001_001_002 38 | Ns -1.960784 39 | Ka 1.000000 1.000000 1.000000 40 | Kd 0.250980 0.250980 0.250980 41 | Ks 0.007812 0.007812 0.007812 42 | Ke 0.000000 0.000000 0.000000 43 | Ni 1.000000 44 | d 1.000000 45 | illum 2 46 | map_Kd colors.png 47 | -------------------------------------------------------------------------------- /oscbf/utils/urdfpy/__init__.py: -------------------------------------------------------------------------------- 1 | from .urdf import ( 2 | URDFType, 3 | Box, 4 | Cylinder, 5 | Sphere, 6 | Mesh, 7 | Geometry, 8 | Texture, 9 | Material, 10 | Collision, 11 | Visual, 12 | Inertial, 13 | JointCalibration, 14 | JointDynamics, 15 | JointLimit, 16 | JointMimic, 17 | SafetyController, 18 | Actuator, 19 | TransmissionJoint, 20 | Transmission, 21 | Joint, 22 | Link, 23 | URDF, 24 | ) 25 | from .utils import rpy_to_matrix, matrix_to_rpy, xyz_rpy_to_matrix, matrix_to_xyz_rpy 26 | from .version import __version__ 27 | 28 | __all__ = [ 29 | "URDFType", 30 | "Box", 31 | "Cylinder", 32 | "Sphere", 33 | "Mesh", 34 | "Geometry", 35 | "Texture", 36 | "Material", 37 | "Collision", 38 | "Visual", 39 | "Inertial", 40 | "JointCalibration", 41 | "JointDynamics", 42 | "JointLimit", 43 | "JointMimic", 44 | "SafetyController", 45 | "Actuator", 46 | "TransmissionJoint", 47 | "Transmission", 48 | "Joint", 49 | "Link", 50 | "URDF", 51 | "rpy_to_matrix", 52 | "matrix_to_rpy", 53 | "xyz_rpy_to_matrix", 54 | "matrix_to_xyz_rpy", 55 | "__version__", 56 | ] 57 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/hand.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 5 3 | 4 | newmtl Part__Feature001_008_005.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.250980 0.250980 0.250980 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature002_005_005.001 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.901961 0.921569 0.929412 19 | Ks 0.015625 0.015625 0.015625 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | 25 | newmtl Part__Feature005_001_005.001 26 | Ns -1.960784 27 | Ka 1.000000 1.000000 1.000000 28 | Kd 1.000000 1.000000 1.000000 29 | Ks 0.015625 0.015625 0.015625 30 | Ke 0.000000 0.000000 0.000000 31 | Ni 1.000000 32 | d 1.000000 33 | illum 2 34 | map_Kd colors.png 35 | 36 | newmtl Part__Feature005_001_005_001.001 37 | Ns -1.960784 38 | Ka 1.000000 1.000000 1.000000 39 | Kd 0.901961 0.921569 0.929412 40 | Ks 0.015625 0.015625 0.015625 41 | Ke 0.000000 0.000000 0.000000 42 | Ni 1.000000 43 | d 1.000000 44 | illum 2 45 | map_Kd colors.png 46 | 47 | newmtl Part__Feature_009_005.001 48 | Ns -1.960784 49 | Ka 1.000000 1.000000 1.000000 50 | Kd 0.250980 0.250980 0.250980 51 | Ks 0.015625 0.015625 0.015625 52 | Ke 0.000000 0.000000 0.000000 53 | Ni 1.000000 54 | d 1.000000 55 | illum 2 56 | map_Kd colors.png 57 | -------------------------------------------------------------------------------- /oscbf/utils/general_utils.py: -------------------------------------------------------------------------------- 1 | """Assorted utility functions""" 2 | 3 | import os 4 | import sys 5 | from contextlib import contextmanager 6 | 7 | 8 | # NOTE: This might be overkill if we always assume that this script is in the utils/ folder. But, it seems to work 9 | def find_toplevel_dir(): 10 | current_dir = os.path.dirname(os.path.abspath(__file__)) 11 | while current_dir != os.path.dirname( 12 | current_dir 13 | ): # Check if we've reached the root 14 | if "setup.py" in os.listdir(current_dir) or ".git" in os.listdir(current_dir): 15 | # Found the top-level package directory 16 | return current_dir 17 | current_dir = os.path.dirname(current_dir) 18 | # Top-level directory not found, handle appropriately 19 | raise RuntimeError("Top-level directory not found") 20 | 21 | 22 | def find_assets_dir(): 23 | assets_dir = os.path.join(find_toplevel_dir(), "oscbf/assets/") 24 | if not os.path.exists(assets_dir): 25 | raise RuntimeError("Assets directory not found") 26 | return assets_dir 27 | 28 | 29 | # The motivation for this is Pybullet prints a ton of logging info when it launches, but I don't necessarily want this 30 | # printed out. See the following stack overflow thread for the source: 31 | # https://stackoverflow.com/questions/5081657/how-do-i-prevent-a-c-shared-library-to-print-on-stdout-in-python/17954769#17954769 32 | @contextmanager 33 | def stdout_redirected(to: str = os.devnull): 34 | """Temporarily redirects `sys.stdout` to the specified file 35 | 36 | This context manager is useful for silencing output or redirecting it to a 37 | file or other writable stream during the execution of a code block. 38 | 39 | Example: 40 | ``` 41 | import os 42 | with stdout_redirected(to=filename): 43 | print("from Python") 44 | os.system("echo non-Python applications are also supported") 45 | ``` 46 | 47 | Args: 48 | to (str): The target file where stdout should be redirected. 49 | Defaults to `os.devnull` (silencing output). 50 | """ 51 | fd = sys.stdout.fileno() 52 | 53 | ##### assert that Python and C stdio write using the same file descriptor 54 | ####assert libc.fileno(ctypes.c_void_p.in_dll(libc, "stdout")) == fd == 1 55 | 56 | def _redirect_stdout(to): 57 | sys.stdout.close() # + implicit flush() 58 | os.dup2(to.fileno(), fd) # fd writes to 'to' file 59 | sys.stdout = os.fdopen(fd, "w") # Python writes to fd 60 | 61 | with os.fdopen(os.dup(fd), "w") as old_stdout: 62 | with open(to, "w") as file: 63 | _redirect_stdout(to=file) 64 | try: 65 | yield # allow code to be run with the redirected stdout 66 | finally: 67 | _redirect_stdout(to=old_stdout) # restore stdout. 68 | # buffering and flags such as 69 | # CLOEXEC may be different 70 | -------------------------------------------------------------------------------- /oscbf/scripts/visualize_franka_collision_model.py: -------------------------------------------------------------------------------- 1 | """Script to visualize the hand-designed collision model of the Franka Panda in Pybullet""" 2 | 3 | import pybullet 4 | import numpy as np 5 | 6 | import oscbf.core.franka_collision_model as colmodel 7 | from oscbf.core.manipulator import create_transform_numpy, load_panda 8 | from oscbf.utils.visualization import visualize_3D_sphere 9 | 10 | np.random.seed(0) 11 | 12 | URDF = "oscbf/assets/franka_panda/panda.urdf" 13 | FRANKA_INIT_QPOS = np.array( 14 | [0.0, -np.pi / 6, 0.0, -3 * np.pi / 4, 0.0, 5 * np.pi / 9, 0.0] 15 | ) 16 | RANDOMIZE = False 17 | 18 | 19 | def visualize_collision_model( 20 | positions, 21 | radii, 22 | pairs=None, 23 | base_position=None, 24 | base_radius=None, 25 | base_sc_idxs=None, 26 | ): 27 | pybullet.connect(pybullet.GUI) 28 | robot = pybullet.loadURDF( 29 | URDF, 30 | useFixedBase=True, 31 | flags=pybullet.URDF_USE_INERTIA_FROM_FILE | pybullet.URDF_MERGE_FIXED_LINKS, 32 | ) 33 | pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0) 34 | 35 | manipulator = load_panda() 36 | for link_idx in range(manipulator.num_joints): 37 | pybullet.changeVisualShape(robot, link_idx, rgbaColor=(0, 0, 0, 0.5)) 38 | pybullet.changeVisualShape(robot, -1, rgbaColor=(0, 0, 0, 0.5)) 39 | 40 | # input("Press Enter to randomize the joints") 41 | if RANDOMIZE: 42 | q = np.random.rand(manipulator.num_joints) 43 | else: 44 | q = FRANKA_INIT_QPOS 45 | for i in range(manipulator.num_joints): 46 | pybullet.resetJointState(robot, i, q[i]) 47 | pybullet.stepSimulation() 48 | joint_transforms = manipulator.joint_to_world_transforms(q) 49 | 50 | sphere_ids = [] 51 | sphere_positions = [] 52 | # Determine the world-frame positions of the collision geometry 53 | for i in range(manipulator.num_joints): 54 | parent_to_world_tf = joint_transforms[i] 55 | num_collision_spheres = len(positions[i]) 56 | for j in range(num_collision_spheres): 57 | collision_to_parent_tf = create_transform_numpy(np.eye(3), positions[i][j]) 58 | collision_to_world_tf = parent_to_world_tf @ collision_to_parent_tf 59 | world_pos = collision_to_world_tf[:3, 3] 60 | sphere_ids.append(visualize_3D_sphere(world_pos, radii[i][j])) 61 | sphere_positions.append(world_pos) 62 | 63 | if pairs is not None: 64 | for pair in pairs: 65 | i, j = pair 66 | rgb = (1, 0, 0) 67 | pybullet.addUserDebugLine(sphere_positions[i], sphere_positions[j], rgb) 68 | 69 | if base_position is not None: 70 | visualize_3D_sphere(base_position, base_radius) 71 | 72 | if base_sc_idxs is not None: 73 | for idx in base_sc_idxs: 74 | rgb = (1, 0, 0) 75 | pybullet.addUserDebugLine(base_position, sphere_positions[idx], rgb) 76 | 77 | input("Press Enter to exit") 78 | 79 | 80 | def main(self_collision=False): 81 | if self_collision: 82 | visualize_collision_model( 83 | colmodel.positions_list_sc, 84 | colmodel.radii_list_sc, 85 | colmodel.pairs_sc, 86 | colmodel.base_position, 87 | colmodel.base_radius, 88 | colmodel.base_sc_idxs, 89 | ) 90 | else: 91 | visualize_collision_model(colmodel.positions_list, colmodel.radii_list) 92 | 93 | 94 | if __name__ == "__main__": 95 | main() 96 | -------------------------------------------------------------------------------- /oscbf/core/franka_collision_model.py: -------------------------------------------------------------------------------- 1 | """Creating a link collision model for the Franka with a series of spheres of various radii""" 2 | 3 | # All positions are in link frame, not link COM frame 4 | 5 | link_1_pos = ( 6 | (0, 0, -0.15), 7 | (0, -0.065, 0), 8 | ) 9 | link_1_radii = ( 10 | 0.06, 11 | 0.06, 12 | ) 13 | 14 | link_2_pos = ( 15 | (0, 0, 0.065), 16 | (0, -0.14, 0), 17 | ) 18 | link_2_radii = ( 19 | 0.06, 20 | 0.06, 21 | ) 22 | 23 | link_3_pos = ( 24 | (0, 0, -0.065), 25 | (0.08, 0.065, 0), 26 | ) 27 | link_3_radii = ( 28 | 0.06, 29 | 0.055, 30 | ) 31 | 32 | link_4_pos = ( 33 | (0, 0, 0.065), 34 | (-0.08, 0.08, 0), 35 | ) 36 | link_4_radii = ( 37 | 0.055, 38 | 0.055, 39 | ) 40 | 41 | link_5_pos = ( 42 | (0, 0, -0.23), 43 | (0, 0.06, -0.18), 44 | (0, 0.08, -0.125), 45 | (0, 0.09, -0.075), 46 | (0, 0.08, 0), 47 | ) 48 | link_5_radii = ( 49 | 0.06, 50 | 0.04, 51 | 0.025, 52 | 0.025, 53 | 0.055, 54 | ) 55 | 56 | link_6_pos = ( 57 | (0, 0, 0.01), 58 | (0.08, 0.035, 0), 59 | # (0.08, -0.02, 0), 60 | ) 61 | link_6_radii = ( 62 | 0.05, 63 | 0.05, 64 | # 0.05, 65 | ) 66 | 67 | link_7_pos = ( 68 | (0, 0, 0.08), 69 | (0.04, 0.04, 0.09), 70 | (0.055, 0.055, 0.15), 71 | (-0.055, -0.055, 0.15), 72 | (-0.055, -0.055, 0.11), 73 | (0, 0, 0.20), 74 | ) 75 | link_7_radii = ( 76 | 0.05, 77 | 0.04, 78 | 0.03, 79 | 0.03, 80 | 0.03, 81 | 0.02, 82 | ) 83 | 84 | positions_list = ( 85 | link_1_pos, 86 | link_2_pos, 87 | link_3_pos, 88 | link_4_pos, 89 | link_5_pos, 90 | link_6_pos, 91 | link_7_pos, 92 | ) 93 | radii_list = ( 94 | link_1_radii, 95 | link_2_radii, 96 | link_3_radii, 97 | link_4_radii, 98 | link_5_radii, 99 | link_6_radii, 100 | link_7_radii, 101 | ) 102 | 103 | franka_collision_data = {"positions": positions_list, "radii": radii_list} 104 | 105 | ##### Self collisions ##### 106 | 107 | # Note: this is a very approximate model with only a small subset of the full geometry. 108 | # However, these few collision pairs are the most relevant for the Franka, and 109 | # tabletop-like tasks in a standard configuration 110 | 111 | link_1_pos_sc = ( 112 | (0, 0, -0.13), # 0 113 | (0.0, -0.05, 0.0), # 1 114 | ) 115 | link_1_radii_sc = ( 116 | 0.085, 117 | 0.085, 118 | ) 119 | 120 | link_2_pos_sc = ((0, 0, 0.05),) # 2 121 | link_2_radii_sc = (0.085,) 122 | 123 | link_3_pos_sc = () 124 | link_3_radii_sc = () 125 | 126 | link_4_pos_sc = () 127 | link_4_radii_sc = () 128 | 129 | link_5_pos_sc = ( 130 | (0, 0, -0.23), # 3 131 | (0, 0.09, -0.075), # 4 132 | (0, 0.08, 0), # 5 133 | ) 134 | link_5_radii_sc = ( 135 | 0.08, 136 | 0.04, 137 | 0.065, 138 | ) 139 | 140 | link_6_pos_sc = () 141 | link_6_radii_sc = () 142 | 143 | link_7_pos_sc = ((0, 0, 0.12),) # 6 144 | link_7_radii_sc = (0.12,) 145 | 146 | positions_list_sc = ( 147 | link_1_pos_sc, 148 | link_2_pos_sc, 149 | link_3_pos_sc, 150 | link_4_pos_sc, 151 | link_5_pos_sc, 152 | link_6_pos_sc, 153 | link_7_pos_sc, 154 | ) 155 | radii_list_sc = ( 156 | link_1_radii_sc, 157 | link_2_radii_sc, 158 | link_3_radii_sc, 159 | link_4_radii_sc, 160 | link_5_radii_sc, 161 | link_6_radii_sc, 162 | link_7_radii_sc, 163 | ) 164 | pairs_sc = ( 165 | (0, 6), 166 | (1, 4), 167 | (1, 5), 168 | (1, 6), 169 | (2, 6), 170 | (3, 6), 171 | ) 172 | 173 | franka_self_collision_data = { 174 | "positions": positions_list_sc, 175 | "radii": radii_list_sc, 176 | "pairs": pairs_sc, 177 | } 178 | 179 | base_position = (-0.04, 0.0, 0.05) 180 | base_radius = 0.135 181 | base_sc_idxs = (6,) 182 | 183 | base_self_collision_data = { 184 | "position": base_position, 185 | "radius": base_radius, 186 | "indices": base_sc_idxs, 187 | } -------------------------------------------------------------------------------- /oscbf/scripts/validate_urdf_parsing.py: -------------------------------------------------------------------------------- 1 | """Script to check that the URDF was properly parsed 2 | 3 | This will bring up a visualizer in Pybullet and show the transformations between joints 4 | based on the parsed kinematics. 5 | 6 | If the URDF was not parsed properly, the lines will not match up with the robot, as 7 | the robot is moved around in the GUI 8 | 9 | NOTE: Since reference frames may be coincident with eachother, the number of points and lines 10 | might appear to be lower than expected 11 | """ 12 | 13 | import pybullet 14 | import numpy as np 15 | 16 | from oscbf.core.manipulator import Manipulator 17 | 18 | URDF = "oscbf/assets/franka_panda/panda.urdf" 19 | FRANKA_INIT_QPOS = np.array( 20 | [0.0, -np.pi / 6, 0.0, -3 * np.pi / 4, 0.0, 5 * np.pi / 9, 0.0] 21 | ) 22 | 23 | 24 | def visualize_parsed_tfs(urdf: str, randomize: bool = False): 25 | pybullet.connect(pybullet.GUI) 26 | pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0) 27 | pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_WIREFRAME, 1) 28 | robot = pybullet.loadURDF( 29 | urdf, 30 | useFixedBase=True, 31 | flags=pybullet.URDF_MERGE_FIXED_LINKS | pybullet.URDF_USE_INERTIA_FROM_FILE, 32 | ) 33 | manipulator = Manipulator.from_urdf(urdf) 34 | 35 | if randomize: 36 | # Set some random joint angles 37 | for i in range(manipulator.num_joints): 38 | pybullet.resetJointState(robot, i, 2 * np.random.rand() - 1) 39 | else: 40 | # Use a "good" initial joint configuration 41 | for i in range(manipulator.num_joints): 42 | pybullet.resetJointState(robot, i, FRANKA_INIT_QPOS[i]) 43 | 44 | num_lines = manipulator.num_joints - 1 45 | lines = [-1] * num_lines # init 46 | joint_pts = -1 # init 47 | joint_texts = [-1] * manipulator.num_joints # init 48 | com_pts = [-1] * manipulator.num_joints # init 49 | com_texts = [-1] * manipulator.num_joints # init 50 | states = pybullet.getJointStates(robot, range(manipulator.num_joints)) 51 | q = np.array([states[i][0] for i in range(manipulator.num_joints)]) 52 | 53 | joint_transforms = manipulator.joint_to_world_transforms(q) 54 | 55 | rgbs = [[1, 0, 0]] * num_lines 56 | 57 | while True: 58 | states = pybullet.getJointStates(robot, range(manipulator.num_joints)) 59 | q = np.array([states[i][0] for i in range(manipulator.num_joints)]) 60 | joint_transforms = manipulator.joint_to_world_transforms(q) 61 | link_transforms = manipulator.link_inertial_to_world_transforms(q) 62 | joint_pts = pybullet.addUserDebugPoints( 63 | [tf[:3, 3] for tf in joint_transforms], 64 | [[0, 0, 0]] * manipulator.num_joints, 65 | 10, 66 | replaceItemUniqueId=joint_pts, 67 | ) 68 | for i in range(manipulator.num_joints): 69 | com_pts[i] = pybullet.addUserDebugPoints( 70 | [link_transforms[i][:3, 3]], 71 | [[0, 1, 0]], 72 | 10, 73 | replaceItemUniqueId=com_pts[i], 74 | ) 75 | com_texts[i] = pybullet.addUserDebugText( 76 | f"COM {i}", 77 | link_transforms[i][:3, 3], 78 | [0, 1, 0], 79 | replaceItemUniqueId=com_texts[i], 80 | ) 81 | joint_texts[i] = pybullet.addUserDebugText( 82 | f"Joint {i}", 83 | joint_transforms[i][:3, 3], 84 | [0, 0, 0], 85 | replaceItemUniqueId=joint_texts[i], 86 | ) 87 | for i in range(num_lines): 88 | lines[i] = pybullet.addUserDebugLine( 89 | joint_transforms[i][:3, 3], 90 | joint_transforms[i + 1][:3, 3], 91 | rgbs[i], 92 | 3, 93 | replaceItemUniqueId=lines[i], 94 | ) 95 | pybullet.stepSimulation() 96 | 97 | 98 | def main(): 99 | visualize_parsed_tfs(URDF, randomize=False) 100 | 101 | 102 | if __name__ == "__main__": 103 | main() 104 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![oscbf_gif](https://github.com/user-attachments/assets/43b615a3-dcec-4fc4-bc7f-2ba674562323) 2 | 3 | # Operational Space Control Barrier Functions 4 | 5 | Code for *"Safe, Task-Consistent Manipulation with Operational Space Control Barrier Functions"* -- Daniel Morton and Marco Pavone 6 | 7 | Accepted to IROS 2025, Hangzhou 8 | 9 | [![Paper](http://img.shields.io/badge/arXiv-2503.06736-B31B1B.svg)](https://arxiv.org/abs/2503.06736) 10 | 11 | ## What is OSCBF? 12 | 13 | This is a safe, high-performance, and easy-to-use controller / safety filter for robotic manipulators. 14 | 15 | With OSCBF, you can... 16 | - Operate at kilohertz speed even with over 400 active safety constraints 17 | - Design safety constraints (barrier functions) easily via CBFpy 18 | - Enforce safety on both torque-controlled and velocity-controlled robots 19 | - Either apply a safety filter on top of your existing controller, or use our provided controller 20 | 21 | In general, this will be especialy useful for enforcing safety during **teleoperation** or while executing **learned policies** 22 | 23 | For more details and videos, check out the [project webpage](https://stanfordasl.github.io/oscbf/) as well as the [CBFpy documentation](https://danielpmorton.github.io/cbfpy/). 24 | 25 | 26 | ## How do I use this? 27 | 28 | Check out the `examples/` folder for interactive demos in Pybullet! This is the best place to start 29 | 30 | If you're applying this to a different robot, you'll need to provide a URDF -- we can parse the kinematics and dynamics from that. Some notes: 31 | - I haven't written an MJCF parser yet, but it should be feasible 32 | - All joints that shouldn't be controlled as part of the kinematic chain should be set to `fixed` (gripper joints, for instance). Since you probably want to still be able to use the gripper joints in simulation, the best way to handle this is to make a copy of the URDF: load the non-fixed one in sim, and parse the fixed one with OSCBF 33 | - I manually defined the collision model for the Franka. There are probably better ways to parse or generate this data from meshes, but I haven't done it yet. For now, I'd recommend doing the same for your robot. 34 | 35 | ## FAQ 36 | 37 | - "I already have a well-tuned operational space controller! I don't want to replace that" 38 | - You can use OSCBF as a safety filter on top of your existing controller! 39 | - "I'm working with a mobile manipulator. Does this still work?" 40 | - Yes! We support prismatic and revolute joints, so adding a mobile base just adds 3DOF (PPR) to the beginning of the kinematic chain. 41 | - "I'm controlling joint-space motions -- does this still apply?" 42 | - Depending on the task, you will likely want to modify the objective function. But, the robot dynamics and CBF formulation will still be useful! 43 | 44 | 45 | ## Installation 46 | 47 | A virtual environment is optional, but highly recommended. For `pyenv` installation instructions, see [here](https://danielpmorton.github.io/cbfpy/pyenv). 48 | 49 | ``` 50 | git clone https://github.com/stanfordasl/oscbf 51 | cd oscbf 52 | pip install -e . 53 | ``` 54 | 55 | Note: This code will work with most versions of `jax`, but there seems to have been a CPU slowdown introduced in version `0.4.32`. To avoid this, I use version `0.4.30`, which is the version indicated in this repo's `pyproject.toml` as well. However, feel free to use any version you like. 56 | 57 | This has been tested on Python 3.10 and 3.11, on Ubuntu 22.04. 58 | 59 | 60 | ## Documentation 61 | 62 | See the CBFpy documentation, available at [this link](https://danielpmorton.github.io/cbfpy) 63 | 64 | 65 | ## Hardware / ROS2 66 | 67 | The code used to run the Franka Panda hardware experiments is available at [this link](https://github.com/StanfordASL/oscbf_hardware_ws) 68 | 69 | 70 | ## Citation 71 | ``` 72 | @inproceedings{morton2025oscbf, 73 | author={Morton, Daniel and Pavone, Marco}, 74 | booktitle={2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 75 | title={Safe, Task-Consistent Manipulation with Operational Space Control Barrier Functions}, 76 | year={2025}, 77 | pages={187-194}, 78 | doi={10.1109/IROS60139.2025.11246389} 79 | } 80 | ``` 81 | 82 | -------------------------------------------------------------------------------- /oscbf/utils/trajectory.py: -------------------------------------------------------------------------------- 1 | """Trajectories""" 2 | 3 | from abc import ABC, abstractmethod 4 | import numpy as np 5 | 6 | 7 | class TaskTrajectory(ABC): 8 | """Base Operational Space Trajectory implementation 9 | 10 | Inherited classes must implement the following methods: 11 | - position(t: float) -> np.ndarray 12 | - velocity(t: float) -> np.ndarray 13 | - acceleration(t: float) -> np.ndarray 14 | - rotation(t: float) -> np.ndarray 15 | - omega(t: float) -> np.ndarray 16 | - alpha(t: float) -> np.ndarray 17 | """ 18 | 19 | def __init__(self): 20 | pass 21 | 22 | @abstractmethod 23 | def position(self, t: float) -> np.ndarray: 24 | pass 25 | 26 | @abstractmethod 27 | def velocity(self, t: float) -> np.ndarray: 28 | pass 29 | 30 | @abstractmethod 31 | def acceleration(self, t: float) -> np.ndarray: 32 | pass 33 | 34 | @abstractmethod 35 | def rotation(self, t: float) -> np.ndarray: 36 | pass 37 | 38 | @abstractmethod 39 | def omega(self, t: float) -> np.ndarray: 40 | pass 41 | 42 | @abstractmethod 43 | def alpha(self, t: float) -> np.ndarray: 44 | pass 45 | 46 | 47 | class JointTrajectory(ABC): 48 | """Base Joint Space Trajectory implementation 49 | 50 | Inherited classes must implement the following methods: 51 | - joint_positions(t: float) -> np.ndarray 52 | - joint_velocities(t: float) -> np.ndarray 53 | - joint_accelerations(t: float) -> np.ndarray 54 | """ 55 | 56 | def __init__(self): 57 | pass 58 | 59 | @abstractmethod 60 | def joint_positions(self, t: float) -> np.ndarray: 61 | pass 62 | 63 | @abstractmethod 64 | def joint_velocities(self, t: float) -> np.ndarray: 65 | pass 66 | 67 | @abstractmethod 68 | def joint_accelerations(self, t: float) -> np.ndarray: 69 | pass 70 | 71 | 72 | class SinusoidalTaskTrajectory(TaskTrajectory): 73 | """An example sinusoidal task-space position trajectory for the robot to follow 74 | 75 | Args: 76 | init_pos (np.ndarray): Initial position of the end-effector, shape (3,) 77 | init_rot (np.ndarray): Initial rotation of the end-effector, shape (3, 3) 78 | amplitude (np.ndarray): X,Y,Z amplitudes of the sinusoid, shape (3,) 79 | angular_freq (np.ndarray): X,Y,Z angular frequencies of the sinusoid, shape (3,) 80 | phase (np.ndarray): X,Y,Z phase offsets of the sinusoid, shape (3,) 81 | """ 82 | 83 | def __init__( 84 | self, 85 | init_pos: np.ndarray, 86 | init_rot: np.ndarray, 87 | amplitude: np.ndarray, 88 | angular_freq: np.ndarray, 89 | phase: np.ndarray, 90 | ): 91 | self.init_pos = np.asarray(init_pos) 92 | self.init_rot = np.asarray(init_rot) 93 | self.amplitude = np.asarray(amplitude) 94 | self.angular_freq = np.asarray(angular_freq) 95 | self.phase = np.asarray(phase) 96 | 97 | assert self.init_pos.shape == (3,) 98 | assert self.init_rot.shape == (3, 3) 99 | assert self.amplitude.shape == (3,) 100 | assert self.angular_freq.shape == (3,) 101 | assert self.phase.shape == (3,) 102 | 103 | # Simple sinusoidal positional trajectory 104 | 105 | def position(self, t: float) -> np.ndarray: 106 | return self.init_pos + self.amplitude * np.sin( 107 | self.angular_freq * t + self.phase 108 | ) 109 | 110 | def velocity(self, t: float) -> np.ndarray: 111 | return ( 112 | self.amplitude 113 | * self.angular_freq 114 | * np.cos(self.angular_freq * t + self.phase) 115 | ) 116 | 117 | def acceleration(self, t: float) -> np.ndarray: 118 | return ( 119 | -self.amplitude 120 | * self.angular_freq**2 121 | * np.sin(self.angular_freq * t + self.phase) 122 | ) 123 | 124 | # Maintain a fixed orientation 125 | 126 | def rotation(self, t: float) -> np.ndarray: 127 | return self.init_rot 128 | 129 | def omega(self, t: float) -> np.ndarray: 130 | return np.zeros(3) 131 | 132 | def alpha(self, t: float) -> np.ndarray: 133 | return np.zeros(3) 134 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/visual/link6.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 17 3 | 4 | newmtl Face064_002_001_002_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 0.000000 0.000000 8 | Ks 0.003906 0.003906 0.003906 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Face065_002_001_002_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.000000 1.000000 0.000000 18 | Ks 0.003906 0.003906 0.003906 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Face374_002_001_002_001.001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.003906 0.003906 0.003906 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Face539_002_001_002_001.001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.250980 0.250980 0.250980 38 | Ks 0.003906 0.003906 0.003906 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | map_Kd colors.png 44 | 45 | newmtl Part__Feature001_009_001_002_001.001 46 | Ns -1.960784 47 | Ka 1.000000 1.000000 1.000000 48 | Kd 0.250980 0.250980 0.250980 49 | Ks 0.003906 0.003906 0.003906 50 | Ke 0.000000 0.000000 0.000000 51 | Ni 1.000000 52 | d 1.000000 53 | illum 2 54 | map_Kd colors.png 55 | 56 | newmtl Part__Feature002_006_001_002_001.001 57 | Ns -1.960784 58 | Ka 1.000000 1.000000 1.000000 59 | Kd 0.250980 0.250980 0.250980 60 | Ks 0.003906 0.003906 0.003906 61 | Ke 0.000000 0.000000 0.000000 62 | Ni 1.000000 63 | d 1.000000 64 | illum 2 65 | map_Kd colors.png 66 | 67 | newmtl Shell002_002_001_002_001.001 68 | Ns -1.960784 69 | Ka 1.000000 1.000000 1.000000 70 | Kd 1.000000 1.000000 1.000000 71 | Ks 0.003906 0.003906 0.003906 72 | Ke 0.000000 0.000000 0.000000 73 | Ni 1.000000 74 | d 1.000000 75 | illum 2 76 | 77 | newmtl Shell003_002_001_002_001.001 78 | Ns -1.960784 79 | Ka 1.000000 1.000000 1.000000 80 | Kd 1.000000 1.000000 1.000000 81 | Ks 0.003906 0.003906 0.003906 82 | Ke 0.000000 0.000000 0.000000 83 | Ni 1.000000 84 | d 1.000000 85 | illum 2 86 | 87 | newmtl Shell004_001_001_002_001.001 88 | Ns -1.960784 89 | Ka 1.000000 1.000000 1.000000 90 | Kd 1.000000 1.000000 1.000000 91 | Ks 0.003906 0.003906 0.003906 92 | Ke 0.000000 0.000000 0.000000 93 | Ni 1.000000 94 | d 1.000000 95 | illum 2 96 | 97 | newmtl Shell005_001_001_002_001.001 98 | Ns -1.960784 99 | Ka 1.000000 1.000000 1.000000 100 | Kd 1.000000 1.000000 1.000000 101 | Ks 0.003906 0.003906 0.003906 102 | Ke 0.000000 0.000000 0.000000 103 | Ni 1.000000 104 | d 1.000000 105 | illum 2 106 | 107 | newmtl Shell006_003_002_001.001 108 | Ns -1.960784 109 | Ka 1.000000 1.000000 1.000000 110 | Kd 0.901961 0.921569 0.929412 111 | Ks 0.015625 0.015625 0.015625 112 | Ke 0.000000 0.000000 0.000000 113 | Ni 1.000000 114 | d 1.000000 115 | illum 2 116 | 117 | newmtl Shell007_002_002_001.001 118 | Ns -1.960784 119 | Ka 1.000000 1.000000 1.000000 120 | Kd 0.250000 0.250000 0.250000 121 | Ks 0.003906 0.003906 0.003906 122 | Ke 0.000000 0.000000 0.000000 123 | Ni 1.000000 124 | d 1.000000 125 | illum 2 126 | 127 | newmtl Shell011_002_002_001.001 128 | Ns -1.960784 129 | Ka 1.000000 1.000000 1.000000 130 | Kd 1.000000 1.000000 1.000000 131 | Ks 0.003906 0.003906 0.003906 132 | Ke 0.000000 0.000000 0.000000 133 | Ni 1.000000 134 | d 1.000000 135 | illum 2 136 | 137 | newmtl Shell012_002_002_001.001 138 | Ns -1.960784 139 | Ka 1.000000 1.000000 1.000000 140 | Kd 1.000000 1.000000 1.000000 141 | Ks 0.003906 0.003906 0.003906 142 | Ke 0.000000 0.000000 0.000000 143 | Ni 1.000000 144 | d 1.000000 145 | illum 2 146 | map_Kd colors.png 147 | 148 | newmtl Shell_003_001_002_001.001 149 | Ns -1.960784 150 | Ka 1.000000 1.000000 1.000000 151 | Kd 0.250980 0.250980 0.250980 152 | Ks 0.003906 0.003906 0.003906 153 | Ke 0.000000 0.000000 0.000000 154 | Ni 1.000000 155 | d 1.000000 156 | illum 2 157 | map_Kd colors.png 158 | 159 | newmtl Union001_001_001_002_001.001 160 | Ns -1.960784 161 | Ka 1.000000 1.000000 1.000000 162 | Kd 0.039216 0.541176 0.780392 163 | Ks 0.003906 0.003906 0.003906 164 | Ke 0.000000 0.000000 0.000000 165 | Ni 1.000000 166 | d 1.000000 167 | illum 2 168 | 169 | newmtl Union_001_001_002_001.001 170 | Ns -1.960784 171 | Ka 1.000000 1.000000 1.000000 172 | Kd 0.039216 0.541176 0.780392 173 | Ks 0.003906 0.003906 0.003906 174 | Ke 0.000000 0.000000 0.000000 175 | Ni 1.000000 176 | d 1.000000 177 | illum 2 178 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/collision/link6.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 17 3 | 4 | newmtl Face064_002_001_002_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 0.000000 0.000000 8 | Ks 0.003906 0.003906 0.003906 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Face065_002_001_002_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.000000 1.000000 0.000000 18 | Ks 0.003906 0.003906 0.003906 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Face374_002_001_002_001.001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.003906 0.003906 0.003906 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Face539_002_001_002_001.001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.250980 0.250980 0.250980 38 | Ks 0.003906 0.003906 0.003906 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 44 | 45 | newmtl Part__Feature001_009_001_002_001.001 46 | Ns -1.960784 47 | Ka 1.000000 1.000000 1.000000 48 | Kd 0.250980 0.250980 0.250980 49 | Ks 0.003906 0.003906 0.003906 50 | Ke 0.000000 0.000000 0.000000 51 | Ni 1.000000 52 | d 1.000000 53 | illum 2 54 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 55 | 56 | newmtl Part__Feature002_006_001_002_001.001 57 | Ns -1.960784 58 | Ka 1.000000 1.000000 1.000000 59 | Kd 0.250980 0.250980 0.250980 60 | Ks 0.003906 0.003906 0.003906 61 | Ke 0.000000 0.000000 0.000000 62 | Ni 1.000000 63 | d 1.000000 64 | illum 2 65 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 66 | 67 | newmtl Shell002_002_001_002_001.001 68 | Ns -1.960784 69 | Ka 1.000000 1.000000 1.000000 70 | Kd 1.000000 1.000000 1.000000 71 | Ks 0.003906 0.003906 0.003906 72 | Ke 0.000000 0.000000 0.000000 73 | Ni 1.000000 74 | d 1.000000 75 | illum 2 76 | 77 | newmtl Shell003_002_001_002_001.001 78 | Ns -1.960784 79 | Ka 1.000000 1.000000 1.000000 80 | Kd 1.000000 1.000000 1.000000 81 | Ks 0.003906 0.003906 0.003906 82 | Ke 0.000000 0.000000 0.000000 83 | Ni 1.000000 84 | d 1.000000 85 | illum 2 86 | 87 | newmtl Shell004_001_001_002_001.001 88 | Ns -1.960784 89 | Ka 1.000000 1.000000 1.000000 90 | Kd 1.000000 1.000000 1.000000 91 | Ks 0.003906 0.003906 0.003906 92 | Ke 0.000000 0.000000 0.000000 93 | Ni 1.000000 94 | d 1.000000 95 | illum 2 96 | 97 | newmtl Shell005_001_001_002_001.001 98 | Ns -1.960784 99 | Ka 1.000000 1.000000 1.000000 100 | Kd 1.000000 1.000000 1.000000 101 | Ks 0.003906 0.003906 0.003906 102 | Ke 0.000000 0.000000 0.000000 103 | Ni 1.000000 104 | d 1.000000 105 | illum 2 106 | 107 | newmtl Shell006_003_002_001.001 108 | Ns -1.960784 109 | Ka 1.000000 1.000000 1.000000 110 | Kd 0.901961 0.921569 0.929412 111 | Ks 0.015625 0.015625 0.015625 112 | Ke 0.000000 0.000000 0.000000 113 | Ni 1.000000 114 | d 1.000000 115 | illum 2 116 | 117 | newmtl Shell007_002_002_001.001 118 | Ns -1.960784 119 | Ka 1.000000 1.000000 1.000000 120 | Kd 0.250000 0.250000 0.250000 121 | Ks 0.003906 0.003906 0.003906 122 | Ke 0.000000 0.000000 0.000000 123 | Ni 1.000000 124 | d 1.000000 125 | illum 2 126 | 127 | newmtl Shell011_002_002_001.001 128 | Ns -1.960784 129 | Ka 1.000000 1.000000 1.000000 130 | Kd 1.000000 1.000000 1.000000 131 | Ks 0.003906 0.003906 0.003906 132 | Ke 0.000000 0.000000 0.000000 133 | Ni 1.000000 134 | d 1.000000 135 | illum 2 136 | 137 | newmtl Shell012_002_002_001.001 138 | Ns -1.960784 139 | Ka 1.000000 1.000000 1.000000 140 | Kd 1.000000 1.000000 1.000000 141 | Ks 0.003906 0.003906 0.003906 142 | Ke 0.000000 0.000000 0.000000 143 | Ni 1.000000 144 | d 1.000000 145 | illum 2 146 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 147 | 148 | newmtl Shell_003_001_002_001.001 149 | Ns -1.960784 150 | Ka 1.000000 1.000000 1.000000 151 | Kd 0.250980 0.250980 0.250980 152 | Ks 0.003906 0.003906 0.003906 153 | Ke 0.000000 0.000000 0.000000 154 | Ni 1.000000 155 | d 1.000000 156 | illum 2 157 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 158 | 159 | newmtl Union001_001_001_002_001.001 160 | Ns -1.960784 161 | Ka 1.000000 1.000000 1.000000 162 | Kd 0.039216 0.541176 0.780392 163 | Ks 0.003906 0.003906 0.003906 164 | Ke 0.000000 0.000000 0.000000 165 | Ni 1.000000 166 | d 1.000000 167 | illum 2 168 | 169 | newmtl Union_001_001_002_001.001 170 | Ns -1.960784 171 | Ka 1.000000 1.000000 1.000000 172 | Kd 0.039216 0.541176 0.780392 173 | Ks 0.003906 0.003906 0.003906 174 | Ke 0.000000 0.000000 0.000000 175 | Ni 1.000000 176 | d 1.000000 177 | illum 2 178 | -------------------------------------------------------------------------------- /oscbf/utils/visualization.py: -------------------------------------------------------------------------------- 1 | """Pybullet debug visualizer helper functions""" 2 | 3 | from typing import Optional 4 | 5 | import numpy as np 6 | import numpy.typing as npt 7 | import pybullet 8 | from pybullet_utils.bullet_client import BulletClient 9 | 10 | 11 | def visualize_3D_box( 12 | box: npt.ArrayLike, 13 | padding: Optional[npt.ArrayLike] = None, 14 | rgba: npt.ArrayLike = (1, 0, 0, 0.5), 15 | client: Optional[BulletClient] = None, 16 | ) -> int: 17 | """Visualize a box in Pybullet 18 | 19 | Args: 20 | box (npt.ArrayLike): Lower and upper xyz limits of the axis-aligned box, shape (2, 3) 21 | padding (Optional[npt.ArrayLike]): If expanding (or contracting) the boxes by a certain amount, include the 22 | (x, y, z) padding distances here (shape (3,)). Defaults to None. 23 | rgba (npt.ArrayLike): Color of the box (RGB + alpha), shape (4,). Defaults to (1, 0, 0, 0.5). 24 | client (BulletClient, optional): If connecting to multiple physics servers, include the client 25 | (the class instance, not just the ID) here. Defaults to None (use default connected client) 26 | 27 | Returns: 28 | int: Pybullet ID of the box 29 | """ 30 | lower, upper = box 31 | if padding is not None: 32 | lower -= padding 33 | upper += padding 34 | return create_box( 35 | pos=(lower + (upper - lower) / 2), # Midpoint 36 | orn=(0, 0, 0, 1), 37 | mass=0, 38 | sidelengths=(upper - lower), 39 | use_collision=False, 40 | rgba=rgba, 41 | client=client, 42 | ) 43 | 44 | 45 | def create_box( 46 | pos: npt.ArrayLike, 47 | orn: npt.ArrayLike, 48 | mass: float, 49 | sidelengths: npt.ArrayLike, 50 | use_collision: bool, 51 | rgba: npt.ArrayLike = (1, 1, 1, 1), 52 | client: Optional[BulletClient] = None, 53 | ) -> int: 54 | """Creates a rigid box in the Pybullet simulation 55 | 56 | Args: 57 | pos (npt.ArrayLike): Position of the box in world frame, shape (3) 58 | orn (npt.ArrayLike): Orientation (XYZW quaternion) of the box in world frame, shape (4,) 59 | mass (float): Mass of the box. If set to 0, the box is fixed in space 60 | sidelengths (npt.ArrayLike): Sidelengths of the box along the local XYZ axes, shape (3,) 61 | use_collision (bool): Whether or not collision is enabled for the box 62 | rgba (npt.ArrayLike, optional): Color of the box, with each RGBA value being in [0, 1]. 63 | Defaults to (1, 1, 1, 1) (white) 64 | client (BulletClient, optional): If connecting to multiple physics servers, include the client 65 | (the class instance, not just the ID) here. Defaults to None (use default connected client) 66 | 67 | Returns: 68 | int: ID of the box in Pybullet 69 | """ 70 | client: pybullet = pybullet if client is None else client 71 | if len(sidelengths) != 3: 72 | raise ValueError("Must provide the dimensions of the three sides of the box") 73 | half_extents = np.asarray(sidelengths) / 2 74 | visual_id = client.createVisualShape( 75 | pybullet.GEOM_BOX, 76 | halfExtents=half_extents, 77 | rgbaColor=rgba, 78 | ) 79 | if use_collision: 80 | collision_id = client.createCollisionShape( 81 | pybullet.GEOM_BOX, 82 | halfExtents=half_extents, 83 | ) 84 | else: 85 | collision_id = -1 86 | box_id = client.createMultiBody( 87 | baseMass=mass, 88 | basePosition=pos, 89 | baseOrientation=orn, 90 | baseCollisionShapeIndex=collision_id, 91 | baseVisualShapeIndex=visual_id, 92 | ) 93 | return box_id 94 | 95 | 96 | def visualize_3D_sphere( 97 | center: npt.ArrayLike, 98 | radius: float, 99 | rgba: npt.ArrayLike = (1, 0, 0, 0.5), 100 | client: Optional[BulletClient] = None, 101 | ) -> int: 102 | """Visualize a sphere in Pybullet 103 | 104 | Args: 105 | center (npt.ArrayLike): Center of the sphere, shape (3,) 106 | radius (float): Radius of the sphere 107 | rgba (npt.ArrayLike): Color of the sphere (RGB + alpha), shape (4,). Defaults to (0, 1, 0, 0.5). 108 | client (BulletClient, optional): If connecting to multiple physics servers, include the client 109 | (the class instance, not just the ID) here. Defaults to None (use default connected client) 110 | 111 | Returns: 112 | int: Pybullet ID of the sphere 113 | """ 114 | client: pybullet = pybullet if client is None else client 115 | visual_id = client.createVisualShape( 116 | pybullet.GEOM_SPHERE, 117 | radius=radius, 118 | rgbaColor=rgba, 119 | ) 120 | collision_id = -1 121 | sphere_id = client.createMultiBody( 122 | baseMass=0, 123 | basePosition=center, 124 | baseCollisionShapeIndex=collision_id, 125 | baseVisualShapeIndex=visual_id, 126 | ) 127 | return sphere_id 128 | -------------------------------------------------------------------------------- /test/test_manipulator_dynamics.py: -------------------------------------------------------------------------------- 1 | """Test cases for the Manipulator class""" 2 | 3 | import unittest 4 | 5 | from oscbf.core.manipulator import Manipulator 6 | from oscbf.utils.general_utils import find_assets_dir 7 | 8 | import pybullet 9 | import jax.numpy as jnp 10 | import numpy as np 11 | 12 | try: 13 | import pinocchio as pin 14 | 15 | PIN_INSTALLED = True 16 | except ImportError: 17 | PIN_INSTALLED = False 18 | 19 | URDF = find_assets_dir() + "franka_panda/panda.urdf" 20 | 21 | 22 | class PinocchioDynamicsTest(unittest.TestCase): 23 | """Test cases to validate the manipulator dynamics against Pinocchio's values""" 24 | 25 | @classmethod 26 | def setUpClass(cls): 27 | if not PIN_INSTALLED: 28 | raise unittest.SkipTest("Pinocchio not installed") 29 | cls.model = pin.buildModelFromUrdf(URDF) 30 | cls.data = pin.Data(cls.model) 31 | cls.robot = Manipulator.from_urdf(URDF) 32 | cls.num_joints = cls.robot.num_joints 33 | np.random.seed(0) 34 | 35 | def test_mass_matrix(self): 36 | for i in range(10): 37 | q = np.random.uniform(-np.pi / 2, np.pi / 2, self.num_joints) 38 | dq = np.zeros(self.num_joints) 39 | pin.forwardKinematics(self.model, self.data, q, dq) 40 | pin.updateFramePlacements(self.model, self.data) 41 | M = self.robot.mass_matrix(q) 42 | Mpin = pin.crba(self.model, self.data, q) 43 | np.testing.assert_array_almost_equal(M, Mpin, decimal=4) 44 | 45 | def test_nonlinear_effects(self): 46 | for i in range(10): 47 | q = np.random.uniform(-np.pi / 2, np.pi / 2, self.num_joints) 48 | dq = np.random.rand(self.num_joints) 49 | pin.forwardKinematics(self.model, self.data, q, dq) 50 | pin.updateFramePlacements(self.model, self.data) 51 | bias = pin.nle(self.model, self.data, q, dq) 52 | G = self.robot.gravity_vector(q) 53 | C = self.robot.centrifugal_coriolis_vector(q, dq) 54 | np.testing.assert_array_almost_equal(G + C, bias, decimal=4) 55 | 56 | 57 | class PybulletDynamicsTest(unittest.TestCase): 58 | """Test cases to validate the manipulator dynamics against Pybullet's values""" 59 | 60 | @classmethod 61 | def setUpClass(cls): 62 | # Setup pybullet environment 63 | pybullet.connect(pybullet.DIRECT) 64 | cls.robot_id = pybullet.loadURDF( 65 | URDF, 66 | useFixedBase=True, 67 | flags=pybullet.URDF_USE_INERTIA_FROM_FILE, 68 | ) 69 | pybullet.setGravity(0, 0, -9.81) 70 | cls.robot = Manipulator.from_urdf(URDF) 71 | cls.num_joints = cls.robot.num_joints 72 | np.random.seed(0) 73 | 74 | @classmethod 75 | def tearDownClass(cls): 76 | pybullet.disconnect() 77 | 78 | def test_mass_matrix(self): 79 | for i in range(10): 80 | q = np.random.uniform(-np.pi / 2, np.pi / 2, self.num_joints) 81 | mass_matrix = pybullet.calculateMassMatrix(self.robot_id, q.tolist()) 82 | M = self.robot.mass_matrix(q) 83 | np.testing.assert_array_almost_equal(M, mass_matrix, decimal=4) 84 | 85 | def test_gravity_vector(self): 86 | for i in range(10): 87 | q = np.random.uniform(-np.pi / 2, np.pi / 2, self.num_joints) 88 | zero_velocities = [0.0] * self.num_joints 89 | zero_accelerations = [0.0] * self.num_joints 90 | 91 | gravity_torques = pybullet.calculateInverseDynamics( 92 | self.robot_id, q.tolist(), zero_velocities, zero_accelerations 93 | ) 94 | G = self.robot.gravity_vector(q) 95 | np.testing.assert_array_almost_equal(G, gravity_torques, decimal=4) 96 | 97 | def test_coriolis_vector(self): 98 | for _ in range(10): 99 | q = jnp.array(np.random.rand(self.num_joints)) 100 | qd = jnp.array(np.random.rand(self.num_joints)) 101 | zero_velocities = [0.0] * self.num_joints 102 | zero_accelerations = [0.0] * self.num_joints 103 | 104 | gravity_torques = pybullet.calculateInverseDynamics( 105 | self.robot_id, q.tolist(), zero_velocities, zero_accelerations 106 | ) 107 | 108 | coriolis_gravity_torques = pybullet.calculateInverseDynamics( 109 | self.robot_id, q.tolist(), qd.tolist(), zero_accelerations 110 | ) 111 | 112 | coriolis_torques = np.array(coriolis_gravity_torques) - np.array( 113 | gravity_torques 114 | ) 115 | 116 | C = self.robot.centrifugal_coriolis_vector(q, qd) 117 | np.testing.assert_array_almost_equal(C, coriolis_torques, decimal=4) 118 | 119 | def test_jacobian(self): 120 | for _ in range(10): 121 | q = jnp.array(np.random.rand(self.num_joints)) 122 | qdot = np.zeros(self.num_joints).tolist() 123 | qddot = np.zeros(self.num_joints).tolist() 124 | link_index = self.num_joints - 1 125 | jv, jw = pybullet.calculateJacobian( 126 | self.robot_id, link_index, (0, 0, 0), q.tolist(), qdot, qddot 127 | ) 128 | J_pybullet = np.row_stack([np.array(jv), np.array(jw)]) 129 | J = self.robot.ee_jacobian(q) 130 | np.testing.assert_array_almost_equal(J, J_pybullet, decimal=4) 131 | 132 | 133 | if __name__ == "__main__": 134 | unittest.main() 135 | -------------------------------------------------------------------------------- /oscbf/examples/self_collision.py: -------------------------------------------------------------------------------- 1 | """Testing self-collision avoidance""" 2 | 3 | import numpy as np 4 | import jax 5 | import jax.numpy as jnp 6 | from jax.typing import ArrayLike 7 | from cbfpy import CBF 8 | 9 | from oscbf.core.manipulator import Manipulator, load_panda 10 | from oscbf.core.manipulation_env import FrankaTorqueControlEnv 11 | from oscbf.core.oscbf_configs import OSCBFTorqueConfig 12 | from oscbf.core.controllers import PoseTaskTorqueController 13 | 14 | @jax.tree_util.register_static 15 | class SelfCollisionConfig(OSCBFTorqueConfig): 16 | 17 | def __init__( 18 | self, 19 | robot: Manipulator, 20 | ): 21 | self.q_min = robot.joint_lower_limits 22 | self.q_max = robot.joint_upper_limits 23 | self.singularity_tol = 1e-3 24 | super().__init__(robot, rot_obj_weight=0.1) 25 | 26 | def h_2(self, z, **kwargs): 27 | # Extract values 28 | q = z[: self.num_joints] 29 | 30 | # Self collision avoidance 31 | robot_collision_pos_rad = self.robot.link_self_collision_data(q) 32 | robot_collision_positions = robot_collision_pos_rad[:, :3] 33 | robot_collision_radii = robot_collision_pos_rad[:, 3] 34 | pairs = jnp.asarray(self.robot.self_collision_pairs) 35 | pos_a = robot_collision_positions[pairs[:, 0]] 36 | pos_b = robot_collision_positions[pairs[:, 1]] 37 | rad_a = robot_collision_radii[pairs[:, 0]] 38 | rad_b = robot_collision_radii[pairs[:, 1]] 39 | center_deltas = pos_b - pos_a 40 | h_self_collision = jnp.linalg.norm(center_deltas, axis=-1) - rad_a - rad_b 41 | 42 | # Base self collision avoidance 43 | base_position = jnp.asarray(self.robot.base_self_collision_position) 44 | base_radius = self.robot.base_self_collision_radius 45 | base_sc_idxs = jnp.asarray(self.robot.base_self_collision_idxs) 46 | base_sc_deltas = robot_collision_positions[base_sc_idxs] - base_position 47 | h_base_self_collision = ( 48 | jnp.linalg.norm(base_sc_deltas, axis=-1) 49 | - robot_collision_radii[base_sc_idxs] 50 | - base_radius 51 | ) 52 | 53 | # Joint Limit Avoidance 54 | q_min = jnp.asarray(self.q_min) 55 | q_max = jnp.asarray(self.q_max) 56 | h_joint_limits = jnp.concatenate([q_max - q, q - q_min]) 57 | 58 | # Singularity Avoidance 59 | sigmas = jax.lax.linalg.svd(self.robot.ee_jacobian(q), compute_uv=False) 60 | h_singularity = jnp.array([jnp.prod(sigmas) - self.singularity_tol]) 61 | 62 | return jnp.concatenate([h_self_collision, h_base_self_collision, h_joint_limits, h_singularity]) 63 | 64 | def alpha(self, h): 65 | return 10.0 * h 66 | 67 | def alpha_2(self, h_2): 68 | return 10.0 * h_2 69 | 70 | 71 | # @partial(jax.jit, static_argnums=(0, 1, 2)) 72 | def compute_control( 73 | robot: Manipulator, 74 | osc_controller: PoseTaskTorqueController, 75 | cbf: CBF, 76 | z: ArrayLike, 77 | z_ee_des: ArrayLike, 78 | ): 79 | q = z[: robot.num_joints] 80 | qdot = z[robot.num_joints :] 81 | M, M_inv, g, c, J, ee_tmat = robot.torque_control_matrices(q, qdot) 82 | # Set nullspace desired joint position 83 | nullspace_posture_goal = jnp.array( 84 | [ 85 | 0.0, 86 | -jnp.pi / 6, 87 | 0.0, 88 | -3 * jnp.pi / 4, 89 | 0.0, 90 | 5 * jnp.pi / 9, 91 | 0.0, 92 | ] 93 | ) 94 | 95 | # Compute nominal control 96 | u_nom = osc_controller( 97 | q, 98 | qdot, 99 | pos=ee_tmat[:3, 3], 100 | rot=ee_tmat[:3, :3], 101 | des_pos=z_ee_des[:3], 102 | des_rot=jnp.reshape(z_ee_des[3:12], (3, 3)), 103 | des_vel=z_ee_des[12:15], 104 | des_omega=z_ee_des[15:18], 105 | des_accel=jnp.zeros(3), 106 | des_alpha=jnp.zeros(3), 107 | des_q=nullspace_posture_goal, 108 | des_qdot=jnp.zeros(robot.num_joints), 109 | J=J, 110 | M=M, 111 | M_inv=M_inv, 112 | g=g, 113 | c=c, 114 | ) 115 | # Apply the CBF safety filter 116 | return cbf.safety_filter(z, u_nom) 117 | 118 | 119 | def main(): 120 | robot = load_panda() 121 | config = SelfCollisionConfig(robot) 122 | cbf = CBF.from_config(config) 123 | env = FrankaTorqueControlEnv( 124 | load_floor=False, 125 | bg_color=(1, 1, 1), 126 | real_time=True, 127 | ) 128 | 129 | env.client.resetDebugVisualizerCamera( 130 | cameraDistance=2, 131 | cameraPitch=-27.80, 132 | cameraYaw=36.80, 133 | cameraTargetPosition=(0.08, 0.49, -0.04), 134 | ) 135 | 136 | kp_pos = 50.0 137 | kp_rot = 20.0 138 | kd_pos = 20.0 139 | kd_rot = 10.0 140 | kp_joint = 10.0 141 | kd_joint = 5.0 142 | osc_controller = PoseTaskTorqueController( 143 | n_joints=robot.num_joints, 144 | kp_task=np.concatenate([kp_pos * np.ones(3), kp_rot * np.ones(3)]), 145 | kd_task=np.concatenate([kd_pos * np.ones(3), kd_rot * np.ones(3)]), 146 | kp_joint=kp_joint, 147 | kd_joint=kd_joint, 148 | # Note: torque limits will be enforced via the QP. We'll set them to None here 149 | # because we don't want to clip the values before the QP 150 | tau_min=None, 151 | tau_max=None, 152 | ) 153 | 154 | @jax.jit 155 | def compute_control_jit(z, z_des): 156 | return compute_control(robot, osc_controller, cbf, z, z_des) 157 | 158 | while True: 159 | joint_state = env.get_joint_state() 160 | ee_state_des = env.get_desired_ee_state() 161 | tau = compute_control_jit(joint_state, ee_state_des) 162 | env.apply_control(tau) 163 | env.step() 164 | 165 | 166 | if __name__ == "__main__": 167 | main() 168 | -------------------------------------------------------------------------------- /oscbf/examples/multiple_safety_conditions.py: -------------------------------------------------------------------------------- 1 | """Testing the performance of OSCBF under many different safety constraints, namely: 2 | 3 | 1. End-effector set containment 4 | 2. Joint limit avoidance 5 | 3. Singularity avoidance 6 | 4. Collision avoidance 7 | 5. Whole-body set containment 8 | 9 | While there are many other safety constraints that we could also account for, this 10 | should give a good view of the controller's performance under common situations 11 | encountered in practice. 12 | """ 13 | 14 | from functools import partial 15 | 16 | import numpy as np 17 | import jax 18 | import jax.numpy as jnp 19 | from jax.typing import ArrayLike 20 | from cbfpy import CBF 21 | 22 | from oscbf.core.manipulator import Manipulator, load_panda 23 | from oscbf.core.manipulation_env import FrankaTorqueControlEnv 24 | from oscbf.core.oscbf_configs import OSCBFTorqueConfig 25 | from oscbf.core.controllers import PoseTaskTorqueController 26 | 27 | 28 | @jax.tree_util.register_static 29 | class CombinedConfig(OSCBFTorqueConfig): 30 | 31 | def __init__( 32 | self, 33 | robot: Manipulator, 34 | pos_min: ArrayLike, 35 | pos_max: ArrayLike, 36 | collision_positions: ArrayLike, 37 | collision_radii: ArrayLike, 38 | whole_body_pos_min: ArrayLike, 39 | whole_body_pos_max: ArrayLike, 40 | ): 41 | self.pos_min = np.asarray(pos_min) 42 | self.pos_max = np.asarray(pos_max) 43 | self.q_min = robot.joint_lower_limits 44 | self.q_max = robot.joint_upper_limits 45 | self.singularity_tol = 1e-3 46 | self.collision_positions = np.atleast_2d(collision_positions) 47 | self.collision_radii = np.ravel(collision_radii) 48 | assert len(collision_positions) == len(collision_radii) 49 | self.num_collision_bodies = len(collision_positions) 50 | self.whole_body_pos_min = np.asarray(whole_body_pos_min) 51 | self.whole_body_pos_max = np.asarray(whole_body_pos_max) 52 | super().__init__(robot) 53 | 54 | def h_2(self, z, **kwargs): 55 | # Extract values 56 | q = z[: self.num_joints] 57 | ee_pos = self.robot.ee_position(q) 58 | q_min = jnp.asarray(self.q_min) 59 | q_max = jnp.asarray(self.q_max) 60 | 61 | # EE Set Containment 62 | h_ee_safe_set = jnp.concatenate([self.pos_max - ee_pos, ee_pos - self.pos_min]) 63 | 64 | # Joint Limit Avoidance 65 | h_joint_limits = jnp.concatenate([q_max - q, q - q_min]) 66 | 67 | # Singularity Avoidance 68 | sigmas = jax.lax.linalg.svd(self.robot.ee_jacobian(q), compute_uv=False) 69 | h_singularity = jnp.array([jnp.prod(sigmas) - self.singularity_tol]) 70 | 71 | # Collision Avoidance 72 | robot_collision_pos_rad = self.robot.link_collision_data(q) 73 | robot_collision_positions = robot_collision_pos_rad[:, :3] 74 | robot_collision_radii = robot_collision_pos_rad[:, 3, None] 75 | robot_num_pts = robot_collision_positions.shape[0] 76 | center_deltas = ( 77 | robot_collision_positions[:, None, :] - self.collision_positions[None, :, :] 78 | ).reshape(-1, 3) 79 | radii_sums = ( 80 | robot_collision_radii[:, None] + self.collision_radii[None, :] 81 | ).reshape(-1) 82 | h_collision = jnp.linalg.norm(center_deltas, axis=1) - radii_sums 83 | 84 | # Whole-body Set Containment 85 | h_whole_body_upper = ( 86 | jnp.tile(self.whole_body_pos_max, (robot_num_pts, 1)) 87 | - robot_collision_positions 88 | - robot_collision_radii 89 | ).ravel() 90 | h_whole_body_lower = ( 91 | robot_collision_positions 92 | - jnp.tile(self.whole_body_pos_min, (robot_num_pts, 1)) 93 | - robot_collision_radii 94 | ).ravel() 95 | 96 | return jnp.concatenate( 97 | [ 98 | h_ee_safe_set, 99 | h_joint_limits, 100 | h_singularity, 101 | h_collision, 102 | h_whole_body_upper, 103 | h_whole_body_lower, 104 | ] 105 | ) 106 | 107 | def alpha(self, h): 108 | return 10.0 * h 109 | 110 | def alpha_2(self, h_2): 111 | return 10.0 * h_2 112 | 113 | 114 | # @partial(jax.jit, static_argnums=(0, 1, 2)) 115 | def compute_control( 116 | robot: Manipulator, 117 | osc_controller: PoseTaskTorqueController, 118 | cbf: CBF, 119 | z: ArrayLike, 120 | z_ee_des: ArrayLike, 121 | ): 122 | q = z[: robot.num_joints] 123 | qdot = z[robot.num_joints :] 124 | M, M_inv, g, c, J, ee_tmat = robot.torque_control_matrices(q, qdot) 125 | # Set nullspace desired joint position 126 | nullspace_posture_goal = jnp.array( 127 | [ 128 | 0.0, 129 | -jnp.pi / 6, 130 | 0.0, 131 | -3 * jnp.pi / 4, 132 | 0.0, 133 | 5 * jnp.pi / 9, 134 | 0.0, 135 | ] 136 | ) 137 | 138 | # Compute nominal control 139 | u_nom = osc_controller( 140 | q, 141 | qdot, 142 | pos=ee_tmat[:3, 3], 143 | rot=ee_tmat[:3, :3], 144 | des_pos=z_ee_des[:3], 145 | des_rot=jnp.reshape(z_ee_des[3:12], (3, 3)), 146 | des_vel=z_ee_des[12:15], 147 | des_omega=z_ee_des[15:18], 148 | des_accel=jnp.zeros(3), 149 | des_alpha=jnp.zeros(3), 150 | des_q=nullspace_posture_goal, 151 | des_qdot=jnp.zeros(robot.num_joints), 152 | J=J, 153 | M=M, 154 | M_inv=M_inv, 155 | g=g, 156 | c=c, 157 | ) 158 | # Apply the CBF safety filter 159 | return cbf.safety_filter(z, u_nom) 160 | 161 | 162 | def main(): 163 | robot = load_panda() 164 | ee_pos_min = np.array([0.15, -0.25, 0.25]) 165 | ee_pos_max = np.array([0.75, 0.25, 0.75]) 166 | wb_pos_min = np.array([-0.5, -0.5, 0.0]) 167 | wb_pos_max = np.array([0.75, 0.5, 1.0]) 168 | collision_pos = np.array([[0.5, 0.5, 0.5]]) 169 | collision_radii = np.array([0.3]) 170 | collision_data = {"positions": collision_pos, "radii": collision_radii} 171 | config = CombinedConfig( 172 | robot, 173 | ee_pos_min, 174 | ee_pos_max, 175 | collision_pos, 176 | collision_radii, 177 | wb_pos_min, 178 | wb_pos_max, 179 | ) 180 | cbf = CBF.from_config(config) 181 | env = FrankaTorqueControlEnv( 182 | config.pos_min, 183 | config.pos_max, 184 | collision_data=collision_data, 185 | wb_xyz_min=wb_pos_min, 186 | wb_xyz_max=wb_pos_max, 187 | load_floor=False, 188 | bg_color=(1, 1, 1), 189 | real_time=True, 190 | ) 191 | 192 | env.client.resetDebugVisualizerCamera( 193 | cameraDistance=2, 194 | cameraPitch=-27.80, 195 | cameraYaw=36.80, 196 | cameraTargetPosition=(0.08, 0.49, -0.04), 197 | ) 198 | 199 | kp_pos = 50.0 200 | kp_rot = 20.0 201 | kd_pos = 20.0 202 | kd_rot = 10.0 203 | kp_joint = 10.0 204 | kd_joint = 5.0 205 | osc_controller = PoseTaskTorqueController( 206 | n_joints=robot.num_joints, 207 | kp_task=np.concatenate([kp_pos * np.ones(3), kp_rot * np.ones(3)]), 208 | kd_task=np.concatenate([kd_pos * np.ones(3), kd_rot * np.ones(3)]), 209 | kp_joint=kp_joint, 210 | kd_joint=kd_joint, 211 | # Note: torque limits will be enforced via the QP. We'll set them to None here 212 | # because we don't want to clip the values before the QP 213 | tau_min=None, 214 | tau_max=None, 215 | ) 216 | 217 | @jax.jit 218 | def compute_control_jit(z, z_des): 219 | return compute_control(robot, osc_controller, cbf, z, z_des) 220 | 221 | while True: 222 | joint_state = env.get_joint_state() 223 | ee_state_des = env.get_desired_ee_state() 224 | tau = compute_control_jit(joint_state, ee_state_des) 225 | env.apply_control(tau) 226 | env.step() 227 | 228 | 229 | if __name__ == "__main__": 230 | main() 231 | -------------------------------------------------------------------------------- /oscbf/examples/dynamic_obstacle.py: -------------------------------------------------------------------------------- 1 | """Testing dynamic obstacle avoidance 2 | 3 | This is a bit hacked-together, but works. 4 | """ 5 | 6 | import numpy as np 7 | import jax 8 | import jax.numpy as jnp 9 | from jax.typing import ArrayLike 10 | from cbfpy import CBF 11 | 12 | from oscbf.core.manipulator import Manipulator, load_panda 13 | from oscbf.core.manipulation_env import FrankaTorqueControlEnv 14 | from oscbf.core.oscbf_configs import OSCBFTorqueConfig 15 | from oscbf.core.controllers import PoseTaskTorqueController 16 | 17 | @jax.tree_util.register_static 18 | class SelfCollisionConfig(OSCBFTorqueConfig): 19 | 20 | def __init__( 21 | self, 22 | robot: Manipulator, 23 | ): 24 | self.q_min = robot.joint_lower_limits 25 | self.q_max = robot.joint_upper_limits 26 | self.singularity_tol = 1e-3 27 | dummy_z_obs = np.zeros(6) 28 | super().__init__(robot, init_args=(dummy_z_obs,)) 29 | 30 | def h_2(self, z, z_obs): 31 | # Extract values 32 | q = z[: self.num_joints] 33 | qdot = z[self.num_joints:] 34 | 35 | # Self collision avoidance 36 | robot_collision_pos_rad = self.robot.link_self_collision_data(q) 37 | robot_collision_positions = robot_collision_pos_rad[:, :3] 38 | robot_collision_radii = robot_collision_pos_rad[:, 3] 39 | pairs = jnp.asarray(self.robot.self_collision_pairs) 40 | pos_a = robot_collision_positions[pairs[:, 0]] 41 | pos_b = robot_collision_positions[pairs[:, 1]] 42 | rad_a = robot_collision_radii[pairs[:, 0]] 43 | rad_b = robot_collision_radii[pairs[:, 1]] 44 | center_deltas = pos_b - pos_a 45 | h_self_collision = jnp.linalg.norm(center_deltas, axis=-1) - rad_a - rad_b 46 | 47 | # Base self collision avoidance 48 | base_position = jnp.asarray(self.robot.base_self_collision_position) 49 | base_radius = self.robot.base_self_collision_radius 50 | base_sc_idxs = jnp.asarray(self.robot.base_self_collision_idxs) 51 | base_sc_deltas = robot_collision_positions[base_sc_idxs] - base_position 52 | h_base_self_collision = ( 53 | jnp.linalg.norm(base_sc_deltas, axis=-1) 54 | - robot_collision_radii[base_sc_idxs] 55 | - base_radius 56 | ) 57 | 58 | # Joint Limit Avoidance 59 | q_min = jnp.asarray(self.q_min) 60 | q_max = jnp.asarray(self.q_max) 61 | h_joint_limits = jnp.concatenate([q_max - q, q - q_min]) 62 | 63 | # Singularity Avoidance 64 | ee_jacobian = self.robot.ee_jacobian(q) 65 | sigmas = jax.lax.linalg.svd(ee_jacobian, compute_uv=False) 66 | h_singularity = jnp.array([jnp.prod(sigmas) - self.singularity_tol]) 67 | 68 | # Dynamic Obstacle Avoidance (LAST LINK/SPHERE ONLY -- FOR NOW) 69 | # TODO make a version of this with all spheres 70 | obstacle_radius = 0.25 * 0.2 71 | pos_obs = z_obs[:3] 72 | vel_obs = z_obs[3:] 73 | pos_robot = robot_collision_positions[-1] 74 | robot_radius = robot_collision_radii[-1] 75 | dist_between_centers = jnp.linalg.norm(pos_obs - pos_robot) 76 | dir_obs_to_robot = (pos_robot - pos_obs) / dist_between_centers 77 | # TODO use the actual relative velocity between the robot and the obstacle 78 | # This would require using the CORRECT jacobian though 79 | # The ee jacobian is aligned with the tip of the gripper, not necessarily 80 | # the center of the collision sphere used for this demo 81 | # If so, the barrier would be relative degree 1 and should be moved to h_1 82 | vel_robot = jnp.zeros(3) 83 | # Inflate the sphere by a small amount depending on the relative velocity 84 | collision_velocity_component = (vel_obs - vel_robot).T @ dir_obs_to_robot 85 | lookahead_time = 0.25 86 | h_dynamic_obstacle = jnp.array( 87 | [ 88 | dist_between_centers 89 | - collision_velocity_component * lookahead_time 90 | - obstacle_radius 91 | - robot_radius 92 | ] 93 | ) 94 | 95 | return jnp.concatenate([h_self_collision, h_base_self_collision, h_dynamic_obstacle, h_joint_limits, h_singularity]) 96 | 97 | def alpha(self, h): 98 | return 10.0 * h 99 | 100 | def alpha_2(self, h_2): 101 | return 10.0 * h_2 102 | 103 | 104 | # @partial(jax.jit, static_argnums=(0, 1, 2)) 105 | def compute_control( 106 | robot: Manipulator, 107 | osc_controller: PoseTaskTorqueController, 108 | cbf: CBF, 109 | z: ArrayLike, 110 | z_obs: ArrayLike, 111 | ): 112 | # HACK: Assume a fixed desired end effector state for this demo 113 | # This ee pose is from running FK on the initial joint state 114 | ee_pos_des = jnp.array([2.39585972e-01, -6.18426969e-12, 4.28947096e-01]) 115 | ee_rmat_des = jnp.diag(jnp.array([1.0, -1.0, -1.0])) 116 | ee_twist_des = jnp.zeros(6) 117 | z_ee_des = jnp.concatenate([ee_pos_des, ee_rmat_des.ravel(), ee_twist_des]) 118 | 119 | q = z[: robot.num_joints] 120 | qdot = z[robot.num_joints :] 121 | M, M_inv, g, c, J, ee_tmat = robot.torque_control_matrices(q, qdot) 122 | # Set nullspace desired joint position 123 | nullspace_posture_goal = jnp.array( 124 | [ 125 | 0.0, 126 | -jnp.pi / 6, 127 | 0.0, 128 | -3 * jnp.pi / 4, 129 | 0.0, 130 | 5 * jnp.pi / 9, 131 | 0.0, 132 | ] 133 | ) 134 | 135 | # Compute nominal control 136 | u_nom = osc_controller( 137 | q, 138 | qdot, 139 | pos=ee_tmat[:3, 3], 140 | rot=ee_tmat[:3, :3], 141 | des_pos=z_ee_des[:3], 142 | des_rot=jnp.reshape(z_ee_des[3:12], (3, 3)), 143 | des_vel=z_ee_des[12:15], 144 | des_omega=z_ee_des[15:18], 145 | des_accel=jnp.zeros(3), 146 | des_alpha=jnp.zeros(3), 147 | des_q=nullspace_posture_goal, 148 | des_qdot=jnp.zeros(robot.num_joints), 149 | J=J, 150 | M=M, 151 | M_inv=M_inv, 152 | g=g, 153 | c=c, 154 | ) 155 | # Apply the CBF safety filter 156 | return cbf.safety_filter(z, u_nom, z_obs) 157 | 158 | 159 | def main(): 160 | robot = load_panda() 161 | config = SelfCollisionConfig(robot) 162 | cbf = CBF.from_config(config) 163 | env = FrankaTorqueControlEnv( 164 | load_floor=False, 165 | bg_color=(1, 1, 1), 166 | real_time=True, 167 | ) 168 | 169 | env.client.resetDebugVisualizerCamera( 170 | cameraDistance=2, 171 | cameraPitch=-27.80, 172 | cameraYaw=36.80, 173 | cameraTargetPosition=(0.08, 0.49, -0.04), 174 | ) 175 | 176 | kp_pos = 50.0 177 | kp_rot = 20.0 178 | kd_pos = 20.0 179 | kd_rot = 10.0 180 | kp_joint = 10.0 181 | kd_joint = 5.0 182 | osc_controller = PoseTaskTorqueController( 183 | n_joints=robot.num_joints, 184 | kp_task=np.concatenate([kp_pos * np.ones(3), kp_rot * np.ones(3)]), 185 | kd_task=np.concatenate([kd_pos * np.ones(3), kd_rot * np.ones(3)]), 186 | kp_joint=kp_joint, 187 | kd_joint=kd_joint, 188 | # Note: torque limits will be enforced via the QP. We'll set them to None here 189 | # because we don't want to clip the values before the QP 190 | tau_min=None, 191 | tau_max=None, 192 | ) 193 | 194 | @jax.jit 195 | def compute_control_jit(z, z_obs): 196 | return compute_control(robot, osc_controller, cbf, z, z_obs) 197 | 198 | while True: 199 | joint_state = env.get_joint_state() 200 | # HACK: Make the ee state represent the state of the obstacle 201 | # instead of the position, rotation, and twist of the EE 202 | z_obs_full = env.get_desired_ee_state() 203 | z_obs = np.concatenate([z_obs_full[:3], z_obs_full[12:15]]) 204 | tau = compute_control_jit(joint_state, z_obs) 205 | env.apply_control(tau) 206 | env.step() 207 | 208 | 209 | if __name__ == "__main__": 210 | main() 211 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/collision/finger.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.0.1262) 3 | 4 | mtllib finger.stl.obj.mtl 5 | 6 | # 96 vertex positions 7 | v 0.01036 0.0264034 0.000154629 8 | v 0.0104486 0.0025833 0.000146801 9 | v -0.0103872 0.00253418 0.000131696 10 | v -0.0103872 0.00253418 0.000131696 11 | v -0.0104795 0.0161016 0.0189882 12 | v -0.0104013 0.0263094 0.00016651 13 | v -0.0104013 0.0263094 0.00016651 14 | v -0.0104795 0.0161016 0.0189882 15 | v -0.0103889 0.0252203 0.0191876 16 | v 0.0104486 0.0025833 0.000146801 17 | v -0.0087304 -2.35252e-05 0.0361648 18 | v -0.0103872 0.00253418 0.000131696 19 | v 0.0104005 0.0252534 0.0190366 20 | v -0.0103889 0.0252203 0.0191876 21 | v 0.00583983 0.0142743 0.0538035 22 | v -0.0103872 0.00253418 0.000131696 23 | v -0.0104013 0.0263094 0.00016651 24 | v 0.01036 0.0264034 0.000154629 25 | v 0.00861608 0.0139887 0.0513279 26 | v 0.0104948 0.0151026 0.0184356 27 | v 0.0104005 0.0252534 0.0190366 28 | v 0.0104948 0.0151026 0.0184356 29 | v 0.00869277 -0.000132643 0.0501662 30 | v 0.0104486 0.0025833 0.000146801 31 | v 0.00861608 0.0139887 0.0513279 32 | v 0.00869277 -0.000132643 0.0501662 33 | v 0.0104948 0.0151026 0.0184356 34 | v -0.00862294 -5.68019e-05 0.0509528 35 | v -0.0103872 0.00253418 0.000131696 36 | v -0.0087304 -2.35252e-05 0.0361648 37 | v 0.0104486 0.0025833 0.000146801 38 | v 0.00869277 -0.000132643 0.0501662 39 | v -0.0087304 -2.35252e-05 0.0361648 40 | v 0.00869277 -0.000132643 0.0501662 41 | v -0.00548142 -9.11208e-05 0.0537247 42 | v -0.0087304 -2.35252e-05 0.0361648 43 | v 0.0104005 0.0252534 0.0190366 44 | v 0.0104948 0.0151026 0.0184356 45 | v 0.0104486 0.0025833 0.000146801 46 | v 0.0104005 0.0252534 0.0190366 47 | v 0.0104486 0.0025833 0.000146801 48 | v 0.01036 0.0264034 0.000154629 49 | v 0.0104005 0.0252534 0.0190366 50 | v 0.01036 0.0264034 0.000154629 51 | v -0.0103889 0.0252203 0.0191876 52 | v -0.0103889 0.0252203 0.0191876 53 | v -0.00527792 0.0142931 0.053849 54 | v 0.00583983 0.0142743 0.0538035 55 | v -0.00777838 0.0142182 0.0523659 56 | v -0.00527792 0.0142931 0.053849 57 | v -0.0103889 0.0252203 0.0191876 58 | v -0.00862294 -5.68019e-05 0.0509528 59 | v -0.00548142 -9.11208e-05 0.0537247 60 | v -0.00777838 0.0142182 0.0523659 61 | v -0.0103872 0.00253418 0.000131696 62 | v -0.00862294 -5.68019e-05 0.0509528 63 | v -0.0104795 0.0161016 0.0189882 64 | v 0.0104005 0.0252534 0.0190366 65 | v 0.00583983 0.0142743 0.0538035 66 | v 0.00861608 0.0139887 0.0513279 67 | v 0.01036 0.0264034 0.000154629 68 | v -0.0104013 0.0263094 0.00016651 69 | v -0.0103889 0.0252203 0.0191876 70 | v -0.0104795 0.0161016 0.0189882 71 | v -0.00884117 0.0139176 0.0505894 72 | v -0.0103889 0.0252203 0.0191876 73 | v -0.00884117 0.0139176 0.0505894 74 | v -0.00777838 0.0142182 0.0523659 75 | v -0.0103889 0.0252203 0.0191876 76 | v 0.00583983 0.0142743 0.0538035 77 | v -0.00548142 -9.11208e-05 0.0537247 78 | v 0.00613802 -2.06026e-05 0.0535776 79 | v -0.00527792 0.0142931 0.053849 80 | v -0.00548142 -9.11208e-05 0.0537247 81 | v 0.00583983 0.0142743 0.0538035 82 | v 0.00869277 -0.000132643 0.0501662 83 | v 0.00613802 -2.06026e-05 0.0535776 84 | v -0.00548142 -9.11208e-05 0.0537247 85 | v 0.00613802 -2.06026e-05 0.0535776 86 | v 0.00869277 -0.000132643 0.0501662 87 | v 0.00861608 0.0139887 0.0513279 88 | v -0.00548142 -9.11208e-05 0.0537247 89 | v -0.00862294 -5.68019e-05 0.0509528 90 | v -0.0087304 -2.35252e-05 0.0361648 91 | v -0.00777838 0.0142182 0.0523659 92 | v -0.00548142 -9.11208e-05 0.0537247 93 | v -0.00527792 0.0142931 0.053849 94 | v -0.00862294 -5.68019e-05 0.0509528 95 | v -0.00884117 0.0139176 0.0505894 96 | v -0.0104795 0.0161016 0.0189882 97 | v 0.00613802 -2.06026e-05 0.0535776 98 | v 0.00861608 0.0139887 0.0513279 99 | v 0.00583983 0.0142743 0.0538035 100 | v -0.00777838 0.0142182 0.0523659 101 | v -0.00884117 0.0139176 0.0505894 102 | v -0.00862294 -5.68019e-05 0.0509528 103 | 104 | # 0 UV coordinates 105 | 106 | # 96 vertex normals 107 | vn 0.000724164 0.000331308 -1 108 | vn 0.000724164 0.000331308 -1 109 | vn 0.000724164 0.000331308 -1 110 | vn -0.99999 -0.000585782 -0.00447174 111 | vn -0.99999 -0.000585782 -0.00447174 112 | vn -0.99999 -0.000585782 -0.00447174 113 | vn -0.99995 0.00990875 0.00122006 114 | vn -0.99995 0.00990875 0.00122006 115 | vn -0.99995 0.00990875 0.00122006 116 | vn 0.00240304 -0.997479 -0.0709137 117 | vn 0.00240304 -0.997479 -0.0709137 118 | vn 0.00240304 -0.997479 -0.0709137 119 | vn 0.000668274 0.953556 0.301214 120 | vn 0.000668274 0.953556 0.301214 121 | vn 0.000668274 0.953556 0.301214 122 | vn -0.0005789 0.00146393 -0.999999 123 | vn -0.0005789 0.00146393 -0.999999 124 | vn -0.0005789 0.00146393 -0.999999 125 | vn 0.998344 0.00589157 0.0572229 126 | vn 0.998344 0.00589157 0.0572229 127 | vn 0.998344 0.00589157 0.0572229 128 | vn 0.998185 -0.050838 0.0322792 129 | vn 0.998185 -0.050838 0.0322792 130 | vn 0.998185 -0.050838 0.0322792 131 | vn 0.998371 0.000729252 0.0570496 132 | vn 0.998371 0.000729252 0.0570496 133 | vn 0.998371 0.000729252 0.0570496 134 | vn -0.871287 -0.490747 0.00522707 135 | vn -0.871287 -0.490747 0.00522707 136 | vn -0.871287 -0.490747 0.00522707 137 | vn 0.0362712 -0.99794 -0.0529128 138 | vn 0.0362712 -0.99794 -0.0529128 139 | vn 0.0362712 -0.99794 -0.0529128 140 | vn -0.00372285 -0.999988 -0.00316058 141 | vn -0.00372285 -0.999988 -0.00316058 142 | vn -0.00372285 -0.999988 -0.00316058 143 | vn 0.999909 0.00984192 -0.00926313 144 | vn 0.999909 0.00984192 -0.00926313 145 | vn 0.999909 0.00984192 -0.00926313 146 | vn 0.999991 0.00372285 -0.00191858 147 | vn 0.999991 0.00372285 -0.00191858 148 | vn 0.999991 0.00372285 -0.00191858 149 | vn -0.0011495 0.99815 0.0607926 150 | vn -0.0011495 0.99815 0.0607926 151 | vn -0.0011495 0.99815 0.0607926 152 | vn 0.00284562 0.953846 0.300284 153 | vn 0.00284562 0.953846 0.300284 154 | vn 0.00284562 0.953846 0.300284 155 | vn -0.218924 0.920873 0.32259 156 | vn -0.218924 0.920873 0.32259 157 | vn -0.218924 0.920873 0.32259 158 | vn -0.661425 -0.0350314 0.749193 159 | vn -0.661425 -0.0350314 0.749193 160 | vn -0.661425 -0.0350314 0.749193 161 | vn -0.998169 -0.0513123 0.0320353 162 | vn -0.998169 -0.0513123 0.0320353 163 | vn -0.998169 -0.0513123 0.0320353 164 | vn 0.377717 0.867563 0.323518 165 | vn 0.377717 0.867563 0.323518 166 | vn 0.377717 0.867563 0.323518 167 | vn -0.00448618 0.998355 0.0571685 168 | vn -0.00448618 0.998355 0.0571685 169 | vn -0.00448618 0.998355 0.0571685 170 | vn -0.998589 0.0087761 0.0523757 171 | vn -0.998589 0.0087761 0.0523757 172 | vn -0.998589 0.0087761 0.0523757 173 | vn -0.665951 0.690851 0.281485 174 | vn -0.665951 0.690851 0.281485 175 | vn -0.665951 0.690851 0.281485 176 | vn 0.0127505 -0.0155288 0.999798 177 | vn 0.0127505 -0.0155288 0.999798 178 | vn 0.0127505 -0.0155288 0.999798 179 | vn 0.00408502 -0.00870048 0.999954 180 | vn 0.00408502 -0.00870048 0.999954 181 | vn 0.00408502 -0.00870048 0.999954 182 | vn 0.006542 -0.999267 0.0377181 183 | vn 0.006542 -0.999267 0.0377181 184 | vn 0.006542 -0.999267 0.0377181 185 | vn 0.798906 -0.0450007 0.59977 186 | vn 0.798906 -0.0450007 0.59977 187 | vn 0.798906 -0.0450007 0.59977 188 | vn -0.00899609 -0.999957 -0.00218479 189 | vn -0.00899609 -0.999957 -0.00218479 190 | vn -0.00899609 -0.999957 -0.00218479 191 | vn -0.510144 -0.000216662 0.860089 192 | vn -0.510144 -0.000216662 0.860089 193 | vn -0.510144 -0.000216662 0.860089 194 | vn -0.998608 -0.0142745 0.0507836 195 | vn -0.998608 -0.0142745 0.0507836 196 | vn -0.998608 -0.0142745 0.0507836 197 | vn 0.665648 0.00209596 0.746263 198 | vn 0.665648 0.00209596 0.746263 199 | vn 0.665648 0.00209596 0.746263 200 | vn -0.858159 -4.90786e-05 0.513384 201 | vn -0.858159 -4.90786e-05 0.513384 202 | vn -0.858159 -4.90786e-05 0.513384 203 | 204 | # Mesh '' with 32 faces 205 | g 206 | usemtl DefaultMaterial 207 | f 1//1 2//2 3//3 208 | f 4//4 5//5 6//6 209 | f 7//7 8//8 9//9 210 | f 10//10 11//11 12//12 211 | f 13//13 14//14 15//15 212 | f 16//16 17//17 18//18 213 | f 19//19 20//20 21//21 214 | f 22//22 23//23 24//24 215 | f 25//25 26//26 27//27 216 | f 28//28 29//29 30//30 217 | f 31//31 32//32 33//33 218 | f 34//34 35//35 36//36 219 | f 37//37 38//38 39//39 220 | f 40//40 41//41 42//42 221 | f 43//43 44//44 45//45 222 | f 46//46 47//47 48//48 223 | f 49//49 50//50 51//51 224 | f 52//52 53//53 54//54 225 | f 55//55 56//56 57//57 226 | f 58//58 59//59 60//60 227 | f 61//61 62//62 63//63 228 | f 64//64 65//65 66//66 229 | f 67//67 68//68 69//69 230 | f 70//70 71//71 72//72 231 | f 73//73 74//74 75//75 232 | f 76//76 77//77 78//78 233 | f 79//79 80//80 81//81 234 | f 82//82 83//83 84//84 235 | f 85//85 86//86 87//87 236 | f 88//88 89//89 90//90 237 | f 91//91 92//92 93//93 238 | f 94//94 95//95 96//96 239 | 240 | -------------------------------------------------------------------------------- /oscbf/core/oscbf_configs.py: -------------------------------------------------------------------------------- 1 | """OSCBF Configs 2 | 3 | Includes the dynamics and task-consistent objective functions for both 4 | velocity control and torque control. 5 | 6 | (These objectives assume that the desired task is tracking a desired pose 7 | with a secondary null space posture target) 8 | 9 | Note: The CBFs themselves are not included in this file. These should 10 | inherit from these configs, and implement the h_1/h_2 methods 11 | depending on the desired safety criteria. For examples of this, 12 | see the `oscbf/examples` folder 13 | """ 14 | 15 | import jax.numpy as jnp 16 | import numpy as np 17 | from cbfpy import CBFConfig 18 | 19 | from oscbf.core.manipulator import Manipulator 20 | 21 | 22 | class OSCBFTorqueConfig(CBFConfig): 23 | """CBF Configuration for safe torque-controlled manipulation 24 | 25 | State: z = [q, qdot] (length = 2 * num_joints) 26 | Control: u = [joint torques] (length = num_joints) 27 | 28 | Args: 29 | robot (Manipulator): The robot model (kinematics and dynamics) 30 | pos_obj_weight (float, optional): Objective function weight for the safety filter's 31 | impact on the end-effector's position tracking performance. Defaults to 1.0. 32 | rot_obj_weight (float, optional): Objective function weight for the safety filter's 33 | impact on the end-effector's orientation tracking performance. Defaults to 1.0. 34 | joint_obj_weight (float, optional): Objective function weight for the safety filter's 35 | impact on the joint space tracking performance. Defaults to 1.0. 36 | compensate_centrifugal_coriolis (bool, optional): Whether to compensate for centrifugal/coriolis 37 | terms in the robot dynamics. This term is often neglected (set to False). Defaults to True. 38 | init_args (tuple, optional): Optional initial seed for testing additional CBFpy barrier arguments. 39 | Defaults to (). 40 | """ 41 | 42 | def __init__( 43 | self, 44 | robot: Manipulator, 45 | pos_obj_weight: float = 1.0, 46 | rot_obj_weight: float = 1.0, 47 | joint_obj_weight: float = 1.0, 48 | compensate_centrifugal_coriolis: bool = True, 49 | init_args: tuple = (), 50 | ): 51 | assert isinstance(robot, Manipulator) 52 | assert isinstance(pos_obj_weight, (tuple, float)) and pos_obj_weight >= 0 53 | assert isinstance(rot_obj_weight, (tuple, float)) and rot_obj_weight >= 0 54 | assert isinstance(joint_obj_weight, (tuple, float)) and joint_obj_weight >= 0 55 | assert isinstance(compensate_centrifugal_coriolis, bool) 56 | self.robot = robot 57 | self.num_joints = self.robot.num_joints 58 | self.task_dim = 6 # Pose 59 | self.is_redundant = self.robot.num_joints > self.task_dim 60 | self.compensate_centrifugal_coriolis = compensate_centrifugal_coriolis 61 | self.pos_obj_weight = float(pos_obj_weight) 62 | self.rot_obj_weight = float(rot_obj_weight) 63 | self.joint_space_obj_weight = float(joint_obj_weight) 64 | # Store the diagonal of W.T @ W for the task and joint space weighting matrices 65 | # This assumes that W is a diagonal matrix with only positive values 66 | self.W_T_W_task_diag = tuple( 67 | np.array([self.pos_obj_weight] * 3 + [self.rot_obj_weight] * 3) ** 2 68 | ) 69 | self.W_T_W_joint_diag = tuple( 70 | np.array([self.joint_space_obj_weight] * self.num_joints) ** 2 71 | ) 72 | 73 | super().__init__( 74 | n=self.num_joints * 2, 75 | m=self.num_joints, 76 | u_min=-np.asarray(robot.joint_max_forces), 77 | u_max=np.asarray(robot.joint_max_forces), 78 | init_args=init_args, 79 | ) 80 | 81 | def f(self, z, *args, **kwargs): 82 | q = z[: self.num_joints] 83 | q_dot = z[self.num_joints : self.num_joints * 2] 84 | M = self.robot.mass_matrix(q) 85 | M_inv = jnp.linalg.inv(M) 86 | bias = self.robot.gravity_vector(q) 87 | if self.compensate_centrifugal_coriolis: 88 | bias += self.robot.centrifugal_coriolis_vector(q, q_dot) 89 | return jnp.concatenate( 90 | [ 91 | q_dot, # Joint velocity 92 | -M_inv @ bias, # Joint acceleration 93 | ] 94 | ) 95 | 96 | def g(self, z, *args, **kwargs): 97 | q = z[: self.num_joints] 98 | M = self.robot.mass_matrix(q) 99 | M_inv = jnp.linalg.inv(M) 100 | return jnp.block( 101 | [ 102 | [jnp.zeros((self.num_joints, self.m))], 103 | [M_inv], 104 | ] 105 | ) 106 | 107 | def _P(self, z, *args, **kwargs): 108 | """Helper function to compute the QP P matrix. This get reused in both the P and q QP functions""" 109 | q = z[: self.num_joints] 110 | transforms = self.robot.joint_to_world_transforms(q) 111 | M = self.robot._mass_matrix(transforms) 112 | M_inv = jnp.linalg.inv(M) 113 | J = self.robot._ee_jacobian(transforms) 114 | task_inertia_inv = J @ M_inv @ J.T 115 | task_inertia = jnp.linalg.inv(task_inertia_inv) 116 | J_bar = M_inv @ J.T @ task_inertia 117 | NT = jnp.eye(self.num_joints) - J.T @ J_bar.T 118 | 119 | W_T_W_joint = jnp.diag(jnp.asarray(self.W_T_W_joint_diag)) 120 | W_T_W_task = jnp.diag(jnp.asarray(self.W_T_W_task_diag)) 121 | 122 | return ( 123 | NT.T @ M_inv.T @ W_T_W_joint @ M_inv @ NT 124 | + M_inv.T @ J.T @ W_T_W_task @ J @ M_inv 125 | ) 126 | 127 | def P(self, z, u_des, *args, **kwargs): 128 | return self._P(z) 129 | 130 | def q(self, z, u_des, *args, **kwargs): 131 | return -u_des.T @ self._P(z) 132 | 133 | 134 | class OSCBFVelocityConfig(CBFConfig): 135 | """CBF Configuration for safe velocity-controlled manipulation 136 | 137 | State: z = [q] (length = num_joints) 138 | Control: u = [joint velocities] (length = num_joints) 139 | 140 | Args: 141 | robot (Manipulator): The robot model (kinematics and dynamics) 142 | pos_obj_weight (float, optional): Objective function weight for the safety filter's 143 | impact on the end-effector's position tracking performance. Defaults to 1.0. 144 | rot_obj_weight (float, optional): Objective function weight for the safety filter's 145 | impact on the end-effector's orientation tracking performance. Defaults to 1.0. 146 | joint_obj_weight (float, optional): Objective function weight for the safety filter's 147 | impact on the joint space tracking performance. Defaults to 1.0. 148 | init_args (tuple, optional): Optional initial seed for testing additional CBFpy barrier arguments. 149 | Defaults to (). 150 | """ 151 | 152 | def __init__( 153 | self, 154 | robot: Manipulator, 155 | pos_obj_weight: float = 1.0, 156 | rot_obj_weight: float = 1.0, 157 | joint_obj_weight: float = 1.0, 158 | init_args: tuple = (), 159 | ): 160 | assert isinstance(robot, Manipulator) 161 | assert isinstance(pos_obj_weight, (tuple, float)) and pos_obj_weight >= 0 162 | assert isinstance(rot_obj_weight, (tuple, float)) and rot_obj_weight >= 0 163 | assert isinstance(joint_obj_weight, (tuple, float)) and joint_obj_weight >= 0 164 | self.robot = robot 165 | self.num_joints = self.robot.num_joints 166 | self.task_dim = 6 # Pose 167 | self.is_redundant = self.robot.num_joints > self.task_dim 168 | 169 | self.pos_obj_weight = float(pos_obj_weight) 170 | self.rot_obj_weight = float(rot_obj_weight) 171 | self.joint_space_obj_weight = float(joint_obj_weight) 172 | # Store the diagonal of W.T @ W for the task and joint space weighting matrices 173 | # This assumes that W is a diagonal matrix with only positive values 174 | self.W_T_W_task_diag = tuple( 175 | np.array([self.pos_obj_weight] * 3 + [self.rot_obj_weight] * 3) ** 2 176 | ) 177 | self.W_T_W_joint_diag = tuple( 178 | np.array([self.joint_space_obj_weight] * self.num_joints) ** 2 179 | ) 180 | 181 | super().__init__( 182 | n=self.num_joints, 183 | m=self.num_joints, 184 | u_min=-np.asarray(robot.joint_max_velocities), 185 | u_max=np.asarray(robot.joint_max_velocities), 186 | init_args=init_args, 187 | ) 188 | 189 | def f(self, z, *args, **kwargs): 190 | return jnp.zeros(self.n) 191 | 192 | def g(self, z, *args, **kwargs): 193 | return jnp.eye(self.num_joints) 194 | 195 | def _P(self, z, *args, **kwargs): 196 | q = z 197 | transforms = self.robot.joint_to_world_transforms(q) 198 | J = self.robot._ee_jacobian(transforms) 199 | M = self.robot._mass_matrix(transforms) 200 | M_inv = jnp.linalg.inv(M) 201 | task_inertia_inv = J @ M_inv @ J.T 202 | task_inertia = jnp.linalg.inv(task_inertia_inv) 203 | J_bar = M_inv @ J.T @ task_inertia 204 | J_hash = J_bar 205 | N = jnp.eye(self.num_joints) - J_hash @ J 206 | W_T_W_joint = jnp.diag(jnp.asarray(self.W_T_W_joint_diag)) 207 | W_T_W_task = jnp.diag(jnp.asarray(self.W_T_W_task_diag)) 208 | 209 | return N.T @ W_T_W_joint @ N + J.T @ W_T_W_task @ J 210 | 211 | def P(self, z, u_des, *args, **kwargs): 212 | return self._P(z) 213 | 214 | def q(self, z, u_des, *args, **kwargs): 215 | return -u_des.T @ self._P(z) 216 | -------------------------------------------------------------------------------- /oscbf/utils/urdfpy/utils.py: -------------------------------------------------------------------------------- 1 | """Utilities for URDF parsing. 2 | """ 3 | 4 | import os 5 | 6 | from lxml import etree as ET 7 | import numpy as np 8 | import trimesh 9 | 10 | 11 | def rpy_to_matrix(coords): 12 | """Convert roll-pitch-yaw coordinates to a 3x3 homogenous rotation matrix. 13 | 14 | The roll-pitch-yaw axes in a typical URDF are defined as a 15 | rotation of ``r`` radians around the x-axis followed by a rotation of 16 | ``p`` radians around the y-axis followed by a rotation of ``y`` radians 17 | around the z-axis. These are the Z1-Y2-X3 Tait-Bryan angles. See 18 | Wikipedia_ for more information. 19 | 20 | .. _Wikipedia: https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix 21 | 22 | Parameters 23 | ---------- 24 | coords : (3,) float 25 | The roll-pitch-yaw coordinates in order (x-rot, y-rot, z-rot). 26 | 27 | Returns 28 | ------- 29 | R : (3,3) float 30 | The corresponding homogenous 3x3 rotation matrix. 31 | """ 32 | coords = np.asanyarray(coords, dtype=np.float64) 33 | c3, c2, c1 = np.cos(coords) 34 | s3, s2, s1 = np.sin(coords) 35 | 36 | return np.array( 37 | [ 38 | [c1 * c2, (c1 * s2 * s3) - (c3 * s1), (s1 * s3) + (c1 * c3 * s2)], 39 | [c2 * s1, (c1 * c3) + (s1 * s2 * s3), (c3 * s1 * s2) - (c1 * s3)], 40 | [-s2, c2 * s3, c2 * c3], 41 | ], 42 | dtype=np.float64, 43 | ) 44 | 45 | 46 | def matrix_to_rpy(R, solution=1): 47 | """Convert a 3x3 transform matrix to roll-pitch-yaw coordinates. 48 | 49 | The roll-pitchRyaw axes in a typical URDF are defined as a 50 | rotation of ``r`` radians around the x-axis followed by a rotation of 51 | ``p`` radians around the y-axis followed by a rotation of ``y`` radians 52 | around the z-axis. These are the Z1-Y2-X3 Tait-Bryan angles. See 53 | Wikipedia_ for more information. 54 | 55 | .. _Wikipedia: https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix 56 | 57 | There are typically two possible roll-pitch-yaw coordinates that could have 58 | created a given rotation matrix. Specify ``solution=1`` for the first one 59 | and ``solution=2`` for the second one. 60 | 61 | Parameters 62 | ---------- 63 | R : (3,3) float 64 | A 3x3 homogenous rotation matrix. 65 | solution : int 66 | Either 1 or 2, indicating which solution to return. 67 | 68 | Returns 69 | ------- 70 | coords : (3,) float 71 | The roll-pitch-yaw coordinates in order (x-rot, y-rot, z-rot). 72 | """ 73 | R = np.asanyarray(R, dtype=np.float64) 74 | r = 0.0 75 | p = 0.0 76 | y = 0.0 77 | 78 | if np.abs(R[2, 0]) >= 1.0 - 1e-12: 79 | y = 0.0 80 | if R[2, 0] < 0: 81 | p = np.pi / 2 82 | r = np.arctan2(R[0, 1], R[0, 2]) 83 | else: 84 | p = -np.pi / 2 85 | r = np.arctan2(-R[0, 1], -R[0, 2]) 86 | else: 87 | if solution == 1: 88 | p = -np.arcsin(R[2, 0]) 89 | else: 90 | p = np.pi + np.arcsin(R[2, 0]) 91 | r = np.arctan2(R[2, 1] / np.cos(p), R[2, 2] / np.cos(p)) 92 | y = np.arctan2(R[1, 0] / np.cos(p), R[0, 0] / np.cos(p)) 93 | 94 | return np.array([r, p, y], dtype=np.float64) 95 | 96 | 97 | def matrix_to_xyz_rpy(matrix): 98 | """Convert a 4x4 homogenous matrix to xyzrpy coordinates. 99 | 100 | Parameters 101 | ---------- 102 | matrix : (4,4) float 103 | The homogenous transform matrix. 104 | 105 | Returns 106 | ------- 107 | xyz_rpy : (6,) float 108 | The xyz_rpy vector. 109 | """ 110 | xyz = matrix[:3, 3] 111 | rpy = matrix_to_rpy(matrix[:3, :3]) 112 | return np.hstack((xyz, rpy)) 113 | 114 | 115 | def xyz_rpy_to_matrix(xyz_rpy): 116 | """Convert xyz_rpy coordinates to a 4x4 homogenous matrix. 117 | 118 | Parameters 119 | ---------- 120 | xyz_rpy : (6,) float 121 | The xyz_rpy vector. 122 | 123 | Returns 124 | ------- 125 | matrix : (4,4) float 126 | The homogenous transform matrix. 127 | """ 128 | matrix = np.eye(4, dtype=np.float64) 129 | matrix[:3, 3] = xyz_rpy[:3] 130 | matrix[:3, :3] = rpy_to_matrix(xyz_rpy[3:]) 131 | return matrix 132 | 133 | 134 | def parse_origin(node): 135 | """Find the ``origin`` subelement of an XML node and convert it 136 | into a 4x4 homogenous transformation matrix. 137 | 138 | Parameters 139 | ---------- 140 | node : :class`lxml.etree.Element` 141 | An XML node which (optionally) has a child node with the ``origin`` 142 | tag. 143 | 144 | Returns 145 | ------- 146 | matrix : (4,4) float 147 | The 4x4 homogneous transform matrix that corresponds to this node's 148 | ``origin`` child. Defaults to the identity matrix if no ``origin`` 149 | child was found. 150 | """ 151 | matrix = np.eye(4, dtype=np.float64) 152 | origin_node = node.find("origin") 153 | if origin_node is not None: 154 | if "xyz" in origin_node.attrib: 155 | matrix[:3, 3] = np.fromstring(origin_node.attrib["xyz"], sep=" ") 156 | if "rpy" in origin_node.attrib: 157 | rpy = np.fromstring(origin_node.attrib["rpy"], sep=" ") 158 | matrix[:3, :3] = rpy_to_matrix(rpy) 159 | return matrix 160 | 161 | 162 | def unparse_origin(matrix): 163 | """Turn a 4x4 homogenous matrix into an ``origin`` XML node. 164 | 165 | Parameters 166 | ---------- 167 | matrix : (4,4) float 168 | The 4x4 homogneous transform matrix to convert into an ``origin`` 169 | XML node. 170 | 171 | Returns 172 | ------- 173 | node : :class`lxml.etree.Element` 174 | An XML node whose tag is ``origin``. The node has two attributes: 175 | 176 | - ``xyz`` - A string with three space-delimited floats representing 177 | the translation of the origin. 178 | - ``rpy`` - A string with three space-delimited floats representing 179 | the rotation of the origin. 180 | """ 181 | node = ET.Element("origin") 182 | node.attrib["xyz"] = "{} {} {}".format(*matrix[:3, 3]) 183 | node.attrib["rpy"] = "{} {} {}".format(*matrix_to_rpy(matrix[:3, :3])) 184 | return node 185 | 186 | 187 | def get_filename(base_path, file_path, makedirs=False): 188 | """Formats a file path correctly for URDF loading. 189 | 190 | Parameters 191 | ---------- 192 | base_path : str 193 | The base path to the URDF's folder. 194 | file_path : str 195 | The path to the file. 196 | makedirs : bool, optional 197 | If ``True``, the directories leading to the file will be created 198 | if needed. 199 | 200 | Returns 201 | ------- 202 | resolved : str 203 | The resolved filepath -- just the normal ``file_path`` if it was an 204 | absolute path, otherwise that path joined to ``base_path``. 205 | """ 206 | if file_path.startswith("package://"): 207 | file_path = file_path.replace("package://", "") 208 | fn = os.path.join(base_path, file_path) 209 | if not os.path.isfile(fn): 210 | base_path_ = base_path.split("/") 211 | file_path_ = file_path.split("/") 212 | if file_path_[0] in base_path_: 213 | idx = base_path_.index(file_path_[0]) 214 | fn = "/".join(base_path_[:idx] + file_path_) 215 | else: 216 | if not os.path.isabs(file_path): 217 | fn = os.path.join(base_path, file_path) 218 | else: 219 | fn = file_path 220 | 221 | if not os.path.isfile(fn): 222 | raise Exception(f"Asset file not found: {fn}") 223 | 224 | if makedirs: 225 | d, _ = os.path.split(fn) 226 | if not os.path.exists(d): 227 | os.makedirs(d) 228 | return fn 229 | 230 | 231 | def load_meshes(filename): 232 | """Loads triangular meshes from a file. 233 | 234 | Parameters 235 | ---------- 236 | filename : str 237 | Path to the mesh file. 238 | 239 | Returns 240 | ------- 241 | meshes : list of :class:`~trimesh.base.Trimesh` 242 | The meshes loaded from the file. 243 | """ 244 | meshes = trimesh.load(filename) 245 | 246 | # If we got a scene, dump the meshes 247 | if isinstance(meshes, trimesh.Scene): 248 | meshes = list(meshes.dump()) 249 | meshes = [g for g in meshes if isinstance(g, trimesh.Trimesh)] 250 | 251 | if isinstance(meshes, (list, tuple, set)): 252 | meshes = list(meshes) 253 | if len(meshes) == 0: 254 | raise ValueError("At least one mesh must be present in file") 255 | for r in meshes: 256 | if not isinstance(r, trimesh.Trimesh): 257 | raise TypeError("Could not load meshes from file") 258 | elif isinstance(meshes, trimesh.Trimesh): 259 | meshes = [meshes] 260 | else: 261 | raise ValueError("Unable to load mesh from file") 262 | 263 | return meshes 264 | 265 | 266 | def configure_origin(value): 267 | """Convert a value into a 4x4 transform matrix. 268 | 269 | Parameters 270 | ---------- 271 | value : None, (6,) float, or (4,4) float 272 | The value to turn into the matrix. 273 | If (6,), interpreted as xyzrpy coordinates. 274 | 275 | Returns 276 | ------- 277 | matrix : (4,4) float or None 278 | The created matrix. 279 | """ 280 | if value is None: 281 | value = np.eye(4, dtype=np.float64) 282 | elif isinstance(value, (list, tuple, np.ndarray)): 283 | value = np.asanyarray(value, dtype=np.float64) 284 | if value.shape == (6,): 285 | value = xyz_rpy_to_matrix(value) 286 | elif value.shape != (4, 4): 287 | raise ValueError( 288 | "Origin must be specified as a 4x4 " "homogenous transformation matrix" 289 | ) 290 | else: 291 | raise TypeError("Invalid type for origin, expect 4x4 matrix") 292 | return value 293 | -------------------------------------------------------------------------------- /oscbf/examples/dynamic_motion.py: -------------------------------------------------------------------------------- 1 | """Testing the performance of OSCBF during dynamic motions and input constraints 2 | 3 | In general, we will command a rapid motion of the end-effector into the unsafe set, 4 | and observe the controller's behavior under velocity control and torque control. 5 | 6 | The reduced-order (velocity-control) OSCBF has no lower-level understanding of torque 7 | limits, so the full-order (torque-control) OSCBF should perform better in this case. 8 | """ 9 | 10 | import argparse 11 | from functools import partial 12 | 13 | import numpy as np 14 | import jax 15 | import jax.numpy as jnp 16 | from jax.typing import ArrayLike 17 | 18 | 19 | from cbfpy import CBF 20 | from oscbf.core.manipulator import Manipulator, load_panda 21 | from oscbf.core.manipulation_env import FrankaTorqueControlEnv, FrankaVelocityControlEnv 22 | from oscbf.core.oscbf_configs import OSCBFTorqueConfig, OSCBFVelocityConfig 23 | from oscbf.utils.trajectory import SinusoidalTaskTrajectory 24 | from oscbf.core.controllers import ( 25 | PoseTaskTorqueController, 26 | PoseTaskVelocityController, 27 | ) 28 | 29 | DATA_DIR = "oscbf/experiments/data/" 30 | SAVE_DATA = False 31 | PAUSE_FOR_PICTURES = False 32 | RECORD_VIDEO = False 33 | PICTURE_IDXS = [1000, 1250, 1600, 1900, 2200] 34 | 35 | 36 | @jax.tree_util.register_static 37 | class EESafeSetTorqueConfig(OSCBFTorqueConfig): 38 | 39 | def __init__( 40 | self, 41 | robot: Manipulator, 42 | pos_min: ArrayLike, 43 | pos_max: ArrayLike, 44 | compensate_centrifugal_coriolis: bool, 45 | ): 46 | self.pos_min = np.asarray(pos_min) 47 | self.pos_max = np.asarray(pos_max) 48 | super().__init__( 49 | robot, compensate_centrifugal_coriolis=compensate_centrifugal_coriolis 50 | ) 51 | 52 | def h_2(self, z, **kwargs): 53 | q = z[: self.num_joints] 54 | ee_pos = self.robot.ee_position(q) 55 | return jnp.concatenate([self.pos_max - ee_pos, ee_pos - self.pos_min]) 56 | 57 | def alpha(self, h): 58 | return 10.0 * h 59 | 60 | def alpha_2(self, h_2): 61 | return 10.0 * h_2 62 | 63 | 64 | @jax.tree_util.register_static 65 | class EESafeSetVelocityConfig(OSCBFVelocityConfig): 66 | 67 | def __init__(self, robot: Manipulator, pos_min: ArrayLike, pos_max: ArrayLike): 68 | self.pos_min = np.asarray(pos_min) 69 | self.pos_max = np.asarray(pos_max) 70 | super().__init__(robot) 71 | 72 | def h_1(self, z, **kwargs): 73 | q = z[: self.num_joints] 74 | ee_pos = self.robot.ee_position(q) 75 | return jnp.concatenate([self.pos_max - ee_pos, ee_pos - self.pos_min]) 76 | 77 | def alpha(self, h): 78 | return 10.0 * h 79 | 80 | def alpha_2(self, h_2): 81 | return 10.0 * h_2 82 | 83 | 84 | # @partial(jax.jit, static_argnums=(0, 1, 2, 3)) 85 | def compute_torque_control( 86 | robot: Manipulator, 87 | osc_controller: PoseTaskTorqueController, 88 | cbf: CBF, 89 | compensate_centrifugal_coriolis: bool, 90 | z: ArrayLike, 91 | z_ee_des: ArrayLike, 92 | ): 93 | q = z[: robot.num_joints] 94 | qdot = z[robot.num_joints :] 95 | M, M_inv, g, c, J, ee_tmat = robot.torque_control_matrices(q, qdot) 96 | 97 | if not compensate_centrifugal_coriolis: 98 | c = jnp.zeros(robot.num_joints) 99 | 100 | # Set nullspace desired joint position 101 | nullspace_posture_goal = jnp.array( 102 | [ 103 | 0.0, 104 | -jnp.pi / 6, 105 | 0.0, 106 | -3 * jnp.pi / 4, 107 | 0.0, 108 | 5 * jnp.pi / 9, 109 | 0.0, 110 | ] 111 | ) 112 | 113 | # Compute nominal control 114 | u_nom = osc_controller( 115 | q, 116 | qdot, 117 | pos=ee_tmat[:3, 3], 118 | rot=ee_tmat[:3, :3], 119 | des_pos=z_ee_des[:3], 120 | des_rot=jnp.reshape(z_ee_des[3:12], (3, 3)), 121 | des_vel=z_ee_des[12:15], 122 | des_omega=z_ee_des[15:18], 123 | des_accel=jnp.zeros(3), 124 | des_alpha=jnp.zeros(3), 125 | des_q=nullspace_posture_goal, 126 | des_qdot=jnp.zeros(robot.num_joints), 127 | J=J, 128 | M=M, 129 | M_inv=M_inv, 130 | g=g, 131 | c=c, 132 | ) 133 | # Apply the CBF safety filter 134 | return cbf.safety_filter(z, u_nom) 135 | 136 | 137 | # @partial(jax.jit, static_argnums=(0, 1, 2)) 138 | def compute_velocity_control( 139 | robot: Manipulator, 140 | osc_controller: PoseTaskVelocityController, 141 | cbf: CBF, 142 | z: ArrayLike, 143 | z_ee_des: ArrayLike, 144 | ): 145 | q = z[: robot.num_joints] 146 | M_inv, J, ee_tmat = robot.dynamically_consistent_velocity_control_matrices(q) 147 | pos = ee_tmat[:3, 3] 148 | rot = ee_tmat[:3, :3] 149 | des_pos = z_ee_des[:3] 150 | des_rot = jnp.reshape(z_ee_des[3:12], (3, 3)) 151 | des_vel = z_ee_des[12:15] 152 | des_omega = z_ee_des[15:18] 153 | # Set nullspace desired joint position 154 | des_q = jnp.array( 155 | [ 156 | 0.0, 157 | -jnp.pi / 6, 158 | 0.0, 159 | -3 * jnp.pi / 4, 160 | 0.0, 161 | 5 * jnp.pi / 9, 162 | 0.0, 163 | ] 164 | ) 165 | u_nom = osc_controller( 166 | q, pos, rot, des_pos, des_rot, des_vel, des_omega, des_q, J, M_inv 167 | ) 168 | return cbf.safety_filter(q, u_nom) 169 | 170 | 171 | def main(control_method="torque"): 172 | assert control_method in ["torque", "velocity"] 173 | 174 | robot = load_panda() 175 | pos_min = (0.25, -0.25, 0.25) 176 | pos_max = (0.65, 0.25, 0.65) 177 | 178 | # NOTE: This term has a noticeable impact on the performance for this demo. 179 | # It's often neglected due to computational demands and model error 180 | compensate_centrifugal_coriolis = False 181 | 182 | torque_config = EESafeSetTorqueConfig( 183 | robot, 184 | pos_min, 185 | pos_max, 186 | compensate_centrifugal_coriolis=compensate_centrifugal_coriolis, 187 | ) 188 | torque_cbf = CBF.from_config(torque_config) 189 | velocity_config = EESafeSetVelocityConfig(robot, pos_min, pos_max) 190 | velocity_cbf = CBF.from_config(velocity_config) 191 | traj = SinusoidalTaskTrajectory( 192 | init_pos=(0.55, 0, 0.45), 193 | init_rot=np.array( 194 | [ 195 | [1, 0, 0], 196 | [0, -1, 0], 197 | [0, 0, -1], 198 | ] 199 | ), 200 | amplitude=(0.25, 0, 0), 201 | angular_freq=(5, 0, 0), 202 | phase=(0, 0, 0), 203 | ) 204 | timestep = 1 / 1000 205 | bg_color = (1, 1, 1) 206 | if control_method == "torque": 207 | env = FrankaTorqueControlEnv( 208 | torque_config.pos_min, 209 | torque_config.pos_max, 210 | traj=traj, 211 | real_time=True, 212 | bg_color=bg_color, 213 | load_floor=False, 214 | timestep=timestep, 215 | ) 216 | else: 217 | env = FrankaVelocityControlEnv( 218 | velocity_config.pos_min, 219 | velocity_config.pos_max, 220 | traj=traj, 221 | real_time=True, 222 | bg_color=bg_color, 223 | load_floor=False, 224 | timestep=timestep, 225 | ) 226 | 227 | env.client.resetDebugVisualizerCamera( 228 | cameraDistance=1.00, 229 | cameraYaw=12, 230 | cameraPitch=-2.6, 231 | cameraTargetPosition=(0.44, 0.16, 0.28), 232 | ) 233 | 234 | kp_pos = 50.0 235 | kp_rot = 20.0 236 | kd_pos = 20.0 237 | kd_rot = 10.0 238 | kp_joint = 10.0 239 | kd_joint = 5.0 240 | osc_torque_controller = PoseTaskTorqueController( 241 | n_joints=robot.num_joints, 242 | kp_task=np.concatenate([kp_pos * np.ones(3), kp_rot * np.ones(3)]), 243 | kd_task=np.concatenate([kd_pos * np.ones(3), kd_rot * np.ones(3)]), 244 | kp_joint=kp_joint, 245 | kd_joint=kd_joint, 246 | # Note: torque limits will be enforced via the QP. We'll set them to None here 247 | # because we don't want to clip the values before the QP 248 | tau_min=None, 249 | tau_max=None, 250 | ) 251 | 252 | osc_velocity_controller = PoseTaskVelocityController( 253 | n_joints=robot.num_joints, 254 | kp_task=np.array([kp_pos, kp_pos, kp_pos, kp_rot, kp_rot, kp_rot]), 255 | kp_joint=kp_joint, 256 | # Note: velocity limits will be enforced via the QP 257 | qdot_min=None, 258 | qdot_max=None, 259 | ) 260 | 261 | @jax.jit 262 | def compute_torque_control_jit(z, z_ee_des): 263 | return compute_torque_control( 264 | robot, 265 | osc_torque_controller, 266 | torque_cbf, 267 | compensate_centrifugal_coriolis, 268 | z, 269 | z_ee_des, 270 | ) 271 | 272 | @jax.jit 273 | def compute_velocity_control_jit(z, z_ee_des): 274 | return compute_velocity_control( 275 | robot, osc_velocity_controller, velocity_cbf, z, z_ee_des 276 | ) 277 | 278 | if control_method == "torque": 279 | compute_control = compute_torque_control_jit 280 | elif control_method == "velocity": 281 | compute_control = compute_velocity_control_jit 282 | else: 283 | raise ValueError(f"Invalid control method: {control_method}") 284 | 285 | while True: 286 | q_qdot = env.get_joint_state() 287 | z_zdot_ee_des = env.get_desired_ee_state() 288 | tau = compute_control(q_qdot, z_zdot_ee_des) 289 | env.apply_control(tau) 290 | env.step() 291 | 292 | 293 | if __name__ == "__main__": 294 | parser = argparse.ArgumentParser( 295 | description="Run end-effector safe-set containment experiment." 296 | ) 297 | parser.add_argument( 298 | "--control_method", 299 | type=str, 300 | choices=["torque", "velocity"], 301 | default="torque", 302 | help="Control method to use (default: torque)", 303 | ) 304 | args = parser.parse_args() 305 | main(control_method=args.control_method) 306 | -------------------------------------------------------------------------------- /oscbf/examples/cluttered_tabletop.py: -------------------------------------------------------------------------------- 1 | """Testing the performance of OSCBF in highly-constrained settings 2 | 3 | We consider a cluttered tabletop environment with many randomized obstacles, 4 | each represented as a sphere. We then enforce collision avoidance with 5 | all of the obstacles, and all of the collision bodies on the robot 6 | 7 | There are likely "smarter" ways to filter out the collision pairs that are 8 | least likely to cause a collision, but for now, this test just tries to see 9 | how much we can scale up the collision avoidance while retaining real-time 10 | performance. 11 | """ 12 | 13 | import argparse 14 | 15 | import numpy as np 16 | import jax 17 | import jax.numpy as jnp 18 | from jax.typing import ArrayLike 19 | 20 | from cbfpy import CBF 21 | from oscbf.core.manipulator import Manipulator, load_panda 22 | from oscbf.core.manipulation_env import FrankaTorqueControlEnv, FrankaVelocityControlEnv 23 | from oscbf.core.oscbf_configs import OSCBFTorqueConfig, OSCBFVelocityConfig 24 | from oscbf.core.controllers import PoseTaskTorqueController, PoseTaskVelocityController 25 | 26 | 27 | np.random.seed(0) 28 | 29 | 30 | @jax.tree_util.register_static 31 | class CollisionsConfig(OSCBFTorqueConfig): 32 | 33 | def __init__( 34 | self, 35 | robot: Manipulator, 36 | z_min: float, 37 | collision_positions: ArrayLike, 38 | collision_radii: ArrayLike, 39 | ): 40 | self.z_min = z_min 41 | self.collision_positions = np.atleast_2d(collision_positions) 42 | self.collision_radii = np.ravel(collision_radii) 43 | super().__init__(robot) 44 | 45 | def h_2(self, z, **kwargs): 46 | # Extract values 47 | q = z[: self.num_joints] 48 | 49 | # Collision Avoidance 50 | robot_collision_pos_rad = self.robot.link_collision_data(q) 51 | robot_collision_positions = robot_collision_pos_rad[:, :3] 52 | robot_collision_radii = robot_collision_pos_rad[:, 3, None] 53 | center_deltas = ( 54 | robot_collision_positions[:, None, :] - self.collision_positions[None, :, :] 55 | ).reshape(-1, 3) 56 | radii_sums = ( 57 | robot_collision_radii[:, None] + self.collision_radii[None, :] 58 | ).reshape(-1) 59 | h_collision = jnp.linalg.norm(center_deltas, axis=1) - radii_sums 60 | 61 | # Whole body table avoidance 62 | h_table = ( 63 | robot_collision_positions[:, 2] - self.z_min - robot_collision_radii.ravel() 64 | ) 65 | 66 | return jnp.concatenate([h_collision, h_table]) 67 | 68 | def alpha(self, h): 69 | return 10.0 * h 70 | 71 | def alpha_2(self, h_2): 72 | return 10.0 * h_2 73 | 74 | 75 | @jax.tree_util.register_static 76 | class CollisionsVelocityConfig(OSCBFVelocityConfig): 77 | 78 | def __init__( 79 | self, 80 | robot: Manipulator, 81 | z_min: float, 82 | collision_positions: ArrayLike, 83 | collision_radii: ArrayLike, 84 | ): 85 | self.z_min = z_min 86 | self.collision_positions = np.atleast_2d(collision_positions) 87 | self.collision_radii = np.ravel(collision_radii) 88 | super().__init__(robot) 89 | 90 | def h_1(self, z, **kwargs): 91 | # Extract values 92 | q = z[: self.num_joints] 93 | 94 | # Collision Avoidance 95 | robot_collision_pos_rad = self.robot.link_collision_data(q) 96 | robot_collision_positions = robot_collision_pos_rad[:, :3] 97 | robot_collision_radii = robot_collision_pos_rad[:, 3, None] 98 | center_deltas = ( 99 | robot_collision_positions[:, None, :] - self.collision_positions[None, :, :] 100 | ).reshape(-1, 3) 101 | radii_sums = ( 102 | robot_collision_radii[:, None] + self.collision_radii[None, :] 103 | ).reshape(-1) 104 | h_collision = jnp.linalg.norm(center_deltas, axis=1) - radii_sums 105 | 106 | # Whole body table avoidance 107 | h_table = ( 108 | robot_collision_positions[:, 2] - self.z_min - robot_collision_radii.ravel() 109 | ) 110 | 111 | return jnp.concatenate([h_collision, h_table]) 112 | 113 | def alpha(self, h): 114 | return 10.0 * h 115 | 116 | def alpha_2(self, h_2): 117 | return 10.0 * h_2 118 | 119 | 120 | # @partial(jax.jit, static_argnums=(0, 1, 2)) 121 | def compute_torque_control( 122 | robot: Manipulator, 123 | osc_controller: PoseTaskTorqueController, 124 | cbf: CBF, 125 | z: ArrayLike, 126 | z_ee_des: ArrayLike, 127 | ): 128 | q = z[: robot.num_joints] 129 | qdot = z[robot.num_joints :] 130 | M, M_inv, g, c, J, ee_tmat = robot.torque_control_matrices(q, qdot) 131 | # Set nullspace desired joint position 132 | nullspace_posture_goal = jnp.array( 133 | [ 134 | 0.0, 135 | -jnp.pi / 6, 136 | 0.0, 137 | -3 * jnp.pi / 4, 138 | 0.0, 139 | 5 * jnp.pi / 9, 140 | 0.0, 141 | ] 142 | ) 143 | 144 | # Compute nominal control 145 | u_nom = osc_controller( 146 | q, 147 | qdot, 148 | pos=ee_tmat[:3, 3], 149 | rot=ee_tmat[:3, :3], 150 | des_pos=z_ee_des[:3], 151 | des_rot=jnp.reshape(z_ee_des[3:12], (3, 3)), 152 | des_vel=z_ee_des[12:15], 153 | des_omega=z_ee_des[15:18], 154 | des_accel=jnp.zeros(3), 155 | des_alpha=jnp.zeros(3), 156 | des_q=nullspace_posture_goal, 157 | des_qdot=jnp.zeros(robot.num_joints), 158 | J=J, 159 | M=M, 160 | M_inv=M_inv, 161 | g=g, 162 | c=c, 163 | ) 164 | # Apply the CBF safety filter 165 | return cbf.safety_filter(z, u_nom) 166 | 167 | 168 | # @partial(jax.jit, static_argnums=(0, 1, 2)) 169 | def compute_velocity_control( 170 | robot: Manipulator, 171 | osc_controller: PoseTaskVelocityController, 172 | cbf: CBF, 173 | z: ArrayLike, 174 | z_ee_des: ArrayLike, 175 | ): 176 | q = z[: robot.num_joints] 177 | M_inv, J, ee_tmat = robot.dynamically_consistent_velocity_control_matrices(q) 178 | pos = ee_tmat[:3, 3] 179 | rot = ee_tmat[:3, :3] 180 | des_pos = z_ee_des[:3] 181 | des_rot = jnp.reshape(z_ee_des[3:12], (3, 3)) 182 | des_vel = z_ee_des[12:15] 183 | des_omega = z_ee_des[15:18] 184 | # Set nullspace desired joint position 185 | des_q = jnp.array( 186 | [ 187 | 0.0, 188 | -jnp.pi / 6, 189 | 0.0, 190 | -3 * jnp.pi / 4, 191 | 0.0, 192 | 5 * jnp.pi / 9, 193 | 0.0, 194 | ] 195 | ) 196 | u_nom = osc_controller( 197 | q, pos, rot, des_pos, des_rot, des_vel, des_omega, des_q, J, M_inv 198 | ) 199 | return cbf.safety_filter(q, u_nom) 200 | 201 | 202 | def main(control_method="torque", num_bodies=25): 203 | assert control_method in ["torque", "velocity"] 204 | 205 | robot = load_panda() 206 | z_min = 0.1 207 | 208 | max_num_bodies = 50 209 | 210 | # Sample a lot of collision bodies 211 | all_collision_pos = np.random.uniform( 212 | low=[0.2, -0.4, 0.1], high=[0.8, 0.4, 0.3], size=(max_num_bodies, 3) 213 | ) 214 | all_collision_radii = np.random.uniform(low=0.01, high=0.1, size=(max_num_bodies,)) 215 | # Only use a subset of them based on the desired quantity 216 | collision_pos = np.atleast_2d(all_collision_pos[:num_bodies]) 217 | collision_radii = all_collision_radii[:num_bodies] 218 | collision_data = {"positions": collision_pos, "radii": collision_radii} 219 | 220 | torque_config = CollisionsConfig(robot, z_min, collision_pos, collision_radii) 221 | torque_cbf = CBF.from_config(torque_config) 222 | velocity_config = CollisionsVelocityConfig( 223 | robot, z_min, collision_pos, collision_radii 224 | ) 225 | velocity_cbf = CBF.from_config(velocity_config) 226 | 227 | timestep = 1 / 240 # 1 / 1000 228 | bg_color = (1, 1, 1) 229 | if control_method == "torque": 230 | env = FrankaTorqueControlEnv( 231 | real_time=True, 232 | bg_color=bg_color, 233 | load_floor=False, 234 | timestep=timestep, 235 | collision_data=collision_data, 236 | load_table=True, 237 | ) 238 | else: 239 | env = FrankaVelocityControlEnv( 240 | real_time=True, 241 | bg_color=bg_color, 242 | load_floor=False, 243 | timestep=timestep, 244 | collision_data=collision_data, 245 | load_table=True, 246 | ) 247 | 248 | env.client.resetDebugVisualizerCamera( 249 | cameraDistance=1.40, 250 | cameraYaw=104.40, 251 | cameraPitch=-37, 252 | cameraTargetPosition=(0.20, 0.07, -0.09), 253 | ) 254 | 255 | kp_pos = 50.0 256 | kp_rot = 20.0 257 | kd_pos = 20.0 258 | kd_rot = 10.0 259 | kp_joint = 10.0 260 | kd_joint = 5.0 261 | osc_torque_controller = PoseTaskTorqueController( 262 | n_joints=robot.num_joints, 263 | kp_task=np.concatenate([kp_pos * np.ones(3), kp_rot * np.ones(3)]), 264 | kd_task=np.concatenate([kd_pos * np.ones(3), kd_rot * np.ones(3)]), 265 | kp_joint=kp_joint, 266 | kd_joint=kd_joint, 267 | # Note: torque limits will be enforced via the QP. We'll set them to None here 268 | # because we don't want to clip the values before the QP 269 | tau_min=None, 270 | tau_max=None, 271 | ) 272 | 273 | osc_velocity_controller = PoseTaskVelocityController( 274 | n_joints=robot.num_joints, 275 | kp_task=np.array([kp_pos, kp_pos, kp_pos, kp_rot, kp_rot, kp_rot]), 276 | kp_joint=kp_joint, 277 | # Note: velocity limits will be enforced via the QP 278 | qdot_min=None, 279 | qdot_max=None, 280 | ) 281 | 282 | @jax.jit 283 | def compute_torque_control_jit(z, z_ee_des): 284 | return compute_torque_control( 285 | robot, osc_torque_controller, torque_cbf, z, z_ee_des 286 | ) 287 | 288 | @jax.jit 289 | def compute_velocity_control_jit(z, z_ee_des): 290 | return compute_velocity_control( 291 | robot, osc_velocity_controller, velocity_cbf, z, z_ee_des 292 | ) 293 | 294 | if control_method == "torque": 295 | compute_control = compute_torque_control_jit 296 | elif control_method == "velocity": 297 | compute_control = compute_velocity_control_jit 298 | else: 299 | raise ValueError(f"Invalid control method: {control_method}") 300 | 301 | while True: 302 | q_qdot = env.get_joint_state() 303 | z_zdot_ee_des = env.get_desired_ee_state() 304 | tau = compute_control(q_qdot, z_zdot_ee_des) 305 | env.apply_control(tau) 306 | env.step() 307 | 308 | 309 | if __name__ == "__main__": 310 | parser = argparse.ArgumentParser( 311 | description="Run highly-constrained collision avoidance experiment." 312 | ) 313 | parser.add_argument( 314 | "--control_method", 315 | type=str, 316 | choices=["torque", "velocity"], 317 | default="torque", 318 | help="Control method to use (default: torque)", 319 | ) 320 | parser.add_argument( 321 | "--num_bodies", 322 | type=int, 323 | default=25, 324 | help="Number of collision bodies to simulate (default: 25)", 325 | ) 326 | args = parser.parse_args() 327 | main(control_method=args.control_method, num_bodies=args.num_bodies) 328 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/panda.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 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | -------------------------------------------------------------------------------- /test/test_speed.py: -------------------------------------------------------------------------------- 1 | """Speed tests for the CBF solver 2 | 3 | We evaluate the speed of the solver NOT just via the QP solve but via the whole process 4 | (solving for the nominal control input, constructing the QP matrices, and then solving). 5 | This provides a more accurate view of what the controller frequency would actually be if 6 | deployed on the robot. 7 | 8 | These test cases can also be used to check that modifications to the CBF implementation 9 | do not significantly degrade performance 10 | """ 11 | 12 | import unittest 13 | from typing import Callable 14 | import time 15 | import jax 16 | import jax.numpy as jnp 17 | from jax.typing import ArrayLike 18 | import numpy as np 19 | import matplotlib.pyplot as plt 20 | 21 | from cbfpy import CBF, CLFCBF 22 | 23 | from oscbf.core.oscbf_configs import OSCBFTorqueConfig, OSCBFVelocityConfig 24 | from oscbf.core.controllers import PoseTaskTorqueController, PoseTaskVelocityController 25 | from oscbf.core.manipulator import Manipulator, load_panda 26 | from oscbf.utils.general_utils import find_assets_dir 27 | 28 | # Seed RNG for repeatability 29 | np.random.seed(0) 30 | 31 | 32 | # TODO Make this work if there are additional arguments for the barrier function 33 | def eval_speed( 34 | controller_func: Callable, 35 | states: np.ndarray, 36 | des_states: np.ndarray, 37 | verbose: bool = True, 38 | plot: bool = True, 39 | ) -> float: 40 | """Tests the speed of a controller function via evaluation on a set of inputs 41 | 42 | Timing details (average solve time / Hz, distributions of times, etc.) can be printed to the terminal 43 | or visualized in plots, via the `verbose` and `plot` inputs 44 | 45 | Args: 46 | controller_func (Callable): Function to time. This should be the highest-level CBF-based controller 47 | function which includes the nominal controller, QP construction, and QP solve 48 | states (np.ndarray): Set of states to evaluate on, shape (num_evals, state_dim) 49 | des_states (np.ndarray): Set of desired states to evaluate on, shape (num_evals, des_state_dim) 50 | verbose (bool, optional): Whether to print timing details to the terminal. Defaults to True. 51 | plot (bool, optional): Whether to visualize the distribution of solve times. Defaults to True. 52 | 53 | Returns: 54 | float: Average solver Hz 55 | """ 56 | assert isinstance(controller_func, Callable) 57 | assert isinstance(states, np.ndarray) 58 | assert isinstance(des_states, np.ndarray) 59 | assert isinstance(verbose, bool) 60 | assert isinstance(plot, bool) 61 | assert states.shape[0] > 1 62 | assert states.shape[0] == des_states.shape[0] 63 | controller_func: Callable = jax.jit(controller_func) 64 | 65 | # Do an initial solve to jit-compile the function 66 | start_time = time.perf_counter() 67 | u = controller_func(states[0], des_states[0]) 68 | first_solve_time = time.perf_counter() - start_time 69 | 70 | # Solve for the remainder of the controls using the jit-compiled controller 71 | times = [] 72 | for i in range(1, states.shape[0]): 73 | start_time = time.perf_counter() 74 | u = controller_func(states[i], des_states[i]).block_until_ready() 75 | times.append(time.perf_counter() - start_time) 76 | times = np.asarray(times) 77 | avg_solve_time = np.mean(times) 78 | max_solve_time = np.max(times) 79 | avg_hz = 1 / avg_solve_time 80 | worst_case_hz = 1 / max_solve_time 81 | 82 | if verbose: 83 | # Print info about solver stats 84 | print(f"Solved for the first control input in {first_solve_time} seconds") 85 | print(f"Average solve time: {avg_solve_time} seconds") 86 | print(f"Average Hz: {avg_hz}") 87 | print(f"Worst-case solve time: {max_solve_time}") 88 | print(f"Worst-case Hz: {worst_case_hz}") 89 | print( 90 | "NOTE: Worst-case behavior might be inaccurate due to how the OS manages background processes" 91 | ) 92 | 93 | if plot: 94 | # Create a plot to visualize the distribution of times 95 | fig, axs = plt.subplots(2, 2) 96 | axs[0, 0].hist(times, 20) 97 | axs[0, 0].set_title("Solve Times") 98 | axs[0, 0].set_ylabel("Frequency") 99 | axs[0, 0].set_xscale("log") 100 | axs[0, 1].boxplot(times, vert=False) 101 | axs[0, 1].set_title("Solve Times") 102 | axs[0, 1].set_xscale("log") 103 | axs[1, 0].hist(1 / times, 20) 104 | axs[1, 0].set_title("Hz") 105 | axs[1, 0].set_ylabel("Frequency") 106 | axs[1, 1].boxplot(1 / times, vert=False) 107 | axs[1, 1].set_title("Hz") 108 | plt.show() 109 | 110 | return avg_hz 111 | 112 | 113 | @jax.tree_util.register_static 114 | class EESafeSetTorqueConfig(OSCBFTorqueConfig): 115 | 116 | def __init__( 117 | self, 118 | robot: Manipulator, 119 | pos_min: ArrayLike, 120 | pos_max: ArrayLike, 121 | ): 122 | self.pos_min = np.asarray(pos_min) 123 | self.pos_max = np.asarray(pos_max) 124 | self.q_min = robot.joint_lower_limits 125 | self.q_max = robot.joint_upper_limits 126 | super().__init__(robot) 127 | 128 | def h_2(self, z, **kwargs): 129 | q = z[: self.num_joints] 130 | ee_pos = self.robot.ee_position(q) 131 | h_ee_safe_set = jnp.concatenate([self.pos_max - ee_pos, ee_pos - self.pos_min]) 132 | return h_ee_safe_set 133 | 134 | def alpha(self, h): 135 | return 10.0 * h 136 | 137 | def alpha_2(self, h_2): 138 | return 10.0 * h_2 139 | 140 | @jax.tree_util.register_static 141 | class WBSafeSetTorqueConfig(OSCBFTorqueConfig): 142 | 143 | def __init__( 144 | self, 145 | robot: Manipulator, 146 | whole_body_pos_min: ArrayLike, 147 | whole_body_pos_max: ArrayLike, 148 | ): 149 | self.whole_body_pos_min = np.asarray(whole_body_pos_min) 150 | self.whole_body_pos_max = np.asarray(whole_body_pos_max) 151 | super().__init__(robot) 152 | 153 | def h_2(self, z, **kwargs): 154 | # Extract values 155 | q = z[: self.num_joints] 156 | 157 | # Collision Avoidance 158 | robot_collision_pos_rad = self.robot.link_collision_data(q) 159 | robot_collision_positions = robot_collision_pos_rad[:, :3] 160 | robot_collision_radii = robot_collision_pos_rad[:, 3, None] 161 | robot_num_pts = robot_collision_positions.shape[0] 162 | 163 | # Whole-body Set Containment 164 | h_whole_body_upper = ( 165 | jnp.tile(self.whole_body_pos_max, (robot_num_pts, 1)) 166 | - robot_collision_positions 167 | - robot_collision_radii 168 | ).ravel() 169 | h_whole_body_lower = ( 170 | robot_collision_positions 171 | - jnp.tile(self.whole_body_pos_min, (robot_num_pts, 1)) 172 | - robot_collision_radii 173 | ).ravel() 174 | 175 | return jnp.concatenate( 176 | [ 177 | h_whole_body_upper, 178 | h_whole_body_lower, 179 | ] 180 | ) 181 | 182 | def alpha(self, h): 183 | return 10.0 * h 184 | 185 | def alpha_2(self, h_2): 186 | return 10.0 * h_2 187 | 188 | 189 | # TODO clean this up alongside the velocity control test 190 | @jax.tree_util.register_static 191 | class OSCBFTorqueControlTest: 192 | """Test the speed of the manipulator demo, using randomly sampled states""" 193 | 194 | def __init__(self, test_name = "ee_safe_set"): 195 | self.robot = load_panda() 196 | 197 | # Position ranges for sampling end effector states 198 | self.pos_min = jnp.array([0.15, -0.25, 0.25]) 199 | self.pos_max = jnp.array([0.75, 0.25, 0.75]) 200 | 201 | if test_name == "ee_safe_set": 202 | self.config = EESafeSetTorqueConfig(self.robot, self.pos_min, self.pos_max) 203 | elif test_name == "wb_safe_set": 204 | wb_pos_min = np.array([-0.5, -0.5, 0.0]) 205 | wb_pos_max = np.array([0.75, 0.5, 1.0]) 206 | self.config = WBSafeSetTorqueConfig(self.robot, wb_pos_min, wb_pos_max) 207 | else: 208 | raise ValueError("Unrecognized test name: ", test_name) 209 | 210 | self.cbf = CBF.from_config(self.config) 211 | 212 | kp_pos = 50.0 213 | kp_rot = 20.0 214 | kd_pos = 20.0 215 | kd_rot = 10.0 216 | kp_joint = 10.0 217 | kd_joint = 5.0 218 | self.osc_controller = PoseTaskTorqueController( 219 | n_joints=self.robot.num_joints, 220 | kp_task=np.concatenate([kp_pos * np.ones(3), kp_rot * np.ones(3)]), 221 | kd_task=np.concatenate([kd_pos * np.ones(3), kd_rot * np.ones(3)]), 222 | kp_joint=kp_joint, 223 | kd_joint=kd_joint, 224 | # Note: torque limits will be enforced via the QP. We'll set them to None here 225 | # because we don't want to clip the values before the QP 226 | tau_min=None, 227 | tau_max=None, 228 | ) 229 | 230 | def sample_joint_states(self, num_samples: int) -> np.ndarray: 231 | num_joints = self.robot.num_joints 232 | default_joint_pos = np.array( 233 | [0, -np.pi / 3, 0, -5 * np.pi / 6, 0, np.pi / 2, 0] 234 | ) 235 | joint_angles = np.random.multivariate_normal( 236 | default_joint_pos, np.diag(0.1 * np.ones(num_joints)), num_samples 237 | ) 238 | joint_velocities = np.random.multivariate_normal( 239 | np.zeros(num_joints), np.diag(0.1 * np.ones(num_joints)), num_samples 240 | ) 241 | return np.column_stack([joint_angles, joint_velocities]) 242 | 243 | def sample_ee_states(self, num_samples: int) -> np.ndarray: 244 | # Sample positions uniformly inside the keep-in region 245 | positions = np.asarray(self.pos_min) + np.random.rand( 246 | num_samples, 3 247 | ) * np.subtract(self.pos_max, self.pos_min) 248 | # Assume identity rotation for now 249 | # NOTE: switched to flat rotation matrix instead of quaternion 250 | rmats_flat = np.tile(np.eye(3).ravel(), (num_samples, 1)) 251 | # Sample velocities uniformly in [-1, 1] 252 | velocities = -1.0 + 2 * np.random.rand(num_samples, 3) 253 | # Assume no angular velocity for now 254 | omegas = np.zeros((num_samples, 3)) 255 | return np.column_stack([positions, rmats_flat, velocities, omegas]) 256 | 257 | @jax.jit 258 | def controller( 259 | self, 260 | z: ArrayLike, 261 | z_ee_des: ArrayLike, 262 | ): 263 | q = z[: self.robot.num_joints] 264 | qdot = z[self.robot.num_joints :] 265 | M, M_inv, g, c, J, ee_tmat = self.robot.torque_control_matrices(q, qdot) 266 | # Set nullspace desired joint position 267 | nullspace_posture_goal = jnp.array( 268 | [ 269 | 0.0, 270 | -jnp.pi / 6, 271 | 0.0, 272 | -3 * jnp.pi / 4, 273 | 0.0, 274 | 5 * jnp.pi / 9, 275 | 0.0, 276 | ] 277 | ) 278 | 279 | # Compute nominal control 280 | u_nom = self.osc_controller( 281 | q, 282 | qdot, 283 | pos=ee_tmat[:3, 3], 284 | rot=ee_tmat[:3, :3], 285 | des_pos=z_ee_des[:3], 286 | des_rot=jnp.reshape(z_ee_des[3:12], (3, 3)), 287 | des_vel=z_ee_des[12:15], 288 | des_omega=z_ee_des[15:18], 289 | des_accel=jnp.zeros(3), 290 | des_alpha=jnp.zeros(3), 291 | des_q=nullspace_posture_goal, 292 | des_qdot=jnp.zeros(self.robot.num_joints), 293 | J=J, 294 | M=M, 295 | M_inv=M_inv, 296 | g=g, 297 | c=c, 298 | ) 299 | # Apply the CBF safety filter 300 | return self.cbf.safety_filter(z, u_nom) 301 | 302 | def test_speed(self, verbose: bool = True, plot: bool = True): 303 | n_samples = 10000 304 | states = self.sample_joint_states(n_samples) 305 | des_ee_states = self.sample_ee_states(n_samples) 306 | avg_hz = eval_speed(self.controller, states, des_ee_states, verbose, plot) 307 | return avg_hz 308 | 309 | 310 | class SpeedTest(unittest.TestCase): 311 | """Test case to guarantee that the CBFs run at least at kilohertz rates""" 312 | 313 | @classmethod 314 | def setUpClass(cls) -> None: 315 | cls.ee_safe_set_torque_test = OSCBFTorqueControlTest("ee_safe_set") 316 | cls.wb_collision_torque_test = OSCBFTorqueControlTest("wb_safe_set") 317 | 318 | def test_ee_safe_set(self): 319 | avg_hz = self.ee_safe_set_torque_test.test_speed(verbose=False, plot=False) 320 | print("EE safe set average Hz: ", avg_hz) 321 | self.assertTrue(avg_hz >= 1000) 322 | 323 | def test_wb_collisions(self): 324 | avg_hz = self.wb_collision_torque_test.test_speed(verbose=False, plot=False) 325 | print("WB collision avoidance average Hz: ", avg_hz) 326 | self.assertTrue(avg_hz >= 1000) 327 | 328 | 329 | if __name__ == "__main__": 330 | unittest.main() -------------------------------------------------------------------------------- /pylintrc: -------------------------------------------------------------------------------- 1 | # This Pylint rcfile contains a best-effort configuration to uphold the 2 | # best-practices and style described in the Google Python style guide: 3 | # https://google.github.io/styleguide/pyguide.html 4 | # 5 | # Its canonical open-source location is: 6 | # https://google.github.io/styleguide/pylintrc 7 | 8 | [MASTER] 9 | 10 | # Files or directories to be skipped. They should be base names, not paths. 11 | ignore=third_party 12 | 13 | # Files or directories matching the regex patterns are skipped. The regex 14 | # matches against base names, not paths. 15 | ignore-patterns= 16 | 17 | # Pickle collected data for later comparisons. 18 | persistent=no 19 | 20 | # List of plugins (as comma separated values of python modules names) to load, 21 | # usually to register additional checkers. 22 | # load-plugins=pylint_django 23 | # Use multiple processes to speed up Pylint. 24 | jobs=4 25 | 26 | # Allow loading of arbitrary C extensions. Extensions are imported into the 27 | # active Python interpreter and may run arbitrary code. 28 | unsafe-load-any-extension=no 29 | 30 | 31 | [MESSAGES CONTROL] 32 | 33 | # Only show warnings with the listed confidence levels. Leave empty to show 34 | # all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED 35 | confidence= 36 | 37 | # Enable the message, report, category or checker with the given id(s). You can 38 | # either give multiple identifier separated by comma (,) or put this option 39 | # multiple time (only on the command line, not in the configuration file where 40 | # it should appear only once). See also the "--disable" option for examples. 41 | #enable= 42 | 43 | # Disable the message, report, category or checker with the given id(s). You 44 | # can either give multiple identifiers separated by comma (,) or put this 45 | # option multiple times (only on the command line, not in the configuration 46 | # file where it should appear only once).You can also use "--disable=all" to 47 | # disable everything first and then reenable specific checks. For example, if 48 | # you want to run only the similarities checker, you can use "--disable=all 49 | # --enable=similarities". If you want to run only the classes checker, but have 50 | # no Warning level messages displayed, use"--disable=all --enable=classes 51 | # --disable=W" 52 | disable=abstract-method, 53 | apply-builtin, 54 | arguments-differ, 55 | attribute-defined-outside-init, 56 | backtick, 57 | bad-option-value, 58 | bad-mcs-classmethod-argument, 59 | basestring-builtin, 60 | buffer-builtin, 61 | c-extension-no-member, 62 | consider-using-enumerate, 63 | cmp-builtin, 64 | cmp-method, 65 | coerce-builtin, 66 | coerce-method, 67 | delslice-method, 68 | div-method, 69 | duplicate-code, 70 | eq-without-hash, 71 | execfile-builtin, 72 | file-builtin, 73 | filter-builtin-not-iterating, 74 | fixme, 75 | getslice-method, 76 | global-statement, 77 | hex-method, 78 | idiv-method, 79 | implicit-str-concat-in-sequence, 80 | import-error, 81 | import-self, 82 | import-star-module-level, 83 | inconsistent-return-statements, 84 | input-builtin, 85 | intern-builtin, 86 | invalid-str-codec, 87 | locally-disabled, 88 | long-builtin, 89 | long-suffix, 90 | map-builtin-not-iterating, 91 | misplaced-comparison-constant, 92 | missing-function-docstring, 93 | metaclass-assignment, 94 | next-method-called, 95 | next-method-defined, 96 | no-absolute-import, 97 | no-else-break, 98 | no-else-continue, 99 | no-else-raise, 100 | no-else-return, 101 | no-init, # added 102 | no-member, 103 | no-name-in-module, 104 | no-self-use, 105 | nonzero-method, 106 | oct-method, 107 | old-division, 108 | old-ne-operator, 109 | old-octal-literal, 110 | old-raise-syntax, 111 | parameter-unpacking, 112 | print-statement, 113 | raising-string, 114 | range-builtin-not-iterating, 115 | raw_input-builtin, 116 | rdiv-method, 117 | reduce-builtin, 118 | relative-import, 119 | reload-builtin, 120 | round-builtin, 121 | setslice-method, 122 | signature-differs, 123 | standarderror-builtin, 124 | suppressed-message, 125 | sys-max-int, 126 | too-few-public-methods, 127 | too-many-ancestors, 128 | too-many-arguments, 129 | too-many-boolean-expressions, 130 | too-many-branches, 131 | too-many-instance-attributes, 132 | too-many-locals, 133 | too-many-nested-blocks, 134 | too-many-public-methods, 135 | too-many-return-statements, 136 | too-many-statements, 137 | trailing-newlines, 138 | unichr-builtin, 139 | unicode-builtin, 140 | unnecessary-pass, 141 | unpacking-in-except, 142 | useless-else-on-loop, 143 | useless-object-inheritance, 144 | useless-suppression, 145 | using-cmp-argument, 146 | wrong-import-order, 147 | xrange-builtin, 148 | zip-builtin-not-iterating, 149 | 150 | 151 | 152 | [REPORTS] 153 | 154 | # Set the output format. Available formats are text, parseable, colorized, msvs 155 | # (visual studio) and html. You can also give a reporter class, eg 156 | # mypackage.mymodule.MyReporterClass. 157 | output-format=text 158 | 159 | # Put messages in a separate file for each module / package specified on the 160 | # command line instead of printing them on stdout. Reports (if any) will be 161 | # written in a file name "pylint_global.[txt|html]". This option is deprecated 162 | # and it will be removed in Pylint 2.0. 163 | # files-output=no 164 | 165 | # Tells whether to display a full report or only the messages 166 | reports=no 167 | 168 | # Python expression which should return a note less than 10 (10 is the highest 169 | # note). You have access to the variables errors warning, statement which 170 | # respectively contain the number of errors / warnings messages and the total 171 | # number of statements analyzed. This is used by the global evaluation report 172 | # (RP0004). 173 | evaluation=10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10) 174 | 175 | # Template used to display messages. This is a python new-style format string 176 | # used to format the message information. See doc for all details 177 | #msg-template= 178 | 179 | 180 | [BASIC] 181 | 182 | # Good variable names which should always be accepted, separated by a comma 183 | good-names=main,_ 184 | 185 | # Bad variable names which should always be refused, separated by a comma 186 | bad-names= 187 | 188 | # Colon-delimited sets of names that determine each other's naming style when 189 | # the name regexes allow several styles. 190 | name-group= 191 | 192 | # Include a hint for the correct naming format with invalid-name 193 | include-naming-hint=no 194 | 195 | # List of decorators that produce properties, such as abc.abstractproperty. Add 196 | # to this list to register other decorators that produce valid properties. 197 | property-classes=abc.abstractproperty,cached_property.cached_property,cached_property.threaded_cached_property,cached_property.cached_property_with_ttl,cached_property.threaded_cached_property_with_ttl 198 | 199 | # Regular expression matching correct function names 200 | function-rgx=^(?:(?PsetUp|tearDown|setUpModule|tearDownModule)|(?P_?[A-Z][a-zA-Z0-9]*)|(?P_?[a-z][a-z0-9_]*))$ 201 | 202 | # Regular expression matching correct variable names 203 | variable-rgx=^[a-z][a-z0-9_]*$ 204 | 205 | # Regular expression matching correct constant names 206 | const-rgx=^(_?[A-Z][A-Z0-9_]*|__[a-z0-9_]+__|_?[a-z][a-z0-9_]*)$ 207 | 208 | # Regular expression matching correct attribute names 209 | attr-rgx=^_{0,2}[a-z][a-z0-9_]*$ 210 | 211 | # Regular expression matching correct argument names 212 | argument-rgx=^[a-z][a-z0-9_]*$ 213 | 214 | # Regular expression matching correct class attribute names 215 | class-attribute-rgx=^(_?[A-Z][A-Z0-9_]*|__[a-z0-9_]+__|_?[a-z][a-z0-9_]*)$ 216 | 217 | # Regular expression matching correct inline iteration names 218 | inlinevar-rgx=^[a-z][a-z0-9_]*$ 219 | 220 | # Regular expression matching correct class names 221 | class-rgx=^_?[A-Z][a-zA-Z0-9]*$ 222 | 223 | # Regular expression matching correct module names 224 | module-rgx=^(_?[a-z][a-z0-9_]*|__init__)$ 225 | 226 | # Regular expression matching correct method names 227 | method-rgx=(?x)^(?:(?P_[a-z0-9_]+__|runTest|setUp|tearDown|setUpTestCase|tearDownTestCase|setupSelf|tearDownClass|setUpClass|(test|assert)_*[A-Z0-9][a-zA-Z0-9_]*|next)|(?P_{0,2}[A-Z][a-zA-Z0-9_]*)|(?P_{0,2}[a-z][a-z0-9_]*))$ 228 | 229 | # Regular expression which should only match function or class names that do 230 | # not require a docstring. 231 | no-docstring-rgx=(__.*__|main|test.*|.*test|.*Test)$ 232 | 233 | # Minimum line length for functions/classes that require docstrings, shorter 234 | # ones are exempt. 235 | docstring-min-length=10 236 | 237 | 238 | [TYPECHECK] 239 | 240 | # List of decorators that produce context managers, such as 241 | # contextlib.contextmanager. Add to this list to register other decorators that 242 | # produce valid context managers. 243 | contextmanager-decorators=contextlib.contextmanager,contextlib2.contextmanager 244 | 245 | # Tells whether missing members accessed in mixin class should be ignored. A 246 | # mixin class is detected if its name ends with "mixin" (case insensitive). 247 | ignore-mixin-members=yes 248 | 249 | # List of module names for which member attributes should not be checked 250 | # (useful for modules/projects where namespaces are manipulated during runtime 251 | # and thus existing member attributes cannot be deduced by static analysis. It 252 | # supports qualified module names, as well as Unix pattern matching. 253 | ignored-modules= 254 | 255 | # List of class names for which member attributes should not be checked (useful 256 | # for classes with dynamically set attributes). This supports the use of 257 | # qualified names. 258 | ignored-classes=optparse.Values,thread._local,_thread._local 259 | 260 | # List of members which are set dynamically and missed by pylint inference 261 | # system, and so shouldn't trigger E1101 when accessed. Python regular 262 | # expressions are accepted. 263 | generated-members= 264 | 265 | 266 | [FORMAT] 267 | 268 | # Maximum number of characters on a single line. 269 | max-line-length=120 270 | 271 | # TODO(https://github.com/PyCQA/pylint/issues/3352): Direct pylint to exempt 272 | # lines made too long by directives to pytype. 273 | 274 | # Regexp for a line that is allowed to be longer than the limit. 275 | ignore-long-lines=(?x)( 276 | ^\s*(\#\ )??$| 277 | ^\s*(from\s+\S+\s+)?import\s+.+$) 278 | 279 | # Allow the body of an if to be on the same line as the test if there is no 280 | # else. 281 | single-line-if-stmt=yes 282 | 283 | # List of optional constructs for which whitespace checking is disabled. `dict- 284 | # separator` is used to allow tabulation in dicts, etc.: {1 : 1,\n222: 2}. 285 | # `trailing-comma` allows a space between comma and closing bracket: (a, ). 286 | # `empty-line` allows space-only lines. 287 | # no-space-check= 288 | 289 | # Maximum number of lines in a module 290 | max-module-lines=99999 291 | 292 | # String used as indentation unit. The internal Google style guide mandates 2 293 | # spaces. Google's externaly-published style guide says 4, consistent with 294 | # PEP 8. 295 | indent-string=' ' 296 | 297 | # Number of spaces of indent required inside a hanging or continued line. 298 | indent-after-paren=4 299 | 300 | # Expected format of line ending, e.g. empty (any line ending), LF or CRLF. 301 | expected-line-ending-format= 302 | 303 | 304 | [MISCELLANEOUS] 305 | 306 | # List of note tags to take in consideration, separated by a comma. 307 | notes=TODO 308 | 309 | 310 | [STRING] 311 | 312 | # This flag controls whether inconsistent-quotes generates a warning when the 313 | # character used as a quote delimiter is used inconsistently within a module. 314 | check-quote-consistency=yes 315 | 316 | 317 | [VARIABLES] 318 | 319 | # Tells whether we should check for unused import in __init__ files. 320 | init-import=no 321 | 322 | # A regular expression matching the name of dummy variables (i.e. expectedly 323 | # not used). 324 | dummy-variables-rgx=^\*{0,2}(_$|unused_|dummy_) 325 | 326 | # List of additional names supposed to be defined in builtins. Remember that 327 | # you should avoid to define new builtins when possible. 328 | additional-builtins= 329 | 330 | # List of strings which can identify a callback function by name. A callback 331 | # name must start or end with one of those strings. 332 | callbacks=cb_,_cb 333 | 334 | # List of qualified module names which can have objects that can redefine 335 | # builtins. 336 | redefining-builtins-modules=six,six.moves,past.builtins,future.builtins,functools 337 | 338 | 339 | [LOGGING] 340 | 341 | # Logging modules to check that the string format arguments are in logging 342 | # function parameter format 343 | logging-modules=logging,absl.logging,tensorflow.io.logging 344 | 345 | 346 | [SIMILARITIES] 347 | 348 | # Minimum lines number of a similarity. 349 | min-similarity-lines=4 350 | 351 | # Ignore comments when computing similarities. 352 | ignore-comments=yes 353 | 354 | # Ignore docstrings when computing similarities. 355 | ignore-docstrings=yes 356 | 357 | # Ignore imports when computing similarities. 358 | ignore-imports=no 359 | 360 | 361 | [SPELLING] 362 | 363 | # Spelling dictionary name. Available dictionaries: none. To make it working 364 | # install python-enchant package. 365 | spelling-dict= 366 | 367 | # List of comma separated words that should not be checked. 368 | spelling-ignore-words= 369 | 370 | # A path to a file that contains private dictionary; one word per line. 371 | spelling-private-dict-file= 372 | 373 | # Tells whether to store unknown words to indicated private dictionary in 374 | # --spelling-private-dict-file option instead of raising a message. 375 | spelling-store-unknown-words=no 376 | 377 | 378 | [IMPORTS] 379 | 380 | # Deprecated modules which should not be used, separated by a comma 381 | deprecated-modules=regsub, 382 | TERMIOS, 383 | Bastion, 384 | rexec, 385 | sets 386 | 387 | # Create a graph of every (i.e. internal and external) dependencies in the 388 | # given file (report RP0402 must not be disabled) 389 | import-graph= 390 | 391 | # Create a graph of external dependencies in the given file (report RP0402 must 392 | # not be disabled) 393 | ext-import-graph= 394 | 395 | # Create a graph of internal dependencies in the given file (report RP0402 must 396 | # not be disabled) 397 | int-import-graph= 398 | 399 | # Force import order to recognize a module as part of the standard 400 | # compatibility libraries. 401 | known-standard-library= 402 | 403 | # Force import order to recognize a module as part of a third party library. 404 | known-third-party=enchant, absl 405 | 406 | # Analyse import fallback blocks. This can be used to support both Python 2 and 407 | # 3 compatible code, which means that the block might have code that exists 408 | # only in one or another interpreter, leading to false positives when analysed. 409 | analyse-fallback-blocks=no 410 | 411 | 412 | [CLASSES] 413 | 414 | # List of method names used to declare (i.e. assign) instance attributes. 415 | defining-attr-methods=__init__, 416 | __new__, 417 | setUp 418 | 419 | # List of member names, which should be excluded from the protected access 420 | # warning. 421 | exclude-protected=_asdict, 422 | _fields, 423 | _replace, 424 | _source, 425 | _make 426 | 427 | # List of valid names for the first argument in a class method. 428 | valid-classmethod-first-arg=cls, 429 | class_ 430 | 431 | # List of valid names for the first argument in a metaclass class method. 432 | valid-metaclass-classmethod-first-arg=mcs 433 | 434 | 435 | [EXCEPTIONS] 436 | 437 | # Exceptions that will emit a warning when being caught. Defaults to 438 | # "Exception" 439 | overgeneral-exceptions=StandardError, 440 | Exception, 441 | BaseException -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/collision/link0.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.1.187496374) 3 | 4 | mtllib link0.obj.mtl 5 | 6 | # 102 vertex positions 7 | v -0.00122674 -0.0946137 -3.24928e-05 8 | v 0.0710809 0.00844602 0.00133993 9 | v 0.071567 -0.00127396 0.0013494 10 | v -0.00051723 0.0946704 -6.42576e-06 11 | v 0.0706646 -0.0115626 0.00133336 12 | v -0.135131 -0.0658707 0.00751985 13 | v -0.136305 -0.0659457 3.26395e-07 14 | v -0.131625 -0.0687992 -5.27309e-07 15 | v 0.0543353 -0.0469489 0.00104792 16 | v 0.0619449 -0.0359785 0.00118116 17 | v 0.0673593 -0.0245271 0.00127621 18 | v 0.0526517 -0.0163561 0.14 19 | v 0.0548547 0.00447165 0.14 20 | v -0.0549666 0.00345713 0.14 21 | v 0.0391015 0.0387489 0.14 22 | v 0.00777804 -0.0910717 0.00846191 23 | v 0.0395665 -0.0384696 0.140003 24 | v -0.029337 -0.0466451 0.140001 25 | v 0.000349224 -0.0550819 0.14 26 | v 0.0686186 0.0208178 0.00129774 27 | v 0.0635871 0.0329931 0.00120988 28 | v 0.0525783 0.048771 0.00101755 29 | v 0.05859 0.0412413 0.00112268 30 | v 0.0509475 0.0207886 0.14 31 | v -0.0218528 0.0506849 0.14 32 | v -0.0317059 0.0450099 0.14 33 | v 0.0317161 0.0450507 0.14 34 | v -0.0400853 0.0378337 0.14 35 | v -0.0477037 0.0275963 0.14 36 | v 0.00485769 0.0549582 0.14 37 | v 0.0474762 -0.0279259 0.14 38 | v -0.00886536 0.0543586 0.140002 39 | v 0.0276805 -0.0476799 0.14 40 | v -0.054766 -0.0054659 0.14 41 | v -0.0528838 -0.015585 0.14 42 | v -0.122615 0.00563594 0.0611487 43 | v -0.122711 -0.00614431 0.060957 44 | v 0.0143701 -0.0532489 0.139999 45 | v -0.071675 -0.0756205 0.0575204 46 | v -0.111235 -0.0474773 0.0590551 47 | v -0.137824 -0.0624363 0.00755269 48 | v -0.133699 0.0669321 0.00741502 49 | v -0.13583 0.0663151 8.33834e-07 50 | v -0.136293 0.0646171 0.00756638 51 | v -0.131612 0.0688472 7.4968e-07 52 | v 0.0031716 0.0933497 0.00863144 53 | v 0.00408648 0.0939818 0.000170711 54 | v 0.0462474 0.0300326 0.14 55 | v 0.0548768 -0.00456585 0.14 56 | v -0.129159 0.0686836 0.00744486 57 | v -0.0528609 0.0157105 0.14 58 | v 0.0200202 0.0514462 0.139999 59 | v 0.00648893 0.0735192 0.08108 60 | v 0.00826788 -0.0918247 0.00024289 61 | v -0.0464371 -0.0299037 0.14 62 | v -0.140043 0.0607227 -1.6486e-07 63 | v 0.00769447 0.0922715 0.000234291 64 | v -0.00159938 0.0936322 0.00849181 65 | v 0.00734398 0.0914466 0.00831775 66 | v -0.148808 -0.0420547 -7.21858e-08 67 | v -0.149244 -0.0371861 0.00781712 68 | v -0.152578 -0.0248176 1.68373e-07 69 | v 0.00907648 0.0737151 0.0784549 70 | v -0.121673 -0.0147367 0.0614178 71 | v 0.00678171 -0.0735316 0.0809757 72 | v -0.130951 -0.0681401 0.00766805 73 | v -0.0189172 -0.051755 0.14 74 | v -0.00908423 -0.0542971 0.14 75 | v -0.0658985 -0.0765477 0.0589793 76 | v -0.0376956 -0.0401377 0.14 77 | v -0.143472 -0.0548617 -4.38612e-08 78 | v -0.145452 -0.0485961 0.00768247 79 | v -0.1203 -0.0235449 0.0615099 80 | v -0.118609 -0.03105 0.06134 81 | v -0.114761 -0.0419663 0.0601784 82 | v -0.154079 -0.00554168 -3.75503e-07 83 | v -0.152725 -0.0137992 0.00819143 84 | v -0.153166 -0.00203576 0.00835078 85 | v -0.142117 0.0555469 0.00762839 86 | v 0.0535544 0.0128185 0.14 87 | v -0.109983 0.0486112 0.0588796 88 | v 0.00366946 -0.0932492 0.00847566 89 | v 0.00349566 -0.0942157 0.000177738 90 | v 0.00906588 -0.073708 0.0785962 91 | v -0.117004 -0.0366378 0.0608364 92 | v -0.151587 -0.0248333 0.00804774 93 | v -0.15241 0.0258275 -1.09161e-08 94 | v -0.153841 0.0105757 0 95 | v -0.15202 0.0217804 0.00810954 96 | v -0.121149 0.0181378 0.0615703 97 | v -0.113238 0.0447149 0.0596848 98 | v -0.000866581 -0.0937369 0.00817823 99 | v -0.141842 -0.0559592 0.00761703 100 | v -0.149634 0.0355816 0.00786878 101 | v -0.14929 0.0402499 3.01802e-07 102 | v -0.152949 0.0100689 0.00821985 103 | v -0.145377 0.0508924 -1.27878e-07 104 | v -0.11571 0.0395945 0.0607111 105 | v -0.0231112 -0.0759725 0.0689683 106 | v -0.117233 0.035881 0.060957 107 | v -0.146172 0.0467529 0.00770437 108 | v -0.119414 0.0287903 0.0611397 109 | 110 | # 0 UV coordinates 111 | 112 | # 200 vertex normals 113 | vn 0.0190098 -2.30831e-05 -0.999819 114 | vn 0.0188816 6.69136e-05 -0.999822 115 | vn 0.0191344 -0.000120273 -0.999817 116 | vn -0.518479 -0.850405 0.0893954 117 | vn 0.0222843 -0.00331509 -0.999746 118 | vn 0.0205047 -0.00139704 -0.999789 119 | vn 7.25974e-07 -5.81537e-06 1 120 | vn 9.85433e-07 -5.68784e-06 1 121 | vn 0.991829 0.0497189 0.11749 122 | vn 0.692904 -0.711464 0.11706 123 | vn 4.45103e-05 3.61862e-05 1 124 | vn 0.0193277 0.000437566 -0.999813 125 | vn 0.0199677 0.0010368 -0.9998 126 | vn 0.0232843 0.0046319 -0.999718 127 | vn 0.0210964 0.0022106 -0.999775 128 | vn 0.0197075 -0.000616531 -0.999806 129 | vn 6.42989e-05 2.18738e-05 1 130 | vn 0.91755 0.380023 0.116978 131 | vn 3.35708e-05 -1.87847e-05 1 132 | vn -7.63878e-06 1.53928e-05 1 133 | vn -8.50133e-06 1.24836e-05 1 134 | vn 8.62742e-06 2.71648e-06 1 135 | vn 4.31916e-06 1.10982e-05 1 136 | vn 0.000248513 -0.00151077 0.999999 137 | vn -0.000100683 -0.000160507 1 138 | vn 0.000441559 2.26903e-05 1 139 | vn -0.759017 -0.00436697 0.651056 140 | vn -0.758153 -0.0170388 0.651854 141 | vn -4.08147e-05 -6.51499e-05 1 142 | vn 3.63574e-05 2.96612e-05 1 143 | vn -0.488489 -0.714003 0.501575 144 | vn -0.779928 -0.612675 0.127831 145 | vn -0.6563 0.74377 0.126791 146 | vn -0.51319 0.854867 0.0764032 147 | vn 0.143998 0.98555 0.0891981 148 | vn 6.6605e-05 0.00021454 1 149 | vn 0.794096 -0.595678 0.120749 150 | vn 0.815277 -0.566949 0.117862 151 | vn 0.897326 -0.425249 0.118194 152 | vn 0.906239 -0.40539 0.119958 153 | vn 0.848855 0.515505 0.117047 154 | vn 0.992823 0.00242477 0.11957 155 | vn 0.000137166 -1.78568e-05 1 156 | vn -0.256878 0.90791 0.331229 157 | vn -4.88056e-05 2.05051e-05 1 158 | vn 1.50367e-05 -7.30032e-06 1 159 | vn -3.22116e-05 6.85885e-05 1 160 | vn -0.0415945 0.953296 0.29916 161 | vn 0.0339572 -0.0169327 -0.99928 162 | vn -9.01903e-05 -3.23136e-05 1 163 | vn -0.795774 0.599474 0.0858742 164 | vn 0.423964 0.898605 0.112979 165 | vn 0.047941 0.0640793 -0.996793 166 | vn 0.0548988 0.990257 0.12797 167 | vn 0.0494285 0.963916 0.261577 168 | vn 0.0344071 0.0175091 -0.999255 169 | vn 0.691328 0.715168 0.102958 170 | vn 0.691451 0.712649 0.118438 171 | vn -0.973898 -0.213048 0.0783147 172 | vn 7.42399e-05 3.32698e-05 1 173 | vn 0.884937 0.449942 0.120157 174 | vn 0.989315 -0.0869511 0.117026 175 | vn 0.962281 -0.245852 0.116503 176 | vn -0.192564 0.977601 0.0849446 177 | vn -0.191831 0.976828 0.0949088 178 | vn -0.123193 0.952732 0.277713 179 | vn -0.441485 0.766549 0.466362 180 | vn 0.467327 0.85467 0.22615 181 | vn 0.692847 -0.713157 0.106629 182 | vn -0.756883 -0.0565346 0.651101 183 | vn 0.123333 -0.943265 0.308285 184 | vn -0.493923 -0.720807 0.48629 185 | vn -0.477017 -0.8711 0.116787 186 | vn -0.44611 -0.794944 0.411157 187 | vn -6.11448e-05 -0.000331755 1 188 | vn -1.43156e-05 -8.03482e-05 1 189 | vn -0.228986 -0.885719 0.403816 190 | vn -0.608389 -0.519659 0.599848 191 | vn -0.920452 -0.383492 0.0755124 192 | vn -0.736949 -0.180808 0.651318 193 | vn -0.611742 -0.514007 0.601306 194 | vn 0.419302 0.901141 0.110141 195 | vn -0.991437 -0.0387918 0.124694 196 | vn -0.993276 -0.0773118 0.0861707 197 | vn -0.833082 0.535892 0.137092 198 | vn 8.17296e-05 2.2162e-05 1 199 | vn 0.943552 0.308615 0.120276 200 | vn 0.981039 0.152834 0.119183 201 | vn 0.973953 0.194242 0.116982 202 | vn 0.767761 0.629431 0.119836 203 | vn 0.775121 0.620511 0.118969 204 | vn 0.975511 -0.184098 0.120356 205 | vn -0.334118 0.859344 0.38716 206 | vn -0.186878 0.944088 0.271613 207 | vn -0.461627 0.746886 0.478605 208 | vn 0.418676 0.882839 0.21285 209 | vn 0.516842 0.832916 0.197804 210 | vn 0.465813 -0.878241 0.10822 211 | vn 0.444917 -0.890581 0.0944139 212 | vn 0.0505572 -0.0737673 -0.995993 213 | vn 0.458814 -0.864421 0.205587 214 | vn 0.516064 -0.83361 0.196908 215 | vn -0.387155 -0.789577 0.476109 216 | vn -0.244693 -0.878338 0.410666 217 | vn -0.208468 -0.972704 0.101918 218 | vn -0.193464 -0.977355 0.0857284 219 | vn -0.182203 -0.978184 0.0997879 220 | vn -0.519874 -0.667682 0.532852 221 | vn -0.000155222 -6.31553e-05 1 222 | vn -4.55781e-05 -2.14887e-05 1 223 | vn -0.773654 -0.387569 0.501247 224 | vn -0.836842 -0.541097 0.0831258 225 | vn -0.938711 -0.313668 0.142944 226 | vn 0.642112 0.752523 0.146293 227 | vn -0.975003 -0.187197 0.119691 228 | vn -0.98721 -0.103388 0.121358 229 | vn -0.991212 0.0930191 0.0940533 230 | vn -0.745994 0.128207 0.653495 231 | vn -0.357321 0.923722 0.138052 232 | vn -2.44058e-05 1.82076e-05 1 233 | vn -0.562279 0.66248 0.494937 234 | vn -0.609088 0.612822 0.50345 235 | vn 0.183429 0.950262 0.251704 236 | vn 0.21534 0.929691 0.298837 237 | vn 0.229004 0.927667 0.294942 238 | vn 0.604019 -0.779541 0.165763 239 | vn 0.230026 -0.929068 0.289689 240 | vn 0.372231 -0.889715 0.264293 241 | vn -0.171199 -0.978844 0.112047 242 | vn 0.0790207 -0.991596 0.102437 243 | vn -0.774998 -0.384377 0.501629 244 | vn -0.890148 -0.437507 0.127373 245 | vn -0.67661 -0.535887 0.505 246 | vn -0.844574 -0.525041 0.105012 247 | vn -0.695542 -0.313148 0.646653 248 | vn -0.714122 -0.263551 0.648514 249 | vn -0.974069 0.210719 0.082385 250 | vn -0.866667 0.0208868 0.498449 251 | vn -0.993956 0.0146296 0.108802 252 | vn -0.994723 0.0188777 0.100843 253 | vn -0.990466 0.0795982 0.112429 254 | vn -0.875827 0.475205 0.0843008 255 | vn -0.648594 0.437886 0.622561 256 | vn -0.631999 0.470323 0.615934 257 | vn -0.542549 0.633495 0.551656 258 | vn -0.569254 0.593159 0.569309 259 | vn -0.854631 -0.127937 0.503228 260 | vn 0.187376 -0.949866 0.250288 261 | vn -0.0299751 -0.962324 0.270247 262 | vn -0.733474 -0.46004 0.500378 263 | vn -0.819413 -0.278244 0.501142 264 | vn -0.818885 -0.280329 0.500842 265 | vn -0.840131 -0.200724 0.503876 266 | vn -0.865477 -0.0010483 0.500948 267 | vn -0.861767 -0.077237 0.501391 268 | vn -0.734892 0.455474 0.502471 269 | vn -0.789187 0.35685 0.499842 270 | vn -0.899886 0.416094 0.130652 271 | vn -0.934904 0.343772 0.0881805 272 | vn -0.750321 -0.110129 0.651836 273 | vn -0.744296 -0.138475 0.653336 274 | vn -0.0486186 -0.96337 0.263731 275 | vn -0.0569915 -0.952558 0.298973 276 | vn -0.0786935 -0.945844 0.314939 277 | vn 0.0865387 -0.962264 0.257989 278 | vn 0.0988769 -0.988653 0.113084 279 | vn -0.040165 -0.954927 0.294112 280 | vn -0.679455 -0.364641 0.636692 281 | vn -0.676925 -0.535527 0.504959 282 | vn -0.860556 -0.0952615 0.500368 283 | vn -0.866101 -0.0391639 0.498333 284 | vn -0.848661 -0.170347 0.500757 285 | vn -0.976331 0.171097 0.132301 286 | vn -0.784297 0.365988 0.500931 287 | vn -0.743675 0.147478 0.652072 288 | vn -0.75636 0.0667582 0.650741 289 | vn -0.726694 0.470061 0.500957 290 | vn -0.946042 0.295169 0.133715 291 | vn -0.699993 0.303725 0.646344 292 | vn -0.690574 0.326044 0.645603 293 | vn -0.722223 0.238827 0.64912 294 | vn -0.862635 0.0731211 0.500513 295 | vn -0.860436 0.0839585 0.502594 296 | vn -0.823144 0.266065 0.501641 297 | vn -0.850508 0.155797 0.502359 298 | vn -0.850275 0.158766 0.501823 299 | vn -0.824425 0.262885 0.501214 300 | vn -0.000104827 0.000138107 -1 301 | vn -6.4345e-05 4.8788e-05 -1 302 | vn -0.000255153 -5.05889e-05 -1 303 | vn 8.56579e-06 1.35717e-06 -1 304 | vn 3.45225e-06 -5.63502e-06 -1 305 | vn 0.000304041 -4.54974e-06 -1 306 | vn 0.0004587 -0.000166978 -1 307 | vn -4.30629e-05 3.84989e-05 -1 308 | vn -0.000206776 -3.99625e-05 -1 309 | vn -7.87823e-05 -3.27836e-06 -1 310 | vn -0.000409956 0.000110369 -1 311 | vn 6.31049e-05 7.3988e-06 -1 312 | vn -0.0003035 2.77655e-05 -1 313 | 314 | # Mesh '' with 200 faces 315 | g 316 | usemtl DefaultMaterial 317 | f 1//1 2//1 3//1 318 | f 4//2 2//2 1//2 319 | f 3//3 5//3 1//3 320 | f 6//4 7//4 8//4 321 | f 9//5 1//5 10//5 322 | f 1//6 11//6 10//6 323 | f 12//7 13//7 14//7 324 | f 13//7 15//8 14//7 325 | f 2//9 13//9 3//9 326 | f 16//10 9//10 17//10 327 | f 14//11 18//11 19//11 328 | f 20//12 2//12 4//12 329 | f 21//13 20//13 4//13 330 | f 22//14 23//14 4//14 331 | f 23//15 21//15 4//15 332 | f 1//16 5//16 11//16 333 | f 13//17 24//17 15//17 334 | f 20//18 21//18 24//18 335 | f 25//19 26//19 14//19 336 | f 15//20 27//20 14//20 337 | f 28//21 29//21 14//20 338 | f 14//22 30//22 25//22 339 | f 14//22 31//23 12//23 340 | f 32//24 25//24 30//24 341 | f 33//25 17//25 14//25 342 | f 34//26 35//26 14//26 343 | f 36//27 37//27 14//27 344 | f 14//28 37//28 34//28 345 | f 38//29 33//29 14//29 346 | f 19//30 38//30 14//30 347 | f 39//31 18//31 40//31 348 | f 7//32 6//32 41//32 349 | f 42//33 43//33 44//33 350 | f 45//34 43//34 42//34 351 | f 4//35 46//35 47//35 352 | f 17//36 31//36 14//36 353 | f 9//37 31//37 17//37 354 | f 10//38 31//38 9//38 355 | f 11//39 31//39 10//39 356 | f 12//40 31//40 11//40 357 | f 21//41 23//41 48//41 358 | f 13//42 49//42 3//42 359 | f 12//43 49//43 13//43 360 | f 25//44 32//44 50//44 361 | f 14//45 29//45 51//45 362 | f 52//46 30//46 14//46 363 | f 27//47 52//47 14//47 364 | f 53//48 32//48 30//48 365 | f 9//49 54//49 1//49 366 | f 35//50 55//50 14//50 367 | f 56//51 44//51 43//51 368 | f 57//52 47//52 46//52 369 | f 4//53 47//53 57//53 370 | f 58//54 46//54 4//54 371 | f 53//55 46//55 58//55 372 | f 57//56 22//56 4//56 373 | f 57//57 59//57 22//57 374 | f 59//58 15//58 22//58 375 | f 60//59 61//59 62//59 376 | f 24//60 48//60 15//60 377 | f 24//61 21//61 48//61 378 | f 49//62 5//62 3//62 379 | f 5//63 12//63 11//63 380 | f 50//64 4//64 45//64 381 | f 4//65 50//65 58//65 382 | f 32//66 53//66 58//66 383 | f 42//67 26//67 25//67 384 | f 52//68 27//68 63//68 385 | f 9//69 16//69 54//69 386 | f 37//70 64//70 34//70 387 | f 38//71 19//71 65//71 388 | f 40//72 6//72 39//72 389 | f 8//73 66//73 6//73 390 | f 6//74 66//74 39//74 391 | f 18//75 67//75 19//75 392 | f 68//76 19//76 67//76 393 | f 67//77 69//77 68//77 394 | f 40//78 70//78 55//78 395 | f 71//79 72//79 60//79 396 | f 73//80 74//80 35//80 397 | f 55//81 75//81 40//81 398 | f 57//82 46//82 59//82 399 | f 76//83 77//83 78//83 400 | f 62//84 77//84 76//84 401 | f 79//85 44//85 56//85 402 | f 80//86 24//86 13//86 403 | f 24//87 80//87 20//87 404 | f 2//88 80//88 13//88 405 | f 20//89 80//89 2//89 406 | f 15//90 48//90 22//90 407 | f 22//91 48//91 23//91 408 | f 49//92 12//92 5//92 409 | f 25//93 50//93 42//93 410 | f 58//94 50//94 32//94 411 | f 42//95 81//95 26//95 412 | f 59//96 46//96 63//96 413 | f 59//97 63//97 27//97 414 | f 82//98 54//98 16//98 415 | f 82//99 83//99 54//99 416 | f 54//100 83//100 1//100 417 | f 16//101 84//101 82//101 418 | f 33//102 84//102 16//102 419 | f 39//103 67//103 18//103 420 | f 69//104 67//104 39//104 421 | f 8//105 39//105 66//105 422 | f 1//106 39//106 8//106 423 | f 69//107 39//107 1//107 424 | f 40//108 18//108 70//108 425 | f 70//109 18//109 14//109 426 | f 55//110 70//110 14//110 427 | f 72//111 75//111 85//111 428 | f 7//112 41//112 71//112 429 | f 60//113 72//113 61//113 430 | f 59//114 27//114 15//114 431 | f 62//115 61//115 86//115 432 | f 86//116 77//116 62//116 433 | f 87//117 88//117 89//117 434 | f 51//118 90//118 14//118 435 | f 42//119 50//119 45//119 436 | f 14//120 26//120 28//120 437 | f 42//121 44//121 81//121 438 | f 44//122 91//122 81//122 439 | f 53//123 63//123 46//123 440 | f 52//124 53//124 30//124 441 | f 63//125 53//125 52//125 442 | f 33//126 16//126 17//126 443 | f 65//127 84//127 38//127 444 | f 33//128 38//128 84//128 445 | f 69//129 1//129 92//129 446 | f 83//130 92//130 1//130 447 | f 75//131 72//131 93//131 448 | f 93//132 72//132 71//132 449 | f 6//133 40//133 75//133 450 | f 41//134 93//134 71//134 451 | f 85//135 55//135 35//135 452 | f 74//136 85//136 35//136 453 | f 87//137 94//137 95//137 454 | f 36//138 96//138 78//138 455 | f 76//139 96//139 88//139 456 | f 78//140 96//140 76//140 457 | f 88//141 96//141 89//141 458 | f 97//142 79//142 56//142 459 | f 29//143 91//143 98//143 460 | f 28//144 91//144 29//144 461 | f 26//145 81//145 28//145 462 | f 91//146 28//146 81//146 463 | f 73//147 64//147 86//147 464 | f 65//148 82//148 84//148 465 | f 92//149 65//149 99//149 466 | f 93//150 41//150 75//150 467 | f 72//151 85//151 61//151 468 | f 61//152 85//152 74//152 469 | f 61//153 74//153 73//153 470 | f 36//154 78//154 37//154 471 | f 37//155 77//155 64//155 472 | f 98//156 91//156 79//156 473 | f 79//157 100//157 98//157 474 | f 97//158 101//158 79//158 475 | f 95//159 101//159 97//159 476 | f 64//160 73//160 34//160 477 | f 73//161 35//161 34//161 478 | f 69//162 92//162 99//162 479 | f 19//163 69//163 99//163 480 | f 68//164 69//164 19//164 481 | f 92//165 82//165 65//165 482 | f 92//166 83//166 82//166 483 | f 99//167 65//167 19//167 484 | f 85//168 75//168 55//168 485 | f 41//169 6//169 75//169 486 | f 64//170 77//170 86//170 487 | f 77//171 37//171 78//171 488 | f 86//172 61//172 73//172 489 | f 87//173 89//173 94//173 490 | f 79//174 101//174 100//174 491 | f 102//175 90//175 51//175 492 | f 36//176 14//176 90//176 493 | f 91//177 44//177 79//177 494 | f 95//178 94//178 101//178 495 | f 29//179 98//179 51//179 496 | f 100//180 51//180 98//180 497 | f 102//181 51//181 100//181 498 | f 89//182 96//182 36//182 499 | f 36//183 90//183 89//183 500 | f 101//184 102//184 100//184 501 | f 94//185 89//185 102//185 502 | f 90//186 102//186 89//186 503 | f 101//187 94//187 102//187 504 | f 4//188 1//188 87//188 505 | f 4//189 87//189 45//189 506 | f 1//190 8//190 87//190 507 | f 60//191 87//191 71//191 508 | f 56//192 87//191 97//192 509 | f 76//193 87//193 62//193 510 | f 56//194 43//194 87//194 511 | f 45//195 87//195 43//195 512 | f 8//196 7//196 87//196 513 | f 62//197 87//197 60//197 514 | f 97//198 87//198 95//198 515 | f 71//199 87//199 7//199 516 | f 88//200 87//200 76//200 517 | 518 | -------------------------------------------------------------------------------- /oscbf/assets/franka_panda/meshes/collision/hand.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.1.187496374) 3 | 4 | mtllib hand.obj.mtl 5 | 6 | # 102 vertex positions 7 | v 0.0167349 0.0901289 0.0644319 8 | v 0.0171343 -0.0892712 0.0636144 9 | v 0.0315898 -0.000558518 0.00621423 10 | v -0.0170537 -0.0955248 0.0221212 11 | v -0.0175104 -0.0979076 0.0516662 12 | v -0.0315975 -0.0019865 0.00115293 13 | v -0.025981 0.0757697 -0.00508223 14 | v -0.0316359 -0.000578733 0.00597307 15 | v 0.0170332 -0.0968023 0.030322 16 | v 0.0316158 -0.00186086 0.00117971 17 | v 0.0175851 -0.0974071 0.0519846 18 | v 0.0185559 0.0946019 0.0566173 19 | v -0.0168042 -0.0919061 0.00629548 20 | v -0.0168122 -0.0875329 -0.00120645 21 | v 0.0162301 -0.0995375 0.0515841 22 | v -4.10844e-05 -0.10399 0.00569088 23 | v -1.72733e-05 -0.103743 -0.0091993 24 | v -0.00361677 -0.101158 -0.0126103 25 | v -0.00342956 -0.102598 -0.0089643 26 | v -0.0252704 0.0820651 0.00519704 27 | v -0.0280109 0.0417843 -0.0063964 28 | v -0.000112569 0.0928085 0.0659622 29 | v -0.0125667 0.0937125 0.0651685 30 | v -0.014422 0.0912765 0.0656897 31 | v -0.0172547 -0.0907003 0.063082 32 | v -0.0154717 -0.0907063 0.0651901 33 | v -0.0167672 0.0902522 0.0643322 34 | v 0.0169555 -0.0916029 0.00556283 35 | v 0.0176072 -0.0938476 0.0584485 36 | v 0.0125057 -0.100188 0.0568941 37 | v 0.0155509 0.1002 0.0546726 38 | v 0.0192688 0.0984936 0.0449417 39 | v 0.0162184 0.100285 0.0458698 40 | v 0.00191744 -0.0455199 -0.0256594 41 | v 0.0280151 0.0420423 -0.0063496 42 | v 0.00381034 -0.0829662 -0.0244493 43 | v 0.0206709 0.0936918 0.0433083 44 | v 0.0254479 0.0811805 0.0035006 45 | v -0.00363682 -0.0835859 -0.0246063 46 | v -0.0165327 -0.0822363 -0.00511811 47 | v -0.0140171 -0.100091 0.0560269 48 | v -0.0160131 -0.0996286 0.0493014 49 | v -0.00321509 -0.087766 -0.0238844 50 | v -0.0129879 -0.0924958 0.0657119 51 | v -0.000505639 -0.0929453 0.0659531 52 | v 0.0135396 0.0926721 0.0656879 53 | v 0.0134248 -0.091927 0.0657313 54 | v -0.0192292 0.0972606 0.0505884 55 | v 0.0165582 -0.0851322 -0.00387274 56 | v 0.00349477 -0.102716 -0.00802934 57 | v 0.0238692 0.0904014 0.00436824 58 | v 0.0227078 0.0937356 0.01434 59 | v 0.0260343 0.0752976 -0.00503085 60 | v 0.0248167 0.0743775 -0.0071557 61 | v -0.00150015 -0.0458698 -0.0259248 62 | v -0.000833771 0.08026 -0.00732172 63 | v 0.0229591 0.0779014 -0.00739904 64 | v -0.0227611 0.0801744 -0.00690954 65 | v 3.18097e-05 -0.0994514 -0.0171612 66 | v -0.0166321 -0.098554 0.0547339 67 | v 0.00360213 -0.101195 -0.0126276 68 | v -0.00161673 0.0974711 0.0623073 69 | v 0.0157184 -0.0917796 0.0649125 70 | v -0.0184487 0.0920155 0.0583932 71 | v -0.000626994 -0.0879747 -0.0249616 72 | v 0.003076 -0.0879729 -0.0239787 73 | v 0.019802 0.0970111 0.020914 74 | v 0.00994155 0.100035 0.0583408 75 | v 0.0171295 0.0990672 0.0543052 76 | v -0.000314131 0.0946153 0.00757466 77 | v -1.59471e-05 0.0984961 0.0270818 78 | v -0.000337323 0.0865616 -0.00416328 79 | v 0.0223143 0.08825 -0.0019332 80 | v 0.022377 0.0833552 -0.00552611 81 | v -0.021325 0.0909269 0.000979802 82 | v -0.0227328 0.0854345 -0.00412033 83 | v -0.0244953 0.0858923 -0.00183273 84 | v 0.00184279 -0.0840023 -0.0255249 85 | v -0.0246502 0.076668 -0.00693213 86 | v 0.0131387 -0.0940629 0.0646832 87 | v -0.00932387 -0.0999826 0.0584892 88 | v -0.0190668 0.0976971 0.0246275 89 | v -0.00449439 0.100426 0.0581191 90 | v -0.0146231 0.100168 0.0557349 91 | v 0.0158049 -0.0994163 0.0549703 92 | v 0.0213426 0.0943331 0.00894201 93 | v -0.000173406 0.090995 0.000576784 94 | v 0.0214829 0.0917947 0.00268413 95 | v 0.0245152 0.0817026 -0.00505142 96 | v 0.0247294 0.0857401 -0.00123169 97 | v -0.0206576 0.0946679 0.00953798 98 | v 0.00119059 -0.100301 0.0586231 99 | v 0.000469602 -0.0966559 0.0630784 100 | v 0.0153969 0.0929056 0.0645563 101 | v -0.000867829 -0.0825485 -0.0258702 102 | v -0.0235558 0.0911663 0.00460792 103 | v -0.014617 -0.0934417 0.0645 104 | v -0.0230261 0.0928969 0.00987686 105 | v -0.0218221 0.0954417 0.0217564 106 | v -0.0181926 0.0995179 0.0438142 107 | v -0.0160455 0.0932093 0.0636169 108 | v -0.0169948 0.0993629 0.0541148 109 | 110 | # 0 UV coordinates 111 | 112 | # 200 vertex normals 113 | vn 0.96934 0.00103808 0.245721 114 | vn -0.986759 -0.159734 -0.0281361 115 | vn -0.997162 0.0697563 -0.0283202 116 | vn 0.986571 -0.160626 -0.0296212 117 | vn 0.99137 -0.125585 0.0376046 118 | vn 0.972565 0.0101093 0.232413 119 | vn -0.981257 -0.166939 -0.096261 120 | vn -0.991092 -0.129756 0.0299978 121 | vn -0.984842 -0.16507 -0.0532708 122 | vn 0.303856 -0.952595 -0.0153173 123 | vn -0.745916 -0.631674 -0.211181 124 | vn -0.996795 0.0769815 0.0217643 125 | vn -0.992516 0.0633187 -0.104418 126 | vn -0.044276 0.241169 0.969473 127 | vn -0.763533 -0.00240486 0.645764 128 | vn 0.98594 -0.162885 -0.0372967 129 | vn 0.986849 -0.0650596 0.147976 130 | vn 0.991549 -0.115042 0.0599665 131 | vn 0.203398 -0.97883 0.0228453 132 | vn 0.516573 0.854926 0.0474658 133 | vn 0.587131 0.00352045 -0.809484 134 | vn 0.99247 0.0777169 0.094674 135 | vn -0.832137 -0.113798 -0.542768 136 | vn -0.990867 -0.10289 0.0871544 137 | vn -0.24667 -0.969077 0.00656585 138 | vn -0.796752 -0.375025 -0.473859 139 | vn -0.0190316 -2.76701e-05 0.999819 140 | vn -0.0193168 -7.80837e-06 0.999813 141 | vn 0.0200855 -9.11851e-05 0.999798 142 | vn 0.0159035 0.000225346 0.999874 143 | vn -0.991348 0.0773534 0.106043 144 | vn 0.846324 -0.531423 -0.0364002 145 | vn 0.838336 -0.119317 -0.531936 146 | vn 0.713391 -0.685355 -0.146159 147 | vn 0.752915 -0.0507794 -0.656156 148 | vn 0.962439 0.234445 0.136915 149 | vn 0.98673 0.0540529 0.153111 150 | vn 0.984464 0.162241 0.0671482 151 | vn 0.963684 0.258201 0.0681518 152 | vn 0.988283 0.13579 0.0696969 153 | vn 0.992401 0.0632947 -0.105521 154 | vn 0.997132 0.0735636 0.0178061 155 | vn 0.997125 0.0744552 -0.0141125 156 | vn 0.525671 0.0308018 -0.85013 157 | vn 0.0112429 0.145846 -0.989244 158 | vn -0.0191625 0.145985 -0.989101 159 | vn 0.295801 0.090268 -0.950975 160 | vn -0.753521 -0.0512984 -0.655419 161 | vn -0.557064 -0.525003 -0.643468 162 | vn -0.826525 -0.175737 -0.534765 163 | vn -0.815256 -0.315571 -0.485564 164 | vn -0.947514 -0.156066 -0.279037 165 | vn -0.93384 -0.292644 0.205677 166 | vn 0.168928 -0.868066 -0.466825 167 | vn -0.713161 -0.698683 0.0569532 168 | vn -0.304509 -0.95237 -0.0162868 169 | vn 0.0219418 0.621147 0.783387 170 | vn -0.00551132 0.61581 0.787876 171 | vn 0.36612 0.000413018 0.930568 172 | vn 0.336199 1.25396e-05 0.941791 173 | vn -0.969753 -7.34049e-05 0.244089 174 | vn -0.962126 0.000708185 0.272604 175 | vn -0.988229 0.0614231 0.14011 176 | vn -0.360462 -0.538401 -0.761703 177 | vn 0.829279 -0.171621 -0.53183 178 | vn 0.677571 -0.00184298 0.735455 179 | vn 0.496348 0.867083 -0.0424839 180 | vn 0.0755333 0.722595 0.687133 181 | vn 0.904977 0.383975 0.183246 182 | vn 0.0127058 0.980664 -0.195286 183 | vn 0.851811 0.0713165 -0.518972 184 | vn 0.0351954 0.591653 -0.805424 185 | vn -0.538898 0.643215 -0.543933 186 | vn 0.0777842 -0.00363624 -0.996964 187 | vn -0.19873 0.11334 -0.973479 188 | vn -0.793118 -0.605993 -0.0611343 189 | vn 0.283571 -0.958905 -0.00945907 190 | vn -0.178475 -0.865689 -0.467685 191 | vn -0.987868 0.153186 -0.0255232 192 | vn -0.206645 -0.00149425 0.978415 193 | vn -0.501132 0.000514844 0.865371 194 | vn 0.0467749 -0.445101 0.894258 195 | vn -0.0152497 -0.997215 0.0730019 196 | vn -0.0160242 0.980702 -0.194854 197 | vn -0.10472 0.777309 0.620342 198 | vn -0.0162484 0.999109 -0.0389505 199 | vn 0.812943 -0.46373 -0.352249 200 | vn 0.734943 -0.648754 -0.197427 201 | vn 0.355505 -0.889833 -0.28603 202 | vn 0.77975 -0.493229 0.385637 203 | vn 0.264343 -0.962054 0.0676396 204 | vn 0.542077 -0.664141 0.514848 205 | vn 0.470493 -0.711825 0.52148 206 | vn 0.584351 -0.807368 -0.0817933 207 | vn 0.826668 -0.548978 0.123462 208 | vn 0.873315 -0.427986 0.232701 209 | vn 0.0134981 0.821385 0.570215 210 | vn 0.481279 -0.00399798 -0.876558 211 | vn 0.0262961 0.976262 -0.21499 212 | vn 0.035393 0.991525 -0.125004 213 | vn -0.0252692 0.468427 -0.883141 214 | vn 0.029533 0.795427 -0.605329 215 | vn 0.0095104 0.447459 -0.894254 216 | vn 0.0293922 0.327461 -0.944407 217 | vn 0.0628955 0.135567 -0.98877 218 | vn -0.512379 0.0361849 -0.857997 219 | vn -0.82575 0.0709576 -0.559555 220 | vn 0.671124 0.490386 -0.555981 221 | vn -0.583104 0.00463288 -0.812385 222 | vn -0.0162994 0.980639 -0.195146 223 | vn -0.157039 0.79873 0.580834 224 | vn 0.973711 -0.16665 -0.155286 225 | vn 0.0757908 -0.76565 0.638776 226 | vn -0.0242742 0.999674 0.00785851 227 | vn 0.0265568 0.999088 0.0334344 228 | vn 0.763663 0.633795 -0.122975 229 | vn 0.59401 0.799769 0.086723 230 | vn 0.277533 0.743174 0.608825 231 | vn 0.344643 0.752738 0.560898 232 | vn 0.784743 0.463667 0.411327 233 | vn -0.0358679 0.684935 -0.72772 234 | vn 0.932801 0.220325 -0.285201 235 | vn 0.624229 0.728724 -0.281602 236 | vn -0.0116186 0.888023 -0.459652 237 | vn 0.0118944 0.888215 -0.459274 238 | vn 0.12674 0.000708284 -0.991936 239 | vn -0.412606 -0.00846907 -0.91087 240 | vn -0.798626 -0.520306 -0.302452 241 | vn -0.541514 -0.834316 0.103337 242 | vn -0.319295 -0.94743 -0.0206758 243 | vn -0.723656 -0.67023 -0.164661 244 | vn 0.024012 -0.608318 0.79333 245 | vn -0.0315557 -0.776033 0.629902 246 | vn 0.0205919 -0.997403 0.0690183 247 | vn -0.0311286 -0.997047 0.0701972 248 | vn -0.982775 0.184638 -0.00783846 249 | vn 0.216124 -0.540324 -0.813229 250 | vn 0.246085 -0.284389 -0.926588 251 | vn 0.556314 -0.528433 -0.641306 252 | vn 0.792539 -0.37877 -0.477928 253 | vn 0.880372 -0.312129 0.3571 254 | vn 0.325605 -0.451322 0.830837 255 | vn 0.101105 -0.786037 0.609856 256 | vn 0.53596 -0.156588 -0.829594 257 | vn 0.580736 0.650676 0.489251 258 | vn 0.491562 0.198884 0.847828 259 | vn 0.828341 0.380744 0.410957 260 | vn 0.035746 0.926351 -0.374962 261 | vn 0.656777 0.749488 -0.083134 262 | vn 0.720531 0.692906 -0.0267625 263 | vn 0.572813 0.248018 -0.781263 264 | vn 0.990951 0.132224 -0.0230681 265 | vn 0.985773 0.167665 0.011866 266 | vn -0.0153618 0.730516 -0.682723 267 | vn 0.0128537 0.730055 -0.683268 268 | vn -0.0185446 0.449169 -0.893255 269 | vn -0.619205 -0.779582 -0.0940023 270 | vn -0.320427 -0.88655 -0.333701 271 | vn -0.0989435 -0.715546 0.691523 272 | vn -0.0841099 -0.712852 0.696252 273 | vn -0.60803 -0.644894 0.463045 274 | vn -0.423761 -0.343804 0.837989 275 | vn -0.605208 0.788757 -0.107633 276 | vn -0.0174541 0.996656 -0.0798282 277 | vn -0.0193589 0.998627 -0.0486783 278 | vn -0.634294 0.640925 -0.432304 279 | vn 0.0379495 -0.163387 -0.985832 280 | vn 0.538782 0.502971 -0.675821 281 | vn 0.813644 0.191216 -0.549017 282 | vn 0.652479 0.653287 -0.384041 283 | vn 0.717815 0.58535 -0.37697 284 | vn -0.286085 -0.763532 0.578942 285 | vn -0.0374327 -0.618147 0.785171 286 | vn -0.709079 -0.37258 0.598657 287 | vn -0.878489 0.351219 0.323886 288 | vn -0.890816 0.299973 0.341268 289 | vn -0.868133 0.372523 0.327981 290 | vn -0.605163 0.772391 -0.192849 291 | vn -0.984555 0.156402 0.0786764 292 | vn -0.982298 0.176886 0.06166 293 | vn -0.981595 0.187328 0.0371542 294 | vn -0.44589 0.832394 -0.329094 295 | vn -0.363723 0.729882 0.578773 296 | vn -0.575721 0.322294 0.751446 297 | vn 0.442358 0.333164 -0.832659 298 | vn -0.70625 0.351573 -0.614497 299 | vn -0.654528 0.356931 -0.666478 300 | vn -0.838844 0.274101 -0.47033 301 | vn -0.354702 0.933343 0.0552966 302 | vn -0.561743 0.820557 -0.105505 303 | vn -0.851741 0.52214 0.0436597 304 | vn -0.759618 0.642942 0.0980105 305 | vn -0.0360333 0.916716 -0.39791 306 | vn -0.560808 0.66826 0.488798 307 | vn -0.414234 0.480837 0.772791 308 | vn -0.390321 -0.168871 -0.905059 309 | vn -0.752648 -0.506645 0.420514 310 | vn -0.029743 0.995213 -0.0930902 311 | vn -0.346496 -0.193469 -0.917884 312 | vn -0.602128 0.796943 -0.0481963 313 | 314 | # Mesh '' with 200 faces 315 | g 316 | usemtl DefaultMaterial 317 | f 1//1 2//1 3//1 318 | f 4//2 5//2 6//2 319 | f 7//3 6//3 8//3 320 | f 9//4 10//4 11//4 321 | f 10//5 3//5 11//5 322 | f 12//6 1//6 3//6 323 | f 13//7 6//7 14//7 324 | f 8//8 6//8 5//8 325 | f 4//9 6//9 13//9 326 | f 15//10 16//10 17//10 327 | f 18//11 19//11 13//11 328 | f 20//12 7//12 8//12 329 | f 21//13 6//13 7//13 330 | f 22//14 23//14 24//14 331 | f 25//15 26//15 27//15 332 | f 28//16 10//16 9//16 333 | f 2//17 29//17 3//17 334 | f 29//18 11//18 3//18 335 | f 30//19 16//19 15//19 336 | f 31//20 32//20 33//20 337 | f 34//21 35//21 36//21 338 | f 37//22 3//22 38//22 339 | f 6//23 39//23 40//23 340 | f 25//24 8//24 5//24 341 | f 41//25 42//25 16//25 342 | f 18//26 14//26 43//26 343 | f 22//27 24//27 44//27 344 | f 44//28 45//28 22//28 345 | f 46//29 22//29 45//29 346 | f 45//30 47//30 46//30 347 | f 20//31 8//31 48//31 348 | f 9//32 11//32 15//32 349 | f 36//33 10//33 49//33 350 | f 50//34 28//34 9//34 351 | f 10//35 36//35 35//35 352 | f 12//36 37//36 32//36 353 | f 37//37 12//37 3//37 354 | f 37//38 38//38 51//38 355 | f 37//39 52//39 32//39 356 | f 52//40 37//40 51//40 357 | f 10//41 35//41 53//41 358 | f 10//42 53//42 38//42 359 | f 10//43 38//43 3//43 360 | f 54//44 35//44 34//44 361 | f 55//45 56//45 57//45 362 | f 56//46 55//46 58//46 363 | f 57//47 54//47 34//47 364 | f 39//48 6//48 21//48 365 | f 59//49 18//49 43//49 366 | f 40//50 39//50 43//50 367 | f 14//51 40//51 43//51 368 | f 14//52 6//52 40//52 369 | f 60//53 25//53 5//53 370 | f 59//54 61//54 17//54 371 | f 42//55 60//55 5//55 372 | f 16//56 42//56 17//56 373 | f 62//57 22//57 46//57 374 | f 22//58 62//58 23//58 375 | f 1//59 46//59 63//59 376 | f 47//60 63//60 46//60 377 | f 25//61 64//61 8//61 378 | f 64//62 25//62 27//62 379 | f 64//63 48//63 8//63 380 | f 65//64 59//64 43//64 381 | f 66//65 36//65 49//65 382 | f 63//66 2//66 1//66 383 | f 33//67 32//67 67//67 384 | f 68//68 62//68 46//68 385 | f 32//69 69//69 12//69 386 | f 70//70 71//70 67//70 387 | f 53//71 35//71 54//71 388 | f 72//72 73//72 74//72 389 | f 75//73 76//73 77//73 390 | f 78//74 55//74 34//74 391 | f 55//75 79//75 58//75 392 | f 4//76 42//76 5//76 393 | f 50//77 15//77 17//77 394 | f 59//78 17//78 18//78 395 | f 7//79 20//79 77//79 396 | f 24//80 26//80 44//80 397 | f 27//81 26//81 24//81 398 | f 45//82 80//82 47//82 399 | f 41//83 16//83 81//83 400 | f 82//84 71//84 70//84 401 | f 62//85 83//85 23//85 402 | f 33//86 84//86 83//86 403 | f 49//87 28//87 61//87 404 | f 61//88 28//88 50//88 405 | f 61//89 50//89 17//89 406 | f 63//90 85//90 29//90 407 | f 85//91 30//91 15//91 408 | f 80//92 85//92 63//92 409 | f 85//93 80//93 30//93 410 | f 9//94 15//94 50//94 411 | f 85//95 15//95 11//95 412 | f 11//96 29//96 85//96 413 | f 83//97 62//97 68//97 414 | f 34//98 36//98 78//98 415 | f 70//99 67//99 86//99 416 | f 71//100 33//100 67//100 417 | f 58//101 76//101 72//101 418 | f 73//102 87//102 88//102 419 | f 72//103 74//103 56//103 420 | f 56//104 74//104 57//104 421 | f 34//105 55//105 57//105 422 | f 21//106 79//106 55//106 423 | f 21//107 7//107 79//107 424 | f 89//108 73//108 90//108 425 | f 39//109 21//109 55//109 426 | f 91//110 82//110 70//110 427 | f 84//111 23//111 83//111 428 | f 28//112 49//112 10//112 429 | f 92//113 80//113 93//113 430 | f 31//114 33//114 68//114 431 | f 68//115 33//115 83//115 432 | f 51//116 86//116 52//116 433 | f 69//117 32//117 31//117 434 | f 46//118 94//118 68//118 435 | f 94//119 31//119 68//119 436 | f 94//120 12//120 69//120 437 | f 76//121 75//121 72//121 438 | f 90//122 53//122 89//122 439 | f 86//123 51//123 88//123 440 | f 75//124 70//124 87//124 441 | f 87//125 70//125 88//125 442 | f 95//126 55//126 78//126 443 | f 39//127 55//127 95//127 444 | f 14//128 18//128 13//128 445 | f 42//129 41//129 60//129 446 | f 19//130 17//130 42//130 447 | f 19//131 4//131 13//131 448 | f 45//132 93//132 80//132 449 | f 92//133 93//133 81//133 450 | f 16//134 30//134 92//134 451 | f 92//135 81//135 16//135 452 | f 96//136 77//136 20//136 453 | f 66//137 59//137 65//137 454 | f 65//138 78//138 66//138 455 | f 59//139 66//139 61//139 456 | f 49//140 61//140 66//140 457 | f 2//141 63//141 29//141 458 | f 80//142 63//142 47//142 459 | f 80//143 92//143 30//143 460 | f 78//144 36//144 66//144 461 | f 94//145 69//145 31//145 462 | f 94//146 46//146 1//146 463 | f 12//147 94//147 1//147 464 | f 70//148 86//148 88//148 465 | f 67//149 52//149 86//149 466 | f 52//150 67//150 32//150 467 | f 57//151 89//151 54//151 468 | f 53//152 90//152 38//152 469 | f 90//153 51//153 38//153 470 | f 75//154 87//154 72//154 471 | f 72//155 87//155 73//155 472 | f 56//156 58//156 72//156 473 | f 42//157 4//157 19//157 474 | f 19//158 18//158 17//158 475 | f 97//159 81//159 44//159 476 | f 81//160 93//160 44//160 477 | f 41//161 97//161 60//161 478 | f 26//162 97//162 44//162 479 | f 98//163 99//163 91//163 480 | f 100//164 33//164 71//164 481 | f 84//165 33//165 100//165 482 | f 96//166 75//166 77//166 483 | f 95//167 78//167 65//167 484 | f 73//168 89//168 74//168 485 | f 53//169 54//169 89//169 486 | f 73//170 88//170 51//170 487 | f 90//171 73//171 51//171 488 | f 41//172 81//172 97//172 489 | f 44//173 93//173 45//173 490 | f 25//174 97//174 26//174 491 | f 101//175 48//175 64//175 492 | f 64//176 27//176 101//176 493 | f 102//177 48//177 101//177 494 | f 96//178 98//178 91//178 495 | f 48//179 99//179 20//179 496 | f 99//180 98//180 20//180 497 | f 98//181 96//181 20//181 498 | f 96//182 91//182 75//182 499 | f 84//183 101//183 23//183 500 | f 27//184 24//184 101//184 501 | f 89//185 57//185 74//185 502 | f 77//186 76//186 79//186 503 | f 79//187 76//187 58//187 504 | f 79//188 7//188 77//188 505 | f 102//189 84//189 100//189 506 | f 91//190 99//190 82//190 507 | f 48//191 100//191 99//191 508 | f 100//192 48//192 102//192 509 | f 75//193 91//193 70//193 510 | f 84//194 102//194 101//194 511 | f 24//195 23//195 101//195 512 | f 43//196 95//196 65//196 513 | f 25//197 60//197 97//197 514 | f 82//198 100//198 71//198 515 | f 43//199 39//199 95//199 516 | f 100//200 82//200 99//200 517 | 518 | -------------------------------------------------------------------------------- /oscbf/core/manipulation_env.py: -------------------------------------------------------------------------------- 1 | """Simulation environment for manipulator end-effector pose tracking""" 2 | 3 | import time 4 | from typing import Optional, Dict, Tuple 5 | from functools import partial 6 | import argparse 7 | 8 | import jax 9 | from jax import Array 10 | import jax.numpy as jnp 11 | from jax.typing import ArrayLike 12 | import numpy as np 13 | import pybullet 14 | import pybullet_data 15 | from pybullet_utils.bullet_client import BulletClient 16 | 17 | from oscbf.core.manipulator import Manipulator, load_panda 18 | from oscbf.utils.visualization import visualize_3D_box 19 | from oscbf.utils.general_utils import stdout_redirected, find_assets_dir 20 | from oscbf.core.controllers import PoseTaskVelocityController, PoseTaskTorqueController 21 | from oscbf.utils.trajectory import TaskTrajectory 22 | 23 | 24 | class ManipulationEnv: 25 | """Simulation environment for manipulator end-effector pose tracking 26 | 27 | Args: 28 | urdf (str): Path to the URDF file of the robot 29 | control_mode (str): Control mode, either "torque" or "velocity" 30 | xyz_min (Optional[ArrayLike]): Minimum bounds of the safe region, shape (3,). Defaults to None. 31 | xyz_max (Optional[ArrayLike]): Maximum bounds of the safe region, shape (3,). Defaults to None. 32 | target_pos (ArrayLike): Initial position of the target, shape (3,). Defaults to (0.5, 0, 0.5). 33 | q_init (Optional[ArrayLike]): Initial joint positions of the robot, shape (num_joints,). Defaults to None. 34 | traj (Optional[TaskTrajectory]): Task-space trajectory for the target to follow. Defaults to None, in 35 | which case the target's position and orientation can be controlled by the user in the GUI. 36 | collision_data (Optional[Dict[str, Tuple[ArrayLike]]]): Collision information, represented as spheres. 37 | The dictionary should have keys "positions" and "radii". Defaults to None. 38 | wb_xyz_min (Optional[ArrayLike]): Minimum bounds of the whole-body safe region, shape (3,). Defaults to None. 39 | wb_xyz_max (Optional[ArrayLike]): Maximum bounds of the whole-body safe region, shape (3,). Defaults to None. 40 | bg_color (Optional[ArrayLike]): RGB background color of the simulation. Defaults to None 41 | (use default background color) 42 | load_floor (bool): Whether to load a floor into the simulation. Defaults to True. 43 | qdot_max (Optional[ArrayLike]): Maximum joint velocities, shape (num_joints,). Defaults to None. 44 | tau_max (Optional[ArrayLike]): Maximum joint torques, shape (num_joints,). Defaults to None. 45 | real_time (bool): Whether to run the simulation in "real time". Defaults to False. 46 | timestep (float): Simulation timestep. Defaults to 1/240 (Same as PyBullet's default timestep). 47 | load_table (bool): Whether to load a table into the simulation. Defaults to False. 48 | """ 49 | 50 | def __init__( 51 | self, 52 | urdf: str, 53 | control_mode: str, 54 | xyz_min: Optional[ArrayLike] = None, 55 | xyz_max: Optional[ArrayLike] = None, 56 | target_pos: ArrayLike = (0.5, 0, 0.5), 57 | q_init: Optional[ArrayLike] = None, 58 | traj: Optional[TaskTrajectory] = None, 59 | collision_data: Optional[Dict[str, Tuple[ArrayLike]]] = None, 60 | wb_xyz_min: Optional[ArrayLike] = None, 61 | wb_xyz_max: Optional[ArrayLike] = None, 62 | bg_color: Optional[ArrayLike] = None, 63 | load_floor: bool = True, 64 | qdot_max: Optional[ArrayLike] = None, 65 | tau_max: Optional[ArrayLike] = None, 66 | real_time: bool = False, 67 | timestep: float = 1 / 240, 68 | load_table: bool = False, 69 | ): 70 | assert isinstance(urdf, str) 71 | self.urdf = urdf 72 | assert control_mode in ["torque", "velocity"] 73 | self.control_mode = control_mode 74 | assert isinstance(traj, TaskTrajectory) or traj is None 75 | self.traj = traj 76 | with stdout_redirected(): 77 | self.client: pybullet = BulletClient(pybullet.GUI) 78 | assert isinstance(timestep, float) and timestep > 0 79 | self.client.setTimeStep(timestep) 80 | self.client.setAdditionalSearchPath(pybullet_data.getDataPath()) 81 | self.robot = self.client.loadURDF( 82 | urdf, 83 | useFixedBase=True, 84 | flags=self.client.URDF_USE_INERTIA_FROM_FILE 85 | | self.client.URDF_MERGE_FIXED_LINKS, 86 | ) 87 | assert isinstance(load_table, bool) 88 | table_z_offset = -0.35 89 | if load_table: 90 | self.table = pybullet.loadURDF( 91 | "table/table.urdf", 92 | [0.5, 0, table_z_offset], 93 | globalScaling=0.7, 94 | baseOrientation=pybullet.getQuaternionFromEuler((0, 0, np.pi / 2)), 95 | ) 96 | if load_floor: 97 | if load_table: 98 | self.floor = self.client.loadURDF("plane.urdf", [0, 0, table_z_offset]) 99 | else: 100 | self.floor = self.client.loadURDF("plane.urdf") 101 | self.client.configureDebugVisualizer(self.client.COV_ENABLE_GUI, 0) 102 | if bg_color is not None: 103 | assert len(bg_color) == 3 104 | self.client.configureDebugVisualizer(rgbBackground=bg_color) 105 | self.num_joints = self.client.getNumJoints(self.robot) 106 | # "Unlock" the joints 107 | self.client.setJointMotorControlArray( 108 | self.robot, 109 | list(range(self.num_joints)), 110 | pybullet.VELOCITY_CONTROL, 111 | forces=[0.1] * self.num_joints, 112 | ) 113 | self.target = self.client.loadURDF( 114 | find_assets_dir() + "point_robot.urdf", 115 | basePosition=target_pos, 116 | # baseOrientation=self.client.getQuaternionFromEuler([np.pi, 0, 0]), 117 | globalScaling=0.2, 118 | ) 119 | self.target_mass = 1.0 120 | self.client.changeVisualShape(self.target, -1, rgbaColor=[1, 0, 0, 0.6]) 121 | self.client.changeDynamics(self.target, -1, linearDamping=10, angularDamping=30) 122 | 123 | # If this environment is set up for safe-set-invariance, visualize the safe box 124 | if xyz_min is not None and xyz_max is not None: 125 | self.xyz_min = np.asarray(xyz_min) 126 | self.xyz_max = np.asarray(xyz_max) 127 | assert self.xyz_min.shape == self.xyz_max.shape == (3,) 128 | self.box_id = visualize_3D_box( 129 | [self.xyz_min, self.xyz_max], rgba=(0, 1, 0, 0.3) 130 | ) 131 | else: 132 | self.box_id = None 133 | self.xyz_min = None 134 | self.xyz_max = None 135 | 136 | # Slight HACK: Duplicate logic for whole-body safe set 137 | if wb_xyz_min is not None and wb_xyz_max is not None: 138 | self.wb_xyz_min = np.asarray(wb_xyz_min) 139 | self.wb_xyz_max = np.asarray(wb_xyz_max) 140 | assert self.wb_xyz_min.shape == self.wb_xyz_max.shape == (3,) 141 | self.wb_box_id = visualize_3D_box( 142 | [self.wb_xyz_min, self.wb_xyz_max], rgba=(0, 1, 0, 0.3) 143 | ) 144 | else: 145 | self.wb_box_id = None 146 | self.wb_xyz_min = None 147 | self.wb_xyz_max = None 148 | 149 | self.qdot_max = qdot_max 150 | self.tau_max = tau_max 151 | 152 | self.real_time = real_time 153 | self.last_time = time.time() 154 | 155 | # Disable collisions for all links and base of the robot with the target 156 | for i in range(-1, self.num_joints + 1): 157 | self.client.setCollisionFilterPair(self.robot, self.target, i, -1, 0) 158 | disable_robot_floor_collisions = False 159 | disable_target_floor_collisions = True 160 | if load_floor: 161 | if disable_robot_floor_collisions: 162 | for i in range(-1, self.num_joints + 1): 163 | self.client.setCollisionFilterPair(self.robot, self.floor, i, -1, 0) 164 | if disable_target_floor_collisions: 165 | self.client.setCollisionFilterPair(self.floor, self.target, -1, -1, 0) 166 | 167 | # Disable collisions between the target and the table 168 | disable_robot_table_collisions = False 169 | disable_target_table_collisions = True 170 | if load_table: 171 | if disable_target_table_collisions: 172 | self.client.setCollisionFilterPair(self.table, self.target, -1, -1, 0) 173 | if disable_robot_table_collisions: 174 | for i in range(-1, self.num_joints + 1): 175 | self.client.setCollisionFilterPair(self.robot, self.table, i, -1, 0) 176 | # Add a stand for the robot 177 | self.robot_stand_id = visualize_3D_box( 178 | np.asarray([(-0.2, -0.1, -0.35), (0.1, 0.1, 0)]), rgba=(1, 1, 1, 1) 179 | ) 180 | 181 | # Set initial joint positions if provided 182 | if q_init is not None: 183 | assert len(q_init) == self.num_joints 184 | for joint_index in range(len(q_init)): 185 | self.client.resetJointState( 186 | self.robot, joint_index, q_init[joint_index] 187 | ) 188 | 189 | # Handle collision info 190 | if collision_data is not None: 191 | self.collision_positions = collision_data["positions"] 192 | self.collision_radii = collision_data["radii"] 193 | self.collision_ids = [] 194 | assert len(self.collision_positions) == len(self.collision_radii) 195 | for p, r in zip(self.collision_positions, self.collision_radii): 196 | # coll_id = self.client.createCollisionShape( 197 | # self.client.GEOM_SPHERE, 198 | # radius=r, 199 | # collisionFramePosition=p, 200 | # ) 201 | # We don't actually want to create a collision shape, 202 | # because then it would be hard to tell if we are avoiding it 203 | coll_id = -1 204 | vis_id = self.client.createVisualShape( 205 | self.client.GEOM_SPHERE, 206 | radius=r, 207 | visualFramePosition=p, 208 | rgbaColor=[0, 0, 1, 0.5], 209 | ) 210 | body_id = self.client.createMultiBody( 211 | baseMass=0, 212 | baseCollisionShapeIndex=coll_id, 213 | baseVisualShapeIndex=vis_id, 214 | ) 215 | self.collision_ids.append(body_id) 216 | else: 217 | self.collision_positions = None 218 | self.collision_radii = None 219 | self.collision_ids = None 220 | 221 | self.client.setGravity(0, 0, -9.81) 222 | self.dt = self.client.getPhysicsEngineParameters()["fixedTimeStep"] 223 | self.t = 0 224 | 225 | def get_joint_state(self) -> Array: 226 | joint_angles = [] 227 | joint_velocities = [] 228 | joint_states = self.client.getJointStates( 229 | self.robot, list(range(self.num_joints)) 230 | ) 231 | joint_angles = [joint_state[0] for joint_state in joint_states] 232 | joint_velocities = [joint_state[1] for joint_state in joint_states] 233 | return np.array([*joint_angles, *joint_velocities]) 234 | 235 | def get_desired_ee_state(self) -> Array: 236 | # Follow a desired task-space trajectory if provided 237 | if self.traj is not None: 238 | pos = self.traj.position(self.t) 239 | rot = self.traj.rotation(self.t).ravel() 240 | vel = self.traj.velocity(self.t) 241 | omega = self.traj.omega(self.t) 242 | # Update the target's visuals to match the desired state 243 | # HACK: Assume fixed rotation (TODO get a conversion function in here) 244 | # quat = rotation_to_xyzw(rot.reshape(3, 3)) 245 | quat = np.array([0, 0, 0, 1]) 246 | self.client.resetBasePositionAndOrientation(self.target, pos, quat) 247 | self.client.resetBaseVelocity(self.target, vel, omega) 248 | return np.array([*pos, *rot, *vel, *omega]) 249 | # Otherwise, respond to GUI inputs from the user 250 | pos, orn = self.client.getBasePositionAndOrientation(self.target) 251 | vel, omega = self.client.getBaseVelocity(self.target) 252 | 253 | # HACK: reset the angular vel of the target to 0 254 | # Sometimes, the target can start spinning out of control -- this fixes that 255 | self.client.resetBaseVelocity(self.target, vel, [0, 0, 0]) 256 | 257 | # rot = np.ravel(self.client.getMatrixFromQuaternion(orn)) 258 | # Rotate the target so that the Franka EE naturaly faces downwards instead of upwards 259 | # Also, flatten the rotation matrix to a 1D array 260 | rot = np.array( 261 | [ 262 | [1, 0, 0], 263 | [0, -1, 0], 264 | [0, 0, -1], 265 | ] 266 | ).ravel() 267 | # TEMP Ignore angular velocity for now 268 | omega = np.zeros(3) 269 | return np.array([*pos, *rot, *vel, *omega]) 270 | 271 | def apply_control(self, u: Array) -> None: 272 | if self.control_mode == "velocity": 273 | if self.qdot_max is not None: 274 | u = np.clip(u, -self.qdot_max, self.qdot_max) 275 | if self.tau_max is not None: 276 | self.client.setJointMotorControlArray( 277 | self.robot, 278 | list(range(self.num_joints)), 279 | self.client.VELOCITY_CONTROL, 280 | targetVelocities=u, 281 | forces=self.tau_max, 282 | ) 283 | else: 284 | self.client.setJointMotorControlArray( 285 | self.robot, 286 | list(range(self.num_joints)), 287 | self.client.VELOCITY_CONTROL, 288 | targetVelocities=u, 289 | ) 290 | else: # Torque control 291 | if self.tau_max is not None: 292 | u = np.clip(u, -self.tau_max, self.tau_max) 293 | self.client.setJointMotorControlArray( 294 | self.robot, 295 | list(range(self.num_joints)), 296 | self.client.TORQUE_CONTROL, 297 | forces=u, 298 | ) 299 | # Gravity compensation for the target robot so it doesn't fly away 300 | self.client.applyExternalForce( 301 | self.target, 302 | -1, 303 | [0, 0, 9.81 * self.target_mass], 304 | self.client.getBasePositionAndOrientation(self.target)[0], 305 | self.client.WORLD_FRAME, 306 | ) 307 | 308 | def step(self): 309 | self.client.stepSimulation() 310 | self.t += self.dt 311 | if self.real_time: 312 | time.sleep(max(0, self.dt - (time.time() - self.last_time))) 313 | self.last_time = time.time() 314 | 315 | 316 | class FrankaTorqueControlEnv(ManipulationEnv): 317 | """Simulation environment for Franka end-effector pose tracking, with torque control""" 318 | 319 | def __init__( 320 | self, 321 | xyz_min=None, 322 | xyz_max=None, 323 | target_pos=(0.5, 0, 0.5), 324 | q_init=(0, -np.pi / 3, 0, -5 * np.pi / 6, 0, np.pi / 2, 0), 325 | traj=None, 326 | collision_data=None, 327 | wb_xyz_min=None, 328 | wb_xyz_max=None, 329 | bg_color=None, 330 | load_floor=True, 331 | real_time=False, 332 | timestep=1 / 240, 333 | load_table=False, 334 | ): 335 | super().__init__( 336 | "oscbf/assets/franka_panda/panda.urdf", 337 | "torque", 338 | xyz_min, 339 | xyz_max, 340 | target_pos, 341 | q_init, 342 | traj, 343 | collision_data, 344 | wb_xyz_min, 345 | wb_xyz_max, 346 | bg_color, 347 | load_floor, 348 | qdot_max=np.array((2.175, 2.175, 2.175, 2.175, 2.61, 2.61, 2.61)), 349 | tau_max=np.array((87.0, 87.0, 87.0, 87.0, 12.0, 12.0, 12.0)), 350 | real_time=real_time, 351 | timestep=timestep, 352 | load_table=load_table, 353 | ) 354 | 355 | 356 | class FrankaVelocityControlEnv(ManipulationEnv): 357 | """Simulation environment for Franka end-effector pose tracking, with velocity control""" 358 | 359 | def __init__( 360 | self, 361 | xyz_min=None, 362 | xyz_max=None, 363 | target_pos=(0.5, 0, 0.5), 364 | q_init=(0, -np.pi / 3, 0, -5 * np.pi / 6, 0, np.pi / 2, 0), 365 | traj=None, 366 | collision_data=None, 367 | wb_xyz_min=None, 368 | wb_xyz_max=None, 369 | bg_color=None, 370 | load_floor=True, 371 | real_time=False, 372 | timestep=1 / 240, 373 | load_table=False, 374 | ): 375 | super().__init__( 376 | "oscbf/assets/franka_panda/panda.urdf", 377 | "velocity", 378 | xyz_min, 379 | xyz_max, 380 | target_pos, 381 | q_init, 382 | traj, 383 | collision_data, 384 | wb_xyz_min, 385 | wb_xyz_max, 386 | bg_color, 387 | load_floor, 388 | qdot_max=np.array((2.175, 2.175, 2.175, 2.175, 2.61, 2.61, 2.61)), 389 | tau_max=np.array((87.0, 87.0, 87.0, 87.0, 12.0, 12.0, 12.0)), 390 | real_time=real_time, 391 | timestep=timestep, 392 | load_table=load_table, 393 | ) 394 | 395 | 396 | # TODO make this more general -- the des_q only works for a 7DOF robot 397 | # (same thing applies for the nominal torque controller) 398 | @partial(jax.jit, static_argnums=(0, 1)) 399 | def nominal_velocity_controller( 400 | manipulator: Manipulator, controller: PoseTaskVelocityController, q, z_ee_des 401 | ): 402 | M_inv, J, ee_tmat = manipulator.dynamically_consistent_velocity_control_matrices(q) 403 | pos = ee_tmat[:3, 3] 404 | rot = ee_tmat[:3, :3] 405 | des_pos = z_ee_des[:3] 406 | des_rot = jnp.reshape(z_ee_des[3:12], (3, 3)) 407 | des_vel = z_ee_des[12:15] 408 | des_omega = z_ee_des[15:18] 409 | # Set nullspace desired joint position 410 | des_q = jnp.array( 411 | [ 412 | 0.0, 413 | -jnp.pi / 6, 414 | 0.0, 415 | -3 * jnp.pi / 4, 416 | 0.0, 417 | 5 * jnp.pi / 9, 418 | 0.0, 419 | ] 420 | ) 421 | return controller( 422 | q, pos, rot, des_pos, des_rot, des_vel, des_omega, des_q, J, M_inv 423 | ) 424 | 425 | 426 | @partial(jax.jit, static_argnums=(0, 1)) 427 | def nominal_torque_controller( 428 | manipulator: Manipulator, controller: PoseTaskTorqueController, z, z_ee_des 429 | ): 430 | q = z[: manipulator.num_joints] 431 | qdot = z[manipulator.num_joints :] 432 | M, M_inv, g, c, J, ee_tmat = manipulator.torque_control_matrices(q, qdot) 433 | pos = ee_tmat[:3, 3] 434 | rot = ee_tmat[:3, :3] 435 | des_pos = z_ee_des[:3] 436 | des_rot = jnp.reshape(z_ee_des[3:12], (3, 3)) 437 | des_vel = z_ee_des[12:15] 438 | des_omega = z_ee_des[15:18] 439 | des_accel = jnp.zeros(3) 440 | des_alpha = jnp.zeros(3) 441 | # Set nullspace desired joint position 442 | des_q = jnp.array( 443 | [ 444 | 0.0, 445 | -jnp.pi / 6, 446 | 0.0, 447 | -3 * jnp.pi / 4, 448 | 0.0, 449 | 5 * jnp.pi / 9, 450 | 0.0, 451 | ] 452 | ) 453 | des_qdot = jnp.zeros(manipulator.num_joints) 454 | return controller( 455 | q, 456 | qdot, 457 | pos, 458 | rot, 459 | des_pos, 460 | des_rot, 461 | des_vel, 462 | des_omega, 463 | des_accel, 464 | des_alpha, 465 | des_q, 466 | des_qdot, 467 | J, 468 | M, 469 | M_inv, 470 | g, 471 | c, 472 | ) 473 | 474 | 475 | def test_velocity_control(): 476 | env = FrankaVelocityControlEnv() 477 | robot = load_panda() 478 | kp_pos = 5.0 479 | kp_rot = 2.0 480 | kp_task = np.concatenate([kp_pos * np.ones(3), kp_rot * np.ones(3)]) 481 | kp_joint = 1.0 482 | controller = PoseTaskVelocityController( 483 | robot.num_joints, 484 | kp_task, 485 | kp_joint, 486 | -1 * np.asarray(robot.joint_max_velocities), 487 | robot.joint_max_velocities, 488 | ) 489 | 490 | while True: 491 | z = env.get_joint_state() 492 | z_des = env.get_desired_ee_state() 493 | q = z[:7] 494 | u = nominal_velocity_controller(robot, controller, q, z_des) 495 | env.apply_control(u) 496 | env.step() 497 | 498 | 499 | def test_torque_control(): 500 | env = FrankaTorqueControlEnv() 501 | robot = load_panda() 502 | kp_pos = 50.0 503 | kp_rot = 20.0 504 | kd_pos = 20.0 505 | kd_rot = 10.0 506 | controller = PoseTaskTorqueController( 507 | n_joints=robot.num_joints, 508 | kp_task=np.concatenate([kp_pos * np.ones(3), kp_rot * np.ones(3)]), 509 | kd_task=np.concatenate([kd_pos * np.ones(3), kd_rot * np.ones(3)]), 510 | kp_joint=10.0, 511 | kd_joint=5.0, 512 | tau_min=None, 513 | tau_max=None, 514 | ) 515 | 516 | while True: 517 | z = env.get_joint_state() 518 | z_ee_des = env.get_desired_ee_state() 519 | u = nominal_torque_controller(robot, controller, z, z_ee_des) 520 | env.apply_control(u) 521 | env.step() 522 | 523 | 524 | if __name__ == "__main__": 525 | parser = argparse.ArgumentParser( 526 | description="Test velocity or torque control on the manipulator." 527 | ) 528 | parser.add_argument( 529 | "--control", 530 | choices=["velocity", "torque"], 531 | default="velocity", 532 | help="Specify the control mode to test: 'velocity' or 'torque'.", 533 | ) 534 | args = parser.parse_args() 535 | 536 | if args.control == "velocity": 537 | print("Testing velocity control") 538 | test_velocity_control() 539 | elif args.control == "torque": 540 | print("Testing torque control") 541 | test_torque_control() 542 | --------------------------------------------------------------------------------