text:
/*=============================================================================
  Copyright (c) 2011-2017 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprout

  Distributed under the Boost Software License, Version 1.0. (See accompanying
  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#ifndef SPROUT_CONTAINER_BOOST_ARRAY_HPP
#define SPROUT_CONTAINER_BOOST_ARRAY_HPP

#include <utility>
#include <boost/array.hpp>
#include <sprout/config.hpp>
#include <sprout/workaround/std/cstddef.hpp>
#include <sprout/container/traits.hpp>
#include <sprout/container/range_index_check.hpp>
#include <sprout/iterator/index_iterator.hpp>
#include <sprout/utility/forward.hpp>
#include <sprout/functional/transparent.hpp>

namespace sprout {
	namespace detail {
		template<typename T = void>
		struct elems_subscript;
		template<>
		struct elems_subscript<void>
			: public sprout::transparent<>
		{
		public:
			template<typename T, typename U>
			SPROUT_CONSTEXPR decltype(std::declval<T>().elems[std::declval<U>()])
			operator()(T&& x, U&& y)
			const SPROUT_NOEXCEPT_IF_EXPR(std::declval<T>().elems[std::declval<U>()])
			{
				return SPROUT_FORWARD(T, x).elems[SPROUT_FORWARD(U, y)];
			}
		};
	}	// namespace detail

	//
	// container_traits
	//
	template<typename T, std::size_t N>
	struct container_traits<boost::array<T, N> >
		: public sprout::container_traits_default<boost::array<T, N> >
	{
	public:
		typedef sprout::index_iterator<boost::array<T, N>&, true, sprout::detail::elems_subscript<> > iterator;
		typedef sprout::index_iterator<boost::array<T, N> const&, true, sprout::detail::elems_subscript<> > const_iterator;
	};

	//
	// container_range_traits
	//
	template<typename T, std::size_t N>
	struct container_range_traits<boost::array<T, N> >
		: public sprout::container_range_traits_default<boost::array<T, N> >
	{
	public:
		// iterators:
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> >::iterator
		range_begin(boost::array<T, N>& cont) {
			return typename sprout::container_traits<boost::array<T, N> >::iterator(cont, 0);
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::iterator
		range_begin(boost::array<T, N> const& cont) {
			return typename sprout::container_traits<boost::array<T, N> const>::iterator(cont, 0);
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> >::iterator
		range_end(boost::array<T, N>& cont) {
			return typename sprout::container_traits<boost::array<T, N> >::iterator(cont, cont.size());
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::iterator
		range_end(boost::array<T, N> const& cont) {
			return typename sprout::container_traits<boost::array<T, N> const>::iterator(cont, cont.size());
		}
		// capacity:
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::size_type
		range_size(boost::array<T, N> const& cont) {
			return N;
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::size_type
		range_empty(boost::array<T, N> const& cont) {
			return N != 0;
		}
		// element access:
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> >::reference
		range_front(boost::array<T, N>& cont) {
			return cont.elems[0];
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::reference
		range_front(boost::array<T, N> const& cont) {
			return cont.elems[0];
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> >::reference
		range_back(boost::array<T, N>& cont) {
			return cont.elems[N - 1];
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::reference
		range_back(boost::array<T, N> const& cont) {
			return cont.elems[N - 1];
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> >::reference
		range_subscript_at(boost::array<T, N>& cont, typename sprout::container_traits<boost::array<T, N> >::size_type i) {
			return cont.elems[i];
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::reference
		range_subscript_at(boost::array<T, N> const& cont, typename sprout::container_traits<boost::array<T, N> const>::size_type i) {
			return cont.elems[i];
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> >::reference
		range_at(boost::array<T, N>& cont, typename sprout::container_traits<boost::array<T, N> >::size_type i) {
			return cont.elems[sprout::range_index_check(cont, i)];
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::reference
		range_at(boost::array<T, N> const& cont, typename sprout::container_traits<boost::array<T, N> const>::size_type i) {
			return cont.elems[sprout::range_index_check(cont, i)];
		}
		// data access:
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> >::pointer
		range_data(boost::array<T, N>& cont) {
			return cont.elems;
		}
		static SPROUT_CONSTEXPR typename sprout::container_traits<boost::array<T, N> const>::pointer
		range_data(boost::array<T, N> const& cont) {
			return cont.elems;
		}
	};
}	// namespace sprout

#endif	// #ifndef SPROUT_CONTAINER_BOOST_ARRAY_HPP

max_stars_repo_name: kariya-mitsuru/Sprout

max_issues_repo_name: kariya-mitsuru/Sprout

max_forks_repo_name: kariya-mitsuru/Sprout


text:
#pragma once

#include "value.hpp"
#include <boost/range/iterator_range.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/range/traversal.hpp>

namespace boost {
	namespace adaptors {
		struct value_internal
		{

		};

		namespace {
			const auto valued = value_internal{};
		}
} }

template< class TIter, class TCategory >
class value_iterator
: public boost::iterator_facade< value_iterator< TIter, TCategory >,
		std::decay_t< recursive_value_trait_t< TIter > >,
	  	TCategory,
	  	std::conditional_t< std::is_const< TIter >::value,
	  		const std::decay_t< recursive_value_trait_t< TIter > >,
			      std::decay_t< recursive_value_trait_t< TIter > > >
        >
{
   public:
      using iterator = value_iterator< std::decay_t< TIter >, TCategory >;
      using const_iterator = value_iterator< const std::decay_t< TIter >, TCategory >;
      using difference_type = std::streamoff;

      friend class boost::iterator_core_access;

      value_iterator( std::decay_t< TIter > iter )
      : iter_( iter )
      {
      }

   private:
      bool equal( const iterator & rhs ) const
      {
		  return iter_ == rhs.iter_;
      }

      bool equal( const const_iterator & rhs ) const
      {
		  return iter_ == rhs.iter_;
      }

      void increment()
      {
         ++iter_;
      }

      void decrement()
      {
         --iter_;
      }

      difference_type distance_to( const iterator & rhs) const
      {
         return rhs.iter_ - iter_;
      }

      difference_type distance_to( const const_iterator & rhs) const
      {
         return rhs.iter_ - iter_;
      }

      typename iterator::reference dereference()
      {
		  return recursive_value( iter_ );
      }

      typename const_iterator::reference dereference() const
      {
		  return recursive_value( iter_ );
      }

      void advance( std::streamoff n )
      {
         iter_ += n;
      }

      std::decay_t< TIter > iter_;
};

template< typename SinglePassRange, typename TCategory >
struct value_range
    : boost::iterator_range<
        value_iterator<
            std::conditional_t< std::is_const< SinglePassRange >::value,
                const std::decay_t< typename boost::range_iterator< SinglePassRange >::type >,
                typename boost::range_iterator< SinglePassRange >::type >,
            TCategory
        >
    >
{
	using iterator = value_iterator<
            std::conditional_t< std::is_const< SinglePassRange >::value,
                const std::decay_t< typename boost::range_iterator< SinglePassRange >::type >,
                typename boost::range_iterator< SinglePassRange >::type >,
            TCategory
        >;

    using base_t = boost::iterator_range< iterator >;

    BOOST_RANGE_CONCEPT_ASSERT((
        boost::SinglePassRangeConcept<SinglePassRange>));
public:

    value_range( SinglePassRange & r )
        : base_t( iterator( std::begin( r ) ), iterator( std::end( r ) )  )
    {
    }
};

namespace boost {
	namespace adaptors {

template< typename SinglePassRange >
inline auto operator|( SinglePassRange& r, boost::adaptors::value_internal )
{
    BOOST_RANGE_CONCEPT_ASSERT((
        boost::SinglePassRangeConcept< SinglePassRange >
    ));

    return value_range< SinglePassRange, typename boost::range_traversal< SinglePassRange >::type >( r );
}

template< typename SinglePassRange >
inline auto operator|( const SinglePassRange& r, boost::adaptors::value_internal )
{
    BOOST_RANGE_CONCEPT_ASSERT((
        boost::SinglePassRangeConcept< const SinglePassRange >
    ));

    return value_range< const SinglePassRange, typename boost::range_traversal< const SinglePassRange >::type >( r );
}

template< typename SinglePassRange >
auto value( SinglePassRange && rng )
{
	return rng | boost::adaptors::valued;
}

} } /// namespace

ext: hpp

max_stars_repo_name: rabdumalikov/value

max_issues_repo_name: rabdumalikov/value

max_forks_repo_name: rabdumalikov/value


text:
#ifndef __COUNTER_HPP__
#define __COUNTER_HPP__

#include <fstream>
#include <algorithm>
// #include <boost/unordered_map.hpp>
#include <unordered_map>
using namespace std;
//using namespace boost;

template <class T>
bool asc_sort_pair(T& pair1, T& pair2)
{
    return pair1.first > pair2.first;
}

template <class T>
class Histogram {
public:
    void Add(const T& val)
    {
        typename unordered_map<T, int>::iterator it =
                counts__.find(val);
        if (it == counts__.end()) {
            counts__.insert(pair<T, int>(val, 1));
        }
        else {
            it->second += 1;
        }
        return;
    }

    void GetTopK(int k, vector<pair<T, int> >& values)
    {
        if (counts__.size() <= k) {
            // for (auto t:counts__) {
            for (int i = 0, N = values.size(); i < N; i++) {
                const pair<T, int>& t = values[i];
                values.push_back(pair<T, int>(t.first, t.second));
            }
            return;
        }

        vector<pair<int, T> > scratch(counts__.size());
        // for (auto entry:counts__) {
        for (typename unordered_map<T, int>::const_iterator entry = counts__.begin();
                entry != counts__.end();
                entry++) {
            scratch.push_back(pair<int, T>(entry->second,
                    entry->first));
        }

        partial_sort(scratch.begin(), scratch.begin() + k,
                scratch.end(), asc_sort_pair<pair<int, T> >);

        for (int i = 0; i < k; i++) {
            pair<int, T> tmp = scratch[i];
            values.push_back(pair<T, int>(tmp.second, tmp.first));
        }
        return;
    }

    void Size()
    {
        return counts__.size();
    }

private:
    unordered_map<T, int> counts__;
};

/**
 * Use KeyGenerator if you need to map values to unique ids and also need
 * to retrive the value using the id.
 * If you always need a value to id mapping, use IdGenerator.
 */
template <class T>
class KeyGenerator {
public:
    typedef typename std::map<T, int>::iterator IdMapIterator;
    KeyGenerator() : key_count__(0) {}

    bool Exists(const T& entry)
    {
        IdMapIterator it = id_map__.find(entry);
        return (it != id_map__.end());
    }

    int GetKey(const T& entry)
    {
        int id;
        IdMapIterator it = id_map__.find(entry);
        if (it == id_map__.end()) {
            id = id_map__.size();
            id_map__.insert(std::pair<T, int>(entry, id));
            value_map__.insert(std::pair<int, T>(id, entry));
        }
        else {
            id = it->second;
        }
        return id;
    }

    void GetValue(int id, T& val) const
    {
        typedef typename std::map<int, T>::const_iterator ValueMapIterator;
        ValueMapIterator it = value_map__.find(id);
        val = it->second;
        return;
    }

