module-asynchronous_machine.cc

Go to the documentation of this file.

/* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-asynchronous_machine/module-asynchronous_machine.cc,v 1.11 2017/01/12 14:47:25 masarati Exp $ */
/* 
 * MBDyn (C) is a multibody analysis code. 
 * http://www.mbdyn.org
 *
 * Copyright (C) 1996-2017
 *
 * Pierangelo Masarati  <masarati@aero.polimi.it>
 * Paolo Mantegazza     <mantegazza@aero.polimi.it>
 *
 * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
 * via La Masa, 34 - 20156 Milano, Italy
 * http://www.aero.polimi.it
 *
 * Changing this copyright notice is forbidden.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation (version 2 of the License).
 * 
 *
 * This program 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, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
/*
 AUTHOR: Reinhard Resch <r.resch@secop.com>
        Copyright (C) 2011(-2017) all rights reserved.

        The copyright of this code is transferred
        to Pierangelo Masarati and Paolo Mantegazza
        for use in the software MBDyn as described
        in the GNU Public License version 2.1
*/

#include "mbconfig.h"           // This goes first in every *.c,*.cc file

#include <cassert>
#include <cstdio>
#include <cmath>
#include <cfloat>
#include <iostream>
#include <iomanip>
#include <limits>

#include "dataman.h"
#include "userelem.h"
#include "module-asynchronous_machine.h"

class asynchronous_machine
: virtual public Elem, public UserDefinedElem
{
public:
        asynchronous_machine(unsigned uLabel, const DofOwner *pDO,
                DataManager* pDM, MBDynParser& HP);
        virtual ~asynchronous_machine(void);
        virtual void Output(OutputHandler& OH) const;
        virtual unsigned int iGetNumDof(void) const;
        virtual DofOrder::Order GetDofType(unsigned int i) const;
        virtual std::ostream& DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const;
        virtual std::ostream& DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const;
        virtual unsigned int iGetNumPrivData(void) const;
        virtual unsigned int iGetPrivDataIdx(const char *s) const;
        virtual doublereal dGetPrivData(unsigned int i) const;
        virtual void SetInitialValue(VectorHandler& X);
        virtual void Update(const VectorHandler& XCurr,const VectorHandler& XPrimeCurr);
        virtual void AfterPredict(VectorHandler& X, VectorHandler& XP);
        virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
        VariableSubMatrixHandler&
        AssJac(VariableSubMatrixHandler& WorkMat,
                doublereal dCoef,
                const VectorHandler& XCurr,
                const VectorHandler& XPrimeCurr);
        SubVectorHandler&
        AssRes(SubVectorHandler& WorkVec,
                doublereal dCoef,
                const VectorHandler& XCurr,
                const VectorHandler& XPrimeCurr);
        virtual int iGetNumConnectedNodes(void) const;
        virtual void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
        virtual void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
                        SimulationEntity::Hints *ph);
        virtual std::ostream& Restart(std::ostream& out) const;
        virtual unsigned int iGetInitialNumDof(void) const;
        virtual void
        InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
        virtual VariableSubMatrixHandler&
        InitialAssJac(VariableSubMatrixHandler& WorkMat,
                      const VectorHandler& XCurr);
        virtual SubVectorHandler&
        InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr);

private:
        bool IsMotorOn(void) const;

private:
        const StructNode* m_pRotorNode; 
        const StructNode* m_pStatorNode;        
        doublereal m_MK;                                
        doublereal m_sK;                                
        DriveOwner m_OmegaS;            
        DriveOwner m_MotorOn;           
        doublereal m_M;                                 
        doublereal m_dM_dt;                             
        doublereal m_dM_dt2;                    
        doublereal m_omega;                             
        doublereal m_domega_dt;                 
    static const doublereal sm_SingTol;
};

const doublereal asynchronous_machine::sm_SingTol = std::pow(std::numeric_limits<doublereal>::epsilon(), 0.9);

asynchronous_machine::asynchronous_machine(
        unsigned uLabel, const DofOwner *pDO,
        DataManager* pDM, MBDynParser& HP)
