Drake
rigid_body_ik.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <string>
4 #include <vector>
5 
6 #include <Eigen/Dense>
7 
11 
12 /**
13  * inverseKin solves the inverse kinematics problem
14  * min_q (q-q_nom)'*Q*(q-q_nom)
15  * s.t lb<=constraint(q)<=ub
16  * @param q_seed an nq x 1 double vector. The initial guess for the
17  * optimization problem
18  * @param q_nom an nq x 1 double vector. The nominal posture that the robot
19  * wants to stay close to
20  * @param num_constraints The number of RigidBodyConstraints
21  * @param constraint_array Each entry in constraint_array is a
22  * RigidBodyConstraint type. Currently support QuasiStaticConstraintType,
23  * PostureConstraintType, SingleTimeKinematicConstraintType and
24  * SingleTimeLinearPostureConstraintType.
25  * @return q_sol an nq x 1 double vector. The optimized posture
26  * @return info = 1 The optimization is successful
27  * = 3 The optimization is successful. But optimality is not
28  * strictly satisfied
29  * = 4 The optimization is successful. But feasibility is not
30  * strictly satisfied
31  * = 5 The optimization is successful. But SNOPT runs out of
32  * iterations
33  * = 6 The optimization is successful. But SNOPT runs out of
34  * major iterations
35  * = 12 Fails to find a solution. The linear constraints are
36  * not satisfied
37  * = 13 Fails to find a solution. The nonlinear constraints
38  * are not satisfied
39  * = 31 Fails to find a solution. The iterations limit is
40  * reached. Set iterations limit in ikoptions
41  * = 32 Fails to find a solution. The major iterations limit
42  * is reached. Set major iterations limit in ikoptions
43  * = 41 Fails to find a solution because the numerics of the
44  * problem is bad.
45  * for more information of these info, check SNOPT manual for
46  * reference www.stanford.edu/group/SOL/guides/sndoc7.pdf
47  * @return infeasible_constraint. When the problem is infeasible,
48  * infeasible_constraint contains the name of the infeasible constraints
49  * @param ikoptions The options to set parameters of IK problem.
50  */
51 template <typename DerivedA, typename DerivedB, typename DerivedC>
52 void inverseKin(
53  RigidBodyTree<double>* model,
54  const Eigen::MatrixBase<DerivedA>& q_seed,
55  const Eigen::MatrixBase<DerivedB>& q_nom,
56  const int num_constraints,
57  const RigidBodyConstraint* const* constraint_array,
58  const IKoptions& ikoptions,
59  Eigen::MatrixBase<DerivedC>* q_sol, int* info,
60  std::vector<std::string>* infeasible_constraint);
61 
62 /**
63  * Return type for simplified versions of IK functions.
64  */
65 struct IKResults {
66  std::vector<Eigen::VectorXd> q_sol;
67  std::vector<int> info;
68  std::vector<std::string> infeasible_constraints;
69 };
70 
71 /**
72  * Simplified (non-template) version of inverseKin. Useful for
73  * generating bindings to non-C++ languages.
74  */
76  RigidBodyTree<double>* model, const Eigen::VectorXd& q_seed,
77  const Eigen::VectorXd& q_nom,
78  const std::vector<RigidBodyConstraint*>& constraint_array,
79  const IKoptions& ikoptions);
80 
81 
82 /**
83  * approximateIK solves the same problem as inverseKin. But for speed reason, it
84  * linearizes all constraints around q_seed, and solve a quadratic problem
85  * instead of a nonlinear problem.
86  * @param q_seed an nq x 1 double vector. The initial guess. Also where to
87  * linearize the constraints
88  * @param q_nom Same as in inverseKin
89  * @param num_constraints Same as in inverseKin
90  * @param constraint_array Same as in inverseKin, but do not accept
91  * SingleTimeLinearPostureConstraint for the moment
92  * @return q_sol Same as in inverseKin
93  * @return info = 0 Success
94  * = 1 Fail
95  * @param ikoptions Same as in inverseKin
96  */
97 template <typename DerivedA, typename DerivedB, typename DerivedC>
98 void approximateIK(
99  RigidBodyTree<double>* model,
100  const Eigen::MatrixBase<DerivedA>& q_seed,
101  const Eigen::MatrixBase<DerivedB>& q_nom,
102  const int num_constraints,
103  const RigidBodyConstraint* const* constraint_array,
104  const IKoptions& ikoptions,
105  Eigen::MatrixBase<DerivedC>* q_sol,
106  int* info);
107 
108 /**
109  * inverseKinPointwise solves inverse kinematics problem at each t[i]
110  * individually
111  * @param nT The length of time samples
112  * @param t t[i] is the i'th time to solve the inverse kinematics
113  * problem
114  * @param q_seed An nq x nT double matrix. q_seed.col(i) is the seed for the
115  * inverse kinematics problem at t[i]
116  * @param q_nom An nq x nT double matrix. q_nom.col(i) is the nominal
117  * posture for the inverse kinematics problem at t[i]
118  * @param num_constraints Same as in inverseKin
119  * @param constraint_array Same as in inverseKin
120  * @return q_sol An nq x nT double matrix. q_sol.col(i) is the solution to
121  * inverse kinematics problem at t[i]
122  * @return info info[i] is the info for the inverse kinematics problem at
123  * t[i]. The meaning of info[i] is explained in inverseKin
124  * @return infeasible_constraint infeasible_constraint[i] are the names of
125  * infeasible constraints at t[i]
126  * @param ikoptions The options to set parameters of IK problem.
127  * if ikoptions.sequentialSeedFlag = true, then the at time
128  * t[i], if t[i-1] is solved successfully, then q_sol.col(i-1) would be used as
129  * the seed for t[i]. If the solver fails to find a posture at t[i-1], then
130  * q_seed.col(i) would be used as the seed for t[i]
131  * if ikoptions.sequentialSeedFlag = false, then
132  * q_seed.col(i) would always be used as the seed at t[i]
133  */
134 template <typename DerivedA, typename DerivedB, typename DerivedC>
136  RigidBodyTree<double>* model, const int nT, const double* t,
137  const Eigen::MatrixBase<DerivedA>& q_seed,
138  const Eigen::MatrixBase<DerivedB>& q_nom, const int num_constraints,
139  const RigidBodyConstraint* const* constraint_array,
140  const IKoptions& ikoptions,
141  Eigen::MatrixBase<DerivedC>* q_sol, int* info,
142  std::vector<std::string>* infeasible_constraint);
143 
144 /**
145  * Simplified (non-template) version of inverseKinPointwise. Useful
146  * for generating bindings to non-C++ languages.
147  */
149  RigidBodyTree<double>* model,
150  const Eigen::VectorXd& t,
151  const Eigen::MatrixXd& q_seed,
152  const Eigen::MatrixXd& q_nom,
153  const std::vector<RigidBodyConstraint*>& constraint_array,
154  const IKoptions& ikoptions);
155 
156 /**
157  * inverseKinTraj solves the inverse kinematics problem at all time together.
158  * Try to generate a smooth trajectory by assuming cubic spline for the posture,
159  * and minimize the acceleration of the interpolated trajectory
160  * min_(q, qdot, qddot)
161  * sum(q(t(i))-q_nom(t(i)))'*Q*(q(t(i))-q_nom(t(i)))+qdot(t(i))'*Qv*qdot(t(i))+qddot(t(i))'*Qa*qddot(t(i))
162  * @param nT The length of time samples
163  * @param t t[i] is the i'th time
164  * @param q_seed An nq x nT double matrix. q_seed.col(i) is the seed for the
165  * inverse kinematics problem at t[i]
166  * @param q_nom An nq x nT double matrix. q_nom.col(i) is the seed for the
167  * inverse kinematics problem at t[i]
168  * @param num_constraints The number of constraints
169  * @param constraint_array Accept all categories of RigidBodyConstraint
170  * @return q_sol An nq x nT double matrix. q_sol.col(i) is the solution
171  * posture at time t[i]
172  * @return qdot_sol An nq x nT double matrix. qdot_sol.col(i) is the solution
173  * posture velocity at time t[i]
174  * @return qddot_sol An nq x nT double matrix. qddot_sol.col(i) is the solution
175  * posture acceleration at time t[i]
176  * @return info Same as in inverseKin
177  * @return infeasible_constraint Same as in inverseKin
178  * @return ikoptions Set parameters for inverse kinematics problem
179  * ikoptions.fixInitialState = True The initial posture and velocity
180  * at time t[0] will be fixed to the seed posture q_seed.col(0) and seed
181  * velocity qdot0_seed
182  * ikoptions.fixInitialState = False The solver will search for the
183  * initial posture and velocity
184  */
185 template <typename DerivedA, typename DerivedB, typename DerivedC,
186  typename DerivedD, typename DerivedE, typename DerivedF>
187 void inverseKinTraj(
188  RigidBodyTree<double>* model, const int nT, const double* t,
189  const Eigen::MatrixBase<DerivedA>& qdot0_seed,
190  const Eigen::MatrixBase<DerivedB>& q_seed,
191  const Eigen::MatrixBase<DerivedC>& q_nom, const int num_constraints,
192  const RigidBodyConstraint* const* constraint_array,
193  const IKoptions& ikoptions,
194  Eigen::MatrixBase<DerivedD>* q_sol, Eigen::MatrixBase<DerivedE>* qdot_sol,
195  Eigen::MatrixBase<DerivedF>* qddot_sol, int* info,
196  std::vector<std::string>* infeasible_constraint);
197 
198 // TODO(sam.creasey) This version is missing qdot_sol and qddot_sol in
199 // the results.
200 /**
201  * Simplified (non-template) version of inverseKinTraj. Useful
202  * for generating bindings to non-C++ languages.
203  */
205  RigidBodyTree<double>* model,
206  const Eigen::VectorXd& t,
207  const Eigen::MatrixXd& q_seed,
208  const Eigen::MatrixXd& q_nom,
209  const std::vector<RigidBodyConstraint*>& constraint_array,
210  const IKoptions& ikoptions);
void inverseKinPointwise(RigidBodyTree< double > *model, const int nT, const double *t, const Eigen::MatrixBase< DerivedA > &q_seed, const Eigen::MatrixBase< DerivedB > &q_nom, const int num_constraints, const RigidBodyConstraint *const *constraint_array, const IKoptions &ikoptions, Eigen::MatrixBase< DerivedC > *q_sol, int *info, std::vector< std::string > *infeasible_constraint)
inverseKinPointwise solves inverse kinematics problem at each t[i] individually
Return type for simplified versions of IK functions.
Definition: rigid_body_ik.h:65
IKResults inverseKinTrajSimple(RigidBodyTree< double > *model, const Eigen::VectorXd &t, const Eigen::MatrixXd &q_seed, const Eigen::MatrixXd &q_nom, const std::vector< RigidBodyConstraint * > &constraint_array, const IKoptions &ikoptions)
Simplified (non-template) version of inverseKinTraj.
Definition: inverse_kinematics_trajectory.cc:54
std::vector< Eigen::VectorXd > q_sol
Definition: rigid_body_ik.h:66
IKResults inverseKinSimple(RigidBodyTree< double > *model, const Eigen::VectorXd &q_seed, const Eigen::VectorXd &q_nom, const std::vector< RigidBodyConstraint * > &constraint_array, const IKoptions &ikoptions)
Simplified (non-template) version of inverseKin.
Definition: inverse_kinematics.cc:46
std::vector< std::string > infeasible_constraints
Definition: rigid_body_ik.h:68
std::vector< int > info
Definition: rigid_body_ik.h:67
void inverseKin(RigidBodyTree< double > *model, const Eigen::MatrixBase< DerivedA > &q_seed, const Eigen::MatrixBase< DerivedB > &q_nom, const int num_constraints, const RigidBodyConstraint *const *constraint_array, const IKoptions &ikoptions, Eigen::MatrixBase< DerivedC > *q_sol, int *info, std::vector< std::string > *infeasible_constraint)
inverseKin solves the inverse kinematics problem min_q (q-q_nom)&#39;Q(q-q_nom) s.t lb<=constraint(q)<=ub...
void inverseKinTraj(RigidBodyTree< double > *model, const int nT, const double *t, const Eigen::MatrixBase< DerivedA > &qdot0_seed, const Eigen::MatrixBase< DerivedB > &q_seed, const Eigen::MatrixBase< DerivedC > &q_nom, const int num_constraints, const RigidBodyConstraint *const *constraint_array, const IKoptions &ikoptions, Eigen::MatrixBase< DerivedD > *q_sol, Eigen::MatrixBase< DerivedE > *qdot_sol, Eigen::MatrixBase< DerivedF > *qddot_sol, int *info, std::vector< std::string > *infeasible_constraint)
inverseKinTraj solves the inverse kinematics problem at all time together.
Definition: ik_options.h:8
base class.
Definition: rigid_body_constraint.h:27
void approximateIK(RigidBodyTree< double > *model, const Eigen::MatrixBase< DerivedA > &q_seed, const Eigen::MatrixBase< DerivedB > &q_nom, const int num_constraints, const RigidBodyConstraint *const *constraint_array, const IKoptions &ikoptions, Eigen::MatrixBase< DerivedC > *q_sol, int *info)
approximateIK solves the same problem as inverseKin.
IKResults inverseKinPointwiseSimple(RigidBodyTree< double > *model, const Eigen::VectorXd &t, const Eigen::MatrixXd &q_seed, const Eigen::MatrixXd &q_nom, const std::vector< RigidBodyConstraint * > &constraint_array, const IKoptions &ikoptions)
Simplified (non-template) version of inverseKinPointwise.
Definition: inverse_kinematics_pointwise.cc:43