Drake
kinematics_cache.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <functional>
4 #include <numeric>
5 #include <stdexcept>
6 #include <string>
7 #include <type_traits>
8 #include <unordered_map>
9 #include <utility>
10 #include <vector>
11 
12 #include <Eigen/Core>
13 #include <Eigen/Geometry>
14 
15 #include "drake/common/constants.h"
19 #include "drake/multibody/joints/drake_joint.h"
20 #include "drake/multibody/kinematic_path.h"
21 
22 template <typename T>
24  public:
25  /*
26  * Configuration dependent
27  */
28 
29  Eigen::Transform<T, drake::kSpaceDimension, Eigen::Isometry>
31  Eigen::Matrix<T, drake::kTwistSize, Eigen::Dynamic, 0, drake::kTwistSize,
33  motion_subspace_in_body; // gradient w.r.t. q_i only
34  Eigen::Matrix<T, drake::kTwistSize, Eigen::Dynamic, 0, drake::kTwistSize,
36  motion_subspace_in_world; // gradient w.r.t. q
37 
38  // Jacobian matrix of quasi-coordinate variables computed with respect
39  // to generalized coordinate variables.
40  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, 0,
43 
44  // Jacobian matrix of generalized coordinate variables computed with respect
45  // to quasi-coordinate variables.
46  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, 0,
48  DrakeJoint::MAX_NUM_VELOCITIES> v_to_qdot;
51 
52  /*
53  * Configuration and velocity dependent
54  */
55 
56  // Gradient with respect to q only. The gradient with respect to v is
57  // motion_subspace_in_world.
59  // Gradient with respect to q_i and v_i only.
61  // Gradient with respect to q and v.
63 
64  public:
65  KinematicsCacheElement(int num_positions_joint, int num_velocities_joint);
66 
67  int get_num_positions() const { return static_cast<int>(v_to_qdot.rows()); }
68  int get_num_velocities() const { return static_cast<int>(v_to_qdot.cols()); }
69 
70  public:
71  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 };
73 
74 template <typename T>
76  private:
77  std::vector<KinematicsCacheElement<T>,
78  Eigen::aligned_allocator<KinematicsCacheElement<T>>> elements_;
79  int num_positions_;
80  int num_velocities_;
81  Eigen::Matrix<T, Eigen::Dynamic, 1> q;
82  Eigen::Matrix<T, Eigen::Dynamic, 1> v;
83  bool velocity_vector_valid;
84  bool position_kinematics_cached;
85  bool jdotV_cached;
86  bool inertias_cached;
87 
88  public:
89  /// Preallocated scratch pad variables. These variables are used to prevent
90  /// dynamic memory allocation during runtime.
91 
92  /// Preallocated variables used in GeometricJacobian. Preallocated as the size
93  /// of the path is dependent on the base body/frame and end effector
94  /// body/frame.
97  std::vector<int> start_body_ancestors;
98  std::vector<int> end_body_ancestors;
99  };
100  mutable DataInGeometricJacobian geometric_jacobian_temp;
101 
102  /// Preallocated variables used in
103  /// CalcFrameSpatialVelocityJacobianInWorldFrame.
105  /// Jacobians used as an intermediate representation since the Jacobian can
106  /// not be transformed in place.
109  /// Vector of indices used to transform to the world frame.
110  std::vector<int> v_or_q_indices;
111  };
114 
115  /// Constructor for a KinematicsCache given the number of positions and
116  /// velocities per body in the vectors @p num_joint_positions and
117  /// @p num_joint_velocities, respectively.
118  ///
119  /// For a RigidBodyTree with `nbodies` rigid bodies, `num_joint_positions`
120  /// and `num_joint_velocities` are vectors of size `nbodies` containing in
121  /// the i-th entry the number of positions and the number of velocities for
122  /// the i-th RigidBody in the RigidBodyTree.
123  ///
124  /// Note that you will typically not create a KinematicsCache object using
125  /// this constructor. Instead, you usually obtain a KinematicsCache object
126  /// by calling RigidBodyTree::CreateKinematicsCache() or
127  /// RigidBodyTree::CreateKinematicsCacheWithType(). The second option is
128  /// useful if you need a particular type for your cache like
129  /// Eigen::AutoDiffScalar.
130  ///
131  /// For examples on how to create and use the KinematicsCache, see
132  /// rigid_body_tree_dynamics_test.cc and rigid_body_tree_kinematics_test.cc.
133  ///
134  /// @param num_positions Total number of positions in the RigidBodyTree.
135  /// @param num_velocities Total number of velocities in the RigidBodyTree.
136  /// @param num_joint_positions A `std::vector<int>` containing in the i-th
137  /// entry the number of positions for the i-th body in the RigidBodyTree.
138  /// @param num_joint_velocities A `std::vector<int>` containing in the i-th
139  /// entry the number of velocities for the i-th body in the RigidBodyTree.
140  KinematicsCache(int num_positions, int num_velocities,
141  const std::vector<int>& num_joint_positions,
142  const std::vector<int>& num_joint_velocities);
143 
144  /// Requests a cache entry for a body mobilized by a joint with
145  /// @p num_positions and @p num_velocities.
146  void CreateCacheElement(int num_positions, int num_velocities);
147 
148  /// Returns constant reference to a cache entry for body @p body_id.
149  const KinematicsCacheElement<T>& get_element(int body_id) const;
150 
151  /// Returns mutable pointer to a cache entry for body @p body_id.
152  KinematicsCacheElement<T>* get_mutable_element(int body_id);
153 
154  template <typename Derived>
155  void initialize(const Eigen::MatrixBase<Derived>& q_in);
156 
157  template <typename DerivedQ, typename DerivedV>
158  void initialize(const Eigen::MatrixBase<DerivedQ>& q_in,
159  const Eigen::MatrixBase<DerivedV>& v_in);
160 
161  void checkCachedKinematicsSettings(bool velocity_kinematics_required,
162  bool jdot_times_v_required,
163  const char* method_name) const;
164 
165  /// Returns `q`, the generalized position vector of the RigidBodyTree that was
166  /// used to compute this KinematicsCache.
167  const Eigen::Matrix<T, Eigen::Dynamic, 1>& getQ() const;
168 
169  /// Returns `v`, the generalized velocity vector of the RigidBodyTree that was
170  /// used to compute this KinematicsCache.
171  const Eigen::Matrix<T, Eigen::Dynamic, 1>& getV() const;
172 
173  /// Returns `x`, the state vector of the RigidBodyTree that was used to
174  /// compute this KinematicsCache. This is the concatenation of `q`, the
175  /// RigidBodyTree's generalized position vector, and `v` the RigidBodyTree's
176  /// generalized velocity vector into a single vector. Within `x`, `q` precedes
177  /// `v`.
178  Eigen::Matrix<T, Eigen::Dynamic, 1> getX() const;
179 
180  /// Returns `true` if this KinematicsCache object has a valid `v` vector. `v`
181  /// is the generalized velocity vector of the RigidBodyTree that was used to
182  /// compute this KinematicsCache.
183  bool hasV() const;
184 
185  void setInertiasCached();
186 
187  bool areInertiasCached();
188 
189  void setPositionKinematicsCached();
190 
191  void setJdotVCached(bool jdotV_cached_in);
192 
193  int get_num_cache_elements() const;
194 
195  int get_num_positions() const;
196 
197 // TODO(liang.fok): Remove this deprecated method prior to Release 1.0.
198  DRAKE_DEPRECATED("Please use get_num_positions().")
199  int getNumPositions() const;
200 
201  int get_num_velocities() const;
202 
203 // TODO(liang.fok): Remove this deprecated method prior to Release 1.0.
204  DRAKE_DEPRECATED("Please use get_num_velocities().")
205  int getNumVelocities() const;
206 
207  private:
208  void invalidate();
209 
210  public:
211  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
212 };
Definition: kinematic_path.h:6
static const int MAX_NUM_POSITIONS
Defines the maximum number of position states a joint can have.
Definition: drake_joint.h:71
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
drake::SquareTwistMatrix< T > inertia_in_world
Definition: kinematics_cache.h:49
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Provides a portable macro for use in generating compile-time warnings for use of code that is permitt...
drake::SquareTwistMatrix< T > crb_in_world
Definition: kinematics_cache.h:50
Eigen::Matrix< T, drake::kTwistSize, Eigen::Dynamic, 0, drake::kTwistSize, DrakeJoint::MAX_NUM_VELOCITIES > motion_subspace_in_body
Definition: kinematics_cache.h:33
drake::TwistVector< T > twist_in_world
Definition: kinematics_cache.h:58
std::pair< DrakeShapes::Element, DrakeShapes::Element > elements_
Definition: model_test.cc:270
Eigen::Matrix< Scalar, 6, Eigen::Dynamic > Matrix6X
A matrix of 6 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:104
Preallocated scratch pad variables.
Definition: kinematics_cache.h:95
drake::TwistVector< T > motion_subspace_in_body_dot_times_v
Definition: kinematics_cache.h:60
double T
Definition: benchmarks_py.cc:14
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic, 0, DrakeJoint::MAX_NUM_POSITIONS, DrakeJoint::MAX_NUM_VELOCITIES > v_to_qdot
Definition: kinematics_cache.h:48
Preallocated variables used in CalcFrameSpatialVelocityJacobianInWorldFrame.
Definition: kinematics_cache.h:104
Eigen::Matrix< T, drake::kTwistSize, Eigen::Dynamic, 0, drake::kTwistSize, DrakeJoint::MAX_NUM_VELOCITIES > motion_subspace_in_world
Definition: kinematics_cache.h:36
constexpr int kTwistSize
https://en.wikipedia.org/wiki/Screw_theory#Twist
Definition: constants.h:12
int get_num_velocities() const
Definition: kinematics_cache.h:68
Provides Drake&#39;s assertion implementation.
Eigen::Matrix< Scalar, kTwistSize, 1 > TwistVector
A column vector consisting of one twist.
Definition: eigen_types.h:135
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
Eigen::Transform< T, drake::kSpaceDimension, Eigen::Isometry > transform_to_world
Definition: kinematics_cache.h:30
drake::Matrix6X< T > J_velocities
Definition: kinematics_cache.h:108
static const int MAX_NUM_VELOCITIES
Defines the maximum number of velocity states a joint can have.
Definition: drake_joint.h:76
int get_num_positions() const
Definition: kinematics_cache.h:67
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic, 0, DrakeJoint::MAX_NUM_VELOCITIES, DrakeJoint::MAX_NUM_POSITIONS > qdot_to_v
Definition: kinematics_cache.h:42
#define DRAKE_DEPRECATED(message)
Use DRAKE_DEPRECATED("message") to discourage use of particular classes, typedefs, variables, non-static data members, functions, arguments, enumerations, and template specializations.
Definition: drake_deprecated.h:33
std::vector< int > v_or_q_indices
Vector of indices used to transform to the world frame.
Definition: kinematics_cache.h:110
drake::TwistVector< T > motion_subspace_in_world_dot_times_v
Definition: kinematics_cache.h:62
int num_positions_
Definition: gyroscope_test.cc:71
KinematicsCacheElement(int num_positions_joint, int num_velocities_joint)
Definition: kinematics_cache-inl.h:17
Definition: kinematics_cache.h:23
std::vector< int > end_body_ancestors
Definition: kinematics_cache.h:98
Eigen::Matrix< Scalar, kTwistSize, kTwistSize > SquareTwistMatrix
A six-by-six matrix.
Definition: eigen_types.h:143
Definition: kinematics_cache.h:75
DataInCalcFrameSpatialVelocityJacobianInWorldFrame spatial_velocity_jacobian_temp
Definition: kinematics_cache.h:113
DataInGeometricJacobian geometric_jacobian_temp
Definition: kinematics_cache.h:100