Skip to content

Commit 827622e

Browse files
petercorkepetercorke
committed
Merge branch 'future' of github.com:petercorke/robotics-toolbox-python into future
2 parents e1af8ef + 66cf821 commit 827622e

File tree

4 files changed

+554
-255
lines changed

4 files changed

+554
-255
lines changed

‎roboticstoolbox/robot/BaseRobot.py

Lines changed: 251 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
from copy import deepcopy
1111
from functools import lru_cache
1212
from typing import (
13+
IO,
1314
TYPE_CHECKING,
1415
Any,
1516
Callable,
@@ -49,6 +50,7 @@
4950
from roboticstoolbox.backends.PyPlot import PyPlot, PyPlot2
5051
from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot
5152

53+
5254
if TYPE_CHECKING:
5355
from matplotlib.cm import Color # pragma nocover
5456
else:
@@ -1013,7 +1015,11 @@ def qlim(self) -> NDArray:
10131015

10141016
for link in self.links:
10151017
if link.isrevolute:
1016-
if link.qlim is None or np.any(np.isnan(link.qlim)):
1018+
if (
1019+
link.qlim is None
1020+
or link.qlim[0] is None
1021+
or np.any(np.isnan(link.qlim))
1022+
):
10171023
v = [-np.pi, np.pi]
10181024
else:
10191025
v = link.qlim
@@ -2838,3 +2844,247 @@ def teach(
28382844
return env
28392845

28402846
# --------------------------------------------------------------------- #
2847+
2848+
# --------------------------------------------------------------------- #
2849+
# --------- Utility Methods ------------------------------------------- #
2850+
# --------------------------------------------------------------------- #
2851+
2852+
def showgraph(self, display_graph: bool = True, **kwargs) -> Union[None, str]:
2853+
"""
2854+
Display a link transform graph in browser
2855+
2856+
``robot.showgraph()`` displays a graph of the robot's link frames
2857+
and the ETS between them. It uses GraphViz dot.
2858+
2859+
The nodes are:
2860+
- Base is shown as a grey square. This is the world frame origin,
2861+
but can be changed using the ``base`` attribute of the robot.
2862+
- Link frames are indicated by circles
2863+
- ETS transforms are indicated by rounded boxes
2864+
2865+
The edges are:
2866+
- an arrow if `jtype` is False or the joint is fixed
2867+
- an arrow with a round head if `jtype` is True and the joint is
2868+
revolute
2869+
- an arrow with a box head if `jtype` is True and the joint is
2870+
prismatic
2871+
2872+
Edge labels or nodes in blue have a fixed transformation to the
2873+
preceding link.
2874+
2875+
Parameters
2876+
----------
2877+
display_graph
2878+
Open the graph in a browser if True. Otherwise will return the
2879+
file path
2880+
etsbox
2881+
Put the link ETS in a box, otherwise an edge label
2882+
jtype
2883+
Arrowhead to node indicates revolute or prismatic type
2884+
static
2885+
Show static joints in blue and bold
2886+
2887+
Examples
2888+
--------
2889+
>>> import roboticstoolbox as rtb
2890+
>>> panda = rtb.models.URDF.Panda()
2891+
>>> panda.showgraph()
2892+
2893+
.. image:: ../figs/panda-graph.svg
2894+
:width: 600
2895+
2896+
See Also
2897+
--------
2898+
:func:`dotfile`
2899+
2900+
"""
2901+
2902+
# Lazy import
2903+
import tempfile
2904+
import subprocess
2905+
import webbrowser
2906+
2907+
# create the temporary dotfile
2908+
dotfile = tempfile.TemporaryFile(mode="w")
2909+
self.dotfile(dotfile, **kwargs)
2910+
2911+
# rewind the dot file, create PDF file in the filesystem, run dot
2912+
dotfile.seek(0)
2913+
pdffile = tempfile.NamedTemporaryFile(suffix=".pdf", delete=False)
2914+
subprocess.run("dot -Tpdf", shell=True, stdin=dotfile, stdout=pdffile)
2915+
2916+
# open the PDF file in browser (hopefully portable), then cleanup
2917+
if display_graph: # pragma nocover
2918+
webbrowser.open(f"file://{pdffile.name}")
2919+
else:
2920+
return pdffile.name
2921+
2922+
def dotfile(
2923+
self,
2924+
filename: Union[str, IO[str]],
2925+
etsbox: bool = False,
2926+
ets: L["full", "brief"] = "full",
2927+
jtype: bool = False,
2928+
static: bool = True,
2929+
):
2930+
"""
2931+
Write a link transform graph as a GraphViz dot file
2932+
2933+
The file can be processed using dot:
2934+
% dot -Tpng -o out.png dotfile.dot
2935+
2936+
The nodes are:
2937+
- Base is shown as a grey square. This is the world frame origin,
2938+
but can be changed using the ``base`` attribute of the robot.
2939+
- Link frames are indicated by circles
2940+
- ETS transforms are indicated by rounded boxes
2941+
2942+
The edges are:
2943+
- an arrow if `jtype` is False or the joint is fixed
2944+
- an arrow with a round head if `jtype` is True and the joint is
2945+
revolute
2946+
- an arrow with a box head if `jtype` is True and the joint is
2947+
prismatic
2948+
2949+
Edge labels or nodes in blue have a fixed transformation to the
2950+
preceding link.
2951+
2952+
Note
2953+
----
2954+
If ``filename`` is a file object then the file will *not*
2955+
be closed after the GraphViz model is written.
2956+
2957+
Parameters
2958+
----------
2959+
file
2960+
Name of file to write to
2961+
etsbox
2962+
Put the link ETS in a box, otherwise an edge label
2963+
ets
2964+
Display the full ets with "full" or a brief version with "brief"
2965+
jtype
2966+
Arrowhead to node indicates revolute or prismatic type
2967+
static
2968+
Show static joints in blue and bold
2969+
2970+
See Also
2971+
--------
2972+
:func:`showgraph`
2973+
2974+
"""
2975+
2976+
if isinstance(filename, str):
2977+
file = open(filename, "w")
2978+
else:
2979+
file = filename
2980+
2981+
header = r"""digraph G {
2982+
graph [rankdir=LR];
2983+
"""
2984+
2985+
def draw_edge(link, etsbox, jtype, static):
2986+
# draw the edge
2987+
if jtype:
2988+
if link.isprismatic:
2989+
edge_options = 'arrowhead="box", arrowtail="inv", dir="both"'
2990+
elif link.isrevolute:
2991+
edge_options = 'arrowhead="dot", arrowtail="inv", dir="both"'
2992+
else:
2993+
edge_options = 'arrowhead="normal"'
2994+
else:
2995+
edge_options = 'arrowhead="normal"'
2996+
2997+
if link.parent is None:
2998+
parent = "BASE"
2999+
else:
3000+
parent = link.parent.name
3001+
3002+
if etsbox:
3003+
# put the ets fragment in a box
3004+
if not link.isjoint and static:
3005+
node_options = ', fontcolor="blue"'
3006+
else:
3007+
node_options = ""
3008+
3009+
try:
3010+
file.write(
3011+
' {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format(
3012+
link.name,
3013+
link.ets.__str__(q=f"q{link.jindex}"),
3014+
node_options,
3015+
)
3016+
)
3017+
except UnicodeEncodeError: # pragma nocover
3018+
file.write(
3019+
' {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format(
3020+
link.name,
3021+
link.ets.__str__(q=f"q{link.jindex}")
3022+
.encode("ascii", "ignore")
3023+
.decode("ascii"),
3024+
node_options,
3025+
)
3026+
)
3027+
3028+
file.write(" {} -> {}_ets;\n".format(parent, link.name))
3029+
file.write(
3030+
" {}_ets -> {} [{}];\n".format(link.name, link.name, edge_options)
3031+
)
3032+
else:
3033+
# put the ets fragment as an edge label
3034+
if not link.isjoint and static:
3035+
edge_options += 'fontcolor="blue"'
3036+
if ets == "full":
3037+
estr = link.ets.__str__(q=f"q{link.jindex}")
3038+
elif ets == "brief":
3039+
if link.jindex is None:
3040+
estr = ""
3041+
else:
3042+
estr = f"...q{link.jindex}"
3043+
else:
3044+
return
3045+
try:
3046+
file.write(
3047+
' {} -> {} [label="{}", {}];\n'.format(
3048+
parent,
3049+
link.name,
3050+
estr,
3051+
edge_options,
3052+
)
3053+
)
3054+
except UnicodeEncodeError: # pragma nocover
3055+
file.write(
3056+
' {} -> {} [label="{}", {}];\n'.format(
3057+
parent,
3058+
link.name,
3059+
estr.encode("ascii", "ignore").decode("ascii"),
3060+
edge_options,
3061+
)
3062+
)
3063+
3064+
file.write(header)
3065+
3066+
# add the base link
3067+
file.write(" BASE [shape=square, style=filled, fillcolor=gray]\n")
3068+
3069+
# add the links
3070+
for link in self:
3071+
# draw the link frame node (circle) or ee node (doublecircle)
3072+
if link in self.ee_links:
3073+
# end-effector
3074+
node_options = 'shape="doublecircle", color="blue", fontcolor="blue"'
3075+
else:
3076+
node_options = 'shape="circle"'
3077+
3078+
file.write(" {} [{}];\n".format(link.name, node_options))
3079+
3080+
draw_edge(link, etsbox, jtype, static)
3081+
3082+
for gripper in self.grippers:
3083+
for link in gripper.links:
3084+
file.write(" {} [shape=cds];\n".format(link.name))
3085+
draw_edge(link, etsbox, jtype, static)
3086+
3087+
file.write("}\n")
3088+
3089+
if isinstance(filename, str):
3090+
file.close() # noqa

