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"
 
// #include <map>
 
#include <RobotAPI/libraries/aron/common/aron_conversions/eigen.h>
#include <RobotAPI/libraries/aron/core/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,
    //                                .enableSafeGuardForce = dto.enableSafeGuardForce,
    //                                .enableSafeGuardTorque = dto.enableSafeGuardTorque,
    //                                .safeGuardForceThreshold = dto.safeGuardForceThreshold,
    //                                .safeGuardTorqueThreshold = dto.safeGuardTorqueThreshold,
    //                                .forceBaseline = dto.forceBaseline,
    //                                .torqueBaseline = dto.torqueBaseline};
    //    }
 
    //    void
    //    toAron(arondto::FTConfig& dto, const FTSensor::FTConfig& bo)
    //    {
    //        dto.sensorName = bo.sensorName;
    //        dto.ftFilter = bo.ftFilter;
    //        dto.deadZoneForce = bo.deadZoneForce;
    //        dto.deadZoneTorque = bo.deadZoneTorque;
    //        dto.timeLimit = bo.timeLimit;
    //        dto.enableTCPGravityCompensation = bo.enableTCPGravityCompensation;
    //        dto.reCalibrate = bo.reCalibrate;
    //        dto.enableSafeGuardForce = bo.enableSafeGuardForce;
    //        dto.enableSafeGuardTorque = bo.enableSafeGuardTorque;
    //        dto.safeGuardForceThreshold = bo.safeGuardForceThreshold;
    //        dto.safeGuardTorqueThreshold = bo.safeGuardTorqueThreshold;
    //        dto.forceBaseline = bo.forceBaseline;
    //        dto.torqueBaseline = bo.torqueBaseline;
    //    }
} // namespace armarx::control::common::ft
 
namespace armarx::control::common::control_law
{
    //    // Impedance controller
    //    void
    //    fromAron(const arondto::TaskspaceImpedanceControllerConfig& dto,
    //             TaskspaceImpedanceController::Config& bo)
    //    {
    //        auto ftBO = ::armarx::fromAron<ft::FTSensor::FTConfig, ft::arondto::FTConfig>(dto.ftConfig);
    //        bo = TaskspaceImpedanceController::Config{.kpImpedance = dto.kpImpedance,
    //                                                  .kdImpedance = dto.kdImpedance,
    //                                                  .kpNullspaceTorque = dto.kpNullspaceTorque,
    //                                                  .kdNullspaceTorque = dto.kdNullspaceTorque,
    //                                                  .desiredPose = dto.desiredPose,
    //                                                  .desiredTwist = dto.desiredTwist,
    //                                                  .desiredNullspaceJointAngles =
    //                                                      dto.desiredNullspaceJointAngles,
    //                                                  .torqueLimit = dto.torqueLimit,
    //                                                  .qvelFilter = dto.qvelFilter,
    //                                                  .ftConfig = ftBO};
    //    }
 
    //    void
    //    fromAron(const arondto::TaskspaceImpedanceControllerConfigDict& dto,
    //             TaskspaceImpedanceController::ConfigDict& bo)
    //    {
    //        for (const auto& pair : dto.cfg)
    //        {
    //            bo.cfg.emplace(
    //                pair.first,
    //                ::armarx::fromAron<TaskspaceImpedanceController::Config,
    //                                   arondto::TaskspaceImpedanceControllerConfig>(pair.second));
    //        }
    //    }
 
    //    void
    //    toAron(arondto::TaskspaceImpedanceControllerConfigDict& dto,
    //           const TaskspaceImpedanceController::ConfigDict& bo)
    //    {
    //        for (const auto& pair : bo.cfg)
    //        {
    //            dto.cfg.emplace(pair.first,
    //                            ::armarx::toAron<arondto::TaskspaceImpedanceControllerConfig,
    //                                             TaskspaceImpedanceController::Config>(pair.second));
    //        }
    //    }
 
