{"id":19134011,"url":"https://github.com/um-arm-lab/pytorch_kinematics","last_synced_at":"2026-04-16T02:04:53.419Z","repository":{"id":40599664,"uuid":"331721571","full_name":"UM-ARM-Lab/pytorch_kinematics","owner":"UM-ARM-Lab","description":"Robot kinematics implemented in pytorch","archived":false,"fork":false,"pushed_at":"2026-03-07T00:00:46.000Z","size":1413,"stargazers_count":748,"open_issues_count":9,"forks_count":67,"subscribers_count":18,"default_branch":"master","last_synced_at":"2026-03-08T22:42:39.345Z","etag":null,"topics":["differentiable-programming","jacobian","kinematics","pytorch","robotics"],"latest_commit_sha":null,"homepage":"","language":"Python","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"mit","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/UM-ARM-Lab.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE.txt","code_of_conduct":null,"threat_model":null,"audit":null,"citation":"CITATION.cff","codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null,"zenodo":null,"notice":null,"maintainers":null,"copyright":null,"agents":null,"dco":null,"cla":null}},"created_at":"2021-01-21T18:44:10.000Z","updated_at":"2026-03-05T14:59:59.000Z","dependencies_parsed_at":"2023-11-27T21:28:25.264Z","dependency_job_id":"c32f7233-3ee8-47a6-85d8-6a507208edd2","html_url":"https://github.com/UM-ARM-Lab/pytorch_kinematics","commit_stats":{"total_commits":91,"total_committers":4,"mean_commits":22.75,"dds":0.2417582417582418,"last_synced_commit":"eec98b00d8223324fabc6ea72d926a86d761d3bb"},"previous_names":[],"tags_count":8,"template":false,"template_full_name":null,"purl":"pkg:github/UM-ARM-Lab/pytorch_kinematics","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fpytorch_kinematics","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fpytorch_kinematics/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fpytorch_kinematics/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fpytorch_kinematics/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/UM-ARM-Lab","download_url":"https://codeload.github.com/UM-ARM-Lab/pytorch_kinematics/tar.gz/refs/heads/master","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fpytorch_kinematics/sbom","scorecard":null,"host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":286080680,"owners_count":30407769,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2026-03-11T22:36:59.286Z","status":"ssl_error","status_checked_at":"2026-03-11T22:36:57.544Z","response_time":84,"last_error":"SSL_read: unexpected eof while reading","robots_txt_status":"success","robots_txt_updated_at":"2025-07-24T06:49:26.215Z","robots_txt_url":"https://github.com/robots.txt","online":false,"can_crawl_api":true,"host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["differentiable-programming","jacobian","kinematics","pytorch","robotics"],"created_at":"2024-11-09T06:24:45.088Z","updated_at":"2026-04-16T02:04:53.410Z","avatar_url":"https://github.com/UM-ARM-Lab.png","language":"Python","funding_links":[],"categories":[],"sub_categories":[],"readme":"# PyTorch Robot Kinematics\n- Parallel and differentiable forward kinematics (FK), Jacobian calculation, and damped least squares inverse kinematics (IK)\n- Load robot description from URDF, SDF, and MJCF formats \n- SDF queries batched across configurations and points via [pytorch-volumetric](https://github.com/UM-ARM-Lab/pytorch_volumetric)\n\n# Installation\n```shell\npip install pytorch-kinematics\n```\n\nFor development, clone repository somewhere, then `pip3 install -e .` to install in editable mode.\n\n## Reference\n[![DOI](https://zenodo.org/badge/331721571.svg)](https://zenodo.org/badge/latestdoi/331721571)\n\nIf you use this package in your research, consider citing\n```\n@software{Zhong_PyTorch_Kinematics_2024,\nauthor = {Zhong, Sheng and Power, Thomas and Gupta, Ashwin and Mitrano, Peter},\ndoi = {10.5281/zenodo.7700587},\nmonth = feb,\ntitle = {{PyTorch Kinematics}},\nversion = {v0.7.1},\nyear = {2024}\n}\n```\n\n# Usage\n\nSee `tests` for code samples; some are also shown here.\n\n## Loading Robots\n```python\nimport pytorch_kinematics as pk\n\nurdf = \"widowx/wx250s.urdf\"\n# there are multiple natural end effector links so it's not a serial chain\nchain = pk.build_chain_from_urdf(open(urdf, mode=\"rb\").read())\n# visualize the frames (the string is also returned)\nchain.print_tree()\n\"\"\"\nbase_link\n└── shoulder_link\n    └── upper_arm_link\n        └── upper_forearm_link\n            └── lower_forearm_link\n                └── wrist_link\n                    └── gripper_link\n                        └── ee_arm_link\n                            ├── gripper_prop_link\n                            └── gripper_bar_link\n                                └── fingers_link\n                                    ├── left_finger_link\n                                    ├── right_finger_link\n                                    └── ee_gripper_link\n\"\"\"\n\n# extract a specific serial chain such as for inverse kinematics\nserial_chain = pk.SerialChain(chain, \"ee_gripper_link\", \"base_link\")\nserial_chain.print_tree()\n\"\"\"\nbase_link\n└── shoulder_link\n    └── upper_arm_link\n        └── upper_forearm_link\n            └── lower_forearm_link\n                └── wrist_link\n                    └── gripper_link\n                        └── ee_arm_link\n                            └── gripper_bar_link\n                                └── fingers_link\n                                    └── ee_gripper_link\n\"\"\"\n\n# you can also extract a serial chain with a different root than the original chain\nserial_chain = pk.SerialChain(chain, \"ee_gripper_link\", \"gripper_link\")\nserial_chain.print_tree()\n\"\"\"\n gripper_link\n└── ee_arm_link\n    └── gripper_bar_link\n        └── fingers_link\n            └── ee_gripper_link\n\"\"\"\n```\n\n## Forward Kinematics (FK)\n```python\nimport math\nimport pytorch_kinematics as pk\n\n# load robot description from URDF and specify end effector link\nchain = pk.build_serial_chain_from_urdf(open(\"kuka_iiwa.urdf\").read(), \"lbr_iiwa_link_7\")\n# prints out the (nested) tree of links\nprint(chain)\n# prints out list of joint names\nprint(chain.get_joint_parameter_names())\n\n# specify joint values (can do so in many forms)\nth = [0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0]\n# do forward kinematics and get transform objects; end_only=False gives a dictionary of transforms for all links\nret = chain.forward_kinematics(th, end_only=False)\n# look up the transform for a specific link\ntg = ret['lbr_iiwa_link_7']\n# get transform matrix (1,4,4), then convert to separate position and unit quaternion\nm = tg.get_matrix()\npos = m[:, :3, 3]\nrot = pk.matrix_to_quaternion(m[:, :3, :3])\n```\n\nWe can parallelize FK by passing in 2D joint values, and also use CUDA if available\n```python\nimport torch\nimport pytorch_kinematics as pk\n\nd = \"cuda\" if torch.cuda.is_available() else \"cpu\"\ndtype = torch.float64\n\nchain = pk.build_serial_chain_from_urdf(open(\"kuka_iiwa.urdf\").read(), \"lbr_iiwa_link_7\")\nchain = chain.to(dtype=dtype, device=d)\n\nN = 1000\nth_batch = torch.rand(N, len(chain.get_joint_parameter_names()), dtype=dtype, device=d)\n\n# order of magnitudes faster when doing FK in parallel\n# elapsed 0.008678913116455078s for N=1000 when parallel\n# (N,4,4) transform matrix; only the one for the end effector is returned since end_only=True by default\ntg_batch = chain.forward_kinematics(th_batch)\n\n# elapsed 8.44686508178711s for N=1000 when serial\nfor i in range(N):\n    tg = chain.forward_kinematics(th_batch[i])\n```\n\nWe can compute gradients through the FK\n```python\nimport torch\nimport math\nimport pytorch_kinematics as pk\n\nchain = pk.build_serial_chain_from_urdf(open(\"kuka_iiwa.urdf\").read(), \"lbr_iiwa_link_7\")\n\n# require gradient through the input joint values\nth = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0], requires_grad=True)\ntg = chain.forward_kinematics(th)\nm = tg.get_matrix()\npos = m[:, :3, 3]\npos.norm().backward()\n# now th.grad is populated\n```\n\n### Analytical FK Backward\n\n`forward_kinematics_tensor` uses an **analytical geometric Jacobian** for the backward pass by default.\nInstead of replaying all the forward ops through autograd, it computes `d(transform)/d(joint_angles)`\ndirectly from joint axes and the kinematic tree structure. This is ~9x faster than standard autograd\non GPU for large batch sizes.\n\n```python\nimport torch\nimport pytorch_kinematics as pk\n\nchain = pk.build_serial_chain_from_urdf(open(\"kuka_iiwa.urdf\").read(), \"lbr_iiwa_link_7\")\n\nth = torch.randn(1000, 7, requires_grad=True)\nT_all = chain.forward_kinematics_tensor(th)  # (num_frames, 1000, 4, 4)\n\n# backward uses the analytical Jacobian automatically\nloss = T_all.sum()\nloss.backward()  # th.grad is populated, ~9x faster than autograd on GPU\n```\n\nThe analytical backward is compatible with `torch.compile(fullgraph=True)` — all arguments crossing the\nautograd boundary are plain tensors.\n\n**Escape hatch**: set `analytical_grad=False` when you need:\n- **Higher-order gradients** (`create_graph=True` / double backward)\n- **Gradients w.r.t. chain parameters** (e.g. differentiating through link offsets for calibration)\n\n```python\n# Standard autograd — supports create_graph=True and parameter gradients\nT_all = chain.forward_kinematics_tensor(th, analytical_grad=False)\n```\n\nWe can load SDF and MJCF descriptions too, and pass in joint values via a dictionary (unspecified joints get th=0) for non-serial chains\n```python\nimport math\nimport torch\nimport pytorch_kinematics as pk\n\nchain = pk.build_chain_from_sdf(open(\"simple_arm.sdf\").read())\nret = chain.forward_kinematics({'arm_elbow_pan_joint': math.pi / 2.0, 'arm_wrist_lift_joint': -0.5})\n# recall that we specify joint values and get link transforms\ntg = ret['arm_wrist_roll']\n\n# can also do this in parallel\nN = 100\nret = chain.forward_kinematics({'arm_elbow_pan_joint': torch.rand(N, 1), 'arm_wrist_lift_joint': torch.rand(N, 1)})\n# (N, 4, 4) transform object\ntg = ret['arm_wrist_roll']\n\n# building the robot from a MJCF file\nchain = pk.build_chain_from_mjcf(open(\"ant.xml\").read())\nprint(chain)\nprint(chain.get_joint_parameter_names())\nth = {'hip_1': 1.0, 'ankle_1': 1}\nret = chain.forward_kinematics(th)\n\nchain = pk.build_chain_from_mjcf(open(\"humanoid.xml\").read())\nprint(chain)\nprint(chain.get_joint_parameter_names())\nth = {'left_knee': 0.0, 'right_knee': 0.0}\nret = chain.forward_kinematics(th)\n```\n\n## torch.compile Support\n\nThe FK computation can be compiled with `torch.compile(fullgraph=True)` for significant speedups,\nespecially for applications that call FK thousands of times (e.g. inverse kinematics, trajectory optimization).\n\nUse `forward_kinematics_tensor`, a compile-friendly variant that accepts and returns raw tensors\ninstead of dicts and `Transform3d` objects. The analytical FK backward (see above) works under\n`torch.compile` — both forward and backward are fully traced:\n\n```python\nimport torch\nimport pytorch_kinematics as pk\n\nchain = pk.build_serial_chain_from_urdf(open(\"kuka_iiwa.urdf\").read(), \"lbr_iiwa_link_7\")\n\n# compile the FK kernel (one-time cost)\ncompiled_fk = torch.compile(chain.forward_kinematics_tensor, fullgraph=True, dynamic=True)\n\n# input: (B, n_joints) tensor\nth = torch.randn(1000, 7)\n\n# output: (num_frames, B, 4, 4) tensor of homogeneous transforms for all frames\nall_transforms = compiled_fk(th)\n\n# index by frame: use chain.frame_to_idx to look up the index for a frame name\nee_idx = chain.frame_to_idx['lbr_iiwa_link_7']\nee_transform = all_transforms[ee_idx]  # (B, 4, 4)\n```\n\nTypical speedups on CPU (Kuka IIWA 7-DOF):\n\n| Batch size | `forward_kinematics` | Compiled `forward_kinematics_tensor` | Speedup |\n|---:|---:|---:|---:|\n| 1 | 0.21 ms | 0.04 ms | **4.7x** |\n| 64 | 0.26 ms | 0.08 ms | **3.5x** |\n| 1024 | 1.13 ms | 0.51 ms | **2.2x** |\n\nSpeedups are larger for complex robots with more frames (e.g. 6x+ for 49-frame robots at batch=1).\nGPU speedups from CUDA graph capture provide additional gains at large batch sizes.\n\nThe standard `forward_kinematics` method (returning `Dict[str, Transform3d]`) also benefits from the\nrefactored internals without any code changes. It is 2-6x faster at small batch sizes compared to v0.7.\n\nThe rotation conversion functions (`matrix_to_quaternion`, `quaternion_to_axis_angle`,\n`axis_angle_to_quaternion`, etc.) are also compatible with `torch.compile(fullgraph=True)` and can\nbe compiled standalone or as part of a larger compiled graph:\n\n```python\nimport torch\nimport pytorch_kinematics as pk\n\n# compile a rotation conversion function\ncompiled_m2q = torch.compile(pk.matrix_to_quaternion, fullgraph=True)\nR = torch.randn(100, 3, 3)\nq = compiled_m2q(R)\n```\n\n## Jacobian calculation\nThe Jacobian (in the kinematics context) is a matrix describing how the end effector changes with respect to joint value changes\n(where ![dx](https://latex.codecogs.com/png.latex?%5Cinline%20%5Cdot%7Bx%7D) is the twist, or stacked velocity and angular velocity):\n![jacobian](https://latex.codecogs.com/png.latex?%5Cinline%20%5Cdot%7Bx%7D%3DJ%5Cdot%7Bq%7D)\n\nFor `SerialChain` we provide a differentiable and parallelizable method for computing the Jacobian with respect to the base frame.\n```python\nimport math\nimport torch\nimport pytorch_kinematics as pk\n\n# can convert Chain to SerialChain by choosing end effector frame\nchain = pk.build_chain_from_sdf(open(\"simple_arm.sdf\").read())\n# print(chain) to see the available links for use as end effector\n# note that any link can be chosen; it doesn't have to be a link with no children\nchain = pk.SerialChain(chain, \"arm_wrist_roll_frame\")\n\nchain = pk.build_serial_chain_from_urdf(open(\"kuka_iiwa.urdf\").read(), \"lbr_iiwa_link_7\")\nth = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0])\n# (1,6,7) tensor, with 7 corresponding to the DOF of the robot\nJ = chain.jacobian(th)\n\n# get Jacobian in parallel and use CUDA if available\nN = 1000\nd = \"cuda\" if torch.cuda.is_available() else \"cpu\"\ndtype = torch.float64\n\nchain = chain.to(dtype=dtype, device=d)\n# Jacobian calculation is differentiable\nth = torch.rand(N, 7, dtype=dtype, device=d, requires_grad=True)\n# (N,6,7)\nJ = chain.jacobian(th)\n\n# can get Jacobian at a point offset from the end effector (location is specified in EE link frame)\n# by default location is at the origin of the EE frame\nloc = torch.rand(N, 3, dtype=dtype, device=d)\nJ = chain.jacobian(th, locations=loc)\n```\n\nLike FK, the Jacobian has a `torch.compile`-compatible variant `jacobian_tensor`:\n\n```python\nchain = pk.build_serial_chain_from_urdf(open(\"kuka_iiwa.urdf\").read(), \"lbr_iiwa_link_7\")\n\n# compile the Jacobian kernel (one-time cost)\ncompiled_jac = torch.compile(chain.jacobian_tensor, fullgraph=True)\n\n# input: (B, n_joints) tensor; output: (B, 6, n_joints) Jacobian\nth = torch.randn(100, 7)\nJ = compiled_jac(th)\n```\n\nFor maximum performance in tight loops (e.g. IK), pass `mode='max-autotune'` to `torch.compile`\nfor additional kernel tuning at the cost of longer compilation.\n\nTypical speedups on CPU (Kuka IIWA 7-DOF, vs the old v0.7 `calc_jacobian`):\n\n| Batch size | Old `calc_jacobian` | New `jacobian` (eager) | Compiled `jacobian_tensor` | Speedup (compiled vs old) |\n|---:|---:|---:|---:|---:|\n| 1 | 0.63 ms | 0.12 ms | 0.03 ms | **21x** |\n| 10 | 0.79 ms | 0.15 ms | 0.05 ms | **15x** |\n| 100 | 0.75 ms | 0.30 ms | 0.09 ms | **8x** |\n| 1,000 | 1.67 ms | 1.23 ms | 0.53 ms | **3x** |\n| 10,000 | 7.93 ms | 7.01 ms | 4.91 ms | **1.6x** |\n| 100,000 | 59.42 ms | 64.58 ms | 46.46 ms | **1.3x** |\n\nAt small-to-medium batch sizes (the typical IK regime), the new eager implementation is 2-5x faster\ndue to vectorized computation replacing the per-frame Python loop.\n`torch.compile` provides an additional 2-4x on top of that, and is faster than the old implementation\nat every batch size.\n\nThe Jacobian can be used to do inverse kinematics. See [IK survey](https://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf)\nfor a survey of ways to do so. Note that IK may be better performed through other means (but doing it through the Jacobian can give an end-to-end differentiable method).\n\n## Inverse Kinematics (IK)\nInverse kinematics is available via damped least squares (iterative steps with Jacobian pseudo-inverse damped to avoid oscillation near singularlities). \nCompared to other IK libraries, these are the typical advantages over them:\n- not ROS dependent (many IK libraries need the robot description on the ROS parameter server)\n- batched in both goal specification and retries from different starting configurations\n- goal orientation in addition to goal position\n\n![IK](https://i.imgur.com/QgaUME9.gif)\n\nSee `tests/test_inverse_kinematics.py` for usage, but generally what you need is below:\n```python\nfull_urdf = os.path.join(search_path, urdf)\nchain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), \"lbr_iiwa_link_7\")\n\n# goals are specified as Transform3d poses in the **robot frame**\n# so if you have the goals specified in the world frame, you also need the robot frame in the world frame\npos = torch.tensor([0.0, 0.0, 0.0], device=device)\nrot = torch.tensor([0.0, 0.0, 0.0], device=device)\nrob_tf = pk.Transform3d(pos=pos, rot=rot, device=device)\n\n# specify goals as Transform3d poses in world frame\ngoal_in_world_frame_tf = ...\n# convert to robot frame (skip if you have it specified in robot frame already, or if world = robot frame)\ngoal_in_rob_frame_tf = rob_tf.inverse().compose(goal_tf)\n\n# get robot joint limits\nlim = torch.tensor(chain.get_joint_limits(), device=device)\n\n# create the IK object\n# see the constructor for more options and their explanations, such as convergence tolerances\nik = pk.PseudoInverseIK(chain, max_iterations=30, num_retries=10,\n                        joint_limits=lim.T,\n                        early_stopping_any_converged=True,\n                        early_stopping_no_improvement=\"all\",\n                        debug=False,\n                        lr=0.2)\n# solve IK\nsol = ik.solve(goal_in_rob_frame_tf)\n# num goals x num retries x DOF tensor of joint angles; if not converged, best solution found so far\nprint(sol.solutions)\n# num goals x num retries can check for the convergence of each run\nprint(sol.converged)\n# num goals x num retries can look at errors directly\nprint(sol.err_pos)\nprint(sol.err_rot)\n```\n\n### Compiled IK\n\nFor workloads that solve IK repeatedly (e.g. in a planning loop), pass `use_compile=True` to compile the\nFK, Jacobian, and damped least squares kernels inside the IK loop with `torch.compile`.\nThe outer IK loop with its data-dependent convergence checks remains in eager mode.\n\n```python\nik = pk.PseudoInverseIK(chain, max_iterations=30, num_retries=10,\n                         joint_limits=lim.T,\n                         early_stopping_any_converged=True,\n                         early_stopping_no_improvement=\"all\",\n                         lr=0.2,\n                         use_compile=True)  # compile inner kernels\n\n# first solve() incurs a one-time compilation cost; subsequent calls are faster\nsol = ik.solve(goal_in_rob_frame_tf)\n```\n\nTypical speedups on CPU (Kuka IIWA 7-DOF, 30 iterations):\n\n| Retries | Eager (v0.7) | Eager (new) | Compiled | Speedup (compiled vs v0.7) |\n|---:|---:|---:|---:|---:|\n| 10 | 25.6 ms | 23.3 ms | 11.8 ms | **2.2x** |\n| 50 | 31.5 ms | 29.0 ms | 15.8 ms | **2.0x** |\n\nNote: `use_compile=True` requires PyTorch 2.0+. You cannot wrap `ik.solve()` directly with\n`torch.compile` due to data-dependent control flow in the outer loop — use this flag instead.\n\n## SDF Queries\nSee [pytorch-volumetric](https://github.com/UM-ARM-Lab/pytorch_volumetric) for the latest details, some instructions are pasted here:\n\nFor many applications such as collision checking, it is useful to have the\nSDF of a multi-link robot in certain configurations.\nFirst, we create the robot model (loaded from URDF, SDF, MJCF, ...) with\n[pytorch kinematics](https://github.com/UM-ARM-Lab/pytorch_kinematics).\nFor example, we will be using the KUKA 7 DOF arm model from pybullet data\n\n```python\nimport os\nimport torch\nimport pybullet_data\nimport pytorch_kinematics as pk\nimport pytorch_volumetric as pv\n\nurdf = \"kuka_iiwa/model.urdf\"\nsearch_path = pybullet_data.getDataPath()\nfull_urdf = os.path.join(search_path, urdf)\nchain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), \"lbr_iiwa_link_7\")\nd = \"cuda\" if torch.cuda.is_available() else \"cpu\"\n\nchain = chain.to(device=d)\n# paths to the link meshes are specified with their relative path inside the URDF\n# we need to give them the path prefix as we need their absolute path to load\ns = pv.RobotSDF(chain, path_prefix=os.path.join(search_path, \"kuka_iiwa\"))\n```\n\nBy default, each link will have a `MeshSDF`. To instead use `CachedSDF` for faster queries\n\n```python\ns = pv.RobotSDF(chain, path_prefix=os.path.join(search_path, \"kuka_iiwa\"),\n                link_sdf_cls=pv.cache_link_sdf_factory(resolution=0.02, padding=1.0, device=d))\n```\n\nWhich when the `y=0.02` SDF slice is visualized:\n\n![sdf slice](https://i.imgur.com/Putw72A.png)\n\nWith surface points corresponding to:\n\n![wireframe](https://i.imgur.com/L3atG9h.png)\n![solid](https://i.imgur.com/XiAks7a.png)\n\nQueries on this SDF is dependent on the joint configurations (by default all zero).\n**Queries are batched across configurations and query points**. For example, we have a batch of\njoint configurations to query\n\n```python\nth = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0], device=d)\nN = 200\nth_perturbation = torch.randn(N - 1, 7, device=d) * 0.1\n# N x 7 joint values\nth = torch.cat((th.view(1, -1), th_perturbation + th))\n```\n\nAnd also a batch of points to query (same points for each configuration):\n\n```python\ny = 0.02\nquery_range = np.array([\n    [-1, 0.5],\n    [y, y],\n    [-0.2, 0.8],\n])\n# M x 3 points\ncoords, pts = pv.get_coordinates_and_points_in_grid(0.01, query_range, device=s.device)\n```\n\nWe set the batch of joint configurations and query:\n\n```python\ns.set_joint_configuration(th)\n# N x M SDF value\n# N x M x 3 SDF gradient\nsdf_val, sdf_grad = s(pts)\n```\n\n\n# Credits\n- `pytorch_kinematics/transforms` is extracted from [pytorch3d](https://github.com/facebookresearch/pytorch3d) with minor extensions.\nThis was done instead of including `pytorch3d` as a dependency because it is hard to install and most of its code is unrelated.\n  An important difference is that we use left hand multiplied transforms as is convention in robotics (T * pt) instead of their\n  right hand multiplied transforms.\n- `pytorch_kinematics/urdf_parser_py`, and `pytorch_kinematics/mjcf_parser` is extracted from [kinpy](https://github.com/neka-nat/kinpy), as well as the FK logic.\nThis repository ports the logic to pytorch, parallelizes it, and provides some extensions.\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fum-arm-lab%2Fpytorch_kinematics","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fum-arm-lab%2Fpytorch_kinematics","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fum-arm-lab%2Fpytorch_kinematics/lists"}