‎roboticstoolbox/robot/ETS.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -2578,8 +2578,8 @@ def ikine_LM(
25782578
**kwargs,
25792579
)
25802580

2581-
if isinstance(Tep, SE3):
2582-
Tep = Tep.A
2581+
# if isinstance(Tep, SE3):
2582+
# Tep = Tep.A
25832583

25842584
return solver.solve(ets=self, Tep=Tep, q0=q0)
25852585

@@ -2717,8 +2717,8 @@ def ikine_NR(
27172717
**kwargs,
27182718
)
27192719

2720-
if isinstance(Tep, SE3):
2721-
Tep = Tep.A
2720+
# if isinstance(Tep, SE3):
2721+
# Tep = Tep.A
27222722

27232723
return solver.solve(ets=self, Tep=Tep, q0=q0)
27242724

@@ -2871,8 +2871,8 @@ def ikine_GN(
28712871
**kwargs,
28722872
)
28732873

2874-
if isinstance(Tep, SE3):
2875-
Tep = Tep.A
2874+
# if isinstance(Tep, SE3):
2875+
# Tep = Tep.A
28762876

28772877
return solver.solve(ets=self, Tep=Tep, q0=q0)
28782878

@@ -3068,8 +3068,8 @@ def ikine_QP(
30683068
**kwargs,
30693069
)
30703070