:       Elem(uLabel, flag(0)),
        UserDefinedElem(uLabel, pDO),
        m_pRotorNode(0),
        m_pStatorNode(0),
        m_MK(0.),
        m_sK(0.),
        m_OmegaS(0),
        m_MotorOn(0),
        m_M(0.),
        m_dM_dt(0.),
        m_dM_dt2(0.),
        m_omega(0.),
        m_domega_dt(0.)
{
        if (HP.IsKeyWord("help")) {
                silent_cout(
                        "\n"
                        "Module:        asynchronous_machine\n"
                        "\n"
                        "       This module implements an asynchronous electric machine\n"
                        "   according to\n"
                        "\n"
                        "       Maschinendynamik\n"
                        "       Hans Dresig, Franz Holzweißig\n"
                        "       Springer, 2009\n"
                        "       page 58 equation 1.122 and 1.123\n"
                        "\n"
                        "Syntax:\n"
                        "       asynchronous_machine,\n"
                        "               rotor, (label) <rotor_node>,\n"
                        "               stator, (label) <stator_node>,\n"
                        "               MK, (real) <MK>,\n"
                        "               sK, (real) <sK>,\n"
                        "               OmegaS, (DriveCaller) <omega_s>\n"
                        "               [ , motor on , (DriveCaller) <motor_on> ]\n"
                        "               [ , M0 , (real) <M0> ]\n"
                        "               [ , MP0 , (real) <MP0> ]\n"
                        "\n"
                        "       MK ... breakdown torque ( MK > 0 )\n"
                        "       sK ... breakdown slip ( sK > 0 )\n"
                        "       OmegaS = 2 * pi * f / p * sense_of_rotation ... synchronous angular velocity\n"
                        "       motor_on ... power supply is turned off when 0, on otherwise\n"
                        "       M0 ... initial torque ( default 0 )\n"
                        "       MP0 ... initial torque derivative ( default 0 )\n"
                        "\n"
                        "       The axis of rotation is assumed to be axis 3 of the reference frame of the stator node.\n"
                        << std::endl);

                if (!HP.IsArg()) {
                        // Exit quietly if nothing else is provided
                        throw NoErr(MBDYN_EXCEPT_ARGS);
                }
        }

        if (!HP.IsKeyWord("rotor")) {
                silent_cerr("asynchronous_machine(" << GetLabel() << "): keyword \"rotor\" expected at line " << HP.GetLineData() << std::endl);
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        }

        m_pRotorNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);

        if (!m_pRotorNode) {
                silent_cerr("asynchronous_machine(" << GetLabel() << "): structural node expected at line " << HP.GetLineData() << std::endl);
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        }

        if (!HP.IsKeyWord("stator")) {
                silent_cerr("asynchronous_machine(" << GetLabel() << "): keyword \"stator\" expected at line " << HP.GetLineData() << std::endl);
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        }

        m_pStatorNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);

        if (!m_pStatorNode) {
                silent_cerr("asynchronous_machine(" << GetLabel() << "): structural node expected at line " << HP.GetLineData() << std::endl);
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        }

        if (!HP.IsKeyWord("MK")) {
                silent_cerr("asynchronous_machine(" << GetLabel() << "): keyword \"MK\" expected at line " << HP.GetLineData() << std::endl);
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        }
        m_MK = HP.GetReal();

        if (!HP.IsKeyWord("sK")) {
                silent_cerr("asynchronous_machine(" << GetLabel() << "): keyword \"sK\" expected at line " << HP.GetLineData() << std::endl);
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        }
        m_sK = HP.GetReal();

        if (!HP.IsKeyWord("OmegaS")) {
                silent_cerr("asynchronous_machine(" << GetLabel() << "): keyword \"OmegaS\" expected" << std::endl);
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        }
        m_OmegaS.Set(HP.GetDriveCaller());

        if (HP.IsKeyWord("motor" "on") || HP.IsKeyWord("motor_on")) {
                m_MotorOn.Set(HP.GetDriveCaller());
        } else {
                m_MotorOn.Set(new OneDriveCaller());
        }

        if (HP.IsKeyWord("M0")) {
                m_M = HP.GetReal();
        }

        if (HP.IsKeyWord("MP0")) {
                m_dM_dt = HP.GetReal();
        }

        SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));

        Vec3 e3(m_pStatorNode->GetRCurr().GetCol(3));
        const Vec3& omega1 = m_pRotorNode->GetWCurr();
        const Vec3& omega2 = m_pStatorNode->GetWCurr();

        m_omega = e3.Dot(omega1 - omega2);

        const doublereal OmegaS = m_OmegaS.dGet();

        pDM->GetLogFile() << "asynchronous machine: "
                << uLabel << " "
                << m_pRotorNode->GetLabel() << " "
                << m_pStatorNode->GetLabel() << " "
                << m_MK << " "
                << m_sK << " "
                << OmegaS << " "
                << IsMotorOn() << " "
                << m_M << " "
                << m_dM_dt << " "
                << m_omega
                << std::endl;

        // Note: The ODE for the asynchronous machine is defined for positive sign of OmegaS only!
        // At user level the signs of M, dM/dt and d^2M/dt^2 are defined to be positive
        // in positive coordinate system direction in the reference frame of the stator.
        m_M *= copysign(1., OmegaS);
        m_dM_dt *= copysign(1., OmegaS);
        m_dM_dt2 *= copysign(1., OmegaS);
}

