kbm.cpp

Go to the documentation of this file.

/* *********************
 * @file kbm.cpp
 *
 * @author Stefan Ulbrich
 * @date 2013-2014
 *
 * @brief This file contains the implementation the Kinematic Bezier Maps minus the methods related to subdivision.
 * *********************/
#include "kbm.h"
 
#include <assert.h>
 
#include <algorithm> // std::sort
#include <cmath>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <vector>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
namespace memoryx::KBM::Models
{
    std::ostream&
    operator<<(std::ostream& os, const KBM::ErrorValuesType& et)
    {
        os << "Mean: " << et.Mean << ", STD:" << et.STD << ", Median: " << et.Median
           << ", IQR: " << et.IQR << ", Max: " << et.Max << std::endl;
        return os;
    }
 
    KBM::KBM(const KBM& other)
    {
        this->nControlPoints = other.nControlPoints;
        this->nDoF = other.nDoF;
        this->dim = other.dim;
        this->controlNet = other.controlNet;
        this->spreadAngles = other.spreadAngles;
        this->index = other.index;
        this->center = other.center;
        this->partialDerivatives = other.partialDerivatives;
    }
 
    KBM::KBM(int nDoF, int dim, Real spreadAngle)
    {
        this->nDoF = nDoF;
        this->dim = dim;
        //this->spreadAngle = spreadAngle;
        //this->spreadAngles = std::vector<Real>(nDoF,spreadAngle);
        this->spreadAngles = Vector::Constant(nDoF, spreadAngle);
        this->nControlPoints = pow(3.0, this->nDoF);
        //    ARMARX_DEBUG_S << "Creating KBM with" << this->nDoF << " degrees of freedom and " << this->dim << " output dimensions." << std::endl;
        this->controlNet = Matrix::Zero(this->dim, this->nControlPoints);
        this->createIndices();
        this->center = Vector::Zero(this->nDoF);
        this->partialDerivatives =
            std::vector<Matrix>(this->nDoF, Matrix::Zero(this->dim, this->nControlPoints));
    }
 
    KBM::ErrorValuesType
    KBM::getErrors(const Matrix& proprioception, const Matrix& shape)
    {
        int nSamples = shape.cols();
        //    ARMARX_DEBUG_S << this->spreadAngle << std::endl;
        ErrorValuesType error_values;
 
        Matrix errors = this->controlNet * this->createSLE(proprioception) - shape;
        //ARMARX_DEBUG_S << errors;
        std::vector<Real> summed_squared_errors(nSamples, 0.0f);
 
        error_values.Mean = 0; //mean squared error
 
        for (int iSamples = 0; iSamples < nSamples; iSamples++)
        {
            Real sum = 0.0;
 
            for (int iDim = 0; iDim < this->dim; iDim++)
            {
                sum += pow(errors(iDim, iSamples), 2);
                //ARMARX_DEBUG_S << pow(errors(iDim,iSamples),2) << std::endl;
            }
 
            summed_squared_errors[iSamples] = sqrt(sum);
            error_values.Mean += sqrt(sum);
        }
 
        error_values.Mean /= nSamples;
        std::sort(summed_squared_errors.begin(), summed_squared_errors.end());
        //for (int i =0; i<summed_squared_errors.size(); i++) ARMARX_DEBUG_S << summed_squared_errors[i] << std::endl;
        error_values.Median = summed_squared_errors[nSamples / 2];
        error_values.IQR =
            summed_squared_errors[(nSamples * 3) / 4] - summed_squared_errors[nSamples / 4];
        error_values.STD = 0;
        error_values.Max =
            *std::max_element(summed_squared_errors.begin(), summed_squared_errors.end());
        //ARMARX_DEBUG_S << error_values << std::endl;
        return error_values;
    }
 
    void
    KBM::createIndices()
    {
 
        this->index.resize(nControlPoints, nDoF);
 
        for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
        {
            int decimal = iControlPoints; //-1;
 
            //ARMARX_DEBUG_S << decimal << std::endl;
            for (int iDoF = 0; iDoF < nDoF; iDoF++)
            {
                //ARMARX_DEBUG_S << iDoF << std::endl;
                this->index(iControlPoints, iDoF) = decimal % 3;
                //ARMARX_DEBUG_S << this->index(iControlPoints,iDoF) << " ";
                decimal /= 3;
            }
 
            //ARMARX_DEBUG_S<<std::endl;
        }
 
        //ARMARX_DEBUG_S <<  this->index << std::endl;
    }
 
