8000 rationalize properties · navrobot/robotics-toolbox-python@4a47e84 · GitHub
[go: up one dir, main page]
Skip to content

Commit 4a47e84

Browse files
petercorkepetercorke
committed
rationalize properties
1 parent ffbf433 commit 4a47e84

File tree

1 file changed

+58
-162
lines changed

1 file changed

+58
-162
lines changed

‎roboticstoolbox/robot/ERobot.py

Lines changed: 58 additions & 162 deletions
Original file line numberDiff line numberDiff line change
@@ -58,18 +58,13 @@ def __init__(
5858
**kwargs
5959
):
6060

61-
super().__init__(elinks, **kwargs)
6261

6362
self._ets = []
6463
self._elinks = {}
6564
self._n = 0
6665
self._M = 0
6766
self._q_idx = []
6867

69-
# Verify elinks
70-
if not isinstance(elinks, list):
71-
raise TypeError('The links must be stored in a list.')
72-
7368
# Set up a dictionary for looking up links by name
7469
for link in elinks:
7570
if isinstance(link, ELink):
@@ -111,7 +106,7 @@ def add_links(link, lst, q_idx):
111106

112107
lst.append(link)
113108

114-
# Figure out the order of links with resepect to joint variables
109+
# Figure out the order of links with respect to joint variables
115110
self.bfs_link(
116111
lambda link: add_links(link, self._ets, self._q_idx))
117112
self._n = len(self._q_idx)
@@ -125,6 +120,9 @@ def add_links(link, lst, q_idx):
125120
self.qdd = np.zeros(self.n)
126121
self.control_type = 'v'
127122

123+
super().__init__(elinks, **kwargs)
124+
125+
128126
def _reset_fk_path(self):
129127
# Pre-calculate the forward kinematics path
130128
self._fkpath = self.dfs_path(self.base_link, self.ee_link)
@@ -357,114 +355,21 @@ def qlim(self):
357355
# def qdlim(self):
358356
# return self.qdlim
359357

360-
@property
361-
def elinks(self):
362-
return self._elinks
363-
364-
@property
365-
def base_link(self):
366-
return self._base_link
367-
368-
@property
369-
def ee_link(self):
370-
return self._ee_link
371-
372-
@property
373-
def q(self):
374-
return self._q
375-
376-
@property
377-
def qd(self):
378-
return self._qd
379-
380-
@property
381-
def qdd(self):
382-
return self._qdd
383-
384-
@property
385-
def control_type(self):
386-
return self._control_type
387-
388-
@property
389-
def ets(self):
390-
return self._ets
391-
392-
@property
393-
def name(self):
394-
return self._name
395-
396-
@property
397-
def manufacturer(self):
398-
return self._manufacturer
399-
400-
@property
401-
def base(self):
402-
return self._base
403-
404-
@property
405-
def tool(self):
406-
return self._tool
358+
# --------------------------------------------------------------------- #
407359

408360
@property
409361
def n(self):
410362
return self._n
363+
# --------------------------------------------------------------------- #
411364

412365
@property
413-
def M(self):
414-
return self._M
415-
416-
@property
417-
def q_idx(self):
418-
return self._q_idx
366+
def elinks(self):
367+
return self._elinks
368+
# --------------------------------------------------------------------- #
419369

420370
@property
421-
def gravity(self):
422-
return self._gravity
423-
424-
@name.setter
425-
def name(self, name_new):
426-
self._name = name_new
427-
428-
@manufacturer.setter
429-
def manufacturer(self, manufacturer_new):
430-
self._manufacturer = manufacturer_new
431-
432-
@gravity.setter
433-
def gravity(self, gravity_new):
434-
self._gravity = getvector(gravity_new, 3, 'col')
435-
436-
@q.setter
437-
def q(self, q_new):
438-
q_new = getvector(q_new, self.n)
439-
self._q = q_new
440-
441-
@qd.setter
442-
def qd(self, qd_new):
443-
self._qd = getvector(qd_new, self.n)
444-
445-
@qdd.setter
446-
def qdd(self, qdd_new):
447-
self._qdd = getvector(qdd_new, self.n)
448-
449-
@control_type.setter
450-
def control_type(self, cn):
451-
if cn == 'p' or cn == 'v' or cn == 'a':
452-
self._control_type = cn
453-
else:
454-
raise ValueError(
455-
'Control type must be one of \'p\', \'v\', or \'a\'')
456-
457-
@base.setter
458-
def base(self, T):
459-
if not isinstance(T, SE3):
460-
T = SE3(T)
461-
self._base = T
462-
463-
@tool.setter
464-
def tool(self, T):
465-
if not isinstance(T, SE3): # pragma nocover
466-
T = SE3(T)
467-
self._tool = T
371+
def base_link(self):
372+
return self._base_link
468373

