Drake
global_inverse_kinematics.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <vector>
4 
7 
8 namespace drake {
9 namespace multibody {
19 // TODO(hongkai.dai): create a function globalIK, with interface similar to
20 // inverseKin(), that accepts RigidBodyConstraint objects and cost function.
21  public:
23 
24 
37  int num_binary_vars_per_half_axis = 2);
38 
40 
50  int body_index) const;
51 
59  const solvers::VectorDecisionVariable<3>& body_position(int body_index) const;
60 
76  Eigen::VectorXd ReconstructGeneralizedPositionSolution() const;
77 
128  int body_idx, const Eigen::Vector3d& p_BQ,
129  const Eigen::Vector3d& box_lb_F, const Eigen::Vector3d& box_ub_F,
130  const Eigen::Isometry3d& X_WF = Eigen::Isometry3d::Identity());
131 
157  int body_idx, const Eigen::Quaterniond& desired_orientation,
158  double angle_tol);
159 
194  void AddPostureCost(
195  const Eigen::Ref<const Eigen::VectorXd>& q_desired,
196  const Eigen::Ref<const Eigen::VectorXd>& body_position_cost,
197  const Eigen::Ref<const Eigen::VectorXd>& body_orientation_cost);
198 
234  int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ,
235  const std::vector<Eigen::Matrix3Xd>& region_vertices);
236 
244  void AddJointLimitConstraint(int body_index, double joint_lower_bound,
245  double joint_upper_bound);
246 
247  private:
248  // This is an utility function for `ReconstructGeneralizedPositionSolution`.
249  // This function computes the joint generalized position on the body with
250  // index body_idx. Note that the orientation of the parent link of the body
251  // body_idx should have been reconstructed, in reconstruct_R_WB.
252  void ReconstructGeneralizedPositionSolutionForBody(
253  int body_idx, Eigen::Ref<Eigen::VectorXd> q,
254  std::vector<Eigen::Matrix3d>* reconstruct_R_WB) const;
255 
256  const RigidBodyTree<double>* robot_;
257 
258  // joint_lower_bounds_ and joint_upper_bounds_ are column vectors of size
259  // robot_->get_num_positions() x 1.
260  // joint_lower_bounds_(i) is the lower bound of the i'th joint.
261  // joint_upper_bounds_(i) is the upper bound of the i'th joint.
262  // These joint bounds include those specified in the robot (like in the URDF
263  // file), and the bounds imposed by the user, through AddJointLimitConstraint.
264  Eigen::VectorXd joint_lower_bounds_;
265  Eigen::VectorXd joint_upper_bounds_;
266 
267  // R_WB_[i] is the orientation of body i in the world reference frame,
268  // it is expressed in the world frame.
269  std::vector<solvers::MatrixDecisionVariable<3, 3>> R_WB_;
270 
271  // p_WBo_[i] is the position of the origin Bo of body frame B for the i'th
272  // body, measured and expressed in the world frame.
273  std::vector<solvers::VectorDecisionVariable<3>> p_WBo_;
274 };
275 } // namespace multibody
276 } // namespace drake
GlobalInverseKinematics(const GlobalInverseKinematics &)=delete
Eigen::VectorXd ReconstructGeneralizedPositionSolution() const
After solving the inverse kinematics problem and finding out the pose of each body, reconstruct the robot generalized position (joint angles, etc) that matches with the body poses.
Definition: global_inverse_kinematics.cc:237
Eigen::Matrix< symbolic::Variable, rows, cols > MatrixDecisionVariable
Definition: decision_variable.h:12
const solvers::MatrixDecisionVariable< 3, 3 > & body_rotation_matrix(int body_index) const
Getter for the decision variables on the rotation matrix R_WB for a body with the specified index...
Definition: global_inverse_kinematics.cc:149
Definition: automotive_demo.cc:88
solvers::Binding< solvers::LinearConstraint > AddWorldOrientationConstraint(int body_idx, const Eigen::Quaterniond &desired_orientation, double angle_tol)
Add a constraint that the angle between the body orientation and the desired orientation should not b...
Definition: global_inverse_kinematics.cc:290
~GlobalInverseKinematics() override
Definition: global_inverse_kinematics.h:39
A binding on constraint type C is a mapping of the decision variables onto the inputs of C...
Definition: binding.h:17
void AddJointLimitConstraint(int body_index, double joint_lower_bound, double joint_upper_bound)
Adds joint limits on a specified joint.
Definition: global_inverse_kinematics.cc:407
solvers::Binding< solvers::LinearConstraint > AddWorldPositionConstraint(int body_idx, const Eigen::Vector3d &p_BQ, const Eigen::Vector3d &box_lb_F, const Eigen::Vector3d &box_ub_F, const Eigen::Isometry3d &X_WF=Eigen::Isometry3d::Identity())
Adds the constraint that the position of a point Q on a body B (whose index is body_idx), is within a box in a specified frame F.
Definition: global_inverse_kinematics.cc:280
void AddPostureCost(const Eigen::Ref< const Eigen::VectorXd > &q_desired, const Eigen::Ref< const Eigen::VectorXd > &body_position_cost, const Eigen::Ref< const Eigen::VectorXd > &body_orientation_cost)
Penalizes the deviation to the desired posture.
Definition: global_inverse_kinematics.cc:303
const solvers::VectorDecisionVariable< 3 > & body_position(int body_index) const
Getter for the decision variables on the position p_WBo of the body B&#39;s origin measured and expressed...
Definition: global_inverse_kinematics.cc:157
VectorDecisionVariable< Eigen::Dynamic > VectorXDecisionVariable
Definition: decision_variable.h:17
Solves the inverse kinematics problem as a mixed integer convex optimization problem.
Definition: global_inverse_kinematics.h:18
MatrixDecisionVariable< rows, 1 > VectorDecisionVariable
Definition: decision_variable.h:14
solvers::VectorXDecisionVariable BodyPointInOneOfRegions(int body_index, const Eigen::Ref< const Eigen::Vector3d > &p_BQ, const std::vector< Eigen::Matrix3Xd > &region_vertices)
Constrain the point Q lying within one of the convex polytopes.
Definition: global_inverse_kinematics.cc:369
Definition: mathematical_program.h:286
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33