DSRTBimanualController.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  * @package DSController::ArmarXObjects::DSRTBimanualController
17  * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #ifndef _ARMARX_LIB_DSController_DSRTBimanualController_H
24 #define _ARMARX_LIB_DSController_DSRTBimanualController_H
25 
26 #include <VirtualRobot/VirtualRobot.h>
27 
29 
35 
36 #include <armarx/control/ds_controller/DSControllerInterface.h>
37 
38 #include "GMRDynamics.h"
39 #include "MathLib.h"
40 
42 {
44  {
45  public:
47  Eigen::Vector3f left_tcpDesiredAngularError;
48 
51 
52  Eigen::Vector3f left_right_distanceInMeter;
53  };
54 
56 
58  {
59  int K_gmm_;
60  int dim_;
61  std::vector<double> Priors_;
62  std::vector<double> Mu_;
63  std::vector<double> Sigma_;
64  std::vector<double> attractor_;
65  double dt_;
66  };
67 
69  {
70  public:
72  {
73  }
74 
75  BimanualGMMMotionGen(const std::string& fileName)
76  {
77  getGMMParamsFromJsonFile(fileName);
78  }
79 
82 
85 
86  Eigen::Vector3f leftarm_Target;
87  Eigen::Vector3f rightarm_Target;
88 
89  Eigen::Vector3f left_DS_DesiredVelocity;
90  Eigen::Vector3f right_DS_DesiredVelocity;
92 
93 
94  float scaling;
95  float v_max;
96 
97  void
98  getGMMParamsFromJsonFile(const std::string& fileName)
99  {
100  std::ifstream infile{fileName};
101  std::string objDefs = {std::istreambuf_iterator<char>(infile),
102  std::istreambuf_iterator<char>()};
103  // StructuralJsonParser jsonParser(objDefs);
104  // jsonParser.parse();
105  // JPathNavigator jpnav(jsonParser.parsedJson);
106  // float k = jpnav.selectSingleNode("left/K").asFloat();
107  // ARMARX_INFO << "k: " << k ;
108  // jpnav.selectSingleNode("left")
109  JSONObjectPtr json = new JSONObject();
110  json->fromString(objDefs);
111 
112  // leftarm_gmmParas.K_gmm_ = AbstractObjectSerializerPtr::dynamicCast<JSONObjectPtr>(json->getElement("left"))->getInt("K");
113  // leftarm_gmmParas.dim_ = json->getElement("left")->getInt("dim");
114  // boost::dynamic_pointer_cast<JSONObjectPtr>(json->getElement("left"))->getArray<double>("Priors", leftarm_gmmParas.Priors_);
115 
116  // json->getElement("left")->getArray<double>("Mu", leftarm_gmmParas.Mu_);
117  // json->getElement("left")->getArray<double>("attractor", leftarm_gmmParas.attractor_);
118  // json->getElement("left")->getArray<double>("Sigma", leftarm_gmmParas.Sigma_);
119 
120  // rightarm_gmmParas.K_gmm_ = json->getElement("right")->getInt("K");
121  // rightarm_gmmParas.dim_ = json->getElement("right")->getInt("dim");
122  // json->getElement("right")->getArray<double>("Priors", rightarm_gmmParas.Priors_);
123  // json->getElement("right")->getArray<double>("Mu", rightarm_gmmParas.Mu_);
124  // json->getElement("right")->getArray<double>("attractor", rightarm_gmmParas.attractor_);
125  // json->getElement("right")->getArray<double>("Sigma", rightarm_gmmParas.Sigma_);
126 
127 
128  leftarm_gmmParas.K_gmm_ = json->getInt("leftK");
129  leftarm_gmmParas.dim_ = json->getInt("leftDim");
130  json->getArray<double>("leftPriors", leftarm_gmmParas.Priors_);
131  json->getArray<double>("leftMu", leftarm_gmmParas.Mu_);
132  json->getArray<double>("leftAttractor", leftarm_gmmParas.attractor_);
133  json->getArray<double>("leftSigma", leftarm_gmmParas.Sigma_);
134 
135 
136  rightarm_gmmParas.K_gmm_ = json->getInt("rightK");
137  rightarm_gmmParas.dim_ = json->getInt("rightDim");
138  json->getArray<double>("rightPriors", rightarm_gmmParas.Priors_);
139  json->getArray<double>("rightMu", rightarm_gmmParas.Mu_);
140  json->getArray<double>("rightAttractor", rightarm_gmmParas.attractor_);
141  json->getArray<double>("rightSigma", rightarm_gmmParas.Sigma_);
142 
143 
144  scaling = json->getDouble("Scaling");
145  v_max = json->getDouble("MaxVelocity");
146 
153  leftarm_gmm->initGMR(0, 2, 3, 5);
154 
161  rightarm_gmm->initGMR(0, 2, 3, 5);
162 
163 
164  // if (leftarm_gmmParas.attractor_.size() < 3)
165  // {
166  // ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
167  // }
168 
169  // if (rightarm_gmmParas.attractor_.size() < 3)
170  // {
171  // ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
172  // }
173 
174  std::cout << "line 162." << std::endl;
175 
176 
177  for (int i = 0; i < 3; ++i)
178  {
181  }
182 
183  std::cout << "Finished GMM." << std::endl;
184  }
185 
186  void
187  updateDesiredVelocity(const Eigen::Vector3f& leftarm_PositionInMeter,
188  const Eigen::Vector3f& rightarm_PositionInMeter,
189  float positionErrorToleranceInMeter,
190  float desiredZ,
191  float correction_x,
192  float correction_y,
193  float correction_z)
194  {
195  MathLib::Vector position_error;
196  position_error.Resize(3);
197 
198  MathLib::Vector desired_vel;
199  desired_vel.Resize(3);
200 
201 
202  Eigen::Vector3f leftarm_Target_corrected = leftarm_Target;
203  Eigen::Vector3f rightarm_Target_corrected = rightarm_Target;
204 
205  leftarm_Target_corrected(0) += correction_x;
206  rightarm_Target_corrected(0) += correction_x;
207 
208  leftarm_Target_corrected(1) += correction_y;
209  rightarm_Target_corrected(1) += correction_y;
210 
211  leftarm_Target_corrected(2) = desiredZ + correction_z;
212  rightarm_Target_corrected(2) = desiredZ + correction_z;
213 
214 
215  // for the left arm
216  Eigen::Vector3f leftarm_PositionError =
217  leftarm_PositionInMeter - leftarm_Target_corrected;
218  if (leftarm_PositionError.norm() < positionErrorToleranceInMeter)
219  {
220  leftarm_PositionError.setZero();
221  }
222 
223  for (int i = 0; i < 3; ++i)
224  {
225  position_error(i) = leftarm_PositionError(i);
226  }
227 
228  desired_vel = leftarm_gmm->getVelocity(position_error);
229 
230  Eigen::Vector3f leftarm_desired_vel;
231  leftarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
232 
233  leftarm_desired_vel = scaling * leftarm_desired_vel;
234 
235  float lenVec = leftarm_desired_vel.norm();
236 
237  if (std::isnan(lenVec))
238  {
239  leftarm_desired_vel.setZero();
240  }
241 
242  if (desiredZ < 1.5)
243  {
244  v_max = 0.2;
245  }
246  else
247  {
248  v_max = 0.5;
249  }
250 
251  if (lenVec > v_max)
252  {
253  leftarm_desired_vel = (v_max / lenVec) * leftarm_desired_vel;
254  }
255 
256  left_DS_DesiredVelocity = leftarm_desired_vel;
257 
258 
259  // for the right arm
260  Eigen::Vector3f rightarm_PositionError =
261  rightarm_PositionInMeter - rightarm_Target_corrected;
262  if (rightarm_PositionError.norm() < positionErrorToleranceInMeter)
263  {
264  rightarm_PositionError.setZero();
265  }
266 
267  for (int i = 0; i < 3; ++i)
268  {
269  position_error(i) = rightarm_PositionError(i);
270  }
271 
272  desired_vel = rightarm_gmm->getVelocity(position_error);
273 
274  Eigen::Vector3f rightarm_desired_vel;
275  rightarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
276 
277  rightarm_desired_vel = scaling * rightarm_desired_vel;
278 
279  lenVec = rightarm_desired_vel.norm();
280  if (std::isnan(lenVec))
281  {
282  rightarm_desired_vel.setZero();
283  }
284 
285  if (lenVec > v_max)
286  {
287  rightarm_desired_vel = (v_max / lenVec) * rightarm_desired_vel;
288  }
289 
290  right_DS_DesiredVelocity = rightarm_desired_vel;
291 
292  left_right_position_errorInMeter = leftarm_PositionError - rightarm_PositionError;
293  }
294  };
295 
297 
298  /**
299  * @defgroup Library-DSRTBimanualController DSRTBimanualController
300  * @ingroup armarx_control
301  * A description of the library DSRTBimanualController.
302  *
303  * @class DSRTBimanualController
304  * @ingroup Library-DSRTBimanualController
305  * @brief Brief description of class DSRTBimanualController.
306  *
307  * Detailed description of class DSRTBimanualController.
308  */
310  public NJointControllerWithTripleBuffer<DSRTBimanualControllerControlData>,
312  {
313 
314  // ManagedIceObject interface
315  protected:
316  void onInitNJointController();
318 
319 
320  void controllerRun();
321 
322 
323  // NJointControllerInterface interface
324  public:
325  using ConfigPtrT = DSRTBimanualControllerConfigPtr;
326 
327  DSRTBimanualController(const RobotUnitPtr& robotUnit,
328  const NJointControllerConfigPtr& config,
329  const VirtualRobot::RobotPtr&);
330 
331  std::string
332  getClassName(const Ice::Current&) const
333  {
334  return "DSRTBimanualController";
335  }
336 
337  // NJointController interface
338  void rtRun(const IceUtil::Time& sensorValuesTimestamp,
339  const IceUtil::Time& timeSinceLastIteration);
340 
341  void setToDefaultTarget(const Ice::Current&);
342 
343  private:
344  float deadzone(float currentValue, float targetValue, float threshold);
346  quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
347 
348  // PeriodicTask<DSRTBimanualController>::pointer_type controllerTask;
349  BimanualGMMMotionGenPtr gmmMotionGenerator;
350 
351  struct DSRTBimanualControllerSensorData
352  {
353  Eigen::Matrix4f left_tcpPose;
354  Eigen::Matrix4f right_tcpPose;
355 
356  // Eigen::Vector3f left_linearVelocity;
357  // Eigen::Vector3f right_linearVelocity;
358 
359  Eigen::Vector3f left_force;
360  Eigen::Vector3f right_force;
361 
362 
363  double currentTime;
364  };
365 
367 
368  struct DSCtrlDebugInfo
369  {
370  float desredZ;
371  float force_error_z;
372  float guardXYHighFrequency;
373  float guard_mounting_correction_x;
374  float guard_mounting_correction_y;
375  float guard_mounting_correction_z;
376  };
377 
378  TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
379 
380  struct DSRTDebugInfo
381  {
382  StringFloatDictionary desired_torques;
383  float desiredForce_x;
384  float desiredForce_y;
385  float desiredForce_z;
386  float tcpDesiredTorque_x;
387  float tcpDesiredTorque_y;
388  float tcpDesiredTorque_z;
389 
390  float tcpDesiredAngularError_x;
391  float tcpDesiredAngularError_y;
392  float tcpDesiredAngularError_z;
393 
394  float currentTCPAngularVelocity_x;
395  float currentTCPAngularVelocity_y;
396  float currentTCPAngularVelocity_z;
397 
398  float currentTCPLinearVelocity_x;
399  float currentTCPLinearVelocity_y;
400  float currentTCPLinearVelocity_z;
401 
402  // force torque sensor in root
403  float left_realForce_x;
404  float left_realForce_y;
405  float left_realForce_z;
406  float right_realForce_x;
407  float right_realForce_y;
408  float right_realForce_z;
409  float left_force_error;
410  float right_force_error;
411 
412  float left_tcpDesiredTorque_x;
413  float left_tcpDesiredTorque_y;
414  float left_tcpDesiredTorque_z;
415  float right_tcpDesiredTorque_x;
416  float right_tcpDesiredTorque_y;
417  float right_tcpDesiredTorque_z;
418  };
419 
420  TripleBuffer<DSRTDebugInfo> debugDataInfo;
421 
422 
423  std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
424  std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
425  std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
426  std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
427 
428  std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
429  std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
430  std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
431  std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
432 
433  const SensorValueForceTorque* leftForceTorque;
434  const SensorValueForceTorque* rightForceTorque;
435 
436  std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
437  std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
438 
439 
440  VirtualRobot::RobotNodePtr left_arm_tcp;
441  VirtualRobot::RobotNodePtr right_arm_tcp;
442 
443  VirtualRobot::RobotNodePtr left_sensor_frame;
444  VirtualRobot::RobotNodePtr right_sensor_frame;
445 
446  VirtualRobot::DifferentialIKPtr left_ik;
447  Eigen::MatrixXf left_jacobip;
448  Eigen::MatrixXf left_jacobio;
449 
450  VirtualRobot::DifferentialIKPtr right_ik;
451  Eigen::MatrixXf right_jacobip;
452  Eigen::MatrixXf right_jacobio;
453 
454  Eigen::Quaternionf left_desiredQuaternion;
455  Eigen::Vector3f left_oldPosition;
456  Eigen::Matrix3f left_oldOrientation;
457 
458  Eigen::Quaternionf right_desiredQuaternion;
459  Eigen::Vector3f right_oldPosition;
460  Eigen::Matrix3f right_oldOrientation;
461 
462  Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
463  Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
464 
465  Eigen::VectorXf left_jointVelocity_filtered;
466  Eigen::VectorXf right_jointVelocity_filtered;
467 
468  Eigen::VectorXf left_desiredTorques_filtered;
469  Eigen::VectorXf right_desiredTorques_filtered;
470 
471 
472  Eigen::Vector3f left_tcpDesiredTorque_filtered;
473  Eigen::Vector3f right_tcpDesiredTorque_filtered;
474 
475  Eigen::Vector3f leftForceOffset;
476  Eigen::Vector3f rightForceOffset;
477 
478  float left_contactForceZ_correction;
479  float right_contactForceZ_correction;
480 
481  float smooth_startup;
482 
483  DSRTBimanualControllerConfigPtr cfg;
484 
485  Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
486  Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
487 
488  float filterTimeConstant;
489 
490  std::vector<std::string> left_jointNames;
491  std::vector<std::string> right_jointNames;
492 
493  float posiKp;
494  float v_max;
495  std::vector<float> Damping;
496  float torqueLimit;
497  float null_torqueLimit;
498 
499  float Coupling_stiffness;
500  float Coupling_force_limit;
501 
502  float forward_gain;
503 
504  float oriKp;
505  float oriDamping;
506 
507  float nullspaceKp;
508  float nullspaceDamping;
509 
510  float contactForce;
511 
512  float guardTargetZUp;
513  float guardTargetZDown;
514  float guardDesiredZ;
515  float guard_mounting_correction_z;
516 
517  float guardXYHighFrequency;
518  Eigen::Vector3f left_force_old;
519  Eigen::Vector3f right_force_old;
520 
521  Eigen::VectorXf left_qnullspace;
522  Eigen::VectorXf right_qnullspace;
523 
524  Eigen::Quaternionf left_oriUp;
525  Eigen::Quaternionf left_oriDown;
526  Eigen::Quaternionf right_oriUp;
527  Eigen::Quaternionf right_oriDown;
528 
529  // std::vector<BimanualGMMMotionGenPtr> BimanualGMMMotionGenList;
530 
531 
532  float forceFilterCoeff;
533 
534  float positionErrorTolerance;
535  bool controllerStopRequested = false;
536  bool controllerRunFinished = false;
537 
538  bool isDefaultTarget = true;
539 
540  /// intermediate variables
541  Eigen::Vector6f left_tcptwist;
542  Eigen::Vector6f right_tcptwist;
543 
544  Eigen::Matrix4f left_currentTCPPose;
545  Eigen::Matrix4f right_currentTCPPose;
546 
547  unsigned int nJointLeft = 0;
548  unsigned int nJointRight = 0;
549 
550  Eigen::MatrixXf left_jacobi;
551  Eigen::MatrixXf right_jacobi;
552 
553  Eigen::VectorXf left_qpos;
554  Eigen::VectorXf left_qvel;
555 
556  Eigen::VectorXf right_qpos;
557  Eigen::VectorXf right_qvel;
558 
559  Eigen::Vector3f left_forceInRoot;
560  Eigen::Vector3f right_forceInRoot;
561 
562  double deltaT = 0.0;
563 
564  Eigen::Vector3f left_DS_force;
565  Eigen::Vector3f right_DS_force;
566  Eigen::Vector3f coupling_force;
567 
568  Eigen::Vector3f left_tcpDesiredForce;
569  Eigen::Vector3f right_tcpDesiredForce;
570  Eigen::Vector3f left_tcpDesiredTorque;
571  Eigen::Vector3f right_tcpDesiredTorque;
572 
573  Eigen::VectorXf left_nullspaceTorque;
574  Eigen::VectorXf right_nullspaceTorque;
575 
576  Eigen::MatrixXf I;
577 
578  Eigen::Vector6f left_tcpDesiredWrench;
579  Eigen::Vector6f right_tcpDesiredWrench;
580 
581  Eigen::VectorXf left_jointDesiredTorques;
582  Eigen::VectorXf right_jointDesiredTorques;
583 
584  Eigen::MatrixXf left_jtpinv;
585  Eigen::MatrixXf right_jtpinv;
586 
587  Eigen::VectorXf left_nullqerror;
588  Eigen::VectorXf right_nullqerror;
589 
590  // NJointController interface
591  protected:
592  void onPublish(const SensorAndControl&,
595 
596  // NJointController interface
597  protected:
600  };
601 
602 } // namespace armarx::control::ds_controller
603 
604 #endif
armarx::control::ds_controller::BimanualGMMMotionGen::left_DS_DesiredVelocity
Eigen::Vector3f left_DS_DesiredVelocity
Definition: DSRTBimanualController.h:89
armarx::control::ds_controller::BimanualGMMMotionGenPtr
boost::shared_ptr< BimanualGMMMotionGen > BimanualGMMMotionGenPtr
Definition: DSRTBimanualController.h:296
armarx::control::ds_controller::DSRTBimanualController::DSRTBimanualController
DSRTBimanualController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: DSRTBimanualController.cpp:74
armarx::control::ds_controller::DSRTBimanualController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: DSRTBimanualController.cpp:540
armarx::control::ds_controller::DSRTBimanualControllerControlData::right_tcpDesiredLinearVelocity
Eigen::Vector3f right_tcpDesiredLinearVelocity
Definition: DSRTBimanualController.h:49
armarx::control::ds_controller::DSRTBimanualControllerControlData::left_tcpDesiredAngularError
Eigen::Vector3f left_tcpDesiredAngularError
Definition: DSRTBimanualController.h:47
armarx::control::ds_controller::BimanualGMRParameters::Sigma_
std::vector< double > Sigma_
Definition: DSRTBimanualController.h:63
boost::shared_ptr< GMRDynamics >
armarx::control::ds_controller::BimanualGMMMotionGen::leftarm_gmm
BimanualGMMPtr leftarm_gmm
Definition: DSRTBimanualController.h:80
Vector
Eigen::Matrix< T, 3, 1 > Vector
Definition: UnscentedKalmanFilterTest.cpp:39
JSONObject.h
armarx::NJointControllerWithTripleBuffer
Definition: NJointControllerWithTripleBuffer.h:10
RobotUnit.h
armarx::control::ds_controller::BimanualGMRParameters::dt_
double dt_
Definition: DSRTBimanualController.h:65
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::control::ds_controller::DSRTBimanualControllerControlData::left_tcpDesiredLinearVelocity
Eigen::Vector3f left_tcpDesiredLinearVelocity
Definition: DSRTBimanualController.h:46
armarx::control::ds_controller::BimanualGMMMotionGen::rightarm_gmm
BimanualGMMPtr rightarm_gmm
Definition: DSRTBimanualController.h:81
armarx::JSONObject
The JSONObject class is used to represent and (de)serialize JSON objects.
Definition: JSONObject.h:43
armarx::DebugObserverInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface > DebugObserverInterfacePrx
Definition: JointController.h:44
armarx::control::ds_controller::DSRTBimanualController::controllerRun
void controllerRun()
Definition: DSRTBimanualController.cpp:352
SensorValueForceTorque.h
armarx::control::ds_controller::BimanualGMMMotionGen::v_max
float v_max
Definition: DSRTBimanualController.h:95
armarx::control::ds_controller::BimanualGMMMotionGen::rightarm_Target
Eigen::Vector3f rightarm_Target
Definition: DSRTBimanualController.h:87
armarx::control::ds_controller
Definition: DSJointCarryController.cpp:32
armarx::control::ds_controller::BimanualGMMMotionGen::updateDesiredVelocity
void updateDesiredVelocity(const Eigen::Vector3f &leftarm_PositionInMeter, const Eigen::Vector3f &rightarm_PositionInMeter, float positionErrorToleranceInMeter, float desiredZ, float correction_x, float correction_y, float correction_z)
Definition: DSRTBimanualController.h:187
armarx::NJointControllerBase::ConfigPtrT
NJointControllerConfigPtr ConfigPtrT
Definition: NJointControllerBase.h:587
armarx::control::ds_controller::DSRTBimanualController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: DSRTBimanualController.cpp:1058
armarx::control::ds_controller::DSRTBimanualControllerControlData::right_tcpDesiredAngularError
Eigen::Vector3f right_tcpDesiredAngularError
Definition: DSRTBimanualController.h:50
armarx::SensorAndControl
detail::ControlThreadOutputBufferEntry SensorAndControl
Definition: NJointControllerBase.h:73
armarx::control::ds_controller::DSRTBimanualController::rtPostDeactivateController
void rtPostDeactivateController()
This function is called after the controller is deactivated.
Definition: DSRTBimanualController.cpp:1105
armarx::control::ds_controller::BimanualGMMMotionGen
Definition: DSRTBimanualController.h:68
armarx::control::ds_controller::BimanualGMMMotionGen::leftarm_Target
Eigen::Vector3f leftarm_Target
Definition: DSRTBimanualController.h:86
IceInternal::Handle< JSONObject >
GMRDynamics
Definition: GMRDynamics.h:21
armarx::control::ds_controller::BimanualGMMMotionGen::scaling
float scaling
Definition: DSRTBimanualController.h:94
armarx::control::ds_controller::DSRTBimanualController
Brief description of class DSRTBimanualController.
Definition: DSRTBimanualController.h:309
armarx::control::ds_controller::BimanualGMRParameters::attractor_
std::vector< double > attractor_
Definition: DSRTBimanualController.h:64
armarx::control::ds_controller::DSRTBimanualController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: DSRTBimanualController.cpp:956
armarx::control::ds_controller::BimanualGMMMotionGen::leftarm_gmmParas
BimanualGMRParameters leftarm_gmmParas
Definition: DSRTBimanualController.h:83
armarx::control::ds_controller::BimanualGMMMotionGen::BimanualGMMMotionGen
BimanualGMMMotionGen()
Definition: DSRTBimanualController.h:71
armarx::control::ds_controller::BimanualGMRParameters
Definition: DSRTBimanualController.h:57
armarx::control::ds_controller::BimanualGMMMotionGen::getGMMParamsFromJsonFile
void getGMMParamsFromJsonFile(const std::string &fileName)
Definition: DSRTBimanualController.h:98
armarx::control::ds_controller::DSRTBimanualController::getClassName
std::string getClassName(const Ice::Current &) const
Definition: DSRTBimanualController.h:332
ControlTarget1DoFActuator.h
armarx::control::ds_controller::DSRTBimanualController::onInitNJointController
void onInitNJointController()
Definition: DSRTBimanualController.cpp:38
armarx::control::ds_controller::DSRTBimanualControllerControlData
Definition: DSRTBimanualController.h:43
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::ds_controller::BimanualGMRParameters::Mu_
std::vector< double > Mu_
Definition: DSRTBimanualController.h:62
NJointController.h
armarx::control::ds_controller::BimanualGMRParameters::dim_
int dim_
Definition: DSRTBimanualController.h:60
armarx::control::ds_controller::BimanualGMMMotionGen::left_right_position_errorInMeter
Eigen::Vector3f left_right_position_errorInMeter
Definition: DSRTBimanualController.h:91
armarx::control::ds_controller::BimanualGMMMotionGen::rightarm_gmmParas
BimanualGMRParameters rightarm_gmmParas
Definition: DSRTBimanualController.h:84
armarx::Quaternion< float, 0 >
IceUtil::Handle< class RobotUnit >
armarx::control::ds_controller::DSRTBimanualControllerControlData::left_right_distanceInMeter
Eigen::Vector3f left_right_distanceInMeter
Definition: DSRTBimanualController.h:52
Eigen::Matrix< float, 6, 1 >
armarx::control::ds_controller::BimanualGMMMotionGen::BimanualGMMMotionGen
BimanualGMMMotionGen(const std::string &fileName)
Definition: DSRTBimanualController.h:75
GMRDynamics.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::control::ds_controller::BimanualGMMPtr
boost::shared_ptr< GMRDynamics > BimanualGMMPtr
Definition: DSRTBimanualController.h:55
armarx::DebugDrawerInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition: JointController.h:40
armarx::control::ds_controller::BimanualGMRParameters::K_gmm_
int K_gmm_
Definition: DSRTBimanualController.h:59
SensorValue1DoFActuator.h
armarx::control::ds_controller::BimanualGMRParameters::Priors_
std::vector< double > Priors_
Definition: DSRTBimanualController.h:61
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::DSBimanualControllerInterface
Definition: DSControllerInterface.ice:128
armarx::DSBimanualControllerInterface::setToDefaultTarget
void setToDefaultTarget()
armarx::control::ds_controller::DSRTBimanualController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: DSRTBimanualController.cpp:63
armarx::TripleBuffer< DSRTBimanualControllerSensorData >
armarx::control::ds_controller::BimanualGMMMotionGen::right_DS_DesiredVelocity
Eigen::Vector3f right_DS_DesiredVelocity
Definition: DSRTBimanualController.h:90