    //    void
    //    toAron(arondto::TaskspaceImpedanceControllerConfig& dto,
    //           const TaskspaceImpedanceController::Config& bo)
    //    {
    //        dto.kpImpedance = bo.kpImpedance;
    //        dto.kdImpedance = bo.kdImpedance;
    //        dto.kpNullspaceTorque = bo.kpNullspaceTorque;
    //        dto.kdNullspaceTorque = bo.kdNullspaceTorque;
    //        dto.desiredPose = bo.desiredPose;
    //        dto.desiredTwist = bo.desiredTwist;
    //        dto.desiredNullspaceJointAngles = bo.desiredNullspaceJointAngles;
    //        dto.torqueLimit = bo.torqueLimit;
    //        dto.qvelFilter = bo.qvelFilter;
    //        dto.ftConfig = ::armarx::toAron<ft::arondto::FTConfig, ft::FTSensor::FTConfig>(bo.ftConfig);
    //    }
 
    // // Mixed Impedance-velocity controller
    // void
    // fromAron(const arondto::TaskspaceMixedImpedanceVelocityControllerConfig& dto,
    //          TaskspaceMixedImpedanceVelocityController::Config& bo)
    // {
    //     auto ftBO = ::armarx::fromAron<ft::FTSensor::FTConfig, ft::arondto::FTConfig>(dto.ftConfig);
    //     bo = TaskspaceMixedImpedanceVelocityController::Config{
    //         .jointNameListTorque = dto.jointNameListTorque,
    //         .jointNameListVelocity = dto.jointNameListVelocity,
    //         .kpImpedance = dto.kpImpedance,
    //         .kdImpedance = dto.kdImpedance,
    //         .kpNullspaceTorque = dto.kpNullspaceTorque,
    //         .kdNullspaceTorque = dto.kdNullspaceTorque,
    //         .kpCartesianVel = dto.kpCartesianVel,
    //         .kdCartesianVel = dto.kdCartesianVel,
    //         .kpNullspaceVel = dto.kpNullspaceVel,
    //         .kdNullspaceVel = dto.kdNullspaceVel,
    //         .desiredPose = dto.desiredPose,
    //         .desiredTwist = dto.desiredTwist,
    //         .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles,
    //         .torqueLimit = dto.torqueLimit,
    //         .velocityLimit = dto.velocityLimit,
    //         .qvelFilter = dto.qvelFilter,
    //         .safeDistanceToGoal = dto.safeDistanceToGoal,
    //         .ftConfig = ftBO};
    // }
    //
    // void
    // fromAron(const arondto::TaskspaceMixedImpedanceVelocityControllerConfigDict& dto,
    //          TaskspaceMixedImpedanceVelocityController::ConfigDict& bo)
    // {
    //     for (const auto& pair : dto.cfg)
    //     {
    //         bo.cfg.emplace(
    //             pair.first,
    //             ::armarx::fromAron<TaskspaceMixedImpedanceVelocityController::Config,
    //                                arondto::TaskspaceMixedImpedanceVelocityControllerConfig>(
    //                 pair.second));
    //     }
    // }
    //
    // void
    // toAron(arondto::TaskspaceMixedImpedanceVelocityControllerConfigDict& dto,
    //        const TaskspaceMixedImpedanceVelocityController::ConfigDict& bo)
    // {
    //     for (const auto& pair : bo.cfg)
    //     {
    //         dto.cfg.emplace(
    //             pair.first,
    //             ::armarx::toAron<arondto::TaskspaceMixedImpedanceVelocityControllerConfig,
    //                              TaskspaceMixedImpedanceVelocityController::Config>(pair.second));
    //     }
    // }
    //
    // void
    // toAron(arondto::TaskspaceMixedImpedanceVelocityControllerConfig& dto,
    //        const TaskspaceMixedImpedanceVelocityController::Config& bo)
    // {
    //     dto.jointNameListTorque = bo.jointNameListTorque;
    //     dto.jointNameListVelocity = bo.jointNameListVelocity;
    //     dto.kpImpedance = bo.kpImpedance;
    //     dto.kdImpedance = bo.kdImpedance;
    //     dto.kpNullspaceTorque = bo.kpNullspaceTorque;
    //     dto.kdNullspaceTorque = bo.kdNullspaceTorque;
    //     dto.kpCartesianVel = bo.kpCartesianVel;
    //     dto.kdCartesianVel = bo.kdCartesianVel;
    //     dto.kpNullspaceVel = bo.kpNullspaceVel;
    //     dto.kdNullspaceVel = bo.kdNullspaceVel;
    //     dto.desiredPose = bo.desiredPose;
    //     dto.desiredTwist = bo.desiredTwist;
    //     dto.desiredNullspaceJointAngles = bo.desiredNullspaceJointAngles;
    //     dto.torqueLimit = bo.torqueLimit;
    //     dto.velocityLimit = bo.velocityLimit;
    //     dto.qvelFilter = bo.qvelFilter;
    //     dto.safeDistanceToGoal = bo.safeDistanceToGoal;
    //     dto.ftConfig = ::armarx::toAron<ft::arondto::FTConfig, ft::FTSensor::FTConfig>(bo.ftConfig);
    // }
    //
    // // admittance controller
    // void
    // fromAron(const arondto::TaskspaceAdmittanceControllerConfig& dto,
    //          TaskspaceAdmittanceController::Config& bo)
    // {
    //     auto ftBO = ::armarx::fromAron<ft::FTSensor::FTConfig, ft::arondto::FTConfig>(dto.ftConfig);
    //     bo = TaskspaceAdmittanceController::Config{.kpImpedance = dto.kpImpedance,
    //                                                .kdImpedance = dto.kdImpedance,
    //                                                .kpAdmittance = dto.kpAdmittance,
    //                                                .kdAdmittance = dto.kdAdmittance,
    //                                                .kmAdmittance = dto.kmAdmittance,
    //                                                .kpNullspaceTorque = dto.kpNullspaceTorque,
    //                                                .kdNullspaceTorque = dto.kdNullspaceTorque,
    //                                                .desiredPose = dto.desiredPose,
    //                                                .desiredTwist = dto.desiredTwist,
    //                                                .desiredNullspaceJointAngles =
    //                                                    dto.desiredNullspaceJointAngles,
    //                                                .torqueLimit = dto.torqueLimit,
    //                                                .qvelFilter = dto.qvelFilter,
    //                                                .ftConfig = ftBO};
    // }
    //
    // void
    // toAron(arondto::TaskspaceAdmittanceControllerConfig& dto,
    //        const TaskspaceAdmittanceController::Config& bo)
    // {
    //     dto.kpImpedance = bo.kpImpedance;
    //     dto.kdImpedance = bo.kdImpedance;
    //     dto.kpAdmittance = bo.kpAdmittance;
    //     dto.kdAdmittance = bo.kdAdmittance;
    //     dto.kmAdmittance = bo.kmAdmittance;
    //     dto.kpNullspaceTorque = bo.kpNullspaceTorque;
    //     dto.kdNullspaceTorque = bo.kdNullspaceTorque;
    //     dto.desiredPose = bo.desiredPose;
    //     dto.desiredTwist = bo.desiredTwist;
    //     dto.desiredNullspaceJointAngles = bo.desiredNullspaceJointAngles;
    //     dto.torqueLimit = bo.torqueLimit;
    //     dto.qvelFilter = bo.qvelFilter;
    //     dto.ftConfig = ::armarx::toAron<ft::arondto::FTConfig, ft::FTSensor::FTConfig>(bo.ftConfig);
    // }
    //
    // void
    // fromAron(const arondto::TaskspaceAdmittanceControllerConfigDict& dto,
    //          TaskspaceAdmittanceController::ConfigDict& bo)
    // {
    //     for (const auto& pair : dto.cfg)
    //     {
    //         bo.cfg.emplace(
    //             pair.first,
    //             ::armarx::fromAron<TaskspaceAdmittanceController::Config,
    //                                arondto::TaskspaceAdmittanceControllerConfig>(pair.second));
    //     }
    // }
    //
    // void
    // toAron(arondto::TaskspaceAdmittanceControllerConfigDict& dto,
    //        const TaskspaceAdmittanceController::ConfigDict& bo)
    // {
    //     for (const auto& pair : bo.cfg)
    //     {
    //         dto.cfg.emplace(pair.first,
    //                         ::armarx::toAron<arondto::TaskspaceAdmittanceControllerConfig,
    //                                          TaskspaceAdmittanceController::Config>(pair.second));
    //     }
    // }
    //
    // void
    // fromAron(const arondto::KeypointControllerConfig& dto, KeypointsImpedanceController::Config& bo)
    // {
    //     bo = KeypointsImpedanceController::Config{
    //         .nodeSetName = dto.nodeSetName,
    //         .kpImpedance = dto.kpImpedance,
    //         .kdImpedance = dto.kdImpedance,
    //         //                .kpAdmittance = dto.kpAdmittance,
    //         .kdAdmittance = dto.kdAdmittance,
    //         .kmAdmittance = dto.kmAdmittance,
    //         .kpNullspaceTorque = dto.kpNullspaceTorque,
    //         .kdNullspaceTorque = dto.kdNullspaceTorque,
    //         //                .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};
    // }
    //
    // void
    // toAron(arondto::KeypointControllerConfig& dto, const KeypointsImpedanceController::Config& bo)
    // {
    //     dto.nodeSetName = bo.nodeSetName;
    //     dto.kpImpedance = bo.kpImpedance;
    //     dto.kdImpedance = bo.kdImpedance;
    //     //      dto          .kpAdmittance = bo.kpAdmittance;
    //     dto.kdAdmittance = bo.kdAdmittance;
    //     dto.kmAdmittance = bo.kmAdmittance;
    //     dto.kpNullspaceTorque = bo.kpNullspaceTorque;
    //     dto.kdNullspaceTorque = bo.kdNullspaceTorque;
    //     //      dto          .currentForceTorque = bo.currentForceTorque;
    //     //      dto          .desiredPose = bo.desiredPose;
    //     //      dto          .desiredTwist = bo.desiredTwist;
    //     dto.desiredNullspaceJointAngles = bo.desiredNullspaceJointAngles;
    //     dto.torqueLimit = bo.torqueLimit;
    //     dto.qvelFilter = bo.qvelFilter;
    //     dto.numPoints = bo.numPoints;
    //     dto.keypointKp = bo.keypointKp;
    //     dto.keypointKd = bo.keypointKd;
    //     dto.fixedTranslation = bo.fixedTranslation;
    //     dto.densityForceScaling = bo.densityForceScaling;
    //     dto.keypointPositionFilter = bo.keypointPositionFilter;
    //     dto.keypointVelocityFilter = bo.keypointVelocityFilter;
    //     dto.isRigid = bo.isRigid;
    // }
 
} // namespace armarx::control::common::control_law
 
