Robotics Algorithms#
The purpose of this notebook is to demonstrate how kdflex’s various algorithms can be used. This notebook uses the Barret WAM arm to showcase these algorithms.
Resources
Acknowledgements The Barret WAM urdf is sourced from the public repository barrett-ros2-pkg. Thank you to the authors and contributors.
Import kdFlex Python modules objects for use later
import atexit
import numpy as np
from math import pi
from pathlib import Path
import Karana.Core as kc
import Karana.Frame as kf
import Karana.Dynamics as kd
import Karana.KUtils as ku
from Karana.KUtils import BasicPrefab
import Karana.Dynamics.SOADyn_types as kdt
Build the filepath to the URDF file we will import
resources = Path().absolute().parent / "resources"
urdf_file = resources / "wam7dof" / "wam7dof_hand.urdf"
Import the URDF file to create a SOADyn_types.MultibodyDS. The “DS” is short for “Data Structure”. This is kdFlex’s native, file-format agnostic way of specifying a multibody. We’ll use this later on to instantiate the multibody.
sg_ds = ku.BasicPrefab.BasicPrefabDS.fromFile(urdf_file).params.subtree
frame_container = kf.FrameContainer("root")
sg_ds2 = kdt.SubGraphDS(name=sg_ds.name, base_bodies=sg_ds.base_bodies)
multibody = sg_ds2.toMultibody(frame_container)
# print(multibody_info)
multibody.ensureHealthy()
multibody.resetData()
multibody.dumpTree()
LEGEND: [<hinge type[/dofs]> <prescribed subhinges>] <body name>[/num embedded bodies > 0] <flex dofs > 0>
|-wam7dof_MBVROOT_
|-[REVOLUTE] FirstLink
|-[REVOLUTE] SecondLink
|-[REVOLUTE] ThirdLink
|-[REVOLUTE] FourthLink
|-[REVOLUTE] FifthLink
|-[REVOLUTE] SixthLink
|-[REVOLUTE] SeventhLink
|-[LOCKED] bhand_base
|-[REVOLUTE] bhand_link11
|-[REVOLUTE] bhand_link12
|-[REVOLUTE] bhand_link13
|-[REVOLUTE] bhand_link21
|-[REVOLUTE] bhand_link22
|-[REVOLUTE] bhand_link23
|-[REVOLUTE] bhand_link32
|-[REVOLUTE] bhand_link33
Set up the 3D graphics.
cleanup_graphics, web_scene = multibody.setupGraphics(port=0)
web_scene.defaultCamera().pointCameraAt(
offset=[0.2, 0.5, 1.7],
target=[0, 0, 1],
up=[1, 0, 0],
)
proxy_scene = multibody.getScene()
Display the bodies in the multibody model.
multibody.displayModel()
[WebUI] Web server is running on port 38295
You may be able to connect in your browser at:
http://localhost:38295
LEGEND: <body number> <body name> <parent body> <hinge type> [prescribed subhinges] <U dofs/offset> [flex dofs/offset]
Body Parent Hinge Dofs
____ ______ _____ ____
1. FirstLink wam7dof_MBVROOT_ REVOLUTE 1/0
2. SecondLink FirstLink REVOLUTE 1/1
3. ThirdLink SecondLink REVOLUTE 1/2
4. FourthLink ThirdLink REVOLUTE 1/3
5. FifthLink FourthLink REVOLUTE 1/4
6. SixthLink FifthLink REVOLUTE 1/5
7. SeventhLink SixthLink REVOLUTE 1/6
8. bhand_base SeventhLink LOCKED 0/7
9. bhand_link11 bhand_base REVOLUTE 1/7
10. bhand_link12 bhand_link11 REVOLUTE 1/8
11. bhand_link13 bhand_link12 REVOLUTE 1/9
12. bhand_link21 bhand_base REVOLUTE 1/10
13. bhand_link22 bhand_link21 REVOLUTE 1/11
14. bhand_link23 bhand_link22 REVOLUTE 1/12
15. bhand_link32 bhand_base REVOLUTE 1/13
16. bhand_link33 bhand_link32 REVOLUTE 1/14
____
15
Display the topological structure of the multibody system
Testing robotics algorithms#
multibody.setUniformGravAccel([0, 0, 9.8])
print("Uniform Gravity Acceleration Set")
print(f"Udot: {multibody.getUdot()}")
Uniform Gravity Acceleration Set
Udot: [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]
print("Without Gravity Compensation")
multibody.setUdot(0)
kd.Algorithms.evalForwardDynamics(multibody)
print(f"Udot: {multibody.getUdot()}")
assert not np.allclose(multibody.getUdot(), 0)
Without Gravity Compensation
Udot: [-1.25788279e-04 -1.24733017e+01 3.58365891e-02 4.93454986e+01
-8.47109747e+01 -3.70940309e+01 8.46752641e+01 4.09569893e-07
-1.68881352e-01 -5.29529891e-02 4.09569893e-07 -1.68881352e-01
-5.29529892e-02 2.74942539e-01 -5.31081971e-02]
print("With Gravity Compensation")
multibody.setUdot(0)
T = kd.Algorithms.evalGravityCompensation(multibody)
print(f"Gravity Compensation Torque: {T}")
multibody.setT(T)
kd.Algorithms.evalForwardDynamics(multibody)
print(f"Udot: {multibody.getUdot()}")
assert np.allclose(multibody.getUdot(), 0)
With Gravity Compensation
Gravity Compensation Torque: [ 8.91121417e-05 4.24085429e-01 -2.44276030e-07 -2.63264584e+00
3.16021202e-06 1.66622295e-01 6.11944300e-07 -6.11951481e-07
-6.86000000e-02 0.00000000e+00 -6.11951481e-07 -6.86000000e-02
0.00000000e+00 -6.86000000e-02 0.00000000e+00]
Udot: [ 0.00000000e+00 0.00000000e+00 3.11755858e-15 -9.76298595e-19
-3.09934158e-15 -8.18770417e-18 -1.82169618e-17 7.78027041e-23
-9.18981707e-18 2.91143785e-21 7.78027041e-23 -9.18980914e-18
2.90350589e-21 9.19042584e-18 -3.52021250e-21]
print("Evaluated the Computed Torque")
print("Reset Q, U, and Udot")
multibody.setUdot(0)
multibody.setQ(0)
multibody.setU(0)
multibody.setT(0)
T = kd.Algorithms.evalComputedTorque(multibody)
print(f"Computed Torque: {T}")
multibody.setT(T)
kd.Algorithms.evalForwardDynamics(multibody)
print(f"Udot: {multibody.getUdot()}")
assert np.allclose(multibody.getUdot(), 0)
Evaluated the Computed Torque
Reset Q, U, and Udot
Computed Torque: [ 8.91121417e-05 4.24085429e-01 -2.44276030e-07 -2.63264584e+00
3.16021202e-06 1.66622295e-01 6.11944300e-07 -6.11951481e-07
-6.86000000e-02 0.00000000e+00 -6.11951481e-07 -6.86000000e-02
0.00000000e+00 -6.86000000e-02 0.00000000e+00]
Udot: [ 0.00000000e+00 0.00000000e+00 3.11755858e-15 -9.76298595e-19
-3.09934158e-15 -8.18770417e-18 -1.82169618e-17 7.78027041e-23
-9.18981707e-18 2.91143785e-21 7.78027041e-23 -9.18980914e-18
2.90350589e-21 9.19042584e-18 -3.52021250e-21]