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"
8 
9 namespace drake {
10 namespace multibody {
11 /** Solves the inverse kinematics problem as a mixed integer convex optimization
12  * problem.
13  * We use a mixed-integer convex relaxation of the rotation matrix. So if this
14  * global inverse kinematics problem says the solution is infeasible, then it is
15  * guaranteed that the kinematics constraints are not satisfiable.
16  * If the global inverse kinematics returns a solution, the posture should
17  * approximately satisfy the kinematics constraints, with some error.
18  * The approach is described in Global Inverse Kinematics via Mixed-integer
19  * Convex Optimization by Hongkai Dai, Gregory Izatt and Russ Tedrake, ISRR,
20  * 2017.
21  */
23 // TODO(hongkai.dai): create a function globalIK, with interface similar to
24 // inverseKin(), that accepts RigidBodyConstraint objects and cost function.
25  public:
27 
28  struct Options {
29  // This constructor is needed, otherwise the compiler complains.
30  Options() {}
31 
32  int num_intervals_per_half_axis{2};
36  solvers::IntervalBinning interval_binning{
38  /** If true, add only mixed-integer linear constraints in the
39  * constructor of GlobalInverseKinematics. The mixed-integer relaxation
40  * is tighter with nonlinear constraints (such as Lorentz cone constraint)
41  * than with linear constraints, but the optimization takes more time with
42  * nonlinear constraints.
43  */
44  bool linear_constraint_only{false};
45  };
46 
47  /**
48  * Parses the robot kinematics tree. The decision variables include the
49  * pose for each body (position/orientation). This constructor loops through
50  * each body inside the robot kinematics tree, adds the constraint on each
51  * body pose, so that the adjacent bodies are connected correctly by the joint
52  * in between the bodies.
53  * @param robot The robot on which the inverse kinematics problem is solved.
54  * @param options The options to relax SO(3) constraint as mixed-integer
55  * convex constraints. Refer to MixedIntegerRotationConstraintGenerator for
56  * more details on the parameters in options.
57  */
58  explicit GlobalInverseKinematics(const RigidBodyTreed& robot,
59  const Options& options = Options());
60 
62 
63  /** Getter for the decision variables on the rotation matrix `R_WB` for a body
64  * with the specified index. This is the orientation of body i's frame
65  * measured and expressed in the world frame.
66  * @param body_index The index of the queried body. Notice that body 0 is
67  * the world, and thus not a decision variable. Throws a runtime_error if
68  * the index is smaller than 1, or no smaller than the total number of bodies
69  * in the robot.
70  */
72  int body_index) const;
73 
74  /** Getter for the decision variables on the position p_WBo of the body B's
75  * origin measured and expressed in the world frame.
76  * @param body_index The index of the queried body. Notice that body 0 is
77  * the world, and thus not a decision variable. Throws a runtime_error if
78  * the index is smaller than 1, or greater than or equal to the total number
79  * of bodies in the robot.
80  */
81  const solvers::VectorDecisionVariable<3>& body_position(int body_index) const;
82 
83  /**
84  * After solving the inverse kinematics problem and finding out the pose of
85  * each body, reconstruct the robot generalized position (joint angles, etc)
86  * that matches with the body poses. Notice that since the rotation matrix is
87  * approximated, that the solution of body_rotmat() might not be on SO(3)
88  * exactly, the reconstructed body posture might not match with the body poses
89  * exactly, and the kinematics constraint might not be satisfied exactly with
90  * this reconstructed posture.
91  * @warning Do not call this method if the problem is not solved successfully!
92  * The returned value can be NaN or meaningless number if the problem is
93  * not solved.
94  * @retval q The reconstructed posture of the robot of the generalized
95  * coordinates, corresponding to the RigidBodyTree on which the inverse
96  * kinematics problem is solved.
97  */
98  Eigen::VectorXd ReconstructGeneralizedPositionSolution() const;
99 
100  /**
101  * Adds the constraint that the position of a point `Q` on a body `B`
102  * (whose index is `body_idx`), is within a box in a specified frame `F`.
103  * The constraint is that the point position, computed as
104  * <pre>
105  * p_WQ = p_WBo + R_WB * p_BQ
106  * </pre>
107  * where
108  * - p_WQ is the position of the body point Q measured and expressed in the
109  * world frame `W`.
110  * - p_WBo is the position of the body origin Bo measured and expressed in
111  * the world frame `W`.
112  * - R_WB is the rotation matrix of the body measured and expressed in the
113  * world frame `W`.
114  * - p_BQ is the position of the body point Q measured and expressed in the
115  * body frame `B`.
116  * p_WQ should lie within a bounding box in the frame `F`. Namely
117  * <pre>
118  * box_lb_F <= p_FQ <= box_ub_F
119  * </pre>
120  * where p_FQ is the position of the point Q measured and expressed in the
121  * `F`.
122  * The inequality is imposed elementwisely.
123  *
124  * Notice that since the rotation matrix `R_WB` does not lie exactly on the
125  * SO(3), due to the McCormick envelope relaxation, this constraint is subject
126  * to the accumulated error from the root of the kinematics tree.
127  * @param body_idx The index of the body on which the position of a point is
128  * constrained.
129  * @param p_BQ The position of the point Q measured and expressed in the
130  * body frame B.
131  * @param box_lb_F The lower bound of the box in frame `F`.
132  * @param box_ub_F The upper bound of the box in frame `F`.
133  * @param X_WF. The frame in which the box is specified. This
134  * frame is represented by an isometry transform X_WF, the transform from
135  * the constraint frame F to the world frame W. Namely if the position of
136  * the point `Q` in the world frame is `p_WQ`, then the constraint is
137  * <pre>
138  * box_lb_F <= R_FW * (p_WQ-p_WFo) <= box_ub_F
139  * </pre>
140  * where
141  * - R_FW is the rotation matrix of frame `W` expressed and measured in
142  * frame `F`. `R_FW = X_WF.linear().transpose()`.
143  * - p_WFo is the position of frame `F`'s origin, expressed and measured in
144  * frame `W`. `p_WFo = X_WF.translation()`.
145  * @default is the identity transform.
146  * @retval binding The newly added constraint, together with the bound
147  * variables.
148  */
150  int body_idx, const Eigen::Vector3d& p_BQ,
151  const Eigen::Vector3d& box_lb_F, const Eigen::Vector3d& box_ub_F,
152  const Eigen::Isometry3d& X_WF = Eigen::Isometry3d::Identity());
153 
154  /**
155  * Add a constraint that the angle between the body orientation and the
156  * desired orientation should not be larger than `angle_tol`. If we denote the
157  * angle between two rotation matrices `R1` and `R2` as `θ`, namely θ is the
158  * angle of the angle-axis representation of the rotation matrix `R1ᵀ * R2`,
159  * we then know
160  * <pre>
161  * trace(R1ᵀ * R2) = 2 * cos(θ) + 1
162  * </pre>
163  * as in
164  * http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/
165  * To constraint `θ < angle_tol`, we can impose the following constraint
166  * <pre>
167  * 2 * cos(angle_tol) + 1 <= trace(R1ᵀ * R2) <= 3
168  * </pre>
169  *
170  * @param body_idx The index of the body whose orientation will be
171  * constrained.
172  * @param desired_orientation The desired orientation of the body.
173  * @param angle_tol The tolerance on the angle between the body orientation
174  * and the desired orientation. Unit is radians.
175  * @retval binding The newly added constraint, together with the bound
176  * variables.
177  */
179  int body_idx, const Eigen::Quaterniond& desired_orientation,
180  double angle_tol);
181 
182  /** Penalizes the deviation to the desired posture.
183  * For each body (except the world) in the kinematic tree, we add the cost
184  * `∑ᵢ body_position_cost(i) * body_position_error(i) +
185  * body_orientation_cost(i) * body_orientation_error(i)`
186  * where `body_position_error(i)` is computed as the Euclidean distance error
187  * |p_WBo(i) - p_WBo_desired(i)|
188  * where
189  * - p_WBo(i) : position of body i'th origin `Bo` in the world frame
190  * `W`.
191  * - p_WBo_desired(i): position of body i'th origin `Bo` in the world frame
192  * `W`, computed from the desired posture `q_desired`.
193  *
194  * body_orientation_error(i) is computed as (1 - cos(θ)), where θ is the
195  * angle between the orientation of body i'th frame and body i'th frame using
196  * the desired posture. Notice that 1 - cos(θ) = θ²/2 + O(θ⁴), so this cost
197  * is on the square of θ, when θ is small.
198  * Notice that since body 0 is the world, the cost on that body is always 0,
199  * no matter what value `body_position_cost(0)` and `body_orientation_cost(0)`
200  * take.
201  * @param q_desired The desired posture.
202  * @param body_position_cost The cost for each body's position error. Unit is
203  * [1/m] (one over meters).
204  * @pre
205  * 1. body_position_cost.rows() == robot->get_num_bodies(), where `robot`
206  * is the input argument in the constructor of the class.
207  * 2. body_position_cost(i) is non-negative.
208  * @throw a runtime error if the precondition is not satisfied.
209  * @param body_orientation_cost The cost for each body's orientation error.
210  * @pre
211  * 1. body_orientation_cost.rows() == robot->get_num_bodies() , where
212  * `robot` is the input argument in the constructor of the class.
213  * 2. body_position_cost(i) is non-negative.
214  * @throw a runtime_error if the precondition is not satisfied.
215  */
216  void AddPostureCost(
217  const Eigen::Ref<const Eigen::VectorXd>& q_desired,
218  const Eigen::Ref<const Eigen::VectorXd>& body_position_cost,
219  const Eigen::Ref<const Eigen::VectorXd>& body_orientation_cost);
220 
221  /**
222  * Constrain the point `Q` lying within one of the convex polytopes.
223  * Each convex polytope Pᵢ is represented by its vertices as
224  * Pᵢ = ConvexHull(v_i1, v_i2, ... v_in). Mathematically we want to impose the
225  * constraint that the p_WQ, i.e., the position of point `Q` in world frame
226  * `W`, satisfies
227  * <pre>
228  * p_WQ ∈ Pᵢ for one i.
229  * </pre>
230  * To impose this constraint, we consider to introduce binary variable zᵢ, and
231  * continuous variables w_i1, w_i2, ..., w_in for each vertex of Pᵢ, with the
232  * following constraints
233  * <pre>
234  * p_WQ = sum_i (w_i1 * v_i1 + w_i2 * v_i2 + ... + w_in * v_in)
235  * w_ij >= 0, ∀i,j
236  * w_i1 + w_i2 + ... + w_in = zᵢ
237  * sum_i zᵢ = 1
238  * zᵢ ∈ {0, 1}
239  * </pre>
240  * Notice that if zᵢ = 0, then w_i1 * v_i1 + w_i2 * v_i2 + ... + w_in * v_in
241  * is just 0.
242  * This function can be used for collision avoidance, where each region Pᵢ is
243  * a free space region. It can also be used for grasping, where each region Pᵢ
244  * is a surface patch on the grasped object.
245  * Note this approach also works if the region Pᵢ overlaps with each other.
246  * @param body_index The index of the body to on which point `Q` is attached.
247  * @param p_BQ The position of point `Q` in the body frame `B`.
248  * @param region_vertices region_vertices[i] is the vertices for the i'th
249  * region.
250  * @retval z The newly added binary variables. If point `Q` is in the i'th
251  * region, z(i) = 1.
252  * @pre region_vertices[i] has at least 3 columns. Throw a std::runtime_error
253  * if the precondition is not satisfied.
254  */
256  int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ,
257  const std::vector<Eigen::Matrix3Xd>& region_vertices);
258 
259  /**
260  * Adds joint limits on a specified joint.
261  * @param body_index The joint connecting the parent link to this body will be
262  * constrained.
263  * @param joint_lower_bound The lower bound for the joint.
264  * @param joint_upper_bound The upper bound for the joint.
265  * @param linear_constraint_approximation If true, joint limits are
266  * approximated as linear constraints on parent and child link orientations,
267  * otherwise they are imposed as Lorentz cone constraints.
268  * With the Lorentz cone formulation, the joint limit constraint would be
269  * tight if our mixed-integer constraint on SO(3) were tight. By enforcing the
270  * joint limits as linear constraint, the original inverse kinematics problem
271  * is further relaxed, on top of SO(3) relaxation, but potentially with faster
272  * computation. @default is false.
273  */
274  void AddJointLimitConstraint(int body_index, double joint_lower_bound,
275  double joint_upper_bound,
276  bool linear_constraint_approximation = false);
277 
278  private:
279  // This is an utility function for `ReconstructGeneralizedPositionSolution`.
280  // This function computes the joint generalized position on the body with
281  // index body_idx. Note that the orientation of the parent link of the body
282  // body_idx should have been reconstructed, in reconstruct_R_WB.
283  void ReconstructGeneralizedPositionSolutionForBody(
284  int body_idx, Eigen::Ref<Eigen::VectorXd> q,
285  std::vector<Eigen::Matrix3d>* reconstruct_R_WB) const;
286 
287  const RigidBodyTree<double>* robot_;
288 
289  // joint_lower_bounds_ and joint_upper_bounds_ are column vectors of size
290  // robot_->get_num_positions() x 1.
291  // joint_lower_bounds_(i) is the lower bound of the i'th joint.
292  // joint_upper_bounds_(i) is the upper bound of the i'th joint.
293  // These joint bounds include those specified in the robot (like in the URDF
294  // file), and the bounds imposed by the user, through AddJointLimitConstraint.
295  Eigen::VectorXd joint_lower_bounds_;
296  Eigen::VectorXd joint_upper_bounds_;
297 
298  // R_WB_[i] is the orientation of body i in the world reference frame,
299  // it is expressed in the world frame.
300  std::vector<solvers::MatrixDecisionVariable<3, 3>> R_WB_;
301 
302  // p_WBo_[i] is the position of the origin Bo of body frame B for the i'th
303  // body, measured and expressed in the world frame.
304  std::vector<solvers::VectorDecisionVariable<3>> p_WBo_;
305 };
306 } // namespace multibody
307 } // 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:246
Definition: global_inverse_kinematics.h:28
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:157
Definition: bullet_model.cc:22
Relax SO(3) constraint by considering the McCormick envelope on the bilinear product.
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:299
~GlobalInverseKinematics() override
Definition: global_inverse_kinematics.h:61
IntervalBinning
For a continuous variable whose range is cut into small intervals, we will use binary variables to re...
Definition: mixed_integer_optimization_util.h:146
A binding on constraint type C is a mapping of the decision variables onto the inputs of C...
Definition: binding.h:18
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:289
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:312
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:165
Options()
Definition: global_inverse_kinematics.h:30
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:22
Approach
Definition: mixed_integer_rotation_constraint.h:54
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:378
void AddJointLimitConstraint(int body_index, double joint_lower_bound, double joint_upper_bound, bool linear_constraint_approximation=false)
Adds joint limits on a specified joint.
Definition: global_inverse_kinematics.cc:446
MathematicalProgram stores the decision variables, the constraints and costs of an optimization probl...
Definition: mathematical_program.h:301
#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