KVILImpedanceController.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  * @package ...
17  * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
18  * @date 2023
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "KVILImpedanceController.h"
24 
25 #include <armarx/control/common/math.h>
26 #include <armarx/control/common/utils.h>
27 #include <armarx/control/njoint_mp_controller/task_space/aron/aron_conversions.h>
28 
29 namespace armarx::control::njoint_mp_controller::task_space
30 {
31  NJointControllerRegistration<NJointKVILImpedanceMPController>
32  registrationControllerNJointKVILImpedanceMPController("NJointKVILImpedanceMPController");
33 
34  NJointKVILImpedanceMPController::NJointKVILImpedanceMPController(
35  const RobotUnitPtr& robotUnit,
36  const NJointControllerConfigPtr& config,
37  const VirtualRobot::RobotPtr& robot) :
38  NJointKeypointsImpedanceController{robotUnit, config, robot}, mp::MPPool{}
39  {
40  {
41  ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
42  auto configData = ::armarx::fromAronDict<AronDTO, BO>(cfg->config);
43  validateConfigData(configData);
44  bufferUserToAdditionalTask.reinitAllBuffers(configData);
45  bufferAdditionalTaskToUser.reinitAllBuffers(configData);
46  bufferUserToRt.reinitAllBuffers(configData);
47  ARMARX_IMPORTANT << VAROUT(configData.mpList.size());
48  createMPs(configData.mpList);
49 
50  // configureConstraints(configData.constraintList);
51  }
52  ARMARX_IMPORTANT << "NJointTSImpedanceMPController construction done";
53  // const auto& cfg = bufferUserToAdditionalTask.getUpToDateReadBuffer();
54  // createMPs(cfg.mpConfig);
55  // configureConstraints(cfg);
56  }
57 
58  std::string
59  NJointKVILImpedanceMPController::getClassName(const Ice::Current&) const
60  {
61  return "NJointKVILImpedanceMPController";
62  }
63 
64  //void NJointKVILImpedanceMPController::reconfigureController(const std::string &filename, const Ice::Current &)
65  //{
66  // NJointKeypointsImpedanceController::reconfigureController(filename);
67  // reconfigureMPs(filename);
68  //}
69 
70  void
71  NJointKVILImpedanceMPController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
72  const Ice::Current& iceCurrent)
73  {
74  NJointKeypointsImpedanceController::updateConfig(dto);
75  if (isFinished("all"))
76  {
77  isMPReady.store(false);
78  const auto& cfg = bufferUserToAdditionalTask.getUpToDateReadBuffer();
79  reconfigureMPs(cfg.mpList);
80  // configureConstraints(cfg);
81  isMPReady.store(true);
82  }
83  }
84 
85  ::armarx::aron::data::dto::DictPtr
86  NJointKVILImpedanceMPController::getConfig(const Ice::Current& iceCurrent)
87  {
88  return ::armarx::toAronDict<AronDTO, BO>(
89  bufferAdditionalTaskToUser.getUpToDateReadBuffer());
90  }
91 
92  void
93  NJointKVILImpedanceMPController::configureConstraints(const ConstraintList& c)
94  {
95  // n_constraints = c.size();
96  // for (int i = 0; i < n_constraints; i ++)
97  // {
98  // std::string name = "mp_"+std::to_string(i);
99  // auto c = mp_cfg[name];
100  // ConstraintPtr constraint(new Constraint());
101  // constraint->type = c["constraint_type"];
102  // ARMARX_INFO << VAROUT(constraint->type);
103  //
104  // constraint->localFrameOrigin = c["local_frame_origin_obs"];
105  // ARMARX_CHECK_EQUAL(constraint->localFrameOrigin.size(), 3);
106  // ARMARX_INFO << VAROUT(constraint->localFrameOrigin);
107  //
108  // constraint->localFrameRotMat = c["local_frame_rot_mat_obs"];
109  // ARMARX_CHECK_EQUAL(constraint->localFrameRotMat.cols(), 3);
110  // ARMARX_CHECK_EQUAL(constraint->localFrameRotMat.rows(), 3);
111  // ARMARX_INFO << VAROUT(constraint->localFrameRotMat);
112  //
113  // constraint->vmpStart = common::getEigenVec(c, c, "vmp_start");
114  // ARMARX_INFO << VAROUT(constraint->vmpStart);
115  // if (c["manifold"] == "linear")
116  // {
117  // constraint->pcaMean = c["pca_mean"];
118  // constraint->pcaComponents = c["pca_components"];
119  // ARMARX_CHECK_EQUAL(constraint->pcaMean.size(), 3);
120  // ARMARX_CHECK_EQUAL(constraint->pcaComponents.cols(), 3);
121  // ARMARX_CHECK_EQUAL(constraint->pcaComponents.rows(), 3);
122  // ARMARX_IMPORTANT << VAROUT(constraint->pcaMean) << VAROUT(constraint->pcaComponents);
123  // }
124  // else
125  // {
126  // constraint->pcaMean.setZero();
127  // constraint->pcaComponents.setZero();
128  // }
129  // if (constraint->type == "p2c")
130  // {
131  // // constraint->pcaMean = c["pca_mean"];
132  // constraint->pm_scale = c["pm_scale"];
133  // constraint->pm_init_proj_point = common::getEigenVec(c, c, "pm_init_proj_point");
134  // constraint->pm_init_tangent_vec = common::getEigenVec(c, c, "pm_init_tangent_vec");
135  // constraint->pm_init_proj_value = c["pm_init_proj_value"];
136  //
137  // ARMARX_INFO << VAROUT(constraint->pm_init_proj_point)
138  // << VAROUT(constraint->pm_init_tangent_vec)
139  // << VAROUT(constraint->pm_init_proj_value);
140  //
141  // NonlinearManifoldInfoStruct curve_info;
142  // curve_info.pm_proj_point = constraint->pm_init_proj_point;
143  // curve_info.pm_proj_point_to_mean_distance = constraint->pm_init_proj_value;
144  // curve_info.pm_tangent_vec = constraint->pm_init_tangent_vec;
145  // nl_mani_buffer.reinitAllBuffers(curve_info);
146  //
147  // p2CStruct p2c_data;
148  // p2c_data.curve_kpt_position = common::getEigenVec(c, c, "pm_imit_kpts_position_local");
149  // kpt_status_buffer.reinitAllBuffers(p2c_data);
150  //
151  // // ARMARX_CHECK_EQUAL(constraint->pcaMean.size(), 3);
152  // // ARMARX_IMPORTANT << VAROUT(constraint->pcaMean);
153  // }
154  // if (constraint->type == "p2l" or constraint->type == "p2P" or constraint->type == "p2c")
155  // {
156  // constraint->priority = c["priority"];
157  // constraint->density_std_dev = c["density_std_dev"];
158  // constraint->density_weights = getEigenVec(c, c, "density_weights");
159  // ARMARX_IMPORTANT << VAROUT(constraint->priority) << VAROUT(constraint->density_std_dev) << VAROUT(constraint->density_weights);
160  // getEigenMatrix(c, "density_mu", constraint->density_mu);
161  // ARMARX_IMPORTANT << VAROUT(constraint->density_mu);
162  // ARMARX_CHECK_EQUAL(constraint->density_mu.rows(), constraint->density_weights.size());
163  // // ARMARX_CHECK_EQUAL(constraint->density_mu.cols(), 1);
164  // }
165  // else
166  // {
167  // constraint->density_mu = Eigen::Matrix1f::Zero();
168  // constraint->density_weights.setZero(0);
169  // }
170  // constraint->spatial_scale = c["spatial_scale"];
171  // constraints.insert({name, constraint});
172  // ARMARX_INFO << " -- added constraint: " << name;
173  // }
174  // ARMARX_IMPORTANT << "--------------------------------------- Done! ---------------------------------------";
175  }
176 
177  void
178  NJointKVILImpedanceMPController::setCurveProjection(const Eigen::Vector3f& curve_proj_point_,
179  float proj_value_,
180  const Eigen::Vector3f& curve_vec_,
181  const Ice::Current&)
182  {
183  nl_mani_buffer.getWriteBuffer().pm_proj_point = curve_proj_point_;
184  nl_mani_buffer.getWriteBuffer().pm_proj_point_to_mean_distance = proj_value_;
185  nl_mani_buffer.getWriteBuffer().pm_tangent_vec = curve_vec_;
186  nl_mani_buffer.commitWrite();
187  // curve_proj_point = curve_proj_point_;
188  // curve_proj_point_to_mean_distance = proj_value_;
189  // vec_to_min_on_curve = curve_vec_;
190  // curve_set.store(true);
191  }
192 
193  Eigen::Vector3f
194  NJointKVILImpedanceMPController::getCurveKeypointPosition(const Ice::Current&)
195  {
196  return kpt_status_buffer.getUpToDateReadBuffer().curve_kpt_position;
197  }
198 
199  void
200  NJointKVILImpedanceMPController::additionalTask()
201  {
202  // Eigen::VectorXf kptsPosition = controlStatusBuffer.getReadBuffer().filteredKeypointPosition;
203  // Eigen::VectorXf kptsVelocity = controlStatusBuffer.getReadBuffer().currentKeypointVelocity;
204  // double deltaT = controlStatusBuffer.getReadBuffer().deltaT;
205  // ARMARX_CHECK_EQUAL(kptsPosition.size(), n_constraints * 3);
206  // ARMARX_CHECK_EQUAL(kptsVelocity.size(), n_constraints * 3);
207  //
208  // /// parse arguments and run mp
209  //// ARMARX_INFO << VAROUT(controller.s.numPoints) << VAROUT(kptsPosition) << VAROUT(kptsVelocity) << VAROUT(deltaT);
210  // for (int i = 0; i < n_constraints; i++)
211  // {
212  // std::string name = "mp_"+std::to_string(i);
213  // mp::KeypointsMPInputPtr in = std::dynamic_pointer_cast<mp::KeypointsMPInput>(mps[name].input);
214  //
215  // auto c = constraints[name];
216  // Eigen::Vector3f kptPositionLocal = c->localFrameRotMat.transpose() * (kptsPosition.segment(3*i, 3) - c->localFrameOrigin);
217  // Eigen::Vector3f kptVelocityLocal = c->localFrameRotMat.transpose() * kptsVelocity.segment(3*i, 3);
218  // if (c->type == "p2p"/* or c->type == "p2c"*/)
219  // {
220  // in->keypointPosition = kptPositionLocal;
221  // in->keypointVelocity = kptVelocityLocal;
222  // }
223  //
224  //
225  // else if (c->type == "p2c")
226  // {
227  // kpt_status_buffer.getWriteBuffer().curve_kpt_position = kptPositionLocal;
228  // kpt_status_buffer.commitWrite();
229  //
230  // Eigen::Vector3f devi_vec = kptPositionLocal - nl_mani_buffer.getUpToDateReadBuffer().pm_proj_point;
231  // float dist = devi_vec.norm();
232  // devi_vec /= dist;
233  // float vel_proj = kptVelocityLocal.dot(devi_vec);
234  // in->keypointPosition(0) = dist; // c->pcaComponents.row(0).cross(kptPositionLocal - c->pcaMean).norm();
235  // in->keypointVelocity(0) = vel_proj;
236  // }
237  //
238  //
239  //
240  // else if (c->type == "p2l")
241  // {
242  // Eigen::Vector3f line_vec = c->pcaComponents.row(0);
243  // float proj_value = line_vec.dot(kptPositionLocal - c->pcaMean);
244  // Eigen::Vector3f proj_point = c->pcaMean + proj_value * line_vec;
245  // Eigen::Vector3f devi_vec = kptPositionLocal - proj_point;
246  // float dist = devi_vec.norm();
247  // devi_vec /= dist;
248  // float vel_proj = kptVelocityLocal.dot(devi_vec);
249  // in->keypointPosition(0) = dist; // c->pcaComponents.row(0).cross(kptPositionLocal - c->pcaMean).norm();
250  // in->keypointVelocity(0) = vel_proj;
251  // }
252  // else if (c->type == "p2P")
253  // {
254  // Eigen::Vector3f plane_norm_vec = c->pcaComponents.row(2);
255  // in->keypointPosition(0) = plane_norm_vec.dot(kptPositionLocal - c->pcaMean);
256  // in->keypointVelocity(0) = plane_norm_vec.dot(kptVelocityLocal);
257  // }
258  // in->deltaT = deltaT;
259  // }
260  // {
261  // // null space mp
262  // mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(mps["null"].input);
263  // in->angleRadian = controlStatusBuffer.getReadBuffer().qpos;
264  // in->angularVel = controlStatusBuffer.getReadBuffer().qvel;
265  // in->deltaT = controlStatusBuffer.getReadBuffer().deltaT;
266  // }
267  //
268  // runMPs();
269  //
270  // Eigen::VectorXf targetKeypointPosition;
271  // Eigen::VectorXf targetKeypointVelocity;
272  // Eigen::VectorXf targetDensityForce;
273  // targetKeypointPosition.setZero(kptsPosition.size());
274  // targetKeypointVelocity.setZero(kptsPosition.size());
275  // targetDensityForce.setZero(kptsPosition.size());
276  // /// parse mp target
277  // for (int i = 0; i < n_constraints; i++)
278  // {
279  // std::string name = "mp_"+std::to_string(i);
280  // mp::KeypointsMPOutputPtr out = std::dynamic_pointer_cast<mp::KeypointsMPOutput>(mps[name].output);
281  //
282  // auto c = constraints[name];
283  // Eigen::Vector3f kptPositionLocal = c->localFrameRotMat.transpose() * (kptsPosition.segment(3*i, 3) - c->localFrameOrigin);
284  //
285  // Eigen::Vector3f targetPosition;
286  // Eigen::Vector3f targetVelocity;
287  // Eigen::Vector3f density_force = Eigen::Vector3f::Zero();
288  //
289  // if (c->type == "p2p"/* or c->type == "p2c"*/)
290  // {
291  // targetPosition = out->keypointPosition;
292  // targetVelocity = out->keypointVelocity;
293  // }
294  //
295  //
296  // else if (c->type == "p2c")
297  // {
298  //// LockGuardType guard {curveDataMutex};
299  //// if (curve_set.load())
300  //// {
301  //
302  // Eigen::Vector3f curve_proj_point = nl_mani_buffer.getUpToDateReadBuffer().pm_proj_point;
303  // Eigen::Vector3f curve_tangent_vec = nl_mani_buffer.getUpToDateReadBuffer().pm_tangent_vec;
304  // float curve_proj_point_to_mean_distance = nl_mani_buffer.getUpToDateReadBuffer().pm_proj_point_to_mean_distance;
305  //
306  // Eigen::Vector3f devi_vec = kptPositionLocal - curve_proj_point;
307  // float dist = devi_vec.norm();
308  // devi_vec /= dist;
309  //
310  // /// density force
311  // // float kp = 0.0f;
312  // // if (curve_proj_point_to_mean_distance > 100.0f)
313  // // {
314  // // kp = std::signbit(curve_proj_point_to_mean_distance) ? -1 : 1 * 100.0f;
315  // // }
316  // // else
317  // // {
318  // // kp = curve_proj_point_to_mean_distance;
319  // // }
320  // Eigen::Vector3f base_force = - curve_tangent_vec * curve_proj_point_to_mean_distance * 0.0000015f;
321  // base_force = c->localFrameRotMat * base_force;
322  //
323  // Eigen::Vector1f prob_derivative = Eigen::Vector1f::Zero();
324  // Eigen::Vector1f data {curve_proj_point_to_mean_distance};
325  // for (int i = 0; i < c->density_mu.rows(); i++)
326  // {
327  // prob_derivative = prob_derivative + pdf_gradient(c->density_mu.row(i), c->density_std_dev, data);
328  // }
329  // density_force = prob_derivative(0) * curve_tangent_vec * 10.0f;
330  // density_force = c->localFrameRotMat * density_force;
331  //
332  // if (base_force.norm() > density_force.norm())
333  // {
334  // ARMARX_RT_LOGF_INFO("use base force");
335  // density_force = base_force;
336  // }
337  //
338  // /// priority
339  // if (c->priority > 0)
340  // {
341  // if (constraints["mp_0"]->type == "p2p")
342  // {
343  // /// already in root frame
344  // Eigen::Vector3f devi = kptsPosition.segment(3*i, 3) - kptsPosition.segment(0, 3);
345  // Eigen::Vector3f line_vec_root = c->localFrameRotMat * curve_tangent_vec;
346  // Eigen::Vector3f tang_vec = devi.cross(line_vec_root).cross(devi);
347  // tang_vec.normalize();
348  // float force_proj = density_force.dot(tang_vec);
349  // density_force = force_proj * tang_vec;
350  // }
351  // }
352  //
353  // targetPosition = curve_proj_point + devi_vec * out->keypointPosition(0);
354  // targetVelocity = devi_vec * out->keypointVelocity(0);
355  //
356  // // }
357  // // else
358  // // {
359  // // targetPosition = kptsPosition.segment(3*i, 3);
360  // // targetVelocity.setZero();
361  // // }
362  // }
363  //
364  //
365  // else if (c->type == "p2l")
366  // {
367  // Eigen::Vector3f line_vec = c->pcaComponents.row(0);
368  // float proj_value = line_vec.dot(kptPositionLocal - c->pcaMean);
369  // Eigen::Vector3f proj_point = c->pcaMean + proj_value * line_vec;
370  // Eigen::Vector3f devi_vec = kptPositionLocal - proj_point;
371  // float dist = devi_vec.norm();
372  // devi_vec /= dist;
373  //
374  // /// density force
375  // float kp = 0.0f;
376  // if (abs(proj_value) > 100.0f)
377  // {
378  // kp = std::signbit(proj_value) ? -1 : 1 * 100.0f;
379  // }
380  // else
381  // {
382  // kp = proj_value;
383  // }
384  // Eigen::Vector3f base_force = - c->pcaComponents.row(0) * proj_value * 0.0000015f;
385  // base_force = c->localFrameRotMat * base_force;
386  //
387  // Eigen::Vector1f prob_derivative = Eigen::Vector1f::Zero();
388  // Eigen::Vector1f data {proj_value};
389  // for (int i = 0; i < c->density_mu.rows(); i++)
390  // {
391  // prob_derivative = prob_derivative + pdf_gradient(c->density_mu.row(i), c->density_std_dev, data);
392  // }
393  // density_force = prob_derivative(0) * c->pcaComponents.row(0) * 10.0f;
394  // density_force = c->localFrameRotMat * density_force;
395  //
396  // if (base_force.norm() > density_force.norm())
397  // {
398  // ARMARX_RT_LOGF_INFO("use base force");
399  // density_force = base_force;
400  // }
401  //
402  // /// priority
403  // if (c->priority > 0)
404  // {
405  // if (constraints["mp_0"]->type == "p2p")
406  // {
407  // /// already in root frame
408  // Eigen::Vector3f devi = kptsPosition.segment(3*i, 3) - kptsPosition.segment(0, 3);
409  // Eigen::Vector3f line_vec_local = c->pcaComponents.row(0);
410  // Eigen::Vector3f line_vec_root = c->localFrameRotMat * line_vec_local;
411  // Eigen::Vector3f tang_vec = devi.cross(line_vec_root).cross(devi);
412  // tang_vec.normalize();
413  // float force_proj = density_force.dot(tang_vec);
414  // density_force = force_proj * tang_vec;
415  // }
416  // }
417  //
418  // targetPosition = proj_point + devi_vec * out->keypointPosition(0);
419  // targetVelocity = devi_vec * out->keypointVelocity(0);
420  // }
421  // else if (c->type == "p2P")
422  // {
423  // Eigen::Vector3f proj_value_vec = c->pcaComponents * (kptPositionLocal - c->pcaMean);
424  // Eigen::Vector3f plane_norm_vec = c->pcaComponents.row(2);
425  // Eigen::Vector3f proj_point = kptPositionLocal - plane_norm_vec * proj_value_vec(2);
426  //
427  // /// density force
428  // Eigen::Vector2f proj_value_on_plane = proj_value_vec.head<2>();
429  // proj_value_on_plane.normalize();
430  //
431  // Eigen::Vector3f base_force = - c->pcaComponents.row(0) * proj_value_on_plane(0) - c->pcaComponents.row(1) * proj_value_on_plane(1);
432  // base_force = c->localFrameRotMat * base_force;
433  //
434  // Eigen::Vector2f prob_derivative = Eigen::Vector2f::Zero();
435  // Eigen::Vector2f data = proj_value_vec.head<2>();
436  // for (int i = 0; i < c->density_mu.rows(); i++)
437  // {
438  // prob_derivative = prob_derivative + pdf_gradient(c->density_mu.row(i), c->density_std_dev, data);
439  // }
440  // density_force = (prob_derivative(0) * c->pcaComponents.row(0) + prob_derivative(1) * c->pcaComponents.row(1)) * 10.0f;
441  // density_force = c->localFrameRotMat * density_force;
442  //
443  // if (base_force.norm() > density_force.norm())
444  // {
445  // ARMARX_RT_LOGF_INFO("use base force");
446  // density_force = base_force;
447  // }
448  //
449  // /// handle priority
450  // if (c->priority > 0)
451  // {
452  // if (constraints["mp_0"]->type == "p2p")
453  // {
454  // Eigen::Vector3f devi = kptsPosition.segment(3*i, 3) - kptsPosition.segment(0, 3);
455  // Eigen::Vector3f plane_norm_vec_root = c->localFrameRotMat * plane_norm_vec;
456  // Eigen::Vector3f tang_vec = devi.cross(plane_norm_vec_root);
457  // tang_vec.normalize();
458  // float force_proj = density_force.dot(tang_vec);
459  // density_force = force_proj * tang_vec;
460  // }
461  // }
462  //
463  //
464  // targetPosition = proj_point + plane_norm_vec * out->keypointPosition(0);
465  // targetVelocity = out->keypointVelocity(0) * plane_norm_vec;
466  // }
467  // targetKeypointPosition.segment(3*i, 3) = c->localFrameRotMat * targetPosition + c->localFrameOrigin;
468  // targetKeypointVelocity.segment(3*i, 3) = c->localFrameRotMat * targetVelocity;
469  // targetDensityForce.segment(3*i, 3) = density_force;
470  // }
471  // ARMARX_CHECK_EQUAL(targetKeypointPosition.size(), n_constraints * 3);
472  // ARMARX_CHECK_EQUAL(targetKeypointVelocity.size(), n_constraints * 3);
473  // ARMARX_CHECK_EQUAL(targetDensityForce.size(), n_constraints * 3);
474  // getWriterControlStruct().desiredKeypointPosition = targetKeypointPosition;
475  // getWriterControlStruct().desiredKeypointVelocity = targetKeypointVelocity;
476  // getWriterControlStruct().desiredDensityForce = targetDensityForce;
477  // {
478  // mp::JSMPOutputPtr out = std::dynamic_pointer_cast<mp::JSMPOutput>(mps["null"].output);
479  // getWriterControlStruct().desiredNullspaceJointAngles = out->angleRadian;
480  // }
481  // writeControlStruct();
482  }
483 
484  void
485  NJointKVILImpedanceMPController::rtPreActivateController()
486  {
487  // ARMARX_IMPORTANT << "rt pre activate: reinitialize the mp input output, as well as the rt related buffer values";
488  // NJointKeypointsImpedanceController::rtPreActivateController();
489  // for (int i = 0; i < controller.s.numPoints; i++)
490  // {
491  // std::string name = "mp_"+std::to_string(i);
492  //
493  // ARMARX_INFO << "initialize " << name;
494  //
495  // auto in = std::dynamic_pointer_cast<mp::KeypointsMPInput>(mps[name].input);
496  // auto out = std::dynamic_pointer_cast<mp::KeypointsMPOutput>(mps[name].output);
497  // auto c = constraints[name];
498  //
499  // in->keypointPosition = c->vmpStart;
500  // out->keypointPosition = c->vmpStart;
501  //
502  // if (c->type == "p2p")
503  // {
504  // in->keypointVelocity = controller.s.currentKeypointVelocity;
505  // out->keypointVelocity = controller.s.currentKeypointVelocity;
506  // }
507  // else if (c->type == "p2l")
508  // {
509  // in->keypointVelocity = Eigen::Vector1f::Zero();
510  // out->keypointVelocity = Eigen::Vector1f::Zero();
511  // }
512  // else if (c->type == "p2P")
513  // {
514  // in->keypointVelocity = Eigen::Vector1f::Zero();
515  // out->keypointVelocity = Eigen::Vector1f::Zero();
516  // }
517  // else if (c->type == "p2c")
518  // {
519  // in->keypointVelocity = Eigen::Vector1f::Zero();
520  // out->keypointVelocity = Eigen::Vector1f::Zero();
521  // }
522  // }
523  // {
524  // ARMARX_IMPORTANT << "initialize input output buffers between nullspace mp";
525  // auto in = std::dynamic_pointer_cast<mp::JSMPInput>(mps["null"].input);
526  // auto out = std::dynamic_pointer_cast<mp::JSMPOutput>(mps["null"].output);
527  // in->angleRadian = controller.s.qpos;
528  // in->angularVel = controller.s.qvel;
529  // in->deltaT = 0.0;
530  // out->angleRadian = controller.s.qpos;
531  // out->angularVel = controller.s.qvel;
532  // }
533  }
534 } // namespace armarx::control::njoint_mp_controller::task_space
KVILImpedanceController.h
armarx::control::njoint_controller::task_space::NJointKeypointsImpedanceController::robotUnit
RobotUnitPtr robotUnit
Definition: KeypointsImpedanceController.h:115
armarx::control::common::mp::MPPool::isMPReady
std::atomic< bool > isMPReady
Definition: MPPool.h:129
armarx::control::common::mp::MPPool::reconfigureMPs
void reconfigureMPs(const MPListConfig &mpListConfig)
Definition: MPPool.cpp:80
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::control::njoint_controller::task_space::NJointKeypointsImpedanceController
Brief description of class NJointKeypointsImpedanceController.
Definition: KeypointsImpedanceController.h:49
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::NJointKVILImpedanceMPController
NJointKVILImpedanceMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: KVILImpedanceController.cpp:34
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
armarx::control::njoint_mp_controller::task_space::ConstraintList
std::vector< Constraint1 > ConstraintList
Definition: KVILImpedanceController.h:57
armarx::control::NJointKVILImpedanceMPControllerInterface::getCurveKeypointPosition
Eigen::Vector3f getCurveKeypointPosition()
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::bufferAdditionalTaskToUser
TripleBuffer< BO > bufferAdditionalTaskToUser
Definition: KVILImpedanceController.h:127
armarx::control::common::control_law::collision::validateConfigData
void validateConfigData(const arondto::CollisionAvoidanceConfig &cfg)
Definition: CollisionAvoidance.cpp:979
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
math.h
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::updateConfig
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: KVILImpedanceController.cpp:71
armarx::control::njoint_mp_controller::task_space::registrationControllerNJointKVILImpedanceMPController
NJointControllerRegistration< NJointKVILImpedanceMPController > registrationControllerNJointKVILImpedanceMPController("NJointKVILImpedanceMPController")
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::getConfig
::armarx::aron::data::dto::DictPtr getConfig(const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: KVILImpedanceController.cpp:86
utils.h
armarx::control::njoint_mp_controller::task_space
This file is part of ArmarX.
Definition: AdmittanceController.cpp:29
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: KVILImpedanceController.cpp:485
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
IceUtil::Handle< class RobotUnit >
armarx::control::njoint_controller::task_space::NJointKeypointsImpedanceController::updateConfig
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
NJointController interface.
Definition: KeypointsImpedanceController.cpp:198
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::setCurveProjection
void setCurveProjection(const Eigen::Vector3f &curve_proj_point_, float proj_value_, const Eigen::Vector3f &curve_vec_, const Ice::Current &) override
Definition: KVILImpedanceController.cpp:178
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::additionalTask
void additionalTask() override
Definition: KVILImpedanceController.cpp:200
armarx::control::common::mp::MPPool
Definition: MPPool.h:47
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::bufferUserToAdditionalTask
TripleBuffer< BO > bufferUserToAdditionalTask
Definition: KVILImpedanceController.h:126
aron_conversions.h
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:116
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: KVILImpedanceController.cpp:59
armarx::control::njoint_mp_controller::task_space::NJointKVILImpedanceMPController::configureConstraints
void configureConstraints(const ConstraintList &c)
Definition: KVILImpedanceController.cpp:93
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::control::common::mp::MPPool::isFinished
bool isFinished(const std::string &mpName="all", const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: MPPool.cpp:324