├── .idea
    ├── CSDN_BLOG_CODE.iml
    ├── libraries
    │   └── R_User_Library.xml
    ├── modules.xml
    ├── vcs.xml
    └── workspace.xml
├── README.md
├── ROS_UR5_CIRCLE_V0
    ├── Quaternion.py
    ├── README.md
    ├── UR5Script_move_circle.py
    ├── demo
    │   └── webwxgetvideo
    ├── transfer.py
    ├── ur5_pose_get.py
    ├── ur_kinematics.py
    └── utility.py
└── stand.gif


/.idea/CSDN_BLOG_CODE.iml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <module type="PYTHON_MODULE" version="4">
 3 |   <component name="NewModuleRootManager">
 4 |     <content url="file://$MODULE_DIR$" />
 5 |     <orderEntry type="inheritedJdk" />
 6 |     <orderEntry type="sourceFolder" forTests="false" />
 7 |     <orderEntry type="library" name="R User Library" level="project" />
 8 |     <orderEntry type="library" name="R Skeletons" level="application" />
 9 |   </component>
10 |   <component name="TestRunnerService">
11 |     <option name="PROJECT_TEST_RUNNER" value="Unittests" />
12 |   </component>
13 | </module>


--------------------------------------------------------------------------------
/.idea/libraries/R_User_Library.xml:
--------------------------------------------------------------------------------
1 | <component name="libraryTable">
2 |   <library name="R User Library">
3 |     <CLASSES />
4 |     <SOURCES />
5 |   </library>
6 | </component>


--------------------------------------------------------------------------------
/.idea/modules.xml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <project version="4">
3 |   <component name="ProjectModuleManager">
4 |     <modules>
5 |       <module fileurl="file://$PROJECT_DIR$/.idea/CSDN_BLOG_CODE.iml" filepath="$PROJECT_DIR$/.idea/CSDN_BLOG_CODE.iml" />
6 |     </modules>
7 |   </component>
8 | </project>


--------------------------------------------------------------------------------
/.idea/vcs.xml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <project version="4">
3 |   <component name="VcsDirectoryMappings">
4 |     <mapping directory="$PROJECT_DIR$" vcs="Git" />
5 |   </component>
6 | </project>


--------------------------------------------------------------------------------
/.idea/workspace.xml:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="UTF-8"?>
  2 | <project version="4">
  3 |   <component name="ChangeListManager">
  4 |     <list default="true" id="2f0b9591-6423-4421-85f1-632d55e81d04" name="Default Changelist" comment="">
  5 |       <change afterPath="$PROJECT_DIR$/ROS_UR5_CIRCLE_V0/ur_kinematics.py" afterDir="false" />
  6 |     </list>
  7 |     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
  8 |     <option name="SHOW_DIALOG" value="false" />
  9 |     <option name="HIGHLIGHT_CONFLICTS" value="true" />
 10 |     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
 11 |     <option name="LAST_RESOLUTION" value="IGNORE" />
 12 |   </component>
 13 |   <component name="FUSProjectUsageTrigger">
 14 |     <session id="-1429781908">
 15 |       <usages-collector id="statistics.lifecycle.project">
 16 |         <counts>
 17 |           <entry key="project.open.time.0" value="1" />
 18 |           <entry key="project.opened" value="1" />
 19 |         </counts>
 20 |       </usages-collector>
 21 |     </session>
 22 |   </component>
 23 |   <component name="FileEditorManager">
 24 |     <leaf>
 25 |       <file pinned="false" current-in-tab="true">
 26 |         <entry file="file://$PROJECT_DIR$/ROS_UR5_CIRCLE_V0/ur_kinematics.py">
 27 |           <provider selected="true" editor-type-id="text-editor">
 28 |             <state relative-caret-position="150">
 29 |               <caret line="10" column="13" lean-forward="true" selection-start-line="10" selection-start-column="13" selection-end-line="10" selection-end-column="13" />
 30 |               <folding>
 31 |                 <element signature="e#114#125#0" expanded="true" />
 32 |               </folding>
 33 |             </state>
 34 |           </provider>
 35 |         </entry>
 36 |       </file>
 37 |     </leaf>
 38 |   </component>
 39 |   <component name="FileTemplateManagerImpl">
 40 |     <option name="RECENT_TEMPLATES">
 41 |       <list>
 42 |         <option value="Python Script" />
 43 |       </list>
 44 |     </option>
 45 |   </component>
 46 |   <component name="Git.Settings">
 47 |     <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
 48 |   </component>
 49 |   <component name="IdeDocumentHistory">
 50 |     <option name="CHANGED_PATHS">
 51 |       <list>
 52 |         <option value="$PROJECT_DIR$/ROS_UR5_CIRCLE_V0/ur_kinematics.py" />
 53 |       </list>
 54 |     </option>
 55 |   </component>
 56 |   <component name="ProjectFrameBounds">
 57 |     <option name="x" value="20" />
 58 |     <option name="y" value="43" />
 59 |     <option name="width" value="1400" />
 60 |     <option name="height" value="788" />
 61 |   </component>
 62 |   <component name="ProjectView">
 63 |     <navigator proportions="" version="1">
 64 |       <foldersAlwaysOnTop value="true" />
 65 |     </navigator>
 66 |     <panes>
 67 |       <pane id="Scope" />
 68 |       <pane id="ProjectPane">
 69 |         <subPane>
 70 |           <expand>
 71 |             <path>
 72 |               <item name="CSDN_BLOG_CODE" type="b2602c69:ProjectViewProjectNode" />
 73 |               <item name="CSDN_BLOG_CODE" type="462c0819:PsiDirectoryNode" />
 74 |             </path>
 75 |             <path>
 76 |               <item name="CSDN_BLOG_CODE" type="b2602c69:ProjectViewProjectNode" />
 77 |               <item name="CSDN_BLOG_CODE" type="462c0819:PsiDirectoryNode" />
 78 |               <item name="ROS_UR5_CIRCLE_V0" type="462c0819:PsiDirectoryNode" />
 79 |             </path>
 80 |           </expand>
 81 |           <select />
 82 |         </subPane>
 83 |       </pane>
 84 |     </panes>
 85 |   </component>
 86 |   <component name="PropertiesComponent">
 87 |     <property name="last_opened_file_path" value="$PROJECT_DIR$" />
 88 |     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
 89 |   </component>
 90 |   <component name="RunDashboard">
 91 |     <option name="ruleStates">
 92 |       <list>
 93 |         <RuleState>
 94 |           <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
 95 |         </RuleState>
 96 |         <RuleState>
 97 |           <option name="name" value="StatusDashboardGroupingRule" />
 98 |         </RuleState>
 99 |       </list>
100 |     </option>
101 |   </component>
102 |   <component name="SvnConfiguration">
103 |     <configuration />
104 |   </component>
105 |   <component name="TaskManager">
106 |     <task active="true" id="Default" summary="Default task">
107 |       <changelist id="2f0b9591-6423-4421-85f1-632d55e81d04" name="Default Changelist" comment="" />
108 |       <created>1560826357781</created>
109 |       <option name="number" value="Default" />
110 |       <option name="presentableId" value="Default" />
111 |       <updated>1560826357781</updated>
112 |     </task>
113 |     <servers />
114 |   </component>
115 |   <component name="ToolWindowManager">
116 |     <frame x="20" y="43" width="1400" height="788" extended-state="0" />
117 |     <editor active="true" />
118 |     <layout>
119 |       <window_info id="Favorites" side_tool="true" />
120 |       <window_info active="true" content_ui="combo" id="Project" order="0" visible="true" weight="0.2496318" />
121 |       <window_info id="Structure" order="1" side_tool="true" weight="0.25" />
122 |       <window_info anchor="bottom" id="Version Control" show_stripe_button="false" />
123 |       <window_info anchor="bottom" id="Python Console" />
124 |       <window_info anchor="bottom" id="Terminal" />
125 |       <window_info anchor="bottom" id="Event Log" side_tool="true" />
126 |       <window_info anchor="bottom" id="Message" order="0" />
127 |       <window_info anchor="bottom" id="Find" order="1" />
128 |       <window_info anchor="bottom" id="Run" order="2" />
129 |       <window_info anchor="bottom" id="Debug" order="3" weight="0.4" />
130 |       <window_info anchor="bottom" id="Cvs" order="4" weight="0.25" />
131 |       <window_info anchor="bottom" id="Inspection" order="5" weight="0.4" />
132 |       <window_info anchor="bottom" id="TODO" order="6" />
133 |       <window_info anchor="right" id="R Packages" />
134 |       <window_info anchor="right" id="R Graphics" />
135 |       <window_info anchor="right" id="Commander" internal_type="SLIDING" order="0" type="SLIDING" weight="0.4" />
136 |       <window_info anchor="right" id="Ant Build" order="1" weight="0.25" />
137 |       <window_info anchor="right" content_ui="combo" id="Hierarchy" order="2" weight="0.25" />
138 |     </layout>
139 |   </component>
140 |   <component name="VcsContentAnnotationSettings">
141 |     <option name="myLimit" value="2678400000" />
142 |   </component>
143 |   <component name="editorHistoryManager">
144 |     <entry file="file://$PROJECT_DIR$/ROS_UR5_CIRCLE_V0/ur_kinematics.py">
145 |       <provider selected="true" editor-type-id="text-editor">
146 |         <state relative-caret-position="150">
147 |           <caret line="10" column="13" lean-forward="true" selection-start-line="10" selection-start-column="13" selection-end-line="10" selection-end-column="13" />
148 |           <folding>
149 |             <element signature="e#114#125#0" expanded="true" />
150 |           </folding>
151 |         </state>
152 |       </provider>
153 |     </entry>
154 |   </component>
155 | </project>


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # CSDN_BLOG_CODE
2 | 
3 | ## Blog code and demo displayed respositories. You can just copy to your project for free.
4 | ## Welcome to communicate with me about my blog article or robots' techniques.


--------------------------------------------------------------------------------
/ROS_UR5_CIRCLE_V0/Quaternion.py:
--------------------------------------------------------------------------------
  1 | """
  2 | Quaternion class.
  3 | 
  4 | Python implementation by: Luis Fernando Lara Tobar and Peter Corke.
  5 | Based on original Robotics Toolbox for Matlab code by Peter Corke.
  6 | Permission to use and copy is granted provided that acknowledgement of
  7 | the authors is made.
  8 | 
  9 | @author: Luis Fernando Lara Tobar and Peter Corke
 10 | """
 11 | 
 12 | from numpy import *
 13 | from utility import *
 14 | import transfer as T
 15 | import copy
 16 | 
 17 | 
 18 | # Copyright (C) 1999-2002, by Peter I. Corke
 19 | 
 20 | class quaternion:
 21 |     """A quaternion is a compact method of representing a 3D rotation that has
 22 |     computational advantages including speed and numerical robustness.
 23 | 
 24 |     A quaternion has 2 parts, a scalar s, and a vector v and is typically written::
 25 | 
 26 |         q = s <vx vy vz>
 27 | 
 28 |     A unit quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}.
 29 | 
 30 |     A quaternion can be considered as a rotation about a vector in space where
 31 |     q = cos (theta/2) sin(theta/2) <vx vy vz>
 32 |     where <vx vy vz> is a unit vector.
 33 | 
 34 |     Various functions such as INV, NORM, UNIT and PLOT are overloaded for
 35 |     quaternion objects.
 36 | 
 37 |     Arithmetic operators are also overloaded to allow quaternion multiplication,
 38 |     division, exponentiaton, and quaternion-vector multiplication (rotation).
 39 |     """
 40 | 
 41 |     def __init__(self, *args):
 42 |         '''
 43 | Constructor for quaternion objects:
 44 | 
 45 |     - q = quaternion(v, theta)    from vector plus angle
 46 |     - q = quaternion(R)       from a 3x3 or 4x4 matrix
 47 |     - q = quaternion(q)       from another quaternion
 48 |     - q = quaternion(s)       from a scalar
 49 |     - q = quaternion(v)       from a matrix/array/list v = [s v1 v2 v3]
 50 |     - q = quaternion(s, v1, v2, v3)    from 4 elements
 51 |     - q = quaternion(s, v)    from 4 elements
 52 | '''
 53 | 
 54 |         self.vec = []
 55 | 
 56 |         if len(args) == 0:
 57 |             # default is a null rotation
 58 |             self.s = 1.0
 59 |             self.v = matrix([0.0, 0.0, 0.0])
 60 | 
 61 |         elif len(args) == 1:
 62 |             arg = args[0]
 63 | 
 64 |             if isinstance(arg, quaternion):
 65 |                 # Q = QUATERNION(q) from another quaternion
 66 |                 self.s = arg.s
 67 |                 self.v = arg.v
 68 |                 return
 69 | 
 70 |             if type(arg) is matrix:
 71 |                 # Q = QUATERNION(R) from a 3x3
 72 |                 if (arg.shape == (3, 3)):
 73 |                     self.tr2q(arg)
 74 |                     return
 75 | 
 76 |                 # Q = QUATERNION(R) from a 4x4
 77 |                 if (arg.shape == (4, 4)):
 78 |                     self.tr2q(arg[0:3, 0:3]);
 79 |                     return
 80 | 
 81 |             # some kind of list, vector, scalar...
 82 | 
 83 |             v = arg2array(arg);
 84 | 
 85 |             if len(v) == 4:
 86 |                 # Q = QUATERNION([s v1 v2 v3]) from 4 elements
 87 |                 self.s = v[0]
 88 |                 self.v = mat(v[1:4])
 89 | 
 90 |             elif len(v) == 3:
 91 |                 self.s = 0
 92 |                 self.v = mat(v[0:3])
 93 | 
 94 |             elif len(v) == 1:
 95 |                 # Q = QUATERNION(s) from a scalar
 96 |                 # Q = QUATERNION(s)               from a scalar
 97 |                 self.s = v[0]
 98 |                 self.v = mat([0, 0, 0])
 99 | 
