|
1 | 1 | #!/usr/bin/env python
|
2 | 2 |
|
3 | 3 | 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) |
85 | 26 | 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 |
126 | 54 |
|
127 |
| - @property |
128 |
| - def mdh(self): |
129 |
| - return self._mdh |
| 55 | + def __iter__(self): |
| 56 | + return (each for each in self.links) |
130 | 57 |
|
131 | 58 | @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