469374
@base_link.setter
470375
def base_link(self, link):
@@ -473,6 +378,11 @@ def base_link(self, link):
473378
else:
474379
self._base_link = self.ets[link]
475380
self._reset_fk_path()
381+
# --------------------------------------------------------------------- #
382+
383+
@property
384+
def ee_link(self):
385+
return self._ee_link
476386

477387
@ee_link.setter
478388
def ee_link(self, link):
@@ -481,6 +391,22 @@ def ee_link(self, link):
481391
else:
482392
self._ee_link = self.ets[link]
483393
self._reset_fk_path()
394+
# --------------------------------------------------------------------- #
395+
396+
@property
397+
def ets(self):
398+
return self._ets
399+
# --------------------------------------------------------------------- #
400+
401+
@property
402+
def M(self):
403+
return self._M
404+
# --------------------------------------------------------------------- #
405+
406+
@property
407+
def q_idx(self):
408+
return self._q_idx
409+
# --------------------------------------------------------------------- #
484410

485411
def fkine(self, q=None):
486412
'''
@@ -1142,69 +1068,39 @@ def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
11421068

11431069
return Ain, Bin
11441070

1145-
def link_collision_damper(
1146-
self, shape, q=None, di=0.3, ds=0.05, xi=1.0,
1147-
from_link=None, to_link=None):
1148-
1149-
if from_link is None:
1150-
from_link = self.base_link
1151-
1152-
if to_link is None:
1153-
to_link = self.ee_link
1154-
1155-
links, n = self.get_path(from_link, to_link)
1156-
1157-
if q is None:
1158-
q = np.copy(self.q)
1159-
else:
1160-
q = getvector(q, n)
1071+
# def link_collision_damper(self, links=None, col, ob, q):
1072+
# dii = 5
1073+
# di = 0.3
1074+
# ds = 0.05
11611075

1162-
j = 0
1163-
Ain = None
1164-
bin = None
1165-
1166-
def indiv_calculation(link, link_col, q):
1167-
d, wTlp, wTcp = link_col.closest_point(shape, di)
1168-
1169-
if d is not None:
1170-
lpTcp = wTlp.inv() * wTcp
1171-
norm = lpTcp.t / d
1172-
norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)
1173-
1174-
Je = self.jacobe(
1175-
q, from_link=self.base_link, to_link=link,
1176-
offset=link_col.base)
1177-
n_dim = Je.shape[1]
1178-
dp = norm_h @ shape.v
1179-
l_Ain = np.zeros((1, n))
1180-
l_Ain[0, :n_dim] = norm_h @ Je
1181-
l_bin = (xi * (d - ds) / (di - ds)) + dp
1182-
else:
1183-
l_Ain = None
1184-
l_bin = None
1076+
# if links is None:
1077+
# links = self.links[1:]
11851078

1186-
return l_Ain, l_bin, d, wTcp
1079+
# ret = p.getClosestPoints(col.co, ob.co, dii)
11871080

1188-
for link in links:
1189-
if link.jtype == link.VARIABLE:
1190-
j += 1
1081+
# if len(ret) > 0:
1082+
# ret = ret[0]
1083+
# wTlp = sm.SE3(ret[5])
1084+
# wTcp = sm.SE3(ret[6])
1085+
# lpTcp = wTlp.inv() * wTcp
11911086

1192-
for link_col in link.collision:
1193-
l_Ain, l_bin, d, wTcp = i E920 ndiv_calculation(
1194-
link, link_col, q[:j])
1087+
# d = ret[8]
11951088

1196-
if l_Ain is not None and l_bin is not None:
1197-
if Ain is None:
1198-
Ain = l_Ain
1199-
else:
1200-
Ain = np.r_[Ain, l_Ain]
1089+
# if d < di:
1090+
# n = lpTcp.t / d
1091+
# nh = np.expand_dims(np.r_[n, 0, 0, 0], axis=0)
12011092

1202-
if bin is None:
1203-
bin = np.array(l_bin)
1204-
else:
1205-
bin = np.r_[bin, l_bin]
1093+
# Je = r.jacobe(q, from_link=r.base_link, to_link=link, offset=col.base)
1094+
# n = Je.shape[1]
1095+
# dp = nh @ ob.v
1096+
# l_Ain = np.zeros((1, 13))
1097+
# l_Ain[0, :n] = nh @ Je
1098+
# l_bin = (0.8 * (d - ds) / (di - ds)) + dp
1099+
# else:
1100+
# l_Ain = None
1101+
# l_bin = None
12061102

1207-
return Ain, bin
1103+
# return l_Ain, l_bin, d, wTcp
12081104

12091105
def closest_point(self, shape, inf_dist=1.0):
12101106
'''

0 commit comments

Comments
 (0)
0