com-between-feet.hh

Go to the documentation of this file.

//
// Copyright (c) 2015 CNRS
// Authors: Joseph Mirabel
//
//
// This file is part of hpp-constraints.
// hpp-constraints 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-constraints 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-constraints. If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_CONSTRAINTS_COM_BETWEEN_FEET_HH
# define HPP_CONSTRAINTS_COM_BETWEEN_FEET_HH

# include <boost/assign/list_of.hpp>
# include <hpp/constraints/differentiable-function.hh>
# include <hpp/constraints/config.hh>
# include <hpp/constraints/fwd.hh>
# include <hpp/constraints/tools.hh>
# include <hpp/constraints/symbolic-calculus.hh>

namespace hpp {
  namespace constraints {

    class HPP_CONSTRAINTS_DLLAPI ComBetweenFeet : public DifferentiableFunction
    {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        static ComBetweenFeetPtr_t create (
            const std::string& name, const DevicePtr_t& robot,
            const JointPtr_t& jointLeft, const JointPtr_t& jointRight,
            const vector3_t   pointLeft, const vector3_t   pointRight,
            const JointPtr_t& jointReference, const vector3_t pointRef,
            std::vector <bool> mask = boost::assign::list_of (true)(true)(true)(true));

        static ComBetweenFeetPtr_t create (
            const std::string& name, const DevicePtr_t& robot,
            const CenterOfMassComputationPtr_t& comc,
            const JointPtr_t& jointLeft, const JointPtr_t& jointRight,
            const vector3_t   pointLeft, const vector3_t   pointRight,
            const JointPtr_t& jointReference, const vector3_t pointRef,
            std::vector <bool> mask = boost::assign::list_of (true)(true)(true)(true));

        virtual ~ComBetweenFeet () {}

        ComBetweenFeet (const std::string& name, const DevicePtr_t& robot,
            const CenterOfMassComputationPtr_t& comc,
            const JointPtr_t& jointLeft, const JointPtr_t& jointRight,
            const vector3_t   pointLeft, const vector3_t   pointRight,
            const JointPtr_t& jointReference, const vector3_t pointRef,
            std::vector <bool> mask);

      protected:
        virtual void impl_compute (LiegroupElementRef result,
                                   ConfigurationIn_t argument) const;

        virtual void impl_jacobian (matrixOut_t jacobian,
            ConfigurationIn_t arg) const;
      private:
        DevicePtr_t robot_;
        mutable Traits<PointCom>::Ptr_t com_;
        Traits<PointInJoint>::Ptr_t left_, right_;
        eigen::vector3_t pointRef_;
        JointPtr_t jointRef_;
        typedef Difference < PointCom, PointInJoint > DiffPCPiJ;
        typedef Difference < PointInJoint, PointInJoint > DiffPiJPiJ;
        typedef Sum < PointInJoint, PointInJoint > SumPiJPiJ;
        typedef CrossProduct < Difference < PointCom,
                                            ScalarMultiply < SumPiJPiJ > >,
                               DiffPiJPiJ > ECrossU_t;
        mutable Traits<DiffPCPiJ>::Ptr_t xmxl_, xmxr_;
        mutable Traits<DiffPiJPiJ>::Ptr_t u_;
        mutable Traits<ECrossU_t>::Ptr_t ecrossu_;
        mutable Traits<RotationMultiply <ECrossU_t> >::Ptr_t expr_;
        mutable Traits<ScalarProduct<DiffPCPiJ,DiffPiJPiJ> >::Ptr_t xmxlDotu_, xmxrDotu_;
        std::vector <bool> mask_;
        mutable eigen::matrix3_t cross_;
    }; // class ComBetweenFeet
  } // namespace constraints
} // namespace hpp
#endif // HPP_CONSTRAINTS_COM_BETWEEN_FEET_HH