100 |         elif len(args) == 2:
101 |             # Q = QUATERNION(v, theta) from vector plus angle
102 |             # Q = quaternion(s, v);
103 | 
104 |             a1 = arg2array(args[0])
105 |             a2 = arg2array(args[1])
106 |             if len(a1) == 1 and len(a2) == 3:
107 |                 # s, v
108 |                 self.s = a1[0]
109 |                 self.v = mat(a2)
110 |             elif len(a1) == 3 and len(a2) == 1:
111 |                 # v, theta
112 |                 self.s = a2[0];
113 |                 self.v = mat(a1);
114 | 
115 |         elif len(args) == 4:
116 |             self.s = args[0];
117 |             self.v = mat(args[1:4])
118 | 
119 |         else:
120 |             print "error"
121 |             return None
122 | 
123 |     def __repr__(self):
124 |         return "%f <%f, %f, %f>" % (self.s, self.v[0, 0], self.v[0, 1], self.v[0, 2])
125 | 
126 |     def tr2q(self, t):
127 |         # TR2Q   Convert homogeneous transform to a unit-quaternion
128 |         #
129 |         #   Q = tr2q(T)
130 |         #
131 |         #   Return a unit quaternion corresponding to the rotational part of the
132 |         #   homogeneous transform T.
133 | 
134 |         qs = sqrt(trace(t) + 1) / 2.0
135 |         kx = t[2, 1] - t[1, 2]  # Oz - Ay
136 |         ky = t[0, 2] - t[2, 0]  # Ax - Nz
137 |         kz = t[1, 0] - t[0, 1]  # Ny - Ox
138 | 
139 |         if (t[0, 0] >= t[1, 1]) and (t[0, 0] >= t[2, 2]):
140 |             kx1 = t[0, 0] - t[1, 1] - t[2, 2] + 1  # Nx - Oy - Az + 1
141 |             ky1 = t[1, 0] + t[0, 1]  # Ny + Ox
142 |             kz1 = t[2, 0] + t[0, 2]  # Nz + Ax
143 |             add = (kx >= 0)
144 |         elif (t[1, 1] >= t[2, 2]):
145 |             kx1 = t[1, 0] + t[0, 1]  # Ny + Ox
146 |             ky1 = t[1, 1] - t[0, 0] - t[2, 2] + 1  # Oy - Nx - Az + 1
147 |             kz1 = t[2, 1] + t[1, 2]  # Oz + Ay
148 |             add = (ky >= 0)
149 |         else:
150 |             kx1 = t[2, 0] + t[0, 2]  # Nz + Ax
151 |             ky1 = t[2, 1] + t[1, 2]  # Oz + Ay
152 |             kz1 = t[2, 2] - t[0, 0] - t[1, 1] + 1  # Az - Nx - Oy + 1
153 |             add = (kz >= 0)
154 | 
155 |         if add:
156 |             kx = kx + kx1
157 |             ky = ky + ky1
158 |             kz = kz + kz1
159 |         else:
160 |             kx = kx - kx1
161 |             ky = ky - ky1
162 |             kz = kz - kz1
163 | 
164 |         kv = matrix([kx, ky, kz])
165 |         nm = linalg.norm(kv)
166 |         if nm == 0:
167 |             self.s = 1.0
168 |             self.v = matrix([0.0, 0.0, 0.0])
169 | 
170 |         else:
171 |             self.s = qs
172 |             self.v = (sqrt(1 - qs ** 2) / nm) * kv
173 | 
174 |     ############### OPERATORS #########################################
175 |     # PLUS Add two quaternion objects
176 |     #
177 |     # Invoked by the + operator
178 |     #
179 |     # q1+q2 standard quaternion addition
180 |     def __add__(self, q):
181 |         '''
182 |         Return a new quaternion that is the element-wise sum of the operands.
183 |         '''
184 |         if isinstance(q, quaternion):
185 |             qr = quaternion()
186 |             qr.s = 0
187 | 
188 |             qr.s = self.s + q.s
189 |             qr.v = self.v + q.v
190 | 
191 |             return qr
192 |         else:
193 |             raise ValueError
194 | 
195 |     # MINUS Subtract two quaternion objects
196 |     #
197 |     # Invoked by the - operator
198 |     #
199 |     # q1-q2 standard quaternion subtraction
200 | 
201 |     def __sub__(self, q):
202 |         '''
203 |         Return a new quaternion that is the element-wise difference of the operands.
204 |         '''
205 |         if isinstance(q, quaternion):
206 |             qr = quaternion()
207 |             qr.s = 0
208 | 
209 |             qr.s = self.s - q.s
210 |             qr.v = self.v - q.v
211 | 
212 |             return qr
213 |         else:
214 |             raise ValueError
215 | 
216 |     # q * q  or q * const
217 |     def __mul__(self, q2):
218 |         '''
219 |         Quaternion product. Several cases are handled
220 | 
221 |             - q * q   quaternion multiplication
222 |             - q * c   element-wise multiplication by constant
223 |             - q * v   quaternion-vector multiplication q * v * q.inv();
224 |         '''
225 |         qr = quaternion();
226 | 
227 |         if isinstance(q2, quaternion):
228 | 
229 |             # Multiply unit-quaternion by unit-quaternion
230 |             #
231 |             #   QQ = qqmul(Q1, Q2)
232 | 
233 |             # decompose into scalar and vector components
234 |             s1 = self.s;
235 |             v1 = self.v
236 |             s2 = q2.s;
237 |             v2 = q2.v
238 | 
239 |             # form the product
240 |             qr.s = s1 * s2 - v1 * v2.T
241 |             qr.v = s1 * v2 + s2 * v1 + cross(v1, v2)
242 | 
243 |         elif type(q2) is matrix:
244 | 
245 |             # Multiply vector by unit-quaternion
246 |             #
247 |             #   Rotate the vector V by the unit-quaternion Q.
248 | 
249 |             if q2.shape == (1, 3) or q2.shape == (3, 1):
250 |                 qr = self * quaternion(q2) * self.inv()
251 |                 return qr.v;
252 |             else:
253 |                 raise ValueError;
254 | 
255 |         else:
256 |             qr.s = self.s * q2
257 |             qr.v = self.v * q2
258 | 
259 |         return qr
260 | 
261 |     def __rmul__(self, c):
262 |         '''
263 |         Quaternion product. Several cases are handled
264 | 
265 |             - c * q   element-wise multiplication by constant
266 |         '''
267 |         qr = quaternion()
268 |         qr.s = self.s * c
269 |         qr.v = self.v * c
270 | 
271 |         return qr
272 | 
273 |     def __imul__(self, x):
274 |         '''
275 |         Quaternion in-place multiplication
276 | 
277 |             - q *= q2
278 | 
279 |         '''
280 | 
281 |         if isinstance(x, quaternion):
282 |             s1 = self.s;
283 |             v1 = self.v
284 |             s2 = x.s
285 |             v2 = x.v
286 | 
287 |             # form the product
288 |             self.s = s1 * s2 - v1 * v2.T
289 |             self.v = s1 * v2 + s2 * v1 + cross(v1, v2)
290 | 
291 |         elif isscalar(x):
292 |             self.s *= x;
293 |             self.v *= x;
294 | 
295 |         return self;
296 | 
297 |     def __div__(self, q):
298 |         '''Return quaternion quotient.  Several cases handled:
299 |             - q1 / q2      quaternion division implemented as q1 * q2.inv()
300 |             - q1 / c       element-wise division
301 |         '''
302 |         if isinstance(q, quaternion):
303 |             qr = quaternion()
304 |             qr = self * q.inv()
305 |         elif isscalar(q):
306 |             qr.s = self.s / q
307 |             qr.v = self.v / q
308 | 
309 |         return qr
310 | 
311 |     def __pow__(self, p):
312 |         '''
313 |         Quaternion exponentiation.  Only integer exponents are handled.  Negative
314 |         integer exponents are supported.
315 |         '''
316 | 
317 |         # check that exponent is an integer
318 |         if not isinstance(p, int):
319 |             raise ValueError
320 | 
321 |         qr = quaternion()
322 |         q = quaternion(self);
323 | 
324 |         # multiply by itself so many times
325 |         for i in range(0, abs(p)):
326 |             qr *= q
327 | 
328 |         # if exponent was negative, invert it
329 |         if p < 0:
330 |             qr = qr.inv()
331 | 
332 |         return qr
333 | 
334 |     def copy(self):
335 |         """
336 |         Return a copy of the quaternion.
337 |         """
338 |         return copy.copy(self);
339 | 
340 |     def inv(self):
341 |         """Return the inverse.
342 | 
343 |         @rtype: quaternion
344 |         @return: the inverse
345 |         """
346 | 
347 |         qi = quaternion(self);
348 |         qi.v = -qi.v;
349 | 
350 |         return qi;
351 | 
352 |     def norm(self):
353 |         """Return the norm of this quaternion.
354 | 
355 |         @rtype: number
356 |         @return: the norm
357 |         """
358 | 
359 |         return linalg.norm(self.double())
360 | 
361 |     def double(self):
362 |         """Return the quaternion as 4-element vector.
363 | 
364 |         @rtype: 4-vector
365 |         @return: the quaternion elements
366 |         """
367 |         return concatenate((mat(self.s), self.v), 1)
368 | 
369 |     def unit(self):
370 |         """Return an equivalent unit quaternion
371 | 
372 |         @rtype: quaternion
373 |         @return: equivalent unit quaternion
374 |         """
375 | 
376 |         qr = quaternion()
377 |         nm = self.norm()
378 | 
379 |         qr.s = self.s / nm
380 |         qr.v = self.v / nm
381 | 
382 |         return qr
383 | 
384 |     def tr(self):
385 |         """Return an equivalent rotation matrix.
386 | 
387 |         @rtype: 4x4 homogeneous transform
388 |         @return: equivalent rotation matrix
389 |         """
390 | 
391 |         return T.r2t(self.r())
392 | 
393 |     def r(self):
394 |         """Return an equivalent rotation matrix.
395 | 
396 |         @rtype: 3x3 orthonormal rotation matrix
397 |         @return: equivalent rotation matrix
398 |         """
399 | 
400 |         s = self.s
401 |         x = self.v[0, 0]
402 |         y = self.v[0, 1]
403 |         z = self.v[0, 2]
404 | 
405 |         return matrix([[1 - 2 * (y ** 2 + z ** 2), 2 * (x * y - s * z), 2 * (x * z + s * y)],
406 |                        [2 * (x * y + s * z), 1 - 2 * (x ** 2 + z ** 2), 2 * (y * z - s * x)],
407 |                        [2 * (x * z - s * y), 2 * (y * z + s * x), 1 - 2 * (x ** 2 + y ** 2)]])
408 | 
409 |     # QINTERP Interpolate rotations expressed by quaternion objects
410 |     #
411 |     #   QI = qinterp(Q1, Q2, R)
412 |     #
413 |     # Return a unit-quaternion that interpolates between Q1 and Q2 as R moves
414 |     # from 0 to 1.  This is a spherical linear interpolation (slerp) that can
415 |     # be interpretted as interpolation along a great circle arc on a sphere.
416 |     #
417 |     # If r is a vector, QI, is a cell array of quaternions, each element
418 |     # corresponding to sequential elements of R.
419 |     #
420 |     # See also: CTRAJ, QUATERNION.
421 | 
422 |     # MOD HISTORY
423 |     # 2/99 convert to use of objects
424 |     # $Log: qinterp.m,v $
425 |     # Revision 1.3  2002/04/14 11:02:54  pic
426 |     # Changed see also line.
427 |     #
428 |     # Revision 1.2  2002/04/01 12:06:48  pic
429 |     # General tidyup, help comments, copyright, see also, RCS keys.
430 |     #
431 |     # $Revision: 1.3 $
432 |     #
433 |     # Copyright (C) 1999-2002, by Peter I. Corke
434 | 
435 |     def interp(Q1, Q2, r):
436 |         q1 = Q1.double()
437 |         q2 = Q2.double()
438 | 
439 |         theta = arccos(q1 * q2.T)
440 |         q = []
441 |         count = 0
442 | 
443 |         if isscalar(r):
444 |             if r < 0 or r > 1:
445 |                 raise 'R out of range'
446 |             if theta == 0:
447 |                 q = quaternion(Q1)
448 |             else:
449 |                 q = quaternion((sin((1 - r) * theta) * q1 + sin(r * theta) * q2) / sin(theta))
450 |         else:
451 |             for R in r:
452 |                 if theta == 0:
453 |                     qq = Q1
454 |                 else:
455 |                     qq = quaternion((sin((1 - R) * theta) * q1 + sin(R * theta) * q2) / sin(theta))
456 |                 q.append(qq)
457 |         return q
458 | 
459 | 


