WholeBodyTrajectoryController.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       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "WholeBodyTrajectoryController.h"
 
#include <cstdint>
#include <optional>
#include <set>
 
#include <SimoxUtility/algorithm/string/string_conversion_eigen.h>
#include <SimoxUtility/json/eigen_conversion.h>
#include <SimoxUtility/json/json.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/forward_declarations.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <armarx/control/common/type.h>
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/control/njoint_controller/joint_space/aron/WholeBodyTrajectoryControllerConfig.aron.generated.h>
 
#include <range/v3/view/zip.hpp>
 
namespace armarx::control::njoint_controller::joint_space::whole_body_trajectory_controller
{
 
    const NJointControllerRegistration<Controller> Registration(
        common::ControllerTypeNames.to_name(common::ControllerType::WholeBodyTrajectoryController));
 
    Controller::Controller(const RobotUnitPtr& robotUnit,
                           const NJointControllerConfigPtr& config,
                           const VirtualRobot::RobotPtr& robot) :
        ConfigurableNJointControllerBaseT(config),
        pidPlatform_(initialConfigData().pidPlatform.Kp,
                     initialConfigData().pidPlatform.Ki,
                     initialConfigData().pidPlatform.Kd,
                     initialConfigData().pidPlatform.maxControlValue,
                     initialConfigData().pidPlatform.maxDerivation),
        opidPlatform_(initialConfigData().opidPlatform.Kp,
                      initialConfigData().opidPlatform.Ki,
                      initialConfigData().opidPlatform.Kd,
                      initialConfigData().opidPlatform.maxControlValue,
                      initialConfigData().opidPlatform.maxDerivation,
                      true)
    {
        ARMARX_IMPORTANT << "Creating "
                         << common::ControllerTypeNames.to_name(
                                common::ControllerType::WholeBodyTrajectoryController);
 
        ARMARX_CHECK_EXPRESSION(robotUnit);
 
        ARMARX_INFO << "Using RT robot";
        robot_ = useSynchronizedRtRobot();
        ARMARX_CHECK_NOT_NULL(robot_);
 
        ARMARX_INFO << "Created RT robot";
 
 
        // initialize control targets
        {
            ARMARX_INFO << "Initializing control targets";
            controlTargets_.platformTarget =
                useControlTarget(robotUnit->getRobotPlatformName(),
                                 ControlModes::HolonomicPlatformVelocity)
                    ->asA<ControlTargetHolonomicPlatformVelocity>();
            ARMARX_CHECK_EXPRESSION(controlTargets_.platformTarget)
                << "The actuator " << robotUnit->getRobotPlatformName() << " has no control mode "
                << ControlModes::HolonomicPlatformVelocity;
 
            for (const auto& jointName : initialConfigData().jointNames)
            {
                auto* const controlTarget = useControlTarget<ControlTarget1DoFActuatorVelocity>(
                    jointName, ControlModes::Velocity1DoF);
                ARMARX_CHECK_NOT_NULL(controlTarget) << jointName;
 
                controlTargets_.jointTargets.push_back(controlTarget);
            }
            ARMARX_INFO << VAROUT(initialConfigData().jointNames);
 
            for (const auto& jointName : initialConfigData().handJointNames)
            {
                auto* const controlTarget = useControlTarget<ControlTarget1DoFActuatorPosition>(
                    jointName, ControlModes::Position1DoF);
                ARMARX_CHECK_NOT_NULL(controlTarget) << jointName;
 
                controlTargets_.handTargets.push_back(controlTarget);
            }
            ARMARX_INFO << VAROUT(initialConfigData().handJointNames);
        }
 
        // initialize sensor values
        {
            ARMARX_INFO << "Initializing sensor values";
 
            // initialize joint sensor values
            {
                for (const auto& jointName : initialConfigData().jointNames)
                {
                    const SensorValueBase* sv = useSensorValue(jointName);
                    jointSensorValues_.push_back(sv->asA<SensorValue1DoFActuator>());
                }
            }
 
            // initialize global pose sensor value
            {
                const SensorValueBase* sv =
                    useSensorValue(GlobalRobotLocalizationSensorDevice::DeviceName());
                svGlobalRobotLocalization_ =
                    sv->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
            }
        }
 
        // initialize arm PID controller
        {
            ARMARX_INFO << "Initializing PID controller";
 
            std::vector<bool> isJointLimitless;
            for (const auto& jointName : initialConfigData().jointNames)
            {
                const auto node = robot_->getRobotNode(jointName);
                isJointLimitless.push_back(node->isLimitless());
            }
 
            pidArm_.emplace(initialConfigData().pidArm.Kp,
                            initialConfigData().pidArm.Ki,
                            initialConfigData().pidArm.Kd,
                            initialConfigData().pidArm.maxControlValue,
                            initialConfigData().pidArm.maxDerivation,
                            false,
                            isJointLimitless);
 
            const std::size_t nDofArm = initialConfigData().jointNames.size();
            pidArm_->preallocate(nDofArm);
        }
 
        // preallocate other PID controllers
        {
            ARMARX_TRACE;
            pidPlatform_.preallocate(2);
        }
 
        // initialize jointNames
        {
            ARMARX_TRACE;
            jointNames_ = initialConfigData().jointNames;
        }
 
        // initialize helper nodes etc
        {
            ARMARX_INFO << "Initializing helper nodes";
            // FIXME remove hard coded string
            const std::string tcpNodeName = "Hand R TCP";
 
            // maybe this is causing issues (cloning rt robot in non rt part)
            ARMARX_INFO << "Debug robot";
            debugRobot_ = robot->clone();
            ARMARX_INFO << "cloned.";
 
            ARMARX_CHECK_NOT_NULL(debugRobot_);
            publishTcpNode_ = debugRobot_->getRobotNode(tcpNodeName);
            ARMARX_CHECK_NOT_NULL(publishTcpNode_);
 
            rtTcpNode_ = robot_->getRobotNode(tcpNodeName);
            ARMARX_CHECK_NOT_NULL(rtTcpNode_);
        }
 
        // initialize trajectory
        ARMARX_INFO << "Initializing trajectory";
        ARMARX_CHECK_NOT_EMPTY(initialConfigData().trajectory);
        trajectory_.emplace(initialConfigData().trajectory);
 
        reinitTripleBuffer({});
 
        // init debug data (for onPublish), preallocate memory (very important here!)
        {
            ARMARX_INFO << "Initializing debug data";
            const DebugData initDebugData{
                .localTimestamp = 0,
                .platformPosDiff = 0,
                .platformOriDiff = 0,
                .jointPosDiff = std::vector<float>(initialConfigData().jointNames.size(), 0),
                .targetState = trajectory_->initialPoint(),
                .currentTcpPose = Eigen::Isometry3f{rtTcpNode_->getGlobalPose()},
                .currentPlatformPose = Eigen::Isometry3f{robot_->getGlobalPose()},
                .isControllerActive = false};
 
            bufferRtToOnPublish_.reinitAllBuffers(initDebugData);
        }
 
        ARMARX_INFO << "Init done.";
    }
 
