PlanarManipulatorExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 
7 See LICENSE for the license information
8 
9 Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
10 Author: Frank Dellaert
11 """
12 # pylint: disable=invalid-name, E1101
13 
14 from __future__ import print_function
15 
16 import math
17 import unittest
18 from functools import reduce
19 
20 import matplotlib.pyplot as plt
21 import numpy as np
22 from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
23 
24 import gtsam
25 import gtsam.utils.plot as gtsam_plot
26 from gtsam import Pose2
27 from gtsam.utils.test_case import GtsamTestCase
28 
29 
30 def vector3(x, y, z):
31  """Create 3D double numpy array."""
32  return np.array([x, y, z], dtype=float)
33 
34 
35 def compose(*poses):
36  """Compose all Pose2 transforms given as arguments from left to right."""
37  return reduce((lambda x, y: x.compose(y)), poses)
38 
39 
40 def vee(M):
41  """Pose2 vee operator."""
42  return vector3(M[0, 2], M[1, 2], M[1, 0])
43 
44 
45 def delta(g0, g1):
46  """Difference between x,y,,theta components of SE(2) poses."""
47  return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
48 
49 
50 def trajectory(g0, g1, N=20):
51  """ Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
52  g0 and g1 are the initial and final pose, respectively.
53  N is the number of *intervals*
54  Returns N+1 poses
55  """
56  e = delta(g0, g1)
57  return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
58 
59 
61  """Three-link arm class."""
62 
63  def __init__(self):
64  self.L1 = 3.5
65  self.L2 = 3.5
66  self.L3 = 2.5
67  self.xi1 = vector3(0, 0, 1)
68  self.xi2 = vector3(self.L1, 0, 1)
69  self.xi3 = vector3(self.L1+self.L2, 0, 1)
70  self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
71 
72  def fk(self, q):
73  """ Forward kinematics.
74  Takes numpy array of joint angles, in radians.
75  """
76  sXl1 = Pose2(0, 0, math.radians(90))
77  l1Zl1 = Pose2(0, 0, q[0])
78  l1Xl2 = Pose2(self.L1, 0, 0)
79  l2Zl2 = Pose2(0, 0, q[1])
80  l2Xl3 = Pose2(self.L2, 0, 0)
81  l3Zl3 = Pose2(0, 0, q[2])
82  l3Xt = Pose2(self.L3, 0, 0)
83  return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
84 
85  def jacobian(self, q):
86  """ Calculate manipulator Jacobian.
87  Takes numpy array of joint angles, in radians.
88  """
89  a = q[0]+q[1]
90  b = a+q[2]
91  return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
92  -self.L1*math.cos(a)-self.L3*math.cos(b),
93  - self.L3*math.cos(b)],
94  [-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
95  -self.L1*math.sin(a)-self.L3*math.sin(b),
96  - self.L3*math.sin(b)],
97  [1, 1, 1]], float)
98 
99  def poe(self, q):
100  """ Forward kinematics.
101  Takes numpy array of joint angles, in radians.
102  """
103  l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
104  l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
105  l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
106  return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
107 
108  def con(self, q):
109  """ Forward kinematics, conjugation form.
110  Takes numpy array of joint angles, in radians.
111  """
112  def expmap(x, y, theta):
113  """Implement exponential map via conjugation with axis (x,y)."""
114  return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
115 
116  l1Zl1 = expmap(0.0, 0.0, q[0])
117  l2Zl2 = expmap(0.0, self.L1, q[1])
118  l3Zl3 = expmap(0.0, self.L1+self.L2, q[2])
119  return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
120 
121  def ik(self, sTt_desired, e=1e-9):
122  """ Inverse kinematics.
123  Takes desired Pose2 of tool T with respect to base S.
124  Optional: mu, gradient descent rate; e: error norm threshold
125  """
126  q = np.radians(vector3(30, -30, 45)) # well within workspace
127  error = vector3(100, 100, 100)
128 
129  while np.linalg.norm(error) > e:
130  error = delta(sTt_desired, self.fk(q))
131  J = self.jacobian(q)
132  q -= np.dot(np.linalg.pinv(J), error)
133 
134  # return result in interval [-pi,pi)
135  return np.remainder(q+math.pi, 2*math.pi)-math.pi
136 
137  def manipulator_jacobian(self, q):
138  """ Calculate manipulator Jacobian.
139  Takes numpy array of joint angles, in radians.
140  Returns the manipulator Jacobian of differential twists. When multiplied with
141  a vector of joint velocities, will yield a single differential twist which is
142  the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
143  Just like always, differential twists can be hatted and multiplied with spatial
144  coordinates of a point to give the spatial velocity of the point.
145  """
146  l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
147  l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
148  # l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
149 
150  p1 = self.xi1
151  # p1 = Pose2().Adjoint(self.xi1)
152 
153  sTl1 = l1Zl1
154  p2 = sTl1.Adjoint(self.xi2)
155 
156  sTl2 = compose(l1Zl1, l2Zl2)
157  p3 = sTl2.Adjoint(self.xi3)
158 
159  differential_twists = [p1, p2, p3]
160  return np.stack(differential_twists, axis=1)
161 
162  def plot(self, fignum, q):
163  """ Plot arm.
164  Takes figure number, and numpy array of joint angles, in radians.
165  """
166  fig = plt.figure(fignum)
167  axes = fig.gca()
168 
169  sXl1 = Pose2(0, 0, math.radians(90))
170  p1 = sXl1.translation()
171  gtsam_plot.plot_pose2_on_axes(axes, sXl1)
172 
173  def plot_line(p, g, color):
174  q = g.translation()
175  line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
176  axes.plot(line[:, 0], line[:, 1], color)
177  return q
178 
179  l1Zl1 = Pose2(0, 0, q[0])
180  l1Xl2 = Pose2(self.L1, 0, 0)
181  sTl2 = compose(sXl1, l1Zl1, l1Xl2)
182  p2 = plot_line(p1, sTl2, 'r-')
183  gtsam_plot.plot_pose2_on_axes(axes, sTl2)
184 
185  l2Zl2 = Pose2(0, 0, q[1])
186  l2Xl3 = Pose2(self.L2, 0, 0)
187  sTl3 = compose(sTl2, l2Zl2, l2Xl3)
188  p3 = plot_line(p2, sTl3, 'g-')
189  gtsam_plot.plot_pose2_on_axes(axes, sTl3)
190 
191  l3Zl3 = Pose2(0, 0, q[2])
192  l3Xt = Pose2(self.L3, 0, 0)
193  sTt = compose(sTl3, l3Zl3, l3Xt)
194  plot_line(p3, sTt, 'b-')
195  gtsam_plot.plot_pose2_on_axes(axes, sTt)
196 
197 
198 # Create common example configurations.
199 Q0 = vector3(0, 0, 0)
200 Q1 = np.radians(vector3(-30, -45, -90))
201 Q2 = np.radians(vector3(-90, 90, 0))
202 
203 
205  """Unit tests for functions used below."""
206 
207  def setUp(self):
208  self.arm = ThreeLinkArm()
209 
210  def assertPose2Equals(self, actual, expected, tol=1e-2):
211  """Helper function that prints out actual and expected if not equal."""
212  equal = actual.equals(expected, tol)
213  if not equal:
214  raise self.failureException(
215  "Poses are not equal:\n{}!={}".format(actual, expected))
216 
217  def test_fk_arm(self):
218  """Make sure forward kinematics is correct for some known test configurations."""
219  # at rest
220  expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
221  sTt = self.arm.fk(Q0)
222  self.assertIsInstance(sTt, Pose2)
223  self.assertPose2Equals(sTt, expected)
224 
225  # -30, -45, -90
226  expected = Pose2(5.78, 1.52, math.radians(-75))
227  sTt = self.arm.fk(Q1)
228  self.assertPose2Equals(sTt, expected)
229 
230  def test_jacobian(self):
231  """Test Jacobian calculation."""
232  # at rest
233  expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], float)
234  J = self.arm.jacobian(Q0)
235  np.testing.assert_array_almost_equal(J, expected)
236 
237  # at -90, 90, 0
238  expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], float)
239  J = self.arm.jacobian(Q2)
240  np.testing.assert_array_almost_equal(J, expected)
241 
242  def test_con_arm(self):
243  """Make sure POE is correct for some known test configurations."""
244  # at rest
245  expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
246  sTt = self.arm.con(Q0)
247  self.assertIsInstance(sTt, Pose2)
248  self.assertPose2Equals(sTt, expected)
249 
250  # -30, -45, -90
251  expected = Pose2(5.78, 1.52, math.radians(-75))
252  sTt = self.arm.con(Q1)
253  self.assertPose2Equals(sTt, expected)
254 
255  def test_poe_arm(self):
256  """Make sure POE is correct for some known test configurations."""
257  # at rest
258  expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
259  sTt = self.arm.poe(Q0)
260  self.assertIsInstance(sTt, Pose2)
261  self.assertPose2Equals(sTt, expected)
262 
263  # -30, -45, -90
264  expected = Pose2(5.78, 1.52, math.radians(-75))
265  sTt = self.arm.poe(Q1)
266  self.assertPose2Equals(sTt, expected)
267 
268  def test_ik(self):
269  """Check iterative inverse kinematics function."""
270  # at rest
271  actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
272  np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
273 
274  # -30, -45, -90
275  sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
276  actual = self.arm.ik(sTt_desired)
277  self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
278  np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
279 
281  """Test Jacobian calculation."""
282  # at rest
283  expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], float)
284  J = self.arm.manipulator_jacobian(Q0)
285  np.testing.assert_array_almost_equal(J, expected)
286 
287  # at -90, 90, 0
288  expected = np.array(
289  [[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], float)
290  J = self.arm.manipulator_jacobian(Q2)
291  np.testing.assert_array_almost_equal(J, expected)
292 
293 
295  """ Use trajectory interpolation and then trajectory tracking a la Murray
296  to move a 3-link arm on a straight line.
297  """
298  # Create arm
299  arm = ThreeLinkArm()
300 
301  # Get initial pose using forward kinematics
302  q = np.radians(vector3(30, -30, 45))
303  sTt_initial = arm.fk(q)
304 
305  # Create interpolated trajectory in task space to desired goal pose
306  sTt_goal = Pose2(2.4, 4.3, math.radians(0))
307  poses = trajectory(sTt_initial, sTt_goal, 50)
308 
309  # Setup figure and plot initial pose
310  fignum = 0
311  fig = plt.figure(fignum)
312  axes = fig.gca()
313  axes.set_xlim(-5, 5)
314  axes.set_ylim(0, 10)
315  gtsam_plot.plot_pose2(fignum, arm.fk(q))
316 
317  # For all poses in interpolated trajectory, calculate dq to move to next pose.
318  # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose).
319  for pose in poses:
320  sTt = arm.fk(q)
321  error = delta(sTt, pose)
322  J = arm.jacobian(q)
323  q += np.dot(np.linalg.inv(J), error)
324  arm.plot(fignum, q)
325  plt.pause(0.01)
326 
327  plt.pause(10)
328 
329 
330 if __name__ == "__main__":
331  run_example()
332  unittest.main()
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:14