--------------------------------------------------------------------------------
/ROS_UR5_CIRCLE_V0/README.md:
--------------------------------------------------------------------------------
 1 | # CSDN_BLOG_CODE
 2 | ### 1,另外三个属于函数库可直接调用,有的是来自Peter Cork有的是我自己添加进去的,用的时候自己选择。三个文件分别为：
 3 | 
 4 | 
 5 | `Quaternion.py,transfer.py,utility.py`
 6 | 
 7 | 
 8 | ### No.1 There are three py-scripts which are used for 
 9 | function,You can just use them or copy to your project. Some
10 | functions based on Peter Cork's LIB, Some are added by myself, 
11 | You must choose the right one to use. If you have any question,I 
12 | would like to answer it if I have time.The code are followed below 
13 | respectively.
14 | 
15 | 
16 | `Quaternion.py,transfer.py,utility.py`
17 | 
18 | 
19 | ### 2,ur5_pose_get.py 这个文件也可以当成功能函数使用，主要可以自动根据joint_states的数据来解析获取当前UR5的6个关节角。
20 | 
21 | 
22 | ### No.2, The script named "ur5_pose_get.py" used for getting UR5 joint angular automaticly.


--------------------------------------------------------------------------------
/ROS_UR5_CIRCLE_V0/UR5Script_move_circle.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | # -*- coding: utf-8 -*-
  3 | import rospy
  4 | import numpy
  5 | from std_msgs.msg import String
  6 | from frompitoangle import *
  7 | from ur5_kinematics import *
  8 | from ur5_pose_get import *
  9 | from transfer import *
 10 | 
 11 | class UrCircle:
 12 |     def __init__(self,weights,radius):
 13 |         self.weights = weights
 14 |         self.radius=radius#m
 15 |         self.cont=150
 16 |     def Init_node(self):
 17 |         rospy.init_node("move_ur5_circle")
 18 |         pub = rospy.Publisher("/ur_driver/URScript", String, queue_size=10)
 19 |         return pub
 20 |     def get_urobject_ur5kinetmatics(self):
 21 |         ur0 = Kinematic()
 22 |         return ur0
 23 |     def get_draw_circle_xy(self,t,xy_center_pos):
 24 |         x = xy_center_pos[0] + self.radius * math.cos( 2 * math.pi * t / self.cont )
 25 |         y = xy_center_pos[1] + self.radius * math.sin( 2 * math.pi * t / self.cont)
 26 |         return  [x,y]
 27 |     def get_IK_from_T(self,T,q_last):
 28 |         ur0 = self.get_urobject_ur5kinetmatics()
 29 |         return ur0.best_sol(self.weights,q_last,T)
 30 |     def get_q_list(self,T_list,qzero):
 31 |         ur0 = self.get_urobject_ur5kinetmatics()
 32 |         tempq=[]
 33 |         resultq=[]
 34 |         for i in xrange(self.cont):
 35 |             if i==0:
 36 |                 tempq=qzero
 37 |                 firstq = ur0.best_sol(self.weights, tempq, T_list[i])
 38 |                 tempq=firstq
 39 |                 resultq.append(firstq.tolist())
 40 |                 print "firstq", firstq
 41 |             else:
 42 |                 qq = ur0.best_sol(self.weights, tempq, T_list[i])
 43 |                 tempq=qq
 44 |                 print "num i qq",i,qq
 45 |                 resultq.append(tempq.tolist())
 46 |         return resultq
 47 |     # T is numpy.array
 48 |     def get_T_translation(self, T):
 49 |         trans_x = T[3]
 50 |         trans_y = T[7]
 51 |         trans_z = T[11]
 52 |         return [trans_x, trans_y, trans_z]
 53 |     def insert_new_xy(self,T,nx,ny):
 54 |         temp=[]
 55 |         for i in xrange(12):
 56 |             if i==3:
 57 |                 temp.append(nx)
 58 |             elif i==7:
 59 |                 temp.append(ny)
 60 |             else:
 61 |                 temp.append(T[i])
 62 |         return temp
 63 |     def get_new_T(self,InitiT,xy_center_pos):
 64 |         temp=[]
 65 |         for i in xrange(self.cont):
 66 |             new_xy=self.get_draw_circle_xy(i,xy_center_pos)
 67 |             new_T=self.insert_new_xy(InitiT,new_xy[0],new_xy[1])
 68 |             #print "initial T\n",InitiT
 69 |             #print "new_T\n",i,new_T
 70 |             temp.append(new_T)
 71 |         return temp
 72 |     def urscript_pub(self, pub, qq, vel, ace, t):
 73 | 
 74 |         ss = "movej([" + str(qq[0]) + "," + str(qq[1]) + "," + str(qq[2]) + "," + str(
 75 |             qq[3]) + "," + str(qq[4]) + "," + str(qq[5]) + "]," + "a=" + str(ace) + "," + "v=" + str(
 76 |             vel) + "," + "t=" + str(t) + ")"
 77 |         print("---------ss:", ss)
 78 |             # ss="movej([-0.09577000000000001, -1.7111255555555556, 0.7485411111111111, 0.9948566666666667, 1.330836666666667, 2.3684322222222223], a=1.0, v=1.0,t=5)"
 79 |         pub.publish(ss)
 80 | def main():
 81 |     t=0
 82 |     vel=0.2
 83 |     ace=50
 84 |     qstart=[133.70,-79.09,103.45,-111.83,-89.54,-212.29]
 85 |     # qq=[45.91,-72.37,61.52,-78.56,-90.49,33.71]
 86 |     # q=display(getpi(qq))
 87 |     ratet = 30
 88 |     radius=0.3
 89 |     weights = [1.] * 6
 90 |     T_list=[]
 91 |     urc=UrCircle(weights,radius)
 92 |     pub=urc.Init_node()
 93 |     rate = rospy.Rate(ratet)
 94 | 
 95 |     # first step go to initial pos
 96 |     q = display(getpi(qstart))
 97 |     # urc.urscript_pub(pub,q,vel,ace,t)
 98 |     # second get T use urkinematics
 99 |     urk = urc.get_urobject_ur5kinetmatics()
100 |     F_T = urk.Forward(q)
101 |     print "F_T", F_T
102 |     TransT = urc.get_T_translation(F_T)
103 |     print "TransT", TransT
104 |     xy_center_pos = [TransT[0], TransT[1]]
105 |     print "xy_center_pos", xy_center_pos
106 |     T_list = urc.get_new_T(F_T, xy_center_pos)
107 |     print "T_list", T_list
108 |     reslut_q = urc.get_q_list(T_list, q)
109 |     print "reslut_q", reslut_q
110 |     cn=0
111 |     while not rospy.is_shutdown():
112 |         urc.urscript_pub(pub, reslut_q[cn], vel, ace, t)
113 |         cn+=1
114 |         if cn==len(reslut_q):
115 |             cn=0
116 |         # print "cn-----\n",reslut_q[cn]
117 | 
118 |         rate.sleep()
119 | if __name__ == '__main__':
120 |         main()


--------------------------------------------------------------------------------
/ROS_UR5_CIRCLE_V0/demo/webwxgetvideo:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/linzhuyue/CSDN_BLOG_CODE/bef14ca6e3520227042070bdd713d3bdcf11a967/ROS_UR5_CIRCLE_V0/demo/webwxgetvideo


