8000 Merge branch 'master' into swift2 · A905275/robotics-toolbox-python@5c09436 · GitHub
[go: up one dir, main page]
Skip to content

Commit 5c09436

Browse files
jhavljhavl
committed
Merge branch 'master' into swift2
2 parents aeb85de + 906f0e0 commit 5c09436

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

61 files changed

+3128
-682
lines changed

‎docs/Makefile

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,3 +18,6 @@ help:
1818
# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS).
1919
%: Makefile
2020
@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
21+
22+
show:
23+
open $(BUILDDIR)/html/index.html

‎docs/source/intro.rst

Lines changed: 37 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@ Introduction
66
Introduction
77
============
88

9-
*This is a modified version of a paper submitted to ICRA2020*
9+
This is a modified version of a paper accepted to ICRA2021 [corke21a]_.
1010

1111
The Robotics Toolbox for MATLAB® (RTB-M) was created around 1991 to support
1212
Peter Corke’s PhD research and was first published in 1995-6 [Corke95]_
@@ -359,10 +359,11 @@ or pure rotation -- each with either a constant parameter or a free parameter wh
359359
.. runblock:: pycon
360360
:linenos:
361361

362-
>>> from roboticstoolbox import ETS as E
362+
>>> from roboticstoolbox import ETS as ET
363363
>>> import roboticstoolbox as rtb
364-
>>> l1 = 0.672; l2 = 0.2337; l3 = 0.4318; l4 = -0.0837; l5 = 0.4318; l6 = 0.0203
365-
>>> e = E.tz(l1) * E.rz() * E.ty(l2) * E.ry() * E.tz(l3) * E.tx(l6) * E.ty(l4) * E.ry() * E.tz(l5) * E.rz() * E.ry() * E.rz()
364+
>>> # Puma dimensions (m), see RVC2 Fig. 7.4 for details
365+
>>> l1 = 0.672; l2 = -0.2337; l3 = 0.4318; l4 = 0.0203; l5 = 0.0837; l6 = 0.4318
366+
>>> 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()
366367
>>> print(e)
367368
>>> robot = rtb.ERobot(e)
368369
>>> print(robot)
@@ -404,7 +405,7 @@ end-effector must be specified.
404405
>>> import roboticstoolbox as rtb
405406
>>> panda = rtb.models.URDF.Panda()
406407
>>> print(panda)
407-
>>> T = panda.fkine(panda.qz, endlink='panda_hand')
408+
>>> T = panda.fkine(panda.qz, end='panda_hand')
408409
>>> print(T)
409410

