common.h

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     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <cstddef>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/interface/serialization/Eigen.h>
 
namespace armarx::control::common::control_law
{
 
    struct RobotStatus
    {
        Eigen::VectorXf jointPosition;
        Eigen::VectorXf jointVelocity;
        Eigen::VectorXf jointTorque;
 
        double deltaT = 0;
        double accumulateTime = 0.0;
        size_t nDoF = 0;
        size_t nDoFTorque = 0;
        size_t nDoFVelocity = 0;
 
        std::vector<size_t> jointIDTorqueMode;
        std::vector<size_t> jointIDVelocityMode;
 
        Eigen::Matrix4f globalPose;
 
        virtual ~RobotStatus() = default;
        virtual void reset(size_t nJoints,
                           const std::vector<size_t>& jointIDTorqueCtrlMode,
                           const std::vector<size_t>& jointIDVelocityCtrlMode);
    };
 
    struct TSCtrlRtStatus : public RobotStatus
    {
 
 
        /// targets
        Eigen::VectorXf desiredJointTorque;
        Eigen::VectorXf desiredJointVelocity;
        Eigen::VectorXf desiredJointPosition;
        Eigen::VectorXf nullspaceTorque;
        Eigen::VectorXf nullspaceVelocity;
 
        /// force torque
        Eigen::Vector6f currentForceTorque;
        Eigen::Vector6f safeFTGuardOffset;
 
        /// task space variables (command in operation space)
        Eigen::Vector6f forceImpedance; /// for impedance control
        Eigen::Vector6f cartesianVelTarget; /// for velocity control
 
        /// current status
        Eigen::VectorXf qvelFiltered;
        Eigen::Matrix4f currentPose;
        Eigen::Vector6f currentTwist;
        Eigen::Matrix4f desiredPose;
        Eigen::Matrix4f desiredPoseUnsafe;
 
        /// intermediate results
        Eigen::Matrix3f poseDiffMatImp;
        Eigen::Vector6f poseErrorImp;
        Eigen::AngleAxisf oriDiffAngleAxis;
 
        /// intermediate results for admittance controller
        Eigen::Matrix3f poseDiffMatAdm;
        Eigen::Vector6f poseErrorAdm;
 
        Eigen::Matrix4f virtualPose;
        Eigen::Vector6f virtualVel;
        Eigen::Vector6f virtualAcc;
        Eigen::Vector6f pos;
        Eigen::Vector6f vel;
        Eigen::Vector6f acc;
 
        /// inertia
        /// Note, the inertia variables are only used for collision avoidance controllers, as the
        /// inertia is obtained from the simox robot that exists only in collision avoidance controllers
        /// TODO, use simox to obtain inertia
        Eigen::MatrixXd inertia;
        Eigen::MatrixXf inertiaInv;
        Eigen::MatrixXd inertiaVelCtrlJoints;
        Eigen::MatrixXf inertiaInvVelCtrlJoints;
        Eigen::MatrixXf IVelCtrlJoint;
        Eigen::VectorXf qdPos;
        Eigen::VectorXf qdVel;
        Eigen::VectorXf qdAcc;
        Eigen::VectorXf qTemp;
 
        /// others
        /// jacobi matrix [6, nDoF]
        Eigen::MatrixXf jacobi;
        /// pseudo inverse of the jacobi transpose [6, nDoF], for torque control
        Eigen::MatrixXf jtpinv;
 
        /// pseudo inverse of the jacobi [nDoF, 6], for vel control
        Eigen::MatrixXf jpinv;
 
        /// for computing nullspace projection
        Eigen::MatrixXf I;
 
        /// damping term for pseudo inverse of jacobian
        float lambda = 2.0f;
 
        bool rtSafe = false;
        bool rtTargetSafe = false;
        bool forceTorqueSafe = false;
 
        void reset(size_t numDoF,
                   const std::vector<size_t>& jointIDTorqueMode,
                   const std::vector<size_t>& jointIDVelocityMode) override;
        void rtPreActivate(const Eigen::Matrix4f& currentTCPPose);
    };
 
    struct VirtualRobotStatus /// TODO remove, not used anywhere
    {
        Eigen::Matrix4f globalPose;
        std::vector<float> jointValuesFullRobot;
    };
 
    struct ControlTarget /// TODO remove, only used by keypoint controllers
    {
        std::vector<float> targets;
    };
 
    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);
 
 
} // namespace armarx::control::common::control_law