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 
156  int body_idx, const Eigen::Quaterniond& desired_orientation,
157  double angle_tol);
158 
193  void AddPostureCost(
194  const Eigen::Ref<const Eigen::VectorXd>& q_desired,
195  const Eigen::Ref<const Eigen::VectorXd>& body_position_cost,
196  const Eigen::Ref<const Eigen::VectorXd>& body_orientation_cost);
197 
233  int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ,
234  const std::vector<Eigen::Matrix3Xd>& region_vertices);
235 
243  void AddJointLimitConstraint(int body_index, double joint_lower_bound,
244  double joint_upper_bound);
245 
246  private:
247  // This is an utility function for `ReconstructGeneralizedPositionSolution`.
248  // This function computes the joint generalized position on the body with
249  // index body_idx. Note that the orientation of the parent link of the body
250  // body_idx should have been reconstructed, in reconstruct_R_WB.
251  void ReconstructGeneralizedPositionSolutionForBody(
252  int body_idx, Eigen::Ref<Eigen::VectorXd> q,
253  std::vector<Eigen::Matrix3d>* reconstruct_R_WB) const;
254 
255  const RigidBodyTree<double>* robot_;
256 
257  // joint_lower_bounds_ and joint_upper_bounds_ are column vectors of size
258  // robot_->get_num_positions() x 1.
259  // joint_lower_bounds_(i) is the lower bound of the i'th joint.
260  // joint_upper_bounds_(i) is the upper bound of the i'th joint.
261  // These joint bounds include those specified in the robot (like in the URDF
262  // file), and the bounds imposed by the user, through AddJointLimitConstraint.
263  Eigen::VectorXd joint_lower_bounds_;
264  Eigen::VectorXd joint_upper_bounds_;
265 
266  // R_WB_[i] is the orientation of body i in the world reference frame,
267  // it is expressed in the world frame.
268  std::vector<solvers::MatrixDecisionVariable<3, 3>> R_WB_;
269 
270  // p_WBo_[i] is the position of the origin Bo of body frame B for the i'th
271  // body, measured and expressed in the world frame.
272  std::vector<solvers::VectorDecisionVariable<3>> p_WBo_;
273 };
274 } // namespace multibody
275 } // 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
NOTE: The contents of this class are for the most part direct ports of drake/systems/plants//inverseK...
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:266
#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:35