00001 /****************************************************************************** 00002 Copyright (c) 2018, Alexander W. Winkler. All rights reserved. 00003 00004 Redistribution and use in source and binary forms, with or without 00005 modification, are permitted provided that the following conditions are met: 00006 00007 * Redistributions of source code must retain the above copyright notice, this 00008 list of conditions and the following disclaimer. 00009 00010 * Redistributions in binary form must reproduce the above copyright notice, 00011 this list of conditions and the following disclaimer in the documentation 00012 and/or other materials provided with the distribution. 00013 00014 * Neither the name of the copyright holder nor the names of its 00015 contributors may be used to endorse or promote products derived from 00016 this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00021 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00022 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00023 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00024 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00025 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00026 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00027 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00028 ******************************************************************************/ 00029 00030 #ifndef TOWR_CONSTRAINTS_RANGE_OF_MOTION_CONSTRAINT_H_ 00031 #define TOWR_CONSTRAINTS_RANGE_OF_MOTION_CONSTRAINT_H_ 00032 00033 #include <towr/variables/spline.h> 00034 #include <towr/variables/spline_holder.h> 00035 #include <towr/variables/euler_converter.h> 00036 00037 #include <towr/models/kinematic_model.h> 00038 #include <towr/parameters.h> 00039 00040 #include "time_discretization_constraint.h" 00041 00042 namespace towr { 00043 00054 class RangeOfMotionConstraint : public TimeDiscretizationConstraint { 00055 public: 00056 using EE = uint; 00057 using Vector3d = Eigen::Vector3d; 00058 00066 RangeOfMotionConstraint(const KinematicModel::Ptr& robot_model, 00067 const Parameters& params, 00068 const EE& ee, 00069 const SplineHolder& spline_holder); 00070 virtual ~RangeOfMotionConstraint() = default; 00071 00072 private: 00073 NodeSpline::Ptr base_linear_; 00074 EulerConverter base_angular_; 00075 NodeSpline::Ptr ee_motion_; 00076 00077 Eigen::Vector3d max_deviation_from_nominal_; 00078 Eigen::Vector3d nominal_ee_pos_B_; 00079 EE ee_; 00080 00081 // see TimeDiscretizationConstraint for documentation 00082 void UpdateConstraintAtInstance (double t, int k, VectorXd& g) const override; 00083 void UpdateBoundsAtInstance (double t, int k, VecBound&) const override; 00084 virtual void UpdateJacobianAtInstance(double t, int k, std::string, Jacobian&) const override; 00085 00086 int GetRow(int node, int dimension) const; 00087 }; 00088 00089 } /* namespace towr */ 00090 00091 #endif /* TOWR_CONSTRAINTS_RANGE_OF_MOTION_CONSTRAINT_H_ */