TaskspaceMixedImpedanceVelocityController.h
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
17  * @date 2024
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #pragma once
23 
24 #include <VirtualRobot/IK/DifferentialIK.h>
25 #include <VirtualRobot/Robot.h>
26 
28 
31 
32 #include <armarx/control/common/control_law/aron/TaskspaceMixedImpedanceVelocityControllerConfig.aron.generated.h>
35 #include <armarx/control/common/ft/aron/FTConfig.aron.generated.h>
36 
38 {
39 
41  {
42  public:
43  using FTConfig = common::ft::arondto::FTConfig;
44  using Config =
45  common::control_law::arondto::TaskspaceMixedImpedanceVelocityControllerConfig;
46  using ConfigDict =
47  common::control_law::arondto::TaskspaceMixedImpedanceVelocityControllerConfigDict;
48 
49  /// internal status of the controller, containing intermediate variables, mutable targets
50  struct RtStatus : public RobotStatus
51  {
52  /// targets
53  Eigen::VectorXf desiredJointTorques;
54  Eigen::VectorXf desiredJointVelocity;
55  Eigen::VectorXf nullspaceTorque;
56  Eigen::VectorXf nullspaceVelocity;
57 
58  /// force torque
60 
61  /// task space variables
64 
65  /// current status
66  Eigen::VectorXf qpos;
67  Eigen::VectorXf qvel;
68  Eigen::VectorXf qvelFiltered;
72 
73  /// intermediate results
76  Eigen::AngleAxisf oriDiffAngleAxis;
77 
78  /// others
79  Eigen::MatrixXf jacobi;
80  Eigen::MatrixXf jtpinv;
81  Eigen::MatrixXf jpinv;
82  bool rtSafe;
83  bool rtTargetSafe = true;
84 
85  size_t nDoFTorque;
86  size_t nDoFVelocity;
87 
88  void
89  reset(const unsigned int nDoF, const size_t numDoFTorque, const size_t numDoFVelocity);
91  };
92 
93  unsigned int numOfJoints; // for nonRT use case
94  size_t nDoFTorque;
95  size_t nDoFVelocity;
96 
97  private:
98  std::atomic_bool enablePreactivateInit{false};
99 
100  Eigen::MatrixXf I;
101 
102  VirtualRobot::DifferentialIKPtr ik;
103 
104  const float lambda = 2.0f;
105 
106  public:
107  VirtualRobot::RobotNodePtr tcp;
108  VirtualRobot::RobotNodePtr rtTCP;
110 
111  std::vector<size_t> jointIDTorqueMode;
112  std::vector<size_t> jointIDVelocityMode;
113 
114  bool isControlModeValid(const std::vector<std::string>& nameList,
115  const std::map<std::string, std::string>& jointControlModeMap);
116  // void updateFT(const FTConfig& c, RtStatus& rtStatus);
117  void initialize(const VirtualRobot::RobotNodeSetPtr& rns,
118  const VirtualRobot::RobotNodeSetPtr& rtRns,
119  const std::vector<size_t>& torqueControlledIndex,
120  const std::vector<size_t>& velocityControlledIndex);
121  void preactivateInit(const VirtualRobot::RobotNodeSetPtr& rns);
122 
123  void run(Config& c, RtStatus& robotStatus);
124  void firstRun();
125  };
126 } // namespace armarx::control::common::control_law
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::desiredPose
Eigen::Matrix4f desiredPose
Definition: TaskspaceMixedImpedanceVelocityController.h:71
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::rtSafe
bool rtSafe
Definition: TaskspaceMixedImpedanceVelocityController.h:82
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::jointIDTorqueMode
std::vector< size_t > jointIDTorqueMode
Definition: TaskspaceMixedImpedanceVelocityController.h:111
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::poseErrorImp
Eigen::Vector6f poseErrorImp
Definition: TaskspaceMixedImpedanceVelocityController.h:75
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::forceImpedance
Eigen::Vector6f forceImpedance
task space variables
Definition: TaskspaceMixedImpedanceVelocityController.h:62
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::jointIDVelocityMode
std::vector< size_t > jointIDVelocityMode
Definition: TaskspaceMixedImpedanceVelocityController.h:112
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::ftsensor
common::ft::FTSensor ftsensor
Definition: TaskspaceMixedImpedanceVelocityController.h:109
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::oriDiffAngleAxis
Eigen::AngleAxisf oriDiffAngleAxis
Definition: TaskspaceMixedImpedanceVelocityController.h:76
common.h
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::preactivateInit
void preactivateInit(const VirtualRobot::RobotNodeSetPtr &rns)
Definition: TaskspaceMixedImpedanceVelocityController.cpp:41
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::ConfigDict
common::control_law::arondto::TaskspaceMixedImpedanceVelocityControllerConfigDict ConfigDict
Definition: TaskspaceMixedImpedanceVelocityController.h:47
FTSensor.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::currentTwist
Eigen::Vector6f currentTwist
Definition: TaskspaceMixedImpedanceVelocityController.h:70
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::tcp
VirtualRobot::RobotNodePtr tcp
Definition: TaskspaceMixedImpedanceVelocityController.h:107
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::nDoFTorque
size_t nDoFTorque
Definition: TaskspaceMixedImpedanceVelocityController.h:94
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::nullspaceVelocity
Eigen::VectorXf nullspaceVelocity
Definition: TaskspaceMixedImpedanceVelocityController.h:56
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::poseDiffMatImp
Eigen::Matrix3f poseDiffMatImp
intermediate results
Definition: TaskspaceMixedImpedanceVelocityController.h:74
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::nDoFVelocity
size_t nDoFVelocity
Definition: TaskspaceMixedImpedanceVelocityController.h:86
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::currentPose
Eigen::Matrix4f currentPose
Definition: TaskspaceMixedImpedanceVelocityController.h:69
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController
Definition: TaskspaceMixedImpedanceVelocityController.h:40
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::firstRun
void firstRun()
Definition: TaskspaceMixedImpedanceVelocityController.cpp:47
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::rtTargetSafe
bool rtTargetSafe
Definition: TaskspaceMixedImpedanceVelocityController.h:83
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::isControlModeValid
bool isControlModeValid(const std::vector< std::string > &nameList, const std::map< std::string, std::string > &jointControlModeMap)
Definition: TaskspaceMixedImpedanceVelocityController.cpp:53
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::rtPreActivate
void rtPreActivate(const Eigen::Matrix4f &currentPose)
Definition: TaskspaceMixedImpedanceVelocityController.cpp:288
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::Config
common::control_law::arondto::TaskspaceMixedImpedanceVelocityControllerConfig Config
Definition: TaskspaceMixedImpedanceVelocityController.h:45
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::desiredJointVelocity
Eigen::VectorXf desiredJointVelocity
Definition: TaskspaceMixedImpedanceVelocityController.h:54
armarx::control::common::control_law::RobotStatus
Definition: common.h:29
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus
internal status of the controller, containing intermediate variables, mutable targets
Definition: TaskspaceMixedImpedanceVelocityController.h:50
armarx::control::common::control_law
This file is part of ArmarX.
Definition: aron_conversions.cpp:68
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::jpinv
Eigen::MatrixXf jpinv
Definition: TaskspaceMixedImpedanceVelocityController.h:81
ControlTarget1DoFActuator.h
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::reset
void reset(const unsigned int nDoF, const size_t numDoFTorque, const size_t numDoFVelocity)
Definition: TaskspaceMixedImpedanceVelocityController.cpp:237
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::desiredJointTorques
Eigen::VectorXf desiredJointTorques
targets
Definition: TaskspaceMixedImpedanceVelocityController.h:53
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::nDoFVelocity
size_t nDoFVelocity
Definition: TaskspaceMixedImpedanceVelocityController.h:95
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::run
void run(Config &c, RtStatus &robotStatus)
Definition: TaskspaceMixedImpedanceVelocityController.cpp:105
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::initialize
void initialize(const VirtualRobot::RobotNodeSetPtr &rns, const VirtualRobot::RobotNodeSetPtr &rtRns, const std::vector< size_t > &torqueControlledIndex, const std::vector< size_t > &velocityControlledIndex)
Definition: TaskspaceMixedImpedanceVelocityController.cpp:16
TripleBuffer.h
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::jtpinv
Eigen::MatrixXf jtpinv
Definition: TaskspaceMixedImpedanceVelocityController.h:80
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::rtTCP
VirtualRobot::RobotNodePtr rtTCP
Definition: TaskspaceMixedImpedanceVelocityController.h:108
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::FTConfig
common::ft::arondto::FTConfig FTConfig
Definition: TaskspaceMixedImpedanceVelocityController.h:43
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::qvel
Eigen::VectorXf qvel
Definition: TaskspaceMixedImpedanceVelocityController.h:67
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::nullspaceTorque
Eigen::VectorXf nullspaceTorque
Definition: TaskspaceMixedImpedanceVelocityController.h:55
Eigen::Matrix< float, 6, 1 >
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::jacobi
Eigen::MatrixXf jacobi
others
Definition: TaskspaceMixedImpedanceVelocityController.h:79
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::qvelFiltered
Eigen::VectorXf qvelFiltered
Definition: TaskspaceMixedImpedanceVelocityController.h:68
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::cartesianVelTarget
Eigen::Vector6f cartesianVelTarget
Definition: TaskspaceMixedImpedanceVelocityController.h:63
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::qpos
Eigen::VectorXf qpos
current status
Definition: TaskspaceMixedImpedanceVelocityController.h:66
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::nDoFTorque
size_t nDoFTorque
Definition: TaskspaceMixedImpedanceVelocityController.h:85
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::numOfJoints
unsigned int numOfJoints
Definition: TaskspaceMixedImpedanceVelocityController.h:93
armarx::control::common::ft::FTSensor
Definition: FTSensor.h:45
SensorValue1DoFActuator.h
armarx::control::common::control_law::TaskspaceMixedImpedanceVelocityController::RtStatus::currentForceTorque
Eigen::Vector6f currentForceTorque
force torque
Definition: TaskspaceMixedImpedanceVelocityController.h:59