Skip to content
Snippets Groups Projects
NJointKneeHipController.cpp 5.07 KiB
Newer Older
#include "NJointKneeHipController.h"

#include <SimoxUtility/math/distance/angle_between.h>

#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/observers/VariantBase.h>

// #include <armar7/rt/units/njoint_controller/common/controller_types.h>
// #include <armar7/rt/units/njoint_controller/knee_hip_controller/aron/ControllerConfig.aron.generated.h>
// #include <armar7/rt/units/njoint_controller/knee_hip_controller/aron_conversions.h>




namespace armarx

{
    const armarx::NJointControllerRegistration<NJointKneeHipController>
        registrationControllerNJointKneeHipController(
            "NJointKneeHipController");

    NJointKneeHipController::NJointKneeHipController(RobotUnit* robotUnit,
                                         const armarx::NJointControllerConfigPtr& config,
                                         const VirtualRobot::RobotPtr&)
    {
        ARMARX_RT_LOGF("Creating knee hip controller");

        ARMARX_CHECK_EXPRESSION(robotUnit);

        // config
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);

        // const arondto::Config configData = arondto::Config::FromAron(cfg->config);

        // robot
        ARMARX_RT_LOGF("Setting up nodes");
        {
            _rtRobot = useSynchronizedRtRobot();
            _rtRobot->setThreadsafe(false);
            ARMARX_CHECK_NOT_NULL(_rtRobot);

            // _rtKneeNode = _rtRobot->getRobotNode(configData.params.kneeJointName);
            // _rtHipNode = _rtRobot->getRobotNode(configData.params.hipJointName);
            // _rtHipBodyNode = _rtRobot->getRobotNode(configData.params.hipBodyName);
            const auto *kneeJointName = "Knee";
            const auto *hipJointName = "Hip";
            const auto *hipBodyName = "Hip_body";


            _rtKneeNode = _rtRobot->getRobotNode(kneeJointName);
            _rtHipNode = _rtRobot->getRobotNode(hipJointName);
            _rtHipBodyNode = _rtRobot->getRobotNode(hipBodyName);
            ARMARX_CHECK_NOT_NULL(_rtKneeNode);
            ARMARX_CHECK_NOT_NULL(_rtHipNode);
            ARMARX_CHECK_NOT_NULL(_rtHipBodyNode);

            _rtKneeCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
                kneeJointName, armarx::ControlModes::Position1DoF);
            ARMARX_CHECK_NOT_NULL(_rtKneeCtrlTarget);
            _rtHipCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
                hipJointName, armarx::ControlModes::Position1DoF);
            ARMARX_CHECK_NOT_NULL(_rtHipCtrlTarget);
        }

        ARMARX_RT_LOGF("Nodes set up successfully");
    }

    NJointKneeHipController::~NJointKneeHipController() = default;

    // void
    // KneeHipController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
    //                                 const Ice::Current& /*iceCurrent*/)
    // {
    //     // const auto updateConfigDto = arondto::Config::FromAron(dto);

    //     // Config config;
    //     // fromAron(updateConfigDto, config);

    //     // setControlStruct(config);
    // }

    void
    NJointKneeHipController::onInitNJointController()
    {
    }

    void
    NJointKneeHipController::onConnectNJointController()
    {
    }

    std::string
    NJointKneeHipController::getClassName(const Ice::Current&) const
    {
        return "KneeHipController";
    }

    void
    NJointKneeHipController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
                             const IceUtil::Time& /*timeSinceLastIteration*/)
    {
        const float currentHipJointValue = _rtHipNode->getJointValue();
        const auto hipRotation =
            _rtHipBodyNode->getOrientationInRootFrame() * Eigen::Vector3f::UnitY();

        // angle between robot root and hip body in the yz-plane
        // leaning forwards < 90° = upright < leaning backwards
        const float hipAngle = simox::math::angle_between_vec3f_vec3f(
            Eigen::Vector3f::UnitY(), hipRotation, Eigen::Vector3f::UnitX());

        // hip joint has positive angles leaning forwards (inverse to hipAngle)
        float targetHipAngle =
            currentHipJointValue + hipAngle - M_PI_2 + rtGetControlStruct().bowAngle;

        float targetKneeAngle = rtGetControlStruct().kneeAngle;

        // check reachability
        const bool targetReachable = _rtKneeNode->checkJointLimits(targetKneeAngle) &&
                                     _rtHipNode->checkJointLimits(targetHipAngle);
        if (not targetReachable)
        {
            // limit joint ranges to avoid damage
            targetKneeAngle = std::clamp(
                targetKneeAngle, _rtKneeNode->getJointLimitLow(), _rtKneeNode->getJointLimitHigh());
            targetHipAngle = std::clamp(
                targetHipAngle, _rtHipNode->getJointLimitLow(), _rtHipNode->getJointLimitHigh());
        }

        _rtKneeCtrlTarget->position = targetKneeAngle;
        _rtHipCtrlTarget->position = targetHipAngle;
    }

    void
    NJointKneeHipController::rtPreActivateController()
    {
    }

    void
    NJointKneeHipController::rtPostDeactivateController()
    {
    }


} // namespace armarx