    Matrix
    KBM::createSLE(const Matrix& proprioception, int dim, Real a1, Real a2, bool project) const
    {
        assert(proprioception.rows() == nDoF);
        int nSamples = proprioception.cols();
        Matrix SLE(nControlPoints, nSamples);
 
 
        Real bincoeff[] = {1, 2, 1};
 
        for (int iSamples = 0; iSamples < nSamples; iSamples++)
        {
            Real weight = 0;
 
            for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
            {
                SLE(iControlPoints, iSamples) = 1.0;
 
                // ARMARX_DEBUG_S << SLE;
                for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
                {
                    int idx = this->index(iControlPoints, iDoF);
 
                    if (dim != iDoF)
                    {
                        Real t = std::tan((proprioception(iDoF, iSamples) - center(iDoF)) / 2.0) /
                                     tan(this->spreadAngles[iDoF] / 2.0) / 2.0 +
                                 0.5;
                        Real gamma;
 
                        if (idx == 1)
                        {
                            gamma = std::cos(this->spreadAngles[iDoF]);
                        }
                        else
                        {
                            gamma = 1;
                        }
 
                        //ARMARX_DEBUG_S << idx << " "<< t << " " << this->spreadAngle << " " << gamma << std::endl;
                        SLE(iControlPoints, iSamples) *=
                            gamma * bincoeff[idx] * pow(t, idx) *
                            pow(1.0 - t, 2 - idx); // Bernstein polynomial
                    }
                    else //Blossom
                    {
                        Real t1 =
                            std::tan(a1 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
                        Real t2 =
                            std::tan(a2 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
 
                        switch (idx)
                        {
                            case 0:
                                SLE(iControlPoints, iSamples) *= (1 - t1) * (1 - t2);
                                break;
 
                            case 1:
                                SLE(iControlPoints, iSamples) *= ((1 - t1) * t2 + (1 - t2) * t1) *
                                                                 std::cos(this->spreadAngles[iDoF]);
                                break;
 
                            case 2:
                                SLE(iControlPoints, iSamples) *= t1 * t2;
                                break;
                        }
                    }
                }
 
                //ARMARX_DEBUG_S << SLE;
                weight += SLE(iControlPoints, iSamples);
            }
 
            //ARMARX_DEBUG_S << "Weight: " << weight << std::endl;
            if (project)
                for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
                {
                    SLE(iControlPoints, iSamples) /= weight;
                }
        }
 
        //ARMARX_DEBUG_S << SLE << std::endl;
        return SLE;
    }
 
    void
    KBM::batch(const Matrix& proprioception,
               const Matrix& shape,
               Optimization method,
               Real threshold)
    {
        assert(proprioception.cols() == shape.cols());
        assert(proprioception.rows() == this->nDoF);
        assert(shape.rows() == this->dim);
        Matrix SLE = this->createSLE(proprioception);
 
        switch (method)
        {
            case KBM::STANDARD:
                this->controlNet =
                    SLE.transpose().householderQr().solve(shape.transpose()).transpose();
                break;
 
            case KBM::PLS:
                this->controlNet = PLS::solve(SLE, shape, threshold);
                break;
        }
 
        //    ARMARX_DEBUG_S << "Control Net:\n" << this->controlNet << std::endl;
    }
 
    void
    KBM::online(const Matrix& proprioception, const Matrix& shape, Real learnRate)
    {
        assert(proprioception.cols() == shape.cols());
        assert(proprioception.rows() == this->nDoF);
        //                ARMARX_IMPORTANT_S << VAROUT(shape.rows()) << VAROUT(this->dim);
        ARMARX_CHECK_EXPRESSION(shape.rows() == this->dim)
            << armarx::ValueToString(shape.rows()) + " " + armarx::ValueToString(this->dim);
 
        //ARMARX_DEBUG_S << "proprioception: " << proprioception<<std::endl;
        //ARMARX_DEBUG_S << "shape: " << shape<<std::endl;
        int nSamples = proprioception.cols();
        Matrix SLE = createSLE(proprioception);
 
        //ARMARX_DEBUG_S << this->dim << std::endl;
        for (int iSamples = 0; iSamples < nSamples; iSamples++)
        {
            for (int iDim = 0; iDim < this->dim; iDim++)
            {
                Vector temp = SLE.col(iSamples);
                Real delta = shape(iDim, iSamples) - this->controlNet.row(iDim).dot(temp);
                Matrix update(this->nControlPoints, 1);
                update =
                    delta * SLE.col(iSamples) * (1.0 / SLE.col(iSamples).dot(SLE.col(iSamples)));
                //ARMARX_DEBUG_S << iSamples << std::endl << iDim << std::endl << delta << std::endl << update << std::endl;
                this->controlNet.row(iDim) += learnRate * update.transpose();
            }
        }
    }
 
    bool
    KBM::restore(std::string fileName)
    {
        std::ifstream file(fileName.c_str());
 
        if (!file.is_open())
        {
            //        ARMARX_DEBUG_S << "Could not open file: " << fileName << std::endl;
            return false;
        }
 
        std::vector<double> values;
 
        //    ARMARX_DEBUG_S << "Reading " << fileName << std::endl;
        int lastCols = -1; //Compare the number of columns in each row.
        int rows = 0;
        std::string line;
 
        while (getline(file, line))
        {
            int cols = 0;
            std::stringstream linestream(line);
            std::string value;
 
            while (getline(linestream, value, ','))
            {
                values.push_back(strtod(value.c_str(), NULL));
                cols++;
            }
 
            rows++;
 
 
            // check if number of columns changes.
            if (lastCols >= 0)
            {
                assert(lastCols == cols);
            }
 
            lastCols = cols;
        }
 
        //ARMARX_DEBUG_S << values.size() << std::endl;
        assert(this->dim == rows && "Incompatible output dimensions");
        assert(this->nControlPoints == lastCols && "Incompatible input dimensions");
 
        for (int i = 0; i < this->dim; i++)
            for (int j = 0; j < this->nControlPoints; j++)
            {
                this->controlNet(i, j) =
                    values[i * this->nControlPoints +
                           j]; // Not nice, but we checked the dimensions so this is safe.
            }
 
        //ARMARX_DEBUG_S << this->controlNet << std::endl;
        ARMARX_DEBUG_S << "done (rows: " << rows << ", Cols: " << lastCols
                       << ", nDoF: " << this->nDoF << ", dim: " << this->dim << ")" << std::endl;
        return true;
    }
 
    void
    KBM::reset()
    {
        this->controlNet = Matrix::Zero(this->dim, this->nControlPoints);
    }
 
    //ColumnVector crossproduct(const ColumnVector &a, const ColumnVector &b){
    //    ColumnVector result(3);
    //    result(1) = a(2)*b(3)-a(3)*b(2);
    //    result(2) = a(3)*b(1)-a(1)*b(3);
    //    result(3) = a(1)*b(2)-a(2)*b(1);
    //    return result;
    //}
 
    Matrix
    KBM::predict(const Matrix& proprioception, int dim) const
    {
        if (dim == 0)
        {
            return controlNet * createSLE(proprioception);
        }
        else
        {
            //e = bez.controlNet * (blossom(bez,proprioception,dim,-bez.spreadAngle,proprioception(dim))-blossom(bez,proprioception,dim,bez.spreadAngle,proprioception(dim))#
            //e2 = bez.controlNet * (blossom(bez,proprioception,dim,-bez.spreadAngle,proprioception(dim)+diff)-blossom(bez,proprioception,dim,bez.spreadAngle,proprioception(#
            //e = cross(e1,e2);
            //e = cross(e3,e1);
            //frame(:3,:) = [e1/norm(e1) e2/norm(e2) e3/norm(e3) o];))))
            Vector o = controlNet * createSLE(proprioception);
            Vector3 e1 =
                this->controlNet *
                (createSLE(
                     proprioception, dim, (-1) * this->spreadAngles[dim], proprioception(dim, 0)) -
                 createSLE(proprioception, dim, this->spreadAngles[dim], proprioception(dim, 0)));
            Real offset =
                0.1; // pi/6 // Magic number: Small offset that generates another vector to define the plane.
            Vector3 e2 = this->controlNet * (createSLE(proprioception,
                                                       dim,
                                                       (-1) * this->spreadAngles[dim],
                                                       proprioception(dim, 0) + offset) -
                                             createSLE(proprioception,
                                                       dim,
                                                       this->spreadAngles[dim],
                                                       proprioception(dim, 0) + offset));
            //ARMARX_DEBUG_S << e1 << e2 << "e1/2";
            Vector3 e3;
            e3 = e1.cross(e2);
            e2 = e3.cross(e1);
            Matrix result = Matrix::Zero(4, 4);
            //ARMARX_DEBUG_S << e1 << ",\n" << e2 << ",\n" <<e3 << ",\n" <<o;
            result.block(0, 0, 3, 1) = e1 / e1.norm();
            result.block(0, 1, 3, 1) = e2 / e2.norm();
            result.block(0, 2, 3, 1) = e3 / e3.norm();
            result.block(0, 3, 3, 1) = o;
            result(3, 3) = 1.0f;
            return result;
        }
    }
 
    void
    KBM::differentiate()
    {
        for (int i = 0; i < this->nDoF; i++)
        {
        }
    }
 
    Vector
    KBM::getPartialDerivative(const Vector& proprioception, int iDoF) const
    {
 
        Vector SLEa = this->createSLE(
            proprioception, iDoF, this->spreadAngles[iDoF], proprioception[iDoF], false);
        Vector SLEb = this->createSLE(
            proprioception, iDoF, (-1) * this->spreadAngles[iDoF], proprioception[iDoF], false);
        Vector SLEc = this->createSLE(proprioception, -1, 0.0f, 0.0f, false);
 
        Vector SLE = ((SLEa - SLEb) * SLEc.sum() - (SLEa.sum() - SLEb.sum()) * SLEc) /
                     (SLEc.sum() * SLEc.sum());
        return this->controlNet * SLE / cos(proprioception[iDoF] / 2.0f) /
               cos(proprioception[iDoF] / 2.0f) / 2 / tan(this->spreadAngles[iDoF] / 2.0f);
    }
 
    Matrix
    KBM::getJacobian(const Vector& proprioception) const
    {
        Matrix Jacobian(this->dim, this->nDoF);
 
        for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
        {
            Jacobian.col(iDoF) = this->getPartialDerivative(proprioception, iDoF);
        }
 
        return Jacobian;
    }
 
    Vector
    KBM::getSpreadAngles() const
    {
        return this->spreadAngles;
    }
 
    Vector
    KBM::getCenter() const
    {
        return this->center;
    }
 
    int
    KBM::getNDoF() const
    {
        return this->nDoF;
    }
 
    int
    KBM::getNDim() const
    {
        return this->dim;
    }
 
    Matrix
    KBM::getControlNet() const
    {
        return controlNet;
    }
 
    KBM_ptr
    KBM::createKBM(int nDoF,
                   int dim,
                   const Vector& center,
                   const Vector& spreadAngles,
                   const Matrix& controlNet)
    {
        KBM_ptr kbm(new KBM(nDoF, dim, 0.0f));
        kbm->controlNet = controlNet;
        kbm->spreadAngles = spreadAngles;
        kbm->center = center;
        return kbm;
    }
 
    void
    KBM::store(std::string fileName)
    {
        //    ARMARX_DEBUG_S << "Storing to " << fileName << " ... " << std::flush;
        std::ofstream file(fileName.c_str(), std::ofstream::trunc);
        file << std::scientific << std::fixed
             << std::setprecision(std::numeric_limits<double>::digits);
 
        for (int iDim = 0; iDim < this->dim; iDim++)
        {
            for (int iControlPoints = 0; iControlPoints < this->nControlPoints - 1;
                 iControlPoints++)
            {
                file << this->controlNet(iDim, iControlPoints) << ",";
                //            ARMARX_DEBUG_S << this->controlNet(iDim,iControlPoints) << ",";
            }
 
            file << this->controlNet(iDim, nControlPoints - 1) << std::endl;
            //        ARMARX_DEBUG_S << this->controlNet(iDim,nControlPoints-1) << std::endl;
        }
 
        file.close();
        //    ARMARX_DEBUG_S << "Done." << std::endl;
    }
 
    KBM_ptr
    KBM::createFromVirtualRobot(VirtualRobot::KinematicChainPtr chain,
                                VirtualRobot::SceneObjectPtr FoR,
                                const Vector& spreadAngles,
                                bool useOrientation)
    {
        Matrix controlNet;
 
        if (!useOrientation)
        {
            controlNet = FoR->toLocalCoordinateSystem(chain->getTCP()->getGlobalPose())
                             .block(0, 3, 4, 1)
                             .cast<Real>();
        }
        else
        {
            controlNet.resize(12, 1);
            Eigen::Transform<float, 3, Eigen::Affine> axis_x(
                Eigen::Translation<float, 3>(1.0, 0.0, 0.0));
            Eigen::Transform<float, 3, Eigen::Affine> axis_y(
                Eigen::Translation<float, 3>(0.0, 1.0, 0.0));
            controlNet.block(0, 0, 4, 1) =
                FoR->toLocalCoordinateSystem(chain->getTCP()->getGlobalPose())
                    .block(0, 3, 4, 1)
                    .cast<Real>();
            controlNet.block(4, 0, 4, 1) =
                FoR->toLocalCoordinateSystem(chain->getTCP()->getGlobalPose() * axis_x.matrix())
                    .block(0, 3, 4, 1)
                    .cast<Real>();
            controlNet.block(8, 0, 4, 1) =
                FoR->toLocalCoordinateSystem(chain->getTCP()->getGlobalPose() * axis_y.matrix())
                    .block(0, 3, 4, 1)
                    .cast<Real>();
        }
 
        for (int i = chain->getSize() - 1; i >= 0; --i)
        {
            Matrix temp = controlNet;
            controlNet.resize(controlNet.rows(), controlNet.cols() * 3);
 
            VirtualRobot::RobotNodeRevolute* node =
                dynamic_cast<VirtualRobot::RobotNodeRevolute*>(chain->getNode(i).get());
            Vector3 axis = node->getJointRotationAxisInJointCoordSystem().cast<Real>();
            Matrix4 toLocal, toGlobal;
            toLocal = node->getGlobalPose().inverse().cast<Real>();
            toGlobal = node->getGlobalPose().cast<Real>();
            Real factor = 1.0f / cos(spreadAngles(i));
            Eigen::Transform<Real, 3, Eigen::Affine> rotation(
                Eigen::AngleAxis<Real>(spreadAngles(i), axis));
            Eigen::Transform<Real, 3, Eigen::Affine> scaling;
            Vector3 diag = axis + factor * (Vector3::Constant(1.0f) - axis);
            scaling = Eigen::Scaling(diag);
 
            for (int i = 0; i < temp.rows() / 4; i++)
            {
                controlNet.block(i * 4, 0, 4, temp.cols()) =
                    toGlobal * rotation.inverse() * toLocal * temp.block(i * 4, 0, 4, temp.cols());
                controlNet.block(i * 4, temp.cols(), 4, temp.cols()) =
                    toGlobal * scaling * toLocal * temp.block(i * 4, 0, 4, temp.cols());
                controlNet.block(i * 4, 2 * temp.cols(), 4, temp.cols()) =
                    toGlobal * rotation * toLocal * temp.block(i * 4, 0, 4, temp.cols());
            }
        }
 
        Vector center = Vector::Constant(chain->getSize(), 0.0f);
 
        if (!useOrientation)
        {
            return KBM::createKBM(chain->getSize(),
                                  3,
                                  center,
                                  spreadAngles,
                                  controlNet.block(0, 0, 3, controlNet.cols()));
        }
        else
        {
            Matrix reducedControlNet(9, controlNet.cols());
            reducedControlNet.block(0, 0, 3, controlNet.cols()) =
                controlNet.block(0, 0, 3, controlNet.cols());
            reducedControlNet.block(3, 0, 3, controlNet.cols()) =
                controlNet.block(4, 0, 3, controlNet.cols());
            reducedControlNet.block(6, 0, 3, controlNet.cols()) =
                controlNet.block(8, 0, 3, controlNet.cols());
            return KBM::createKBM(chain->getSize(), 9, center, spreadAngles, reducedControlNet);
        }
    }
 
 
} // namespace memoryx::KBM::Models