roboticstoolbox-python 1.1.0__tar.gz → 1.1.1__tar.gz

{roboticstoolbox-python-1.1.0/roboticstoolbox_python.egg-info → roboticstoolbox_python-1.1.1}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: roboticstoolbox-python
-Version: 1.1.0
+Version: 1.1.1
 Summary: A Python library for robotics education and research
 Author-email: Jesse Haviland <j.haviland@qut.edu.au>, Peter Corke <rvc@petercorke.com>
 License: MIT License
@@ -39,10 +39,37 @@ Classifier: Programming Language :: Python :: 3.10
 Classifier: Programming Language :: Python :: 3.11
 Requires-Python: >=3.7
 Description-Content-Type: text/markdown
+License-File: LICENSE
+Requires-Dist: numpy>=1.17.4
+Requires-Dist: spatialmath-python>=1.1.5
+Requires-Dist: spatialgeometry>=1.0.0
+Requires-Dist: pgraph-python
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: ansitable
+Requires-Dist: swift-sim>=1.0.0
+Requires-Dist: rtb-data
+Requires-Dist: progress
+Requires-Dist: typing_extensions
 Provides-Extra: collision
+Requires-Dist: pybullet; extra == "collision"
 Provides-Extra: dev
+Requires-Dist: black; extra == "dev"
+Requires-Dist: pytest; extra == "dev"
+Requires-Dist: pytest-cov; extra == "dev"
+Requires-Dist: flake8; extra == "dev"
+Requires-Dist: pyyaml; extra == "dev"
+Requires-Dist: sympy; extra == "dev"
+Requires-Dist: qpsolvers; extra == "dev"
+Requires-Dist: quadprog; extra == "dev"
+Requires-Dist: pybullet; extra == "dev"
+Requires-Dist: bdsim; extra == "dev"
 Provides-Extra: docs
-License-File: LICENSE
+Requires-Dist: sphinx; extra == "docs"
+Requires-Dist: sphinx_rtd_theme; extra == "docs"
+Requires-Dist: sphinx-autorun; extra == "docs"
+Requires-Dist: sphinx_autodoc_typehints; extra == "docs"
+Requires-Dist: sphinx-favicon; extra == "docs"
 
 # Robotics Toolbox for Python
 