--------------------------------------------------------------------------------
/ROS_UR5_CIRCLE_V0/transfer.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | # -*- coding: utf-8 -*-
  3 | """
  4 | Primitive operations for 3x3 orthonormal and 4x4 homogeneous matrices.
  5 | 
  6 | Python implementation by: Luis Fernando Lara Tobar and Peter Corke.
  7 | Based on original Robotics Toolbox for Matlab code by Peter Corke.
  8 | Permission to use and copy is granted provided that acknowledgement of
  9 | the authors is made.
 10 | 
 11 | @author: Luis Fernando Lara Tobar and Peter Corke
 12 | """
 13 | 
 14 | from numpy import *
 15 | from utility import *
 16 | from numpy.linalg import norm
 17 | import Quaternion as Q
 18 | 
 19 | 
 20 | def rotx(theta):
 21 |     """
 22 |     Rotation about X-axis
 23 | 
 24 |     @type theta: number
 25 |     @param theta: the rotation angle
 26 |     @rtype: 3x3 orthonormal matrix
 27 |     @return: rotation about X-axis
 28 | 
 29 |     @see: L{roty}, L{rotz}, L{rotvec}
 30 |     """
 31 | 
 32 |     ct = cos(theta)
 33 |     st = sin(theta)
 34 |     return mat([[1, 0, 0],
 35 |                 [0, ct, -st],
 36 |                 [0, st, ct]])
 37 | 
 38 | 
 39 | def roty(theta):
 40 |     """
 41 |     Rotation about Y-axis
 42 | 
 43 |     @type theta: number
 44 |     @param theta: the rotation angle
 45 |     @rtype: 3x3 orthonormal matrix
 46 |     @return: rotation about Y-axis
 47 | 
 48 |     @see: L{rotx}, L{rotz}, L{rotvec}
 49 |     """
 50 | 
 51 |     ct = cos(theta)
 52 |     st = sin(theta)
 53 | 
 54 |     return mat([[ct, 0, st],
 55 |                 [0, 1, 0],
 56 |                 [-st, 0, ct]])
 57 | 
 58 | 
 59 | def rotz(theta):
 60 |     """
 61 |     Rotation about Z-axis
 62 | 
 63 |     @type theta: number
 64 |     @param theta: the rotation angle
 65 |     @rtype: 3x3 orthonormal matrix
 66 |     @return: rotation about Z-axis
 67 | 
 68 |     @see: L{rotx}, L{roty}, L{rotvec}
 69 |     """
 70 | 
 71 |     ct = cos(theta)
 72 |     st = sin(theta)
 73 | 
 74 |     return mat([[ct, -st, 0],
 75 |                 [st, ct, 0],
 76 |                 [0, 0, 1]])
 77 | 
 78 | 
 79 | def trotx(theta):
 80 |     """
 81 |     Rotation about X-axis
 82 | 
 83 |     @type theta: number
 84 |     @param theta: the rotation angle
 85 |     @rtype: 4x4 homogeneous matrix
 86 |     @return: rotation about X-axis
 87 | 
 88 |     @see: L{troty}, L{trotz}, L{rotx}
 89 |     """
 90 |     return r2t(rotx(theta))
 91 | 
 92 | 
 93 | def troty(theta):
 94 |     """
 95 |     Rotation about Y-axis
 96 | 
 97 |     @type theta: number
 98 |     @param theta: the rotation angle
 99 |     @rtype: 4x4 homogeneous matrix
100 |     @return: rotation about Y-axis
101 | 
102 |     @see: L{troty}, L{trotz}, L{roty}
103 |     """
104 |     return r2t(roty(theta))
105 | 
106 | 
107 | def trotz(theta):
108 |     """
109 |     Rotation about Z-axis
110 | 
111 |     @type theta: number
112 |     @param theta: the rotation angle
113 |     @rtype: 4x4 homogeneous matrix
114 |     @return: rotation about Z-axis
115 | 
116 |     @see: L{trotx}, L{troty}, L{rotz}
117 |     """
118 |     return r2t(rotz(theta))
119 | 
120 | 
121 | ##################### Euler angles
122 | 
123 | def tr2eul(m):
124 |     """
125 |     Extract Euler angles.
126 |     Returns a vector of Euler angles corresponding to the rotational part of
127 |     the homogeneous transform.  The 3 angles correspond to rotations about
128 |     the Z, Y and Z axes respectively.
129 | 
130 |     @type m: 3x3 or 4x4 matrix
131 |     @param m: the rotation matrix
132 |     @rtype: 1x3 matrix
133 |     @return: Euler angles [S{theta} S{phi} S{psi}]
134 | 
135 |     @see:  L{eul2tr}, L{tr2rpy}
136 |     """
137 | 
138 |     try:
139 |         m = mat(m)
140 |         if ishomog(m):
141 |             euler = mat(zeros((1, 3)))
142 |             if norm(m[0, 2]) < finfo(float).eps and norm(m[1, 2]) < finfo(float).eps:
143 |                 # singularity
144 |                 euler[0, 0] = 0
145 |                 sp = 0
146 |                 cp = 1
147 |                 euler[0, 1] = arctan2(cp * m[0, 2] + sp * m[1, 2], m[2, 2])
148 |                 euler[0, 2] = arctan2(-sp * m[0, 0] + cp * m[1, 0], -sp * m[0, 1] + cp * m[1, 1])
149 |                 return euler
150 |             else:
151 |                 euler[0, 0] = arctan2(m[1, 2], m[0, 2])
152 |                 sp = sin(euler[0, 0])
153 |                 cp = cos(euler[0, 0])
154 |                 euler[0, 1] = arctan2(cp * m[0, 2] + sp * m[1, 2], m[2, 2])
155 |                 euler[0, 2] = arctan2(-sp * m[0, 0] + cp * m[1, 0], -sp * m[0, 1] + cp * m[1, 1])
156 |                 return euler
157 | 
158 |     except ValueError:
159 |         euler = []
160 |         for i in range(0, len(m)):
161 |             euler.append(tr2eul(m[i]))
162 |         return euler
163 | 
164 | 
165 | def eul2r(phi, theta=None, psi=None):
166 |     """
167 |     Rotation from Euler angles.
168 | 
169 |     Two call forms:
170 |         - R = eul2r(S{theta}, S{phi}, S{psi})
171 |         - R = eul2r([S{theta}, S{phi}, S{psi}])
172 |     These correspond to rotations about the Z, Y, Z axes respectively.
173 | 
174 |     @type phi: number or list/array/matrix of angles
175 |     @param phi: the first Euler angle, or a list/array/matrix of angles
176 |     @type theta: number
177 |     @param theta: the second Euler angle
178 |     @type psi: number
179 |     @param psi: the third Euler angle
180 |     @rtype: 3x3 orthonormal matrix
181 |     @return: R([S{theta} S{phi} S{psi}])
182 | 
183 |     @see:  L{tr2eul}, L{eul2tr}, L{tr2rpy}
184 | 
185 |     """
186 | 
187 |     n = 1
188 |     if theta == None and psi == None:
189 |         # list/array/matrix argument
190 |         phi = mat(phi)
191 |         if numcols(phi) != 3:
192 |             error('bad arguments')
193 |         else:
194 |             n = numrows(phi)
195 |             psi = phi[:, 2]
196 |             theta = phi[:, 1]
197 |             phi = phi[:, 0]
198 |     elif (theta != None and psi == None) or (theta == None and psi != None):
199 |         error('bad arguments')
200 |     elif not isinstance(phi, (int, int32, float, float64)):
201 |         # all args are vectors
202 |         phi = mat(phi)
203 |         n = numrows(phi)
204 |         theta = mat(theta)
205 |         psi = mat(psi)
206 | 
207 |     if n > 1:
208 |         R = []
209 |         for i in range(0, n):
210 |             r = rotz(phi[i, 0]) * roty(theta[i, 0]) * rotz(psi[i, 0])
211 |             R.append(r)
212 |         return R
213 |     try:
214 |         r = rotz(phi[0, 0]) * roty(theta[0, 0]) * rotz(psi[0, 0])
215 |         return r
216 |     except:
217 |         r = rotz(phi) * roty(theta) * rotz(psi)
218 |         return r
219 | 
220 | 
221 | def eul2tr(phi, theta=None, psi=None):
222 |     """
223 |     Rotation from Euler angles.
224 | 
225 |     Two call forms:
226 |         - R = eul2tr(S{theta}, S{phi}, S{psi})
227 |         - R = eul2tr([S{theta}, S{phi}, S{psi}])
228 |     These correspond to rotations about the Z, Y, Z axes respectively.
229 | 
230 |     @type phi: number or list/array/matrix of angles
231 |     @param phi: the first Euler angle, or a list/array/matrix of angles
232 |     @type theta: number
233 |     @param theta: the second Euler angle
234 |     @type psi: number
235 |     @param psi: the third Euler angle
236 |     @rtype: 4x4 homogenous matrix
237 |     @return: R([S{theta} S{phi} S{psi}])
238 | 
239 |     @see:  L{tr2eul}, L{eul2r}, L{tr2rpy}
240 | 
241 |     """
242 |     return r2t(eul2r(phi, theta, psi))
243 | 
244 | 
245 | ################################## RPY angles
246 | 
247 | 
248 | def tr2rpy(m):
249 |     """
250 |     Extract RPY angles.
251 |     Returns a vector of RPY angles corresponding to the rotational part of
252 |     the homogeneous transform.  The 3 angles correspond to rotations about
253 |     the Z, Y and X axes respectively.
254 | 
255 |     @type m: 3x3 or 4x4 matrix
256 |     @param m: the rotation matrix
257 |     @rtype: 1x3 matrix
258 |     @return: RPY angles [S{theta} S{phi} S{psi}]
259 | 
260 |     @see:  L{rpy2tr}, L{tr2eul}
261 |     """
262 |     try:
263 |         m = mat(m)
264 |         if ishomog(m):
265 |             rpy = mat(zeros((1, 3)))
266 |             if norm(m[0, 0]) < finfo(float).eps and norm(m[1, 0]) < finfo(float).eps:
267 |                 # singularity
268 |                 rpy[0, 0] = 0
269 |                 rpy[0, 1] = arctan2(-m[2, 0], m[0, 0])
270 |                 rpy[0, 2] = arctan2(-m[1, 2], m[1, 1])
271 |                 return rpy
272 |             else:
273 |                 rpy[0, 0] = arctan2(m[1, 0], m[0, 0])
274 |                 sp = sin(rpy[0, 0])
275 |                 cp = cos(rpy[0, 0])
276 |                 rpy[0, 1] = arctan2(-m[2, 0], cp * m[0, 0] + sp * m[1, 0])
277 |                 rpy[0, 2] = arctan2(sp * m[0, 2] - cp * m[1, 2], cp * m[1, 1] - sp * m[0, 1])
278 |                 return rpy
279 | 
280 |     except ValueError:
281 |         rpy = []
282 |         for i in range(0, len(m)):
283 |             rpy.append(tr2rpy(m[i]))
284 |         return rpy
285 | 
286 | 
287 | def rpy2r(roll, pitch=None, yaw=None):
288 |     """
289 |     Rotation from RPY angles.
290 | 
291 |     Two call forms:
292 |         - R = rpy2r(S{theta}, S{phi}, S{psi})
293 |         - R = rpy2r([S{theta}, S{phi}, S{psi}])
294 |     These correspond to rotations about the Z, Y, X axes respectively.
295 | 
296 |     @type roll: number or list/array/matrix of angles
297 |     @param roll: roll angle, or a list/array/matrix of angles
298 |     @type pitch: number
299 |     @param pitch: pitch angle
300 |     @type yaw: number
301 |     @param yaw: yaw angle
302 |     @rtype: 4x4 homogenous matrix
303 |     @return: R([S{theta} S{phi} S{psi}])
304 | 
305 |     @see:  L{tr2rpy}, L{rpy2r}, L{tr2eul}
306 | 
307 |     """
308 |     n = 1
309 |     if pitch == None and yaw == None:
310 |         roll = mat(roll)
311 |         if numcols(roll) != 3:
312 |             error('bad arguments')
313 |         n = numrows(roll)
314 |         pitch = roll[:, 1]
315 |         yaw = roll[:, 2]
316 |         roll = roll[:, 0]
317 |     if n > 1:
318 |         R = []
319 |         for i in range(0, n):
320 |             r = rotz(roll[i, 0]) * roty(pitch[i, 0]) * rotx(yaw[i, 0])
321 |             R.append(r)
322 |         return R
323 |     try:
324 |         r = rotz(roll[0, 0]) * roty(pitch[0, 0]) * rotx(yaw[0, 0])
325 |         return r
326 |     except:
327 |         r = rotz(roll) * roty(pitch) * rotx(yaw)
328 |         return r
329 | 
330 | 
331 | def rpy2tr(roll, pitch=None, yaw=None):
332 |     """
333 |     Rotation from RPY angles.
334 | 
335 |     Two call forms:
336 |         - R = rpy2tr(r, p, y)
337 |         - R = rpy2tr([r, p, y])
338 |     These correspond to rotations about the Z, Y, X axes respectively.
339 | 
340 |     @type roll: number or list/array/matrix of angles
341 |     @param roll: roll angle, or a list/array/matrix of angles
342 |     @type pitch: number
343 |     @param pitch: pitch angle
344 |     @type yaw: number
345 |     @param yaw: yaw angle
346 |     @rtype: 4x4 homogenous matrix
347 |     @return: R([S{theta} S{phi} S{psi}])
348 | 
349 |     @see:  L{tr2rpy}, L{rpy2r}, L{tr2eul}
350 | 
351 |     """
352 |     return r2t(rpy2r(roll, pitch, yaw))
353 | 
354 | 
355 | ###################################### OA vector form
356 | 
357 | 
358 | def oa2r(o, a):
359 |     """Rotation from 2 vectors.
360 |     The matrix is formed from 3 vectors such that::
361 |         R = [N O A] and N = O x A.
362 | 
363 |     In robotics A is the approach vector, along the direction of the robot's
364 |     gripper, and O is the orientation vector in the direction between the
365 |     fingertips.
366 | 
367 |     The submatrix is guaranteed to be orthonormal so long as O and A are
368 |     not parallel.
369 | 
370 |     @type o: 3-vector
371 |     @param o: The orientation vector.
372 |     @type a: 3-vector
373 |     @param a: The approach vector
374 |     @rtype: 3x3 orthonormal rotation matrix
375 |     @return: Rotatation matrix
376 | 
377 |     @see: L{rpy2r}, L{eul2r}
378 |     """
379 |     n = crossp(o, a)
380 |     n = unit(n)
381 |     o = crossp(a, n);
382 |     o = unit(o).reshape(3, 1)
383 |     a = unit(a).reshape(3, 1)
384 |     return bmat('n o a')
385 | 
386 | 
387 | def oa2tr(o, a):
388 |     """otation from 2 vectors.
389 |     The rotation submatrix is formed from 3 vectors such that::
390 | 
391 |         R = [N O A] and N = O x A.
392 | 
393 |     In robotics A is the approach vector, along the direction of the robot's
394 |     gripper, and O is the orientation vector in the direction between the
395 |     fingertips.
396 | 
397 |     The submatrix is guaranteed to be orthonormal so long as O and A are
398 |     not parallel.
399 | 
400 |     @type o: 3-vector
401 |     @param o: The orientation vector.
402 |     @type a: 3-vector
403 |     @param a: The approach vector
404 |     @rtype: 4x4 homogeneous transformation matrix
405 |     @return: Transformation matrix
406 | 
407 |     @see: L{rpy2tr}, L{eul2tr}
408 |     """
409 |     return r2t(oa2r(o, a))
410 | 
411 | 
412 | ###################################### angle/vector form
413 | 
414 | 
415 | def rotvec2r(theta, v):
416 |     """
417 |     Rotation about arbitrary axis.  Compute a rotation matrix representing
418 |     a rotation of C{theta} about the vector C{v}.
419 | 
420 |     @type v: 3-vector
421 |     @param v: rotation vector
422 |     @type theta: number
423 |     @param theta: the rotation angle
424 |     @rtype: 3x3 orthonormal matrix
425 |     @return: rotation
426 | 
427 |     @see: L{rotx}, L{roty}, L{rotz}
428 |     """
429 |     v = arg2array(v);
430 |     ct = cos(theta)
431 |     st = sin(theta)
432 |     vt = 1 - ct
433 |     r = mat([[ct, -v[2] * st, v[1] * st], \
434 |              [v[2] * st, ct, -v[0] * st], \
435 |              [-v[1] * st, v[0] * st, ct]])
436 |     return v * v.T * vt + r
437 | 
438 | 
439 | def rotvec2tr(theta, v):
440 |     """
441 |     Rotation about arbitrary axis.  Compute a rotation matrix representing
442 |     a rotation of C{theta} about the vector C{v}.
443 | 
444 |     @type v: 3-vector
445 |     @param v: rotation vector
446 |     @type theta: number
447 |     @param theta: the rotation angle
448 |     @rtype: 4x4 homogeneous matrix
449 |     @return: rotation
450 | 
451 |     @see: L{trotx}, L{troty}, L{trotz}
452 |     """
453 |     return r2t(rotvec2r(theta, v))
454 | 
455 | 
456 | ###################################### translational transform
457 | 
458 | 
459 | # def transl(x, y=None, z=None):
460 | #     """
461 | #     Create or decompose translational homogeneous transformations.
462 | #
463 | #     Create a homogeneous transformation
464 | #     ===================================
465 | #
466 | #         - T = transl(v)
467 | #         - T = transl(vx, vy, vz)
468 | #
469 | #         The transformation is created with a unit rotation submatrix.
470 | #         The translational elements are set from elements of v which is
471 | #         a list, array or matrix, or from separate passed elements.
472 | #
473 | #     Decompose a homogeneous transformation
474 | #     ======================================
475 | #
476 | #
477 | #         - v = transl(T)
478 | #
479 | #         Return the translation vector
480 | #     """
481 | #
482 | #     if y == None and z == None:
483 | #         x = mat(x)
484 | #         try:
485 | #             if ishomog(x):
486 | #                 return x[0:3, 3].reshape(3, 1)
487 | #             else:
488 | #                 return concatenate((concatenate((eye(3), x.reshape(3, 1)), 1), mat([0, 0, 0, 1])))
489 | #         except AttributeError:
490 | #             n = len(x)
491 | #             r = [[], [], []]
492 | #             for i in range(n):
493 | #                 r = concatenate((r, x[i][0:3, 3]), 1)
494 | #             return r
495 | #     elif y != None and z != None:
496 | #         return concatenate((concatenate((eye(3), mat([x, y, z]).T), 1), mat([0, 0, 0, 1])))
497 | 
498 | def transl(x, y=None, z=None):
499 |     """
500 |     Create or decompose translational homogeneous transformations.
501 | 
502 |     Create a homogeneous transformation
503 |     ===================================
504 | 
505 |         - T = transl(v)
506 |         - T = transl(vx, vy, vz)
507 | 
508 |         The transformation is created with a unit rotation submatrix.
509 |         The translational elements are set from elements of v which is
510 |         a list, array or matrix, or from separate passed elements.
511 | 
512 |     Decompose a homogeneous transformation
513 |     ======================================
514 | 
515 | 
516 |         - v = transl(T)
517 | 
518 |         Return the translation vector
519 |     """
520 | 
521 |     if y == None and z == None:
522 |         x = mat(x)
523 |         try:
524 |             if ishomog(x):
525 |                 return x[0:3, 3].reshape(3, 1)
526 |             else:
527 |                 return concatenate((concatenate((eye(3), x.reshape(3, 1)), 1), mat([0, 0, 0, 1])))
528 |         except AttributeError:
529 |             n = len(x)
530 |             r = [[], [], []]
531 |             for i in range(n):
532 |                 r = concatenate((r, x[i][0:3, 3]), 1)
533 |             return r
534 |     elif y != None and z != None:
535 |         return concatenate((concatenate((eye(3), mat([x, y, z]).T), 1), mat([0, 0, 0, 1])))
536 | 
537 | 
538 | # ###################################### Skew symmetric transform
539 | #
540 | #
541 | def skew(*args):
542 |     """
543 |     Convert to/from skew-symmetric form.  A skew symmetric matrix is a matrix
544 |     such that M = -M'
545 | 
546 |     Two call forms
547 | 
548 |         -ss = skew(v)
549 |         -v = skew(ss)
550 | 
551 |     The first form builds a 3x3 skew-symmetric from a 3-element vector v.
552 |     The second form takes a 3x3 skew-symmetric matrix and returns the 3 unique
553 |     elements that it contains.
554 | 
555 |     """
556 | 
557 |     def ss(b):
558 |         return matrix([
559 |             [0, -b[2], b[1]],
560 |             [b[2], 0, -b[0]],
561 |             [-b[1], b[0], 0]]);
562 | 
563 |     if len(args) == 1:
564 |         # convert matrix to skew vector
565 |         b = args[0];
566 | 
567 |         if isrot(b):
568 |             return 0.5 * matrix([b[2, 1] - b[1, 2], b[0, 2] - b[2, 0], b[1, 0] - b[0, 1]]);
569 |         elif ishomog(b):
570 |             return vstack((b[0:3, 3], 0.5 * matrix([b[2, 1] - b[1, 2], b[0, 2] - b[2, 0], b[1, 0] - b[0, 1]]).T));
571 | 
572 |         # build skew-symmetric matrix
573 | 
574 |         b = arg2array(b);
575 |         if len(b) == 3:
576 |             return ss(b);
577 |         elif len(b) == 6:
578 |             r = hstack((ss(b[3:6]), mat(b[0:3]).T));
579 |             r = vstack((r, mat([0, 0, 0, 1])));
580 |             return r;
581 | 
582 |     elif len(args) == 3:
583 |         return ss(args);
584 |     elif len(args) == 6:
585 |         r = hstack((ss(args[3:6]), mat(args[0:3]).T));
586 |         r = vstack((r, mat([0, 0, 0, 1])));
587 |         return r;
588 |     else:
589 |         raise ValueError;
590 | 
591 | 
592 | def tr2diff(t1, t2):
593 |     """
594 |     Convert a transform difference to differential representation.
595 |     Returns the 6-element differential motion required to move
596 |     from T1 to T2 in base coordinates.
597 | 
598 |     @type t1: 4x4 homogeneous transform
599 |     @param t1: Initial value
600 |     @type t2: 4x4 homogeneous transform
601 |     @param t2: Final value
602 |     @rtype: 6-vector
603 |     @return: Differential motion [dx dy dz drx dry drz]
604 |     @see: L{skew}
605 |     """
606 | 
607 |     t1 = mat(t1)
608 |     t2 = mat(t2)
609 | 
610 |     d = concatenate(
611 |         (t2[0:3, 3] - t1[0:3, 3],
612 |          0.5 * (crossp(t1[0:3, 0], t2[0:3, 0]) +
613 |                 crossp(t1[0:3, 1], t2[0:3, 1]) +
614 |                 crossp(t1[0:3, 2], t2[0:3, 2]))
615 |          ))
616 |     return d
617 | 
618 | 
619 | ################################## Utility
620 | 
621 | 
622 | def trinterp(T0, T1, r):
623 |     """
624 |     Interpolate homogeneous transformations.
625 |     Compute a homogeneous transform interpolation between C{T0} and C{T1} as
626 |     C{r} varies from 0 to 1 such that::
627 | 
628 |         trinterp(T0, T1, 0) = T0
629 |         trinterp(T0, T1, 1) = T1
630 | 
631 |     Rotation is interpolated using quaternion spherical linear interpolation.
632 | 
633 |     @type T0: 4x4 homogeneous transform
634 |     @param T0: Initial value
635 |     @type T1: 4x4 homogeneous transform
636 |     @param T1: Final value
637 |     @type r: number
638 |     @param r: Interpolation index, in the range 0 to 1 inclusive
639 |     @rtype: 4x4 homogeneous transform
640 |     @return: Interpolated value
641 |     @see: L{quaternion}, L{ctraj}
642 |     """
643 | 
644 |     q0 = Q.quaternion(T0)
645 |     q1 = Q.quaternion(T1)
646 |     p0 = transl(T0)
647 |     p1 = transl(T1)
648 | 
649 |     qr = q0.interp(q1, r)
650 |     pr = p0 * (1 - r) + r * p1
651 | 
652 |     return vstack((concatenate((qr.r(), pr), 1), mat([0, 0, 0, 1])))
653 | 
654 | 
655 | def trnorm(t):
656 |     """
657 |     Normalize a homogeneous transformation.
658 |     Finite word length arithmetic can cause transforms to become `unnormalized',
659 |     that is the rotation submatrix is no longer orthonormal (det(R) != 1).
660 | 
661 |     The rotation submatrix is re-orthogonalized such that the approach vector
662 |     (third column) is unchanged in direction::
663 | 
664 |         N = O x A
665 |         O = A x N
666 | 
667 |     @type t: 4x4 homogeneous transformation
668 |     @param t: the transform matrix to convert
669 |     @rtype: 3x3 orthonormal rotation matrix
670 |     @return: rotation submatrix
671 |     @see: L{oa2tr}
672 |     @bug: Should work for 3x3 matrix as well.
673 |     """
674 | 
675 |     t = mat(t)  # N O A
676 |     n = crossp(t[0:3, 1], t[0:3, 2])  # N = O X A
677 |     o = crossp(t[0:3, 2], t[0:3, 0])  # O = A x N
678 |     return concatenate((concatenate((unit(n), unit(t[0:3, 1]), unit(t[0:3, 2]), t[0:3, 3]), 1),
679 |                         mat([0, 0, 0, 1])))
680 | 
681 | 
682 | def t2r(T):
683 |     """
684 |     Return rotational submatrix of a homogeneous transformation.
685 |     @type T: 4x4 homogeneous transformation
686 |     @param T: the transform matrix to convert
687 |     @rtype: 3x3 orthonormal rotation matrix
688 |     @return: rotation submatrix
689 |     """
690 | 
691 |     if ishomog(T) == False:
692 |         error('input must be a homogeneous transform')
693 |     return T[0:3, 0:3]
694 | 
695 | 
696 | def r2t(R):
697 |     """
698 |     Convert a 3x3 orthonormal rotation matrix to a 4x4 homogeneous transformation::
699 | 
700 |         T = | R 0 |
701 |             | 0 1 |
702 | 
703 |     @type R: 3x3 orthonormal rotation matrix
704 |     @param R: the rotation matrix to convert
705 |     @rtype: 4x4 homogeneous matrix
706 |     @return: homogeneous equivalent
707 |     """
708 | 
709 |     return concatenate((concatenate((R, zeros((3, 1))), 1), mat([0, 0, 0, 1])))
710 | 
711 | def tr2delta(Tstar,Tt):#
712 |     """
713 |     Caculate two homogeneous matrix delta:
714 |             delta = | tt-tstar |
715 |                      | vex(Rt（RstarT-I3x3) |
716 |     @type Tstar: expection 4x4 homogeneous rotation matrix
717 |     @param Tt: real time 4x4 homogeneous rotation matrix(numpy)
718 |     @rtype: 4x4 homogeneous matrix
719 |     @return: 6x1 matrix
720 |     """
721 |     pstar = transl(Tstar)
722 |     pt = transl(Tt)
723 |     part1=pt-pstar
724 |     Rt=t2r(Tt)
725 |     Rstar=t2r(Tstar)
726 |     part2=skew(dot(Rt,Rstar.T)-eye(3))
727 | 
728 |     return column_stack((part1.reshape(1,3),part2.reshape(1,3)))
729 | 
730 | 