void
asynchronous_machine::Update(const VectorHandler& X,const VectorHandler& XP)
{
        const integer iFirstIndex = iGetFirstIndex();

        const integer intTorqueDerivativeRowIndex        = iFirstIndex + 1;
        const integer intTorqueRowIndex                          = iFirstIndex + 2;
        const integer intOmegaRowIndex                           = iFirstIndex + 3;

        // save the current state needed by dGetPrivData() and Output()
        m_M             = X(intTorqueRowIndex);
        m_dM_dt         = X(intTorqueDerivativeRowIndex);
        m_dM_dt2        = XP(intTorqueDerivativeRowIndex);
        m_omega         = X(intOmegaRowIndex);
        m_domega_dt = XP(intOmegaRowIndex);
}

void asynchronous_machine::AfterPredict(VectorHandler& X, VectorHandler& XP)
{
        Update(X, XP);
}

bool
asynchronous_machine::IsMotorOn(void) const
{
        return (m_MotorOn.dGet() != 0.);
}

asynchronous_machine::~asynchronous_machine(void)
{
        // destroy private data
#ifdef DEBUG
        std::cerr << __FILE__ << ":" << __LINE__ << ":" << __PRETTY_FUNCTION__ << std::endl;
#endif
}

void
asynchronous_machine::Output(OutputHandler& OH) const
{
        if (bToBeOutput()) {
                // Note: The ODE for the asynchronous machine is defined for positive sign of OmegaS only!
                // At user level the signs of M, dM/dt and d^2M/dt^2 are defined to be positive
                // in positive coordinate system direction in the reference frame of the stator.
                doublereal OmegaS = m_OmegaS.dGet();
                doublereal M = m_M*copysign(1., OmegaS);
                doublereal dM_dt = m_dM_dt*copysign(1., OmegaS);
                doublereal dM_dt2 = m_dM_dt2*copysign(1., OmegaS);

                if (OH.UseText(OutputHandler::LOADABLE)) {
                        // output the current state: the column layout is as follows

                        // 1 2     3      4     5         6      7
                        // M dM_dt dM_dt2 omega domega_dt OmegaS gamma
                        //
                        // 0    label of the element
                        // 1    motor torque
                        // 2    derivative of the motor torque versus time
                        // 3    second derivative of the motor torque versus time
                        // 4    angular velocity of the rotor node relative to the stator node around axis 3
                        // 5    angular acceleration of the rotor node relative to the stator node around axis 3
                        // 6    synchronous angular velocity (might be a function of the time in case of an frequency inverter)
                        // 7    power supply is turned on or off

                        OH.Loadable() << GetLabel()
                                << " " << M
                                << " " << dM_dt
                                << " " << dM_dt2
                                << " " << m_omega
                                << " " << m_domega_dt
                                << " " << OmegaS
                                << " " << IsMotorOn()
                                << std::endl;
                }
        }
}

void
asynchronous_machine::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
{
        *piNumRows = 9;
        *piNumCols = 9;
}

unsigned int
asynchronous_machine::iGetNumDof(void) const
{
        return 3;
}

DofOrder::Order
asynchronous_machine::GetDofType(unsigned int i) const
{

        return DofOrder::DIFFERENTIAL;
}

std::ostream&
asynchronous_machine::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
{
        integer iIndex = iGetFirstIndex();

        out
                << prefix << iIndex + 1 << ": motor torque derivative [MP]" << std::endl
                << prefix << iIndex + 2 << ": motor torque [M]" << std::endl
                << prefix << iIndex + 3 << ": relative angular velocity [omega]" << std::endl;

        return out;
}

std::ostream&
asynchronous_machine::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
{
        integer iIndex = iGetFirstIndex();

        out
                << prefix << iIndex + 1 << ": motor DAE [f1]" << std::endl
                << prefix << iIndex + 2 << ": motor torque derivative [f2]" << std::endl
                << prefix << iIndex + 3 << ": angular velocity derivative [f9]" << std::endl;

        return out;
}