    int size() const
    {
        return id_map__.size();
    }

private:
    std::map<T, int> id_map__;
    std::map<int, T> value_map__;
    int key_count__;
};

template <class T>
class IdGenerator {
public:
    typedef typename unordered_map<T, int>::iterator iterator;
    typedef typename unordered_map<T, int>::const_iterator const_iterator;
    IdGenerator() {}

    int GetKey(const T& entry)
    {
        int id;
        iterator it = id_map__.find(entry);
        if (it == id_map__.end()) {
            id = id_map__.size();
            id_map__.insert(std::pair<T, int>(entry, id));
        }
        else {
            id = it->second;
        }
        return id;
    }

    int size() const
    {
        return id_map__.size();
    }

    const_iterator begin() const
    {
        return id_map__.begin();
    }

    const_iterator end() const
    {
        return id_map__.end();
    }

    void load(string path)
    {
        ifstream ifs(path.c_str(), ios_base::in);
        if (ifs.is_open() == false) {
            cout << "Failed to open=" << path << endl;
            _exit(1);
            return;
        }

        T key;
        int value, num_pairs(0);
        std::string line;

        while (ifs.good()) {
            getline(ifs, line);
            if (line.size() == 0) {
                continue;
            }
            if (line[0] == '#') {
                continue;
            }
            vector<std::string> tokens;
            std::stringstream strm(line);

            strm >> key >> value;
            id_map__.insert(pair<T, int>(key, value));
            num_pairs++;
        }
        ifs.close();
        cout << "Loaded IdGenerator with " << num_pairs
             << " entries from " << path << endl;
        return;
    }

    void save(string out_path)
    {
        ofstream ofs(out_path.c_str(), ios_base::out);
        for (const_iterator it = id_map__.begin();
                it != id_map__.end(); it++) {
            ofs << it->first << " " << it->second << endl;
        }
        ofs.close();
        return;
    }

private:
    // map<T, int> id_map__;
    unordered_map<T, int> id_map__;
};

template <class T>
class Counter {
public:
    typedef typename unordered_map<T, int>::iterator iterator;
    typedef typename unordered_map<T, int>::const_iterator const_iterator;
    int Add(const T& entry, int count = 1)
    {
        // typedef typename map<T, int>::iterator CounterIterator;
        iterator it = count_table__.find(entry);
        if (it == count_table__.end()) {
            count_table__.insert(pair<T, int>(entry, count));
        } else {
            count = it->second + 1;
            it->second = count;
        }
        return count;
    }

    int Count(const T& entry) const
    {
        // typedef typename map<int, T>::const_iterator CounterIterator;
        const_iterator it = count_table__.find(entry);
        return it->second;
    }

    int size() { return count_table__.size(); }

    void reset()
    {
        count_table__.clear();
    }

    const_iterator begin() const
    {
        return count_table__.begin();
    }

    const_iterator end() const
    {
        return count_table__.end();
    }

private:
    unordered_map<T, int> count_table__;
};

#endif


text:
#include "ompu/io/io_fwd.hpp"

#include "ompu/io/chord_score.hpp"

#pragma warning(push)
#include "ompu/juicy/qi_warnings.hpp"

#include <boost/spirit/include/qi_grammar.hpp>
#include <boost/spirit/include/qi_rule.hpp>
#include <boost/spirit/include/qi_char_class.hpp>
#include <boost/spirit/include/support_unused.hpp>
#pragma warning(pop)

#include <memory>

namespace ompu { namespace io { namespace in {

class ChordPro
    : boost::spirit::qi::grammar<
        in_iterator_type, ChordScore(),
        boost::spirit::qi::standard::blank_type
    >
{
public:
    using root_type = ChordScore;
    using root_rule_type = boost::spirit::qi::rule<
        in_iterator_type,
        root_type(),
        boost::spirit::qi::standard::blank_type
    >;

    explicit ChordPro();

private:
    root_rule_type root_;

    class Impl;
    std::unique_ptr<Impl> impl_;
};

}}} // ompu

max_stars_repo_name: ompu/ompu

max_issues_repo_name: ompu/ompu

max_forks_repo_name: ompu/ompu


text:
#include "api/BookRoute.hpp"
#include "api/CharMapRoute.hpp"
#include "api/DownloadRoute.hpp"
#include "api/LineRoute.hpp"
#include "api/PageRoute.hpp"
#include "api/SearchRoute.hpp"
#include "api/StatisticsRoute.hpp"
#include "api/VersionRoute.hpp"
#include "core/App.hpp"
#include "core/Config.hpp"
#include "core/Route.hpp"
#include "crow.h"
#include <boost/filesystem/operations.hpp>
#include <grp.h>
#include <iostream>
#include <pwd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <vector>

using namespace pcw;
using AppPtr = std::unique_ptr<App>;
static int run(int argc, char **argv);
static void run(App &app);
static AppPtr get_app(int argc, char **argv);
static void create_base_directory(const Config &config);
static std::shared_ptr<Config> config(int argc, char **argv);

////////////////////////////////////////////////////////////////////////////////
int main(int argc, char **argv) {
  try {
    return run(argc, argv);
  } catch (const std::exception &e) {
    CROW_LOG_ERROR << "fatal: " << e.what();
  }
  return 1;
}

////////////////////////////////////////////////////////////////////////////////
int run(int argc, char **argv) {
  auto app = get_app(argc, argv);
  run(*app);
  return 0;
}

////////////////////////////////////////////////////////////////////////////////
void run(App &app) {
  create_base_directory(app.config());
  app.Register(std::make_unique<pcw::BookRoute>());
  app.Register(std::make_unique<pcw::CharMapRoute>());
  app.Register(std::make_unique<pcw::DownloadRoute>());
  app.Register(std::make_unique<pcw::LineRoute>());
  app.Register(std::make_unique<pcw::PageRoute>());
  app.Register(std::make_unique<pcw::SearchRoute>());
  app.Register(std::make_unique<pcw::StatisticsRoute>());
  app.Register(std::make_unique<pcw::VersionRoute>());
  app.run();
}

////////////////////////////////////////////////////////////////////////////////
AppPtr get_app(int argc, char **argv) {
  auto cnf = config(argc, argv);
  auto app = std::make_unique<pcw::App>(cnf);
  app->config().setup_logging();
  app->config().LOG();
  return app;
}

////////////////////////////////////////////////////////////////////////////////
void create_base_directory(const Config &config) {
  boost::system::error_code ec;
  const auto dir = config.daemon.projectdir;
  if (not boost::filesystem::create_directories(dir, ec) and ec.value() != 0) {
    throw std::system_error(ec.value(), std::system_category(),
                            config.daemon.projectdir);
  }
}

////////////////////////////////////////////////////////////////////////////////
std::shared_ptr<Config> config(int argc, char **argv) {
  const char *usage =
      "[-d] [-m mysql-host] [-p mysql-pass] [-u mysql-user] "
      "[-n mysql-table-name] [-l listen-host] [-b projects-dir]";
  const char *flags = "dm:p:n:u:l:b:";
  const char *mpass = "";
  const char *mhost = "pocoweb";
  const char *mname = "pocoweb";
  const char *muser = "pocoweb";
  const char *host = "0.0.0.0:80";
  const char *pdir = "/srv/pocoweb/project-data";
  bool debug = false;

  int c;
  while ((c = getopt(argc, argv, flags)) != -1) {
    switch (c) {
    case 'd':
      debug = true;
      break;
    case 'm':
      mhost = optarg;
      break;
    case 'p':
      mpass = optarg;
      break;
    case 'n':
      mname = optarg;
      break;
    case 'u':
      muser = optarg;
      break;
    case 'l':
      host = optarg;
      break;
    case 'b':
      pdir = optarg;
      break;
    default:
      throw std::runtime_error("Usage: " + std::string(argv[0]) + " " + usage);
    }
  }
  auto ret = std::make_shared<Config>(Config{
      {muser, mhost, mpass, mname, 10, false},
      {host, pdir},
      {debug ? static_cast<int>(crow::LogLevel::Debug)
             : static_cast<int>(crow::LogLevel::Info)},
  });
  return ret;
}

ext: cpp

max_stars_repo_name: cisocrgroup/pocoweb

max_issues_repo_name: cisocrgroup/pocoweb

max_forks_repo_name: cisocrgroup/pocoweb


text:
// ------------------------------------------------------------------------------
// Copyright (c) 2000 Cadenza New Zealand Ltd
// Distributed under the Boost Software License, Version 1.0. (See accompany-
// ing file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
// ------------------------------------------------------------------------------
// Boost functional.hpp header file
// See http://www.boost.org/libs/functional for documentation.
// ------------------------------------------------------------------------------
// $Id$
// ------------------------------------------------------------------------------

#ifndef BOOST_FUNCTIONAL_HPP
#define BOOST_FUNCTIONAL_HPP

#include <boost/config.hpp>
#include <boost/call_traits.hpp>
#include <functional>

namespace boost
{
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // --------------------------------------------------------------------------
    // The following traits classes allow us to avoid the need for ptr_fun
    // because the types of arguments and the result of a function can be
    // deduced.
    //
    // In addition to the standard types defined in unary_function and
    // binary_function, we add
    //
    // - function_type, the type of the function or function object itself.
    //
    // - param_type, the type that should be used for passing the function or
    //   function object as an argument.
    // --------------------------------------------------------------------------
    namespace detail
    {
        template <class Operation>
        struct unary_traits_imp;

        template <class Operation>
        struct unary_traits_imp<Operation*>
        {
            typedef Operation                         function_type;
            typedef const function_type &             param_type;
            typedef typename Operation::result_type   result_type;
            typedef typename Operation::argument_type argument_type;
        };

        template <class R, class A>
        struct unary_traits_imp<R(*)(A)>
        {
            typedef R (*function_type)(A);
            typedef R (*param_type)(A);
            typedef R result_type;
            typedef A argument_type;
        };

        template <class Operation>
        struct binary_traits_imp;

        template <class Operation>
        struct binary_traits_imp<Operation*>
        {
            typedef Operation                                function_type;
            typedef const function_type &                    param_type;
            typedef typename Operation::result_type          result_type;
            typedef typename Operation::first_argument_type  first_argument_type;
            typedef typename Operation::second_argument_type second_argument_type;
        };

        template <class R, class A1, class A2>
        struct binary_traits_imp<R(*)(A1,A2)>
        {
            typedef R (*function_type)(A1,A2);
            typedef R (*param_type)(A1,A2);
            typedef R result_type;
            typedef A1 first_argument_type;
            typedef A2 second_argument_type;
        };
    } // namespace detail

    template <class Operation>
    struct unary_traits
    {
        typedef typename detail::unary_traits_imp<Operation*>::function_type function_type;
        typedef typename detail::unary_traits_imp<Operation*>::param_type    param_type;
        typedef typename detail::unary_traits_imp<Operation*>::result_type   result_type;
        typedef typename detail::unary_traits_imp<Operation*>::argument_type argument_type;
    };

    template <class R, class A>
    struct unary_traits<R(*)(A)>
    {
        typedef R (*function_type)(A);
        typedef R (*param_type)(A);
        typedef R result_type;
        typedef A argument_type;
    };

