aron_conversions.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * 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 Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "aron_conversions.h"
 
 
namespace armarx::control::common::ft
{
    void
    fromAron(const arondto::FTConfig &dto, FTSensor::FTConfig &bo)
    {
        bo = FTSensor::FTConfig{
                .sensorName = dto.sensorName,
                .ftFilter = dto.ftFilter,
                .deadZoneForce = dto.deadZoneForce,
                .deadZoneTorque = dto.deadZoneTorque,
                .timeLimit = dto.timeLimit,
                .enableTCPGravityCompensation = dto.enableTCPGravityCompensation,
                .reCalibrate = dto.reCalibrate,
                .forceBaseline = dto.forceBaseline,
                .torqueBaseline = dto.torqueBaseline
        };
    }
 
    FTSensor::FTConfig
    fromAron(const arondto::FTConfig &dto)
    {
        FTSensor::FTConfig ftConfig = FTSensor::FTConfig{
                .sensorName = dto.sensorName,
                .ftFilter = dto.ftFilter,
                .deadZoneForce = dto.deadZoneForce,
                .deadZoneTorque = dto.deadZoneTorque,
                .timeLimit = dto.timeLimit,
                .enableTCPGravityCompensation = dto.enableTCPGravityCompensation,
                .reCalibrate = dto.reCalibrate,
                .forceBaseline = dto.forceBaseline,
                .torqueBaseline = dto.torqueBaseline
        };
        return ftConfig;
    }
} /// namespace armarx::control::common::ft
 
 
namespace armarx::control::common::control_law
{
    void
    fromAron(const arondto::TaskspaceImpedanceControllerConfig& dto,
             TaskspaceImpedanceController::Config& bo)
    {
        bo = TaskspaceImpedanceController::Config{
                .kpImpedance = dto.kpImpedance,
                .kdImpedance = dto.kdImpedance,
                .kpNullspace = dto.kpNullspace,
                .kdNullspace = dto.kdNullspace,
                .desiredPose = dto.desiredPose,
                .desiredTwist = dto.desiredTwist,
                .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles,
                .torqueLimit = dto.torqueLimit,
                .qvelFilter = dto.qvelFilter
        };
    }
 
    void
    fromAron(const arondto::TaskspaceAdmittanceControllerConfig &dto,
             TaskspaceAdmittanceController::Config &bo)
    {
        auto ftBO = armarx::control::common::ft::fromAron(dto.ftConfig);
        bo = TaskspaceAdmittanceController::Config{
                .kpImpedance = dto.kpImpedance,
                .kdImpedance = dto.kdImpedance,
                .kpAdmittance = dto.kpAdmittance,
                .kdAdmittance = dto.kdAdmittance,
                .kmAdmittance = dto.kmAdmittance,
                .kpNullspace = dto.kpNullspace,
                .kdNullspace = dto.kdNullspace,
                .desiredPose = dto.desiredPose,
                .desiredTwist = dto.desiredTwist,
                .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles,
                .torqueLimit = dto.torqueLimit,
                .qvelFilter = dto.qvelFilter,
                .ftConfig = ftBO
        };
    }
 
    void
    fromAron(const arondto::KeypointControllerConfig& dto,
             KeypointsImpedanceController::Config& bo)
    {
        bo = KeypointsImpedanceController::Config{
                .kpImpedance = dto.kpImpedance,
                .kdImpedance = dto.kdImpedance,
//                .kpAdmittance = dto.kpAdmittance,
                .kdAdmittance = dto.kdAdmittance,
                .kmAdmittance = dto.kmAdmittance,
                .kpNullspace = dto.kpNullspace,
                .kdNullspace = dto.kdNullspace,
//                .currentForceTorque = dto.currentForceTorque,
//                .desiredPose = dto.desiredPose,
//                .desiredTwist = dto.desiredTwist,
                .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles,
                .torqueLimit = dto.torqueLimit,
                .qvelFilter = dto.qvelFilter,
                .numPoints = dto.numPoints,
                .keypointKp = dto.keypointKp,
                .keypointKd = dto.keypointKd,
                .fixedTranslation = dto.fixedTranslation,
                .densityForceScaling = dto.densityForceScaling,
                .keypointPositionFilter = dto.keypointPositionFilter,
                .keypointVelocityFilter = dto.keypointVelocityFilter,
                .isRigid = dto.isRigid
        };
    }
 
} /// namespace armarx::control::common::control_law
 
 
namespace armarx::control::common::mp
{
 