@@ -243,7 +270,7 @@ orientation parallel to y-axis (O=+Y)).
 from spatialmath import SE3
 
 Tep = SE3.Trans(0.6, -0.3, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
-sol = robot.ik_lm_chan(Tep)         # solve IK
+sol = robot.ik_LM(Tep)         # solve IK
 print(sol)
 
 	(array([ 0.20592815,  0.86609481, -0.79473206, -1.68254794,  0.74872915,
@@ -285,14 +312,14 @@ We can also experiment with velocity controllers in Swift. Here is a resolved-ra
 
 ```python
 import swift
-import roboticstoolbox as rp
+import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 
 env = swift.Swift()
 env.launch(realtime=True)
 
-panda = rp.models.Panda()
+panda = rtb.models.Panda()
 panda.q = panda.qr
 
 Tep = panda.fkine(panda.q) * sm.SE3.Trans(0.2, 0.2, 0.45)
@@ -304,7 +331,7 @@ dt = 0.05
 
 while not arrived:
 
-    v, arrived = rp.p_servo(panda.fkine(panda.q), Tep, 1)
+    v, arrived = rtb.p_servo(panda.fkine(panda.q), Tep, 1)
     panda.qd = np.linalg.pinv(panda.jacobe(panda.q)) @ v
     env.step(dt)
 

{roboticstoolbox-python-1.1.0 → roboticstoolbox_python-1.1.1}/README.md

@@ -197,7 +197,7 @@ orientation parallel to y-axis (O=+Y)).
 from spatialmath import SE3
 
 Tep = SE3.Trans(0.6, -0.3, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
-sol = robot.ik_lm_chan(Tep)         # solve IK
+sol = robot.ik_LM(Tep)         # solve IK
 print(sol)
 
 	(array([ 0.20592815,  0.86609481, -0.79473206, -1.68254794,  0.74872915,
@@ -239,14 +239,14 @@ We can also experiment with velocity controllers in Swift. Here is a resolved-ra
 
 ```python
 import swift
-import roboticstoolbox as rp
+import roboticstoolbox as rtb
 import spatialmath as sm
 import numpy as np
 
 env = swift.Swift()
 env.launch(realtime=True)
 
-panda = rp.models.Panda()
+panda = rtb.models.Panda()
 panda.q = panda.qr
 
 Tep = panda.fkine(panda.q) * sm.SE3.Trans(0.2, 0.2, 0.45)
@@ -258,7 +258,7 @@ dt = 0.05
 
 while not arrived:
 
-    v, arrived = rp.p_servo(panda.fkine(panda.q), Tep, 1)
+    v, arrived = rtb.p_servo(panda.fkine(panda.q), Tep, 1)
     panda.qd = np.linalg.pinv(panda.jacobe(panda.q)) @ v
     env.step(dt)
 

{roboticstoolbox-python-1.1.0 → roboticstoolbox_python-1.1.1}/pyproject.toml

@@ -4,7 +4,7 @@ name = "roboticstoolbox-python"
 
 description = "A Python library for robotics education and research"
 
-version = "1.1.0"
+version = "1.1.1"
 
 authors = [
     { name = "Jesse Haviland", email = "j.haviland@qut.edu.au" },
@@ -90,7 +90,7 @@ docs = [
     "sphinx_rtd_theme",
     "sphinx-autorun",
     "sphinx_autodoc_typehints",
-    "sphinx-favicon"
+    "sphinx-favicon",
 ]
 
 

roboticstoolbox_python-1.1.1/roboticstoolbox/backends/PyPlot/test1.py

@@ -0,0 +1,46 @@
+import matplotlib.pyplot as plt
+import spatialgeometry
+from roboticstoolbox.backends.PyPlot import PyPlot
+from spatialmath import SE3
+
+env = PyPlot()
+env.launch()
+
+# box = spatialgeometry.Box(sides=[0.02,]*3)
+# print(box)
+# env.add(box)
+
+# for i in range(1000):
+#     box.base = SE3(i / 1000, i / 1000, i / 500) * SE3.Rx(6 * i / 500)
+#     env.step()
+
+sph = spatialgeometry.Sphere(radius=0.05)
+print(sph)
+env.add(sph)
+
+for i in range(1000):
+    sph.base = SE3(i / 1000, i / 1000, i / 500) * SE3.Rx(6 * i / 500)
+    env.step()
+
+# env.hold()
+
+
+# from mpl_toolkits.mplot3d import Axes3D
+# import matplotlib.pyplot as plt
+# import numpy as np
+# from itertools import product, combinations
+# from numpy import sin, cos
+# from matplotlib.patches import Rectangle, Circle, PathPatch
+# import mpl_toolkits.mplot3d.art3d as art3d
+# from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+# from spatialmath.base import *
+# fig = plt.figure()
+# ax = fig.gca(projection='3d')
+# ax.set_aspect("auto")
+# ax.set_autoscale_on(True)
+
+
+# plot_box3d(sides=[0.2, 0.3, 0.4], wireframe=True, linewidth=1, color='r')
+# # plot_box3d(sides=[0.2, 0.3, 0.4], wireframe=False, facecolors='none', linewidths=1, edgecolors='r', alpha=.25)
+
+# plt.show(block=True)

roboticstoolbox_python-1.1.1/roboticstoolbox/backends/PyPlot/test2.py

@@ -0,0 +1,10 @@
+from roboticstoolbox import *
+
+# puma = models.ETS.Panda()
+# print(puma)
+
+# puma.plot(puma.qr, block=True)
+
+a1 = 1
+e = ET2.R() * ET2.tx(a1)
+e.plot(0, block=True)

roboticstoolbox_python-1.1.1/roboticstoolbox/blocks/test.py

@@ -0,0 +1,156 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import scipy.interpolate
+import math
+
+from roboticstoolbox.blocks.arm import *
+from spatialmath import SE3, SO3
+import roboticstoolbox as rtb
+
+import unittest
+import numpy.testing as nt
+
+
+class ArmTest(unittest.TestCase):
+    def setup(self):
+        # allow assertEqual for SE3
+        self.addTypeEqualityFunc(SE3, lambda x, y: x == y)
+
+    def test_fk(self):
+        robot = rtb.models.DH.Puma560()
+
+        block = Forward_Kinematics(robot)
+        self.assertEqual(block._eval(robot.qn)[0], robot.fkine(robot.qn))
+
+    def test_ik(self):
+        robot = rtb.models.DH.Puma560()
+        T = robot.fkine(robot.qn)
+
+        block = Inverse_Kinematics(robot)
+        q = block._eval(T)[0]
+        self.assertEqual(robot.fkine(q), T)
+
+    def test_rne(self):
+        robot = rtb.models.DH.Puma560()
+        block = Inverse_Dynamics(robot)
+        tau = block._eval(robot.qn, robot.qz, robot.qz)[0]
+        nt.assert_array_equal(tau, robot.rne(robot.qn, robot.qz, robot.qz))
+
+    def test_jacobian(self):
+        robot = rtb.models.DH.Puma560()
+        q = robot.qn
+
+        block = Jacobian(robot)
+        nt.assert_array_equal(block._eval(q)[0], robot.jacob0(robot.qn))
+        block = Jacobian(robot, inverse=True)
+        nt.assert_array_equal(block._eval(q)[0], np.linalg.inv(robot.jacob0(robot.qn)))
+        block = Jacobian(robot, transpose=True)
+        nt.assert_array_equal(block._eval(q)[0], robot.jacob0(robot.qn).T)
+        block = Jacobian(robot, inverse=True, transpose=True)
+        nt.assert_array_almost_equal(
+            block._eval(q)[0], np.linalg.inv(robot.jacob0(robot.qn).T)
+        )
+
+        block = Jacobian(robot, frame="e")
+        nt.assert_array_equal(block._eval(q)[0], robot.jacobe(robot.qn))
+        block = Jacobian(robot, frame="e", inverse=True)
+        nt.assert_array_equal(block._eval(q)[0], np.linalg.inv(robot.jacobe(robot.qn)))
+        block = Jacobian(robot, frame="e", transpose=True)
+        nt.assert_array_equal(block._eval(q)[0], robot.jacobe(robot.qn).T)
+        block = Jacobian(robot, frame="e", inverse=True, transpose=True)
+        nt.assert_array_almost_equal(
+            block._eval(q)[0], np.linalg.inv(robot.jacobe(robot.qn).T)
+        )
+
+    def test_tr2delta(self):
+
+        T1 = SE3(1, 2, 3)
+        T2 = SE3(1.1, 2.2, 3.3) * SE3.RPY(0.1, 0.2, 0.3)
+
+        block = Tr2Delta()
+        nt.assert_array_almost_equal(block._eval(T1, T2)[0], base.tr2delta(T1.A, T2.A))
+
+    def test_delta2tr(self):
+
+        delta = np.r_[0.1, 0.2, 0.3, 0.4, 0.5, 0.6] / 100
+
+        block = Delta2Tr()
+        out = block._eval(delta)[0]
+        self.assertIsInstance(out, SE3)
+        self.assertEqual(out, SE3.Delta(delta))
+
+    def test_tpoly(self):
+
+        block = Traj(0, 1, 10, traj="tpoly")
+        block.start()
+        # initial point
+        nt.assert_array_almost_equal(block._eval(0)[0], 0)
+        nt.assert_array_almost_equal(block._eval(0)[1], 0)
+        nt.assert_array_almost_equal(block._eval(0)[2], 0)
+
+        # final point
+        nt.assert_array_almost_equal(block._eval(10)[0], 1)
+        nt.assert_array_almost_equal(block._eval(10)[1], 0)
+        nt.assert_array_almost_equal(block._eval(10)[2], 0)
+
+        block = Traj([2, 3], [5, 6], 10, traj="tpoly")
+        block.start()
+        # initial point
+        nt.assert_array_almost_equal(block._eval(0)[0], [2, 3])
+        nt.assert_array_almost_equal(block._eval(0)[1], [0, 0])
+        nt.assert_array_almost_equal(block._eval(0)[2], [0, 0])
+
+        # final point
+        nt.assert_array_almost_equal(block._eval(10)[0], [5, 6])
+        nt.assert_array_almost_equal(block._eval(10)[1], [0, 0])
+        nt.assert_array_almost_equal(block._eval(10)[2], [0, 0])
+
+    def test_lspb(self):
+
+        block = Traj(0, 1, 10, traj="lspb")
+        block.start()
+        # initial point
+        nt.assert_array_almost_equal(block._eval(0)[0], 0)
+        nt.assert_array_almost_equal(block._eval(0)[1], 0)
+
+        # final point
+        nt.assert_array_almost_equal(block._eval(10)[0], 1)
+        nt.assert_array_almost_equal(block._eval(10)[1], 0)
+
+        block = Traj([2, 3], [5, 6], 10, traj="lspb")
+        block.start()
+        # initial point
+        nt.assert_array_almost_equal(block._eval(0)[0], [2, 3])
+        nt.assert_array_almost_equal(block._eval(0)[1], [0, 0])
+
+        # final point
+        nt.assert_array_almost_equal(block._eval(10)[0], [5, 6])
+        nt.assert_array_almost_equal(block._eval(10)[1], [0, 0])
+
+    def test_circle(self):
+        block = CirclePath(radius=1, centre=(0, 0, 1))
+        nt.assert_array_almost_equal(block._eval(t=0)[0], [1, 0, 1])
+        nt.assert_array_almost_equal(block._eval(t=0.25)[0], [0, 1, 1])
+        nt.assert_array_almost_equal(block._eval(t=0.5)[0], [-1, 0, 1])
+
+        block = CirclePath(radius=1, pose=SE3.Rx(pi / 2))
+        nt.assert_array_almost_equal(block._eval(t=0)[0], [1, 0, 0])
+        nt.assert_array_almost_equal(block._eval(t=0.25)[0], [0, 0, 1])
+        nt.assert_array_almost_equal(block._eval(t=0.5)[0], [-1, 0, 0])
+
+    def test_point2tr(self):
+        T = SE3.Rx(pi / 2)
+        block = Point2Tr(T)
+        t = [1, 2, 3]
+
+        out = block._eval(t)[0]
+        self.assertIsInstance(out, SE3)
+        nt.assert_array_almost_equal(out.R, T.R)
+        nt.assert_array_almost_equal(out.t, t)
+
+
+# ---------------------------------------------------------------------------------------#
+if __name__ == "__main__":
+
+    unittest.main()

roboticstoolbox_python-1.1.1/roboticstoolbox/examples/armer.py

@@ -0,0 +1,80 @@
+from roboticstoolbox import *
+from spatialmath import *
+from spatialmath.base import *
+import numpy as np
+
+
+robot = models.ETS.Panda()
+q0 = [.1, .3, .1, 0, 0, 0, 0]
+
+T0 = robot.fkine(q0)
+p0 = T0.t
+R0 = SO3(T0)
+
+q = q0
+dt = 0.01
+
+
+# # move at velocity (translational, rotational)
+# # rotational velocity is an Eulervector
+# v = np.r_[0, 0, -0.1, 0, 0, 0]
+# for t in np.arange(0.0, 10.0, dt):
+
+#     p = p0 + v[:3] * t
+#     # print(p)
+#     R = R0  * SO3.EulerVec(v[3:] * t)
+
+#     T = SE3.Rt(R, p)
+
+#     Tactual = robot.fkine(q)
+#     Tactual.printline()
+
+#     delta = Tactual.delta(T)  # Tactual - Tstar  in the Tactual frame
+
+#     qd = np.linalg.pinv(robot.jacobe(q)) @ delta * 0.9
+
+#     q += qd * [0.99, 0.98, 1.01, 1.02, 0.99, 1.02, 0.97] * dt
+
+#     print(p0 - Tactual.t)
+
+
+Tf = SE3(0.3, 0.1, 0.1) * SE3.Rx(np.pi/2) # hand down
+
+delta_vmax = 0.1  # max translational speed in m/s
+delta_rmax = 0.1
+
+for t in np.arange(0.0, 10.0, dt):
+
+    Tactual = robot.fkine(q)
+    Tactual.printline()
+
+    delta = Tactual.delta(Tf)  # Tactual - Tstar in the Tactual frame
+
+    if norm(delta) < 1e-3:
+        print('done at t=', t)
+        break
+    print(norm(delta))
+
+    delta *= 10
+    print(delta)
+    # delta is velocity per tick
+
+    # clip translational velocity
+    max = np.abs(delta[:3]).max()
+    if max > delta_vmax / dt:
+        delta[:3] *= delta_vmax / max / dt
+
+    # clip rotational velocity
+    max = np.abs(delta[3:]).max()
+    if max > delta_rmax / dt:
+        delta[3:] *= delta_rmax / max / dt
+    print(delta)
+    print()
+
+
+    qd = np.linalg.pinv(robot.jacobe(q)) @ delta
+
+    q += qd * [0.99, 0.98, 1.01, 1.02, 0.99, 1.02, 0.97] * dt
+    
+
+

roboticstoolbox_python-1.1.1/roboticstoolbox/examples/aside.py

@@ -0,0 +1,142 @@
+from math import pi
+import numpy as np
+
+# Spatial mathematics
+from spatialmath.base import *
+T = transl(0.5, 0.0, 0.0) @ rpy2tr(0.1, 0.2, 0.3, order='xyz') @ trotx(-90, 'deg')
+print(T)
+
+from spatialmath import *
+T = SE3(0.5, 0.0, 0.0) * SE3.RPY([0.1, 0.2, 0.3], order='xyz') * SE3.Rx(-90, unit='deg')
+print(T)
+      
+T.eul()
+T.R
+
+T.plot(color='red', label='2')
+
+UnitQuaternion.Rx(0.3)
+UnitQuaternion.AngVec(0.3, [1, 0, 0])
+
+R = SE3.Rx(np.linspace(0, pi/2, num=100))
+len(R)
+
+from roboticstoolbox import *
+# robot length values (metres)
+d1 = 0.352
+a1 = 0.070
+a2 = 0.360
+d4 = 0.380
+d6 = 0.065
+
+robot = DHRobot([
+  RevoluteDH(d=d1, a=a1, alpha=-pi/2), 
+  RevoluteDH(a=a2), 
+  RevoluteDH(alpha=pi/2),
+  ], name="my IRB140")
+
+print(robot)
+
+puma = models.DH.Puma560()
+T = puma.fkine([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
+sol = puma.ikine_LM(T)
+print(sol)
+
+puma.plot(sol.q)
+
+puma.ikine_a(T, config="lun")
+
+from roboticstoolbox import ETS as ET
+
+# Puma dimensions (m), see RVC2 Fig. 7.4 for details
+l1 = 0.672
+l2 = -0.2337
+l3 = 0.4318
+l4 = 0.0203
+l5 = 0.0837
+l6 = 0.4318
+
+e = ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() \
+    * ET.tz(l3) * ET.tx(l4) * ET.ty(l5) * ET.ry() \
+    * ET.tz(l6) * ET.rz() * ET.ry() * ET.rz()
+
+robot = ERobot(e)
+print(robot)
+
+panda = models.URDF.Panda()
+
+
+traj = jtraj(puma.qz, puma.qr, 100)
+qplot(traj.q)
+
+t = np.arange(0, 2, 0.010)
+T0 = SE3(0.6, -0.5, 0.3)
+T1 = SE3(0.4, 0.5, 0.2)
+Ts = ctraj(T0, T1, t)
+len(Ts)
+sol = puma.ikine_LM(Ts)
+sol.q.shape
+
+import spatialmath.base.symbolic as sym
+phi, theta, psi = sym.symbol('φ, ϴ, ψ')
+rpy2r(phi, theta, psi)
+
+q = sym.symbol("q_:6") # q = (q_1, q_2, ... q_5)
+T = puma.fkine(q)
+
+puma = models.DH.Puma560(symbolic=True)
+T = puma.fkine(q)
+T.t[0]
+
+puma = models.DH.Puma560(symbolic=False)
+J = puma.jacob0(puma.qn)
+J = puma.jacobe(puma.qn)
+
+J = puma.jacob0(puma.qr)
+np.linalg.matrix_rank(J)
+
+jsingu(J)
+
+H = panda.hessian0(panda.qz)
+H.shape
+
+puma.manipulability(puma.qn)
+puma.manipulability(puma.qn, method="asada")
+
+puma.manipulability(puma.qn, axes="trans")
+
+panda.jacobm(panda.qr)
+
+
+tau = puma.rne(puma.qn, np.zeros((6,)), np.zeros((6,)))
+J = puma.inertia(puma.qn)
+C = puma.coriolis(puma.qn, 0.1 * np.ones((6,)))
+g = puma.gravload(puma.qn)
+qdd = puma.accel(puma.qn, tau, np.zeros((6,)))
+
+
+obstacle = Box([1, 1, 1], SE3(1, 0, 0)) 
+iscollision = panda.collided(obstacle) # boolean
+iscollision = panda.links[0].collided(obstacle)
+
+d, p1, p2 = panda.closest_point(obstacle)
+d, p1, p2 = panda.links[0].closest_point(obstacle)
+
+panda.plot(panda.qr)
+
+from roboticstoolbox.backends.Swift import Swift
+backend = Swift()
+backend.launch()   # create graphical world
+backend.add(panda) # add robot to the world
+panda.q = panda.qr        # update the robot
+backend.step()    # display the world
+
+
+
+# q = puma.fdyn(q, ...)
+# puma.plot(q)
+# pyplot = roboticstoolbox.backends.PyPlot()
+# pyplot.launch()
+# pyplot.add(puma)
+# puma.q = q
+# puma.step()

roboticstoolbox_python-1.1.1/roboticstoolbox/examples/colors.py

@@ -0,0 +1,29 @@
+from matplotlib import colors
+from matplotlib import cm
+
+def linkcolormap(self, linkcolors="viridis"):  # self is a robot object
+
+    if isinstance(linkcolors, list) and len(linkcolors) == self.n:
+        # provided a list of color names
+        return colors.ListedColormap(linkcolors)
+    else:
+        # assume it is a colormap name
+        return cm.get_cmap(linkcolors, 6)
+
+import roboticstoolbox as rtb 
+robot = rtb.models.DH.Puma560()
+
+c = linkcolormap(robot)
+for i in range(6):
+    print(c(i))
+
+c = linkcolormap(robot, "inferno")
+for i in range(6):
+    print(c(i))
+print(type(c))
+c = linkcolormap(robot, ['red', (0,1,0), 'b', 'yellow', 'cyan', 'brown'])
+for i in range(6):
+    print(c(i))
+
+print(c(range(6)))
+print(type(c))

roboticstoolbox_python-1.1.1/roboticstoolbox/examples/eg2.py

@@ -0,0 +1,3 @@
+import roboticstoolbox as rtb
+robot = rtb.models.URDF.Puma560()
+robot.plot(robot.qn)

roboticstoolbox_python-1.1.1/roboticstoolbox/examples/erobot-test.py

@@ -0,0 +1,10 @@
+import roboticstoolbox as rtb 
+
+robot = rtb.models.URDF.PR2()
+
+print(robot)
+print(robot.ee_links[0])
+robot.hierarchy()
+# print(robot.ets())
+
+print(rtb.ETS.SE3(robot.base))