Drake
rigid_body_constraint.h
Go to the documentation of this file.
1 #pragma once
2 
3 // TODO(#2274) NOTE This file has so many cpplint errors that we have
4 // whitelisted it in its entirety. When the file is next rewritten or updates,
5 // we should re-enable cpplint accordingly.
6 
7 #include <set>
8 #include <string>
9 #include <vector>
10 
11 #include <Eigen/Dense>
12 #include <Eigen/SparseCore>
13 
16 
17 namespace DrakeRigidBodyConstraint {
19 }
20 
28  public:
29  /* In each category, constraint classes share the same function interface,
30  * this value needs to be in consistent with that in MATLAB*/
31  static const int SingleTimeKinematicConstraintCategory = -1;
32  static const int MultipleTimeKinematicConstraintCategory = -2;
33  static const int QuasiStaticConstraintCategory = -3;
34  static const int PostureConstraintCategory = -4;
35  static const int MultipleTimeLinearPostureConstraintCategory = -5;
36  static const int SingleTimeLinearPostureConstraintCategory = -6;
37  /* Each non-abstrac RigidBodyConstraint class has a unique type. Make sure
38  * this value stays in consistent with the value in MATLAB*/
39  static const int QuasiStaticConstraintType = 1;
40  static const int PostureConstraintType = 2;
41  static const int SingleTimeLinearPostureConstraintType = 3;
42  static const int AllBodiesClosestDistanceConstraintType = 4;
43  static const int WorldEulerConstraintType = 5;
44  static const int WorldGazeDirConstraintType = 6;
45  static const int WorldGazeOrientConstraintType = 7;
46  static const int WorldGazeTargetConstraintType = 8;
47  static const int RelativeGazeTargetConstraintType = 9;
48  static const int WorldCoMConstraintType = 10;
49  static const int WorldPositionConstraintType = 11;
50  static const int WorldPositionInFrameConstraintType = 12;
51  static const int WorldQuatConstraintType = 13;
52  static const int Point2PointDistanceConstraintType = 14;
53  static const int Point2LineSegDistConstraintType = 15;
54  static const int WorldFixedPositionConstraintType = 16;
55  static const int WorldFixedOrientConstraintType = 17;
56  static const int WorldFixedBodyPoseConstraintType = 18;
57  static const int PostureChangeConstraintType = 19;
58  static const int RelativePositionConstraintType = 20;
59  static const int RelativeQuatConstraintType = 24;
60  static const int RelativeGazeDirConstraintType = 25;
61  static const int MinDistanceConstraintType = 26;
62  static const int GravityCompensationTorqueConstraintType = 27;
63 
65  int category, RigidBodyTree<double>* robot,
67  int getType() const { return type_; }
68  int getCategory() const { return category_; }
70  virtual ~RigidBodyConstraint(void) = 0;
71 
72  protected:
73  std::string getTimeString(const double* t) const;
74  void set_type(int type) { type_ = type; }
75  void set_robot(RigidBodyTree<double>* robot) { robot_ = robot; }
76  const double* tspan() const { return tspan_; }
77 
78  private:
79  int category_{};
80  int type_{};
82  double tspan_[2];
83 };
84 
118  public:
120  RigidBodyTree<double>* robot,
122  const std::set<int>& model_instance_id_set =
123  QuasiStaticConstraint::defaultRobotNumSet);
124  virtual ~QuasiStaticConstraint(void);
125  bool isTimeValid(const double* t) const;
126  int getNumConstraint(const double* t) const;
127  void eval(const double* t, KinematicsCache<double>& cache,
128  const double* weights, Eigen::VectorXd& c,
129  Eigen::MatrixXd& dc) const;
130  void bounds(const double* t, Eigen::VectorXd& lb, Eigen::VectorXd& ub) const;
131  void name(const double* t, std::vector<std::string>& name_str) const;
132  bool isActive() const { return active_; }
133  int getNumWeights() const { return num_pts_; }
134  void addContact(int num_new_bodies, const int* body,
135  const Eigen::Matrix3Xd* body_pts);
136 
137  void addContact(std::vector<int> body, const Eigen::Matrix3Xd& body_pts) {
138  addContact(body.size(), body.data(), &body_pts);
139  }
140 
141  void setShrinkFactor(double factor);
142  void setActive(bool flag) { active_ = flag; }
143  void updateRobot(RigidBodyTree<double>* robot);
144  void updateRobotnum(std::set<int>& model_instance_id_set);
145 
146  private:
147  static const std::set<int> defaultRobotNumSet;
148  std::set<int> m_model_instance_id_set_;
149  double shrink_factor_{};
150  bool active_{};
151  int num_bodies_{};
152  int num_pts_{};
153  std::vector<int> bodies_;
154  std::vector<int> num_body_pts_;
155  std::vector<Eigen::Matrix3Xd> body_pts_;
156 };
157 
158 /*
159  * @class PostureConstraint constrain the joint limits
160  * @param tspan -- The time span of the constraint being valid
161  * @param lb -- The lower bound of the joints
162  * @param ub -- The upper bound of the joints
163  *
164  * @function setJointLimits set the limit of some joints
165  * @param num_idx The number of joints whose limits are going to be set
166  * @param joint_idx joint_idx[i] is the index of the i'th joint whose limits
167  * are going to be set
168  * @param lb lb[i] is the lower bound of the joint joint_idx[i]
169  * @param ub ub[i] is the upper bound of the joint joint_idx[i]
170  */
172  public:
174  RigidBodyTree<double>* model,
176  virtual ~PostureConstraint(void) {}
177  bool isTimeValid(const double* t) const;
178  void setJointLimits(int num_idx, const int* joint_idx,
179  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub);
180  void setJointLimits(const Eigen::VectorXi& joint_idx,
181  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub);
182  void bounds(const double* t, Eigen::VectorXd& joint_min,
183  Eigen::VectorXd& joint_max) const;
184 
185  private:
186  Eigen::VectorXd joint_limit_min0_;
187  Eigen::VectorXd joint_limit_max0_;
188  Eigen::VectorXd lb_;
189  Eigen::VectorXd ub_;
190 };
191 
192 /*
193  * @class MultipleTimeLinearPostureConstraint constrain the posture such that
194  * lb(t(1), t(2),..., t(n)) <=
195  * A_mat(t(1), t(2), t(n))*[q(t(1));q(t(2));...;q(t(n))] <=
196  * ub(t(1), t(2),..., t(n))
197  * where A_mat is a sparse matrix that only depends on t(1), t(2),..., t(n)
198  *
199  * @function eval return the value and gradient of the constraint
200  * @param t array of time
201  * @param n_breaks the length of array t
202  * @param q q.col(i) is the posture at t[i]
203  * @param c the value of the constraint
204  * @param dc the gradient of the constraint w.r.t. q
205  *
206  * @function feval returns the value of the constraint
207  *
208  * @function geval returns the gradient of the constraint, written in the sprase
209  * matrix form
210  * @return iAfun The row index of the non-zero entries in the gradient
211  * matrix
212  * @return jAvar The column index of the non-zero entries in the gradient
213  * matrix
214  * @return A The value of the non-zero entries in the gradient matrix
215  */
217  public:
219  RigidBodyTree<double>* model,
222  std::vector<bool> isTimeValid(const double* t, int n_breaks) const;
223  void eval(const double* t, int n_breaks, const Eigen::MatrixXd& q,
224  Eigen::VectorXd& c, Eigen::SparseMatrix<double>& dc) const;
225  virtual int getNumConstraint(const double* t, int n_breaks) const = 0;
226  virtual void feval(const double* t, int n_breaks, const Eigen::MatrixXd& q,
227  Eigen::VectorXd& c) const = 0;
228  virtual void geval(const double* t, int n_breaks, Eigen::VectorXi& iAfun,
229  Eigen::VectorXi& jAvar, Eigen::VectorXd& A) const = 0;
230  virtual void name(const double* t, int n_breaks,
231  std::vector<std::string>& name_str) const = 0;
232  virtual void bounds(const double* t, int n_breaks, Eigen::VectorXd& lb,
233  Eigen::VectorXd& ub) const = 0;
234 
235  protected:
236  int numValidTime(const std::vector<bool>& valid_flag) const;
237  void validTimeInd(const std::vector<bool>& valid_flag,
238  Eigen::VectorXi& valid_t_ind) const;
239 };
240 
241 /*
242  * @class SingleTimeLinearPostureConstraint constrain the posture satisfies lb<=
243  * A_mat*q <=ub at any time, where A_mat is a sparse matrix
244  *
245  * @function SingleTimeLinearPostureConstraint
246  * @param robot
247  * @param iAfun The row indices of non zero entries
248  * @param jAvar The column indices of non zero entries
249  * @param A The values of non zero entries
250  * @param lb The lower bound of the constraint, a column vector.
251  * @param ub The upper bound of the constraint, a column vector.
252  * @param tspan The time span [tspan[0] tspan[1]] is the time span of the
253  * constraint being active
254  * @function eval return the value and gradient of the constraint
255  * @param t array of time
256  * @param n_breaks the length of array t
257  * @param q q.col(i) is the posture at t[i]
258  * @param c the value of the constraint
259  * @param dc the gradient of the constraint w.r.t. q
260  *
261  * @function feval returns the value of the constraint
262  *
263  * @function geval returns the gradient of the constraint, written in the sprase
264  * matrix form
265  * @return iAfun The row index of the non-zero entries in the gradient
266  * matrix
267  * @return jAvar The column index of the non-zero entries in the gradient
268  * matrix
269  * @return A The value of the non-zero entries in the gradient matrix
270  */
272  public:
274  RigidBodyTree<double>* robot, const Eigen::VectorXi& iAfun,
275  const Eigen::VectorXi& jAvar, const Eigen::VectorXd& A,
276  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub,
279  bool isTimeValid(const double* t) const;
280  int getNumConstraint(const double* t) const;
281  void bounds(const double* t, Eigen::VectorXd& lb, Eigen::VectorXd& ub) const;
282  void feval(const double* t, const Eigen::VectorXd& q,
283  Eigen::VectorXd& c) const;
284  void geval(const double* t, Eigen::VectorXi& iAfun, Eigen::VectorXi& jAvar,
285  Eigen::VectorXd& A) const;
286  void eval(const double* t, const Eigen::VectorXd& q, Eigen::VectorXd& c,
287  Eigen::SparseMatrix<double>& dc) const;
288  void name(const double* t, std::vector<std::string>& name_str) const;
289 
290  private:
291  Eigen::VectorXi iAfun_;
292  Eigen::VectorXi jAvar_;
293  Eigen::VectorXd A_;
294  Eigen::VectorXd lb_;
295  Eigen::VectorXd ub_;
296  int num_constraint_{};
297  Eigen::SparseMatrix<double> A_mat_;
298 };
299 
300 /*
301  * class SingleTimeKinematicConstraint An abstract class that constrain the
302  * kinematics of the robot at individual time. Need to call doKinematics first
303  * for the robot and then evaulate this constraint.
304  */
306  public:
308  RigidBodyTree<double>* model,
311  bool isTimeValid(const double* t) const;
312  int getNumConstraint(const double* t) const;
313  virtual void eval(const double* t, KinematicsCache<double>& cache,
314  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const = 0;
315  virtual void bounds(const double* t, Eigen::VectorXd& lb,
316  Eigen::VectorXd& ub) const = 0;
317  virtual void name(const double* t,
318  std::vector<std::string>& name_str) const = 0;
319  virtual void updateRobot(RigidBodyTree<double>* robot);
320 
321  protected:
322  void set_num_constraint(int num_constraint) {
323  num_constraint_ = num_constraint;
324  }
325 
326  private:
327  int num_constraint_{};
328 };
329 
331  public:
333  RigidBodyTree<double>* model,
336  std::vector<bool> isTimeValid(const double* t, int n_breaks) const;
337  virtual int getNumConstraint(const double* t, int n_breaks) const = 0;
338  void eval(const double* t, int n_breaks, const Eigen::MatrixXd& q,
339  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
340  virtual void eval_valid(const double* valid_t, int num_valid_t,
341  const Eigen::MatrixXd& valid_q, Eigen::VectorXd& c,
342  Eigen::MatrixXd& dc_valid) const = 0;
343  virtual void bounds(const double* t, int n_breaks, Eigen::VectorXd& lb,
344  Eigen::VectorXd& ub) const = 0;
345  virtual void name(const double* t, int n_breaks,
346  std::vector<std::string>& name_str) const = 0;
347  virtual void updateRobot(RigidBodyTree<double>* robot);
348 
349  protected:
350  int numValidTime(const double* t, int n_breaks) const;
351 };
352 
354  public:
356  RigidBodyTree<double>* model, const Eigen::Matrix3Xd& pts,
357  Eigen::MatrixXd lb, Eigen::MatrixXd ub,
359  virtual ~PositionConstraint(void) {}
360  virtual void eval(const double* t, KinematicsCache<double>& cache,
361  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
362  virtual void bounds(const double* t, Eigen::VectorXd& lb,
363  Eigen::VectorXd& ub) const;
364  virtual void name(const double* t, std::vector<std::string>& name_str) const;
365 
366  protected:
367  virtual void evalPositions(KinematicsCache<double>& cache,
368  Eigen::Matrix3Xd& pos,
369  Eigen::MatrixXd& J) const = 0;
370  virtual void evalNames(const double* t,
371  std::vector<std::string>& cnst_names) const = 0;
372  const Eigen::Matrix3Xd& get_pts() const { return pts_; }
373  int get_n_pts() const { return n_pts_; }
374 
375  private:
376  Eigen::VectorXd lb_;
377  Eigen::VectorXd ub_;
378  std::vector<bool> null_constraint_rows_;
379  Eigen::Matrix3Xd pts_;
380  int n_pts_{};
381 };
382 
384  public:
386  RigidBodyTree<double>* model, int body, const Eigen::Matrix3Xd& pts,
387  Eigen::MatrixXd lb, Eigen::MatrixXd ub,
389  virtual ~WorldPositionConstraint();
390 
391  protected:
392  virtual void evalPositions(KinematicsCache<double>& cache,
393  Eigen::Matrix3Xd& pos, Eigen::MatrixXd& J) const;
394  virtual void evalNames(const double* t,
395  std::vector<std::string>& cnst_names) const;
396  int get_body() const { return body_; }
397  const std::string& get_body_name() const { return body_name_; }
398 
399  private:
400  int body_{};
401  std::string body_name_;
402 };
403 
405  public:
407  RigidBodyTree<double>* model, Eigen::Vector3d lb, Eigen::Vector3d ub,
409  const std::set<int>& model_instance_id =
410  WorldCoMConstraint::defaultRobotNumSet);
411  virtual ~WorldCoMConstraint();
412  void updateRobotnum(const std::set<int>& model_instance_id);
413 
414  protected:
415  virtual void evalPositions(KinematicsCache<double>& cache,
416  Eigen::Matrix3Xd& pos, Eigen::MatrixXd& J) const;
417  virtual void evalNames(const double* t,
418  std::vector<std::string>& cnst_names) const;
419 
420  private:
421  static const std::set<int> defaultRobotNumSet;
422 
423  std::set<int> m_model_instance_id_;
424 };
425 
427  public:
429  const Eigen::Matrix3Xd& pts,
430  const Eigen::MatrixXd& lb,
431  const Eigen::MatrixXd& ub, int bodyA_idx,
432  int bodyB_idx,
433  const Eigen::Matrix<double, 7, 1>& bTbp,
434  const Eigen::Vector2d& tspan);
435  virtual ~RelativePositionConstraint();
436 
437  protected:
438  virtual void evalPositions(KinematicsCache<double>& cache,
439  Eigen::Matrix3Xd& pos, Eigen::MatrixXd& J) const;
440  virtual void evalNames(const double* t,
441  std::vector<std::string>& cnst_names) const;
442 
443  private:
444  int bodyA_idx_{};
445  int bodyB_idx_{};
446  std::string bodyA_name_;
447  std::string bodyB_name_;
448  Eigen::Isometry3d bpTb_;
449 };
450 
452  public:
454  RigidBodyTree<double>* model, double tol,
456  virtual ~QuatConstraint();
457  virtual void eval(const double* t, KinematicsCache<double>& cache,
458  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
459  virtual void bounds(const double* t, Eigen::VectorXd& lb,
460  Eigen::VectorXd& ub) const;
461 
462  protected:
463  virtual void evalOrientationProduct(const KinematicsCache<double>& cache,
464  double& prod,
465  Eigen::MatrixXd& dprod) const = 0;
466 
467  private:
468  double tol_{};
469 };
470 
472  public:
474  RigidBodyTree<double>* model, int body, const Eigen::Vector4d& quat_des,
475  double tol,
477  virtual ~WorldQuatConstraint();
478  virtual void name(const double* t, std::vector<std::string>& name_str) const;
479 
480  protected:
481  virtual void evalOrientationProduct(const KinematicsCache<double>& cache,
482  double& prod,
483  Eigen::MatrixXd& dprod) const;
484 
485  private:
486  int body_{};
487  std::string body_name_;
488  Eigen::Vector4d quat_des_;
489 
490  public:
491  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
492 };
493 
495  public:
497  RigidBodyTree<double>* model, int bodyA_idx, int bodyB_idx,
498  const Eigen::Vector4d& quat_des, double tol,
500  virtual void name(const double* t, std::vector<std::string>& name_str) const;
501  virtual ~RelativeQuatConstraint();
502 
503  protected:
504  virtual void evalOrientationProduct(const KinematicsCache<double>& cache,
505  double& prod,
506  Eigen::MatrixXd& dprod) const;
507 
508  private:
509  int bodyA_idx_{};
510  int bodyB_idx_{};
511  std::string bodyA_name_;
512  std::string bodyB_name_;
513  Eigen::Vector4d quat_des_;
514 
515  public:
516  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
517 };
518 
520  public:
522  RigidBodyTree<double>* model, const Eigen::Vector3d& lb,
523  const Eigen::Vector3d& ub,
525  virtual ~EulerConstraint(void) {}
526  virtual void eval(const double* t, KinematicsCache<double>& cache,
527  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
528  virtual void bounds(const double* t, Eigen::VectorXd& lb,
529  Eigen::VectorXd& ub) const;
530 
531  protected:
532  virtual void evalrpy(const KinematicsCache<double>& cache,
533  Eigen::Vector3d& rpy, Eigen::MatrixXd& J) const = 0;
534  bool null_constraint_row(int row) const { return null_constraint_rows_[row]; }
535 
536  private:
537  Eigen::VectorXd ub_;
538  Eigen::VectorXd lb_;
539  bool null_constraint_rows_[3];
540  Eigen::VectorXd avg_rpy_;
541 };
542 
544  public:
546  RigidBodyTree<double>* model, int body, const Eigen::Vector3d& lb,
547  const Eigen::Vector3d& ub,
549  virtual ~WorldEulerConstraint();
550  virtual void name(const double* t, std::vector<std::string>& name_str) const;
551 
552  protected:
553  virtual void evalrpy(const KinematicsCache<double>& cache,
554  Eigen::Vector3d& rpy, Eigen::MatrixXd& J) const;
555 
556  private:
557  int body_{};
558  std::string body_name_;
559 };
560 
562  public:
564  RigidBodyTree<double>* model, const Eigen::Vector3d& axis,
565  double conethreshold = 0.0,
567  virtual ~GazeConstraint(void) {}
568 
569  protected:
570  double get_conethreshold() const { return conethreshold_; }
571  const Eigen::Vector3d& get_axis() const { return axis_; }
572 
573  private:
574  Eigen::Vector3d axis_;
575  double conethreshold_{};
576 
577  public:
578  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
579 };
580 
582  public:
584  RigidBodyTree<double>* model, const Eigen::Vector3d& axis,
585  const Eigen::Vector4d& quat_des, double conethreshold, double threshold,
587  virtual ~GazeOrientConstraint(void) {}
588  virtual void eval(const double* t, KinematicsCache<double>& cache,
589  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
590  virtual void bounds(const double* t, Eigen::VectorXd& lb,
591  Eigen::VectorXd& ub) const;
592 
593  protected:
594  virtual void evalOrientation(const KinematicsCache<double>& cache,
595  Eigen::Vector4d& quat,
596  Eigen::MatrixXd& dquat_dq) const = 0;
597 
598  private:
599  double threshold_{};
600  Eigen::Vector4d quat_des_;
601 
602  public:
603  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
604 };
605 
607  public:
609  RigidBodyTree<double>* model, int body, const Eigen::Vector3d& axis,
610  const Eigen::Vector4d& quat_des, double conethreshold, double threshold,
613  virtual void name(const double* t, std::vector<std::string>& name_str) const;
614 
615  protected:
616  virtual void evalOrientation(const KinematicsCache<double>& cache,
617  Eigen::Vector4d& quat,
618  Eigen::MatrixXd& dquat_dq) const;
619 
620  private:
621  int body_{};
622  std::string body_name_;
623 };
624 
626  public:
628  RigidBodyTree<double>* model, const Eigen::Vector3d& axis,
629  const Eigen::Vector3d& dir, double conethreshold,
631  virtual ~GazeDirConstraint(void) {}
632  virtual void bounds(const double* t, Eigen::VectorXd& lb,
633  Eigen::VectorXd& ub) const;
634 
635  protected:
636  const Eigen::Vector3d& get_dir() const { return dir_; }
637 
638  private:
639  Eigen::Vector3d dir_;
640 
641  public:
642  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
643 };
644 
646  public:
648  RigidBodyTree<double>* model, int body, const Eigen::Vector3d& axis,
649  const Eigen::Vector3d& dir, double conethreshold,
651  virtual ~WorldGazeDirConstraint(void) {}
652  virtual void eval(const double* t, KinematicsCache<double>& cache,
653  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
654  virtual void name(const double* t, std::vector<std::string>& name_str) const;
655 
656  private:
657  int body_{};
658  std::string body_name_;
659 };
660 
662  public:
664  RigidBodyTree<double>* model, const Eigen::Vector3d& axis,
665  const Eigen::Vector3d& target, const Eigen::Vector3d& gaze_origin,
666  double conethreshold,
668  virtual ~GazeTargetConstraint(void) {}
669  virtual void bounds(const double* t, Eigen::VectorXd& lb,
670  Eigen::VectorXd& ub) const;
671 
672  protected:
673  const Eigen::Vector3d& get_target() const { return target_; }
674  const Eigen::Vector3d& get_gaze_origin() const { return gaze_origin_; }
675 
676  private:
677  Eigen::Vector3d target_;
678  Eigen::Vector3d gaze_origin_;
679 
680  public:
681  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
682 };
683 
685  public:
687  RigidBodyTree<double>* model, int body, const Eigen::Vector3d& axis,
688  const Eigen::Vector3d& target, const Eigen::Vector3d& gaze_origin,
689  double conethreshold,
691  virtual ~WorldGazeTargetConstraint(void) {}
692  virtual void eval(const double* t, KinematicsCache<double>& cache,
693  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
694  virtual void name(const double* t, std::vector<std::string>& name_str) const;
695 
696  private:
697  int body_{};
698  std::string body_name_;
699 };
700 
702  public:
704  RigidBodyTree<double>* model, int bodyA_idx, int bodyB_idx,
705  const Eigen::Vector3d& axis, const Eigen::Vector3d& target,
706  const Eigen::Vector3d& gaze_origin, double conethreshold,
709  virtual void eval(const double* t, KinematicsCache<double>& cache,
710  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
711  virtual void name(const double* t, std::vector<std::string>& name_str) const;
712 
713  private:
714  int bodyA_idx_{};
715  int bodyB_idx_{};
716  std::string bodyA_name_;
717  std::string bodyB_name_;
718 };
719 
721  public:
723  RigidBodyTree<double>* model, int bodyA_idx, int bodyB_idx,
724  const Eigen::Vector3d& axis, const Eigen::Vector3d& dir,
725  double conethreshold,
727  virtual ~RelativeGazeDirConstraint(void) {}
728  virtual void eval(const double* t, KinematicsCache<double>& cache,
729  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
730  virtual void name(const double* t, std::vector<std::string>& name_str) const;
731 
732  private:
733  int bodyA_idx_{};
734  int bodyB_idx_{};
735  std::string bodyA_name_;
736  std::string bodyB_name_;
737 };
738 
740  public:
742  RigidBodyTree<double>* model, int bodyA, int bodyB,
743  const Eigen::Matrix3Xd& ptA, const Eigen::Matrix3Xd& ptB,
744  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub,
747  virtual void eval(const double* t, KinematicsCache<double>& cache,
748  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
749  virtual void name(const double* t, std::vector<std::string>& name_str) const;
750  virtual void bounds(const double* t, Eigen::VectorXd& lb,
751  Eigen::VectorXd& ub) const;
752 
753  private:
754  int bodyA_{};
755  int bodyB_{};
756  Eigen::Matrix3Xd ptA_;
757  Eigen::Matrix3Xd ptB_;
758  Eigen::VectorXd dist_lb_;
759  Eigen::VectorXd dist_ub_;
760 };
761 
763  public:
765  RigidBodyTree<double>* model, int pt_body, const Eigen::Vector3d& pt,
766  int line_body, const Eigen::Matrix<double, 3, 2>& line_ends,
767  double dist_lb, double dist_ub,
770  virtual void eval(const double* t, KinematicsCache<double>& cache,
771  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
772  virtual void name(const double* t, std::vector<std::string>& name_str) const;
773  virtual void bounds(const double* t, Eigen::VectorXd& lb,
774  Eigen::VectorXd& ub) const;
775 
776  private:
777  int pt_body_{};
778  Eigen::Vector3d pt_;
779  int line_body_{};
780  Eigen::Matrix3Xd line_ends_;
781  double dist_lb_{};
782  double dist_ub_{};
783 
784  public:
785  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
786 };
787 
789  public:
791  RigidBodyTree<double>* model, int body, const Eigen::Matrix3Xd& pts,
794  virtual int getNumConstraint(const double* t, int n_breaks) const;
795  virtual void eval_valid(const double* valid_t, int num_valid_t,
796  const Eigen::MatrixXd& valid_q, Eigen::VectorXd& c,
797  Eigen::MatrixXd& dc_valid) const;
798  virtual void bounds(const double* t, int n_breaks, Eigen::VectorXd& lb,
799  Eigen::VectorXd& ub) const;
800  virtual void name(const double* t, int n_breaks,
801  std::vector<std::string>& name_str) const;
802 
803  private:
804  int body_{};
805  std::string body_name_;
806  Eigen::Matrix3Xd pts_;
807 };
808 
810  public:
812  RigidBodyTree<double>* model, int body,
814  virtual int getNumConstraint(const double* t, int n_breaks) const;
816  virtual void eval_valid(const double* valid_t, int num_valid_t,
817  const Eigen::MatrixXd& valid_q, Eigen::VectorXd& c,
818  Eigen::MatrixXd& dc_valid) const;
819  virtual void bounds(const double* t, int n_breaks, Eigen::VectorXd& lb,
820  Eigen::VectorXd& ub) const;
821  virtual void name(const double* t, int n_breaks,
822  std::vector<std::string>& name_str) const;
823 
824  private:
825  int body_{};
826  std::string body_name_;
827 };
828 
830  public:
832  RigidBodyTree<double>* model, int body,
835  virtual int getNumConstraint(const double* t, int n_breaks) const;
836  virtual void eval_valid(const double* valid_t, int num_valid_t,
837  const Eigen::MatrixXd& valid_q, Eigen::VectorXd& c,
838  Eigen::MatrixXd& dc_valid) const;
839  virtual void bounds(const double* t, int n_breaks, Eigen::VectorXd& lb,
840  Eigen::VectorXd& ub) const;
841  virtual void name(const double* t, int n_breaks,
842  std::vector<std::string>& name_str) const;
843 
844  private:
845  int body_{};
846  std::string body_name_;
847 };
848 
851  public:
853  RigidBodyTree<double>* model, double lb, double ub,
854  const std::vector<int>& active_bodies_idx,
855  const std::set<std::string>& active_group_names,
858  virtual void updateRobot(RigidBodyTree<double>* robot);
859  virtual void eval(const double* t, KinematicsCache<double>& cache,
860  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
861  virtual void name(const double* t, std::vector<std::string>& name) const;
862  virtual void bounds(const double* t, Eigen::VectorXd& lb,
863  Eigen::VectorXd& ub) const;
864 
865  private:
866  double ub_{};
867  double lb_{};
868  std::vector<int> active_bodies_idx_;
869  std::set<std::string> active_group_names_;
870 };
871 
873  public:
875  RigidBodyTree<double>* model, double min_distance,
876  const std::vector<int>& active_bodies_idx,
877  const std::set<std::string>& active_group_names,
880  virtual void eval(const double* t, KinematicsCache<double>& cache,
881  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
882  virtual void name(const double* t, std::vector<std::string>& name) const;
883  void scaleDistance(const Eigen::VectorXd& dist, Eigen::VectorXd& scaled_dist,
884  Eigen::MatrixXd& dscaled_dist_ddist) const;
885  void penalty(const Eigen::VectorXd& dist, Eigen::VectorXd& cost,
886  Eigen::MatrixXd& dcost_ddist) const;
887  virtual void bounds(const double* t, Eigen::VectorXd& lb,
888  Eigen::VectorXd& ub) const;
889 
890  private:
891  double min_distance_{};
892  std::vector<int> active_bodies_idx_;
893  std::set<std::string> active_group_names_;
894 };
895 
897  public:
899  RigidBodyTree<double>* model, int body, const Eigen::Matrix3Xd& pts,
900  const Eigen::Matrix4d& T_world_to_frame, const Eigen::MatrixXd& lb,
901  const Eigen::MatrixXd& ub,
904 
905  protected:
906  virtual void evalPositions(KinematicsCache<double>& cache,
907  Eigen::Matrix3Xd& pos, Eigen::MatrixXd& J) const;
908  virtual void evalNames(const double* t,
909  std::vector<std::string>& cnst_names) const;
910 
911  private:
912  Eigen::Matrix4d T_world_to_frame_;
913 
914  public:
915  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
916 };
917 
919  public:
921  RigidBodyTree<double>* model, const Eigen::VectorXi& joint_ind,
922  const Eigen::VectorXd& lb_change, const Eigen::VectorXd& ub_change,
925  virtual int getNumConstraint(const double* t, int n_breaks) const;
926  virtual void feval(const double* t, int n_breaks, const Eigen::MatrixXd& q,
927  Eigen::VectorXd& c) const;
928  virtual void geval(const double* t, int n_breaks, Eigen::VectorXi& iAfun,
929  Eigen::VectorXi& jAvar, Eigen::VectorXd& A) const;
930  virtual void name(const double* t, int n_breaks,
931  std::vector<std::string>& name_str) const;
932  virtual void bounds(const double* t, int n_breaks, Eigen::VectorXd& lb,
933  Eigen::VectorXd& ub) const;
934 
935  protected:
936  virtual void setJointChangeBounds(const Eigen::VectorXi& joint_ind,
937  const Eigen::VectorXd& lb_change,
938  const Eigen::VectorXd& ub_change);
939 
940  private:
941  Eigen::VectorXi joint_ind_;
942  Eigen::VectorXd lb_change_;
943  Eigen::VectorXd ub_change_;
944 };
945 
948  public:
950  RigidBodyTree<double>* model, const Eigen::VectorXi& joint_indices,
951  const Eigen::VectorXd& lb, const Eigen::VectorXd& ub,
954  virtual void eval(const double* t, KinematicsCache<double>& cache,
955  Eigen::VectorXd& c, Eigen::MatrixXd& dc) const;
956  virtual void name(const double* t, std::vector<std::string>& name) const;
957  virtual void bounds(const double* t, Eigen::VectorXd& lb,
958  Eigen::VectorXd& ub) const;
959 
960  private:
961  Eigen::VectorXi joint_indices_;
962  Eigen::VectorXd lb_;
963  Eigen::VectorXd ub_;
964 };
virtual ~WorldGazeDirConstraint(void)
Definition: rigid_body_constraint.h:651
Eigen::Vector2d Vector2d
Definition: rigid_contact_solver_test.cc:14
virtual ~RelativeGazeDirConstraint(void)
Definition: rigid_body_constraint.h:727
int getCategory() const
Definition: rigid_body_constraint.h:68
Definition: rigid_body_constraint.h:606
virtual ~PostureConstraint(void)
Definition: rigid_body_constraint.h:176
Definition: rigid_body_constraint.h:383
double get_conethreshold() const
Definition: rigid_body_constraint.h:570
std::vector< snopt::integer > jAvar
Definition: snopt_solver.cc:65
Definition: rigid_body_constraint.h:809
Definition: rigid_body_constraint.h:404
Eigen::Matrix< Expression, 3, 2, Eigen::DontAlign > A_
Definition: symbolic_expression_array_test.cc:32
Definition: rigid_body_constraint.h:330
virtual ~GazeConstraint(void)
Definition: rigid_body_constraint.h:567
virtual ~AllBodiesClosestDistanceConstraint()
Definition: rigid_body_constraint.h:857
void setActive(bool flag)
Definition: rigid_body_constraint.h:142
Definition: rigid_body_constraint.h:720
bool isActive() const
Definition: rigid_body_constraint.h:132
Definition: rigid_body_constraint.h:519
int get_n_pts() const
Definition: rigid_body_constraint.h:373
Definition: rigid_body_constraint.h:216
Definition: rigid_body_constraint.h:918
virtual ~GravityCompensationTorqueConstraint()
Definition: rigid_body_constraint.h:953
virtual ~PositionConstraint(void)
Definition: rigid_body_constraint.h:359
void set_type(int type)
Definition: rigid_body_constraint.h:74
Definition: rigid_body_constraint.h:353
virtual ~EulerConstraint(void)
Definition: rigid_body_constraint.h:525
Definition: rigid_body_constraint.h:661
virtual ~Point2PointDistanceConstraint(void)
Definition: rigid_body_constraint.h:746
Definition: rigid_body_constraint.h:701
virtual ~MultipleTimeLinearPostureConstraint()
Definition: rigid_body_constraint.h:221
void set_robot(RigidBodyTree< double > *robot)
Definition: rigid_body_constraint.h:75
virtual ~PostureChangeConstraint()
Definition: rigid_body_constraint.h:924
Definition: rigid_body_constraint.h:625
Definition: rigid_body_constraint.h:451
AutoDiffXd eval(const Eigen::AutoDiffScalar< Derived > &x)
Force Eigen to evaluate an autodiff expression.
Definition: pydrake_autodiffutils.cc:21
virtual ~WorldFixedPositionConstraint(void)
Definition: rigid_body_constraint.h:793
virtual ~MinDistanceConstraint()
Definition: rigid_body_constraint.h:879
void addContact(std::vector< int > body, const Eigen::Matrix3Xd &body_pts)
Definition: rigid_body_constraint.h:137
virtual ~WorldGazeTargetConstraint(void)
Definition: rigid_body_constraint.h:691
Definition: rigid_body_constraint.h:561
std::vector< snopt::doublereal > A
Definition: snopt_solver.cc:63
Definition: rigid_body_constraint.h:896
virtual ~MultipleTimeKinematicConstraint()
Definition: rigid_body_constraint.h:335
virtual ~WorldFixedBodyPoseConstraint(void)
Definition: rigid_body_constraint.h:834
virtual ~WorldGazeOrientConstraint()
Definition: rigid_body_constraint.h:612
const double * tspan() const
Definition: rigid_body_constraint.h:76
const Eigen::Vector3d & get_target() const
Definition: rigid_body_constraint.h:673
const Eigen::Matrix3Xd & get_pts() const
Definition: rigid_body_constraint.h:372
int getNumWeights() const
Definition: rigid_body_constraint.h:133
virtual ~SingleTimeLinearPostureConstraint(void)
Definition: rigid_body_constraint.h:278
Definition: rigid_body_constraint.h:494
Eigen::Vector2d default_tspan
virtual ~GazeOrientConstraint(void)
Definition: rigid_body_constraint.h:587
const std::string & get_body_name() const
Definition: rigid_body_constraint.h:397
Definition: rigid_body_constraint.h:946
Definition: rigid_body_constraint.h:788
Definition: rigid_body_constraint.h:829
const Eigen::Vector3d & get_dir() const
Definition: rigid_body_constraint.h:636
Definition: rigid_body_constraint.h:426
std::unique_ptr< RigidBodyTree< double > > robot_
Definition: param_parser_test.cc:41
const RigidBody< double > * bodyB_
Definition: frames_test.cc:91
virtual ~RelativeGazeTargetConstraint(void)
Definition: rigid_body_constraint.h:708
bool null_constraint_row(int row) const
Definition: rigid_body_constraint.h:534
virtual ~WorldFixedOrientConstraint(void)
Definition: rigid_body_constraint.h:815
Definition: rigid_body_constraint.h:471
int get_body() const
Definition: rigid_body_constraint.h:396
std::vector< snopt::integer > iAfun
Definition: snopt_solver.cc:64
Definition: rigid_body_constraint.h:645
virtual ~Point2LineSegDistConstraint(void)
Definition: rigid_body_constraint.h:769
Definition: rigid_body_constraint.h:581
the Center of Mass (CoM) is within the support polygon.
Definition: rigid_body_constraint.h:117
Definition: rigid_body_constraint.h:305
virtual ~GazeDirConstraint(void)
Definition: rigid_body_constraint.h:631
RigidBodyTree< double > * getRobotPointer() const
Definition: rigid_body_constraint.h:69
base class.
Definition: rigid_body_constraint.h:27
int getType() const
Definition: rigid_body_constraint.h:67
Definition: rigid_body_constraint.h:739
Definition: rigid_body_constraint.h:543
const Eigen::Vector3d & get_axis() const
Definition: rigid_body_constraint.h:571
std::vector< const Body< double > * > bodies_
Definition: multibody_tree_creation_test.cc:311
Definition: rigid_body_constraint.cc:36
virtual ~SingleTimeKinematicConstraint()
Definition: rigid_body_constraint.h:310
Definition: rigid_body_constraint.h:849
Definition: rigid_body_constraint.h:872
Definition: rigid_body_constraint.h:271
Definition: rigid_body_constraint.h:684
const Eigen::Vector3d & get_gaze_origin() const
Definition: rigid_body_constraint.h:674
Definition: rigid_body_constraint.h:171
Definition: rigid_body_constraint.h:762
void set_num_constraint(int num_constraint)
Definition: rigid_body_constraint.h:322
virtual ~GazeTargetConstraint(void)
Definition: rigid_body_constraint.h:668