3071-
if isinstance(Tep, SE3):
3072-
Tep = Tep.A
3071+
# if isinstance(Tep, SE3):
3072+
# Tep = Tep.A
30733073

30743074
return solver.solve(ets=self, Tep=Tep, q0=q0)
30753075

‎roboticstoolbox/robot/IK.py

Lines changed: 52 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -238,12 +238,62 @@ def solve(
238238

239239
q0 = q0_method
240240

241+
traj = False
242+
243+
methTep: np.ndarray
244+
241245
if isinstance(Tep, SE3):
242-
Tep: np.ndarray = Tep.A
246+
if len(Tep) > 1:
247+
traj = True
248+
methTep = np.empty((len(Tep), 4, 4))
243249

244-
if Tep.shape != (4, 4):
250+
for i, T in enumerate(Tep):
251+
methTep[i] = T.A
252+
else:
253+
methTep = Tep.A
254+
elif Tep.ndim == 3:
255+
traj = True
256+
methTep = Tep
257+
elif Tep.shape != (4, 4):
245258
raise ValueError("Tep must be a 4x4 SE3 matrix")
259+
else:
260+
methTep = Tep
261+
262+
if traj:
263+
q = np.empty((methTep.shape[0], ets.n))
264+
success = True
265+
interations = 0
266+
searches = 0
267+
residual = np.inf
268+
reason = ""
269+
270+
for i, T in enumerate(methTep):
271+
sol = self._solve(ets, T, q0)
272+
q[i] = sol.q
273+
if not sol.success:
274+
success = False
275+
reason = sol.reason
276+
interations += sol.iterations
277+
searches += sol.searches
278+
279+
if sol.residual < residual:
280+
residual = sol.residual
281+
282+
return IKSolution(
283+
q=q,
284+
success=success,
285+
iterations=interations,
286+
searches=searches,
287+
residual=residual,
288+
reason=reason,
289+
)
290+
291+
else:
292+
sol = self._solve(ets, methTep, q0)
293+
294+
return sol
246295

296+
def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution:
247297
# Iteration count
248298
i = 0
249299
total_i = 0

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