Loading...
Searching...
No Matches
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{
45public:
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
112protected:
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