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 <algorithm>
13 #include <string>
14 #include <vector>
15 
16 template <typename T>
18  int num_positions_joint, int num_velocities_joint)
19  : motion_subspace_in_body(drake::kTwistSize, num_velocities_joint),
20  motion_subspace_in_world(drake::kTwistSize, num_velocities_joint),
21  qdot_to_v(num_velocities_joint, num_positions_joint),
22  v_to_qdot(num_positions_joint, num_velocities_joint) {
23  // empty
24 }
25 
26 template <typename T>
28  int num_positions, int num_velocities) {
29  elements_.emplace_back(num_positions, num_velocities);
30 }
31 
32 template <typename T>
34  int num_positions, int num_velocities,
35  const std::vector<int>& num_joint_positions,
36  const std::vector<int>& num_joint_velocities)
37  : num_positions_(num_positions),
38  num_velocities_(num_velocities),
39  q(Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(num_positions_)),
40  v(Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(num_velocities_)),
41  velocity_vector_valid(false) {
42  DRAKE_DEMAND(num_joint_positions.size() == num_joint_velocities.size());
43  for (int body_id = 0;
44  body_id < static_cast<int>(num_joint_positions.size()); ++body_id) {
45  elements_.emplace_back(num_joint_positions[body_id],
46  num_joint_velocities[body_id]);
47  }
48 
49 
50  geometric_jacobian_temp.kinematic_path.joint_path.reserve(num_positions_);
52  kinematic_path.joint_direction_signs.reserve(num_positions_);
53  geometric_jacobian_temp.kinematic_path.body_path.reserve(num_positions_);
54  geometric_jacobian_temp.start_body_ancestors.reserve(num_positions_);
55  geometric_jacobian_temp.end_body_ancestors.reserve(num_positions_);
56  spatial_velocity_jacobian_temp.J_positions.setZero(6, num_positions_);
57  spatial_velocity_jacobian_temp.J_velocities.setZero(6, num_velocities_);
59  v_or_q_indices.reserve(std::max(num_positions_, num_velocities_));
60 
61  invalidate();
62 }
63 
64 template <typename T>
66  int body_id) const {
67  return elements_[body_id];
68 }
69 
70 template <typename T>
72  int body_id) {
73  return &elements_[body_id];
74 }
75 
76 template <typename T>
77 template <typename Derived>
78 void KinematicsCache<T>::initialize(const Eigen::MatrixBase<Derived>& q_in) {
79  static_assert(Derived::ColsAtCompileTime == 1, "q must be a vector");
81  "T type of q must match T type of KinematicsCache");
82  DRAKE_DEMAND(q.rows() == q_in.rows());
83  q = q_in;
84  invalidate();
85  velocity_vector_valid = false;
86 }
87 
88 template <typename T>
89 template <typename DerivedQ, typename DerivedV>
90 void KinematicsCache<T>::initialize(const Eigen::MatrixBase<DerivedQ>& q_in,
91  const Eigen::MatrixBase<DerivedV>& v_in) {
92  initialize(q_in); // also invalidates
93  static_assert(DerivedV::ColsAtCompileTime == 1, "v must be a vector");
95  "T type of v must match T type of KinematicsCache");
96  DRAKE_DEMAND(v.rows() == v_in.rows());
97  v = v_in;
98  velocity_vector_valid = true;
99 }
100 
101 template <typename T>
103  bool velocity_kinematics_required, bool jdot_times_v_required,
104  const char* method_name) const {
105  if (!position_kinematics_cached) {
106  throw std::runtime_error(std::string(method_name) +
107  " requires position kinematics, which have not "
108  "been cached. Please call doKinematics.");
109  }
110  if (velocity_kinematics_required && !hasV()) {
111  throw std::runtime_error(std::string(method_name) +
112  " requires velocity kinematics, which have not "
113  "been cached. Please call doKinematics with a "
114  "velocity vector.");
115  }
116  if (jdot_times_v_required && !jdotV_cached) {
117  throw std::runtime_error(std::string(method_name) +
118  " requires Jdot times v, which has not been cached. Please call "
119  "doKinematics with a velocity vector and compute_JdotV set to true.");
120  }
121 }
122 
123 template <typename T>
124 const Eigen::Matrix<T, Eigen::Dynamic, 1>& KinematicsCache<T>::getQ() const {
125  return q;
126 }
127 
128 template <typename T>
129 const Eigen::Matrix<T, Eigen::Dynamic, 1>& KinematicsCache<T>::getV() const {
130  if (hasV())
131  return v;
132  else
133  throw std::runtime_error(
134  "Kinematics cache has no valid velocity vector.");
135 }
136 
137 template <typename T>
138 Eigen::Matrix<T, Eigen::Dynamic, 1> KinematicsCache<T>::getX() const {
139  if (hasV()) {
140  Eigen::Matrix<T, Eigen::Dynamic, 1> x(get_num_positions() +
142  x << q, v;
143  return x;
144  } else {
145  return getQ();
146  }
147 }
148 
149 template <typename T>
150 bool KinematicsCache<T>::hasV() const { return velocity_vector_valid; }
151 
152 template <typename T>
153 void KinematicsCache<T>::setInertiasCached() { inertias_cached = true; }
154 
155 template <typename T>
156 bool KinematicsCache<T>::areInertiasCached() { return inertias_cached; }
157 
158 template <typename T>
160  position_kinematics_cached = true;
161 }
162 
163 template <typename T>
164 void KinematicsCache<T>::setJdotVCached(bool jdotV_cached_in) {
165  jdotV_cached = jdotV_cached_in;
166 }
167 
168 template <typename T>
170  return static_cast<int>(elements_.size());
171 }
172 
173 template <typename T>
174 int KinematicsCache<T>::get_num_positions() const { return num_positions_; }
175 
176 template <typename T>
178 
179 template <typename T>
180 int KinematicsCache<T>::get_num_velocities() const { return num_velocities_; }
181 
182 template <typename T>
184  return get_num_velocities();
185 }
186 
187 template <typename T>
189  position_kinematics_cached = false;
190  jdotV_cached = false;
191  inertias_cached = false;
192 }
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:65
std::vector< int > start_body_ancestors
Definition: kinematics_cache.h:97
int num_velocities_
Definition: gyroscope_test.cc:72
KinematicPath kinematic_path
Definition: kinematics_cache.h:96
double value
Definition: wrap_test_util_py.cc:12
std::vector< Number > x
Definition: ipopt_solver.cc:150
Definition: automotive_demo.cc:90
KinematicsCacheElement< T > * get_mutable_element(int body_id)
Returns mutable pointer to a cache entry for body body_id.
Definition: kinematics_cache-inl.h:71
int get_num_cache_elements() const
Definition: kinematics_cache-inl.h:169
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:27
void initialize(const Eigen::MatrixBase< Derived > &q_in)
Definition: kinematics_cache-inl.h:78
Definition: autodiff_overloads.h:34
bool hasV() const
Returns true if this KinematicsCache object has a valid v vector.
Definition: kinematics_cache-inl.h:150
void setInertiasCached()
Definition: kinematics_cache-inl.h:153
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:180
int getNumVelocities() const
Definition: kinematics_cache-inl.h:183
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:711
drake::Matrix6X< T > J_positions
Jacobians used as an intermediate representation since the Jacobian can not be transformed in place...
Definition: kinematics_cache.h:107
drake::Matrix6X< T > J_velocities
Definition: kinematics_cache.h:108
#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
std::vector< int > body_path
Definition: kinematic_path.h:9
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:33
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:129
void checkCachedKinematicsSettings(bool velocity_kinematics_required, bool jdot_times_v_required, const char *method_name) const
Definition: kinematics_cache-inl.h:102
std::vector< int > joint_path
Definition: kinematic_path.h:7
int num_positions_
Definition: gyroscope_test.cc:71
KinematicsCacheElement(int num_positions_joint, int num_velocities_joint)
Definition: kinematics_cache-inl.h:17
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:124
Definition: kinematics_cache.h:23
void setJdotVCached(bool jdotV_cached_in)
Definition: kinematics_cache-inl.h:164
bool areInertiasCached()
Definition: kinematics_cache-inl.h:156
std::vector< int > end_body_ancestors
Definition: kinematics_cache.h:98
int getNumPositions() const
Definition: kinematics_cache-inl.h:177
void setPositionKinematicsCached()
Definition: kinematics_cache-inl.h:159
Definition: kinematics_cache.h:75
int get_num_positions() const
Definition: kinematics_cache-inl.h:174
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:138
DataInCalcFrameSpatialVelocityJacobianInWorldFrame spatial_velocity_jacobian_temp
Definition: kinematics_cache.h:113
DataInGeometricJacobian geometric_jacobian_temp
Definition: kinematics_cache.h:100