unsigned int
asynchronous_machine::iGetNumPrivData(void) const
{
        return 5;
}

unsigned int
asynchronous_machine::iGetPrivDataIdx(const char *s) const
{
        static const struct {
                int index;
                char name[7];
        }

        data[] = {
                        { 1, "M" },             // motor torque
                        { 2, "MP"},             // derivative of the motor torque versus time diff(M,t) (named MP instead of dM_dt for compatibility with other elements)
                        { 3, "MPP"},    // second derivative of the motor torque  versus time diff(M,t,2) (named MPP instead of dM_dt2 for compatibility with other elements)
                        { 4, "omega"},  // angular velocity of the rotor node relative to the stator node
                        { 5, "omegaP"}  // angular acceleration of the rotor node relative to the stator node (named omegaP instead of domega_dt for compatibility with other elements)
        };

        for (unsigned i = 0; i < sizeof(data) / sizeof(data[0]); ++i ) {
                if (0 == strcmp(data[i].name,s)) {
                        return data[i].index;
                }
        }

        silent_cerr("asynchronous_machine(" << GetLabel() << "): no private data \"" << s << "\"" << std::endl);

        return 0;
}

doublereal
asynchronous_machine::dGetPrivData(unsigned int i) const
{
        // Note: The ODE for the asynchronous machine is defined for positive sign of OmegaS only!
        // At user level the signs of M, dM/dt and d^2M/dt^2 are defined to be positive
        // in positive coordinate system direction in the reference frame of the stator.

        const doublereal OmegaS = m_OmegaS.dGet();

        switch (i) {
        case 1:
                return m_M*copysign(1., OmegaS);
        case 2:
                return m_dM_dt*copysign(1., OmegaS);
        case 3:
                return m_dM_dt2*copysign(1., OmegaS);
        case 4:
                return m_omega;
        case 5:
                return m_domega_dt;
        }

        throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
}

void
asynchronous_machine::SetInitialValue(VectorHandler& X)
{
        return;
}

