JointImpedance.h

Go to the documentation of this file.

/* ============================================================
 *
 * This file is a part of RCI project
 *
 * Copyright (C) 2011 by Arne Nordmann <anordman at cor-lab dot uni-bielefeld dot de>
 *
 * This file may be licensed under the terms of the
 * GNU Lesser General Public License Version 3 (the ``LGPL''),
 * or (at your option) any later version.
 *
 * Software distributed under the License is distributed
 * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
 * express or implied. See the LGPL for the specific language
 * governing rights and limitations.
 *
 * You should have received a copy of the LGPL along with this
 * program. If not, go to http://www.gnu.org/licenses/lgpl.html
 * or write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The development of this software was supported by:
 *   CoR-Lab, Research Institute for Cognition and Robotics
 *     Bielefeld University
 *
 * ============================================================ */

#pragma once

#include "rci/dto/JointValues.h"

namespace rci {

class JointImpedance;
typedef boost::shared_ptr<JointImpedance> JointImpedancePtr;
class JointStiffness;
typedef boost::shared_ptr<JointStiffness> JointStiffnessPtr;
class JointDamping;
typedef boost::shared_ptr<JointDamping> JointDampingPtr;

class JointStiffness: public JointValues {

public:
    JointStiffness();

    JointStiffness(const nemo::RealVector& values);

    JointStiffness(double stiffness);

    JointStiffness(unsigned int dim, double stiffness);

    JointStiffness operator[](unsigned int index) const;

protected:
    virtual std::string print() const;
};

class JointDamping: public JointValues {

public:
    JointDamping();

    JointDamping(const nemo::RealVector& values);

    JointDamping(double damping);

    JointDamping(unsigned int dim, double damping);

    JointDamping operator[](unsigned int index) const;

protected:
    virtual std::string print() const;
};

class JointImpedance: public JointValues {

public:
    JointImpedance();

    JointImpedance(double stiffness, double damping);

    JointImpedance(const nemo::RealVector& values);

    JointImpedance(const nemo::RealVector& stiffness,
            const nemo::RealVector& damping);

    JointImpedance(JointStiffness stiffness, JointDamping damping);

    static JointImpedancePtr copy(const JointImpedance& impedance);

    static JointImpedancePtr create(const nemo::RealVector& values);

    JointStiffness getStiffness() const;

    unsigned int getDimension() const;

    JointStiffness getStiffness(unsigned int index) const;

    JointDamping getDamping() const;

    JointDamping getDamping(unsigned int index) const;

    JointImpedance operator[](unsigned int index) const;

    virtual double asDouble(unsigned int index) const;

    virtual nemo::RealVector asDoubleVector() const;

    virtual void setValue(unsigned int index, double value);

    template<class DTOType>
    void merge(std::vector<boost::shared_ptr<DTOType> >& inputDTOs) {
        // Check dimensions of input DTOs and my own dimension
        unsigned int inputdimension = 0;
        for (unsigned int inputdto = 0; inputdto < inputDTOs.size();
                ++inputdto) {
            inputdimension += inputDTOs[inputdto]->getDimension();
        }
        if (inputdimension != this->getDimension()) {
            throw std::runtime_error(
                    "Merging failed: My own dimension and the dimension of the summarized input JointImpedance DTOs doesn`t agree.");
        }

        // Start merging
        unsigned int cursor = 0;
        nemo::RealVector dampings(nemo::dim(this->getDimension())), stiffnesses(
                nemo::dim(this->getDimension()));
        for (unsigned int inputdto = 0; inputdto < inputDTOs.size();
                ++inputdto) {
            for (unsigned int inputdim = 0;
                    inputdim < inputDTOs[inputdto]->getDimension();
                    ++inputdim) {
                this->_stiffness.setValue(cursor,
                        inputDTOs[inputdto]->asDouble(2 * inputdim));
                this->_damping.setValue(cursor++,
                        inputDTOs[inputdto]->asDouble(2 * inputdim + 1));
            }
        }
    }

    template<class DTOType>
    std::vector<JointImpedancePtr> split() const {
        std::vector<JointImpedancePtr> vec;

        for (unsigned int cursor = 0; cursor < getDimension(); ++cursor) {
            nemo::RealVector values(nemo::dim(2), this->asDouble(2 * cursor));
            values[1] = this->asDouble(2 * cursor + 1);
            vec.push_back(JointImpedancePtr(new JointImpedance(values)));
        }

        return vec;
    }

protected:
    virtual std::string print() const;

    JointStiffness _stiffness;

    JointDamping _damping;
};

}