Drake
kinematics_cache-inl.h
Go to the documentation of this file.
1 #pragma once
2 
3 /// @file
4 /// Template method implementations for kinematics_cache.h.
5 /// Most users should only include that file, not this one.
6 /// For background, see http://drake.mit.edu/cxx_inl.html.
7 
8 /* clang-format off to disable clang-format-includes */
10 /* clang-format on */
11 
12 #include <string>
13 #include <vector>
14 
15 template <typename T>
17  int num_positions_joint, int num_velocities_joint)
18  : motion_subspace_in_body(drake::kTwistSize, num_velocities_joint),
19  motion_subspace_in_world(drake::kTwistSize, num_velocities_joint),
20  qdot_to_v(num_velocities_joint, num_positions_joint),
21  v_to_qdot(num_positions_joint, num_velocities_joint) {
22  // empty
23 }
24 
25 template <typename T>
27  int num_positions, int num_velocities) {
28  elements_.emplace_back(num_positions, num_velocities);
29 }
30 
31 template <typename T>
33  int num_positions, int num_velocities,
34  const std::vector<int>& num_joint_positions,
35  const std::vector<int>& num_joint_velocities)
36  : num_positions_(num_positions),
37  num_velocities_(num_velocities),
38  q(Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(num_positions_)),
39  v(Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(num_velocities_)),
40  velocity_vector_valid(false) {
41  DRAKE_DEMAND(num_joint_positions.size() == num_joint_velocities.size());
42  for (int body_id = 0;
43  body_id < static_cast<int>(num_joint_positions.size()); ++body_id) {
44  elements_.emplace_back(num_joint_positions[body_id],
45  num_joint_velocities[body_id]);
46  }
47  invalidate();
48 }
49 
50 template <typename T>
52  int body_id) const {
53  return elements_[body_id];
54 }
55 
56 template <typename T>
58  int body_id) {
59  return &elements_[body_id];
60 }
61 
62 template <typename T>
63 template <typename Derived>
64 void KinematicsCache<T>::initialize(const Eigen::MatrixBase<Derived>& q_in) {
65  static_assert(Derived::ColsAtCompileTime == 1, "q must be a vector");
67  "T type of q must match T type of KinematicsCache");
68  DRAKE_ASSERT(q.rows() == q_in.rows());
69  q = q_in;
70  invalidate();
71  velocity_vector_valid = false;
72 }
73 
74 template <typename T>
75 template <typename DerivedQ, typename DerivedV>
76 void KinematicsCache<T>::initialize(const Eigen::MatrixBase<DerivedQ>& q_in,
77  const Eigen::MatrixBase<DerivedV>& v_in) {
78  initialize(q_in); // also invalidates
79  static_assert(DerivedV::ColsAtCompileTime == 1, "v must be a vector");
81  "T type of v must match T type of KinematicsCache");
82  DRAKE_ASSERT(v.rows() == v_in.rows());
83  v = v_in;
84  velocity_vector_valid = true;
85 }
86 
87 template <typename T>
89  bool velocity_kinematics_required, bool jdot_times_v_required,
90  const std::string& method_name) const {
91  if (!position_kinematics_cached) {
92  throw std::runtime_error(method_name +
93  " requires position kinematics, which have not "
94  "been cached. Please call doKinematics.");
95  }
96  if (velocity_kinematics_required && !hasV()) {
97  throw std::runtime_error(method_name +
98  " requires velocity kinematics, which have not "
99  "been cached. Please call doKinematics with a "
100  "velocity vector.");
101  }
102  if (jdot_times_v_required && !jdotV_cached) {
103  throw std::runtime_error(method_name +
104  " requires Jdot times v, which has not been cached. Please call "
105  "doKinematics with a velocity vector and compute_JdotV set to true.");
106  }
107 }
108 
109 template <typename T>
110 const Eigen::Matrix<T, Eigen::Dynamic, 1>& KinematicsCache<T>::getQ() const {
111  return q;
112 }
113 
114 template <typename T>
115 const Eigen::Matrix<T, Eigen::Dynamic, 1>& KinematicsCache<T>::getV() const {
116  if (hasV())
117  return v;
118  else
119  throw std::runtime_error(
120  "Kinematics cache has no valid velocity vector.");
121 }
122 
123 template <typename T>
124 Eigen::Matrix<T, Eigen::Dynamic, 1> KinematicsCache<T>::getX() const {
125  if (hasV()) {
126  Eigen::Matrix<T, Eigen::Dynamic, 1> x(get_num_positions() +
128  x << q, v;
129  return x;
130  } else {
131  return getQ();
132  }
133 }
134 
135 template <typename T>
136 bool KinematicsCache<T>::hasV() const { return velocity_vector_valid; }
137 
138 template <typename T>
139 void KinematicsCache<T>::setInertiasCached() { inertias_cached = true; }
140 
141 template <typename T>
142 bool KinematicsCache<T>::areInertiasCached() { return inertias_cached; }
143 
144 template <typename T>
146  position_kinematics_cached = true;
147 }
148 
149 template <typename T>
150 void KinematicsCache<T>::setJdotVCached(bool jdotV_cached_in) {
151  jdotV_cached = jdotV_cached_in;
152 }
153 
154 template <typename T>
156  return static_cast<int>(elements_.size());
157 }
158 
159 template <typename T>
160 int KinematicsCache<T>::get_num_positions() const { return num_positions_; }
161 
162 template <typename T>
164 
165 template <typename T>
166 int KinematicsCache<T>::get_num_velocities() const { return num_velocities_; }
167 
168 template <typename T>
170  return get_num_velocities();
171 }
172 
173 template <typename T>
175  position_kinematics_cached = false;
176  jdotV_cached = false;
177  inertias_cached = false;
178 }
const KinematicsCacheElement< T > & get_element(int body_id) const
Returns constant reference to a cache entry for body body_id.
Definition: kinematics_cache-inl.h:51
int num_velocities_
Definition: gyroscope_test.cc:72
std::vector< Number > x
Definition: ipopt_solver.cc:150
Definition: automotive_demo.cc:88
KinematicsCacheElement< T > * get_mutable_element(int body_id)
Returns mutable pointer to a cache entry for body body_id.
Definition: kinematics_cache-inl.h:57
int get_num_cache_elements() const
Definition: kinematics_cache-inl.h:155
std::pair< DrakeShapes::Element, DrakeShapes::Element > elements_
Definition: model_test.cc:270
void CreateCacheElement(int num_positions, int num_velocities)
Requests a cache entry for a body mobilized by a joint with num_positions and num_velocities.
Definition: kinematics_cache-inl.h:26
void initialize(const Eigen::MatrixBase< Derived > &q_in)
Definition: kinematics_cache-inl.h:64
Definition: autodiff_overloads.h:34
bool hasV() const
Returns true if this KinematicsCache object has a valid v vector.
Definition: kinematics_cache-inl.h:136
void setInertiasCached()
Definition: kinematics_cache-inl.h:139
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
constexpr int kTwistSize
https://en.wikipedia.org/wiki/Screw_theory#Twist
Definition: constants.h:12
int get_num_velocities() const
Definition: kinematics_cache-inl.h:166
int value
Definition: copyable_unique_ptr_test.cc:61
int getNumVelocities() const
Definition: kinematics_cache-inl.h:169
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
KinematicsCache(int num_positions, int num_velocities, const std::vector< int > &num_joint_positions, const std::vector< int > &num_joint_velocities)
Constructor for a KinematicsCache given the number of positions and velocities per body in the vector...
Definition: kinematics_cache-inl.h:32
const Eigen::Matrix< T, Eigen::Dynamic, 1 > & getV() const
Returns v, the generalized velocity vector of the RigidBodyTree that was used to compute this Kinemat...
Definition: kinematics_cache-inl.h:115
void checkCachedKinematicsSettings(bool velocity_kinematics_required, bool jdot_times_v_required, const std::string &method_name) const
Definition: kinematics_cache-inl.h:88
int num_positions_
Definition: gyroscope_test.cc:71
KinematicsCacheElement(int num_positions_joint, int num_velocities_joint)
Definition: kinematics_cache-inl.h:16
const Eigen::Matrix< T, Eigen::Dynamic, 1 > & getQ() const
Returns q, the generalized position vector of the RigidBodyTree that was used to compute this Kinemat...
Definition: kinematics_cache-inl.h:110
Definition: kinematics_cache.h:22
void setJdotVCached(bool jdotV_cached_in)
Definition: kinematics_cache-inl.h:150
bool areInertiasCached()
Definition: kinematics_cache-inl.h:142
int getNumPositions() const
Definition: kinematics_cache-inl.h:163
void setPositionKinematicsCached()
Definition: kinematics_cache-inl.h:145
Definition: kinematics_cache.h:74
int get_num_positions() const
Definition: kinematics_cache-inl.h:160
Eigen::Matrix< T, Eigen::Dynamic, 1 > getX() const
Returns x, the state vector of the RigidBodyTree that was used to compute this KinematicsCache.
Definition: kinematics_cache-inl.h:124