namespace armarx::control::common::mp
{
    //    void
    //    fromAron(const arondto::ListViaPoint& dto, MP::ListViaPoint& bo)
    //    {
    //        bo = MP::ListViaPoint{.canonicalValue = dto.canonicalValue,
    //                              .viaPointValue = dto.viaPointValue};
    //    }
 
    //    void
    //    toAron(arondto::ListViaPoint& dto, const MP::ListViaPoint& bo)
    //    {
    //        dto.canonicalValue = bo.canonicalValue;
    //        dto.viaPointValue = bo.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 ::armarx::fromAron<MP::ListViaPoint, arondto::ListViaPoint>(vp); });
    //        bo = MP::MPConfig{.name = dto.name,
    //                          .nodeSetName = dto.nodeSetName,
    //                          .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};
    //        armarx::aron::fromAron<armarx::control::common::mp::arondto::MPTraj, MPTraj>(
    //            dto.trajectoryList, bo.trajectoryList);
    //    }
 
    //    void
    //    toAron(arondto::MPConfig& dto, const MP::MPConfig& bo)
    //    {
    //        dto.name = bo.name;
    //        dto.nodeSetName = bo.nodeSetName;
    //        dto.className = bo.className;
    //        dto.mpTypeString = bo.mpTypeString;
    //        dto.mpMode = bo.mpMode;
    //        dto.mpStyle = bo.mpStyle;
    //        dto.regressionModel = bo.regressionModel;
    //        dto.kernelSize = bo.kernelSize;
    //        dto.damping = bo.damping;
    //        dto.tau = bo.tau;
    //        dto.amplitude = bo.amplitude;
    //        dto.durationSec = bo.durationSec;
    //        dto.fileList = bo.fileList;
    //        dto.stopWithMP = bo.stopWithMP;
    //        std::transform(bo.viaPoints.begin(),
    //                       bo.viaPoints.end(),
    //                       std::back_inserter(dto.viaPoints),
    //                       [](const auto& vp) -> arondto::ListViaPoint
    //                       { return ::armarx::toAron<arondto::ListViaPoint, MP::ListViaPoint>(vp); });
    //        dto.enablePhaseStop = bo.enablePhaseStop; /// phase stop
    //        dto.maxValue = bo.maxValue;
    //        dto.slop = bo.slop;
    //        dto.goDist = bo.goDist;
    //        dto.backDist = bo.backDist;
    //        dto.psKpPos = bo.psKpPos;
    //        dto.psKdPos = bo.psKdPos;
    //        dto.psKpOri = bo.psKpOri;
    //        dto.psKdOri = bo.psKdOri;
    //        dto.psMM2Radian = bo.psMM2Radian;
    //        armarx::aron::toAron<armarx::control::common::mp::arondto::MPTraj, MPTraj>(
    //            dto.trajectoryList, bo.trajectoryList);
    //    }
 