    template <class Operation>
    struct binary_traits
    {
        typedef typename detail::binary_traits_imp<Operation*>::function_type        function_type;
        typedef typename detail::binary_traits_imp<Operation*>::param_type           param_type;
        typedef typename detail::binary_traits_imp<Operation*>::result_type          result_type;
        typedef typename detail::binary_traits_imp<Operation*>::first_argument_type  first_argument_type;
        typedef typename detail::binary_traits_imp<Operation*>::second_argument_type second_argument_type;
    };

    template <class R, class A1, class A2>
    struct binary_traits<R(*)(A1,A2)>
    {
        typedef R (*function_type)(A1,A2);
        typedef R (*param_type)(A1,A2);
        typedef R result_type;
        typedef A1 first_argument_type;
        typedef A2 second_argument_type;
    };
#else // BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // --------------------------------------------------------------------------
    // If we have no partial specialisation available, decay to a situation
    // that is no worse than in the Standard, i.e., ptr_fun will be required.
    // --------------------------------------------------------------------------

    template <class Operation>
    struct unary_traits
    {
        typedef Operation                         function_type;
        typedef const Operation&                  param_type;
        typedef typename Operation::result_type   result_type;
        typedef typename Operation::argument_type argument_type;
    };

    template <class Operation>
    struct binary_traits
    {
        typedef Operation                                function_type;
        typedef const Operation &                        param_type;
        typedef typename Operation::result_type          result_type;
        typedef typename Operation::first_argument_type  first_argument_type;
        typedef typename Operation::second_argument_type second_argument_type;
    };
#endif // BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION

    // --------------------------------------------------------------------------
    // unary_negate, not1
    // --------------------------------------------------------------------------
    template <class Predicate>
    class unary_negate
        : public std::unary_function<typename unary_traits<Predicate>::argument_type,bool>
    {
      public:
        explicit unary_negate(typename unary_traits<Predicate>::param_type x)
            :
            pred(x)
        {}
        bool operator()(typename call_traits<typename unary_traits<Predicate>::argument_type>::param_type x) const
        {
            return !pred(x);
        }
      private:
        typename unary_traits<Predicate>::function_type pred;
    };

    template <class Predicate>
    unary_negate<Predicate> not1(const Predicate &pred)
    {
        // The cast is to placate Borland C++Builder in certain circumstances.
        // I don't think it should be necessary.
        return unary_negate<Predicate>((typename unary_traits<Predicate>::param_type)pred);
    }

    template <class Predicate>
    unary_negate<Predicate> not1(Predicate &pred)
    {
        return unary_negate<Predicate>(pred);
    }

    // --------------------------------------------------------------------------
    // binary_negate, not2
    // --------------------------------------------------------------------------
    template <class Predicate>
    class binary_negate
        : public std::binary_function<typename binary_traits<Predicate>::first_argument_type,
                                      typename binary_traits<Predicate>::second_argument_type,
                                      bool>
    {
      public:
        explicit binary_negate(typename binary_traits<Predicate>::param_type x)
            :
            pred(x)
        {}
        bool operator()(typename call_traits<typename binary_traits<Predicate>::first_argument_type>::param_type x,
                        typename call_traits<typename binary_traits<Predicate>::second_argument_type>::param_type y) const
        {
            return !pred(x,y);
        }
      private:
        typename binary_traits<Predicate>::function_type pred;
    };

    template <class Predicate>
    binary_negate<Predicate> not2(const Predicate &pred)
    {
        // The cast is to placate Borland C++Builder in certain circumstances.
        // I don't think it should be necessary.
        return binary_negate<Predicate>((typename binary_traits<Predicate>::param_type)pred);
    }

    template <class Predicate>
    binary_negate<Predicate> not2(Predicate &pred)
    {
        return binary_negate<Predicate>(pred);
    }

    // --------------------------------------------------------------------------
    // binder1st, bind1st
    // --------------------------------------------------------------------------
    template <class Operation>
    class binder1st
        : public std::unary_function<typename binary_traits<Operation>::second_argument_type,
                                     typename binary_traits<Operation>::result_type>
    {
      public:
        binder1st(typename binary_traits<Operation>::param_type x,
                  typename call_traits<typename binary_traits<Operation>::first_argument_type>::param_type y)
            :
            op(x), value(y)
        {}

        typename binary_traits<Operation>::result_type
        operator()(typename call_traits<typename binary_traits<Operation>::second_argument_type>::param_type x) const
        {
            return op(value, x);
        }

      protected:
        typename binary_traits<Operation>::function_type op;
        typename binary_traits<Operation>::first_argument_type value;
    };

    template <class Operation>
    inline binder1st<Operation> bind1st(const Operation &op,
                                        typename call_traits<
                                                    typename binary_traits<Operation>::first_argument_type
                                        >::param_type x)
    {
        // The cast is to placate Borland C++Builder in certain circumstances.
        // I don't think it should be necessary.
        return binder1st<Operation>((typename binary_traits<Operation>::param_type)op, x);
    }

    template <class Operation>
    inline binder1st<Operation> bind1st(Operation &op,
                                        typename call_traits<
                                                    typename binary_traits<Operation>::first_argument_type
                                        >::param_type x)
    {
        return binder1st<Operation>(op, x);
    }

    // --------------------------------------------------------------------------
    // binder2nd, bind2nd
    // --------------------------------------------------------------------------
    template <class Operation>
    class binder2nd
        : public std::unary_function<typename binary_traits<Operation>::first_argument_type,
                                     typename binary_traits<Operation>::result_type>
    {
      public:
        binder2nd(typename binary_traits<Operation>::param_type x,
                  typename call_traits<typename binary_traits<Operation>::second_argument_type>::param_type y)
            :
            op(x), value(y)
        {}

        typename binary_traits<Operation>::result_type
        operator()(typename call_traits<typename binary_traits<Operation>::first_argument_type>::param_type x) const
        {
            return op(x, value);
        }

      protected:
        typename binary_traits<Operation>::function_type op;
        typename binary_traits<Operation>::second_argument_type value;
    };

    template <class Operation>
    inline binder2nd<Operation> bind2nd(const Operation &op,
                                        typename call_traits<
                                                    typename binary_traits<Operation>::second_argument_type
                                        >::param_type x)
    {
        // The cast is to placate Borland C++Builder in certain circumstances.
        // I don't think it should be necessary.
        return binder2nd<Operation>((typename binary_traits<Operation>::param_type)op, x);
    }

    template <class Operation>
    inline binder2nd<Operation> bind2nd(Operation &op,
                                        typename call_traits<
                                                    typename binary_traits<Operation>::second_argument_type
                                        >::param_type x)
    {
        return binder2nd<Operation>(op, x);
    }

    // --------------------------------------------------------------------------
    // mem_fun, etc
    // --------------------------------------------------------------------------
    template <class S, class T>
    class mem_fun_t : public std::unary_function<T*, S>
    {
      public:
        explicit mem_fun_t(S (T::*p)())
            :
            ptr(p)
        {}
        S operator()(T* p) const
        {
            return (p->*ptr)();
        }
      private:
        S (T::*ptr)();
    };

    template <class S, class T, class A>
    class mem_fun1_t : public std::binary_function<T*, A, S>
    {
      public:
        explicit mem_fun1_t(S (T::*p)(A))
            :
            ptr(p)
        {}
        S operator()(T* p, typename call_traits<A>::param_type x) const
        {
            return (p->*ptr)(x);
        }
      private:
        S (T::*ptr)(A);
    };

    template <class S, class T>
    class const_mem_fun_t : public std::unary_function<const T*, S>
    {
      public:
        explicit const_mem_fun_t(S (T::*p)() const)
            :
            ptr(p)
        {}
        S operator()(const T* p) const
        {
            return (p->*ptr)();
        }
      private:
        S (T::*ptr)() const;
    };

    template <class S, class T, class A>
    class const_mem_fun1_t : public std::binary_function<const T*, A, S>
    {
      public:
        explicit const_mem_fun1_t(S (T::*p)(A) const)
            :
            ptr(p)
        {}
        S operator()(const T* p, typename call_traits<A>::param_type x) const
        {
            return (p->*ptr)(x);
        }
      private:
        S (T::*ptr)(A) const;
    };

    template<class S, class T>
    inline mem_fun_t<S,T> mem_fun(S (T::*f)())
    {
        return mem_fun_t<S,T>(f);
    }

    template<class S, class T, class A>
    inline mem_fun1_t<S,T,A> mem_fun(S (T::*f)(A))
    {
        return mem_fun1_t<S,T,A>(f);
    }

#ifndef BOOST_NO_POINTER_TO_MEMBER_CONST
    template<class S, class T>
    inline const_mem_fun_t<S,T> mem_fun(S (T::*f)() const)
    {
        return const_mem_fun_t<S,T>(f);
    }

    template<class S, class T, class A>
    inline const_mem_fun1_t<S,T,A> mem_fun(S (T::*f)(A) const)
    {
        return const_mem_fun1_t<S,T,A>(f);
    }
#endif // BOOST_NO_POINTER_TO_MEMBER_CONST

    // --------------------------------------------------------------------------
    // mem_fun_ref, etc
    // --------------------------------------------------------------------------
    template <class S, class T>
    class mem_fun_ref_t : public std::unary_function<T&, S>
    {
      public:
        explicit mem_fun_ref_t(S (T::*p)())
            :
            ptr(p)
        {}
        S operator()(T& p) const
        {
            return (p.*ptr)();
        }
      private:
        S (T::*ptr)();
    };

    template <class S, class T, class A>
    class mem_fun1_ref_t : public std::binary_function<T&, A, S>
    {
      public:
        explicit mem_fun1_ref_t(S (T::*p)(A))
            :
            ptr(p)
        {}
        S operator()(T& p, typename call_traits<A>::param_type x) const
        {
            return (p.*ptr)(x);
        }
      private:
        S (T::*ptr)(A);
    };

    template <class S, class T>
    class const_mem_fun_ref_t : public std::unary_function<const T&, S>
    {
      public:
        explicit const_mem_fun_ref_t(S (T::*p)() const)
            :
            ptr(p)
        {}

        S operator()(const T &p) const
        {
            return (p.*ptr)();
        }
      private:
        S (T::*ptr)() const;
    };

    template <class S, class T, class A>
    class const_mem_fun1_ref_t : public std::binary_function<const T&, A, S>
    {
      public:
        explicit const_mem_fun1_ref_t(S (T::*p)(A) const)
            :
            ptr(p)
        {}

        S operator()(const T& p, typename call_traits<A>::param_type x) const
        {
            return (p.*ptr)(x);
        }
      private:
        S (T::*ptr)(A) const;
    };

    template<class S, class T>
    inline mem_fun_ref_t<S,T> mem_fun_ref(S (T::*f)())
    {
        return mem_fun_ref_t<S,T>(f);
    }

    template<class S, class T, class A>
    inline mem_fun1_ref_t<S,T,A> mem_fun_ref(S (T::*f)(A))
    {
        return mem_fun1_ref_t<S,T,A>(f);
    }

#ifndef BOOST_NO_POINTER_TO_MEMBER_CONST
    template<class S, class T>
    inline const_mem_fun_ref_t<S,T> mem_fun_ref(S (T::*f)() const)
    {
        return const_mem_fun_ref_t<S,T>(f);
    }

