Drake
global_inverse_kinematics.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <vector>
4 
5 #include "drake/multibody/rigid_body_tree.h"
6 #include "drake/solvers/mathematical_program.h"
7 
8 namespace drake {
9 namespace multibody {
10 /** Solves the inverse kinematics problem as a mixed integer convex optimization
11  * problem.
12  * We use a convex relaxation of the rotation matrix. So if this global inverse
13  * kinematics problem says the solution is infeasible, then it is guaranteed
14  * that the kinematics constraints are not satisfiable.
15  * If the global inverse kinematics returns a solution, the posture should
16  * satisfy the kinematics constraints, with some error.
17  */
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  /**
25  * Parses the robot kinematics tree. The decision variables include the
26  * pose for each body (position/orientation). This constructor loops through
27  * each body inside the robot kinematics tree, adds the constraint on each
28  * body pose, so that the adjacent bodies are connected correctly by the joint
29  * in between the bodies.
30  * @param robot The robot on which the inverse kinematics problem is solved.
31  * @param num_binary_vars_per_half_axis The number of binary variables for
32  * each half axis, to segment the unit circle.
33  * @see AddRotationMatrixMcCormickEnvelopeMilpConstraints() for more details
34  * on num_binary_vars_per_half_axis.
35  */
37  int num_binary_vars_per_half_axis = 2);
38 
40 
41  /** Getter for the decision variables on the rotation matrix `R_WB` for a body
42  * with the specified index. This is the orientation of body i's frame
43  * measured and expressed in the world frame.
44  * @param body_index The index of the queried body. Notice that body 0 is
45  * the world, and thus not a decision variable. Throws a runtime_error if
46  * the index is smaller than 1, or no smaller than the total number of bodies
47  * in the robot.
48  */
50  int body_index) const;
51 
52  /** Getter for the decision variables on the position p_WBo of the body B's
53  * origin measured and expressed in the world frame.
54  * @param body_index The index of the queried body. Notice that body 0 is
55  * the world, and thus not a decision variable. Throws a runtime_error if
56  * the index is smaller than 1, or greater than or equal to the total number
57  * of bodies in the robot.
58  */
59  const solvers::VectorDecisionVariable<3>& body_position(int body_index) const;
60 
61  /**
62  * After solving the inverse kinematics problem and finding out the pose of
63  * each body, reconstruct the robot generalized position (joint angles, etc)
64  * that matches with the body poses. Notice that since the rotation matrix is
65  * approximated, that the solution of body_rotmat() might not be on SO(3)
66  * exactly, the reconstructed body posture might not match with the body poses
67  * exactly, and the kinematics constraint might not be satisfied exactly with
68  * this reconstructed posture.
69  * @warning Do not call this method if the problem is not solved successfully!
70  * The returned value can be NaN or meaningless number if the problem is
71  * not solved.
72  * @retval q The reconstructed posture of the robot of the generalized
73  * coordinates, corresponding to the RigidBodyTree on which the inverse
74  * kinematics problem is solved.
75  */
76  Eigen::VectorXd ReconstructGeneralizedPositionSolution() const;
77 
78  /**
79  * Adds the constraint that the position of a point `Q` on a body `B`
80  * (whose index is `body_idx`), is within a box in a specified frame `F`.
81  * The constraint is that the point position, computed as
82  * <pre>
83  * p_WQ = p_WBo + R_WB * p_BQ
84  * </pre>
85  * where
86  * - p_WQ is the position of the body point Q measured and expressed in the
87  * world frame `W`.
88  * - p_WBo is the position of the body origin Bo measured and expressed in
89  * the world frame `W`.
90  * - R_WB is the rotation matrix of the body measured and expressed in the
91  * world frame `W`.
92  * - p_BQ is the position of the body point Q measured and expressed in the
93  * body frame `B`.
94  * p_WQ should lie within a bounding box in the frame `F`. Namely
95  * <pre>
96  * box_lb_F <= p_FQ <= box_ub_F
97  * </pre>
98  * where p_FQ is the position of the point Q measured and expressed in the
99  * `F`.
100  * The inequality is imposed elementwisely.
101  *
102  * Notice that since the rotation matrix `R_WB` does not lie exactly on the
103  * SO(3), due to the McCormick envelope relaxation, this constraint is subject
104  * to the accumulated error from the root of the kinematics tree.
105  * @param body_idx The index of the body on which the position of a point is
106  * constrained.
107  * @param p_BQ The position of the point Q measured and expressed in the
108  * body frame B.
109  * @param box_lb_F The lower bound of the box in frame `F`.
110  * @param box_ub_F The upper bound of the box in frame `F`.
111  * @param X_WF. The frame in which the box is specified. This
112  * frame is represented by an isometry transform X_WF, the transform from
113  * the constraint frame F to the world frame W. Namely if the position of
114  * the point `Q` in the world frame is `p_WQ`, then the constraint is
115  * <pre>
116  * box_lb_F <= R_FW * (p_WQ-p_WFo) <= box_ub_F
117  * </pre>
118  * where
119  * - R_FW is the rotation matrix of frame `W` expressed and measured in
120  * frame `F`. `R_FW = X_WF.linear().transpose()`.
121  * - p_WFo is the position of frame `F`'s origin, expressed and measured in
122  * frame `W`. `p_WFo = X_WF.translation()`.
123  * @default is the identity transform.
124  * @retval binding The newly added constraint, together with the bound
125  * variables.
126  */
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 
132  /**
133  * Add a constraint that the angle between the body orientation and the
134  * desired orientation should not be larger than `angle_tol`. If we denote the
135  * angle between two rotation matrices `R1` and `R2` as `θ`, namely θ is the
136  * angle of the angle-axis representation of the rotation matrix `R1ᵀ * R2`,
137  * we then know
138  * <pre>
139  * trace(R1ᵀ * R2) = 2 * cos(θ) + 1
140  * </pre>
141  * as in
142  * http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/
143  * To constraint `θ < angle_tol`, we can impose the following constraint
144  * <pre>
145  * 2 * cos(angle_tol) + 1 <= trace(R1ᵀ * R2) <= 3
146  * </pre>
147  *
148  * @param body_idx The index of the body whose orientation will be
149  * constrained.
150  * @param desired_orientation The desired orientation of the body.
151  * @param angle_tol The tolerance on the angle between the body orientation
152  * and the desired orientation. Unit is radians.
153  * @retval binding The newly added constraint, together with the bound
154  * variables.
155  */
157  int body_idx, const Eigen::Quaterniond& desired_orientation,
158  double angle_tol);
159 
160  /** Penalizes the deviation to the desired posture.
161  * For each body (except the world) in the kinematic tree, we add the cost
162  * `∑ᵢ body_position_cost(i) * body_position_error(i) +
163  * body_orientation_cost(i) * body_orientation_error(i)`
164  * where `body_position_error(i)` is computed as the Euclidean distance error
165  * |p_WBo(i) - p_WBo_desired(i)|
166  * where
167  * - p_WBo(i) : position of body i'th origin `Bo` in the world frame
168  * `W`.
169  * - p_WBo_desired(i): position of body i'th origin `Bo` in the world frame
170  * `W`, computed from the desired posture `q_desired`.
171  *
172  * body_orientation_error(i) is computed as (1 - cos(θ)), where θ is the
173  * angle between the orientation of body i'th frame and body i'th frame using
174  * the desired posture. Notice that 1 - cos(θ) = θ²/2 + O(θ⁴), so this cost
175  * is on the square of θ, when θ is small.
176  * Notice that since body 0 is the world, the cost on that body is always 0,
177  * no matter what value `body_position_cost(0)` and `body_orientation_cost(0)`
178  * take.
179  * @param q_desired The desired posture.
180  * @param body_position_cost The cost for each body's position error. Unit is
181  * [1/m] (one over meters).
182  * @pre
183  * 1. body_position_cost.rows() == robot->get_num_bodies(), where `robot`
184  * is the input argument in the constructor of the class.
185  * 2. body_position_cost(i) is non-negative.
186  * @throw a runtime error if the precondition is not satisfied.
187  * @param body_orientation_cost The cost for each body's orientation error.
188  * @pre
189  * 1. body_orientation_cost.rows() == robot->get_num_bodies() , where
190  * `robot` is the input argument in the constructor of the class.
191  * 2. body_position_cost(i) is non-negative.
192  * @throw a runtime_error if the precondition is not satisfied.
193  */
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 
199  /**
200  * Constrain the point `Q` lying within one of the convex polytopes.
201  * Each convex polytope Pᵢ is represented by its vertices as
202  * Pᵢ = ConvexHull(v_i1, v_i2, ... v_in). Mathematically we want to impose the
203  * constraint that the p_WQ, i.e., the position of point `Q` in world frame
204  * `W`, satisfies
205  * <pre>
206  * p_WQ ∈ Pᵢ for one i.
207  * </pre>
208  * To impose this constraint, we consider to introduce binary variable zᵢ, and
209  * continuous variables w_i1, w_i2, ..., w_in for each vertex of Pᵢ, with the
210  * following constraints
211  * <pre>
212  * p_WQ = sum_i (w_i1 * v_i1 + w_i2 * v_i2 + ... + w_in * v_in)
213  * w_ij >= 0, ∀i,j
214  * w_i1 + w_i2 + ... + w_in = zᵢ
215  * sum_i zᵢ = 1
216  * zᵢ ∈ {0, 1}
217  * </pre>
218  * Notice that if zᵢ = 0, then w_i1 * v_i1 + w_i2 * v_i2 + ... + w_in * v_in
219  * is just 0.
220  * This function can be used for collision avoidance, where each region Pᵢ is
221  * a free space region. It can also be used for grasping, where each region Pᵢ
222  * is a surface patch on the grasped object.
223  * Note this approach also works if the region Pᵢ overlaps with each other.
224  * @param body_index The index of the body to on which point `Q` is attached.
225  * @param p_BQ The position of point `Q` in the body frame `B`.
226  * @param region_vertices region_vertices[i] is the vertices for the i'th
227  * region.
228  * @retval z The newly added binary variables. If point `Q` is in the i'th
229  * region, z(i) = 1.
230  * @pre region_vertices[i] has at least 3 columns. Throw a std::runtime_error
231  * if the precondition is not satisfied.
232  */
234  int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ,
235  const std::vector<Eigen::Matrix3Xd>& region_vertices);
236 
237  /**
238  * Adds joint limits on a specified joint.
239  * @param body_index The joint connecting the parent link to this body will be
240  * constrained.
241  * @param joint_lower_bound The lower bound for the joint.
242  * @param joint_upper_bound The upper bound for the joint.
243  */
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:89
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:287
#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