PlanarManipulator.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ryan Luna */
36
37#include <iostream>
38#include "PlanarManipulator.h"
39#include <boost/math/constants/constants.hpp>
40#include <stdexcept>
41
42#define PI boost::math::constants::pi<double>()
43#define TWOPI boost::math::constants::two_pi<double>()
44
45PlanarManipulator::PlanarManipulator(unsigned int numLinks, double linkLength, const std::pair<double, double> &origin)
46 : numLinks_(numLinks), linkLengths_(numLinks_, linkLength)
47{
48 baseFrame_ = Eigen::Affine2d::Identity();
49 baseFrame_.translation()[0] = origin.first;
50 baseFrame_.translation()[1] = origin.second;
51
52 hasBounds_.assign(numLinks_, false);
53 lowerBounds_.assign(numLinks_, std::numeric_limits<double>::min());
54 upperBounds_.assign(numLinks_, std::numeric_limits<double>::max());
55}
56
57PlanarManipulator::PlanarManipulator(unsigned int numLinks, const std::vector<double> &linkLengths,
58 const std::pair<double, double> &origin)
59 : numLinks_(numLinks), linkLengths_(linkLengths)
60{
61 if (linkLengths_.size() != numLinks)
62 {
63 std::cerr << "Length of linkLengths (" << linkLengths.size() << ") is not equal to the number of links ("
64 << numLinks << ")" << std::endl;
65 throw;
66 }
67
68 baseFrame_ = Eigen::Affine2d::Identity();
69 baseFrame_.translation()[0] = origin.first;
70 baseFrame_.translation()[1] = origin.second;
71
72 hasBounds_.assign(numLinks_, false);
73 lowerBounds_.assign(numLinks_, std::numeric_limits<double>::min());
74 upperBounds_.assign(numLinks_, std::numeric_limits<double>::max());
75}
76
77PlanarManipulator::~PlanarManipulator()
78{
79}
80
81// Return the number of links
82unsigned int PlanarManipulator::getNumLinks() const
83{
84 return numLinks_;
85}
86
87const Eigen::Affine2d &PlanarManipulator::getBaseFrame() const
88{
89 return baseFrame_;
90}
91
92void PlanarManipulator::setBaseFrame(const Eigen::Affine2d &frame)
93{
94 baseFrame_ = frame;
95}
96
97// Return the length of each link
98const std::vector<double> &PlanarManipulator::getLinkLengths() const
99{
100 return linkLengths_;
101}
102
103// Set the bounds for link # link
104void PlanarManipulator::setBounds(unsigned int link, double low, double high)
105{
106 assert(link < hasBounds_.size());
107
108 hasBounds_[link] = true;
109 lowerBounds_[link] = low;
110 upperBounds_[link] = high;
111}
112
113// Set the bounds for ALL links
114void PlanarManipulator::setBounds(const std::vector<double> &low, const std::vector<double> &high)
115{
116 assert(low.size() == high.size());
117 assert(low.size() == numLinks_);
118
119 hasBounds_.assign(numLinks_, true);
120 lowerBounds_ = low;
121 upperBounds_ = high;
122}
123
124bool PlanarManipulator::hasBounds(unsigned int link) const
125{
126 return hasBounds_[link];
127}
128
129void PlanarManipulator::getBounds(unsigned int link, double &low, double &high) const
130{
131 low = lowerBounds_[link];
132 high = upperBounds_[link];
133}
134const std::vector<double> &PlanarManipulator::lowerBounds() const
135{
136 return lowerBounds_;
137}
138
139const std::vector<double> &PlanarManipulator::upperBounds() const
140{
141 return upperBounds_;
142}
143
144// Forward kinematics for the given joint configuration. The frames for links
145// 1 through the end-effector are returned.
146void PlanarManipulator::FK(const std::vector<double> &joints, std::vector<Eigen::Affine2d> &frames) const
147{
148 FK(&joints[0], frames);
149}
150
151void PlanarManipulator::FK(const Eigen::VectorXd &joints, std::vector<Eigen::Affine2d> &frames) const
152{
153 FK(&joints(0), frames);
154}
155
156void PlanarManipulator::FK(const double *joints, std::vector<Eigen::Affine2d> &frames) const
157{
158 frames.clear();
159 Eigen::Affine2d frame(baseFrame_);
160
161 for (unsigned int i = 0; i < numLinks_; ++i)
162 {
163 // Rotate, then translate. Just like the old gypsy woman said.
164 Eigen::Affine2d offset(Eigen::Rotation2Dd(joints[i]) * Eigen::Translation2d(linkLengths_[i], 0));
165 frame = frame * offset;
166 frames.push_back(frame);
167 }
168}
169
170void PlanarManipulator::FK(const std::vector<double> &joints, Eigen::Affine2d &eeFrame) const
171{
172 FK(&joints[0], eeFrame);
173}
174
175void PlanarManipulator::FK(const Eigen::VectorXd &joints, Eigen::Affine2d &eeFrame) const
176{
177 FK(&joints(0), eeFrame);
178}
179
180void PlanarManipulator::FK(const double *joints, Eigen::Affine2d &eeFrame) const
181{
182 eeFrame = baseFrame_;
183
184 for (unsigned int i = 0; i < numLinks_; ++i)
185 {
186 // Rotate, then translate. Just like the old gypsy woman said.
187 Eigen::Affine2d offset(Eigen::Rotation2Dd(joints[i]) * Eigen::Translation2d(linkLengths_[i], 0));
188 eeFrame = eeFrame * offset;
189 }
190}
191
192// Inverse kinematics for the given end effector frame. Only one solution is returned.
193// Returns false if no solution exists to the given pose.
194bool PlanarManipulator::IK(std::vector<double> &solution, const Eigen::Affine2d &eeFrame) const
195{
196 std::vector<double> seed(numLinks_, M_PI / 2.0);
197 return IK(solution, seed, eeFrame);
198}
199
200bool PlanarManipulator::IK(std::vector<double> &solution, const std::vector<double> &seed,
201 const Eigen::Affine2d &desiredFrame) const
202{
203 // desired frame is clearly impossible to achieve
204 if (infeasible(desiredFrame))
205 return false;
206
207 // This is the orientation for the end effector
208 double angle = acos(desiredFrame.rotation()(0, 0));
209 // Due to numerical instability, sometimes acos will return nan if
210 // the value is slightly larger than one. Check for this and try the
211 // asin of the next value
212 if (angle != angle) // a nan is never equal to itself
213 angle = asin(desiredFrame.rotation()(0, 1));
214
215 // If still nan, return false
216 if (angle != angle)
217 return false;
218
219 // Get the current pose
220 Eigen::Affine2d frame;
221 FK(seed, frame);
222 Eigen::VectorXd current;
223 frameToPose(frame, current);
224
225 // Get the desired pose
226 Eigen::VectorXd desired;
227 frameToPose(desiredFrame, desired);
228
229 // Compute the error
230 Eigen::VectorXd e(desired - current);
231
232 Eigen::VectorXd joints(seed.size());
233 for (size_t i = 0; i < seed.size(); ++i)
234 joints(i) = seed[i];
235
236 double alpha = 0.1; // step size
237
238 unsigned int iter = 1;
239 double eps = 1e-6;
240
241 Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(3, numLinks_);
242 Eigen::JacobiSVD<Eigen::MatrixXd> svd(3, numLinks_, Eigen::ComputeFullU | Eigen::ComputeFullV);
243 Eigen::MatrixXd D = Eigen::MatrixXd::Zero(numLinks_, 3);
244
245 // Iterate the jacobian
246 while (e.norm() > eps)
247 {
248 // Get jacobian for this joint configuration
249 Jacobian(joints, jac);
250
251 // Compute inverse of the jacobian
252 // Really unstable
253 // Eigen::MatrixXd jac_inv = ((jac.transpose() * jac).inverse()) * jac.transpose();
254
255 // Moore-Penrose Pseudoinverse
256 svd.compute(jac);
257 const Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType &d = svd.singularValues();
258
259 // "Invert" the singular value matrix
260 for (int i = 0; i < d.size(); ++i)
261 if (d(i) > eps)
262 D(i, i) = 1.0 / d(i);
263 else
264 D(i, i) = 0.0;
265
266 // Inverse is V*D^-1*U^t
267 Eigen::MatrixXd jac_inv = svd.matrixV() * D * svd.matrixU().transpose();
268
269 // Get the joint difference
270 Eigen::VectorXd delta_theta = jac_inv * e;
271
272 // Check for failure
273 if (delta_theta(0) != delta_theta(0)) // nan
274 {
275 // std::cout << jac_inv.matrix() << std::endl;
276 return false;
277 }
278
279 // Increment the current joints by a (small) multiple of the difference
280 joints = joints + (alpha * delta_theta);
281
282 // Figure out the current EE pose and update e.
283 FK(joints, frame);
284 frameToPose(frame, current);
285 // double n = e.norm();
286 e = desired - current;
287 iter++;
288
289 if (iter > 5000)
290 return false; // diverge
291 }
292
293 // Store the solution. Make sure angles are in range [-pi, pi]
294 solution.resize(numLinks_);
295 for (size_t i = 0; i < solution.size(); ++i)
296 {
297 double angle = joints(i);
298 while (angle > M_PI)
299 angle -= 2.0 * M_PI;
300 while (angle < -M_PI)
301 angle += 2.0 * M_PI;
302 solution[i] = angle;
303 }
304
305 return true;
306}
307
308bool PlanarManipulator::FABRIK(std::vector<double> &solution, const Eigen::Affine2d &eeFrame, double xyTol,
309 double thetaTol) const
310{
311 std::vector<double> seed(numLinks_, M_PI / 2.0);
312 return FABRIK(solution, seed, eeFrame, xyTol, thetaTol);
313}
314
315bool PlanarManipulator::FABRIK(std::vector<double> &solution, const std::vector<double> &seed,
316 const Eigen::Affine2d &desiredFrame, double xyTol, double thetaTol) const
317{
318 unsigned int numLinks = getNumLinks(); // have to use this local variable. Compiler error (bug?) in
319 // Eigen::Translation2d when I use numLinks_
320
321 if (seed.size() != numLinks)
322 {
323 std::cerr << "Seed has length " << seed.size() << " but there are " << numLinks << " links" << std::endl;
324 return false;
325 }
326
327 // desired frame is clearly impossible to achieve
328 if (infeasible(desiredFrame))
329 return false;
330
331 // copy the seed into the solution
332 solution.assign(seed.begin(), seed.end());
333
334 // Compute the location of the origin of the last link with the correct rotation
335 // This location will not move
336 Eigen::Vector2d loc(-linkLengths_.back(), 0);
337 loc = desiredFrame * loc;
338
339 // The desired orientation of the end effector
340 double v = desiredFrame.matrix()(0, 0);
341 // Go Go Gadget Numerical Stabilizer!
342 if (v < -1.0)
343 v = -1.0;
344 if (v > 1.0)
345 v = 1.0;
346 double eeTheta = acos(v);
347 if (desiredFrame.matrix()(1, 0) < 0) // sin is negative, so the angle is negative
348 eeTheta = -eeTheta;
349
350 // The location of each joint, starting with the origin and ending with the end effector
351 // For an n-joint chain, jointPositions with have n+1 entries.
352 std::vector<Eigen::Vector2d> jointPositions;
353 jointPositions.push_back(baseFrame_.translation()); // root position
354
355 // Compute the initial locations of each joint
356 Eigen::Affine2d frame(baseFrame_);
357 for (unsigned int i = 0; i < seed.size(); ++i)
358 {
359 Eigen::Affine2d offset(Eigen::Rotation2Dd(seed[i]) * Eigen::Translation2d(linkLengths_[i], 0));
360 frame = frame * offset;
361 jointPositions.push_back(frame.translation());
362 }
363
364 // Store the errors here
365 double xyError = std::numeric_limits<double>::max();
366 double thetaError = std::numeric_limits<double>::max();
367
368 int numIterations = 0;
369 int maxIter = 250; // after this many iterations, declare failure. This is probably WAY too many - tends to
370 // converge very fast, after 1-3 iterations
371 while ((xyError > xyTol || thetaError > thetaTol) && numIterations++ < maxIter)
372 {
373 xyError = thetaError = 0.0;
374
375 // Placing last link where it must go
376 jointPositions[numLinks] = desiredFrame.translation(); // goal position, nailed it.
377 jointPositions[numLinks - 1] = loc; // goal orientation, nailed it.
378
379 // A reverse coordinate frame, working backward along the chain. Used to resolve joint limits in backward phase
380 // VERY IMPORTANT to translate first, then rotate. This NOT like the old gypsy woman said. I'm getting my
381 // money back.
382 Eigen::Affine2d backwardFrame(Eigen::Translation2d(jointPositions[numLinks - 1]) *
383 Eigen::Rotation2Dd(PI + eeTheta)); // end of chain, looking down the chain
384
385 // Move backward from the end effector toward the base
386 for (int i = jointPositions.size() - 3; i >= 0;
387 --i) // -1 is end effector position. Skip -2 to preserve the end effector orientation.
388 {
389 // draw a line between position i+1 and i, and place pt i at the kinematically feasible place along the line
390 double t = linkLengths_[i] / (jointPositions[i + 1] - jointPositions[i]).norm();
391 jointPositions[i] = (1 - t) * jointPositions[i + 1] + t * jointPositions[i];
392
393 // Ensure the angle for joint i is within bounds. If not, move the joint position
394 Eigen::Vector2d vec = backwardFrame.inverse() * jointPositions[i];
395 double angle = -atan2(vec(1), vec(0)); // The angle computed is the negative of the real angle (because we
396 // worked backward).
397
398 // If not in bounds, the angle will be one of the joint limits
399 if (hasBounds_[i] && (angle < lowerBounds_[i] || angle > upperBounds_[i]))
400 {
401 double dlow = fabs(angle - lowerBounds_[i]);
402 double dhigh = fabs(angle - upperBounds_[i]);
403 angle = (dlow < dhigh ? lowerBounds_[i] : upperBounds_[i]);
404 }
405
406 Eigen::Affine2d offset(Eigen::Rotation2Dd(-angle) * Eigen::Translation2d(linkLengths_[i], 0));
407 backwardFrame = backwardFrame * offset;
408 jointPositions[i] = backwardFrame.translation();
409 }
410
411 jointPositions[0] = baseFrame_.translation(); // move base back to where it is supposed to be
412 Eigen::Affine2d forwardFrame(baseFrame_);
413
414 // Move forward toward the end effector
415 for (size_t i = 0; i < jointPositions.size() - 1;
416 ++i) // This pass moves everything except end effector. Orientation at the end may be violated, but we
417 // check for this
418 {
419 // draw a line between position i+1 and i and place i+1 at the kinematically feasible place along the line
420 double t = linkLengths_[i] / (jointPositions[i + 1] - jointPositions[i]).norm();
421 jointPositions[i + 1] = (1 - t) * jointPositions[i] + t * jointPositions[i + 1];
422
423 // Figure out the joint angle required for this
424 Eigen::Vector2d vec = forwardFrame.inverse() * jointPositions[i + 1];
425 double angle = atan2(vec(1), vec(0));
426
427 // If not in bounds, the angle will be one of the joint limits
428 if (hasBounds_[i] && (angle < lowerBounds_[i] || angle > upperBounds_[i]))
429 {
430 double dlow = fabs(angle - lowerBounds_[i]);
431 double dhigh = fabs(angle - upperBounds_[i]);
432 angle = (dlow < dhigh ? lowerBounds_[i] : upperBounds_[i]);
433 }
434 solution[i] = angle;
435
436 // Update frame
437 Eigen::Affine2d offset(Eigen::Rotation2Dd(angle) * Eigen::Translation2d(linkLengths_[i], 0));
438 forwardFrame = forwardFrame * offset;
439 jointPositions[i + 1] = forwardFrame.translation(); // joint location is the translation of the frame. Do
440 // this here in case joint limits changed the angle
441 }
442
443 // Compute translation and orientation error
444 xyError = (jointPositions.back() - desiredFrame.translation()).norm();
445
446 v = forwardFrame.matrix()(0, 0);
447 // Go Go Gadget Numerical Stabilizer!
448 if (v > 1.0)
449 v = 1.0;
450 if (v < -1.0)
451 v = -1.0;
452
453 // The real angle
454 double thetaActual = acos(v);
455 if (forwardFrame.matrix()(1, 0) < 0) // sin is negative, so the angle is negative
456 thetaActual = -thetaActual;
457 thetaError = fabs(eeTheta - thetaActual);
458 }
459
460 // Winning. Extract joint angles. Oh wait. Already done BOOM.
461 if (xyError < xyTol && thetaError < thetaTol)
462 return true;
463
464 return false;
465}
466
467void PlanarManipulator::Jacobian(const double *joints, Eigen::MatrixXd &jac) const
468{
469 if (jac.rows() != 3 || jac.cols() != numLinks_)
470 jac = Eigen::MatrixXd::Zero(3, numLinks_);
471
472 std::vector<double> sins(numLinks_);
473 std::vector<double> coss(numLinks_);
474 double theta = 0.0;
475 for (size_t i = 0; i < numLinks_; ++i)
476 {
477 theta += joints[i];
478 sins[i] = sin(theta);
479 coss[i] = cos(theta);
480 }
481
482 for (size_t i = 0; i < numLinks_; ++i)
483 {
484 double entry1 = 0.0;
485 double entry2 = 0.0;
486 for (size_t j = numLinks_; j > i; --j)
487 {
488 entry1 += -(linkLengths_[j - 1] * sins[j - 1]);
489 entry2 += linkLengths_[j - 1] * coss[j - 1];
490 }
491 jac(0, i) = entry1;
492 jac(1, i) = entry2;
493 jac(2, i) = 1.0;
494 }
495}
496
497// Return the Jacobian for the manipulator at the given joint state.
498void PlanarManipulator::Jacobian(const std::vector<double> &joints, Eigen::MatrixXd &jac) const
499{
500 Jacobian(&joints[0], jac);
501}
502
503void PlanarManipulator::Jacobian(const Eigen::VectorXd &joints, Eigen::MatrixXd &jac) const
504{
505 Jacobian(&joints(0), jac); // TODO: This is untested. I dunno if an eigen::vector is contiguous. It probably is.
506}
507
508void PlanarManipulator::frameToPose(const Eigen::Affine2d &frame, Eigen::VectorXd &pose)
509{
510 pose = Eigen::VectorXd(3);
511 pose(0) = frame.translation()(0);
512 pose(1) = frame.translation()(1);
513 pose(2) = acos(frame.matrix()(0, 0));
514}
515
516bool PlanarManipulator::infeasible(const Eigen::Affine2d &frame) const
517{
518 // Check for impossible query
519 // Compute the location of the origin of the last link with the correct rotation
520 Eigen::Vector2d loc(-linkLengths_[numLinks_ - 1], 0);
521 loc = frame * loc;
522
523 // Length of the chain, except for the last link
524 double len = 0.0;
525 for (size_t i = 0; i < numLinks_ - 1; ++i)
526 len += linkLengths_[i];
527
528 // Make sure the chain (without the last link) is long enough to reach the
529 // origin of the last link
530 Eigen::Vector2d org(baseFrame_.translation());
531 if ((org - loc).norm() > len) // infeasible IK request
532 return true;
533
534 return false;
535}