ScaledCSpace.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotComponents
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
#pragma once
 
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
 
#include <RobotComponents/interface/components/MotionPlanning/CSpace/ScaledCSpace.h>
 
#include "CSpace.h"
 
namespace armarx
{
    class ScaledCSpace;
    /**
     * @brief An ice handle to a ScaledCSpace.
     */
    using ScaledCSpacePtr = IceInternal::Handle<ScaledCSpace>;
 
    /**
     * @brief Takes an other cspace and scales its' dimensions.
     * Can be used when lage translations and rotations are planned at the same time.
     */
    class ScaledCSpace : virtual public ScaledCSpaceBase, virtual public CSpaceAdaptor
    {
    public:
        using CSpace::isCollisionFree;
 
        /**
         * @brief ctor.
         * @param cspace The cspace to scale.
         * @param scale The dimensions' scaling factors.
         */
        ScaledCSpace(const CSpaceBasePtr& cspace, const Ice::FloatSeq& scale);
 
        /**
         * @return The dimensions' scaling factors.
         */
        Ice::FloatSeq
        getScalingFactors(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return scalingFactors;
        }
 
        /**
         * @param cfg The configuration.
         * @return Returns the unscaled version of the configuration.
         */
        virtual void unscaleConfig(VectorXf& config) const;
 
        virtual void unscalePath(Path& path) const;
        virtual void unscalePath(PathWithCost& path) const;
        virtual void unscalePath(VectorXfSeq& nodes) const;
 
        virtual void scaleConfig(VectorXf& config) const;
        virtual void scalePath(Path& path) const;
 
        //CSpaceBase
        /**
         * @param cfg The configuration to check.
         * @return Checks whether the given configuration is collision free.
         */
        bool isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& cfg,
                             const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Initializes the collision check.
         */
        void
        initCollisionTest(const Ice::Current& = Ice::emptyCurrent) override
        {
            ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(originalCSpace->getDimensionality()) ==
                                    scalingFactors.size());
            unscaled.resize(scalingFactors.size());
            originalCSpace->initCollisionTest();
        }
 
        /**
         * @return A clone of this object.
         */
        CSpaceBasePtr clone(const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @return The cspace's dimensions.
         */
        FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @return The cspace's dimensionality.
         */
        Ice::Long
        getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return scalingFactors.size();
        }
 
    protected:
        template <class IceBaseClass, class DerivedClass>
        friend class armarx::GenericFactory;
 
        /**
         * @brief Default ctor. Used for ice factories.
         */
        ScaledCSpace() = default;
 
        /**
         * @brief Unscales the given configuration to a buffer.
         * @param cfg The scaled configuration.
         * @param buffer The buffer to fill.
         */
        void unscaleToBuffer(const Ice::Float* cfg, VectorXf& buffer) const;
 
    private:
        /**
         * @brief Buffer for speed up.
         */
        VectorXf unscaled;
    };
} // namespace armarx