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 
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 
65 struct IKResults {
66  std::vector<Eigen::VectorXd> q_sol;
67  std::vector<int> info;
68  std::vector<std::string> infeasible_constraints;
69 };
70 
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 
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 
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 
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 
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.
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