endeffectors.h
Go to the documentation of this file.
00001 /******************************************************************************
00002 Copyright (c) 2017, 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 _XPP_STATES_ENDEFFECTORS_H_
00031 #define _XPP_STATES_ENDEFFECTORS_H_
00032 
00033 #include <deque>
00034 #include <iostream>
00035 #include <vector>
00036 
00037 #include <xpp_states/state.h>
00038 
00039 namespace xpp {
00040 
00041 using EndeffectorID = uint;
00042 
00058 template<typename T>
00059 class Endeffectors {
00060 public:
00061   using Container     = std::deque<T>;   // only to avoid faulty std::vector<bool>
00062   using EndeffectorsT = Endeffectors<T>;
00063 
00064   Endeffectors (int n_ee = 0);
00065   virtual ~Endeffectors () = default;
00066 
00070   void SetCount(int n_ee);
00071 
00075   void SetAll(const T& value);
00076 
00080   int GetEECount() const;
00081 
00085   std::vector<EndeffectorID> GetEEsOrdered() const;
00086 
00091   T& at(EndeffectorID ee);
00092 
00097   const T& at(EndeffectorID ee) const;
00098 
00099   const EndeffectorsT operator-(const EndeffectorsT& rhs) const;
00100   const EndeffectorsT operator/(double scalar) const;
00101 
00105   bool operator!=(const Endeffectors& other) const;
00106 
00110   Container ToImpl() const;
00111 
00112 private:
00113   Container ee_;
00114 };
00115 
00116 // convenience typedefs, can also be extended to derived classes if desired.
00117 using EndeffectorsPos  = Endeffectors<Eigen::Vector3d>;
00118 using EndeffectorsVel  = Endeffectors<Eigen::Vector3d>;
00119 using EndeffectorsAcc  = Endeffectors<Eigen::Vector3d>;
00120 
00125 class EndeffectorsMotion : public Endeffectors<StateLin3d> {
00126 public:
00131   Endeffectors<Vector3d> Get (MotionDerivative deriv) const
00132   {
00133     Endeffectors<Vector3d> val(GetEECount());
00134     for (auto ee : GetEEsOrdered())
00135       val.at(ee) = at(ee).GetByIndex(deriv);
00136 
00137     return val;
00138   }
00139 };
00140 
00141 
00149 class EndeffectorsContact : public Endeffectors<bool> {
00150 public:
00156   EndeffectorsContact (int n_ee=0, bool in_contact=false)
00157       :Endeffectors(n_ee) { SetAll(in_contact);};
00158 
00162   int GetContactCount() const
00163   {
00164     int count = 0;
00165     for (auto ee : GetEEsOrdered())
00166       if (at(ee))
00167         count++;
00168 
00169     return count;
00170   }
00171 };
00172 
00173 
00174 // implementations
00175 template<typename T>
00176 Endeffectors<T>::Endeffectors (int n_ee)
00177 {
00178   SetCount(n_ee);
00179 }
00180 
00181 template<typename T>
00182 void
00183 Endeffectors<T>::SetCount (int n_ee)
00184 {
00185   ee_.resize(n_ee);
00186 }
00187 
00188 template<typename T>
00189 void
00190 Endeffectors<T>::SetAll (const T& value)
00191 {
00192   std::fill(ee_.begin(), ee_.end(), value);
00193 }
00194 
00195 template<typename T>
00196 T&
00197 Endeffectors<T>::at (EndeffectorID idx)
00198 {
00199   return ee_.at(idx);
00200 }
00201 
00202 template<typename T>
00203 const T&
00204 Endeffectors<T>::at (EndeffectorID idx) const
00205 {
00206   return ee_.at(idx);
00207 }
00208 
00209 template<typename T>
00210 int
00211 Endeffectors<T>::GetEECount () const
00212 {
00213   return ee_.size();
00214 }
00215 
00216 template<typename T>
00217 typename Endeffectors<T>::Container
00218 Endeffectors<T>::ToImpl () const
00219 {
00220   return ee_;
00221 }
00222 
00223 template<typename T>
00224 std::vector<EndeffectorID>
00225 Endeffectors<T>::GetEEsOrdered () const
00226 {
00227   std::vector<EndeffectorID> vec;
00228   for (int i=0; i<ee_.size(); ++i)
00229     vec.push_back(i);
00230 
00231   return vec;
00232 }
00233 
00234 template<typename T>
00235 const typename Endeffectors<T>::EndeffectorsT
00236 Endeffectors<T>::operator - (const EndeffectorsT& rhs) const
00237 {
00238   EndeffectorsT result(ee_.size());
00239   for (auto i : GetEEsOrdered())
00240     result.at(i) = ee_.at(i) - rhs.at(i);
00241 
00242   return result;
00243 }
00244 
00245 template<typename T>
00246 const typename Endeffectors<T>::EndeffectorsT
00247 Endeffectors<T>::operator / (double scalar) const
00248 {
00249   EndeffectorsT result(ee_.size());
00250   for (auto i : GetEEsOrdered())
00251     result.at(i) = ee_.at(i)/scalar;
00252 
00253   return result;
00254 }
00255 
00256 template <typename T>
00257 std::ostream& operator<<(std::ostream& stream, Endeffectors<T> endeffectors)
00258 {
00259   for (EndeffectorID ee : endeffectors.GetEEsOrdered())
00260     stream << endeffectors.at(ee) << ", ";
00261 
00262   return stream;
00263 }
00264 
00265 template<typename T>
00266 bool
00267 Endeffectors<T>::operator!=(const Endeffectors& other) const
00268 {
00269   for (auto ee : GetEEsOrdered()) {
00270     if (ee_.at(ee) != other.at(ee))
00271       return true;
00272   }
00273   return false;
00274 }
00275 
00276 } /* namespace xpp */
00277 
00278 #endif /* _XPP_STATES_ENDEFFECTORS_H_ */


xpp_states
Author(s): Alexander W. Winkler
autogenerated on Fri Apr 5 2019 02:55:50