--------------------------------------------------------------------------------
/ROS_UR5_CIRCLE_V0/ur5_pose_get.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | import rospy
  3 | from std_msgs.msg import String
  4 | # from ar_track_alvar_msgs.msg import AlvarMarkers
  5 | #from std_msgs.msg import Empty
  6 | from sensor_msgs.msg import JointState
  7 | from frompitoangle import *
  8 | import os, time
  9 | import sys
 10 | 
 11 | class Urposition():
 12 | 
 13 |     def __init__(self, name = "ur_info_subscriber" ):
 14 | 
 15 |         self.name = name
 16 |         # self.pos_dict = {}
 17 |         self.ur_pose_buff_list = []
 18 |         self.ave_ur_pose = []
 19 |         self.now_ur_pos = []
 20 |         self.tmp_sum = [0]*6
 21 | 
 22 |         # pass
 23 | 
 24 |     def Init_node(self):
 25 |         rospy.init_node(self.name)
 26 |         sub = rospy.Subscriber("/joint_states", JointState, self.callback)
 27 |         return sub
 28 | 
 29 |     def urscript_pub(self, pub, qq, vel, ace, t, rate):
 30 | 
 31 |         rate = rospy.Rate(rate)
 32 | 
 33 |         ss = "movej([" + str(qq[0]) + "," + str(qq[1]) + "," + str(qq[2]) + "," + str(
 34 |             qq[3]) + "," + str(qq[4]) + "," + str(qq[5]) + "]," + "a=" + str(ace) + "," + "v=" + str(
 35 |             vel) + "," + "t=" + str(t) + ")"
 36 |         print("---------ss:", ss)
 37 |             # ss="movej([-0.09577000000000001, -1.7111255555555556, 0.7485411111111111, 0.9948566666666667, 1.330836666666667, 2.3684322222222223], a=1.0, v=1.0,t=5)"
 38 |         pub.publish(ss)
 39 | 
 40 |         rate.sleep()
 41 |                 # rostopic pub /ur_driver/URScript std_msgs/String "movej([-0.09577000000000001, -1.7111255555555556, 0.7485411111111111, 0.9948566666666667, 1.330836666666667, 2.3684322222222223], a=1.4, v=1.0)"
 42 |         # time.sleep(3)
 43 | 
 44 | 
 45 | 
 46 |     def callback(self, msg):
 47 |         self.read_pos_from_ur_joint( msg )
 48 |         self.ur_pose_buff_list, self.ave_ur_pose = self.pos_filter_ur( self.ur_pose_buff_list, self.now_ur_pos )
 49 | 
 50 | 
 51 |     def read_pos_from_ur_joint(self, msg):
 52 |         self.now_ur_pos = list( msg.position )
 53 |         # print("pos_msg:", pos_msg)
 54 |         # ur_pose = [ pos_msg.x , pos_msg.y, pos_msg.z, quaternion_msg.x, quaternion_msg.y, quaternion_msg.w  ]
 55 |         # return pos_msg
 56 | 
 57 |     def pos_filter_ur(self, pos_buff, new_data ):
 58 |         # tmp_sum = [0]*6
 59 |         ave_ur_pose = [0]*6
 60 |         if len( pos_buff ) == 10 :
 61 |             # print("new_data:", new_data)
 62 |             # print("tmp_sum before:", tmp_sum)
 63 |             # print("pos_buff[0]:", pos_buff[0])
 64 |             res1 = list_element_minus( self.tmp_sum , pos_buff[0] )
 65 |             self.tmp_sum = list_element_plus( res1 , new_data )
 66 |             # print("----------res1:", res1)
 67 |             # print("----------tmp_sum after:", tmp_sum)
 68 |             pos_buff = pos_buff[1:]
 69 |             pos_buff.append(new_data)
 70 |             ave_ur_pose = list_element_multiple( self.tmp_sum, 1.0/10 )
 71 |             # print( "len:", len( pos_buff ))
 72 |             # print ("10----ave_pos_ur:", ave_ur_pose)
 73 |             # time.sleep(2)
 74 |             # sys.exit(0)
 75 |         else:
 76 |             pos_buff.append(new_data)
 77 |             self.tmp_sum = list_element_plus( self.tmp_sum, new_data )
 78 |             ave_ur_pose = pos_buff[-1] # get the last element
 79 |             # print ("----------tmp_sum:", self.tmp_sum)
 80 |         # pos_buff.append( new_data )
 81 |         # print("---------------len:", pos_buff)
 82 |         return pos_buff, ave_ur_pose
 83 | 
 84 | 
 85 | 
 86 | def list_element_plus( v1, v2):
 87 |     res = list(map( lambda x: x[0] + x[1] , zip(v1,v2)))
 88 |     # print "plus :", res
 89 |     return res
 90 | 
 91 | def list_element_minus( v1, v2):
 92 |     res = list(map( lambda x: x[0] - x[1] , zip(v1,v2)))
 93 |     # print "minus :", res
 94 |     return res
 95 | 
 96 | def list_element_multiple( v1, num ):
 97 |     return [ item * num  for item in v1 ]
 98 | 
 99 | 
