Drake
kinematics_cache-inl.h
Go to the documentation of this file.
1 #pragma once
2 
7 
9 
10 #include <string>
11 #include <vector>
12 
13 template <typename T>
15  int num_positions_joint, int num_velocities_joint)
16  : motion_subspace_in_body(drake::kTwistSize, num_velocities_joint),
17  motion_subspace_in_world(drake::kTwistSize, num_velocities_joint),
18  qdot_to_v(num_velocities_joint, num_positions_joint),
19  v_to_qdot(num_positions_joint, num_velocities_joint) {
20  // empty
21 }
22 
23 template <typename T>
25  int num_positions, int num_velocities) {
26  elements_.emplace_back(num_positions, num_velocities);
27 }
28 
29 template <typename T>
31  int num_positions, int num_velocities,
32  const std::vector<int>& num_joint_positions,
33  const std::vector<int>& num_joint_velocities)
34  : num_positions_(num_positions),
35  num_velocities_(num_velocities),
36  q(Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(num_positions_)),
37  v(Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(num_velocities_)),
38  velocity_vector_valid(false) {
39  DRAKE_DEMAND(num_joint_positions.size() == num_joint_velocities.size());
40  for (int body_id = 0;
41  body_id < static_cast<int>(num_joint_positions.size()); ++body_id) {
42  elements_.emplace_back(num_joint_positions[body_id],
43  num_joint_velocities[body_id]);
44  }
45  invalidate();
46 }
47 
48 template <typename T>
50  int body_id) const {
51  return elements_[body_id];
52 }
53 
54 template <typename T>
56  int body_id) {
57  return &elements_[body_id];
58 }
59 
60 template <typename T>
61 template <typename Derived>
62 void KinematicsCache<T>::initialize(const Eigen::MatrixBase<Derived>& q_in) {
63  static_assert(Derived::ColsAtCompileTime == 1, "q must be a vector");
65  "T type of q must match T type of KinematicsCache");
66  DRAKE_ASSERT(q.rows() == q_in.rows());
67  q = q_in;
68  invalidate();
69  velocity_vector_valid = false;
70 }
71 
72 template <typename T>
73 template <typename DerivedQ, typename DerivedV>
74 void KinematicsCache<T>::initialize(const Eigen::MatrixBase<DerivedQ>& q_in,
75  const Eigen::MatrixBase<DerivedV>& v_in) {
76  initialize(q_in); // also invalidates
77  static_assert(DerivedV::ColsAtCompileTime == 1, "v must be a vector");
79  "T type of v must match T type of KinematicsCache");
80  DRAKE_ASSERT(v.rows() == v_in.rows());
81  v = v_in;
82  velocity_vector_valid = true;
83 }
84 
85 template <typename T>
87  bool velocity_kinematics_required, bool jdot_times_v_required,
88  const std::string& method_name) const {
89  if (!position_kinematics_cached) {
90  throw std::runtime_error(method_name +
91  " requires position kinematics, which have not "
92  "been cached. Please call doKinematics.");
93  }
94  if (velocity_kinematics_required && !hasV()) {
95  throw std::runtime_error(method_name +
96  " requires velocity kinematics, which have not "
97  "been cached. Please call doKinematics with a "
98  "velocity vector.");
99  }
100  if (jdot_times_v_required && !jdotV_cached) {
101  throw std::runtime_error(method_name +
102  " requires Jdot times v, which has not been cached. Please call "
103  "doKinematics with a velocity vector and compute_JdotV set to true.");
104  }
105 }
106 
107 template <typename T>
108 const Eigen::Matrix<T, Eigen::Dynamic, 1>& KinematicsCache<T>::getQ() const {
109  return q;
110 }
111 
112 template <typename T>
113 const Eigen::Matrix<T, Eigen::Dynamic, 1>& KinematicsCache<T>::getV() const {
114  if (hasV())
115  return v;
116  else
117  throw std::runtime_error(
118  "Kinematics cache has no valid velocity vector.");
119 }
120 
121 template <typename T>
122 Eigen::Matrix<T, Eigen::Dynamic, 1> KinematicsCache<T>::getX() const {
123  if (hasV()) {
124  Eigen::Matrix<T, Eigen::Dynamic, 1> x(get_num_positions() +
126  x << q, v;
127  return x;
128  } else {
129  return getQ();
130  }
131 }
132 
133 template <typename T>
134 bool KinematicsCache<T>::hasV() const { return velocity_vector_valid; }
135 
136 template <typename T>
137 void KinematicsCache<T>::setInertiasCached() { inertias_cached = true; }
138 
139 template <typename T>
140 bool KinematicsCache<T>::areInertiasCached() { return inertias_cached; }
141 
142 template <typename T>
144  position_kinematics_cached = true;
145 }
146 
147 template <typename T>
148 void KinematicsCache<T>::setJdotVCached(bool jdotV_cached_in) {
149  jdotV_cached = jdotV_cached_in;
150 }
151 
152 template <typename T>
154  return static_cast<int>(elements_.size());
155 }
156 
157 template <typename T>
158 int KinematicsCache<T>::get_num_positions() const { return num_positions_; }
159 
160 template <typename T>
162 
163 template <typename T>
164 int KinematicsCache<T>::get_num_velocities() const { return num_velocities_; }
165 
166 template <typename T>
168  return get_num_velocities();
169 }
170 
171 template <typename T>
173  position_kinematics_cached = false;
174  jdotV_cached = false;
175  inertias_cached = false;
176 }
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:49
int num_velocities_
Definition: gyroscope_test.cc:72
std::vector< Number > x
Definition: ipopt_solver.cc:153
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:55
int get_num_cache_elements() const
Definition: kinematics_cache-inl.h:153
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:24
void initialize(const Eigen::MatrixBase< Derived > &q_in)
Definition: kinematics_cache-inl.h:62
Definition: autodiff_overloads.h:30
bool hasV() const
Returns true if this KinematicsCache object has a valid v vector.
Definition: kinematics_cache-inl.h:134
void setInertiasCached()
Definition: kinematics_cache-inl.h:137
int v
Definition: rgbd_camera_test.cc:165
#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:39
constexpr int kTwistSize
https://en.wikipedia.org/wiki/Screw_theory#Twist
Definition: constants.h:14
int get_num_velocities() const
Definition: kinematics_cache-inl.h:164
int value
Definition: copyable_unique_ptr_test.cc:61
int getNumVelocities() const
Definition: kinematics_cache-inl.h:167
#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:47
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:30
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:113
void checkCachedKinematicsSettings(bool velocity_kinematics_required, bool jdot_times_v_required, const std::string &method_name) const
Definition: kinematics_cache-inl.h:86
int num_positions_
Definition: gyroscope_test.cc:71
KinematicsCacheElement(int num_positions_joint, int num_velocities_joint)
Definition: kinematics_cache-inl.h:14
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:108
Definition: kinematics_cache.h:22
void setJdotVCached(bool jdotV_cached_in)
Definition: kinematics_cache-inl.h:148
bool areInertiasCached()
Definition: kinematics_cache-inl.h:140
int getNumPositions() const
Definition: kinematics_cache-inl.h:161
void setPositionKinematicsCached()
Definition: kinematics_cache-inl.h:143
Definition: kinematics_cache.h:74
int get_num_positions() const
Definition: kinematics_cache-inl.h:158
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:122