Merged in feature/kinematics (pull request #334)
Added an example to show how GTSAM can be used to model planar manipulator arms.release/4.3a0
Frank Dellaert
commit
1d16a6c391
|
|
@ -0,0 +1,325 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
import unittest
|
||||
from functools import reduce
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
from gtsam import Pose2
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3D double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
def compose(*poses):
|
||||
"""Compose all Pose2 transforms given as arguments from left to right."""
|
||||
return reduce((lambda x, y: x.compose(y)), poses)
|
||||
|
||||
|
||||
def vee(M):
|
||||
"""Pose2 vee operator."""
|
||||
return vector3(M[0, 2], M[1, 2], M[1, 0])
|
||||
|
||||
|
||||
def delta(g0, g1):
|
||||
"""Difference between x,y,,theta components of SE(2) poses."""
|
||||
return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
|
||||
|
||||
|
||||
def trajectory(g0, g1, N=20):
|
||||
""" Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
|
||||
g0 and g1 are the initial and final pose, respectively.
|
||||
N is the number of *intervals*
|
||||
Returns N+1 poses
|
||||
"""
|
||||
e = delta(g0, g1)
|
||||
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)]
|
||||
|
||||
|
||||
class ThreeLinkArm(object):
|
||||
"""Three-link arm class."""
|
||||
|
||||
def __init__(self):
|
||||
self.L1 = 3.5
|
||||
self.L2 = 3.5
|
||||
self.L3 = 2.5
|
||||
self.xi1 = vector3(0, 0, 1)
|
||||
self.xi2 = vector3(self.L1, 0, 1)
|
||||
self.xi3 = vector3(self.L1+self.L2, 0, 1)
|
||||
self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
|
||||
|
||||
def fk(self, q):
|
||||
""" Forward kinematics.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
sXl1 = Pose2(0, 0, math.radians(90))
|
||||
l1Zl1 = Pose2(0, 0, q[0])
|
||||
l1Xl2 = Pose2(self.L1, 0, 0)
|
||||
l2Zl2 = Pose2(0, 0, q[1])
|
||||
l2Xl3 = Pose2(self.L2, 0, 0)
|
||||
l3Zl3 = Pose2(0, 0, q[2])
|
||||
l3Xt = Pose2(self.L3, 0, 0)
|
||||
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
|
||||
|
||||
def jacobian(self, q):
|
||||
""" Calculate manipulator Jacobian.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
a = q[0]+q[1]
|
||||
b = a+q[2]
|
||||
return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
|
||||
-self.L1*math.cos(a)-self.L3*math.cos(b),
|
||||
- self.L3*math.cos(b)],
|
||||
[-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
|
||||
-self.L1*math.sin(a)-self.L3*math.sin(b),
|
||||
- self.L3*math.sin(b)],
|
||||
[1, 1, 1]], np.float)
|
||||
|
||||
def poe(self, q):
|
||||
""" Forward kinematics.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
|
||||
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
|
||||
l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def con(self, q):
|
||||
""" Forward kinematics, conjugation form.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
def expmap(x, y, theta):
|
||||
"""Implement exponential map via conjugation with axis (x,y)."""
|
||||
return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
|
||||
|
||||
l1Zl1 = expmap(0.0, 0.0, q[0])
|
||||
l2Zl2 = expmap(0.0, self.L1, q[1])
|
||||
l3Zl3 = expmap(0.0, 7.0, q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def ik(self, sTt_desired, e=1e-9):
|
||||
""" Inverse kinematics.
|
||||
Takes desired Pose2 of tool T with respect to base S.
|
||||
Optional: mu, gradient descent rate; e: error norm threshold
|
||||
"""
|
||||
q = np.radians(vector3(30, -30, 45)) # well within workspace
|
||||
error = vector3(100, 100, 100)
|
||||
|
||||
while np.linalg.norm(error) > e:
|
||||
error = delta(sTt_desired, self.fk(q))
|
||||
J = self.jacobian(q)
|
||||
q -= np.dot(np.linalg.pinv(J), error)
|
||||
|
||||
# return result in interval [-pi,pi)
|
||||
return np.remainder(q+math.pi, 2*math.pi)-math.pi
|
||||
|
||||
def manipulator_jacobian(self, q):
|
||||
""" Calculate manipulator Jacobian.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
Returns the manipulator Jacobian of differential twists. When multiplied with
|
||||
a vector of joint velocities, will yield a single differential twist which is
|
||||
the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
|
||||
Just like always, differential twists can be hatted and multiplied with spatial
|
||||
coordinates of a point to give the spatial velocity of the point.
|
||||
"""
|
||||
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
|
||||
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
|
||||
# l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
|
||||
|
||||
p1 = self.xi1
|
||||
# p1 = Pose2().Adjoint(self.xi1)
|
||||
|
||||
sTl1 = l1Zl1
|
||||
p2 = sTl1.Adjoint(self.xi2)
|
||||
|
||||
sTl2 = compose(l1Zl1, l2Zl2)
|
||||
p3 = sTl2.Adjoint(self.xi3)
|
||||
|
||||
differential_twists = [p1, p2, p3]
|
||||
return np.stack(differential_twists, axis=1)
|
||||
|
||||
def plot(self, fignum, q):
|
||||
""" Plot arm.
|
||||
Takes figure number, and numpy array of joint angles, in radians.
|
||||
"""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
|
||||
sXl1 = Pose2(0, 0, math.radians(90))
|
||||
t = sXl1.translation()
|
||||
p1 = np.array([t.x(), t.y()])
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sXl1)
|
||||
|
||||
def plot_line(p, g, color):
|
||||
t = g.translation()
|
||||
q = np.array([t.x(), t.y()])
|
||||
line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], color)
|
||||
return q
|
||||
|
||||
l1Zl1 = Pose2(0, 0, q[0])
|
||||
l1Xl2 = Pose2(self.L1, 0, 0)
|
||||
sTl2 = compose(sXl1, l1Zl1, l1Xl2)
|
||||
p2 = plot_line(p1, sTl2, 'r-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTl2)
|
||||
|
||||
l2Zl2 = Pose2(0, 0, q[1])
|
||||
l2Xl3 = Pose2(self.L2, 0, 0)
|
||||
sTl3 = compose(sTl2, l2Zl2, l2Xl3)
|
||||
p3 = plot_line(p2, sTl3, 'g-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTl3)
|
||||
|
||||
l3Zl3 = Pose2(0, 0, q[2])
|
||||
l3Xt = Pose2(self.L3, 0, 0)
|
||||
sTt = compose(sTl3, l3Zl3, l3Xt)
|
||||
plot_line(p3, sTt, 'b-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTt)
|
||||
|
||||
|
||||
# Create common example configurations.
|
||||
Q0 = vector3(0, 0, 0)
|
||||
Q1 = np.radians(vector3(-30, -45, -90))
|
||||
Q2 = np.radians(vector3(-90, 90, 0))
|
||||
|
||||
|
||||
class TestPose2SLAMExample(unittest.TestCase):
|
||||
"""Unit tests for functions used below."""
|
||||
|
||||
def setUp(self):
|
||||
self.arm = ThreeLinkArm()
|
||||
|
||||
def assertPose2Equals(self, actual, expected, tol=1e-2):
|
||||
"""Helper function that prints out actual and expected if not equal."""
|
||||
equal = actual.equals(expected, tol)
|
||||
if not equal:
|
||||
raise self.failureException(
|
||||
"Poses are not equal:\n{}!={}".format(actual, expected))
|
||||
|
||||
def test_fk_arm(self):
|
||||
"""Make sure forward kinematics is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.fk(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.fk(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_jacobian(self):
|
||||
"""Test Jacobian calculation."""
|
||||
# at rest
|
||||
expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.jacobian(Q0)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
# at -90, 90, 0
|
||||
expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.jacobian(Q2)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
def test_con_arm(self):
|
||||
"""Make sure POE is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.con(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.con(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_poe_arm(self):
|
||||
"""Make sure POE is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.poe(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.poe(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_ik(self):
|
||||
"""Check iterative inverse kinematics function."""
|
||||
# at rest
|
||||
actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
|
||||
np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
|
||||
|
||||
# -30, -45, -90
|
||||
sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
|
||||
actual = self.arm.ik(sTt_desired)
|
||||
self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
|
||||
np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
|
||||
|
||||
def test_manipulator_jacobian(self):
|
||||
"""Test Jacobian calculation."""
|
||||
# at rest
|
||||
expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.manipulator_jacobian(Q0)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
# at -90, 90, 0
|
||||
expected = np.array(
|
||||
[[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
|
||||
J = self.arm.manipulator_jacobian(Q2)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
|
||||
def run_example():
|
||||
""" Use trajectory interpolation and then trajectory tracking a la Murray
|
||||
to move a 3-link arm on a straight line.
|
||||
"""
|
||||
arm = ThreeLinkArm()
|
||||
q = np.radians(vector3(30, -30, 45))
|
||||
sTt_initial = arm.fk(q)
|
||||
sTt_goal = Pose2(2.4, 4.3, math.radians(0))
|
||||
poses = trajectory(sTt_initial, sTt_goal, 50)
|
||||
|
||||
fignum = 0
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
axes.set_xlim(-5, 5)
|
||||
axes.set_ylim(0, 10)
|
||||
gtsam_plot.plot_pose2(fignum, arm.fk(q))
|
||||
|
||||
for pose in poses:
|
||||
sTt = arm.fk(q)
|
||||
error = delta(sTt, pose)
|
||||
J = arm.jacobian(q)
|
||||
q += np.dot(np.linalg.inv(J), error)
|
||||
arm.plot(fignum, q)
|
||||
plt.pause(0.01)
|
||||
|
||||
plt.pause(10)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run_example()
|
||||
unittest.main()
|
||||
Loading…
Reference in New Issue