configuration.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Author: Florent Lamiraux
//
//
// This file is part of hpp-pinocchio
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_PINOCCHIO_CONFIGURATION_HH
# define HPP_PINOCCHIO_CONFIGURATION_HH

# include <iomanip>
# include <hpp/pinocchio/fwd.hh>

namespace hpp {
  namespace pinocchio {
    void saturate (const DevicePtr_t& robot,
                   ConfigurationOut_t configuration);

    bool saturate (const DevicePtr_t& robot,
                   ConfigurationOut_t configuration,
                   ArrayXb& saturation);

    template<bool saturateConfig, typename LieGroup>
    void integrate (const DevicePtr_t& robot,
                    ConfigurationIn_t configuration,
                    vectorIn_t velocity, ConfigurationOut_t result);

    void integrate (const DevicePtr_t& robot,
                    ConfigurationIn_t configuration,
                    vectorIn_t velocity, ConfigurationOut_t result);

    template <typename LieGroup>
    void interpolate  (const DevicePtr_t& robot,
                       ConfigurationIn_t q0,
                       ConfigurationIn_t q1,
                       const value_type& u,
                       ConfigurationOut_t result);

    void interpolate (const DevicePtr_t& robot,
                      ConfigurationIn_t q0,
                      ConfigurationIn_t q1,
                      const value_type& u,
                      ConfigurationOut_t result);

    template <typename LieGroup>
    void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
                     ConfigurationIn_t q2, vectorOut_t result);

    void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
                     ConfigurationIn_t q2, vectorOut_t result);

    bool isApprox (const DevicePtr_t& robot, ConfigurationIn_t q1,
                   ConfigurationIn_t q2, value_type eps);

    value_type distance (const DevicePtr_t& robot, ConfigurationIn_t q1,
                         ConfigurationIn_t q2);

    void normalize (const DevicePtr_t& robot, Configuration_t& q);

    inline void normalize (const DevicePtr_t& robot, ConfigurationOut_t q)
    {
      Configuration_t qq = q;
      normalize(robot, qq);
      q = qq;
    }

    bool isNormalized (const DevicePtr_t& robot, ConfigurationIn_t q,
                       const value_type& eps);

    inline std::string displayConfig (ConfigurationIn_t q, int precision = 20)
    {
      std::ostringstream oss; oss << "(" <<  std::setprecision (precision);
      for (size_type i=0; i < q.size (); ++i) {
    oss << q [i] << ",";
      }
      oss << ")";
      return oss.str ();
    }

    std::ostream& display (std::ostream& os, const SE3& m);
  } // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_CONFIGURATION_HH