PlanarManipulator.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, 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 the 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 #ifndef PLANAR_MANIPULATOR_H_
38 #define PLANAR_MANIPULATOR_H_
39 
40 #include <vector>
41 #include <Eigen/Dense>
42 
44 {
45 public:
46  // A numLinks manipulator with equal length links.
47  PlanarManipulator(unsigned int numLinks, double linkLength, const std::pair<double, double> &origin = {0.0, 0.0});
48 
49  // A numLinks manipulator with variable link lengths.
50  PlanarManipulator(unsigned int numLinks, const std::vector<double> &linkLengths,
51  const std::pair<double, double> &origin = {0.0, 0.0});
52 
53  virtual ~PlanarManipulator();
54 
55  // Return the number of links
56  unsigned int getNumLinks() const;
57 
58  // Return the base frame of the manipulator
59  const Eigen::Affine2d &getBaseFrame() const;
60 
61  // Set the base frame of the manipulator
62  void setBaseFrame(const Eigen::Affine2d &frame);
63 
64  // Return the length of each link
65  const std::vector<double> &getLinkLengths() const;
66 
67  // Set the bounds for link # link
68  void setBounds(unsigned int link, double low, double high);
69  // Set the bounds for ALL links
70  void setBounds(const std::vector<double> &low, const std::vector<double> &high);
71 
72  // Return true if the given link has bounds set
73  bool hasBounds(unsigned int link) const;
74  // Get the lower and upper joint angle bounds for this link
75  void getBounds(unsigned int link, double &low, double &high) const;
76  // Get all lower bound joint limits
77  const std::vector<double> &lowerBounds() const;
78  // Get all upper bound joint limits
79  const std::vector<double> &upperBounds() const;
80 
81  // Forward kinematics for the given joint configuration. The frames for links
82  // 1 through the end-effector are returned.
83  void FK(const double *joints, std::vector<Eigen::Affine2d> &frames) const;
84  void FK(const std::vector<double> &joints, std::vector<Eigen::Affine2d> &frames) const;
85  void FK(const Eigen::VectorXd &joints, std::vector<Eigen::Affine2d> &frames) const;
86  void FK(const double *joints, Eigen::Affine2d &eeFrame) const;
87  void FK(const std::vector<double> &joints, Eigen::Affine2d &eeFrame) const;
88  void FK(const Eigen::VectorXd &joints, Eigen::Affine2d &eeFrame) const;
89 
90  // Inverse kinematics for the given end effector frame. Jacobian pseudo-inverse method
91  // Returns false if no solution is found to the given pose.
92  // NOTE: Joint limits are not respected in this IK solver.
93  bool IK(std::vector<double> &solution, const Eigen::Affine2d &eeFrame) const;
94  bool IK(std::vector<double> &solution, const std::vector<double> &seed, const Eigen::Affine2d &desiredFrame) const;
95 
96  // Forward and backward reaching inverse kinematics (FABRIK)
97  // Aristidou and Lasenby, FABRIK: A fast, iterative solver for the inverse kinematics problem.
98  // Graphical Models 73(5): 243–260.
99  bool FABRIK(std::vector<double> &solution, const Eigen::Affine2d &eeFrame, double xyTol = 1e-5,
100  double thetaTol = 1e-3) const;
101  bool FABRIK(std::vector<double> &solution, const std::vector<double> &seed, const Eigen::Affine2d &desiredFrame,
102  double xyTol = 1e-5, double thetaTol = 1e-3) const;
103 
104  // Return the Jacobian for the manipulator at the given joint state.
105  void Jacobian(const std::vector<double> &joints, Eigen::MatrixXd &jac) const;
106  void Jacobian(const Eigen::VectorXd &joints, Eigen::MatrixXd &jac) const;
107  void Jacobian(const double *joints, Eigen::MatrixXd &jac) const;
108 
109  // Convert the given frame to x,y,theta vector
110  static void frameToPose(const Eigen::Affine2d &frame, Eigen::VectorXd &pose);
111 
112 protected:
113  bool infeasible(const Eigen::Affine2d &frame) const;
114 
115  // The number of links in the chain
116  unsigned int numLinks_;
117  // The length of each link
118  std::vector<double> linkLengths_;
119  // Optional bounds on the joint angles. MUST be a subset of [-pi,pi]
120  std::vector<bool> hasBounds_;
121  std::vector<double> lowerBounds_;
122  std::vector<double> upperBounds_;
123 
124  // The base frame of the kinematic chain
125  Eigen::Affine2d baseFrame_;
126 };
127 
128 #endif