CollisionAvoidanceMixedImpedanceVelocityController.cpp
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 
23 
25 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h> // FIXME avoid this
26 
28 
30 {
31  NJointControllerRegistration<NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController>
33  "NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController");
34 
37  const RobotUnitPtr& robotUnit,
38  const NJointControllerConfigPtr& config,
39  const VirtualRobot::RobotPtr& robot) :
42  {
43  ARMARX_IMPORTANT << getClassName() + " construction done";
44  }
45 
46  std::string
48  const Ice::Current&) const
49  {
50  return "NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController";
51  }
52 
53  void
56  const Ice::Current& iceCurrent)
57  {
59  {
60  for (auto& pair : limb)
61  {
62  pair.second->rtFirstRun.store(true);
63  }
64  }
65  }
66 
67  void
69  {
70  std::lock_guard<std::recursive_mutex> lock(mtx_mps);
71  // bool rtSafe = additionalTaskUpdateStatus();
72  auto [rtTargetSafe, ftSafe] = additionalTaskUpdateStatus();
73 
74  // when ft guard is enabled and rt is not safe due to ft trigger, then we stop the corresponding mp
75  std::map<std::string, bool> flagMPToStop;
76  for (auto& pair : limb)
77  {
78  flagMPToStop[pair.first] = not limb.at(pair.first)->controller.ftsensor.ftSafe.load();
79  if (flagMPToStop[pair.first])
80  {
81  ARMARX_INFO << " --- force triggered at " << pair.first;
82  }
83  pair.second->nonRTDeltaT =
84  pair.second->rtStatusInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
85  pair.second->nonRTAccumulateTime = pair.second->rtStatusInNonRT.accumulateTime;
86  }
87  if (hands)
88  {
89  for (auto& pair : hands->hands)
90  {
91  pair.second->nonRTDeltaT =
92  pair.second->rtsInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
93  pair.second->nonRTAccumulateTime = pair.second->rtsInNonRT.accumulateTime;
94  }
95  }
96 
97  std::map<std::string, bool> mpRunning;
98  for (auto& _mp : mps)
99  {
100  const auto mpNodeSet = _mp.second.mp->getNodeSetName();
101  mpRunning[_mp.second.mp->getMPName()] = _mp.second.mp->isRunning();
102 
103  auto search = flagMPToStop.find(mpNodeSet);
104  if (_mp.second.mp->isStopRequested())
105  {
106  _mp.second.mp->stop();
107  ARMARX_INFO << "user requested to stop mp execution at canonical value: "
108  << _mp.second.mp->getCanonicalValue();
109  }
110  if (search != flagMPToStop.end() and flagMPToStop.at(mpNodeSet) and
111  _mp.second.mp->isRunning() and
112  (limb.at(mpNodeSet)->controller.ftsensor.isForceGuardEnabled() or
113  limb.at(mpNodeSet)->controller.ftsensor.isTorqueGuardEnabled()))
114  {
115  _mp.second.mp->stop();
116  auto& force = limb.at(mpNodeSet)->rtStatusInNonRT.currentForceTorque;
117  ARMARX_INFO << "Due to force/torque safe guard, MP " << _mp.second.mp->getMPName()
118  << " is stopped at " << _mp.second.mp->getCanonicalValue()
119  << " with current ft: " << force;
120  }
121 
122  if (_mp.second.mp->isFinished())
123  {
124  continue;
125  }
126  if (_mp.second.mp->getRole() == "taskspace")
127  {
128  auto& arm = limb.at(_mp.second.mp->getNodeSetName());
129  if (_mp.second.mp->isFirstRun())
130  {
131  ARMARX_INFO << "checking TSMP initial status ...";
132  /// Here we check if the TSMP start pose is too far away from the current pose, if so, we would
133  /// prefer to reset the mp start from the current pose.
134  const std::vector<double> mpStart = _mp.second.mp->getStartVec();
135  const auto mpStartPose = common::dVecToMat4(mpStart);
136  if (_mp.second.mp->getStartFromPrevTarget() or
138  arm->rtStatusInNonRT.currentPose,
139  mpStartPose,
140  "current pose",
141  "TSMP initial start pose",
142  arm->nonRtConfig.safeDistanceMMToGoal,
143  arm->nonRtConfig.safeRotAngleDegreeToGoal,
144  _mp.second.mp->getMPName() + "mp_set_target"))
145  {
146  if (_mp.second.mp->getStartFromPrevTarget())
147  {
148  ARMARX_INFO << "User requested to start the current TS MP from the "
149  "previous target pose";
150  }
151  else
152  {
154  << "deviation from current pose too large, reset MP start pose";
155  }
156  _mp.second.mp->validateInitialState(
157  common::mat4ToDVec(arm->rtStatusInNonRT.desiredPose));
158  }
159  ARMARX_INFO << "done";
160  }
161  mp::TSMPInputPtr in = std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input);
162  in->pose = arm->rtStatusInNonRT.currentPose;
163  in->vel = arm->rtStatusInNonRT.currentTwist;
164  // in->deltaT = arm->rtStatusInNonRT.deltaT;
165  in->deltaT = arm->nonRTDeltaT;
166  }
167  else if (_mp.second.mp->getRole() == "nullspace")
168  {
169  auto& arm = limb.at(_mp.second.mp->getNodeSetName());
170  mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input);
171  ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.numJoints,
172  arm->rtStatusInNonRT.jointPos.size());
173  in->angleRadian = arm->rtStatusInNonRT.jointPos;
174  in->angularVel = arm->rtStatusInNonRT.qvelFiltered;
175  in->deltaT = arm->nonRTDeltaT;
176  }
177  else if (_mp.second.mp->getRole() == "hand")
178  {
179  auto& hand = hands->hands.at(_mp.second.mp->getNodeSetName());
180  mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input);
181  in->angleRadian = hand->rtsInNonRT.jointPosition.value();
182  in->deltaT = hand->nonRTDeltaT;
183  }
184  }
185  if (not rtTargetSafe)
186  {
188 
189  // return;
190  // Make sure that you do not call additionalTaskSetTarget before return;
191  // since we overwrite the arm->nonRtConfig in additionalTaskUpdateStatus() with user config, which does not
192  // align with the up-to-date mp status. userConfig is only updated to align with MP status everytime a MP
193  // finishes execution. Only then, user can use this controller as if no mp is there.
194  }
195  runMPs(rtTargetSafe);
196 
197  /// set mp target to nonRT config data structure
198  for (auto& _mp : mps)
199  {
200  if (not mpRunning.at(_mp.second.mp->getMPName()))
201  {
202  continue;
203  }
204  for (auto& ftGuard : mpConfig.ftGuard)
205  {
206  if (ftGuard.mpName == _mp.second.mp->getMPName())
207  {
208  bool const forceGuard =
209  (ftGuard.force.has_value() and
210  ftGuard.force.value() >= _mp.second.mp->getCanonicalValue());
211  bool const torqueGuard =
212  (ftGuard.torque.has_value() and
213  ftGuard.torque.value() >= _mp.second.mp->getCanonicalValue());
214 
215  auto& arm = limb.at(_mp.second.mp->getNodeSetName());
216  bool const resetForce =
217  not arm->controller.ftsensor.isForceGuardEnabled() and forceGuard;
218  bool const resetTorque =
219  not arm->controller.ftsensor.isTorqueGuardEnabled() and torqueGuard;
220  if (resetForce or resetTorque)
221  {
222  ARMARX_INFO << "Triggering force torque safety guard for "
223  << arm->kinematicChainName << " at can value "
224  << _mp.second.mp->getCanonicalValue();
225  }
226 
227  arm->nonRtConfig.ftConfig.enableSafeGuardForce = forceGuard;
228  arm->nonRtConfig.ftConfig.enableSafeGuardTorque = torqueGuard;
229  arm->controller.ftsensor.enableSafeGuard(resetForce, resetTorque);
230  }
231  }
232 
233  if (_mp.second.mp->getRole() == "taskspace")
234  {
235  auto& arm = limb.at(_mp.second.mp->getNodeSetName());
236  mp::TSMPOutputPtr out =
237  std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output);
238 
239  /// Note that here we only report warning but still set target to RT, we rely on MP phase stop and
240  /// RT safety mechanism to stop or pause the action if needed
242  arm->rtStatusInNonRT.currentPose,
243  out->pose,
244  "current pose",
245  "TSMP pose",
246  arm->nonRtConfig.safeDistanceMMToGoal,
247  arm->nonRtConfig.safeRotAngleDegreeToGoal,
248  _mp.second.mp->getMPName() + "mp_set_target");
249  arm->nonRtConfig.desiredPose = out->pose;
250  arm->nonRtConfig.desiredTwist = out->vel;
251  }
252  else if (_mp.second.mp->getRole() == "nullspace")
253  {
254  auto& arm = limb.at(_mp.second.mp->getNodeSetName());
255  mp::JSMPOutputPtr out =
256  std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output);
257  ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.numJoints, out->angleRadian.size());
258  arm->nonRtConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
259  }
260  else if (_mp.second.mp->getRole() == "hand")
261  {
262  mp::JSMPOutputPtr out =
263  std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output);
264  auto& hand = hands->hands.at(_mp.second.mp->getNodeSetName());
265  mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input);
266  ARMARX_CHECK_EQUAL(hand->targetNonRT.jointPosition.value().size(),
267  out->angleRadian.size());
268  hand->targetNonRT.jointPosition.value() = out->angleRadian;
269  }
270  if (mpRunning.at(_mp.second.mp->getMPName()) and _mp.second.mp->isFinished())
271  {
272  ARMARX_INFO << "reset buffer for nonRtConfig of " << _mp.second.mp->getMPName();
273  for (auto& pair : limb)
274  {
275  auto& arm = pair.second;
276  arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->nonRtConfig);
277  arm->controller.ftsensor.enableSafeGuard(true, true);
278  }
279  for (auto& pair : limb)
280  {
281  userConfig.limbs.at(pair.first) = pair.second->nonRtConfig;
282  }
283  if (hands)
284  {
285  hands->reinitBuffer(userConfig.hands);
286  }
287  }
288  }
290  }
291 } // namespace armarx::control::njoint_mp_controller::task_space
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::userConfig
ConfigDict userConfig
Definition: MixedImpedanceVelocityController.h:182
armarx::control::njoint_mp_controller::task_space::NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController::updateMPConfig
void updateMPConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: CollisionAvoidanceMixedImpedanceVelocityController.cpp:54
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limb
std::map< std::string, ArmPtr > limb
Definition: MixedImpedanceVelocityController.h:179
armarx::control::common::mp::JSMPInputPtr
std::shared_ptr< JSMPInput > JSMPInputPtr
Definition: JSMP.h:47
RobotUnit.h
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::handleRTNotSafeInNonRT
void handleRTNotSafeInNonRT()
Definition: MixedImpedanceVelocityController.cpp:260
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::controllableNodeSets
std::map< std::string, VirtualRobot::RobotNodeSetPtr > controllableNodeSets
Definition: MixedImpedanceVelocityController.h:184
armarx::control::common::detectAndReportPoseDeviationWarning
bool detectAndReportPoseDeviationWarning(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2, float positionThrMM, float angleThrDeg, const std::string &who)
Definition: utils.cpp:646
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController
Brief description of class NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController.
Definition: CollisionAvoidanceMixedImpedanceVelocityController.h:55
armarx::control::common::mp::MPPool::runMPs
void runMPs(const bool rtSafe)
Definition: MPPool.cpp:87
armarx::control::common::mp::JSMPOutputPtr
std::shared_ptr< JSMPOutput > JSMPOutputPtr
Definition: JSMP.h:48
armarx::control::njoint_mp_controller::task_space::NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController::NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController
NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: CollisionAvoidanceMixedImpedanceVelocityController.cpp:36
armarx::control::common::mp::MPPool::resetMPs
bool resetMPs(const ::armarx::aron::data::dto::DictPtr &dto, const std::map< std::string, VirtualRobot::RobotNodeSetPtr > &rnsMap)
Definition: MPPool.cpp:528
armarx::control::common::mp::TSMPOutputPtr
std::shared_ptr< TSMPOutput > TSMPOutputPtr
Definition: TSMP.h:47
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController
Brief description of class NJointTaskspaceMixedImpedanceVelocityController.
Definition: MixedImpedanceVelocityController.h:50
armarx::control::common::mp::MPPool::mtx_mps
std::recursive_mutex mtx_mps
Definition: MPPool.h:130
CollisionAvoidanceMixedImpedanceVelocityController.h
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::additionalTaskUpdateStatus
std::tuple< bool, bool > additionalTaskUpdateStatus()
Definition: MixedImpedanceVelocityController.cpp:224
armarx::control::common::dVecToMat4
Eigen::Matrix4f dVecToMat4(const mplib::core::DVec &dvec)
create Eigen:Matrix4f from 7D double vector.
Definition: utils.cpp:230
utils.h
armarx::control::njoint_mp_controller::task_space
This file is part of ArmarX.
Definition: AdmittanceController.cpp:29
armarx::control::njoint_mp_controller::task_space::NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController::getClassName
std::string getClassName(const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionAvoidanceMixedImpedanceVelocityController.cpp:47
armarx::control::njoint_mp_controller::task_space::NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController::additionalTask
void additionalTask() override
Definition: CollisionAvoidanceMixedImpedanceVelocityController.cpp:68
armarx::control::common::MPType::hand
@ hand
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
IceUtil::Handle< class RobotUnit >
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::hands
core::HandControlPtr hands
Definition: MixedImpedanceVelocityController.h:183
armarx::control::common::mat4ToDVec
mplib::core::DVec mat4ToDVec(const Eigen::Matrix4f &mat)
convert Eigen:Matrix4f to 7D double vector.
Definition: utils.cpp:252
armarx::control::common::mp::MPPool::mps
std::map< std::string, MPInputOutput > mps
Definition: MPPool.h:129
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::additionalTaskSetTarget
void additionalTaskSetTarget()
Definition: MixedImpedanceVelocityController.cpp:247
armarx::control::common::mp::TSMPInputPtr
std::shared_ptr< TSMPInput > TSMPInputPtr
Definition: TSMP.h:46
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::robotUnit
RobotUnitPtr robotUnit
Definition: MixedImpedanceVelocityController.h:181
armarx::control::njoint_mp_controller::task_space::registrationControllerNJointTSCollisionAvoidanceMixedImpedanceVelocityMPController
NJointControllerRegistration< NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController > registrationControllerNJointTSCollisionAvoidanceMixedImpedanceVelocityMPController("NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController")
NJointControllerRegistry.h
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::control::common::mp::MPPool::mpConfig
MPListConfig mpConfig
this variable is only needed when constructing the MP instances, therefore you don't need to use trip...
Definition: MPPool.h:136