    template<class S, class T, class A>
    inline const_mem_fun1_ref_t<S,T,A> mem_fun_ref(S (T::*f)(A) const)
    {
        return const_mem_fun1_ref_t<S,T,A>(f);
    }
#endif // BOOST_NO_POINTER_TO_MEMBER_CONST

    // --------------------------------------------------------------------------
    // ptr_fun
    // --------------------------------------------------------------------------
    template <class Arg, class Result>
    class pointer_to_unary_function : public std::unary_function<Arg,Result>
    {
      public:
        explicit pointer_to_unary_function(Result (*f)(Arg))
            :
            func(f)
        {}

        Result operator()(typename call_traits<Arg>::param_type x) const
        {
            return func(x);
        }

      private:
        Result (*func)(Arg);
    };

    template <class Arg, class Result>
    inline pointer_to_unary_function<Arg,Result> ptr_fun(Result (*f)(Arg))
    {
        return pointer_to_unary_function<Arg,Result>(f);
    }

    template <class Arg1, class Arg2, class Result>
    class pointer_to_binary_function : public std::binary_function<Arg1,Arg2,Result>
    {
      public:
        explicit pointer_to_binary_function(Result (*f)(Arg1, Arg2))
            :
            func(f)
        {}

        Result operator()(typename call_traits<Arg1>::param_type x, typename call_traits<Arg2>::param_type y) const
        {
            return func(x,y);
        }

      private:
        Result (*func)(Arg1, Arg2);
    };

    template <class Arg1, class Arg2, class Result>
    inline pointer_to_binary_function<Arg1,Arg2,Result> ptr_fun(Result (*f)(Arg1, Arg2))
    {
        return pointer_to_binary_function<Arg1,Arg2,Result>(f);
    }
} // namespace boost

#endif

max_stars_repo_name: rudylee/expo

max_issues_repo_name: rudylee/expo

max_forks_repo_name: rudylee/expo


text:
#include "../include/omnibase.h"

#include <Eigen/Geometry>

#include <iostream>

typedef boost::shared_ptr<urdf::Model> URDFModelPtr;
typedef boost::shared_ptr<srdf::Model> SRDFModelPtr;

OmniBase::OmniBase(const std::string &name, const std::string &path_urdf, const std::string &path_srdf)
    : OmniBase::OmniBase(name, path_urdf , path_srdf , 120)
{
}

OmniBase::OmniBase(const std::string &name, const std::string &path_urdf, const std::string &path_srdf, double vel_filter_wc)
    : force_feedback_gain(0),
    last_published_joint5_velocity(0),
    teleop_sensitivity(0),
    teleop_master(true),
    velocity_filter_wc(vel_filter_wc),
    name(name),
    enable_force_flag(false)
{
    node = ros::NodeHandlePtr( new ros::NodeHandle("") );

    // Prepare joint state publisher.
    topic_name = name + "joint_states";
    pub_joint = node->advertise<sensor_msgs::JointState>(topic_name, 10);

    // Prepare joint delta publisher.
    topic_name = name + "joint_delta";
    pub_delta = node->advertise<sensor_msgs::JointState>(topic_name, 10);

    // Prepare pose publisher.
    topic_name = name + "pose";
    pub_pose = node->advertise<geometry_msgs::PoseStamped>(topic_name, 10);
    pose_stamped.header.frame_id = name + "stylus";

    // Prepare pose publisher.
    topic_name = name + "twist";
    pub_twist = node->advertise<geometry_msgs::Twist>(topic_name, 10);

    // Prepare button state publisher and subscribe it.
    topic_name = name + "button_state";
    pub_button = node->advertise<omni_driver::OmniButtonEvent>(topic_name, 10);
    sub_button = node->subscribe(topic_name, 1, &OmniBase::buttonCallback, this);

    // Prepare teleop publisher
    topic_name = name + "teleop";
    pub_teleop_control = node->advertise<omni_driver::TeleopControl>(topic_name, 1);

    // Subscribe omni_control topic.
    topic_name = name + "control";
    sub_torque = node->subscribe(topic_name, 1, &OmniBase::torqueCallback, this);

    // Subscribe enable_control topic.
    topic_name = name + "enable_control";
    sub_enable_control = node->subscribe(topic_name, 1, &OmniBase::enableControlCallback, this);

    // Subscribe and initialize teleoperated joint states.
    topic_name = name + "teleop_joint_states";
    sub_slave_joint_states = node->subscribe(topic_name, 1, &OmniBase::teleopJointStatesCallback, this);
    teleoperated_joint_states.name.resize(6);
    teleoperated_joint_states.position.resize(6);
    teleoperated_joint_states.velocity.resize(6);
    joint_delta_ref.resize(6);
    teleoperated_joint_delta_ref.resize(6);

    // Subscribe to teleop topic if this omni is a slave
    ros::param::param<bool>("~teleop_master", teleop_master, true);
    if (!teleop_master)
        this->teleoperationSlave();
    else
        this->teleoperationForceFeedback();

    // Get the force feedback gain, twist gain, joint states gain and joint states offset
    ros::param::param<double>("~force_feedback_gain", force_feedback_gain, 1);
    ros::param::param<double>("~twist_gain", twist_gain, 1);
    ros::param::get("~joint_states_gain", joint_states_gain);
    if (joint_states_gain.size() != 6)
        throw std::logic_error("Joint states gain is represented as 6 elements vector.");
    ros::param::get("~joint_states_offsets", joint_states_offsets);
    if (joint_states_offsets.size() != 6)
        throw std::logic_error("Joint states offset is represented as 6 elements vector.");

    // Get the link names
    ros::param::param<std::string>("~link_base",         links.base.name, "base");
    ros::param::param<std::string>("~link_torso",        links.torso.name, "torso");
    ros::param::param<std::string>("~link_upper_arm",    links.upper_arm.name, "upper_arm");
    ros::param::param<std::string>("~link_lower_arm",    links.lower_arm.name, "lower_arm");
    ros::param::param<std::string>("~link_wrist",        links.wrist.name, "wrist");
    ros::param::param<std::string>("~link_tip",          links.tip.name, "tip");
    ros::param::param<std::string>("~link_stylus",       links.stylus.name, "stylus");
    ros::param::param<std::string>("~link_end_effector", links.end_effector.name, "end_effector");

    // Get link to teleop rotation
    std::vector<double> rot_data;
    ros::param::get("~rot_link_to_teleop_colwise", rot_data);
    if (rot_data.empty())
        rot_link_to_teleop.setIdentity();
    else if (rot_data.size() == 9)
        rot_link_to_teleop = Eigen::Matrix3d(rot_data.data());
    else
        throw std::logic_error("Rotation matrix is represented by a 9 element array");

    // Initialize teleop_control message fields
    teleop_control.vel_joint.resize(6);
    teleop_control.vel_effector.resize(6);

    std::memset(last_buttons, 0, sizeof(last_buttons));

    // Initializing robot_model for MoveIt!
    URDFModelPtr urdf_model = URDFModelPtr( new urdf::Model() );
    urdf_model->initFile(path_urdf);
    SRDFModelPtr srdf_model = SRDFModelPtr( new srdf::Model() );
    srdf_model->initFile(*urdf_model, path_srdf);

    kinematic_model = robot_model::RobotModelPtr( new robot_model::RobotModel(urdf_model, srdf_model) );

    ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
    kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
    kinematic_state->setToDefaultValues();
    joint_model_group = kinematic_model->getJointModelGroup("all");
    if (!joint_model_group)
    {
        // TODO - quit program
    }
    state.joint_names = joint_model_group->getJointModelNames();
    end_effector_link_model = kinematic_state->getLinkModel(links.end_effector.name);
    //
    const int n = state.joint_names.size();
    joint_state.name.resize(n);
    joint_state.position.resize(n);
    joint_state.velocity.resize(n);
    for (int k = 0; k < n; ++k)
    {
        joint_state.name[k] = name + state.joint_names[k];
    }
}

void OmniBase::enableControlCallback(const std_msgs::Bool::ConstPtr & msg)
{
    enableControl(msg->data);
}

void OmniBase::updateRobotState()
{
    Eigen::VectorXd joint_angles(6);
    joint_angles << state.angles[0],
            state.angles[1],
            state.angles[2],
            state.angles[3],
            state.angles[4],
            state.angles[5];
    kinematic_state->setJointGroupPositions(joint_model_group, joint_angles);
    fwdKin();
//    calculateJointVelocities();   // No need to update the jointvelocity at high rate
//    std::vector<double> filtered_velocities;
//    filterVelocities(filtered_velocities);
//    calculateEffectorVelocities();
}

std::vector<double> OmniBase::calculateJointDeltas(std::vector<bool> button_state, sensor_msgs::JointState joint_state){
    if (button_state[0] || button_state[1]) {
        std::vector<double> joint_delta(6);
        /**
         * Commented for recursion because Tetis joint 4 is mapping Omni joint 5
         */
        // joint_delta.resize(6);
        // for (int i = 0; i < 6; ++i) {
        //     joint_delta[i] = (joint_state.position[i] - joint_delta_ref[i]) + teleoperated_joint_delta_ref[i];
        // }

        joint_delta[0] = joint_states_gain[0] * (joint_state.position[0] - joint_delta_ref[0]) + teleoperated_joint_delta_ref[0];
        joint_delta[1] = joint_states_gain[1] * (joint_state.position[1] - joint_delta_ref[1]) + teleoperated_joint_delta_ref[1];
        joint_delta[2] = joint_states_gain[2] * (joint_state.position[2] - joint_delta_ref[2]) + teleoperated_joint_delta_ref[2];
        joint_delta[3] = joint_states_gain[4] * (joint_state.position[4] - joint_delta_ref[4]) + teleoperated_joint_delta_ref[3];

        return joint_delta;
    }
    else {
        joint_delta_ref = joint_state.position;
        for (int i = 0; i < 4; ++i) {
            teleoperated_joint_delta_ref[i] = teleoperated_joint_states.position[i];
        }
        return teleoperated_joint_states.position;
    }
}

void OmniBase::fwdKin()
{
    Eigen::Affine3d end_effector_state;
    end_effector_state = kinematic_state->getGlobalLinkTransform(links.end_effector.name);
    Eigen::Quaterniond quat(end_effector_state.rotation());
    Eigen::Vector3d pos = end_effector_state.translation();

    state.orientation[0] = quat.w();
    state.orientation[1] = quat.x();
    state.orientation[2] = quat.y();
    state.orientation[3] = quat.z();

    state.position[0] = pos[0];
    state.position[1] = pos[1];
    state.position[2] = pos[2];
}

