8000 Added fkine and ikine to serial_link · navrobot/robotics-toolbox-python@69638a2 · GitHub
[go: up one dir, main page]

Skip to content

Commit 69638a2

Browse files
committed
Added fkine and ikine to serial_link
1 parent cc401c7 commit 69638a2

File tree

4 files changed

+259
-165
lines changed

4 files changed

+259
-165
lines changed

roboticstoolbox/robot/Link.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -200,7 +200,7 @@ def __init__(self, *argv):
200200
parser.add_argument("--offset", help="joint variable offset (default 0)",
201201
type=float, default=0)
202202
parser.add_argument("--qlim", help="joint limit",
203-
type=float, default=0)
203+
type=list, default=[-pi/2, pi/2])
204204
parser.add_argument("--type", help="joint type, 'revolute', 'prismatic' or 'fixed'",
205205
choices=['', 'revolute', 'prismatic', 'fixed'], default='')
206206
parser.add_argument("--convention", help="D&h parameters, 'standard' or 'modified'",
@@ -240,7 +240,7 @@ def __init__(self, *argv):
240240

241241
self.offset = opt.offset
242242
self.flip = opt.flip
243-
self.qlim = opt.qlim
243+
self.qlim = argcheck.getvector(opt.qlim)
244244

245245
self.m = opt.m
246246
self.r = opt.r
@@ -340,7 +340,7 @@ def A(self, q):
340340
"""
341341
Link.A Link transform matrix
342342
343-
T = L.A(Q) is an SE3 object representing the transformation between link
343+
T = L.A(q) is an SE3 object representing the transformation between link
344344
frames when the link variable q which is either the Denavit-Hartenberg
345345
parameter theta (revolute) or d (prismatic). For:
346346
- standard DH parameters, this is from the previous frame to the current.

roboticstoolbox/robot/serial_link.py

Lines changed: 101 additions & 148 deletions
Original file line numberDiff line numberDiff line change
@@ -1,155 +1,108 @@
11
#!/usr/bin/env python
22

33
import numpy as np
4-
from rtb.robot.Link import Link
5-
# from ropy.robot.fkine import fkine
6-
# from ropy.robot.jocobe import jacobe
7-
# from ropy.robot.jocob0 import jacob0
8-
# from ropy.robot.ets import ets
9-
10-
class SerialLink(object):
11-
"""
12-
A superclass for arm type robots
13-
14-
Note: Link subclass elements passed in must be all standard, or all
15-
modified, DH parameters.
16-
17-
Attributes:
18-
--------
19-
name : string
20-
Name of the robot
21-
manufacturer : string
22-
Manufacturer of the robot
23-
base : float np.ndarray(4,4)
24-
Locaation of the base
25-
tool : float np.ndarray(4,4)
26-
Location of the tool
27-
links : List[n]
28-
Series of links which define the robot
29-
mdh : int
30-
0 if standard D&H, else 1
31-
n : int
32-
Number of joints in the robot
33-
T : float np.ndarray(4,4)
34-
The current pose of the robot
35-
q : float np.ndarray(1,n)
36-
The current joint angles of the robot
37-
Je : float np.ndarray(6,n)
38-
The manipulator Jacobian matrix maps joint velocity to end-effector
39-
spatial velocity in the ee frame
40-
J0 : float np.ndarray(6,n)
41-
The manipulator Jacobian matrix maps joint velocity to end-effector
42-
spatial velocity in the 0 frame
43-
He : float np.ndarray(6,n,n)
44-
The manipulator Hessian matrix maps joint acceleration to end-effector
45-
spatial acceleration in the ee frame
46-
H0 : float np.ndarray(6,n,n)
47-
The manipulator Hessian matrix maps joint acceleration to end-effector
48-
spatial acceleration in the 0 frame
49-
50-
Examples
51-
--------
52-
>>> L[0] = Revolute('d', 0, 'a', a1, 'alpha', np.pi/2)
53-
54-
>>> L[1] = Revolute('d', 0, 'a', a2, 'alpha', 0)
55-
56-
>>> twolink = SerialLink(L, 'name', 'two link');
57-
58-
See Also
59-
--------
60-
ropy.robot.ets : A superclass which represents the kinematics of a
61-
serial-link manipulator
62-
ropy.robot.Link : A link superclass for all link types
63-
ropy.robot.Revolute : A revolute link class
64-
"""
65-
66-
def __init__(self,
67-
L,
68-
name = 'noname',
69-
manufacturer = '',
70-
base = np.eye(4,4),
71-
tool = np.eye(4,4)
72-
):
73-
74-
self._name = name
75-
self._manuf = manufacturer
76-
self._links = []
77-
self._base = base
78-
self._tool = tool
79-
self._T = np.eye(4)
80-
81-
super(SerialLink, self).__init__()
82-
83-
if not isinstance(L, list):
84-
raise TypeError('The links L must be stored in a list.')
4+
from roboticstoolbox.robot.Link import *
5+
from spatialmath.pose3d import *
6+
from scipy.optimize import minimize
7+
8+
class SerialLink:
9+
10+
def __init__(self, links, name=None, base=None, tool=None, stl_files=None, q=None, param=None):
11+
"""
12+
Creates a SerialLink object.
13+
:param links: a list of links that will constitute SerialLink object.
14+
:param name: name property of the object.
15+
:param base: base transform applied to the SerialLink object.
16+
:param stl_files: STL file names to associate with links. Only works for pre-implemented models in model module.
17+
:param q: initial angles for link joints.
18+
:param colors: colors of STL files.
19+
"""
20+
self.pipeline = None
21+
self.links = links
22+
if q is None:
23+
self.q = np.array([0 for each in links])
24+
if base is None:
25+
self.base = np.eye(4, 4)
8526
else:
86-
if not isinstance(L[0], Link):
87-
raise TypeError('The links in L must be of Link type.')
88-
else:
89-
self._links = L
90-
91-
self._n = len(self._links)
92-
self._q = np.zeros((self._n,))
93-
94-
self._mdh = self.links[0].mdh
95-
for i in range(self._n):
96-
if not self._links[i].mdh == self._mdh:
97-
raise ValueError('Robot has mixed D&H links conventions.')
98-
99-
100-
101-
# Property methods
102-
103-
@property
104-
def name(self):
105-
return self._name
106-
107-
@property
108-
def manuf(self):
109-
return self._manuf
110-
111-
@property
112-
def links(self):
113-
return self._links
114-
115-
@property
116-
def base(self):
117-
return self._base
118-
119-
@property
120-
def tool(self):
121-
return self._tool
122-
123-
@property
124-
def n(self):
125-
return self._n
27+
assert (type(base) is np.ndarray) and (base.shape == (4, 4))
28+
self.base = base
29+
if tool is None:
30+
self.tool = np.eye(4, 4)
31+
else:
32+
assert (type(tool) is np.ndarray) and (tool.shape == (4, 4))
33+
self.tool = tool
34+
# Following arguments initialised by plot function and animate functions only
35+
if stl_files is None:
36+
# Default stick figure model code goes here
37+
pass
38+
else:
39+
self.stl_files = stl_files
40+
if name is None:
41+
self.name = ''
42+
else:
43+
self.name = name
44+
if param is None:
45+
# If model deosn't pass params, then use these default ones
46+
self.param = {
47+
"cube_axes_x_bounds": np.array([[-1.5, 1.5]]),
48+
"cube_axes_y_bounds": np.array([[-1.5, 1.5]]),
49+
"cube_axes_z_bounds": np.array([[-1.5, 1.5]]),
50+
"floor_position": np.array([[0, -1.5, 0]])
51+
}
52+
else:
53+
self.param = param
12654

127-
@property
128-
def mdh(self):
129-
return self._mdh
55+
def __iter__(self):
56+
return (each for each in self.links)
13057

13158
@property
132-
def q(self):
133-
return self._q
134-
135-
136-
# Setter methods
137-
138-
@base.setter
139-
def f(self, T):
140-
if not isinstance(T, np.ndarray):
141-
raise TypeError('Transformation matrix must be a numpy ndarray')
142-
elif T.shape != (4,4):
143-
raise ValueError('Transformation matrix must be a 4x4')
144-
self._base = T
145-
146-
@q.setter
147-
def q(self, q_new):
148-
if not isinstance(q_new, np.ndarray):
149-
raise TypeError('q array must be a numpy ndarray')
150-
elif q_new.shape != (self._n,):
151-
raise ValueError('q must be a 1 dim (n,) array')
152-
153-
self._q = q_new
154-
155-
59+
def length(self):
60+
"""
61+
length property
62+
:return: int
63+
"""
64+
return len(self.links)
65+
66+
def fkine(self, stance, unit='rad'):
67+
"""
68+
Calculates forward kinematics for a list of joint angles.
69+
:param stance: stance is list of joint angles.
70+
:param unit: unit of input angles.
71+
:return: homogeneous transformation matrix.
72+
"""
73+
if type(stance) is np.ndarray:
74+
stance = stance
75+
if unit == 'deg':
76+
stance = stance * pi / 180
77+
t = SE3(self.base)
78+
for i in range(self.length):
79+
t = t * self.links[i].A(stance[i])
80+
t = t * SE3(self.tool)
81+
return t
82+
83+
def ikine(self, T, q0=None, unit='rad'):
84+
"""
85+
Calculates inverse kinematics for homogeneous transformation matrix using numerical optimisation method.
86+
:param T: homogeneous transformation matrix.
87+
:param q0: initial list of joint angles for optimisation.
88+
:param unit: preferred unit for returned joint angles. Allowed values: 'rad' or 'deg'.
89+
:return: a list of 6 joint angles.
90+
"""
91+
assert T.shape == (4, 4)
92+
bounds = [(link.qlim[0], link.qlim[1]) for link in self]
93+
reach = 0
94+
for link in self:
95+
reach += abs(link.a) + abs(link.d)
96+
omega = np.diag([1, 1, 1, 3 / reach])
97+
if q0 is None:
98+
q0 = np.zeros((1, self.length))
99+
100+
def objective(x):
101+
return (
102+
np.square(((np.linalg.lstsq(T, self.fkine(x).A, rcond=-1)[0]) - np.eye(4, 4)) * omega)).sum()
103+
104+
sol = minimize(objective, x0=q0, bounds=bounds)
105+
if unit == 'deg':
106+
return sol.x * 180 / pi
107+
else:
108+
return sol.x

0 commit comments

Comments
 (0)
0