    //    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 ::armarx::fromAron<MP::MPConfig, arondto::MPConfig>(mp); });
    //    }
 
    //    void
    //    toAron(arondto::MPListConfig& dto, const MP::MPListConfig& bo)
    //    {
    //        std::transform(bo.mpList.begin(),
    //                       bo.mpList.end(),
    //                       std::back_inserter(dto.mpList),
    //                       [](const auto& _mp) -> arondto::MPConfig
    //                       { return ::armarx::toAron<arondto::MPConfig, MP::MPConfig>(_mp); });
    //    }
 
    //    // MPTraj
    //    void
    //    fromAron(const arondto::MPTraj& dto, MPTraj& bo)
    //    {
    //        bo = MPTraj{.time = dto.time, .traj = dto.traj};
    //    }
 
    //    void
    //    toAron(arondto::MPTraj& dto, const MPTraj& bo)
    //    {
    //        dto.time = bo.time;
    //        dto.traj = bo.traj;
    //    }
 
    //    void
    //    fromAron(const arondto::MPTrajs& dto, MPTrajs& bo)
    //    {
    //        std::transform(dto.data.begin(),
    //                       dto.data.end(),
    //                       std::back_inserter(bo),
    //                       [](const auto& traj) -> MPTraj
    //                       { return ::armarx::fromAron<MPTraj, arondto::MPTraj>(traj); });
    //    }
 
    //    void
    //    toAron(arondto::MPTrajs& dto, const MPTrajs& bo)
    //    {
    //        std::transform(bo.begin(),
    //                       bo.end(),
    //                       std::back_inserter(dto),
    //                       [](const auto& traj) -> arondto::MPTraj
    //                       { return ::armarx::toAron<arondto::MPTraj, MPTraj>(traj); });
    //    }
 
    //    void
    //    fromAron(const arondto::MultiMPTrajs& dto, MultiMPTrajs& bo)
    //    {
    //        for (const auto& pair : dto.data)
    //        {
    //            bo.emplace(pair.first, ::armarx::fromAron<MPTrajs, arondto::MPTrajs>(pair.second));
    //        }
    //    }
 
    //    void
    //    toAron(arondto::MultiMPTrajs& dto, const MultiMPTrajs& bo)
    //    {
    //        for (const auto& pair : bo)
    //        {
    //            dto.data.emplace(pair.first, ::armarx::toAron<arondto::MPTrajs, MPTrajs>(pair.second));
    //        }
    //    }
} // namespace armarx::control::common::mp