VariableSubMatrixHandler&
asynchronous_machine::AssJac(VariableSubMatrixHandler& WorkMat_,
        doublereal dCoef,
        const VectorHandler& XCurr,
        const VectorHandler& XPrimeCurr)
{
#ifdef DEBUG
        std::cerr << __FILE__ << ':' << __LINE__ << ':' << __PRETTY_FUNCTION__ << std::endl;
#endif

        integer iNumRows = 0;
        integer iNumCols = 0;

        WorkSpaceDim(&iNumRows, &iNumCols);

        FullSubMatrixHandler& WorkMat = WorkMat_.SetFull();

        WorkMat.ResizeReset(iNumRows, iNumCols);

        const integer iFirstIndex = iGetFirstIndex();

        const integer intTorqueDerivativeColumnIndex = iFirstIndex + 1;
        const integer intTorqueColumnIndex                       = iFirstIndex + 2;
        const integer intOmegaColumnIndex                        = iFirstIndex + 3;

        const integer intTorqueDerivativeRowIndex        = iFirstIndex + 1;
        const integer intTorqueRowIndex                          = iFirstIndex + 2;
        const integer intOmegaRowIndex                           = iFirstIndex + 3;

        const integer intRotorPositionIndex  = m_pRotorNode->iGetFirstPositionIndex();
        const integer intStatorPositionIndex = m_pStatorNode->iGetFirstPositionIndex();

        const integer intRotorMomentumIndex  = m_pRotorNode->iGetFirstMomentumIndex();
        const integer intStatorMomentumIndex = m_pStatorNode->iGetFirstMomentumIndex();

        /*
         * M             ... motor torque
         * diff(M,t) ... derivative of the motor torque versus time
         * g1            ... rotation parameter of the rotor
         * g2            ... rotation parameter of the stator
         * R2            ... rotation matrix of the stator node
         * omega1    ... rotor angular velocity
         * omega2        ... stator angular velocity
         *
         * e3 = ( 0, 0, 1 )^T ... axis of rotation in the reference frame of the stator node
         *
         * omega = e3^T * R2^T * ( omega1 - omega2 )
         *
         * s = 1 - omega / OmegaS
         *
         *              | y1 |   | diff(M,t) |
         *              | y2 |   | M             |
         *      y =     | y3 | = | g1            |
         *              | y4 |   | g2            |
         *              | y5 |   | omega     |
         *
         *              | f1 |   |  diff(y1,t) + ( 2 * sK + sign(OmegaS) * diff(y5,t) / ( s * OmegaS^2 ) ) * y1 * abs(OmegaS) + ( ( sK^2 + s^2 ) * OmegaS^2 + sign(OmegaS) * diff(y5,t) * sK / s ) * y2 - 2 * MK * sK * s * OmegaS^2 |
         *              | f2 |   |  diff(y2,t) - y1                                                                                                                                                                                                                                                                                                                                                              |                                                                                                                                                                                                                                                                                                                 |
         *      f =     | f3 | = |  R2 * e3 * y2                                                                                                                                                                                                                                                                                                                                                                 |
         *              | f4 |   | -R2 * e3 * y2                                                                                                                                                                                                                                                                                                                                                         |
         *              | f5 |   |  y5 - e3^T * R2^T * ( omega1 - omega2 )                                                                                                                                                                                                                                                                                                               |
         *
         *                                            1,       2,       3,       6,       9
         *                        | df1/dy1, df1/dy2, df1/dy3, df1/dy4, df1/dy5 |  1
         *                                | df2/dy1, df2/dy2, df2/dy3, df2/dy4, df2/dy5 |  2
         *      diff(f,y) =   | df3/dy1, df3/dy2, df3/dy3, df3/dy4, df3/dy5 |  3
         *                        | df4/dy1, df4/dy2, df4/dy3, df4/dy4, df4/dy5 |  6
         *                        | df5/dy1, df5/dy2, df5/dy3, df5/dy4, df5/dy5 |  9
         *
         * WorkMat = -diff(f,diff(y,t)) - dCoef * diff(f,y)
         */
        WorkMat.PutRowIndex(1, intTorqueDerivativeRowIndex);
        WorkMat.PutColIndex(1, intTorqueDerivativeColumnIndex);

        WorkMat.PutRowIndex(2, intTorqueRowIndex);
        WorkMat.PutColIndex(2, intTorqueColumnIndex);

        for (int iCnt = 1; iCnt <= 3; ++iCnt) {
                WorkMat.PutRowIndex(iCnt + 2, intRotorMomentumIndex + iCnt + 3);
                WorkMat.PutColIndex(iCnt + 2, intRotorPositionIndex + iCnt + 3);

                WorkMat.PutRowIndex(iCnt + 5, intStatorMomentumIndex + iCnt + 3);
                WorkMat.PutColIndex(iCnt + 5, intStatorPositionIndex + iCnt + 3);
        }

        WorkMat.PutRowIndex(9, intOmegaRowIndex);
        WorkMat.PutColIndex(9, intOmegaColumnIndex);

        Vec3 e3 = m_pStatorNode->GetRCurr().GetCol(3);  // corrected axis of rotation of the stator node
        Vec3 e3_0 = m_pStatorNode->GetRRef().GetCol(3); // predicted axis of rotation of the stator node
        const Vec3& omega1 = m_pRotorNode->GetWCurr();  // corrected angular velocity of the rotor node
        const Vec3& omega2 = m_pStatorNode->GetWCurr(); // corrected angular velocity of the rotor node
        const Vec3& omega1_ref = m_pRotorNode->GetWRef(); // predicted angular velocity of the rotor node
        const Vec3& omega2_ref = m_pStatorNode->GetWRef(); // predicted angular velocity of the stator node

        const doublereal OmegaS = m_OmegaS.dGet(); // synchronous angular velocity

        const doublereal y1             = XCurr(intTorqueDerivativeRowIndex);
        const doublereal y2             = XCurr(intTorqueRowIndex);
        const doublereal y5             = XCurr(intOmegaRowIndex);
#if 0 // unused
        const doublereal y1_dot         = XPrimeCurr(intTorqueDerivativeRowIndex);
        const doublereal y2_dot         = XPrimeCurr(intTorqueRowIndex);
#endif
        const doublereal y5_dot         = XPrimeCurr(intOmegaRowIndex);

        doublereal s = 1 - y5 / OmegaS; // slip of the rotor

    if ( std::abs(s) < sm_SingTol )
    {
        silent_cerr("\nasynchronous_machine(" << GetLabel() << "): warning slip rate s = " << s << " is close to machine precision!\n");
        //FIXME: avoid division by zero
        //FIXME: results might be inaccurate
        s = copysign(sm_SingTol, s);
    }

        doublereal df1_dy1, df1_dy2, df1_dy5;
        doublereal df1_dy1_dot, df1_dy2_dot, df1_dy5_dot;
        doublereal df2_dy1, df2_dy2_dot;

        if (IsMotorOn()) {
                // power supply is turned on

                // df1_dy1 = diff(f1,y1)
                df1_dy1 = ( 2 * m_sK + copysign(1., OmegaS) * y5_dot / ( s * std::pow(OmegaS,2) ) ) * abs(OmegaS);
                // df1_dy2 = diff(f1,y2)
                df1_dy2 = ( std::pow(m_sK,2) + std::pow(s,2) ) * std::pow(OmegaS,2) + copysign(1., OmegaS) * y5_dot * m_sK / s;
                // df1_dy1_dot = diff(f1,diff(y1,t))
                df1_dy1_dot = 1.;
                // df2_dy1 = diff(f2,dy1)
                df2_dy1 = -1.;
                // df2_dy2_dot = diff(f2,diff(y2,t))
                df2_dy2_dot = 1.;
                // df1_ds = diff(f1,s)
                const doublereal df1_ds = -y1 * y5_dot / ( std::pow(s,2) * OmegaS ) + ( 2 * s * std::pow(OmegaS,2) - copysign(1., OmegaS) * y5_dot * m_sK / std::pow(s,2) ) * y2 - 2 * m_MK * m_sK * std::pow(OmegaS,2);
                // df1_dy5 = diff(f1,y5) = diff(f1,s) * diff(s,y5)
                df1_dy5 = df1_ds * ( -1. / OmegaS );
                // df1_dy5_dot = diff(f1,diff(y5,t))
                df1_dy5_dot = y1 / ( s * OmegaS ) + copysign(1., OmegaS) * y2 * m_sK / s;

        } else {
                // power supply is turned off
                df1_dy1 = 0.;
                df1_dy2 = 1.;
                df1_dy5 = 0.;
                df1_dy1_dot = 0.;
                df1_dy2_dot = 0.;
                df1_dy5_dot = 0.;

                df2_dy1 = 1.;
                df2_dy2_dot = 0;
        }

        // df3_dy2 = diff(f3,y2)
        const Vec3 df3_dy2 = e3 * copysign(1., OmegaS); // df3_dy2 = R2 * e3 * sign(OmegaS)
        // df4_dy2 = diff(f4,y2)
        const Vec3 df4_dy2 = -df3_dy2; // df4_dy2 = -R2 * e3
        // df3_dy4 = diff(f3,y4)
        const Mat3x3 df3_dy4 = -Mat3x3(MatCross, e3_0 * (y2 * copysign(1., OmegaS)) ); // df3_dy4 = -skew( R2^(0) * e3 * y2 * sign(OmegaS) )
        // df4_dy4 = diff(f4,y4)
        const Mat3x3 df4_dy4 = -df3_dy4;
        // df5_dy5 = diff(f5,y5)
        const doublereal df5_dy5 = 1.;
        // df5_dy3_dot_T = transpose(diff(f5,diff(y3,t)))
        const Vec3 df5_dy3_dot_T = -e3; // diff(y5,diff(y3,t)) = -e3^T * R2^T
        // df5_dy4_dot_T = transpose(diff(f5,diff(y4,t)))
        const Vec3 df5_dy4_dot_T = -df5_dy3_dot_T;  // diff(y5,diff(y4,t)) = e3^T * R2^T
        // df5_dy3_T = transpose(diff(y5,y3))
        const Vec3 df5_dy3_T = -omega1_ref.Cross(e3);
        // df5_dy4_T = transpose(diff(y5,y4))
        const Vec3 df5_dy4_T = ( omega1 - omega2 ).Cross( e3_0 ) + omega2_ref.Cross( e3 );

        WorkMat.PutCoef( 1, 1,  -df1_dy1_dot   - dCoef * df1_dy1 );
        WorkMat.PutCoef( 1, 2,                             - dCoef * df1_dy2 );
        WorkMat.PutCoef( 1, 9,  -df1_dy5_dot   - dCoef * df1_dy5 );
        WorkMat.PutCoef( 2, 1,                             - dCoef * df2_dy1 );
        WorkMat.PutCoef( 2, 2,  -df2_dy2_dot                                                );
        WorkMat.Put(     3, 2,                                   df3_dy2   * ( -dCoef ) );
        WorkMat.Put(     3, 6,                                   df3_dy4   * ( -dCoef ) );
        WorkMat.Put(     6, 2,                   df4_dy2   * ( -dCoef ) );
        WorkMat.Put(     6, 6,                   df4_dy4   * ( -dCoef ) );
        WorkMat.PutT(    9, 3,  -df5_dy3_dot_T + df5_dy3_T * ( -dCoef ) );
        WorkMat.PutT(    9, 6,  -df5_dy4_dot_T + df5_dy4_T * ( -dCoef ) );
        WorkMat.PutCoef( 9, 9,                               df5_dy5   * ( -dCoef ) );

#ifdef DEBUG
        std::cerr << __FILE__ ":" << __LINE__ << ":" << __PRETTY_FUNCTION__ << std::endl;
        std::cerr << "s = " << s << std::endl;
        std::cerr << "WorkMat=" << std::endl
                 << WorkMat << std::endl << std::endl;
#endif

        return WorkMat_;
}