100 | def main():
101 |     ur_info_reader = Urposition()
102 |     ur_info_reader.Init_node()
103 |     while not rospy.is_shutdown():
104 |         pass
105 |         # print ( "now_pos: ", type(ur_info_reader.now_ur_pos))
106 | 
107 |         # print ("ave_pos_ur:", ur_info_reader.ave_ur_pose)
108 |     # rospy.spin()
109 | 
110 | 
111 | if __name__ == "__main__":
112 |     # v1 = [1,2,43.0,5]
113 |     # v2 = [3, 1, 0.9, 2.9 ]
114 |     # print list_element_minus( v1 , v2 )
115 |     main()


--------------------------------------------------------------------------------
/ROS_UR5_CIRCLE_V0/ur_kinematics.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/python
  2 | """
  3 | author:lz
  4 | time:20180812
  5 | version:v2
  6 | info:1,change class function 2,add best sol function
  7 | """
  8 | import math
  9 | import numpy
 10 | #UR5 parmas
 11 | d1 = 0.089159
 12 | a2 = -0.42500
 13 | a3 = -0.39225
 14 | d4 = 0.10915
 15 | d5 = 0.09465
 16 | d6 = 0.0823
 17 | PI = math.pi
 18 | ZERO_THRESH = 0.00000001
 19 | class Kinematic:
 20 |     def __init__(self):
 21 |         global d1
 22 |         global a2
 23 |         global a3
 24 |         global d4
 25 |         global d5
 26 |         global d6
 27 |         global PI
 28 |         global ZERO_THRESH
 29 |     def Forward(self,q):
 30 |         T=[0]*16
 31 |         #00
 32 |         s1 = math.sin(q[0])
 33 |         c1 = math.cos(q[0])
 34 |         ##01
 35 |         q234 = q[1]
 36 |         s2 = math.sin(q[1])
 37 |         c2 = math.cos(q[1])
 38 |         ##02
 39 |         s3 = math.sin(q[2])
 40 |         c3 = math.cos(q[2])
 41 |         q234 += q[2]
 42 |         ##03
 43 |         q234 += q[3]
 44 |         ##04
 45 |         s5 = math.sin(q[4])
 46 |         c5 = math.cos(q[4])
 47 |         ##05
 48 |         s6 = math.sin(q[5])
 49 |         c6 = math.cos(q[5])
 50 |         s234 = math.sin(q234)
 51 |         c234 = math.cos(q234)
 52 |         #4*4 roate arrary
 53 |         #00 parameter
 54 |         T[0] = ((c1 * c234 - s1 * s234) * s5) / 2.0 - c5 * s1 + ((c1 * c234 + s1 * s234) * s5) / 2.0
 55 |         # 01 parameter
 56 |         T[1] = (c6 * (s1 * s5 + ((c1 * c234 - s1 * s234) * c5) / 2.0 + ((c1 * c234 + s1 * s234) * c5) / 2.0) -
 57 |               (s6 * ((s1 * c234 + c1 * s234) - (s1 * c234 - c1 * s234))) / 2.0)
 58 |         # 02 parameter
 59 |         T[2] = (-(c6 * ((s1 * c234 + c1 * s234) - (s1 * c234 - c1 * s234))) / 2.0 -
 60 |               s6 * (s1 * s5 + ((c1 * c234 - s1 * s234) * c5) / 2.0 + ((c1 * c234 + s1 * s234) * c5) / 2.0))
 61 |         # 03 parameter
 62 |         T[3] = ((d5 * (s1 * c234 - c1 * s234)) / 2.0 - (d5 * (s1 * c234 + c1 * s234)) / 2.0 -
 63 |               d4 * s1 + (d6 * (c1 * c234 - s1 * s234) * s5) / 2.0 + (d6 * (c1 * c234 + s1 * s234) * s5) / 2.0 -
 64 |               a2 * c1 * c2 - d6 * c5 * s1 - a3 * c1 * c2 * c3 + a3 * c1 * s2 * s3)
 65 |         # 04 parameter
 66 |         T[4] = c1 * c5 + ((s1 * c234 + c1 * s234) * s5) / 2.0 + ((s1 * c234 - c1 * s234) * s5) / 2.0
 67 |         # 05 parameter
 68 |         T[5] = (c6 * (((s1 * c234 + c1 * s234) * c5) / 2.0 - c1 * s5 + ((s1 * c234 - c1 * s234) * c5) / 2.0) +
 69 |               s6 * ((c1 * c234 - s1 * s234) / 2.0 - (c1 * c234 + s1 * s234) / 2.0))
 70 |         # 06 parameter
 71 |         T[6] = (c6 * ((c1 * c234 - s1 * s234) / 2.0 - (c1 * c234 + s1 * s234) / 2.0) -
 72 |               s6 * (((s1 * c234 + c1 * s234) * c5) / 2.0 - c1 * s5 + ((s1 * c234 - c1 * s234) * c5) / 2.0))
 73 |         # 07 parameter
 74 |         T[7] = ((d5 * (c1 * c234 - s1 * s234)) / 2.0 - (d5 * (c1 * c234 + s1 * s234)) / 2.0 + d4 * c1 +
 75 |               (d6 * (s1 * c234 + c1 * s234) * s5) / 2.0 + (d6 * (s1 * c234 - c1 * s234) * s5) / 2.0 + d6 * c1 * c5 -
 76 |               a2 * c2 * s1 - a3 * c2 * c3 * s1 + a3 * s1 * s2 * s3)
 77 |         # 08 parameter
 78 |         T[8] = ((c234 * c5 - s234 * s5) / 2.0 - (c234 * c5 + s234 * s5) / 2.0)
 79 |         # 09 parameter
 80 |         T[9] = ((s234 * c6 - c234 * s6) / 2.0 - (s234 * c6 + c234 * s6) / 2.0 - s234 * c5 * c6)
 81 |         # 10 parameter
 82 |         T[10] = (s234 * c5 * s6 - (c234 * c6 + s234 * s6) / 2.0 - (c234 * c6 - s234 * s6) / 2.0)
 83 |         # 11 parameter
 84 |         T[11] = (d1 + (d6 * (c234 * c5 - s234 * s5)) / 2.0 + a3 * (s2 * c3 + c2 * s3) + a2 * s2 -
 85 |               (d6 * (c234 * c5 + s234 * s5)) / 2.0 - d5 * c234)
 86 |         # 12 parameter
 87 |         T[12] = 0.0
 88 |         # 13 parameter
 89 |         T[13] = 0.0
 90 |         # 14 parameter
 91 |         T[14] = 0.0
 92 |         # 15 parameter
 93 |         T[15] = 1.0
 94 |         return T
 95 |     def SIGN(self,x):
 96 |         return (x>0)-(x<0)
 97 |     def Forward_all(self,q):
 98 |         #result
 99 |         result=[]