Eigen::Vector3d OmniBase::calculateTorqueFeedback(const Eigen::Vector3d& force, double feedback_gain)
{
    if (feedback_gain == 0)
        return Eigen::Vector3d::Zero();

    // Get the desired frame
    auto frame = links.lower_arm.name;

    // Get the Jacobian matrix written on the base frame
    Eigen::Vector3d origin(0,0.08,0);
    Eigen::MatrixXd jacobian;
    auto link_model = kinematic_state->getLinkModel(frame);
    kinematic_state->getJacobian(joint_model_group, link_model, origin, jacobian, false);

    // // Rotate the force vector from the desired frame to the base frame
    // Eigen::Vector3d force_on_link_frame = rot_link_to_teleop * force;
    // //
    // auto quat_base_link = kinematic_state->getGlobalLinkTransform(frame).rotation();
    // force_on_link_frame = quat_base_link.conjugate() * force_on_link_frame;

    Eigen::Vector3d force_on_link_frame = force;
    auto ret = feedback_gain * jacobian.block<3,3>(0,0).transpose() * force_on_link_frame;
    return ret.block<3,1>(0,0);
}

void OmniBase::calculateJointVelocities()
{
    double dt = 1/50.0;  // Now run at 50 hz

        for (int i = 0; i < 6; ++i)
        {
            state.vel_error[i] = state.angles[i] - state.vel_z[i];
 //           state.velocities[i] = velocity_filter_wc * state.vel_error[i];    // not sure where wc is set, so i hard coded  30 rad/s
 //           state.vel_z[i] += dt * velocity_filter_wc * state.vel_error[i];
            state.velocities[i] = 30.0 * state.vel_error[i];
            state.vel_z[i] += dt * 30.0 * state.vel_error[i];
        }

        Eigen::VectorXd joint_velocities(6);
        joint_velocities << state.velocities[0],
                state.velocities[1],
                state.velocities[2],
                state.velocities[3],
                state.velocities[4],
                state.velocities[5];
        kinematic_state->setJointGroupVelocities(joint_model_group,joint_velocities);
}

// void OmniBase::calculateVelocities()
// {

//     double deltaAngle;
//     TimeDuration time = state.time_current_angle_acquisition
//             - state.time_last_angle_acquisition;
//     double dt = time.total_microseconds();

//     if (dt >= velocity_filter_minimum_dt)
//     {
//         double tol_dvel = 3; // This is empirical. Deal with it.
//         for (int i = 0; i < 6; ++i)
//         {
//             deltaAngle = state.angles[i] - state.angles_hist1[i];

//             double vel = deltaAngle * 1000000 / dt;

//             if ( std::abs(vel - state.vel_hist1[i]) < tol_dvel )
//             {
//                 state.velocities[i] = vel;
//             }
//             state.vel_hist1[i] = vel;
//         }

//         state.angles_hist1 = state.angles;
//         state.time_last_angle_acquisition = state.time_current_angle_acquisition;
//         Eigen::VectorXd joint_velocities(6);
//         joint_velocities << state.velocities[0],
//                 state.velocities[1],
//                 state.velocities[2],
//                 state.velocities[3],
//                 state.velocities[4],
//                 state.velocities[5];
//         kinematic_state->setJointGroupVelocities(joint_model_group,joint_velocities);
//     }
// }

// void OmniBase::filterVelocities(std::vector<double> &filtered_velocities)
// {

//     if (velocities_filter_size <= 2)
//     {
//         velocities_filter.insert(velocities_filter.begin(), state.velocities);
//         velocities_filter.resize(velocities_filter_size);
//         double velocities_mean = 0;

//         for (unsigned int i =0; i < state.velocities.size(); i++)
//         {
//             for (unsigned int j =0; j < velocities_filter_size; j++)
//             {
//                 velocities_mean += velocities_filter[i][j];
//             }
//             velocities_mean = velocities_mean/velocities_filter_size;
//             filtered_velocities[i] = velocities_mean;
//         }
//         state.velocities = filtered_velocities;
//     }
// }

void OmniBase::calculateEffectorVelocities()
{
    Eigen::Vector3d origin(0,0,0);
    Eigen::MatrixXd jacobian(6,6);

    // To use the pen position  use links.end_effector.name   instead of wrist position   links.tip.name
    auto current_end_effector_link_model = kinematic_state->getLinkModel(links.tip.name);  // Consider the wrist frame (intersection of joint 4-5-6)
    kinematic_state->getJacobian(joint_model_group, current_end_effector_link_model, origin, jacobian, false);

    Eigen::VectorXd thetaDot(6);

    thetaDot(0) = state.velocities[0];
    thetaDot(1) = state.velocities[1];
    thetaDot(2) = state.velocities[2];
    thetaDot(3) = state.velocities[3];
    thetaDot(4) = state.velocities[4];
    thetaDot(5) = state.velocities[5];

    Eigen::VectorXd xDot(6);

    xDot = jacobian * thetaDot;

    for (int i=0; i<6; ++i)
    {
        state.twist[i] = xDot(i);
    }
}

void OmniBase::teleoperationForceFeedback()
{
    sub_force = node->subscribe("force_feedback", 1, &OmniBase::forceFeedbackCallback, this);
}

void OmniBase::forceFeedbackCallback(const std_msgs::Float64MultiArray::ConstPtr& force)
{
    // Optoforce is not precise enough on all axis.
    Eigen::Vector3d force_vector(0, force->data[0], 0);
    Eigen::Vector3d joint_torques = OmniBase::calculateTorqueFeedback(force_vector, force_feedback_gain);
    std::vector<double> torque_input(3);
    std::copy(joint_torques.data(), joint_torques.data() + 3, torque_input.begin());
    this->setTorque(torque_input);
}

void OmniBase::teleoperationSlave()
{
    sub_teleop = node->subscribe("/teleop", 1, &OmniBase::jointTeleopCallback, this);
}

void OmniBase::jointTeleopCallback(const omni_driver::TeleopControl::ConstPtr& msg)
{
    std::vector<double> control;
    switch (msg->mode)
    {
    case 0: // joint space
        if (msg->vel_joint.size() < 3)
        {
            ROS_ERROR("TELEOP: Joint velocity vector should contain at least 3 elements!");
            return;
        }
        control = msg->vel_joint;
        control.resize(3);
        this->setTorque(control);
        break;
    case 1: // cartesian space
        std::cerr << "Control type 1" << std::endl;

        //todo
        break;
    }
}

void OmniBase::buttonCallback(const omni_driver::OmniButtonEvent::ConstPtr &msg)
{
    if (teleop_master)
    {
        // if (msg->grey_button_clicked)
        // {
        //     ++teleop_sensitivity;

        //     ROS_INFO_STREAM(teleop_sensitivity);
        // }
        // if (msg->white_button_clicked)
        // {
        //     if (teleop_sensitivity-- > 0);
        //     else
        //         teleop_sensitivity = 0;

        //     ROS_INFO_STREAM(teleop_sensitivity);
        // }
    }
    else
    {
        if (msg->grey_button_clicked)
        {
            enableControl(true);
            ROS_INFO("TELEOP: EnableControl = true");
        }
        if (msg->white_button_clicked)
        {
            enableControl(false);
            ROS_INFO("TELEOP: EnableControl = false");
        }
    }
}

void OmniBase::publishOmniState()
{
    if (!this->connected() /*|| !this->calibrated()*/)
    {
        // Phantom Omni is not open or calibrated. Don't publish.
        return;
    }

    // Get the joint angles from the omni.
    std::vector<double> joint_angles, joint_velocities;
    this->getJointAngles(joint_angles);
    this->getJointVelocities(joint_velocities);

    // Calculate velocity at 50 hz
    calculateJointVelocities();
    calculateEffectorVelocities();

    // Publish the joint state.
    joint_state.header.stamp = ros::Time::now();

    for (int i = 0; i < 6; ++i)
    {
        joint_state.position[i] = joint_angles[i] + joint_states_offsets[i];
        joint_state.velocity[i] = joint_states_gain[i] * joint_velocities[i];
    }

    // Publish the joint state;
    pub_joint.publish(joint_state);

    // Publish the end effector pose.
    pose_stamped.header.stamp = ros::Time::now();
    pose_stamped.pose.position.x = state.position[0];
    pose_stamped.pose.position.y = state.position[1];
    pose_stamped.pose.position.z = state.position[2];
    pose_stamped.pose.orientation.w = state.orientation[0];
    pose_stamped.pose.orientation.x = state.orientation[1];
    pose_stamped.pose.orientation.y = state.orientation[2];
    pose_stamped.pose.orientation.z = state.orientation[3];
    pub_pose.publish(pose_stamped);

    // Publish the button event.
    std::vector<bool> button_state;
    this->getButtonsState(button_state);
    button_event.grey_button = button_state[0];
    button_event.white_button = button_state[1];
    button_event.grey_button_clicked = !last_buttons[0] && button_state[0];
    button_event.white_button_clicked = !last_buttons[1] && button_state[1];
    last_buttons[0] = button_state[0];
    last_buttons[1] = button_state[1];
    pub_button.publish(button_event);

    // Publish the twist.
    if (button_state[0] || button_state[1]) {
        twist.linear.x = twist_gain * state.twist[0];
        twist.linear.y = twist_gain * state.twist[1];
        twist.linear.z = twist_gain * state.twist[2];
        twist.angular.x = twist_gain * state.twist[3];
        twist.angular.y = twist_gain * state.twist[4];
        twist.angular.z = twist_gain * state.twist[5];
    }
    else {
        twist.linear.x = 0;
        twist.linear.y = 0;
        twist.linear.z = 0;
        twist.angular.x = 0;
        twist.angular.y = 0;
        twist.angular.z = 0;
    }
    pub_twist.publish(twist);

    // Publish the teleoperation data.
    if (teleop_master)
    {
        // Publish the joint delta.
        std_msgs::Float64MultiArray joint_delta;
        joint_delta.data = calculateJointDeltas(button_state, joint_state);
        sensor_msgs::JointState joint_state_delta;

        joint_state_delta.header.stamp = ros::Time::now();
        joint_state_delta.name.resize(6);
        joint_state_delta.position.resize(6);
        joint_state_delta.velocity.resize(6);
        for (int i = 0; i < 6; ++i) {
            joint_state_delta.position[i] = joint_delta.data[i];
        }
        pub_delta.publish(joint_state_delta);
        // Publish teleop data for other omni.
        const double sensitivity_step = 0.001;
        for (unsigned int k = 0; k < state.velocities.size(); ++k)
        {
            teleop_control.vel_joint[k] = teleop_sensitivity * sensitivity_step * state.velocities[k];
        }
        teleop_control.vel_effector = state.twist;
        teleop_control.mode = 0;
        pub_teleop_control.publish(teleop_control);
    }

    // Check if the device is frozen. This usually happens when the read buffer
    // is too small and it is completely filled (at least that is what we think).
    if (state.velocities[5] == last_published_joint5_velocity)
    {
        if (++state.freeze_count > MAX_FREEZE_COUNT)
        {
            state.freeze_count = 0;
            this->wakeup();
        }
    }
    else
    {
        state.freeze_count = 0;
    }
    last_published_joint5_velocity = state.velocities[5];
}

void OmniBase::torqueCallback(const geometry_msgs::Vector3::ConstPtr & msg)
{
    std::vector<double> input = {msg->x, msg->y, msg->z};
    setTorque(input);
}

void OmniBase::teleopJointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
    for (int i = 0; i < 4; ++i) {
        teleoperated_joint_states.position[i] = msg->position[i];
    }
}


