ZeroTorqueOrVelocityWithFT.cpp

Go to the documentation of this file.

#include "ZeroTorqueOrVelocityWithFT.h"
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
 
namespace armarx::control::njoint_controller::joint_space
{
 
    NJointControllerRegistration<NJointZeroTorqueOrVelocityWithFTController>
        registrationControllerNJointZeroTorqueOrVelocityWithFTController(
            "NJointZeroTorqueOrVelocityWithFTController");
 
    std::string
    NJointZeroTorqueOrVelocityWithFTController::getClassName(const Ice::Current&) const
    {
        return "NJointZeroTorqueOrVelocityWithFTController";
    }
 
    NJointZeroTorqueOrVelocityWithFTController::NJointZeroTorqueOrVelocityWithFTController(
        RobotUnitPtr prov,
        const NJointZeroTorqueOrVelocityWithFTControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& r)
    {
        ARMARX_CHECK_EXPRESSION(prov);
        RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
        useSynchronizedRtRobot();
        ARMARX_CHECK_NOT_NULL(robotUnit);
        ARMARX_CHECK_NOT_NULL(config);
        ARMARX_CHECK_EXPRESSION(not(config->robotNodeSetList.empty()));
 
        for (auto& robotNodeSetName : config->robotNodeSetList)
        {
            VirtualRobot::RobotNodeSetPtr rtRns = rtGetRobot()->getRobotNodeSet(robotNodeSetName);
            ARMARX_CHECK_EXPRESSION(rtRns) << robotNodeSetName;
 
            for (size_t i = 0; i < rtRns->getSize(); i++)
            {
                std::string jointName = rtRns->getNode(i)->getName();
                if (jointName.find("wri") != std::string::npos and
                    config->useZeroVelocityModeForWrist)
                {
                    ControlTargetBase* ct =
                        useControlTarget(jointName, ControlModes::ZeroVelocity1DoF);
                    ARMARX_CHECK_NOT_NULL(ct);
                    targetsVel.push_back(ct->asA<ControlTarget1DoFActuatorZeroVelocity>());
                    torqueControlled.push_back(false);
                }
                else
                {
                    ControlTargetBase* ct =
                        useControlTarget(jointName, ControlModes::ZeroTorque1DoF);
                    ARMARX_CHECK_NOT_NULL(ct);
                    targetsTorque.push_back(ct->asA<ControlTarget1DoFActuatorZeroTorque>());
                    torqueControlled.push_back(true);
                }
            }
        }
        size_t numJoints = targetsVel.size() + targetsTorque.size();
        ARMARX_CHECK_EQUAL(numJoints, torqueControlled.size());
 
        maxTorque = config->maxTorque;
        maxVelocity = config->maxVelocity;
 
        NJointZeroTorqueOrVelocityWithFTControllerTarget target;
        target.targetTorqueOrVelocity = Ice::FloatSeq(numJoints, 0.0f);
        this->reinitTripleBuffer(target);
 
        ik.reset(new VirtualRobot::DifferentialIK(
            rtRns, rtRns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
 
        I = Eigen::MatrixXf::Identity(numOfJoints, numOfJoints);
    }
 
    void
    NJointZeroTorqueOrVelocityWithFTController::rtPreActivateController()
    {
        auto numJoints = targetsVel.size() + targetsTorque.size();
        NJointZeroTorqueOrVelocityWithFTControllerTarget target;
        target.targetTorqueOrVelocity = Ice::FloatSeq(numJoints, 0.0f);
        this->reinitTripleBuffer(target);
    }
 
    void
    NJointZeroTorqueOrVelocityWithFTController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                                      const IceUtil::Time& timeSinceLastIteration)
    {
        /// compute jocobi
        rtStatus.jacobi =
            ik->getJacobianMatrix(rtTCP, VirtualRobot::IKSolver::CartesianSelection::All);
        rtStatus.jacobi.block(0, 0, 3, rtStatus.numJoints) =
            0.001 * rtStatus.jacobi.block(0, 0, 3, rtStatus.numJoints);
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jacobi.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jacobi.rows());
 
        rtStatus.jtpinv =
            ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi.transpose(), lambda);
        rtStatus.jpinv = ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi, lambda);
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jtpinv.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jtpinv.rows());
 
 
        size_t torqueIndex = 0;
        size_t velIndex = 0;
        for (size_t i = 0; i < torqueControlled.size(); ++i)
        {
            if (torqueControlled.at(i))
            {
                targetsTorque.at(torqueIndex)->torque = math::MathUtils::LimitTo(
                    rtGetControlStruct().targetTorqueOrVelocity.at(i), maxTorque);
                torqueIndex++;
            }
            else
            {
                targetsVel.at(velIndex)->velocity = math::MathUtils::LimitTo(
                    rtGetControlStruct().targetTorqueOrVelocity.at(i), maxVelocity);
                velIndex++;
            }
        }
    }
 
    void
    NJointZeroTorqueOrVelocityWithFTController::onPublish(const SensorAndControl&,
                                                          const DebugDrawerInterfacePrx&,
                                                          const DebugObserverInterfacePrx&)
    {
    }
 
    WidgetDescription::WidgetPtr
    NJointZeroTorqueOrVelocityWithFTController::GenerateConfigDescription(
        const VirtualRobot::RobotPtr& robot,
        const std::map<std::string, ConstControlDevicePtr>& controlDevices,
        const std::map<std::string, ConstSensorDevicePtr>&)
    {
        using namespace armarx::WidgetDescription;
        HBoxLayoutPtr layout = new HBoxLayout;
 
 
        ::armarx::WidgetDescription::WidgetSeq widgets;
        auto addSlider = [&](const std::string& label, float min, float max, float defaultValue)
        {
            widgets.emplace_back(new Label(false, label));
            {
                FloatSliderPtr c_x = new FloatSlider;
                c_x->name = label;
                c_x->min = min;
                c_x->defaultValue = defaultValue;
                c_x->max = max;
                widgets.emplace_back(c_x);
            }
        };
 
        /// joint names
        LabelPtr label = new Label;
        label->text = "Robot Node Sets";
        layout->children.emplace_back(label);
 
        StringComboBoxPtr box = new StringComboBox;
        box->name = "robotNodeSetSelector";
        box->defaultIndex = 0;
        box->multiSelect = true;
        box->options.push_back("LeftArm");
        box->options.push_back("RightArm");
        layout->children.emplace_back(box);
 
        //        LabelPtr labelZeroVelToggle = new Label;
        //        labelZeroVelToggle->text = "Wrist ZeroVelocity?";
        //        layout->children.emplace_back(labelZeroVelToggle);
 
        CheckBoxPtr checkBox = new CheckBox;
        checkBox->name = "useZeroVelocityModeForWrist";
        checkBox->label = "Wrist ZeroVelocity";
        checkBox->defaultValue = true;
 
        addSlider("maxTorque", 0, 100, 10);
        addSlider("maxVeloicty", 0, 3.14, 2.0);
 
        layout->children.insert(layout->children.end(), widgets.begin(), widgets.end());
        return layout;
    }
 
    NJointZeroTorqueOrVelocityWithFTControllerConfigPtr
    NJointZeroTorqueOrVelocityWithFTController::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        auto varNodeSets = VariantPtr::dynamicCast(values.at("robotNodeSetSelector"));
        ARMARX_CHECK_EXPRESSION(varNodeSets) << "robotNodeSetSelector";
 
        ARMARX_TRACE;
        return new NJointZeroTorqueOrVelocityWithFTControllerConfig{
            varNodeSets->get<SingleTypeVariantList>()->toStdVector<std::string>(),
            values.at("useZeroVelocityModeForWrist")->getBool(),
            values.at("maxTorque")->getFloat(),
            values.at("maxVeloicty")->getFloat()};
    }
 
    void
    NJointZeroTorqueOrVelocityWithFTController::rtPostDeactivateController()
    {
    }
 
    void
    NJointZeroTorqueOrVelocityWithFTController::setControllerTarget(const Ice::FloatSeq& targets,
                                                                    const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        this->getWriterControlStruct().targetTorqueOrVelocity = targets;
        writeControlStruct();
    }
} // namespace armarx::control::njoint_controller::joint_space