100 |         T1=[]
101 |         T2=[]
102 |         T3=[]
103 |         T4=[]
104 |         T5=[]
105 |         T6=[]
106 |         #q01
107 |         s1 = math.sin(q[0])
108 |         c1 = math.cos(q[0])
109 |         #q02
110 |         q23 = q[1]
111 |         q234 = q[1]
112 |         s2 = math.sin(q[1])
113 |         c2 = math.cos(q[1])
114 |         #q03
115 |         s3 = math.sin(q[2])
116 |         c3 = math.cos(q[2])
117 |         q23 += q[2]
118 |         q234 += q[2]
119 |         #q04
120 |         q234 += q[3]
121 |         #q05
122 |         s5 = math.sin(q[4])
123 |         c5 = math.cos(q[4])
124 |         #q06
125 |         s6 = math.sin(q[5])
126 |         c6 = math.cos(q[5])
127 |         s23 = math.sin(q23)
128 |         c23 = math.cos(q23)
129 |         s234 = math.sin(q234)
130 |         c234 = math.cos(q234)
131 |         #01T
132 |         #00-15 param
133 |         T1[0] = c1
134 |         T1[1] = 0
135 |         T1[2] = s1
136 |         T1[3] = 0
137 |         T1[4] = s1
138 |         T1[5] = 0
139 |         T1[6] = -c1
140 |         T1[7] = 0
141 |         T1[8] = 0
142 |         T1[9] = 1
143 |         T1[10] = 0
144 |         T1[11] =d1
145 |         T1[12] = 0
146 |         T1[13] = 0
147 |         T1[14] = 0
148 |         T1[15] = 1
149 |         result.append(("T1",T1))
150 |         #01T*12T=02T
151 |         #00-15 parameters
152 |         T2[0] = c1 * c2
153 |         T2[1] = -c1 * s2
154 |         T2[2] = s1
155 |         T2[3] =a2 * c1 * c2
156 |         T2[4] = c2 * s1
157 |         T2[5] = -s1 * s2
158 |         T2[6] = -c1
159 |         T2[7] =a2 * c2 * s1
160 |         T2[8] = s2
161 |         T2[9] = c2
162 |         T2[10] = 0
163 |         T2[11] = d1 + a2 * s2
164 |         T2[12] = 0
165 |         T2[13] = 0
166 |         T2[14] = 0
167 |         T2[15] = 1
168 |         result.append(("T2",T2))
169 |         #01T*12T*23T=03T
170 |         T3[0] = c23 * c1
171 |         T3[1] = -s23 * c1
172 |         T3[2] = s1
173 |         T3[3] =c1 * (a3 * c23 + a2 * c2)
174 |         T3[4] = c23 * s1
175 |         T3[5] = -s23 * s1
176 |         T3[6] = -c1
177 |         T3[7] =s1 * (a3 * c23 + a2 * c2)
178 |         T3[8] = s23
179 |         T3[9] = c23
180 |         T3[10] = 0
181 |         T3[11] = d1 + a3 * s23 + a2 * s2
182 |         T3[12] = 0
183 |         T3[13] = 0
184 |         T3[14] = 0
185 |         T3[15] =1
186 |         result.append(("T3",T3))
187 |         #01T*12T*23T*34T=04T
188 |         T4[0] = c234 * c1
189 |         T4[1] = s1
190 |         T4[2] = s234 * c1
191 |         T4[3] =c1 * (a3 * c23 + a2 * c2) + d4 * s1
192 |         T4[4] = c234 * s1
193 |         T4[5] = -c1
194 |         T4[6] = s234 * s1
195 |         T4[7] =s1 * (a3 * c23 + a2 * c2) - d4 * c1
196 |         T4[8] = s234
197 |         T4[9] = 0
198 |         T4[10] = -c234
199 |         T4[11] =d1 + a3 * s23 + a2 * s2
200 |         T4[12] =0
201 |         T4[13] = 0
202 |         T4[14] = 0
203 |         T4[15] = 1
204 |         result.append(("T4",T4))
205 |         #01T*12T*23T*34T*45T=05T
206 |         T5[0] = s1 * s5 + c234 * c1 * c5
207 |         T5[1] = -s234 * c1
208 |         T5[2] = c5 * s1 - c234 * c1 * s5
209 |         T5[3] =c1 * (a3 * c23 + a2 * c2) + d4 * s1 + d5 * s234 * c1
210 |         T5[4] = c234 * c5 * s1 - c1 * s5
211 |         T5[5] = -s234 * s1
212 |         T5[6] = - c1 * c5 - c234 * s1 * s5
213 |         T5[7] =s1 * (a3 * c23 + a2 * c2) - d4 * c1 + d5 * s234 * s1
214 |         T5[8] =  s234 * c5
215 |         T5[9] = c234
216 |         T5[10] = -s234 * s5
217 |         T5[11] = d1 + a3 * s23 + a2 * s2 - d5 * c234
218 |         T5[12] = 0
219 |         T5[13] = 0
220 |         T5[14] = 0
221 |         T5[15] =1
222 |         result.append(("T5",T5))
223 |         #01T*12T*23T*34T*45T*56T=06T
224 |         T6[0] =   c6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * s6
225 |         T6[1] = - s6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * c6
226 |         T6[2] = c5 * s1 - c234 * c1 * s5
227 |         T6[3] =d6 * (c5 * s1 - c234 * c1 * s5) + c1 * (a3 * c23 + a2 * c2) + d4 * s1 + d5 * s234 * c1
228 |         T6[4] = - c6 * (c1 * s5 - c234 * c5 * s1) - s234 * s1 * s6
229 |         T6[5] = s6 * (c1 * s5 - c234 * c5 * s1) - s234 * c6 * s1
230 |         T6[6] = - c1 * c5 - c234 * s1 * s5
231 |         T6[7] =s1 * (a3 * c23 + a2 * c2) - d4 * c1 - d6 * (c1 * c5 + c234 * s1 * s5) + d5 * s234 * s1
232 |         T6[8] =c234 * s6 + s234 * c5 * c6
233 |         T6[9] = c234 * c6 - s234 * c5 * s6
234 |         T6[10] = -s234 * s5
235 |         T6[11] = d1 + a3 * s23 + a2 * s2 - d5 * c234 - d6 * s234 * s5
236 |         T6[12] = 0
237 |         T6[13] = 0
238 |         T6[14] = 0
239 |         T6[15] = 1
240 |         result.append(("T6", T6))
241 |         return result
242 |     def Iverse(self,T,q6_des):
243 |         q_sols=[0]*48
244 |         num_sols = 0
245 |         T02 = - T[0]
246 |         T00 = T[1]
247 |         T01 = T[2]
248 |         T03 = -T[3]
249 |         T12 = - T[4]
250 |         T10 = T[5]
251 |         T11 = T[6]
252 |         T13 = - T[7]
253 |         T22 = T[8]
254 |         T20 = - T[9]
255 |         T21 = - T[10]
256 |         T23 = T[11]
257 | 
258 |         #####shoulder rotate  joint(q1) ###########################
259 |         q1=[0,0]
260 |         A = d6 * T12 - T13
261 |         B = d6 * T02 - T03
262 |         R = A * A + B * B
263 |         if math.fabs(A) < ZERO_THRESH:
264 |             if math.fabs(math.fabs(d4) - math.fabs(B)) < ZERO_THRESH:
265 |                 div = -self.SIGN(d4) * self.SIGN(B)
266 |             else:
267 |                 div = -d4 / B
268 |             arcsin = math.asin(div)
269 |             if math.fabs(arcsin) < ZERO_THRESH:
270 |                 arcsin = 0.0
271 |             if arcsin < 0.0:
272 |                 q1[0] = arcsin + 2.0 * PI
273 |             else:
274 |                 q1[0] = arcsin
275 |                 q1[1] = PI - arcsin
276 |         elif math.fabs(B) < ZERO_THRESH:
277 |             if math.fabs(math.fabs(d4) - math.fabs(A)) < ZERO_THRESH:
278 |                 div = self.SIGN(d4) * self.SIGN(A)
279 |             else:
280 |                 div = d4 / A
281 |             arccos = math.acos(div)
282 |             q1[0] = arccos
283 |             q1[1] = 2.0 * PI - arccos
284 |         elif d4 * d4 > R:
285 |             return num_sols
286 |         else:
287 |             arccos = math.acos(d4 / math.sqrt(R))
288 |             arctan = math.atan2(-B, A)
289 |             pos = arccos + arctan
290 |             neg = -arccos + arctan
291 |             if math.fabs(pos) < ZERO_THRESH:
292 |                 pos = 0.0
293 |             if math.fabs(neg) < ZERO_THRESH:
294 |                 neg = 0.0
295 |             if pos >= 0.0:
296 |                 q1[0] = pos
297 |             else:
298 |                 q1[0] = 2.0 * PI + pos
299 |             if neg >= 0.0:
300 |                 q1[1] = neg
301 |             else:
302 |                 q1[1] = 2.0 * PI + neg
303 | 
304 |         ###### wrist2  joint(q5) ##########################
305 |         q5=numpy.zeros((2,2))#define 2*2 q5 array
306 | 
307 |         for i in range(2):
308 |             numer = (T03 * math.sin(q1[i]) - T13 * math.cos(q1[i]) - d4)
309 |             if math.fabs(math.fabs(numer) - math.fabs(d6)) < ZERO_THRESH:
310 |                 div = self.SIGN(numer) * self.SIGN(d6)
311 |             else:
312 |                 div = numer / d6
313 |             arccos = math.acos(div)
314 |             q5[i][0] = arccos
315 |             q5[i][1] = 2.0 * PI - arccos
316 |         #############################################################
317 |         for i in range(2):
318 |             for j in range(2):
319 |                 c1 = math.cos(q1[i])
320 |                 s1 = math.sin(q1[i])
321 |                 c5 = math.cos(q5[i][j])
322 |                 s5 = math.sin(q5[i][j])
323 |                 ######################## wrist 3 joint (q6) ################################
324 |                 if math.fabs(s5) < ZERO_THRESH:
325 |                     q6 = q6_des
326 |                 else:
327 |                     q6 = math.atan2(self.SIGN(s5) * -(T01 * s1 - T11 * c1),self.SIGN(s5) * (T00 * s1 - T10 * c1))
328 |                     if math.fabs(q6) < ZERO_THRESH:
329 |                         q6 = 0.0
330 |                     if (q6 < 0.0):
331 |                         q6 += 2.0 * PI
332 |                 q2=[0,0]
333 |                 q3=[0,0]
334 |                 q4=[0,0]
335 |                 #####################RRR joints (q2, q3, q4) ################################
336 |                 c6 = math.cos(q6)
337 |                 s6 = math.sin(q6)
338 |                 x04x = -s5 * (T02 * c1 + T12 * s1) - c5 * (s6 * (T01 * c1 + T11 * s1) - c6 * (T00 * c1 + T10 * s1))
339 |                 x04y = c5 * (T20 * c6 - T21 * s6) - T22 * s5
340 |                 p13x = d5 * (s6 * (T00 * c1 + T10 * s1) + c6 * (T01 * c1 + T11 * s1)) - d6 * (T02 * c1 + T12 * s1) +T03 * c1 + T13 * s1
341 |                 p13y = T23 - d1 - d6 * T22 + d5 * (T21 * c6 + T20 * s6)
342 |                 c3 = (p13x * p13x + p13y * p13y - a2 * a2 - a3 * a3) / (2.0 * a2 * a3)
343 |                 if math.fabs(math.fabs(c3) - 1.0) < ZERO_THRESH:
344 |                     c3 = self.SIGN(c3)
345 |                 elif math.fabs(c3) > 1.0:
346 |                     # TODO NO SOLUTION
347 |                     continue
348 |                 arccos = math.acos(c3)
349 |                 q3[0] = arccos
350 |                 q3[1] = 2.0 * PI - arccos
351 |                 denom = a2 * a2 + a3 * a3 + 2 * a2 * a3 * c3
352 |                 s3 = math.sin(arccos)
353 |                 A = (a2 + a3 * c3)
354 |                 B = a3 * s3
355 |                 q2[0] = math.atan2((A * p13y - B * p13x) / denom, (A * p13x + B * p13y) / denom)
356 |                 q2[1] = math.atan2((A * p13y + B * p13x) / denom, (A * p13x - B * p13y) / denom)
357 |                 c23_0 = math.cos(q2[0] + q3[0])
358 |                 s23_0 = math.sin(q2[0] + q3[0])
359 |                 c23_1 = math.cos(q2[1] + q3[1])
360 |                 s23_1 = math.sin(q2[1] + q3[1])
361 |                 q4[0] = math.atan2(c23_0 * x04y - s23_0 * x04x, x04x * c23_0 + x04y * s23_0)
362 |                 q4[1] = math.atan2(c23_1 * x04y - s23_1 * x04x, x04x * c23_1 + x04y * s23_1)
363 |                 ###########################################
364 |                 for k in range(2):
365 |                     if math.fabs(q2[k]) < ZERO_THRESH:
366 |                         q2[k] = 0.0
367 |                     elif q2[k] < 0.0:
368 |                         q2[k] += 2.0 * PI
369 |                     if math.fabs(q4[k]) < ZERO_THRESH:
370 |                         q4[k] = 0.0
371 |                     elif q4[k] < 0.0:
372 |                         q4[k] += 2.0 * PI
373 |                     q_sols[num_sols * 6 + 0] = q1[i]
374 |                     q_sols[num_sols * 6 + 1] = q2[k]
375 |                     q_sols[num_sols * 6 + 2] = q3[k]
376 |                     q_sols[num_sols * 6 + 3] = q4[k]
377 |                     q_sols[num_sols * 6 + 4] = q5[i][j]
378 |                     q_sols[num_sols * 6 + 5] = q6
379 |                     num_sols+=1
380 |         return num_sols,q_sols
381 |     """
382 |     q_inverse:ur kinematics solution joint q
383 |     q_last:last joint q
384 |     """
385 |     def solve_2pi_pro(self,q_inverse,q_last):
386 |         if abs(q_inverse-PI*2.0-q_last)<abs(q_inverse-q_last):
387 |             q_inverse-=PI*2.0
388 |             return q_inverse
389 |         else:
390 |             return q_inverse
391 |     #ik_fk,1:ik,0:fk
392 |     def display(self,q,q6_des,ik_fk):
393 |         T = self.Forward(q)
394 |         result_list=[]
395 |         #q_sols=[0]*48
396 |         num_sols, q_sols = self.Iverse(T, q6_des)
397 |         j=0
398 |         if ik_fk==1:
399 |             for i in range(num_sols):
400 |                 print i+1,"th solution"
401 |                 print q_sols[j],q_sols[j+1],q_sols[j+2],q_sols[j+3],q_sols[j+4],q_sols[j+5]
402 |                 print "solve 2 pi pro"
403 |                 print self.solve_2pi_pro(q_sols[j],q[0]),self.solve_2pi_pro(q_sols[j+1],q[1]),self.solve_2pi_pro(q_sols[j+2],q[2]),self.solve_2pi_pro(q_sols[j+3],q[3]),\
404 |                     self.solve_2pi_pro(q_sols[j+4], q[4]),self.solve_2pi_pro(q_sols[j+5],q[5])
405 |                 j+=6
406 |             print "you have ",num_sols,"solutions"
407 |         elif ik_fk==0:
408 |             for i in range(4):
409 |                 print T[j],T[j+1],T[j+2],T[j+3]
410 |                 j+=4
411 |         else:
412 |             print "######################forward solution######################"
413 |             for i in range(4):
414 |                 print T[j],T[j+1],T[j+2],T[j+3]
415 |                 j+=4
416 |             print "######################inverse solutions#####################"
417 |             j=0
418 |             for i in range(num_sols):
419 |                 print i+1,"th solution"
420 |                 print q_sols[j],q_sols[j+1],q_sols[j+2],q_sols[j+3],q_sols[j+4],q_sols[j+5]
421 |                 result_list.append([q_sols[j],q_sols[j+1],q_sols[j+2],q_sols[j+3],q_sols[j+4],q_sols[j+5]])
422 |                 j+=6
423 |             print result_list
424 |             return result_list
425 |             print "you have ",num_sols,"solutions"
426 |     """
427 |     q:last time joint q
428 |     """
429 |     def get_ik_data(self,T,q):
430 |         #T = self.Forward()
431 |         result_list=[]
432 |         #q_sols=[0]*48
433 |         # print "T",T
434 |         num_sols, q_sols = self.Iverse(T, 0)
435 |         # print "num_sols, q_sols",num_sols, q_sols
436 |         j = 0
437 |         for i in range(num_sols):
438 |             result_list.append([self.solve_2pi_pro(q_sols[j],q[0]),self.solve_2pi_pro(q_sols[j+1],q[1]),self.solve_2pi_pro(q_sols[j+2],q[2]),self.solve_2pi_pro(q_sols[j+3],q[3]),\
439 |                     self.solve_2pi_pro(q_sols[j+4], q[4]),self.solve_2pi_pro(q_sols[j+5],q[5])])
440 |             j += 6
441 |         # print result_list
442 |         return result_list
443 | 
444 |     def best_sol(self,weights,q_guess,T):
445 |         #q_guess=q
446 |         sols=self.get_ik_data(T,q_guess)
447 |         # print "sols",sols
448 |         valid_sols = []
449 |         for sol in sols:
450 |             test_sol = numpy.ones(6) * 9999.
451 |             for i in range(6):
452 |                 for add_ang in [-2. * numpy.pi, 0, 2. * numpy.pi]:
453 |                     test_ang = sol[i] + add_ang
454 |                     if (abs(test_ang) <= 2. * numpy.pi and
455 |                             abs(test_ang - q_guess[i]) < abs(test_sol[i] - q_guess[i])):
456 |                         test_sol[i] = test_ang
457 |             if numpy.all(test_sol != 9999.):
458 |                 valid_sols.append(test_sol)
459 |         if len(valid_sols) == 0:
460 |             return None
461 |         best_sol_ind = numpy.argmin(numpy.sum((weights * (valid_sols - numpy.array(q_guess))) ** 2, 1))
462 |         print "#########################the best sol##################"
463 |         print valid_sols[best_sol_ind]
464 |         return valid_sols[best_sol_ind]
465 | 
466 |     def best_sol_for_other_py(self,weights,q_guess,T):
467 |         sols=self.get_ik_data(T)
468 |         valid_sols = []
469 |         for sol in sols:
470 |             test_sol = numpy.ones(6) * 9999.
471 |             for i in range(6):
472 |                 for add_ang in [-2. * numpy.pi, 0, 2. * numpy.pi]:
473 |                     test_ang = sol[i] + add_ang
474 |                     if (abs(test_ang) <= 2. * numpy.pi and
475 |                             abs(test_ang - q_guess[i]) < abs(test_sol[i] - q_guess[i])):
476 |                         test_sol[i] = test_ang
477 |             if numpy.all(test_sol != 9999.):
478 |                 valid_sols.append(test_sol)
479 |         if len(valid_sols) == 0:
480 |             return None
481 |         best_sol_ind = numpy.argmin(numpy.sum((weights * (valid_sols - numpy.array(q_guess))) ** 2, 1))
482 |         print "#########################the best sol##################"
483 |         print valid_sols[best_sol_ind].tolist()
484 |         #print best_sol_ind
485 |         print sols[best_sol_ind]
486 |         return valid_sols[best_sol_ind].tolist()
487 | 
488 | def main():
489 |     q = [0, 0, 1, 0, 1, 0]
490 |     q1=[-1.5654299894915982, -1.6473701635943812, 0.05049753189086914, -1.4097726980792444, -1.140495349565615, -0.8895475069154913]
491 |     q2=[-0.25277, -0.8561733333333333, 1.2195411111111112, -3.557096666666667, -1.3205444444444445, -1.1349355555555556]
492 |     q3=[0,0,0,0,0,0]
493 |     q4=[3.3985266666666667, -1.3765411111111112, -1.944881111111111, 0.14112555555555556, -4.9428833333333335, 5.663687777777778]
494 |     q5=[-3.59860155164,-1.82648800883,-1.41735542252,0.0812199084238,1.27000134315,0.734254316924]
495 |     # q5=[-3.66249992472,-1.27642603705,-1.9559700595,0.0701396996895,1.3338858418,0.73287290524]
496 |     c=Kinematic()
497 |     # c.display(q5,0,3)
498 |     weights=[1.] * 6
499 |     T=[-0.13464668997929136, 0.9394821469191765, -0.3150294660785803, 0.0, 0.13751164454300352, 0.3325644757714387, 0.9330012953206159, 0.0, 0.981305669245164, 0.08230531620134232, -0.17396844090897007, 0.5790479214783786, 0.0, 0.0, 0.0, 1.0]
500 |     print c.Iverse(T,q5)
501 |     # c.best_sol(weights,q5,c.Forward(q5))
502 |     # c.best_sol_for_other_py(weights,q,c.Forward())
503 |     # print type(c.Forward())
504 |     #c.best_sol_new(c.Forward(q2))
505 | if __name__ == '__main__':
506 |     main()