SubVectorHandler&
asynchronous_machine::AssRes(SubVectorHandler& WorkVec,
        doublereal dCoef,
        const VectorHandler& XCurr,
        const VectorHandler& XPrimeCurr)
{
#ifdef DEBUG
        std::cerr << __FILE__ << ':' << __LINE__ << ':' << __PRETTY_FUNCTION__ << std::endl;
#endif
        integer iNumRows = 0;
        integer iNumCols = 0;

        WorkSpaceDim(&iNumRows, &iNumCols);

        WorkVec.ResizeReset(iNumRows);

        const integer iFirstIndex = iGetFirstIndex();

        const integer intTorqueDerivativeRowIndex        = iFirstIndex + 1;
        const integer intTorqueRowIndex                          = iFirstIndex + 2;
        const integer intOmegaRowIndex                           = iFirstIndex + 3;

        const integer intRotorMomentumIndex  = m_pRotorNode->iGetFirstMomentumIndex();
        const integer intStatorMomentumIndex = m_pStatorNode->iGetFirstMomentumIndex();

        WorkVec.PutRowIndex(1, intTorqueDerivativeRowIndex);
        WorkVec.PutRowIndex(2, intTorqueRowIndex);

        for (int iCnt = 1; iCnt <= 3; ++iCnt) {
                WorkVec.PutRowIndex(iCnt + 2, intRotorMomentumIndex + iCnt + 3);
                WorkVec.PutRowIndex(iCnt + 5, intStatorMomentumIndex + iCnt + 3);
        }

        WorkVec.PutRowIndex(9, intOmegaRowIndex);

        Vec3 e3 = m_pStatorNode->GetRCurr().GetCol(3);
        const Vec3& omega1 = m_pRotorNode->GetWCurr();
        const Vec3& omega2 = m_pStatorNode->GetWCurr();

        const doublereal OmegaS = m_OmegaS.dGet();

        const doublereal y1             = XCurr(intTorqueDerivativeRowIndex);                           // y1 = diff(M,t)
        const doublereal y2             = XCurr(intTorqueRowIndex);                                                     // y2 = M
        const doublereal y5             = XCurr(intOmegaRowIndex);                                                      // y5 = omega
        const doublereal y1_dot         = XPrimeCurr(intTorqueDerivativeRowIndex);                      // diff(y1,t) = diff(M,t,2)
        const doublereal y2_dot         = XPrimeCurr(intTorqueRowIndex);                                        // diff(y2,t) = diff(M,t)
        const doublereal y5_dot         = XPrimeCurr(intOmegaRowIndex);                                         // diff(y5,t) = diff(omega,t)

        doublereal s = 1 - y5 / OmegaS;

    if ( std::abs(s) < sm_SingTol )
    {
        silent_cerr("\nasynchronous_machine(" << GetLabel() << "): warning slip rate s = " << s << " is close to machine precision!\n");
        //FIXME: avoid division by zero
        //FIXME: results might be inaccurate
        s = copysign(sm_SingTol, s);
    }

        doublereal f1, f2;

        if (IsMotorOn()) {
                // power supply is switched on
                f1 = y1_dot + ( 2 * m_sK + copysign(1., OmegaS) * y5_dot / ( s * std::pow(OmegaS,2) ) ) * y1 * abs(OmegaS)
                        + ( ( std::pow(m_sK,2) + std::pow(s,2) ) * std::pow(OmegaS,2) + copysign(1., OmegaS) * y5_dot * m_sK / s ) * y2 - 2 * m_MK * m_sK * s * std::pow(OmegaS,2);
                f2 = y2_dot - y1;

        } else {
                // power supply is switched off: torque must be zero
                f1 = y2;
                f2 = y1;
        }

        const Vec3 f3 = e3 * (y2 * copysign(1., OmegaS));
        // const Vec3 f4 = -f3;

        const doublereal f5 = y5 - e3.Dot( omega1 - omega2 );

        WorkVec.PutCoef( 1, f1 );
        WorkVec.PutCoef( 2, f2 );
        WorkVec.Put(     3,     f3 );
        WorkVec.Sub(     6, f3 );
        WorkVec.PutCoef( 9, f5 );

#ifdef DEBUG
        std::cerr
                << __FILE__ ":" << __LINE__ << ":" << __PRETTY_FUNCTION__ << std::endl
                << "s = " << s << std::endl
                << "y1 = " << y1 << std::endl
                << "y1_dot = " << y1_dot << std::endl
                << "y2 = " << y2 << std::endl
                << "y2_dot = " << y2_dot << std::endl
                << "f1 = " << f1 << std::endl
                << "f2 = " << f2 << std::endl
                << "f3 = " << f3 << std::endl
                << "f4 = " << -f3 << std::endl
                << "f5 = " << f5 << std::endl
                << "WorkVec=" << std::endl
                << WorkVec << std::endl << std::endl;
#endif

        return WorkVec;
}
int
asynchronous_machine::iGetNumConnectedNodes(void) const
{
        return 2;
}

