common.cpp

Go to the documentation of this file.

#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <armarx/control/common/control_law/common.h>
 
namespace armarx::control::common::control_law
{
 
    void
    RobotStatus::reset(const size_t nJoints,
                       const std::vector<size_t>& jointIDTorqueCtrlMode,
                       const std::vector<size_t>& jointIDVelocityCtrlMode)
    {
        jointPosition.setZero(nJoints);
        jointVelocity.setZero(nJoints);
        jointTorque.setZero(nJoints);
 
        deltaT = 0;
        accumulateTime = 0.0;
 
        nDoF = nJoints;
        nDoFTorque = jointIDTorqueCtrlMode.size();
        nDoFVelocity = jointIDVelocityCtrlMode.size();
 
        jointIDTorqueMode = jointIDTorqueCtrlMode;
        jointIDVelocityMode = jointIDVelocityCtrlMode;
        ARMARX_CHECK_EQUAL(nDoF, nDoFTorque + nDoFVelocity);
 
        globalPose = Eigen::Matrix4f::Identity();
    }
 
    void
    TSCtrlRtStatus::reset(const size_t numDoF,
                          const std::vector<size_t>& jointIDTorqueMode,
                          const std::vector<size_t>& jointIDVelocityMode)
    {
        /// This function should be called BEFORE the RT thread.
        /// Usually in constructor of the controller
        RobotStatus::reset(numDoF, jointIDTorqueMode, jointIDVelocityMode);
 
        /// targets
        desiredJointTorque.setZero(nDoF);
        desiredJointVelocity.setZero(nDoF);
        desiredJointPosition.setZero(nDoF);
        nullspaceTorque.setZero(nDoF);
        nullspaceVelocity.setZero(nDoF);
 
        /// force torque
        currentForceTorque.setZero();
        safeFTGuardOffset.setZero();
 
        /// task space variables (command in operation space)
        forceImpedance.setZero();
        cartesianVelTarget.setZero();
 
        /// current status
        qvelFiltered.setZero(nDoF);
        currentPose.setZero();
        currentTwist.setZero();
        desiredPose.setZero();
        desiredPoseUnsafe.setZero();
 
        /// intermediate results
        poseDiffMatImp.setZero();
        poseErrorImp.setZero();
        oriDiffAngleAxis = Eigen::AngleAxisf::Identity();
 
        /// intermediate results for admittance controller
        poseDiffMatAdm.setZero();
        poseErrorAdm.setZero();
        virtualPose.setZero();
        virtualVel.setZero();
        virtualAcc.setZero();
        pos.setZero();
        vel.setZero();
        acc.setZero();
 
        /// inertia
        inertia.setZero(nDoF, nDoF);
        inertiaInv.setZero(nDoF, nDoF);
        inertiaVelCtrlJoints.setZero(nDoFVelocity, nDoFVelocity);
        inertiaInvVelCtrlJoints.setZero(nDoFVelocity, nDoFVelocity);
        IVelCtrlJoint = Eigen::MatrixXf::Identity(nDoFVelocity, nDoFVelocity);
        qdPos.setZero(nDoFVelocity);
        qdVel.setZero(nDoFVelocity);
        qdAcc.setZero(nDoFVelocity);
        qTemp.setZero(nDoFVelocity);
 
        /// others
        jacobi.setZero(6, nDoF);
        jtpinv.setZero(6, nDoF);
        jpinv.setZero(nDoF, 6);
        I = Eigen::MatrixXf::Identity(nDoF, nDoF);
 
        rtSafe = false; // TODO check default value
        rtTargetSafe = true;
        forceTorqueSafe = true;
    }
 
    void
    TSCtrlRtStatus::rtPreActivate(const Eigen::Matrix4f& currentTCPPose)
    {
        /// !!!! TO BE UPDATED IN PRE-ACTIVATE !!!!
        /// only then the up-to-date robot joint information is available
        currentPose = currentTCPPose;
        desiredPose = currentTCPPose;
        accumulateTime = 0.0;
    }
 
    Eigen::MatrixXf
    computeJointLimitWeightMatrix(const Eigen::Ref<const Eigen::VectorXf>& jointAngles,
                                  const Eigen::Ref<const Eigen::VectorXf>& jointLimitsLow,
                                  const Eigen::Ref<const Eigen::VectorXf>& jointLimitsHigh,
                                  Eigen::VectorXf& jointAngleLimitGradient)
    {
        // temporally copied from simox control
        // Based on "A weighted least-norm solution based scheme for avoiding joint limits for redundant joint manipulators", T.F.Chan (1995)
        int nbJoints = jointAngles.size();
        Eigen::VectorXf weights(nbJoints);
        Eigen::VectorXf gradient = Eigen::VectorXf::Zero(nbJoints);
 
        if (jointAngleLimitGradient.rows() == 0)
        {
            jointAngleLimitGradient = Eigen::VectorXf::Zero(nbJoints);
        }
 
        for (int i = 0; i < nbJoints; i++)
        {
            if ((jointLimitsLow(i) == 0 && jointLimitsHigh(i) == 0) || jointLimitsHigh(i) > 1e8)
            {
                // Joint is limitless
                weights(i) = 1.;
            }
            else
            {
                gradient(i) = std::pow(jointLimitsHigh(i) - jointLimitsLow(i), 2) *
                              (2 * jointAngles(i) - jointLimitsHigh(i) - jointLimitsLow(i)) /
                              (4 * std::pow(jointLimitsHigh(i) - jointAngles(i), 2) *
                               std::pow(jointAngles(i) - jointLimitsLow(i), 2));
 
                if ((std::abs(gradient(i)) - std::abs(jointAngleLimitGradient(i))) >= 0.)
                {
                    // Joint going towards limits
                    weights(i) = 1. / std::sqrt((1. + std::abs(gradient(i))));
                }
                else
                {
                    // Joint going towards the center
                    weights(i) = 1.;
                }
            }
        }
        jointAngleLimitGradient = gradient;
        return weights.asDiagonal();
    }
 
} // namespace armarx::control::common::control_law