text:
/************************************************************************/
/*                                                                      */
/*               Copyright 2010-2011 by Ullrich Koethe                  */
/*                                                                      */
/*    This file is part of the VIGRA computer vision library.           */
/*    The VIGRA Website is                                              */
/*        http://hci.iwr.uni-heidelberg.de/vigra/                       */
/*    Please direct questions, bug reports, and contributions to        */
/*        ullrich.koethe@iwr.uni-heidelberg.de    or                    */
/*        vigra@informatik.uni-hamburg.de                               */
/*                                                                      */
/*    Permission is hereby granted, free of charge, to any person       */
/*    obtaining a copy of this software and associated documentation    */
/*    files (the "Software"), to deal in the Software without           */
/*    restriction, including without limitation the rights to use,      */
/*    copy, modify, merge, publish, distribute, sublicense, and/or      */
/*    sell copies of the Software, and to permit persons to whom the    */
/*    Software is furnished to do so, subject to the following          */
/*    conditions:                                                       */
/*                                                                      */
/*    The above copyright notice and this permission notice shall be    */
/*    included in all copies or substantial portions of the             */
/*    Software.                                                         */
/*                                                                      */
/*    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND    */
/*    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES   */
/*    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND          */
/*    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT       */
/*    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,      */
/*    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      */
/*    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR     */
/*    OTHER DEALINGS IN THE SOFTWARE.                                   */
/*                                                                      */
/************************************************************************/

#define PY_ARRAY_UNIQUE_SYMBOL vigranumpycore_PyArray_API
#define NO_IMPORT_ARRAY

#include <vigra/numpy_array.hxx>
#include <vigra/axistags.hxx>
#include <boost/python.hpp>
#include <boost/python/slice.hpp>

// disable axistag construction from json for the moment,
// because these libraries require boost 1.41, whereas
// our Ubuntu longterm support has only boost 1.40
//
// as a workaround, we will do this in Python using the 'json' module
#define VIGRA_DISABLE_FROMJSON

#ifndef VIGRA_DISABLE_FROMJSON
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#endif

namespace python = boost::python;

namespace vigra {

template<class T>
inline PyObject * managingPyObject(T *p)
{
    return typename python::manage_new_object::apply<T *>::type()(p);
}

template<class Copyable>
python::object
generic__copy__(python::object copyable)
{
    Copyable* newCopyable(new Copyable(python::extract<const Copyable &>(copyable)()));
    python::object result(python::detail::new_reference(managingPyObject(newCopyable)));

    python::extract<python::dict>(result.attr("__dict__"))().update(copyable.attr("__dict__"));

    return result;
}

template<class Copyable>
python::object
generic__deepcopy__(python::object copyable, python::dict memo)
{
    python::object copyMod = python::import("copy");
    python::object deepcopy = copyMod.attr("deepcopy");
    python::object builtin = python::import("__builtin__");
    python::object globals = builtin.attr("__dict__");

    Copyable* newCopyable(new Copyable(python::extract<const Copyable &>(copyable)()));
    python::object result(python::detail::new_reference(managingPyObject(newCopyable)));

    python::dict locals;
    locals["copyable"] = copyable;
    size_t copyableId = python::extract<size_t>(python::eval("id(copyable)", globals, locals))();
    memo[copyableId] = result;

    python::object dict_copy = deepcopy(python::extract<python::dict>(copyable.attr("__dict__"))(),
                                        memo);
    python::extract<python::dict>(result.attr("__dict__"))().update(dict_copy);
    return result;
}

AxisInfo AxisInfo__call__(AxisInfo const & i, double resolution, std::string const & description)
{
    return AxisInfo(i.key(), i.typeFlags(), resolution, description);
}

AxisInfo AxisInfo_x()
{
    return AxisInfo::x();
}

AxisInfo AxisInfo_y()
{
    return AxisInfo::y();
}

AxisInfo AxisInfo_z()
{
    return AxisInfo::z();
}

AxisInfo AxisInfo_t()
{
    return AxisInfo::t();
}

AxisInfo AxisInfo_fx()
{
    return AxisInfo::fx();
}

AxisInfo AxisInfo_fy()
{
    return AxisInfo::fy();
}

AxisInfo AxisInfo_fz()
{
    return AxisInfo::fz();
}

AxisInfo AxisInfo_ft()
{
    return AxisInfo::ft();
}

AxisInfo AxisInfo_c()
{
    return AxisInfo::c();
}

#ifndef VIGRA_DISABLE_FROMJSON
AxisTags AxisTags::fromJSON(std::string const & repr)
{
    using boost::property_tree::ptree;

    std::istringstream s(repr);
    ptree pt;
    read_json(s, pt);

    AxisTags res;
    for(ptree::iterator v = pt.get_child("axes").begin();
                         v != pt.get_child("axes").end(); ++v)
    {
        std::string key(v->second.get<std::string>("key"));
        unsigned int typeFlags(v->second.get<unsigned int>("typeFlags"));
        double resolution(v->second.get<double>("resolution"));
        std::string description(v->second.get<std::string>("description"));

        res.push_back(AxisInfo(key, (AxisInfo::AxisType)typeFlags, resolution, description));
    }
    return res;
}

AxisTags * AxisTags_readJSON(std::string const & repr)
{
    return new AxisTags(AxisTags::fromJSON(repr));
}
#endif

AxisTags *
AxisTags_create(python::object i1, python::object i2,
                python::object i3, python::object i4, python::object i5)
{
    VIGRA_UNIQUE_PTR<AxisTags> res(new AxisTags());

    python::extract<AxisTags const &> tags(i1);
    if(tags.check())
    {
        res = VIGRA_UNIQUE_PTR<AxisTags>(new AxisTags(tags()));
    }
    else if(PyString_Check(i1.ptr()))
    {
        res = VIGRA_UNIQUE_PTR<AxisTags>(new AxisTags(python::extract<std::string>(i1)()));
    }
    else if(PySequence_Check(i1.ptr()))
    {
        int size = len(i1);
        for(int k=0; k<size; ++k)
        {
            python::extract<AxisInfo const &> info(i1[k]);
            if(!info.check())
            {
                PyErr_SetString(PyExc_TypeError, "AxisTags(): Argument must be a sequence of AxisInfo objects.");
                python::throw_error_already_set();
            }
            res->push_back(info());
        }
    }
    else if(PyInt_Check(i1.ptr()))
    {
        int size = python::extract<int>(i1)();
        for(int k=0; k<size; ++k)
            res->push_back(AxisInfo());
    }
    else
    {
        if(i1 != python::object())
        {
            python::extract<AxisInfo const &> info(i1);
            if(!info.check())
            {
                PyErr_SetString(PyExc_TypeError, "AxisTags(): Argument must be a sequence of AxisInfo objects.");
                python::throw_error_already_set();
            }
            res->push_back(info());
        }
        if(i2 != python::object())
        {
            python::extract<AxisInfo const &> info(i2);
            if(!info.check())
            {
                PyErr_SetString(PyExc_TypeError, "AxisTags(): Argument must be a sequence of AxisInfo objects.");
                python::throw_error_already_set();
            }
            res->push_back(info());
        }
        if(i3 != python::object())
        {
            python::extract<AxisInfo const &> info(i3);
            if(!info.check())
            {
                PyErr_SetString(PyExc_TypeError, "AxisTags(): Argument must be a sequence of AxisInfo objects.");
                python::throw_error_already_set();
            }
            res->push_back(info());
        }
        if(i4 != python::object())
        {
            python::extract<AxisInfo const &> info(i4);
            if(!info.check())
            {
                PyErr_SetString(PyExc_TypeError, "AxisTags(): Argument must be a sequence of AxisInfo objects.");
                python::throw_error_already_set();
            }
            res->push_back(info());
        }
        if(i5 != python::object())
        {
            python::extract<AxisInfo const &> info(i5);
            if(!info.check())
            {
                PyErr_SetString(PyExc_TypeError, "AxisTags(): Argument must be a sequence of AxisInfo objects.");
                python::throw_error_already_set();
            }
            res->push_back(info());
        }
    }

    return res.release();
}

void AxisTags_insertChannelAxis(AxisTags & axistags)
{
    int k = axistags.channelIndex();
    vigra_precondition(k == (int)axistags.size(),
         "AxisTags::insertChannelAxis(): already has a channel axis.");
    if(detail::defaultOrder() == "F")
        axistags.insert(0, AxisInfo::c());
    else
        axistags.push_back(AxisInfo::c());
}

AxisInfo & AxisTags_getitem(AxisTags & axistags, int index)
{
    if(index < 0)
        index += axistags.size();

    if(index >= (int)axistags.size())
    {
        PyErr_SetString(PyExc_IndexError, "AxisTags.__getitem__(): Invalid index or key.");
        python::throw_error_already_set();
    }

    return axistags.get(index);
}

std::string AxisTags_str(AxisTags const & axistags)
{
    std::string res;
    for(unsigned int k=0; k<axistags.size(); ++k)
        res += axistags.get(k).repr() + "\n";
    return res;
}

python::object
AxisTags_permutationToNormalOrder(AxisTags const & axistags)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationToNormalOrder(permutation);
    return python::object(permutation);
}

python::object
AxisTags_permutationToNormalOrder2(AxisTags const & axistags, unsigned int types)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationToNormalOrder(permutation, (AxisInfo::AxisType)types);
    return python::object(permutation);
}

python::object
AxisTags_permutationFromNormalOrder(AxisTags const & axistags)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationFromNormalOrder(permutation);
    return python::object(permutation);
}

python::object
AxisTags_permutationFromNormalOrder2(AxisTags const & axistags, unsigned int types)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationFromNormalOrder(permutation, (AxisInfo::AxisType)types);
    return python::object(permutation);
}

python::object
AxisTags_permutationToNumpyOrder(AxisTags const & axistags)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationToNumpyOrder(permutation);
    return python::object(permutation);
}

python::object
AxisTags_permutationFromNumpyOrder(AxisTags const & axistags)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationFromNumpyOrder(permutation);
    return python::object(permutation);
}

python::object
AxisTags_permutationToVigraOrder(AxisTags const & axistags)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationToVigraOrder(permutation);
    return python::object(permutation);
}

python::object
AxisTags_permutationFromVigraOrder(AxisTags const & axistags)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationFromVigraOrder(permutation);
    return python::object(permutation);
}

python::object
AxisTags_permutationToOrder(AxisTags const & axistags, std::string const & order)
{
    ArrayVector<npy_intp> permutation;
    axistags.permutationToOrder(permutation, order);
    return python::object(permutation);
}