void
asynchronous_machine::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
{
        connectedNodes.resize(iGetNumConnectedNodes());
        connectedNodes[0] = m_pRotorNode;
        connectedNodes[1] = m_pStatorNode;
}

void
asynchronous_machine::SetValue(DataManager *pDM,
        VectorHandler& X, VectorHandler& XP,
        SimulationEntity::Hints *ph)
{
        const integer intFirstIndex = iGetFirstIndex();

        //                             1        2        3
        X.Put( intFirstIndex + 1, Vec3(m_dM_dt, m_M,     m_omega) );
        XP.Put(intFirstIndex + 1, Vec3(     0., m_dM_dt,      0.) );
}

std::ostream&
asynchronous_machine::Restart(std::ostream& out) const
{
        // FIXME: mbdyn crashes when "make restart file;" is specified in the input file before this member function is invoked!
        out << "asynchronous_machine, "
                "rotor, " << m_pRotorNode->GetLabel() << ", "
                "stator, " << m_pStatorNode->GetLabel() << ", "
                "MK, " << m_MK << ", "
                "sK, " << m_sK << ", "
                "OmegaS, ";
                m_OmegaS.pGetDriveCaller()->Restart(out) << ", "
                "motor on, ";
                m_MotorOn.pGetDriveCaller()->Restart(out) << ", "
                "M0, " << m_M * copysign(1., m_OmegaS.dGet()) << ", "
                "MP0, " << m_dM_dt * copysign(1., m_OmegaS.dGet())  << ";" << std::endl;
        
        return out;
}

