10000 Merge branch 'future' of github.com:petercorke/robotics-toolbox-pytho… · sdiebolt/robotics-toolbox-python@827622e · GitHub
[go: up one dir, main page]
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)
0