AxisTags *
AxisTags_transform(AxisTags const & oldTags, python::object index, int lnew)
{
    VIGRA_UNIQUE_PTR<AxisTags> newTags(new AxisTags());
    python::object ellipsis = python::object(python::detail::borrowed_reference(Py_Ellipsis));
    int lold = oldTags.size();
    if(!PySequence_Check(index.ptr()))
    {
        index = python::make_tuple(index);
    }
    int lindex = len(index);
    int lnewaxis = 0, lellipsis = 0;
    for(int k=0; k<lindex; ++k)
    {
        python::object item(index[k]);
        if(item == python::object() || python::extract<AxisInfo const &>(item).check())
            ++lnewaxis;
        else if(item == ellipsis)
            ++lellipsis;
    }
    lindex -= lnewaxis;
    if(lindex < lold && lellipsis == 0)
    {
        index += python::make_tuple(ellipsis);
        ++lindex;
    }
    lellipsis = lold - lindex;
    int knew = 0, kold = 0, kindex = 0;
    while(knew < lnew)
    {
        python::object item = index[kindex];
        if(PyInt_Check(item.ptr()))
        {
            ++kold;
            ++kindex;
        }
        else
        {
            if(item != python::object())
            {
                python::extract<AxisInfo const &> newaxis(item);

                if(newaxis.check())
                {
                    newTags->push_back(newaxis);
                }
                else
                {
                    newTags->push_back(oldTags.get(kold));
                    // adjust the resolution if item has a valid 'step' member
                    python::extract<python::slice> slice(item);
                    if(slice.check())
                    {
                        python::extract<int> step(slice().step());
                        if(step.check())
                        {
                            newTags->get(knew).resolution_ *= step();
                        }
                    }
                    ++kold;
                }
            }
            else
            {
                newTags->push_back(AxisInfo());
            }
            ++knew;
            if(lellipsis > 0 && item == ellipsis)
                --lellipsis;
            else
                ++kindex;
        }
    }
    return newTags.release();
}

// #if 0
// void printAxistags(NumpyAnyArray a)
// {
    // python::object array(python::detail::borrowed_reference(a.pyObject()));
    // python::object tags(getattr(array, "axistags", PyAxisTags()));
    // std::cerr << "Axistags via boost::python:\n";
    // std::cerr << python::extract<PyAxisTags const &>(tags)().repr();

    // std::cerr << "Axistags via C-API:\n";
    // if(PyObject_HasAttrString(a.pyObject(), "axistags"))
    // {
        // python::object tags(python::detail::new_reference(PyObject_GetAttrString(a.pyObject(), "axistags")));
        // std::cerr << python::extract<PyAxisTags const &>(tags)().repr();
    // }
    // else
    // {
        // std::cerr << "attribute 'axistags' missing\n";
    // }
// }
// #endif

void defineAxisTags()
{
    using namespace boost::python;

    docstring_options doc_options(true, false, false);

    enum_<AxisInfo::AxisType>("AxisType",
         "\nEnum to encode the type of an axis described by an\n"
         ":class:`~vigra.AxisInfo` object. Possible values:\n\n"
         "   ``AxisType.Channels:``\n      a channel axis\n"
         "   ``AxisType.Space:``\n      a spatial axis\n"
         "   ``AxisType.Angle:``\n      an axis encoding angles (e.g. polar coordinates)\n"
         "   ``AxisType.Time:``\n      a temporal axis\n"
         "   ``AxisType.Frequency:``\n      an axis in the Fourier domain\n"
         "   ``AxisType.UnknownAxisType:``\n      type not specified\n"
         "   ``AxisType.NonChannel:``\n      any type except Channels\n"
         "   ``AxisType.AllAxes:``\n      any type\n\n"
         "Types can be combined by the bitwise 'or' operator. For example,\n"
         "``Space | Frequency`` denotes a spatial axis in the Fourier domain.\n\n")
        .value("UnknownAxisType", AxisInfo::UnknownAxisType)
        .value("Channels", AxisInfo::Channels)
        .value("Space", AxisInfo::Space)
        .value("Angle", AxisInfo::Angle)
        .value("Time", AxisInfo::Time)
        .value("Frequency", AxisInfo::Frequency)
        .value("NonChannel", AxisInfo::NonChannel)
        .value("AllAxes", AxisInfo::AllAxes)
    ;

    class_<AxisInfo>("AxisInfo",
         "\n"
         "An object describing a single axis.\n\nConstructor:\n\n"
         ".. method:: AxisInfo(key='?', typeFlags=AxisType.UnknownAxisType, resolution=0.0, description='')\n\n"
         "    :param key: the key of the axis,\n"
         "                e.g. 'x' (x-axis), 'c' (channel axis), '?' (unknown)\n"
         "    :type key: string\n"
         "    :param typeFlags: the type of the axis,\n"
         "                      e.g. AxisType.Space or AxisType.Time\n"
         "    :type typeFlags: :class:`~vigra.AxisType`\n"
         "    :param resolution: the resolution (step size) of the axis\n"
         "                       (e.g. 0.0 means 'unknown')\n"
         "    :param description: an arbitrary string giving additional information \n"
         "                        about the axis.\n\n"
         "In addition, AxisInfo defines the following factories for the most common\n"
         "cases:\n\n"
         "   ``AxisInfo.c`` or ``AxisInfo.c(description='a description')``:\n"
         "        Factory for an axisinfo object describing the 'c' (channel) axis.\n"
         "   ``AxisInfo.x`` or ``AxisInfo.x(resolution=0.0, description='')``:\n"
         "        Factory for an axisinfo object describing the 'x' (spatial) axis.\n"
         "   ``AxisInfo.y`` or ``AxisInfo.y(resolution=0.0, description='')``:\n"
         "        Factory for an axisinfo object describing the 'y' (spatial) axis.\n"
         "   ``AxisInfo.z`` or ``AxisInfo.z(resolution=0.0, description='')``:\n"
         "        Factory for an axisinfo object describing the 'z' (spatial) axis.\n"
         "   ``AxisInfo.t`` or ``AxisInfo.t(resolution=0.0, description='')``:\n"
         "        Factory for an axisinfo object describing the 't' (time) axis.\n"
         "   ``AxisInfo.fx`` or ``AxisInfo.fx(resolution=0.0, description='')``:\n"
         "        Factory for an axisinfo object describing the 'x' axis\n"
         "        in the Fourier domain.\n"
         "   ``AxisInfo.fy`` or ``AxisInfo.fy(resolution=0.0, description='')``:\n"
         "        Factory for an axisinfo object describing the 'y' axis\n"
         "        in the Fourier domain.\n"
         "   ``AxisInfo.fz`` or ``AxisInfo.fz(resolution=0.0, description='')``:\n"
         "        Factory for an axisinfo object describing the 'z' axis\n"
         "        in the Fourier domain.\n"
         "   ``AxisInfo.ft`` or ``AxisInfo.ft(resolution=0.0, description='')``:\n"
         "        Factory for an axisinfo object describing the 't' axis\n"
         "        in the Fourier domain.\n\n",
         no_init)
        .def(init<std::string, AxisInfo::AxisType, double, std::string>(
             (arg("key")="?", arg("typeFlags")=AxisInfo::UnknownAxisType,
              arg("resolution")=0.0, arg("description")="")))
        .def(init<AxisInfo const &>())
        .def_readonly("key", &AxisInfo::key_,
             "\n(read-only property, type 'string') the key of the axis.\n")
        .def_readwrite("description", &AxisInfo::description_,
             "\n(read/write property, type 'string') the string description of the axis.\n")
        .def_readwrite("resolution", &AxisInfo::resolution_,
             "\n(read/write property, type 'float') the resolution of the axis. The resolution\n"
             "will be automatically adjusted whenever the image size changes, e.g. due to a call\n"
             "to :func:`~vigra.sampling.resize` or slicing with non-unit step size::\n\n"
             "    >>> a = vigra.RGBImage((200,100))\n"
             "    >>> a.axistags['x'].resolution = 1.0\n"
             "    >>> a.axistags['y'].resolution = 1.2\n"
             "    >>> print a.axistags\n"
             "    AxisInfo: 'x' (type: Space, resolution=1)\n"
             "    AxisInfo: 'y' (type: Space, resolution=1.2)\n"
             "    AxisInfo: 'c' (type: Channels) RGB\n"
             "    >>> b = a[::2, ::4, :]\n"
             "    >>> print b.axistags\n"
             "    AxisInfo: 'x' (type: Space, resolution=2)\n"
             "    AxisInfo: 'y' (type: Space, resolution=4.8)\n"
             "    AxisInfo: 'c' (type: Channels) RGB\n\n")
        .def_readonly("typeFlags", &AxisInfo::flags_,
             "\n(read-only property, type :class:`~vigra.AxisType`) the type of the axis .\n")
        .def("toFrequencyDomain", &AxisInfo::toFrequencyDomain, (arg("size") = 0, arg("sign") = 1))
        .def("fromFrequencyDomain", &AxisInfo::fromFrequencyDomain, (arg("size") = 0))
        .def("isSpatial", &AxisInfo::isSpatial,
             "\naxisinfo.isSSpactial() yields True when :attr:`~vigra.AxisInfo.typeFlags` "
             "contains AxisType.Space\n")
        .def("isTemporal", &AxisInfo::isTemporal,
             "\naxisinfo.isTemporal() yields True when :attr:`~vigra.AxisInfo.typeFlags` "
             "contains AxisType.Time\n")
        .def("isChannel", &AxisInfo::isChannel,
             "\naxisinfo.isChannel() yields True when :attr:`~vigra.AxisInfo.typeFlags` "
             "contains AxisType.Channels\n")
        .def("isFrequency", &AxisInfo::isFrequency,
             "\naxisinfo.isFrequency() yields True when :attr:`~vigra.AxisInfo.typeFlags` "
             "contains AxisType.Frequency\n")
        .def("isAngular", &AxisInfo::isAngular,
             "\naxisinfo.isAngular() yields True when :attr:`~vigra.AxisInfo.typeFlags` "
             "contains AxisType.Angle\n")
        .def("isType", &AxisInfo::isType,
             "\naxisinfo.isType(axistype) yields True when :attr:`~vigra.AxisInfo.typeFlags` "
             "contains the given axistype.\n")
        .def("compatible", &AxisInfo::compatible,
             "\naxisinfo1.compatible(axisinfo2) yields True when the two axisinfo objects "
             "have the same keys and types, or either of the two is 'unknown'.\n")
        .def(self == self)
        .def(self != self)
        .def(self < self)
        .def(self <= self)
        .def(self > self)
        .def(self >= self)
        .def("__copy__", &generic__copy__<AxisInfo>)
        .def("__deepcopy__", &generic__deepcopy__<AxisInfo>)
        .def("__repr__", &AxisInfo::repr)
        .def("__call__", &AxisInfo__call__, (arg("resolution") = 0.0, arg("description") = ""))
        .add_static_property("x", &AxisInfo_x)
        .add_static_property("y", &AxisInfo_y)
        .add_static_property("z", &AxisInfo_z)
        .add_static_property("t", &AxisInfo_t)
        .add_static_property("fx", &AxisInfo_fx)
        .add_static_property("fy", &AxisInfo_fy)
        .add_static_property("fz", &AxisInfo_fz)
        .add_static_property("ft", &AxisInfo_ft)
        .add_static_property("c", &AxisInfo_c)
    ;

    class_<AxisTags >("AxisTags",
            "Object to describe axis properties and axis ordering in a "
            ":class:`~vigra.VigraArray`. \n\nConstructor:\n\n"
            ".. method:: AxisTags(i1=None, i2=None, i3=None, i4=None, i5=None)\n\n"
            "    The parameters 'i1'...'i5' are the :class:`~vigra.AxisInfo` objects\n"
            "    describing the axes. If all are None, an empty AxisTags object is\n"
            "    created. Alternatively, 'i1' can also be a Python sequence of\n"
            "    :class:`~vigra.AxisInfo` objects, or an integer (in which case an\n"
            "    AxisTags object with that many '?' entries is created).\n\n"
            "Most AxisTags methods should not be called directly, but via the\n"
            "corresponding array methods, because this ensures that arrays and\n"
            "their axistags are always kept in sync (rule of thumb: if an axistags\n"
            "function is not documented, you call it on your own risk).\n\n"
            "The entries of an axistags object (i.e. the individual axisinfo objects)\n"
            "can be accessed via the index operator, where the argument can either be\n"
            "the axis index or the axis key::\n\n"
            "    >>> print array.axistags[0]\n"
            "    AxisInfo: 'x' (type: Space, resolution=1.2)\n"
            "    >>> print array.axistags['x']\n"
            "    AxisInfo: 'x' (type: Space, resolution=1.2)\n"
            "    >>> array.axistags['x'].resolution = 2.0\n"
            "    >>> print array.axistags['x']\n"
            "    AxisInfo: 'x' (type: Space, resolution=2)\n\n",
            no_init)
        .def("__init__", make_constructor(&AxisTags_create,
            default_call_policies(),
            (arg("i1")=object(), arg("i2")=object(), arg("i3")=object(),
             arg("i4")=object(), arg("i5")=object())))
        .def("__repr__", &AxisTags::repr)
        .def("__str__", &AxisTags_str)
        .def("__copy__", &generic__copy__<AxisTags>)
        .def("__deepcopy__", &generic__deepcopy__<AxisTags>)
        .def("__len__", &AxisTags::size)
        .def("__getitem__", &AxisTags_getitem, return_internal_reference<>())
        .def("__getitem__",
            (AxisInfo & (AxisTags::*)(std::string const &))&AxisTags::get, return_internal_reference<>())
        .def("__setitem__", (void (AxisTags::*)(int, AxisInfo const &))&AxisTags::set)
        .def("__setitem__",
            (void (AxisTags::*)(std::string const &, AxisInfo const &))&AxisTags::set)
        .def("__delitem__", (void (AxisTags::*)(int))&AxisTags::dropAxis)
        .def("__delitem__", (void (AxisTags::*)(std::string const &))&AxisTags::dropAxis)
        .def("insert", &AxisTags::insert)
        .def("append", &AxisTags::push_back)
        .def("dropChannelAxis", &AxisTags::dropChannelAxis)
        .def("insertChannelAxis", &AxisTags_insertChannelAxis)
        .def("swapaxes", &AxisTags::swapaxes)

        // NOTE: in contrast to arrays, AxisTags::transpose() works
        //       in-place, i.e. changes 'self'
        .def("transpose", (void (AxisTags::*)())&AxisTags::transpose)
        .def("transpose", (void (AxisTags::*)(ArrayVector<npy_intp> const &))&AxisTags::transpose)
        .def("index", &AxisTags::index,
             "Get the axis index of a given axis key::\n\n"
             "    >>> axistags.index('x')\n"
             "    0\n\n"
             "In this example, the 'x'-axis corresponds to index 0 (i.e. the first index).\n")
        .add_property("channelIndex", &AxisTags::channelIndex,
            "(read-only property, type 'int') the index of the channel axis, or ``len(axistags)``\n"
            "when no channel axis exists (i.e. ``axistags.channelIndex`` is similar to\n"
            "``axistags.index('c')``, but doesn't throw an exception when there\n"
            "is no 'c' axis.)\n\n")
        .add_property("innerNonchannelIndex", &AxisTags::innerNonchannelIndex,
            "(read-only property, type 'int') the index of the innermost non-channel axis.\n")
        .def("axisTypeCount", &AxisTags::axisTypeCount,
            "How many axes of the given type(s) are in this axistags object?::\n\n"
            "    axistags.axisTypeCount(types) -> int\n\n"
            "The 'types' of the query must be single :class:`~vigra.AxisType` instances\n"
            "or a combination of them. Examples::\n\n"
            "    >>> a = vigra.defaultAxistags('txyc')\n"
            "    >>> a.axisTypeCount(vigra.AxisType.Space)\n"
            "    2\n"
            "    >>> a.axisTypeCount(vigra.AxisType.Time)\n"
            "    1\n"
            "    >>> a.axisTypeCount(vigra.AxisType(vigra.AxisType.Space | vigra.AxisType.Time))\n"
            "    3\n"
            "    >>> a.axisTypeCount(vigra.AxisType.NonChannel)\n"
            "    3\n\n")

        // IMPORTANT: do not remove are rename the following functions,
        //            they are used by the vigranumpy C++ API
        .def("resolution", (double (AxisTags::*)(int) const)&AxisTags::resolution)
        .def("resolution", (double (AxisTags::*)(std::string const &) const)&AxisTags::resolution)
        .def("setResolution", (void (AxisTags::*)(int, double))&AxisTags::setResolution)
        .def("setResolution",
            (void (AxisTags::*)(std::string const &, double))&AxisTags::setResolution)
        .def("scaleResolution", (void (AxisTags::*)(int, double))&AxisTags::scaleResolution)
        .def("scaleResolution",
            (void (AxisTags::*)(std::string const &, double))&AxisTags::scaleResolution)
        .def("description", (std::string (AxisTags::*)(int) const)&AxisTags::description)
        .def("description",
             (std::string (AxisTags::*)(std::string const &) const)&AxisTags::description)
        .def("setDescription",
            (void (AxisTags::*)(int, std::string const &))&AxisTags::setDescription)
        .def("setDescription",
            (void (AxisTags::*)(std::string const &, std::string const &))&AxisTags::setDescription)
        .def("setChannelDescription", &AxisTags::setChannelDescription,
             "Set a description for the channel axis, if one exists::\n\n"
             "    axistags.setChannelDescription('colors are in Lab color space')\n\n"
             "It is similar to::\n\n"
             "    axistags['c'].description = 'colors are in Lab color space'\n\n"
             "except when the axistags contain no channel axis, in which case\n"
             "setChannelDescription() is simply ignored, whereas axistags['c']\n"
             "would cause an exception.\n")
        .def("toFrequencyDomain", (void (AxisTags::*)(int, int, int))&AxisTags::toFrequencyDomain,
                (arg("index"), arg("size")=0, arg("sign")=1))
        .def("toFrequencyDomain",
               (void (AxisTags::*)(std::string const &, int, int))&AxisTags::toFrequencyDomain,
               (arg("key"), arg("size")=0, arg("sign")=1))
        .def("fromFrequencyDomain", (void (AxisTags::*)(int, int))&AxisTags::fromFrequencyDomain,
                (arg("index"), arg("size")=0))
        .def("fromFrequencyDomain",
               (void (AxisTags::*)(std::string const &, int))&AxisTags::fromFrequencyDomain,
               (arg("key"), arg("size")=0))
        .def("permutationToNormalOrder", &AxisTags_permutationToNormalOrder)
        .def("permutationToNormalOrder", &AxisTags_permutationToNormalOrder2)
        .def("permutationFromNormalOrder", &AxisTags_permutationFromNormalOrder)
        .def("permutationFromNormalOrder", &AxisTags_permutationFromNormalOrder2)
        .def("permutationToNumpyOrder", &AxisTags_permutationToNumpyOrder)
        .def("permutationFromNumpyOrder", &AxisTags_permutationFromNumpyOrder)
        .def("permutationToVigraOrder", &AxisTags_permutationToVigraOrder)
        .def("permutationFromVigraOrder", &AxisTags_permutationFromVigraOrder)
        .def("permutationToOrder", &AxisTags_permutationToOrder)
        .def("transform", &AxisTags_transform,
                             return_value_policy<manage_new_object>())
        .def("compatible", &AxisTags::compatible)
        .def(self == self)
        .def(self != self)
        .def("toJSON", &AxisTags::toJSON,
             "Create a string representation of this axistags in JSON format.")
#ifndef VIGRA_DISABLE_FROMJSON
        .def("fromJSON", &AxisTags_readJSON,
                             return_value_policy<manage_new_object>())
        .staticmethod("fromJSON")
#endif
        ;
}

} // namespace vigra

