Matrix.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-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/>.
 *
 * @author     Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
// Header
#include "Matrix.h"
 
#include <SimoxUtility/meta/type_name.h>
 
#include <RobotAPI/libraries/aron/codegeneration_util/TypeName.h>
 
namespace armarx::aron::codegenerator::cpp::generator
{
    const std::map<type::matrix::ElementType, std::tuple<std::string, int, std::string>>
        ElementType2Cpp = {
            {type::matrix::UINT8,
             {TypeName<unsigned char>::Get(),
              sizeof(unsigned char),
              "::armarx::aron::type::matrix::UINT8"}},
            {type::matrix::UINT16,
             {TypeName<unsigned short>::Get(),
              sizeof(unsigned short),
              "::armarx::aron::type::matrix::UINT16"}},
            {type::matrix::INT8,
             {TypeName<char>::Get(), sizeof(char), "::armarx::aron::type::matrix::INT8"}},
            {type::matrix::INT16,
             {TypeName<short>::Get(), sizeof(short), "::armarx::aron::type::matrix::INT16"}},
            {type::matrix::INT32,
             {TypeName<int>::Get(), sizeof(int), "::armarx::aron::type::matrix::INT32"}},
            {type::matrix::INT64,
             {TypeName<long>::Get(), sizeof(long), "::armarx::aron::type::matrix::INT64"}},
            {type::matrix::FLOAT32,
             {TypeName<float>::Get(), sizeof(float), "::armarx::aron::type::matrix::FLOAT32"}},
            {type::matrix::FLOAT64,
             {TypeName<double>::Get(), sizeof(double), "::armarx::aron::type::matrix::FLOAT64"}}};
 
    // constructors
    Matrix::Matrix(const type::Matrix& n) :
        detail::NDArrayGenerator<type::Matrix, Matrix>(
            "Eigen::Matrix<" + std::get<0>(ElementType2Cpp.at(n.getElementType())) + ", " +
                (n.getRows() == -1 ? "Eigen::Dynamic" : std::to_string(n.getRows())) + ", " +
                (n.getCols() == -1 ? "Eigen::Dynamic" : std::to_string(n.getCols())) +
                (n.getCols() != 1 ? ", Eigen::RowMajor>" : ">"),
            "Eigen::Matrix<" + std::get<0>(ElementType2Cpp.at(n.getElementType())) + ", " +
                (n.getRows() == -1 ? "Eigen::Dynamic" : std::to_string(n.getRows())) + ", " +
                (n.getCols() == -1 ? "Eigen::Dynamic" : std::to_string(n.getCols())) +
                (n.getCols() != 1 ? ", Eigen::RowMajor>" : ">"),
            simox::meta::get_type_name<data::dto::NDArray>(),
            simox::meta::get_type_name<type::dto::Matrix>(),
            n)
    {
    }
 
    std::vector<std::string>
    Matrix::getRequiredIncludes() const
    {
        return {"<RobotAPI/libraries/aron/common/rw/eigen.h>"};
    }
 
    std::pair<std::vector<std::pair<std::string, std::string>>, bool>
    Matrix::getCtorInitializers(const std::string& name) const
    {
        if (type.getCols() == -1 || type.getRows() == -1)
        {
            return {{}, false};
        }
 
        if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT ||
            type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
        {
            return {{{name, getInstantiatedCppTypename() + "::Identity()"}}, true};
        }
        else if (type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
        {
            return {{{name, getInstantiatedCppTypename() + "::Zero()"}}, true};
        }
        else if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
        {
            return {{{name, getInstantiatedCppTypename() + "::One()"}}, true};
        }
        else if (type.getMaybe() == type::Maybe::OPTIONAL)
        {
            return {{{name, "std::nullopt"}}, true};
        }
        else
        {
            // try to parse num. We ensure from typereader that defaultValue is valid number
            return {{{name, getInstantiatedCppTypename() + "::One() * " + type.getDefaultValue()}},
                    true};
        }
    }
 
    CppBlockPtr
    Matrix::getResetHardBlock(const std::string& cppAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
 
        if (type.getCols() == -1 || type.getRows() == -1)
        {
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "();");
        }
 
        else if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
        {
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
                                   "::Identity();");
        }
        else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT ||
                 type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
        {
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
                                   "::Zero();");
        }
        else if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
        {
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::One();");
        }
        else if (not type.getDefaultValue().empty())
        {
            // try to parse num. We ensure from typereader that defaultValue is valid number
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
                                   "::One() * " + type.getDefaultValue() + ";");
        }
        return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
    }
 
    CppBlockPtr
    Matrix::getResetSoftBlock(const std::string& cppAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        block_if_data->addLine(cppAccessor + nextEl() + "setZero();");
        return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
    }
 
    CppBlockPtr
    Matrix::getWriteTypeBlock(const std::string& typeAccessor,
                              const std::string& cppAccessor,
                              const Path& p,
                              std::string& variantAccessor) const
    {
        CppBlockPtr b = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
        b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR +
                   ".writeMatrix(static_cast<int>( " + std::to_string(type.getRows()) + "), " +
                   "static_cast<int>( " + std::to_string(type.getCols()) + "), " +
                   std::get<2>(ElementType2Cpp.at(type.getElementType())) + ", " + "\"" +
                   type.getDefaultValue() + "\", " +
                   conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
                   ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
                   "})); // of " + cppAccessor);
 
        return b;
    }
 
    CppBlockPtr
    Matrix::getWriteBlock(const std::string& cppAccessor,
                          const Path& p,
                          std::string& variantAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
        variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
 
        block_if_data->addLine("armarx::aron::write(" + ARON_WRITER_ACCESSOR + ", " +
                               resolved_accessor + ", " + variantAccessor + ", " +
                               "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
                               simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
 
        return resolveMaybeWriteBlock(block_if_data, cppAccessor);
    }
 
    CppBlockPtr
    Matrix::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
 
        if (const auto reset = resolveMaybeGeneratorWithSetter(cppAccessor); !reset.empty())
        {
            block_if_data->addLine(reset);
        }
 
        block_if_data->addLine("armarx::aron::read(" + ARON_READER_ACCESSOR + ", " +
                               variantAccessor + ", " + resolved_accessor + "); // of " +
                               cppAccessor);
 
        return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
    }
 
    CppBlockPtr
    Matrix::getEqualsBlock(const std::string& accessor,
                           const std::string& otherInstanceAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" +
                               resolveMaybeAccessor(otherInstanceAccessor) + ")))");
        block_if_data->addLineAsBlock("return false;");
        return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
    }
} // namespace armarx::aron::codegenerator::cpp::generator