--------------------------------------------------------------------------------
/ROS_UR5_CIRCLE_V0/utility.py:
--------------------------------------------------------------------------------
  1 | """
  2 | Python toolbox utility and helper functions.
  3 | 
  4 | 
  5 | Python implementation by: Luis Fernando Lara Tobar and Peter Corke.
  6 | Based on original Robotics Toolbox for Matlab code by Peter Corke.
  7 | Permission to use and copy is granted provided that acknowledgement of
  8 | the authors is made.
  9 | 
 10 | @author: Luis Fernando Lara Tobar and Peter Corke
 11 | """
 12 | 
 13 | from numpy import *
 14 | 
 15 | 
 16 | def ishomog(tr):
 17 |     """
 18 |     True if C{tr} is a 4x4 homogeneous transform.
 19 | 
 20 |     @note: Only the dimensions are tested, not whether the rotation submatrix
 21 |     is orthonormal.
 22 | 
 23 |     @rtype: boolean
 24 |     """
 25 | 
 26 |     return tr.shape == (4, 4)
 27 | 
 28 | 
 29 | def isrot(r):
 30 |     """
 31 |     True if C{tr} is a 3x3 matrix.
 32 | 
 33 |     @note: Only the dimensions are tested, not whether the matrix
 34 |     is orthonormal.
 35 | 
 36 |     @rtype: boolean
 37 |     """
 38 |     return r.shape == (3, 3)
 39 | 
 40 | 
 41 | def isvec(v, l=3):
 42 |     """
 43 |     True if C{tr} is an l-vector.
 44 | 
 45 |     @param v: object to test
 46 |     @type l: integer
 47 |     @param l: length of vector (default 3)
 48 | 
 49 |     @rtype: boolean
 50 |     """
 51 |     return v.shape == (l, 1) or v.shape == (1, l) or v.shape == (l,)
 52 | 
 53 | 
 54 | def numcols(m):
 55 |     """
 56 |     Number of columns in a matrix.
 57 | 
 58 |     @type m: matrix
 59 |     @return: the number of columns in the matrix.
 60 |     return m.shape[1];
 61 |     """
 62 |     return m.shape[1];
 63 | 
 64 | 
 65 | def numrows(m):
 66 |     """
 67 |     Number of rows in a matrix.
 68 | 
 69 |     @type m: matrix
 70 |     @return: the number of rows in the matrix.
 71 |     return m.shape[1];
 72 |     """
 73 |     return m.shape[0];
 74 | 
 75 | 
 76 | ################ vector operations
 77 | 
 78 | def unit(v):
 79 |     """
 80 |     Unit vector.
 81 | 
 82 |     @type v: vector
 83 |     @rtype: vector
 84 |     @return: unit-vector parallel to C{v}
 85 |     """
 86 |     return mat(v / linalg.norm(v))
 87 | 
 88 | 
 89 | def crossp(v1, v2):
 90 |     """
 91 |     Vector cross product.
 92 | 
 93 |     @note Differs from L{numpy.cross} in that vectors can be row or
 94 |     column.
 95 | 
 96 |     @type v1: 3-vector
 97 |     @type v2: 3-vector
 98 |     @rtype: 3-vector
 99 |     @return: Cross product M{v1 x v2}
100 |     """
101 |     v1 = mat(v1)
102 |     v2 = mat(v2)
103 |     v1 = v1.reshape(3, 1)
104 |     v2 = v2.reshape(3, 1)
105 |     v = matrix(zeros((3, 1)))
106 |     v[0] = v1[1] * v2[2] - v1[2] * v2[1]
107 |     v[1] = v1[2] * v2[0] - v1[0] * v2[2]
108 |     v[2] = v1[0] * v2[1] - v1[1] * v2[0]
109 |     return v
110 | 
111 | 
112 | ################ misc support functions
113 | 
114 | def arg2array(arg):
115 |     """
116 |     Convert a 1-dimensional argument that is either a list, array or matrix to an
117 |     array.
118 | 
119 |     Useful for functions where the argument might be in any of these formats:::
120 |             func(a)
121 |             func(1,2,3)
122 | 
123 |             def func(*args):
124 |                 if len(args) == 1:
125 |                     v = arg2array(arg[0]);
126 |                 elif len(args) == 3:
127 |                     v = arg2array(args);
128 |              .
129 |              .
130 |              .
131 | 
132 |     @rtype: array
133 |     @return: Array equivalent to C{arg}.
134 |     """
135 |     if isinstance(arg, (matrix, ndarray)):
136 |         s = arg.shape;
137 |         if len(s) == 1:
138 |             return array(arg);
139 |         if min(s) == 1:
140 |             return array(arg).flatten();
141 | 
142 |     elif isinstance(arg, list):
143 |         return array(arg);
144 | 
145 |     elif isinstance(arg, (int, float, float32, float64)):
146 |         return array([arg]);
147 | 
148 |     raise ValueError;
149 | 
150 | 
151 | import traceback;
152 | 
153 | 
154 | def error(s):
155 |     """
156 |     Common error handler.  Display the error string, execute a traceback then raise
157 |     an execption to return to the interactive prompt.
158 |     """
159 |     print 'Robotics toolbox error:', s
160 | 
161 |     # traceback.print_exc();
162 |     raise ValueError
163 | 
164 | 
165 | if __name__ == "__main__":
166 |     print arg2array(1)
167 |     print arg2array(1.0);
168 |     print arg2array(mat([1, 2, 3, 4]))
169 |     print arg2array(mat([1, 2, 3, 4]).T)
170 |     print arg2array(array([1, 2, 3]));
171 |     print arg2array(array([1, 2, 3]).T);
172 |     print arg2array([1, 2, 3]);
173 | 
174 | 
175 | 


--------------------------------------------------------------------------------
/stand.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/linzhuyue/CSDN_BLOG_CODE/bef14ca6e3520227042070bdd713d3bdcf11a967/stand.gif


--------------------------------------------------------------------------------