    std::string
    Controller::getClassName(const Ice::Current& iceCurrent) const
    {
        return common::ControllerTypeNames.to_name(
            common::ControllerType::WholeBodyTrajectoryController);
    }
 
    void
    Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                      const IceUtil::Time& timeSinceLastIteration)
    {
        rtUpdateControlStruct();
 
        auto& debugData = bufferRtToOnPublish_.getWriteBuffer();
 
        // const armarx::DateTime timestamp(sensorValuesTimestamp);
        const std::int64_t timestamp = armarx::rtNow().toMicroSeconds();
 
        ARMARX_CHECK_NOT_NULL(trajectory_);
 
        // trajectory replay finished?
        // if (not trajectory_->isValid(timestamp))
        // {
        //     std::cerr << "Timestamp is not valid!" << std::endl;
        //     std::cerr << timestamp << std::endl;
        //     std::cerr << armarx::rtNow() << std::endl;
 
        //     std::cerr << armarx::rtNow() << std::endl;
 
 
        //     // platform: set to velocity zero
        //     controlTargets_.platformTarget->velocityX = 0;
        //     controlTargets_.platformTarget->velocityY = 0;
        //     controlTargets_.platformTarget->velocityRotation = 0;
 
        //     // joints: we are in position control mode so we just keep the last target
 
        //     return;
        // }
 
        const auto trajPointOpt = trajectory_->at(timestamp);
 
        ARMARX_TRACE;
 
        if (not trajPointOpt.has_value()) // or not trajectory_->timeBounds().isWithin(timestamp))
        {
            // ARMARX_WARNING << "Timestamp is not within bounds!";
 
            // platform: set to velocity zero
            controlTargets_.platformTarget->velocityX = 0;
            controlTargets_.platformTarget->velocityY = 0;
            controlTargets_.platformTarget->velocityRotation = 0;
 
            // joints: we are in velocity control mode so we set the target to zero
            for (auto* const jointControlTarget : controlTargets_.jointTargets)
            {
                jointControlTarget->velocity = 0;
            }
 
            return;
        }
 
        ARMARX_TRACE;
        const Trajectory::TrajectoryPoint& trajPoint = trajPointOpt.value();
 
        debugData.localTimestamp = trajPoint.timestampMicroSeconds;
        debugData.targetState = trajPoint;
        debugData.currentTcpPose = Eigen::Isometry3f{rtTcpNode_->getGlobalPose()};
        debugData.currentPlatformPose = Eigen::Isometry3f{robot_->getGlobalPose()};
        debugData.isControllerActive = isControllerActive();
 
        // update control devices: platform; See: NJointHolonomicPlatformGlobalPositionController
        {
            ARMARX_CHECK_EQUAL(trajPoint.jointValuesPlatform.size(), 3);
            ARMARX_CHECK_EQUAL(trajPoint.jointVelocitiesPlatform.size(), 3);
 
            const Eigen::Vector2f global_P_robot_target{trajPoint.jointValuesPlatform.at(0),
                                                        trajPoint.jointValuesPlatform.at(1)};
            const double targetOrientationGlobal = trajPoint.jointValuesPlatform.at(2);
 
            ARMARX_CHECK_NOT_NULL(svGlobalRobotLocalization_);
            const auto global_T_robot = svGlobalRobotLocalization_->global_T_root;
            const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(global_T_robot);
            const float globalOrientation = rpy.z();
            const Eigen::Vector2f global_P_robot = global_T_robot.block<2, 1>(0, 3);
 
            const float measuredOrientation = globalOrientation;
 
            pidPlatform_.update(
                timeSinceLastIteration.toSecondsDouble(), global_P_robot, global_P_robot_target);
            opidPlatform_.update(timeSinceLastIteration.toSecondsDouble(),
                                 static_cast<double>(measuredOrientation),
                                 targetOrientationGlobal);
 
            {
                debugData.platformPosDiff = (global_P_robot_target - global_P_robot).norm();
                debugData.platformOriDiff =
                    static_cast<float>(targetOrientationGlobal) - measuredOrientation;
            }
 
 
            ARMARX_TRACE;
            {
                // The given velocity describes the velocity in the local frame of the robot relative to robot's global pose.
                // The velocity targets that we must generate however are relative to the robot's platform frame.
                // This means that we must rotate the given velocity targets to the robot's platform frame.
 
                const Eigen::Rotation2Df local_R_global(-measuredOrientation);
                const Eigen::Vector2f& vPosError = local_R_global * pidPlatform_.getControlValue();
                const Eigen::Vector2f vVelFFRobotFrame =
                    local_R_global * Eigen::Vector2f(trajPoint.jointVelocitiesPlatform.at(0),
                                                     trajPoint.jointVelocitiesPlatform.at(1));
 
                const auto& vFF = trajPoint.jointVelocitiesPlatform;
 
                // The feed forward values are in the local frame of the robot. This means that if the robot is rotated compared to desired orientation,
                // then the robot will move in the wrong direction.
 
                // To fix this, we rotate the feed forward values to the global frame of the robot.
                // const Eigen::Rotation2Df global_R_local_target(-targetOrientationGlobal);
                // const Eigen::Rotation2Df local_R_local_target =
                //     global_R_local.inverse() * global_R_local_target;
                // const Eigen::Vector2f vFF_local_target =
                // local_R_local_target * Eigen::Vector2f(vFF.at(0), vFF.at(1));
 
                constexpr float ffFactorPlatform = 1.0; // FIXME check without as well
 
 
                // controlTargets_.platformTarget->velocityX = vFF.at(0) + vPosError.x();
                // controlTargets_.platformTarget->velocityY = vFF.at(1) + vPosError.y();
                controlTargets_.platformTarget->velocityX =
                    ffFactorPlatform * vVelFFRobotFrame.x() + vPosError.x();
                controlTargets_.platformTarget->velocityY =
                    ffFactorPlatform * vVelFFRobotFrame.y() + vPosError.y();
                // controlTargets_.platformTarget->velocityRotation =
                //     vFF.at(2) + static_cast<float>(opidPlatform_.getControlValue());
 
                // FIXME remove 0
 
                controlTargets_.platformTarget->velocityRotation =
                    ffFactorPlatform * vFF.at(2) +
                    static_cast<float>(opidPlatform_.getControlValue());
            }
        }
 
        // update control devices: joints
        ARMARX_TRACE;
        {
            ARMARX_CHECK_EQUAL(controlTargets_.jointTargets.size(), trajPoint.jointValues.size());
            ARMARX_CHECK_EQUAL(controlTargets_.jointTargets.size(),
                               trajPoint.jointVelocities.size());
 
            ARMARX_CHECK_NOT_NULL(pidArm_);
 
 
            const auto toEigen = [](const std::vector<double>& vec) -> Eigen::VectorXf
            {
                Eigen::VectorXf eigenVec(vec.size());
                for (std::size_t i = 0; i < vec.size(); i++)
                {
                    eigenVec(i) = static_cast<float>(vec.at(i));
                }
                return eigenVec;
            };
 
            // update PID controller (internal update)
            {
 
                const Eigen::VectorXf jointPositionTargets = toEigen(trajPoint.jointValues);
 
                // set joint position measured based on sensor values
                Eigen::VectorXf jointPositionMeasured(jointSensorValues_.size());
                for (std::size_t i = 0; i < jointSensorValues_.size(); i++)
                {
                    jointPositionMeasured(i) = jointSensorValues_.at(i)->position;
                }
 
                // update debug data structure
                ARMARX_CHECK_EQUAL(debugData.jointPosDiff.size(), jointSensorValues_.size());
                for (std::size_t i = 0; i < jointSensorValues_.size(); i++)
                {
                    debugData.jointPosDiff.at(i) =
                        std::abs(jointPositionTargets(i) - jointPositionMeasured(i));
                }
 
                // PID control for position error
                pidArm_->update(timeSinceLastIteration.toSecondsDouble(),
                                jointPositionMeasured,
                                jointPositionTargets);
            }
 
            const Eigen::VectorXf& jointVelocityForPosError = pidArm_->getControlValue();
 
            for (std::size_t i = 0; i < controlTargets_.jointTargets.size(); i++)
            {
                auto* const jointControlTarget = controlTargets_.jointTargets.at(i);
 
                // FIXME check if within bounds etc
                // const double jointPosTarget = trajPoint.jointValues.at(i);
                const double jointVelTarget = trajPoint.jointVelocities.at(i);
 
 
                // update control target
                jointControlTarget->velocity =
                    static_cast<float>(jointVelTarget) + jointVelocityForPosError(i);
 
                if (not jointControlTarget->isValid())
                {
                    ARMARX_WARNING << "Invalid control target for " << VAROUT(i)
                                   << ". Value: " << jointControlTarget->velocity;
                }
            }
        }
 
        ARMARX_TRACE;
        // update control devices: hand
        {
            ARMARX_CHECK_EQUAL(controlTargets_.handTargets.size(),
                               trajPoint.handJointValues.size());
            for (std::size_t i = 0; i < controlTargets_.handTargets.size(); i++)
            {
                auto* const handControlTarget = controlTargets_.handTargets.at(i);
                const double jointPosTarget = trajPoint.handJointValues.at(i);
                handControlTarget->position = static_cast<float>(jointPosTarget);
            }
        }
 
        bufferRtToOnPublish_.commitWrite();
 
        // advance
        // trajIdx_++;
    }
 
    void
    Controller::rtPreActivateController()
    {
        ARMARX_CHECK_NOT_NULL(trajectory_);
        // FIXME is this the correct clock?
        const std::int64_t timestampMicroSeconds = armarx::rtNow().toMicroSeconds();
 
        trajectory_->setStartTime(timestampMicroSeconds);
 
        ARMARX_CHECK(
            whatever_.empty()); // controller is single-shot. Should be recreated for each run.
    }
 
    void
    Controller::onPublish(const SensorAndControl&,
                          const DebugDrawerInterfacePrx& debugDrawer,
                          const DebugObserverInterfacePrx& debugObservers)
    {
        StringVariantBaseMap datafields;
 
        const auto& debugData = bufferRtToOnPublish_.getUpToDateReadBuffer();
 
        datafields["localTimestamp"] = new Variant(debugData.localTimestamp);
 
        datafields["platform/dpos"] = new Variant(debugData.platformPosDiff);
        datafields["platform/dori"] = new Variant(debugData.platformOriDiff);
 
        ARMARX_CHECK_EQUAL(jointNames_.size(), debugData.jointPosDiff.size());
        for (const auto& [jointName, diff] :
             ranges::views::zip(jointNames_, debugData.jointPosDiff))
        {
            datafields[jointName + "/dpos"] = new Variant(diff);
        }
 
        const float tcpDiff = [&]() -> float
        {
            namespace rv = ranges::views;
 
            // set target joint values
            std::map<std::string, float> targetJointValues;
            for (const auto& [jointName, jointValue] :
                 rv::zip(jointNames_, debugData.targetState.jointValues))
            {
                targetJointValues.emplace(jointName, jointValue);
            }
 
            const Eigen::Isometry3f targetGlobalPose =
                Eigen::Translation3f{
                    static_cast<float>(debugData.targetState.jointValuesPlatform.at(0)),
                    static_cast<float>(debugData.targetState.jointValuesPlatform.at(1)),
                    0} *
                Eigen::AngleAxisf{
                    static_cast<float>(debugData.targetState.jointValuesPlatform.at(2)),
                    Eigen::Vector3f::UnitZ()};
 
            debugRobot_->setJointValues(targetJointValues);
            debugRobot_->setGlobalPose(targetGlobalPose.matrix());
 
            const Eigen::Isometry3f targetTcpPose(publishTcpNode_->getGlobalPose());
 
            const Eigen::Isometry3f tcpDiffTf = debugData.currentTcpPose.inverse() * targetTcpPose;
 
            auto batchPrx = debugDrawer->ice_batchOneway();
 
            // visu tcp poses
            {
 
                batchPrx->setSphereDebugLayerVisu(
                    "tcp_target",
                    new armarx::Vector3(Eigen::Vector3f{targetTcpPose.translation()}),
                    ::armarx::DrawColor{1, 0, 0, 0},
                    10);
 
                batchPrx->setSphereDebugLayerVisu(
                    "tcp_current",
                    new armarx::Vector3(Eigen::Vector3f{debugData.currentTcpPose.translation()}),
                    ::armarx::DrawColor{0, 1, 0, 0},
                    10);
 
                batchPrx->setPoseDebugLayerVisu("tcp_target_pose", toIce(targetTcpPose.matrix()));
                batchPrx->setPoseDebugLayerVisu("tcp_current_pose",
                                                toIce(debugData.currentTcpPose.matrix()));
            }
 
            // visu platform poses
            {
                batchPrx->setSphereDebugLayerVisu(
                    "platform_target",
                    new armarx::Vector3(Eigen::Vector3f{targetGlobalPose.translation()}),
                    ::armarx::DrawColor{1, 0, 0, 0},
                    10);
 
                batchPrx->setSphereDebugLayerVisu("platform_current",
                                                  new armarx::Vector3(Eigen::Vector3f{
                                                      debugData.currentPlatformPose.translation()}),
                                                  ::armarx::DrawColor{0, 1, 0, 0},
                                                  10);
 
                batchPrx->setPoseDebugLayerVisu("platform_target_pose",
                                                toIce(targetGlobalPose.matrix()));
 
                batchPrx->setPoseDebugLayerVisu("platform_current_pose",
                                                toIce(debugData.currentPlatformPose.matrix()));
            }
 
            batchPrx->ice_flushBatchRequests();
 
            // TODO if we are interested in the orientation diff, implement it here
 
            return tcpDiffTf.translation().norm();
        }();
 
        datafields["tcpDiff"] = new Variant(tcpDiff);
 
        debugObservers->setDebugChannel(common::ControllerTypeNames.to_name(
                                            common::ControllerType::WholeBodyTrajectoryController),
                                        datafields);
 
        // log data
        if (whateverFlag_.load())
        {
            whatever_.push_back(debugData);
        }
    }
 
    namespace arondto
    {
        void
        to_json(nlohmann::json& j, const TrajectoryPoint& trajectoryPoint)
        {
            j["timestampMicroSeconds"] = trajectoryPoint.timestampMicroSeconds;
            j["jointValues"] = trajectoryPoint.jointValues;
            j["jointValuesPlatform"] = trajectoryPoint.jointValuesPlatform;
            j["handJointValues"] = trajectoryPoint.handJointValues;
            j["jointVelocities"] = trajectoryPoint.jointVelocities;
            j["jointVelocitiesPlatform"] = trajectoryPoint.jointVelocitiesPlatform;
        }
    } // namespace arondto
 
    void
    to_json(nlohmann::json& j, const DebugData& debugData)
    {
        j["localTimestamp"] = debugData.localTimestamp;
        j["platformPosDiff"] = debugData.platformPosDiff;
        j["platformOriDiff"] = debugData.platformOriDiff;
        j["jointPosDiff"] = debugData.jointPosDiff;
        j["targetState"] = debugData.targetState;
        j["currentTcpPose"] = Eigen::Matrix4f{debugData.currentTcpPose.matrix()};
        j["currentPlatformPose"] = Eigen::Matrix4f{debugData.currentPlatformPose.matrix()};
        j["isControllerActive"] = debugData.isControllerActive;
    }
 
    void
    Controller::onDisconnectNJointController()
    {
        ARMARX_IMPORTANT << "Disconnecting controller " << getClassName() << "...";
        whateverFlag_.store(false);
 
        nlohmann::json j;
        j["data"] = whatever_;
 
        const std::string filename = armarx::Clock::Now().toDateTimeString() + ".json";
 
        const armarx::PackagePath packagePath{"grasping_on_the_move",
                                              "controller_execution/" + filename};
 
        ARMARX_INFO << "Writing debug data to " << packagePath.toSystemPath();
 
        std::ofstream ofs(packagePath.toSystemPath());
        ofs << std::setw(4) << j << std::endl;
        ofs.close();
    }
} // namespace armarx::control::njoint_controller::joint_space::whole_body_trajectory_controller