aron_conversions.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 Fabian Reister ( fabian dot reister at kit dot edu )
17  * @date 2022
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #include "aron_conversions.h"
23 
24 #include <map>
25 
26 #include <RobotAPI/libraries/aron/common/aron_conversions/eigen.h>
27 #include <RobotAPI/libraries/aron/core/aron_conversions.h>
28 
29 namespace armarx::control::common::ft
30 {
31  // void
32  // fromAron(const arondto::FTConfig& dto, FTSensor::FTConfig& bo)
33  // {
34  // bo = FTSensor::FTConfig{.sensorName = dto.sensorName,
35  // .ftFilter = dto.ftFilter,
36  // .deadZoneForce = dto.deadZoneForce,
37  // .deadZoneTorque = dto.deadZoneTorque,
38  // .timeLimit = dto.timeLimit,
39  // .enableTCPGravityCompensation = dto.enableTCPGravityCompensation,
40  // .reCalibrate = dto.reCalibrate,
41  // .enableSafeGuardForce = dto.enableSafeGuardForce,
42  // .enableSafeGuardTorque = dto.enableSafeGuardTorque,
43  // .safeGuardForceThreshold = dto.safeGuardForceThreshold,
44  // .safeGuardTorqueThreshold = dto.safeGuardTorqueThreshold,
45  // .forceBaseline = dto.forceBaseline,
46  // .torqueBaseline = dto.torqueBaseline};
47  // }
48 
49  // void
50  // toAron(arondto::FTConfig& dto, const FTSensor::FTConfig& bo)
51  // {
52  // dto.sensorName = bo.sensorName;
53  // dto.ftFilter = bo.ftFilter;
54  // dto.deadZoneForce = bo.deadZoneForce;
55  // dto.deadZoneTorque = bo.deadZoneTorque;
56  // dto.timeLimit = bo.timeLimit;
57  // dto.enableTCPGravityCompensation = bo.enableTCPGravityCompensation;
58  // dto.reCalibrate = bo.reCalibrate;
59  // dto.enableSafeGuardForce = bo.enableSafeGuardForce;
60  // dto.enableSafeGuardTorque = bo.enableSafeGuardTorque;
61  // dto.safeGuardForceThreshold = bo.safeGuardForceThreshold;
62  // dto.safeGuardTorqueThreshold = bo.safeGuardTorqueThreshold;
63  // dto.forceBaseline = bo.forceBaseline;
64  // dto.torqueBaseline = bo.torqueBaseline;
65  // }
66 } // namespace armarx::control::common::ft
67 
68 namespace armarx::control::common::control_law
69 {
70  // // Impedance controller
71  // void
72  // fromAron(const arondto::TaskspaceImpedanceControllerConfig& dto,
73  // TaskspaceImpedanceController::Config& bo)
74  // {
75  // auto ftBO = ::armarx::fromAron<ft::FTSensor::FTConfig, ft::arondto::FTConfig>(dto.ftConfig);
76  // bo = TaskspaceImpedanceController::Config{.kpImpedance = dto.kpImpedance,
77  // .kdImpedance = dto.kdImpedance,
78  // .kpNullspace = dto.kpNullspace,
79  // .kdNullspace = dto.kdNullspace,
80  // .desiredPose = dto.desiredPose,
81  // .desiredTwist = dto.desiredTwist,
82  // .desiredNullspaceJointAngles =
83  // dto.desiredNullspaceJointAngles,
84  // .torqueLimit = dto.torqueLimit,
85  // .qvelFilter = dto.qvelFilter,
86  // .ftConfig = ftBO};
87  // }
88 
89  // void
90  // fromAron(const arondto::TaskspaceImpedanceControllerConfigDict& dto,
91  // TaskspaceImpedanceController::ConfigDict& bo)
92  // {
93  // for (const auto& pair : dto.cfg)
94  // {
95  // bo.cfg.emplace(
96  // pair.first,
97  // ::armarx::fromAron<TaskspaceImpedanceController::Config,
98  // arondto::TaskspaceImpedanceControllerConfig>(pair.second));
99  // }
100  // }
101 
102  // void
103  // toAron(arondto::TaskspaceImpedanceControllerConfigDict& dto,
104  // const TaskspaceImpedanceController::ConfigDict& bo)
105  // {
106  // for (const auto& pair : bo.cfg)
107  // {
108  // dto.cfg.emplace(pair.first,
109  // ::armarx::toAron<arondto::TaskspaceImpedanceControllerConfig,
110  // TaskspaceImpedanceController::Config>(pair.second));
111  // }
112  // }
113 
114  // void
115  // toAron(arondto::TaskspaceImpedanceControllerConfig& dto,
116  // const TaskspaceImpedanceController::Config& bo)
117  // {
118  // dto.kpImpedance = bo.kpImpedance;
119  // dto.kdImpedance = bo.kdImpedance;
120  // dto.kpNullspace = bo.kpNullspace;
121  // dto.kdNullspace = bo.kdNullspace;
122  // dto.desiredPose = bo.desiredPose;
123  // dto.desiredTwist = bo.desiredTwist;
124  // dto.desiredNullspaceJointAngles = bo.desiredNullspaceJointAngles;
125  // dto.torqueLimit = bo.torqueLimit;
126  // dto.qvelFilter = bo.qvelFilter;
127  // dto.ftConfig = ::armarx::toAron<ft::arondto::FTConfig, ft::FTSensor::FTConfig>(bo.ftConfig);
128  // }
129 
130  // // Mixed Impedance-velocity controller
131  // void
132  // fromAron(const arondto::TaskspaceMixedImpedanceVelocityControllerConfig& dto,
133  // TaskspaceMixedImpedanceVelocityController::Config& bo)
134  // {
135  // auto ftBO = ::armarx::fromAron<ft::FTSensor::FTConfig, ft::arondto::FTConfig>(dto.ftConfig);
136  // bo = TaskspaceMixedImpedanceVelocityController::Config{
137  // .jointNameListTorque = dto.jointNameListTorque,
138  // .jointNameListVelocity = dto.jointNameListVelocity,
139  // .kpImpedance = dto.kpImpedance,
140  // .kdImpedance = dto.kdImpedance,
141  // .kpNullspaceTorque = dto.kpNullspaceTorque,
142  // .kdNullspaceTorque = dto.kdNullspaceTorque,
143  // .kpCartesianVel = dto.kpCartesianVel,
144  // .kdCartesianVel = dto.kdCartesianVel,
145  // .kpNullspaceVel = dto.kpNullspaceVel,
146  // .kdNullspaceVel = dto.kdNullspaceVel,
147  // .desiredPose = dto.desiredPose,
148  // .desiredTwist = dto.desiredTwist,
149  // .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles,
150  // .torqueLimit = dto.torqueLimit,
151  // .velocityLimit = dto.velocityLimit,
152  // .qvelFilter = dto.qvelFilter,
153  // .safeDistanceToGoal = dto.safeDistanceToGoal,
154  // .ftConfig = ftBO};
155  // }
156  //
157  // void
158  // fromAron(const arondto::TaskspaceMixedImpedanceVelocityControllerConfigDict& dto,
159  // TaskspaceMixedImpedanceVelocityController::ConfigDict& bo)
160  // {
161  // for (const auto& pair : dto.cfg)
162  // {
163  // bo.cfg.emplace(
164  // pair.first,
165  // ::armarx::fromAron<TaskspaceMixedImpedanceVelocityController::Config,
166  // arondto::TaskspaceMixedImpedanceVelocityControllerConfig>(
167  // pair.second));
168  // }
169  // }
170  //
171  // void
172  // toAron(arondto::TaskspaceMixedImpedanceVelocityControllerConfigDict& dto,
173  // const TaskspaceMixedImpedanceVelocityController::ConfigDict& bo)
174  // {
175  // for (const auto& pair : bo.cfg)
176  // {
177  // dto.cfg.emplace(
178  // pair.first,
179  // ::armarx::toAron<arondto::TaskspaceMixedImpedanceVelocityControllerConfig,
180  // TaskspaceMixedImpedanceVelocityController::Config>(pair.second));
181  // }
182  // }
183  //
184  // void
185  // toAron(arondto::TaskspaceMixedImpedanceVelocityControllerConfig& dto,
186  // const TaskspaceMixedImpedanceVelocityController::Config& bo)
187  // {
188  // dto.jointNameListTorque = bo.jointNameListTorque;
189  // dto.jointNameListVelocity = bo.jointNameListVelocity;
190  // dto.kpImpedance = bo.kpImpedance;
191  // dto.kdImpedance = bo.kdImpedance;
192  // dto.kpNullspaceTorque = bo.kpNullspaceTorque;
193  // dto.kdNullspaceTorque = bo.kdNullspaceTorque;
194  // dto.kpCartesianVel = bo.kpCartesianVel;
195  // dto.kdCartesianVel = bo.kdCartesianVel;
196  // dto.kpNullspaceVel = bo.kpNullspaceVel;
197  // dto.kdNullspaceVel = bo.kdNullspaceVel;
198  // dto.desiredPose = bo.desiredPose;
199  // dto.desiredTwist = bo.desiredTwist;
200  // dto.desiredNullspaceJointAngles = bo.desiredNullspaceJointAngles;
201  // dto.torqueLimit = bo.torqueLimit;
202  // dto.velocityLimit = bo.velocityLimit;
203  // dto.qvelFilter = bo.qvelFilter;
204  // dto.safeDistanceToGoal = bo.safeDistanceToGoal;
205  // dto.ftConfig = ::armarx::toAron<ft::arondto::FTConfig, ft::FTSensor::FTConfig>(bo.ftConfig);
206  // }
207  //
208  // // admittance controller
209  // void
210  // fromAron(const arondto::TaskspaceAdmittanceControllerConfig& dto,
211  // TaskspaceAdmittanceController::Config& bo)
212  // {
213  // auto ftBO = ::armarx::fromAron<ft::FTSensor::FTConfig, ft::arondto::FTConfig>(dto.ftConfig);
214  // bo = TaskspaceAdmittanceController::Config{.kpImpedance = dto.kpImpedance,
215  // .kdImpedance = dto.kdImpedance,
216  // .kpAdmittance = dto.kpAdmittance,
217  // .kdAdmittance = dto.kdAdmittance,
218  // .kmAdmittance = dto.kmAdmittance,
219  // .kpNullspace = dto.kpNullspace,
220  // .kdNullspace = dto.kdNullspace,
221  // .desiredPose = dto.desiredPose,
222  // .desiredTwist = dto.desiredTwist,
223  // .desiredNullspaceJointAngles =
224  // dto.desiredNullspaceJointAngles,
225  // .torqueLimit = dto.torqueLimit,
226  // .qvelFilter = dto.qvelFilter,
227  // .ftConfig = ftBO};
228  // }
229  //
230  // void
231  // toAron(arondto::TaskspaceAdmittanceControllerConfig& dto,
232  // const TaskspaceAdmittanceController::Config& bo)
233  // {
234  // dto.kpImpedance = bo.kpImpedance;
235  // dto.kdImpedance = bo.kdImpedance;
236  // dto.kpAdmittance = bo.kpAdmittance;
237  // dto.kdAdmittance = bo.kdAdmittance;
238  // dto.kmAdmittance = bo.kmAdmittance;
239  // dto.kpNullspace = bo.kpNullspace;
240  // dto.kdNullspace = bo.kdNullspace;
241  // dto.desiredPose = bo.desiredPose;
242  // dto.desiredTwist = bo.desiredTwist;
243  // dto.desiredNullspaceJointAngles = bo.desiredNullspaceJointAngles;
244  // dto.torqueLimit = bo.torqueLimit;
245  // dto.qvelFilter = bo.qvelFilter;
246  // dto.ftConfig = ::armarx::toAron<ft::arondto::FTConfig, ft::FTSensor::FTConfig>(bo.ftConfig);
247  // }
248  //
249  // void
250  // fromAron(const arondto::TaskspaceAdmittanceControllerConfigDict& dto,
251  // TaskspaceAdmittanceController::ConfigDict& bo)
252  // {
253  // for (const auto& pair : dto.cfg)
254  // {
255  // bo.cfg.emplace(
256  // pair.first,
257  // ::armarx::fromAron<TaskspaceAdmittanceController::Config,
258  // arondto::TaskspaceAdmittanceControllerConfig>(pair.second));
259  // }
260  // }
261  //
262  // void
263  // toAron(arondto::TaskspaceAdmittanceControllerConfigDict& dto,
264  // const TaskspaceAdmittanceController::ConfigDict& bo)
265  // {
266  // for (const auto& pair : bo.cfg)
267  // {
268  // dto.cfg.emplace(pair.first,
269  // ::armarx::toAron<arondto::TaskspaceAdmittanceControllerConfig,
270  // TaskspaceAdmittanceController::Config>(pair.second));
271  // }
272  // }
273  //
274  // void
275  // fromAron(const arondto::KeypointControllerConfig& dto, KeypointsImpedanceController::Config& bo)
276  // {
277  // bo = KeypointsImpedanceController::Config{
278  // .nodeSetName = dto.nodeSetName,
279  // .kpImpedance = dto.kpImpedance,
280  // .kdImpedance = dto.kdImpedance,
281  // // .kpAdmittance = dto.kpAdmittance,
282  // .kdAdmittance = dto.kdAdmittance,
283  // .kmAdmittance = dto.kmAdmittance,
284  // .kpNullspace = dto.kpNullspace,
285  // .kdNullspace = dto.kdNullspace,
286  // // .currentForceTorque = dto.currentForceTorque,
287  // // .desiredPose = dto.desiredPose,
288  // // .desiredTwist = dto.desiredTwist,
289  // .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles,
290  // .torqueLimit = dto.torqueLimit,
291  // .qvelFilter = dto.qvelFilter,
292  // .numPoints = dto.numPoints,
293  // .keypointKp = dto.keypointKp,
294  // .keypointKd = dto.keypointKd,
295  // .fixedTranslation = dto.fixedTranslation,
296  // .densityForceScaling = dto.densityForceScaling,
297  // .keypointPositionFilter = dto.keypointPositionFilter,
298  // .keypointVelocityFilter = dto.keypointVelocityFilter,
299  // .isRigid = dto.isRigid};
300  // }
301  //
302  // void
303  // toAron(arondto::KeypointControllerConfig& dto, const KeypointsImpedanceController::Config& bo)
304  // {
305  // dto.nodeSetName = bo.nodeSetName;
306  // dto.kpImpedance = bo.kpImpedance;
307  // dto.kdImpedance = bo.kdImpedance;
308  // // dto .kpAdmittance = bo.kpAdmittance;
309  // dto.kdAdmittance = bo.kdAdmittance;
310  // dto.kmAdmittance = bo.kmAdmittance;
311  // dto.kpNullspace = bo.kpNullspace;
312  // dto.kdNullspace = bo.kdNullspace;
313  // // dto .currentForceTorque = bo.currentForceTorque;
314  // // dto .desiredPose = bo.desiredPose;
315  // // dto .desiredTwist = bo.desiredTwist;
316  // dto.desiredNullspaceJointAngles = bo.desiredNullspaceJointAngles;
317  // dto.torqueLimit = bo.torqueLimit;
318  // dto.qvelFilter = bo.qvelFilter;
319  // dto.numPoints = bo.numPoints;
320  // dto.keypointKp = bo.keypointKp;
321  // dto.keypointKd = bo.keypointKd;
322  // dto.fixedTranslation = bo.fixedTranslation;
323  // dto.densityForceScaling = bo.densityForceScaling;
324  // dto.keypointPositionFilter = bo.keypointPositionFilter;
325  // dto.keypointVelocityFilter = bo.keypointVelocityFilter;
326  // dto.isRigid = bo.isRigid;
327  // }
328 
329 } // namespace armarx::control::common::control_law
330 
331 namespace armarx::control::common::mp
332 {
333  // void
334  // fromAron(const arondto::ListViaPoint& dto, MP::ListViaPoint& bo)
335  // {
336  // bo = MP::ListViaPoint{.canonicalValue = dto.canonicalValue,
337  // .viaPointValue = dto.viaPointValue};
338  // }
339 
340  // void
341  // toAron(arondto::ListViaPoint& dto, const MP::ListViaPoint& bo)
342  // {
343  // dto.canonicalValue = bo.canonicalValue;
344  // dto.viaPointValue = bo.viaPointValue;
345  // }
346 
347  // void
348  // fromAron(const arondto::DictViaPoint& dto, MP::DictViaPoint& bo)
349  // {
350  // bo = MP::DictViaPoint{.canonicalValue = dto.canonicalValue,
351  // .viaPointValue = dto.viaPointValue};
352  // }
353 
354  // void
355  // fromAron(const arondto::MPConfig& dto, MP::MPConfig& bo)
356  // {
357  // std::vector<MP::ListViaPoint> viaPoints;
358  // std::transform(dto.viaPoints.begin(),
359  // dto.viaPoints.end(),
360  // std::back_inserter(viaPoints),
361  // [](const auto& vp) -> MP::ListViaPoint
362  // { return ::armarx::fromAron<MP::ListViaPoint, arondto::ListViaPoint>(vp); });
363  // bo = MP::MPConfig{.name = dto.name,
364  // .nodeSetName = dto.nodeSetName,
365  // .className = dto.className,
366  // .mpTypeString = dto.mpTypeString,
367  // .mpMode = dto.mpMode,
368  // .mpStyle = dto.mpStyle,
369  // .regressionModel = dto.regressionModel,
370  // .kernelSize = dto.kernelSize,
371  // .damping = dto.damping,
372  // .tau = dto.tau,
373  // .amplitude = dto.amplitude,
374  // .durationSec = dto.durationSec,
375  // .fileList = dto.fileList,
376  // .stopWithMP = dto.stopWithMP,
377  // .viaPoints = viaPoints,
378  // .enablePhaseStop = dto.enablePhaseStop, /// phase stop
379  // .maxValue = dto.maxValue,
380  // .slop = dto.slop,
381  // .goDist = dto.goDist,
382  // .backDist = dto.backDist,
383  // .psKpPos = dto.psKpPos,
384  // .psKdPos = dto.psKdPos,
385  // .psKpOri = dto.psKpOri,
386  // .psKdOri = dto.psKdOri,
387  // .psMM2Radian = dto.psMM2Radian};
388  // armarx::aron::fromAron<armarx::control::common::mp::arondto::MPTraj, MPTraj>(
389  // dto.trajectoryList, bo.trajectoryList);
390  // }
391 
392  // void
393  // toAron(arondto::MPConfig& dto, const MP::MPConfig& bo)
394  // {
395  // dto.name = bo.name;
396  // dto.nodeSetName = bo.nodeSetName;
397  // dto.className = bo.className;
398  // dto.mpTypeString = bo.mpTypeString;
399  // dto.mpMode = bo.mpMode;
400  // dto.mpStyle = bo.mpStyle;
401  // dto.regressionModel = bo.regressionModel;
402  // dto.kernelSize = bo.kernelSize;
403  // dto.damping = bo.damping;
404  // dto.tau = bo.tau;
405  // dto.amplitude = bo.amplitude;
406  // dto.durationSec = bo.durationSec;
407  // dto.fileList = bo.fileList;
408  // dto.stopWithMP = bo.stopWithMP;
409  // std::transform(bo.viaPoints.begin(),
410  // bo.viaPoints.end(),
411  // std::back_inserter(dto.viaPoints),
412  // [](const auto& vp) -> arondto::ListViaPoint
413  // { return ::armarx::toAron<arondto::ListViaPoint, MP::ListViaPoint>(vp); });
414  // dto.enablePhaseStop = bo.enablePhaseStop; /// phase stop
415  // dto.maxValue = bo.maxValue;
416  // dto.slop = bo.slop;
417  // dto.goDist = bo.goDist;
418  // dto.backDist = bo.backDist;
419  // dto.psKpPos = bo.psKpPos;
420  // dto.psKdPos = bo.psKdPos;
421  // dto.psKpOri = bo.psKpOri;
422  // dto.psKdOri = bo.psKdOri;
423  // dto.psMM2Radian = bo.psMM2Radian;
424  // armarx::aron::toAron<armarx::control::common::mp::arondto::MPTraj, MPTraj>(
425  // dto.trajectoryList, bo.trajectoryList);
426  // }
427 
428  // void
429  // fromAron(const arondto::MPListConfig& dto, MP::MPListConfig& bo)
430  // {
431  // std::transform(dto.mpList.begin(),
432  // dto.mpList.end(),
433  // std::back_inserter(bo.mpList),
434  // [](const auto& mp) -> MP::MPConfig
435  // { return ::armarx::fromAron<MP::MPConfig, arondto::MPConfig>(mp); });
436  // }
437 
438  // void
439  // toAron(arondto::MPListConfig& dto, const MP::MPListConfig& bo)
440  // {
441  // std::transform(bo.mpList.begin(),
442  // bo.mpList.end(),
443  // std::back_inserter(dto.mpList),
444  // [](const auto& _mp) -> arondto::MPConfig
445  // { return ::armarx::toAron<arondto::MPConfig, MP::MPConfig>(_mp); });
446  // }
447 
448  // // MPTraj
449  // void
450  // fromAron(const arondto::MPTraj& dto, MPTraj& bo)
451  // {
452  // bo = MPTraj{.time = dto.time, .traj = dto.traj};
453  // }
454 
455  // void
456  // toAron(arondto::MPTraj& dto, const MPTraj& bo)
457  // {
458  // dto.time = bo.time;
459  // dto.traj = bo.traj;
460  // }
461 
462  // void
463  // fromAron(const arondto::MPTrajs& dto, MPTrajs& bo)
464  // {
465  // std::transform(dto.data.begin(),
466  // dto.data.end(),
467  // std::back_inserter(bo),
468  // [](const auto& traj) -> MPTraj
469  // { return ::armarx::fromAron<MPTraj, arondto::MPTraj>(traj); });
470  // }
471 
472  // void
473  // toAron(arondto::MPTrajs& dto, const MPTrajs& bo)
474  // {
475  // std::transform(bo.begin(),
476  // bo.end(),
477  // std::back_inserter(dto),
478  // [](const auto& traj) -> arondto::MPTraj
479  // { return ::armarx::toAron<arondto::MPTraj, MPTraj>(traj); });
480  // }
481 
482  // void
483  // fromAron(const arondto::MultiMPTrajs& dto, MultiMPTrajs& bo)
484  // {
485  // for (const auto& pair : dto.data)
486  // {
487  // bo.emplace(pair.first, ::armarx::fromAron<MPTrajs, arondto::MPTrajs>(pair.second));
488  // }
489  // }
490 
491  // void
492  // toAron(arondto::MultiMPTrajs& dto, const MultiMPTrajs& bo)
493  // {
494  // for (const auto& pair : bo)
495  // {
496  // dto.data.emplace(pair.first, ::armarx::toAron<arondto::MPTrajs, MPTrajs>(pair.second));
497  // }
498  // }
499 } // namespace armarx::control::common::mp
armarx::control::common::ft
This file is part of ArmarX.
Definition: aron_conversions.cpp:29
armarx::control::common::mp
This file is part of ArmarX.
Definition: aron_conversions.cpp:331
aron_conversions.h
armarx::control::common::control_law
This file is part of ArmarX.
Definition: aron_conversions.cpp:68
eigen.h