410411
In the table above we see the end-effectors indicated by @ (determined automatically
@@ -469,9 +470,8 @@ two points specified by a pair of poses in :math:`\SE{3}`
469470
>>> T1 = SE3(0.4, 0.5, 0.2)
470471
>>> Ts = rtb.tools.trajectory.ctraj(T0, T1, len(t))
471472
>>> len(Ts)
472-
>>> sol = puma.ikine_LM(Ts) # array of named tuples
473-
>>> qt = np.array([x.q for x in sol]) # convert to 2d matrix
474-
>>> qt.shape
473+
>>> sol = puma.ikine_LM(Ts) # named tuple of arrays
474+
>>> sol.q.shape
475475

476476
At line 9 we see that the resulting trajectory, ``Ts``, is an ``SE3`` instance with 200 values.
477477

@@ -493,9 +493,8 @@ As mentioned earlier, the Toolbox supports symbolic manipulation using SymPy. Fo
493493

494494
.. runblock:: pycon
495495

496-
>>> import roboticstoolbox as rtb
497496
>>> import spatialmath.base as base
498-
>>> phi, theta, psi = base.sym.symbol('phi, theta, psi')
497+
>>> phi, theta, psi = base.sym.symbol('φ, ϴ, ψ')
499498
>>> base.rpy2r(phi, theta, psi)
500499

501500
The capability extends to forward kinematics
@@ -510,11 +509,13 @@ The capability extends to forward kinematics
510509
>>> T = puma.fkine(q)
511510
>>> T.t[0]
512511

513-
If we display the value of ``puma`` we see that the :math:`\alpha_j` values are now displayed in red to indicate that they are symbolic constants. The x-coordinate of the end-effector is
51 FEE1 4-
given by line 7.
512+
If we display the value of ``puma`` we see that the :math:`\alpha_j` values are
513+
now displayed in red to indicate that they are symbolic constants. The
514+
x-coordinate of the end-effector is given by line 7.
515515

516516

517-
SymPy allows any expression to be converted to runnable code in a variety of languages including C, Python and Octave/MATLAB.
517+
SymPy allows any expression to be converted to LaTeX or a variety of languages
518+
including C, Python and Octave/MATLAB.
518519

519520
Differential kinematics
520521
=======================
@@ -625,8 +626,8 @@ Python version takes 1.5ms (:math:`65\times` slower). With symbolic operands it
625626
takes 170ms (:math:`113\times` slower) to produce the unsimplified torque
626627
expressions.
627628

628-
For all robots there is also an implementation of Featherstone's spatial vector
629-
method, ``rne_spatial()``, and SMTB-P provides a set of classes for spatial
629+
For ``ERobot`` subclasses there is also an implementation of Featherstone's spatial vector
630+
method, ``rne()``, and SMTB-P provides a set of classes for spatial
630631
velocity, acceleration, momentum, force and inertia.
631632

632633

@@ -727,10 +728,12 @@ to HTML documentation whenever a change is pushed, and this is accessible via
727728
GitHub pages. Issues can be reported via GitHub issues or patches submitted as
728729
pull requests.
729730

730-
RTB-P, and its dependencies, can be installed simply by::
731+
RTB-P, and its dependencies, can be installed simply by either of::
731732

732733
$ pip install roboticstoolbox-python
733734

735+
$ conda install -c conda-forge roboticstoolbox-python
736+
734737
which includes basic visualization using matplotlib.
735738
Options such as ``vpython`` can be used to specify additional dependencies to be installed.
736739
The Toolbox adopts a "when needed" approach to many dependencies and will only attempt
@@ -747,27 +750,31 @@ installed.
747750
Conclusion
748751
==========
749752

750-
This article has introduced and demonstrated in tutorial form the principle features of the Robotics
751-
Toolbox for Python which runs on Mac, Windows and Linux using Python 3.6 or better.
752-
The code is free and open, and released under the MIT licence.
753-
It provides many of the essential tools necessary for
754-
robotic manipulator modelling, simulation and control which is essential for robotics education and research.
755-
It is familiar yet new, and we hope it will serve the community well for the next 25 years.
753+
This article has introduced and demonstrated in tutorial form the principle
754+
features of the Robotics Toolbox for Python which runs on Mac, Windows and Linux
755+
using Python 3.6 or better. The code is free and open, and released under the
756+
MIT licence. It provides many of the essential tools necessary for robotic
757+
manipulator modelling, simulation and control which is essential for robotics
758+
education and research. It is familiar yet new, and we hope it will serve the
759+
community well for the next 25 years.
756760

757-
Currently under development are backend interfaces for CoppeliaSim, Dynamixel
758-
servo chains, and ROS; symbolic dynamics, simplification and code generation;
759-
mobile robotics motion models, planners, EKF localization, map making and SLAM;
760-
and a minimalist block-diagram simulation tool [bdsim]_.
761+
A high-performance reactive motion controller, NEO, is based on this toolbox
762+
[neo]_. Currently under development are backend interfaces for CoppeliaSim,
763+
Dynamixel servo chains, and ROS; symbolic dynamics, simplification and code
764+
generation; mobile robotics motion models, planners, EKF localization, map
765+
making and SLAM; and a minimalist block-diagram simulation tool [bdsim]_.
761766

762767
References
763768
==========
764769

765-
.. [Corke95] P. Corke. A computer tool for simulation and analysis: the Robotics Toolbox for MATLAB. In Proc. National Conf. Australian Robot Association, pages 319–330, Melbourne, July 1995.
766-
.. [Corke96] P. Corke. A robotics toolbox for MATLAB. IEEE Robotics and Automation Magazine, 3(1):24–32, Sept. 1996.
767-
.. [Craig2005] Introduction to Robotics, John Craig, Wiley, 2005.
770+
.. [Corke95] `P. Corke. "A computer tool for simulation and analysis: the Robotics Toolbox for MATLAB". In Proc. National Conf. Australian Robot Association, pages 319–330, Melbourne, July 1995. <http://www.petercorke.com/RTB/ARA95.pdf>`_
771+
.. [Corke96] `P. Corke. "A robotics toolbox for MATLAB". IEEE Robotics and Automation Magazine, 3(1):24–32, Sept. 1996. <https://ieeexplore.ieee.org/document/486658>`_
772+
.. [Craig2005] J. Craig, "Introduction to Robotics", Wiley, 2005.
768773
.. [Featherstone87] R. Featherstone, Robot Dynamics Algorithms. Kluwer Academic, 1987.
769774
.. [Corke07] P. Corke, `“A simple and systematic approach to assigning Denavit- Hartenberg parameters,” IEEE transactions on robotics, vol. 23, no. 3, pp. 590–594, 2007, DOI 10.1109/TRO.2007.896765. <https://ieeexplore.ieee.org/document/4252158>`_.
770775
.. [Haviland20] `J. Haviland and P. Corke, “A systematic approach to computing the manipulator Jacobian and Hessian using the elementary transform sequence,” arXiv preprint, 2020. <https://arxiv.org/abs/2010.08696>`_
771776
.. [PyBullet] `PyBullet <https://pybullet.org/wordpress/>`_
772777
.. [SMTB-P] `Spatial Math Toolbox for Python <https://github.com/petercorke/spatialmath-python>`_
773-
.. [bdsim] `Block diagram simulator for Python <https://github.com/petercorke/bdsim>`_
778+
.. [bdsim] `Block diagram simulator for Python <https://github.com/petercorke/bdsim>`_
779+
.. [neo] `NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulatorshttps://jhavl.github.io/neo>`_
780+
.. [corke21a] P. Corke and J. Haviland, "Not your grandmother’s toolbox – the Robotics Toolbox reinvented for Python", Proc. ICRA 2021.

‎roboticstoolbox/backends/VPython/VPython.py

Lines changed: 14 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
close_localhost_session = None
2020

2121
try:
22-
from roboticstoolbox.backends.VPython.canvas import GraphicsCanvas2D, GraphicsCanvas3D
22+
from roboticstoolbox.backends.VPython.canvas import GraphicsCanvas2D, GraphicsCanvas3D, UImode
2323
from roboticstoolbox.backends.VPython.graphicalrobot import GraphicalRobot
2424
from roboticstoolbox.backends.VPython.grid import GridType
2525
except ImportError:
@@ -171,16 +171,18 @@ def step(self, dt=None, id=None, q=None, fig_num=0):
171171
# If GraphicalRobot given
172172
if isinstance(id, GraphicalRobot):
173173
if self.canvases[fig_num].is_robot_in(id):
174-
poses = id.fkine(q)
175-
id.set_joint_poses(poses)
174+
id.fkine_and_set(q)
175+
if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL:
176+
# Reload the joint sliders
177+
self.canvases[fig_num].teach_mode(teach=True)
176178

177179
# If DHRobot is given (or equivalent)
178180
else:
179-
grpahical_dh_robot = None
181+
graphical_dh_robot = None
180182
# If no ID given, and there are robots available
181183
if id is None and len(self.robots) > 0:
182184
# Obtain the first one
183-
grpahical_dh_robot = self.robots[0]
185+
graphical_dh_robot = self.robots[0]
184186
# If no ID, and no robots available
185187
elif id is None:
186188
print("No robot found")
@@ -191,16 +193,19 @@ def step(self, dt=None, id=None, q=None, fig_num=0):
191193
if self.robots[i].robot is id and \
192194
self.canvases[fig_num].is_robot_in_canvas(
193195
self.robots[i]):
194-
grpahical_dh_robot = self.robots[i]
196+
graphical_dh_robot = self.robots[i]
195197
break
196198

197199
# If no graphical equivalent found, return
198-
if grpahical_dh_robot is None:
200+
if graphical_dh_robot is None:
199201
print("No robot found")
200202
return
201203
# Set poses of graphical robot
202-
poses = grpahical_dh_robot.fkine(q)
203-
grpahical_dh_robot.set_joint_poses(poses)
204+
graphical_dh_robot.fkine_and_set(q)
205+
206+
if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL:
207+
# Reload the joint sliders
208+
self.canvases[fig_num].teach_mode(teach=True)
204209

205210
if dt is not None:
206211
sleep(dt)

‎roboticstoolbox/backends/VPython/canvas.py

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -222,8 +222,9 @@ def add_robot(self, robot):
222222
self.__reload_caption(new_list)
223223

224224
# Set it as selected
225-
self.__ui_controls.get('menu_robots').index = \
226-
len(self.__robots) - 1
225+
if self.__ui_mode == UImode.CANVASCONTROL:
226+
self.__ui_controls.get('menu_robots').index = \
227+
len(self.__robots) - 1
227228

228229
# Place camera based on robots effective radius * 1.25
229230
if robot.robot is not None:
@@ -285,6 +286,9 @@ def set_grid_mode(self, mode):
285286
"""
286287
self.__graphics_grid.set_mode(mode)
287288

289+
def current_mode(self):
290+
return self.__ui_mode
291+
288292
#######################################
289293
# UI Management
290294
#######################################
@@ -605,6 +609,11 @@ def __setup_joint_sliders(self):
605609
if len(self.__teachpanel) == 0:
606610
self.scene.append_to_caption("No robots available\n")
607611
return
612+
613+
# Update the robots to their current joint angles
614+
for joint_idx, joint in enumerate(self.__teachpanel[self.__selected_robot]):
615+
joint[self.__idx_theta] = self.__robots[self.__selected_robot].angles[join 10000 t_idx]
616+
608617
i = 0
609618
for joint in self.__teachpanel[self.__selected_robot]:
610619
if joint[self.__idx_qlim_min] == joint[self.__idx_qlim_max]:
@@ -774,10 +783,7 @@ def __joint_slider(self, s):
774783
angles.append(self.__teachpanel_sliders[idx].value)
775784

776785
# Run fkine
777-
poses = self.__robots[self.__selected_robot].fkine(angles)
778-
779-
# Update joints
780-
self.__robots[self.__selected_robot].set_joint_poses(poses)
786+
self.__robots[self.__selected_robot].fkine_and_set(angles)
781787

782788
for joint in self.__teachpanel[self.__selected_robot]:
783789
if joint[self.__idx_text] is None:

‎roboticstoolbox/backends/VPython/graphicalrobot.py

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -538,8 +538,11 @@ def __init__(self, graphics_canvas, name, robot=None):
538538
zero_angles = np.zeros((self.robot.n,))
539539
all_poses = self.robot.fkine_all(zero_angles, old=False)
540540
# Create the base
541-
if robot.basemesh is not None:
542-
self.append_link("s", all_poses[0], robot.basemesh, [0, 0], 0)
541+
try:
542+
if robot.basemesh is not None:
543+
self.append_link("s", all_poses[0], robot.basemesh, [0, 0], 0)
544+
except:
545+
pass
543546
# else: assume no base joint
544547
robot_colours = robot.linkcolormap()
545548
# Create the joints
@@ -786,7 +789,7 @@ def animate(self, frame_poses, fps):
786789
while t_stop - t_start < f:
787790
t_stop = perf_counter()
788791

789-
def fkine(self, joint_angles):
792+
def fkine_and_set(self, joint_angles):
790793
"""
791794
Call fkine for the robot. If it is based on a seriallink object,
792795
run it's fkine function.
@@ -796,7 +799,13 @@ def fkine(self, joint_angles):
796799
"""
797800
# If seriallink object, run it's fkine
798801
if self.robot is not None:
799-
return self.robot.fkine_all(joint_angles)
802+
poses = self.robot.fkine_all(joint_angles)
803+
if joint_angles is None:
804+
joint_angles = self.robot.q
805+
for a_idx in range(len(joint_angles)):
806+
# Ignore the base's angle (idx == 0)
807+
self.angles[a_idx+1] = joint_angles[a_idx]
808+
self.set_joint_poses(poses)
800809
# Else TODO
801810
else:
802811
pass

‎roboticstoolbox/blocks/arm.py

Lines changed: 21 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -171,14 +171,20 @@ class Jacobian(FunctionBlock):
171171
+------------+---------+---------+
172172
"""
173173

174-
def __init__(self, robot, *inputs, frame='0', inverse=False, transpose=False, **kwargs):
174+
def __init__(self, robot, *inputs, frame='0', inverse=False, pinv=False, transpose=False, **kwargs):
175175
"""
176176
:param ``*inputs``: Optional incoming connections
177177
:type ``*inputs``: Block or Plug
178178
:param robot: Robot model
179179
:type robot: Robot subclass
180180
:param frame: Frame to compute Jacobian for: '0' (default) or 'e'
181181
:type frame: str
182+
:param inverse: output inverse of Jacobian
183+
:type inverse: bool
184+
:param pinv: output pseudo-inverse of Jacobian
185+
:type pinv: bool
186+
:param transpose: output transpose of Jacobian
187+
:type transpose: bool
182188
:param ``**kwargs``: common Block options
183189
:return: a JACOBIAN block
184190
:rtype: Jacobian instance
@@ -193,7 +199,12 @@ def __init__(self, robot, *inputs, frame='0', inverse=False, transpose=False, **
193199
194200
1. Jacobian matrix as an ndarray(6,n)
195201
196-
202+
.. notes::
203+
- Only one of ``inverse`` or ``pinv`` can be True
204+
- ``inverse`` or ``pinv`` can be used in conjunction with ``transpose``
205+
- ``inverse`` requires that the Jacobian is square
206+
- If ``inverse`` is True and the Jacobian is singular a runtime
207+
error will occur.
197208
"""
198209
super().__init__(nin=1, nout=1, inputs=inputs, **kwargs)
199210
self.type = 'jacobian'
@@ -206,7 +217,13 @@ def __init__(self, robot, *inputs, frame='0', inverse=False, transpose=False, **
206217
self.jfunc = robot.jacobe
207218
else:
208219
raise ValueError('unknown frame')
220+
221+
if inverse and robot.n != 6:
222+
raise ValueError('cannot invert a non square Jacobian')
223+
if inverse and pinv:
224+
raise ValueError('can only set one of inverse and pinv')
209225
self.inverse = inverse
226+
self.pinv = pinv
210227
self.transpose = transpose
211228

212229
self.inport_names(('q',))
@@ -216,6 +233,8 @@ def output(self, t=None):
216233
J = self.jfunc(self.inputs[0])
217234
if self.inverse:
218235
J = np.linalg.inv(J)
236+
if self.pinv:
237+
J = np.linalg.pinv(J)
219238
if self.transpose:
220239
J = J.T
221240
return [J]

‎roboticstoolbox/blocks/mobile.py

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,11 +2,14 @@
22
from math import sin, cos, atan2, tan, sqrt, pi
33

44
import matplotlib.pyplot as plt
5+
from matplotlib.patches import Polygon
56
import time
67

78
from bdsim.components import TransferBlock, block
89
from bdsim.graphics import GraphicsBlock
910

11+
from spatialmath import base
12+
1013
# ------------------------------------------------------------------------ #
1114
@block
1215
class Bicycle(TransferBlock):
@@ -336,7 +339,7 @@ def __init__(self, *inputs, path=True, pathstyle=None, shape='triangle', color="
336339
vertices = [(-L1, W), (0.6*L2, W), (L2, 0.5*W), (L2, -0.5*W), (0.6*L2, -W), (-L1, -W)]
337340
else:
338341
raise ValueError('bad vehicle shape specified')
339-
self.vertices_hom = sm.e2h(np.array(vertices).T)
342+
self.vertices_hom = base.e2h(np.array(vertices).T)
340343
self.vertices = np.array(vertices)
341344

342345

@@ -384,8 +387,8 @@ def step(self):
384387
#plt.figure(self.fig.number)
385388
if self.path:
386389
self.line.set_data(self.xdata, self.ydata)
387-
T = sm.transl2(self.inputs[0], self.inputs[1]) @ sm.trot2(self.inputs[2])
388-
new = sm.h2e(T @ self.vertices_hom)
390+
T = base.transl2(self.inputs[0], self.inputs[1]) @ base.trot2(self.inputs[2])
391+
new = base.h2e(T @ self.vertices_hom)
389392
self.vehicle.set_xy(new.T)
390393

391394
if self.bd.options.animation:

0 commit comments

Comments
 (0)
0