unsigned int
asynchronous_machine::iGetInitialNumDof(void) const
{
        return 0;
}

void
asynchronous_machine::InitialWorkSpaceDim(
        integer* piNumRows,
        integer* piNumCols) const
{
        *piNumRows = 0;
        *piNumCols = 0;
}

VariableSubMatrixHandler&
asynchronous_machine::InitialAssJac(
        VariableSubMatrixHandler& WorkMat,
        const VectorHandler& XCurr)
{
        WorkMat.SetNullMatrix();

        return WorkMat;
}

SubVectorHandler&
asynchronous_machine::InitialAssRes(
        SubVectorHandler& WorkVec,
        const VectorHandler& XCurr)
{
        WorkVec.ResizeReset(0);

        return WorkVec;
}

bool
asynchronous_machine_set(void)
{
#ifdef DEBUG
        std::cerr << __FILE__ <<":"<< __LINE__ << ":"<< __PRETTY_FUNCTION__ << std::endl;
#endif

        UserDefinedElemRead *rf = new UDERead<asynchronous_machine>;

        if (!SetUDE("asynchronous_machine", rf)) {
                delete rf;
                return false;
        }

        return true;
}

#ifndef STATIC_MODULES

extern "C" 
{

int
module_init(const char *module_name, void *pdm, void *php)
{
        if (!asynchronous_machine_set()) {
                silent_cerr("asynchronous_machine: "
                        "module_init(" << module_name << ") "
                        "failed" << std::endl);
                return -1;
        }

        return 0;
}

} // extern "C"

#endif // ! STATIC_MODULES