    MP::ListViaPoint
    fromAron(const arondto::ListViaPoint& dto)
    {
        return {dto.canonicalValue, dto.viaPointValue};
    }
 
    MP::DictViaPoint
    fromAron(const arondto::DictViaPoint& dto)
    {
        return {dto.canonicalValue, dto.viaPointValue};
    }
 
    void
    fromAron(const arondto::ListViaPoint &dto, MP::ListViaPoint &bo)
    {
        bo = MP::ListViaPoint{
            .canonicalValue = dto.canonicalValue,
            .viaPointValue = dto.viaPointValue
        };
    }
 
    void
    fromAron(const arondto::DictViaPoint &dto, MP::DictViaPoint &bo)
    {
        bo = MP::DictViaPoint{
                .canonicalValue = dto.canonicalValue,
                .viaPointValue = dto.viaPointValue
        };
    }
 
    void
    fromAron(const arondto::MPConfig &dto, MP::MPConfig &bo)
    {
        std::vector<MP::ListViaPoint> viaPoints;
        std::transform(dto.viaPoints.begin(), dto.viaPoints.end(), std::back_inserter(viaPoints),
                       [](const auto& vp) -> MP::ListViaPoint
                       {
                           return fromAron(vp);
                       });
        bo = MP::MPConfig{
                .name = dto.name,
                .className = dto.className,
                .mpTypeString = dto.mpTypeString,
                .mpMode = dto.mpMode,
                .mpStyle = dto.mpStyle,
                .regressionModel = dto.regressionModel,
                .kernelSize = dto.kernelSize,
                .damping = dto.damping,
                .tau = dto.tau,
                .amplitude = dto.amplitude,
                .durationSec = dto.durationSec,
                .fileList = dto.fileList,
                .stopWithMP = dto.stopWithMP,
                .viaPoints = viaPoints,
                .enablePhaseStop = dto.enablePhaseStop, /// phase stop
                .maxValue = dto.maxValue,
                .slop = dto.slop,
                .goDist = dto.goDist,
                .backDist = dto.backDist,
                .psKpPos = dto.psKpPos,
                .psKdPos = dto.psKdPos,
                .psKpOri = dto.psKpOri,
                .psKdOri = dto.psKdOri,
                .psMM2Radian = dto.psMM2Radian
        };
    }
 
    MP::MPConfig
    fromAron(const arondto::MPConfig &dto)
    {
        std::vector<MP::ListViaPoint> viaPoints;
        std::transform(dto.viaPoints.begin(), dto.viaPoints.end(), std::back_inserter(viaPoints),
                       [](const auto& vp) -> MP::ListViaPoint
                       {
                           return fromAron(vp);
                       });
        MP::MPConfig mpConfig = MP::MPConfig{
                .name = dto.name,
                .className = dto.className,
                .mpTypeString = dto.mpTypeString,
                .mpMode = dto.mpMode,
                .mpStyle = dto.mpStyle,
                .regressionModel = dto.regressionModel,
                .kernelSize = dto.kernelSize,
                .damping = dto.damping,
                .tau = dto.tau,
                .amplitude = dto.amplitude,
                .durationSec = dto.durationSec,
                .fileList = dto.fileList,
                .stopWithMP = dto.stopWithMP,
                .viaPoints = viaPoints,
                .enablePhaseStop = dto.enablePhaseStop, /// phase stop
                .maxValue = dto.maxValue,
                .slop = dto.slop,
                .goDist = dto.goDist,
                .backDist = dto.backDist,
                .psKpPos = dto.psKpPos,
                .psKdPos = dto.psKdPos,
                .psKpOri = dto.psKpOri,
                .psKdOri = dto.psKdOri,
                .psMM2Radian = dto.psMM2Radian
        };
        return mpConfig;
    }
 
    void
    fromAron(const arondto::MPListConfig &dto, MP::MPListConfig &bo)
    {
        std::transform(dto.mpList.begin(), dto.mpList.end(), std::back_inserter(bo.mpList),
                       [](const auto& mp) -> MP::MPConfig
                       {
                           return fromAron(mp);
                       });
    }
} /// namespace armarx::control::common::mp