QPConstr.h
Go to the documentation of this file.
1 /*
2  * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
3  */
4 
5 #pragma once
6 
7 // includes
8 // Eigen
9 #include <Eigen/Core>
10 #include <Eigen/StdVector>
11 
12 // RBDyn
13 #include <RBDyn/CoM.h>
14 #include <RBDyn/Jacobian.h>
15 
16 // sch
17 #include <sch/Matrix/SCH_Types.h>
18 
19 // Tasks
20 #include "QPSolver.h"
21 
22 // unique_ptr
23 #include <memory>
24 
25 // forward declaration
26 // sch
27 namespace sch
28 {
29 class S_Object;
30 class CD_Pair;
31 } // namespace sch
32 
33 namespace tasks
34 {
35 struct QBound;
36 struct AlphaBound;
37 struct AlphaDBound;
38 struct AlphaDDBound;
39 
40 namespace qp
41 {
42 
44 TASKS_DLLAPI sch::Matrix4x4 tosch(const sva::PTransformd & t);
45 
56 class TASKS_DLLAPI JointLimitsConstr : public ConstraintFunction<Bound>
57 {
58 public:
65  JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs, int robotIndex, QBound bound, double step);
66 
74  JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
75  int robotIndex,
76  QBound bound,
77  const AlphaDBound & aDBound,
78  double step);
79 
88  JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
89  int robotIndex,
90  QBound bound,
91  const AlphaDBound & aDBound,
92  const AlphaDDBound & aDDBound,
93  double step);
94 
95  // Constraint
96  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
97 
98  virtual void update(const std::vector<rbd::MultiBody> & mbs,
99  const std::vector<rbd::MultiBodyConfig> & mbcs,
100  const SolverData & data) override;
101 
102  virtual std::string nameBound() const override;
103  virtual std::string descBound(const std::vector<rbd::MultiBody> & mbs, int line) override;
104 
105  // Bound Constraint
106  virtual int beginVar() const override;
107 
108  virtual const Eigen::VectorXd & Lower() const override;
109  virtual const Eigen::VectorXd & Upper() const override;
110 
111 private:
112  int robotIndex_, alphaDBegin_, alphaDOffset_;
113  double step_;
114  Eigen::VectorXd qMin_, qMax_;
115  Eigen::VectorXd qVec_, alphaVec_;
116  Eigen::VectorXd lower_, upper_;
117  Eigen::VectorXd alphaDLower_, alphaDUpper_;
118  Eigen::VectorXd alphaDDLower_, alphaDDUpper_;
119  Eigen::VectorXd prevAlphaD_;
120 };
121 
151 class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction<Bound>
152 {
153 public:
164  DamperJointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
165  int robotIndex,
166  const QBound & qBound,
167  const AlphaBound & aBound,
168  double interPercent,
169  double securityPercent,
170  double damperOffset,
171  double step);
183  DamperJointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
184  int robotIndex,
185  const QBound & qBound,
186  const AlphaBound & aBound,
187  const AlphaDBound & aDBound,
188  double interPercent,
189  double securityPercent,
190  double damperOffset,
191  double step);
192 
205  DamperJointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
206  int robotIndex,
207  const QBound & qBound,
208  const AlphaBound & aBound,
209  const AlphaDBound & aDBound,
210  const AlphaDDBound & aDDBound,
211  double interPercent,
212  double securityPercent,
213  double damperOffset,
214  double step);
215 
216  // Constraint
217  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
218 
219  virtual void update(const std::vector<rbd::MultiBody> & mbs,
220  const std::vector<rbd::MultiBodyConfig> & mbcs,
221  const SolverData & data) override;
222 
223  virtual std::string nameBound() const override;
224  virtual std::string descBound(const std::vector<rbd::MultiBody> & mbs, int line) override;
225 
226  // Bound Constraint
227  virtual int beginVar() const override;
228 
229  virtual const Eigen::VectorXd & Lower() const override;
230  virtual const Eigen::VectorXd & Upper() const override;
231 
233  double computeDamping(double alpha, double dist, double iDist, double sDist);
234  double computeDamper(double dist, double iDist, double sDist, double damping);
235 
236 private:
237  struct DampData
238  {
239  enum State
240  {
241  Low,
242  Upp,
243  Free
244  };
245 
246  DampData(double mi, double ma, double miV, double maV, double idi, double sdi, int aDB, int i)
247  : min(mi), max(ma), minVel(miV), maxVel(maV), iDist(idi), sDist(sdi), jointIndex(i), alphaDBegin(aDB), damping(0.),
248  state(Free)
249  {
250  }
251 
252  double min, max;
253  double minVel, maxVel;
254  double iDist, sDist;
255  int jointIndex;
256  int alphaDBegin;
257  double damping;
258  State state;
259  };
260 
261 private:
262  int robotIndex_, alphaDBegin_;
263  std::vector<DampData> data_;
264 
265  Eigen::VectorXd lower_, upper_;
266  Eigen::VectorXd alphaDLower_, alphaDUpper_;
267  Eigen::VectorXd alphaDDLower_, alphaDDUpper_;
268  Eigen::VectorXd prevAlphaD_;
269  double step_;
270  double damperOff_;
271 };
272 
288 class TASKS_DLLAPI CollisionConstr : public ConstraintFunction<Inequality>
289 {
290 public:
295  CollisionConstr(const std::vector<rbd::MultiBody> & mbs, double step);
296 
324  void addCollision(const std::vector<rbd::MultiBody> & mbs,
325  int collId,
326  int r1Index,
327  const std::string & r1BodyName,
328  sch::S_Object * body1,
329  const sva::PTransformd & X_op1_o1,
330  int r2Index,
331  const std::string & r2BodyName,
332  sch::S_Object * body2,
333  const sva::PTransformd & X_op2_o2,
334  double di,
335  double ds,
336  double damping,
337  double dampingOff = 0.,
338  const Eigen::VectorXd & r1Selector = Eigen::VectorXd::Zero(0),
339  const Eigen::VectorXd & r2Selector = Eigen::VectorXd::Zero(0));
340 
347  bool rmCollision(int collId);
348 
350  std::size_t nrCollisions() const;
351 
353  void reset();
354 
356  void updateNrCollisions();
357 
358  // Constraint
359  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
360 
361  virtual void update(const std::vector<rbd::MultiBody> & mbs,
362  const std::vector<rbd::MultiBodyConfig> & mbcs,
363  const SolverData & data) override;
364 
365  virtual std::string nameInEq() const override;
366  virtual std::string descInEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
367 
368  // In Inequality Constraint
369  virtual int nrInEq() const override;
370  virtual int maxInEq() const override;
371 
372  virtual const Eigen::MatrixXd & AInEq() const override;
373  virtual const Eigen::VectorXd & bInEq() const override;
374 
375 private:
376  struct BodyCollData
377  {
378  BodyCollData(const rbd::MultiBody & mb,
379  int rIndex,
380  const std::string & bodyName,
381  sch::S_Object * hull,
382  const sva::PTransformd & X_op_o,
383  const Eigen::VectorXd & selector);
384 
385  sch::S_Object * hull;
386  rbd::Jacobian jac;
387  sva::PTransformd X_op_o;
388  int rIndex, bIndex;
389  std::string bodyName;
390  Eigen::VectorXd selector;
391  };
392 
393  struct CollData
394  {
395  enum class DampingType
396  {
397  Hard,
398  Soft,
399  Free
400  };
401  CollData(std::vector<BodyCollData> bcds,
402  int collId,
403  sch::S_Object * body1,
404  sch::S_Object * body2,
405  double di,
406  double ds,
407  double damping,
408  double dampingOff);
409  CollData(CollData &&) = default;
410  CollData(const CollData &) = delete;
411  CollData & operator=(const CollData &) = delete;
412  CollData & operator=(CollData &&) = default;
413 
414  std::unique_ptr<sch::CD_Pair> pair;
415  double distance;
416  Eigen::Vector3d p1;
417  Eigen::Vector3d p2;
418  Eigen::Vector3d normVecDist;
419  double di, ds;
420  double damping;
421  std::vector<BodyCollData> bodies;
422 
423  DampingType dampingType;
424  double dampingOff;
425  int collId;
426  };
427 
428 public:
430  const CollData & getCollisionData(int collId) const;
431 
432 private:
433  double computeDamping(const std::vector<rbd::MultiBody> & mbs,
434  const std::vector<rbd::MultiBodyConfig> & mbcs,
435  const CollData & cd,
436  const Eigen::Vector3d & normalVecDist,
437  double dist) const;
438 
439 private:
440  std::vector<CollData> dataVec_;
441  double step_;
442  int nrActivated_, totalAlphaD_;
443 
444  Eigen::MatrixXd AInEq_;
445  Eigen::VectorXd bInEq_;
446 
447  Eigen::MatrixXd fullJac_, distJac_;
448 
449  int nrVars_;
450 
451  CollisionConstr(const CollisionConstr &) = delete;
452  CollisionConstr & operator=(const CollisionConstr &) = delete;
453 };
454 
470 class TASKS_DLLAPI CoMIncPlaneConstr : public ConstraintFunction<Inequality>
471 {
472 public:
478  CoMIncPlaneConstr(const std::vector<rbd::MultiBody> & mbs, int robotIndex, double step);
479 
495  void addPlane(int planeId,
496  const Eigen::Vector3d & normal,
497  double offset,
498  double di,
499  double ds,
500  double damping,
501  double dampingOff = 0.);
502 
503  void addPlane(int planeId,
504  const Eigen::Vector3d & normal,
505  double offset,
506  double di,
507  double ds,
508  double damping,
509  const Eigen::Vector3d & speed,
510  const Eigen::Vector3d & normalDot,
511  double dampingOff = 0.);
512 
519  bool rmPlane(int planeId);
520 
522  std::size_t nrPlanes() const;
523 
525  void reset();
526 
531  inline Eigen::VectorXd & selector() noexcept { return selector_; }
532 
537  inline const Eigen::VectorXd & selector() const noexcept { return selector_; }
538 
540  void updateNrPlanes();
541 
542  // Constraint
543  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
544 
545  virtual void update(const std::vector<rbd::MultiBody> & mbs,
546  const std::vector<rbd::MultiBodyConfig> & mbcs,
547  const SolverData & data) override;
548 
549  virtual std::string nameInEq() const override;
550  virtual std::string descInEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
551 
552  // In Inequality Constraint
553  virtual int nrInEq() const override;
554  virtual int maxInEq() const override;
555 
556  virtual const Eigen::MatrixXd & AInEq() const override;
557  virtual const Eigen::VectorXd & bInEq() const override;
558 
559 private:
560  struct PlaneData
561  {
562  enum class DampingType
563  {
564  Hard,
565  Soft,
566  Free
567  };
568  PlaneData(int planeId,
569  const Eigen::Vector3d & normal,
570  double offset,
571  double di,
572  double ds,
573  double damping,
574  double dampingOff,
575  const Eigen::Vector3d & speed,
576  const Eigen::Vector3d & normalDot);
577  Eigen::Vector3d normal;
578  Eigen::Vector3d normalDot;
579  double offset;
580  double dist;
581  double di, ds;
582  double damping;
583  int planeId;
584  DampingType dampingType;
585  double dampingOff;
586  Eigen::Vector3d speed;
587  };
588 
589 private:
590  int robotIndex_, alphaDBegin_;
591  std::vector<PlaneData> dataVec_;
592  double step_;
593  int nrVars_;
594  int nrActivated_;
595  std::vector<std::size_t> activated_;
596 
597  rbd::CoMJacobian jacCoM_;
598  Eigen::VectorXd selector_;
599  Eigen::MatrixXd AInEq_;
600  Eigen::VectorXd bInEq_;
601 };
602 
603 class TASKS_DLLAPI GripperTorqueConstr : public ConstraintFunction<Inequality>
604 {
605 public:
606  GripperTorqueConstr();
607 
608  void addGripper(const ContactId & cId,
609  double torqueLimit,
610  const Eigen::Vector3d & origin,
611  const Eigen::Vector3d & axis);
612  bool rmGripper(const ContactId & cId);
613  void reset();
614 
615  // Constraint
616  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mb, const SolverData & data) override;
617 
618  virtual void update(const std::vector<rbd::MultiBody> & mb,
619  const std::vector<rbd::MultiBodyConfig> & mbc,
620  const SolverData & data) override;
621 
622  virtual std::string nameInEq() const override;
623  virtual std::string descInEq(const std::vector<rbd::MultiBody> & mb, int line) override;
624 
625  // In Inequality Constraint
626  virtual int maxInEq() const override;
627 
628  virtual const Eigen::MatrixXd & AInEq() const override;
629  virtual const Eigen::VectorXd & bInEq() const override;
630 
631 private:
632  struct GripperData
633  {
634  GripperData(const ContactId & cId, double tl, const Eigen::Vector3d & o, const Eigen::Vector3d & a);
635 
636  ContactId contactId;
637  double torqueLimit;
638  Eigen::Vector3d origin;
639  Eigen::Vector3d axis;
640  };
641 
642 private:
643  std::vector<GripperData> dataVec_;
644 
645  Eigen::MatrixXd AInEq_;
646  Eigen::VectorXd bInEq_;
647 };
648 
661 class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction<GenInequality>
662 {
663 public:
669  BoundedSpeedConstr(const std::vector<rbd::MultiBody> & mbs, int robotIndex, double timeStep);
670 
685  void addBoundedSpeed(const std::vector<rbd::MultiBody> & mbs,
686  const std::string & bodyName,
687  const Eigen::Vector3d & bodyPoint,
688  const Eigen::MatrixXd & dof,
689  const Eigen::VectorXd & speed);
690 
704  void addBoundedSpeed(const std::vector<rbd::MultiBody> & mbs,
705  const std::string & bodyName,
706  const Eigen::Vector3d & bodyPoint,
707  const Eigen::MatrixXd & dof,
708  const Eigen::VectorXd & lowerSpeed,
709  const Eigen::VectorXd & upperSpeed);
710 
717  bool removeBoundedSpeed(const std::string & bodyName);
718 
720  void resetBoundedSpeeds();
721 
723  std::size_t nrBoundedSpeeds() const;
724 
726  void updateBoundedSpeeds();
727 
728  // Constraint
729  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
730 
731  virtual void update(const std::vector<rbd::MultiBody> & mbs,
732  const std::vector<rbd::MultiBodyConfig> & mbc,
733  const SolverData & data) override;
734 
735  virtual std::string nameGenInEq() const override;
736  virtual std::string descGenInEq(const std::vector<rbd::MultiBody> & mb, int line) override;
737 
738  // Inequality Constraint
739  virtual int maxGenInEq() const override;
740 
741  virtual const Eigen::MatrixXd & AGenInEq() const override;
742  virtual const Eigen::VectorXd & LowerGenInEq() const override;
743  virtual const Eigen::VectorXd & UpperGenInEq() const override;
744 
745 private:
746  struct BoundedSpeedData
747  {
748  BoundedSpeedData(rbd::Jacobian j,
749  const Eigen::MatrixXd & d,
750  const Eigen::VectorXd & ls,
751  const Eigen::VectorXd & us,
752  const std::string & bName)
753  : jac(j), bodyPoint(j.point()), dof(d), lSpeed(ls), uSpeed(us), body(j.jointsPath().back()), bodyName(bName)
754  {
755  }
756 
757  rbd::Jacobian jac;
758  sva::PTransformd bodyPoint;
759  Eigen::MatrixXd dof;
760  Eigen::VectorXd lSpeed, uSpeed;
761  int body;
762  std::string bodyName;
763  };
764 
765 private:
766  void updateNrEq();
767 
768 private:
769  int robotIndex_, alphaDBegin_;
770  std::vector<BoundedSpeedData> cont_;
771 
772  Eigen::MatrixXd fullJac_;
773 
774  Eigen::MatrixXd A_;
775  Eigen::VectorXd lower_, upper_;
776 
777  int nrVars_;
778  double timeStep_;
779 };
780 
787 class TASKS_DLLAPI ImageConstr : public ConstraintFunction<Inequality>
788 {
789 public:
800  ImageConstr(const std::vector<rbd::MultiBody> & mbs,
801  int robotIndex,
802  const std::string & bName,
803  const sva::PTransformd & X_b_gaze,
804  double step,
805  double constrDirection = 1.);
806 
808  ImageConstr(const ImageConstr & rhs);
809 
811  ImageConstr(ImageConstr &&) = default;
812 
814  ImageConstr & operator=(const ImageConstr & rhs);
815 
817  ImageConstr & operator=(ImageConstr &&) = default;
818 
827  void setLimits(const Eigen::Vector2d & min,
828  const Eigen::Vector2d & max,
829  const double iPercent,
830  const double sPercent,
831  const double damping,
832  const double dampingOffsetPercent);
833 
840  int addPoint(const Eigen::Vector2d & point2d, const double depthEstimate);
841  int addPoint(const Eigen::Vector3d & point3d);
842 
849  void addPoint(const std::vector<rbd::MultiBody> & mbs,
850  const std::string & bName,
851  const sva::PTransformd & X_b_p = sva::PTransformd::Identity());
852  void reset();
853  void updatePoint(const int pointId, const Eigen::Vector2d & point2d);
854  void updatePoint(const int pointId, const Eigen::Vector2d & point2d, const double depthEstimate);
855  void updatePoint(const int pointId, const Eigen::Vector3d & point3d);
856 
857  void computeComponents(const rbd::MultiBody & mb,
858  const rbd::MultiBodyConfig & mbc,
859  const SolverData & data,
860  const Eigen::Vector2d & point2d,
861  const double depth,
862  rbd::Jacobian & jac,
863  const int bodyIndex,
864  const sva::PTransformd & X_b_p,
865  Eigen::MatrixXd & fullJacobian,
866  Eigen::Vector2d & bCommonTerm);
867 
868  // Constraint
869  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
870 
871  virtual void update(const std::vector<rbd::MultiBody> & mbs,
872  const std::vector<rbd::MultiBodyConfig> & mbcs,
873  const SolverData & data) override;
874 
875  // In Inequality Constraint
876  virtual std::string nameInEq() const override;
877  virtual std::string descInEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
878  virtual int nrInEq() const override;
879  virtual int maxInEq() const override;
880 
881  virtual const Eigen::MatrixXd & AInEq() const override;
882  virtual const Eigen::VectorXd & bInEq() const override;
883 
884 private:
885  struct PointData
886  {
887  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
888  PointData(const Eigen::Vector2d & pt, const double d);
889  Eigen::Vector2d point2d;
890  double depthEstimate;
891  };
892  struct RobotPointData
893  {
894  RobotPointData(const std::string & bName, const sva::PTransformd & X, const rbd::Jacobian & j);
895  std::string bName;
896  sva::PTransformd X_b_p;
897  rbd::Jacobian jac;
898  };
899 
900 private:
901  std::vector<PointData, Eigen::aligned_allocator<PointData>> dataVec_;
902  std::vector<RobotPointData> dataVecRob_;
903  int robotIndex_, bodyIndex_, alphaDBegin_;
904  int nrVars_;
905  double step_, accelFactor_;
906  int nrActivated_;
907 
908  rbd::Jacobian jac_;
909  sva::PTransformd X_b_gaze_;
910  std::unique_ptr<Eigen::Matrix<double, 2, 6>> L_img_;
911  std::unique_ptr<Eigen::Matrix<double, 6, 1>> surfaceVelocity_;
912  std::unique_ptr<Eigen::Matrix<double, 1, 6>> L_Z_dot_;
913  std::unique_ptr<Eigen::Matrix<double, 2, 6>> L_img_dot_;
914  std::unique_ptr<Eigen::Vector2d> speed_;
915  std::unique_ptr<Eigen::Vector2d> normalAcc_;
916  Eigen::MatrixXd jacMat_;
917  std::unique_ptr<Eigen::Vector2d> iDistMin_, iDistMax_, sDistMin_, sDistMax_;
918  double damping_, dampingOffset_, ineqInversion_, constrDirection_;
919  Eigen::MatrixXd AInEq_;
920  Eigen::VectorXd bInEq_;
921 };
922 
923 } // namespace qp
924 
925 } // namespace tasks
tasks::qp::ContactId
Definition: QPContacts.h:49
tasks::qp::GripperTorqueConstr
Definition: QPConstr.h:603
tasks::qp::CoMIncPlaneConstr::selector
const Eigen::VectorXd & selector() const noexcept
Definition: QPConstr.h:537
sch
Definition: QPConstr.h:27
tasks::qp::CoMIncPlaneConstr::selector
Eigen::VectorXd & selector() noexcept
Definition: QPConstr.h:531
tasks::qp::BoundedSpeedConstr
Definition: QPConstr.h:661
tasks::AlphaDDBound
Definition: Bounds.h:76
tasks::qp::JointLimitsConstr
Definition: QPConstr.h:56
tasks::qp::SolverData
Definition: QPSolverData.h:27
tasks::AlphaDBound
Definition: Bounds.h:58
tasks::QBound
Definition: Bounds.h:22
tasks::qp::DamperJointLimitsConstr
Definition: QPConstr.h:151
tasks::qp::ImageConstr
Definition: QPConstr.h:787
tasks::qp::tosch
TASKS_DLLAPI sch::Matrix4x4 tosch(const sva::PTransformd &t)
Convert a sch-core transformation matrix to a sva::PTransformd matrix.
tasks::qp::CollisionConstr
Definition: QPConstr.h:288
tasks::AlphaBound
Definition: Bounds.h:40
tasks
Definition: GenQPUtils.h:18
tasks::qp::CoMIncPlaneConstr
Definition: QPConstr.h:470
tasks::qp::ConstraintFunction
Definition: QPSolver.h:163
QPSolver.h