ext: cxx

max_stars_repo_name: RegiTheSter/vigra

max_issues_repo_name: RegiTheSter/vigra

max_forks_repo_name: RegiTheSter/vigra


text:
#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <vector>

#include "number_representation.hpp"

namespace abo::error_metrics {

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with ADDs and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
double wcre_add(const Cudd& mgr, const std::vector<BDD>& f,
              const std::vector<BDD>& f_hat,
              const abo::util::NumberRepresentation num_rep
                = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes bounds on the maximum relative value of f in relation to g
 * It is defined as the maximum of |f(x)| / max(1, |g(x)|) over all inputs x
 * This function returns a range in which the actual relative value is guaranteed to lie
 * The computed bounds are always within a factor of 2, meaning that the maximum bound
 * returned by this function is at most twice the minimum bound
 * @param mgr The BDD object manager
 * @param f The function to compute the maximum relative value of (must be an unsigned integer)
 * @param g The function to use as a relation. Must have the same number of bits as f (must be an
 * unsigned integer)
 * @return {min, max}, the lower and upper bound on the maximum relative value
 */
std::pair<boost::multiprecision::cpp_dec_float_100,
            boost::multiprecision::cpp_dec_float_100>
maximum_relative_value_bounds(const Cudd& mgr, const std::vector<BDD>& f,
                              const std::vector<BDD>& g);

/**
 * @brief Computes bounds on the maximum relative difference between f and f_hat
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * This function returns a range in which the actual maximum relative error is guaranteed to lie
 * The computed bounds are always within a factor of 2, meaning that the maximum error
 * returned by this function is at most twice the minimum error
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return {min, max}, the lower and upper bound on the maximum relative error
 */
std::pair<boost::multiprecision::cpp_dec_float_100,
            boost::multiprecision::cpp_dec_float_100>
    wcre_bounds(
        const Cudd& mgr,
        const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a binary search to find the maximum value
 * If the correct value is found, the search is aborted. Otherwise, it is run until the desired
 * precision is reached
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_extra_bits The number of additional bits used during the search to represent values
 * smaller than one
 * @param precision The desired precision of the result if the correct value is not found during the
 * binary search Do not set it lower than 2^-num_extra_bits
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
double wcre_search(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int num_extra_bits = 16,
        double precision = 0.0001,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a binary search to find the maximum value
 * If the correct value is found, the search is aborted. Otherwise, it is run until the desired
 * precision is reached
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param samples The number of random input samples drawn in each iteration
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs as a fraction [numerator, denominator]
 */
std::pair<long, long> wcre_randomized_search(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int samples = 1,
        const abo::util::NumberRepresentation num_rep
        = abo::util::NumberRepresentation::BaseTwo);
/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a symbolic division and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_extra_bits The number of additional fixed precision bits to use during the division
 * As the result of each division is not an integer, the result is described as a fixed point number
 * with exactly num_extra_bits bits with a lower significance than one. Roughly correlates the the
 * precision of the result
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
boost::multiprecision::cpp_dec_float_100 wcre_symbolic_division(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int num_extra_bits = 16,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

} // namespace abo::error_metrics

max_stars_repo_name: keszocze/abo

max_issues_repo_name: keszocze/abo

max_forks_repo_name: keszocze/abo