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
514-
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[joint_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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy