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"
20 
21 template <typename T>
23  public:
24  /*
25  * Configuration dependent
26  */
27 
28  Eigen::Transform<T, drake::kSpaceDimension, Eigen::Isometry>
30  Eigen::Matrix<T, drake::kTwistSize, Eigen::Dynamic, 0, drake::kTwistSize,
32  motion_subspace_in_body; // gradient w.r.t. q_i only
33  Eigen::Matrix<T, drake::kTwistSize, Eigen::Dynamic, 0, drake::kTwistSize,
35  motion_subspace_in_world; // gradient w.r.t. q
36 
37  // Jacobian matrix of quasi-coordinate variables computed with respect
38  // to generalized coordinate variables.
39  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, 0,
42 
43  // Jacobian matrix of generalized coordinate variables computed with respect
44  // to quasi-coordinate variables.
45  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, 0,
47  DrakeJoint::MAX_NUM_VELOCITIES> v_to_qdot;
50 
51  /*
52  * Configuration and velocity dependent
53  */
54 
55  // Gradient with respect to q only. The gradient with respect to v is
56  // motion_subspace_in_world.
58  // Gradient with respect to q_i and v_i only.
60  // Gradient with respect to q and v.
62 
63  public:
64  KinematicsCacheElement(int num_positions_joint, int num_velocities_joint);
65 
66  int get_num_positions() const { return static_cast<int>(v_to_qdot.rows()); }
67  int get_num_velocities() const { return static_cast<int>(v_to_qdot.cols()); }
68 
69  public:
70  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71 };
72 
73 template <typename T>
75  private:
76  std::vector<KinematicsCacheElement<T>,
77  Eigen::aligned_allocator<KinematicsCacheElement<T>>> elements_;
78  int num_positions_;
79  int num_velocities_;
80  Eigen::Matrix<T, Eigen::Dynamic, 1> q;
81  Eigen::Matrix<T, Eigen::Dynamic, 1> v;
82  bool velocity_vector_valid;
83  bool position_kinematics_cached;
84  bool jdotV_cached;
85  bool inertias_cached;
86 
87  public:
88  /// Constructor for a KinematicsCache given the number of positions and
89  /// velocities per body in the vectors @p num_joint_positions and
90  /// @p num_joint_velocities, respectively.
91  ///
92  /// For a RigidBodyTree with `nbodies` rigid bodies, `num_joint_positions`
93  /// and `num_joint_velocities` are vectors of size `nbodies` containing in
94  /// the i-th entry the number of positions and the number of velocities for
95  /// the i-th RigidBody in the RigidBodyTree.
96  ///
97  /// Note that you will typically not create a KinematicsCache object using
98  /// this constructor. Instead, you usually obtain a KinematicsCache object
99  /// by calling RigidBodyTree::CreateKinematicsCache() or
100  /// RigidBodyTree::CreateKinematicsCacheWithType(). The second option is
101  /// useful if you need a particular type for your cache like
102  /// Eigen::AutoDiffScalar.
103  ///
104  /// For examples on how to create and use the KinematicsCache, see
105  /// rigid_body_tree_dynamics_test.cc and rigid_body_tree_kinematics_test.cc.
106  ///
107  /// @param num_positions Total number of positions in the RigidBodyTree.
108  /// @param num_velocities Total number of velocities in the RigidBodyTree.
109  /// @param num_joint_positions A `std::vector<int>` containing in the i-th
110  /// entry the number of positions for the i-th body in the RigidBodyTree.
111  /// @param num_joint_velocities A `std::vector<int>` containing in the i-th
112  /// entry the number of velocities for the i-th body in the RigidBodyTree.
113  KinematicsCache(int num_positions, int num_velocities,
114  const std::vector<int>& num_joint_positions,
115  const std::vector<int>& num_joint_velocities);
116 
117  /// Requests a cache entry for a body mobilized by a joint with
118  /// @p num_positions and @p num_velocities.
119  void CreateCacheElement(int num_positions, int num_velocities);
120 
121  /// Returns constant reference to a cache entry for body @p body_id.
122  const KinematicsCacheElement<T>& get_element(int body_id) const;
123 
124  /// Returns mutable pointer to a cache entry for body @p body_id.
125  KinematicsCacheElement<T>* get_mutable_element(int body_id);
126 
127  template <typename Derived>
128  void initialize(const Eigen::MatrixBase<Derived>& q_in);
129 
130  template <typename DerivedQ, typename DerivedV>
131  void initialize(const Eigen::MatrixBase<DerivedQ>& q_in,
132  const Eigen::MatrixBase<DerivedV>& v_in);
133 
134  void checkCachedKinematicsSettings(bool velocity_kinematics_required,
135  bool jdot_times_v_required,
136  const std::string& method_name) const;
137 
138  /// Returns `q`, the generalized position vector of the RigidBodyTree that was
139  /// used to compute this KinematicsCache.
140  const Eigen::Matrix<T, Eigen::Dynamic, 1>& getQ() const;
141 
142  /// Returns `v`, the generalized velocity vector of the RigidBodyTree that was
143  /// used to compute this KinematicsCache.
144  const Eigen::Matrix<T, Eigen::Dynamic, 1>& getV() const;
145 
146  /// Returns `x`, the state vector of the RigidBodyTree that was used to
147  /// compute this KinematicsCache. This is the concatenation of `q`, the
148  /// RigidBodyTree's generalized position vector, and `v` the RigidBodyTree's
149  /// generalized velocity vector into a single vector. Within `x`, `q` precedes
150  /// `v`.
151  Eigen::Matrix<T, Eigen::Dynamic, 1> getX() const;
152 
153  /// Returns `true` if this KinematicsCache object has a valid `v` vector. `v`
154  /// is the generalized velocity vector of the RigidBodyTree that was used to
155  /// compute this KinematicsCache.
156  bool hasV() const;
157 
158  void setInertiasCached();
159 
160  bool areInertiasCached();
161 
162  void setPositionKinematicsCached();
163 
164  void setJdotVCached(bool jdotV_cached_in);
165 
166  int get_num_cache_elements() const;
167 
168  int get_num_positions() const;
169 
170 // TODO(liang.fok): Remove this deprecated method prior to Release 1.0.
171  DRAKE_DEPRECATED("Please use get_num_positions().")
172  int getNumPositions() const;
173 
174  int get_num_velocities() const;
175 
176 // TODO(liang.fok): Remove this deprecated method prior to Release 1.0.
177  DRAKE_DEPRECATED("Please use get_num_velocities().")
178  int getNumVelocities() const;
179 
180  private:
181  void invalidate();
182 
183  public:
184  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
185 };
static const int MAX_NUM_POSITIONS
Defines the maximum number of position states a joint can have.
Definition: drake_joint.h:67
int num_velocities_
Definition: gyroscope_test.cc:72
drake::SquareTwistMatrix< T > inertia_in_world
Definition: kinematics_cache.h:48
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:49
Eigen::Matrix< T, drake::kTwistSize, Eigen::Dynamic, 0, drake::kTwistSize, DrakeJoint::MAX_NUM_VELOCITIES > motion_subspace_in_body
Definition: kinematics_cache.h:32
drake::TwistVector< T > twist_in_world
Definition: kinematics_cache.h:57
std::pair< DrakeShapes::Element, DrakeShapes::Element > elements_
Definition: model_test.cc:270
drake::TwistVector< T > motion_subspace_in_body_dot_times_v
Definition: kinematics_cache.h:59
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic, 0, DrakeJoint::MAX_NUM_POSITIONS, DrakeJoint::MAX_NUM_VELOCITIES > v_to_qdot
Definition: kinematics_cache.h:47
Eigen::Matrix< T, drake::kTwistSize, Eigen::Dynamic, 0, drake::kTwistSize, DrakeJoint::MAX_NUM_VELOCITIES > motion_subspace_in_world
Definition: kinematics_cache.h:35
constexpr int kTwistSize
https://en.wikipedia.org/wiki/Screw_theory#Twist
Definition: constants.h:12
int get_num_velocities() const
Definition: kinematics_cache.h:67
Provides Drake&#39;s assertion implementation.
Eigen::Matrix< Scalar, kTwistSize, 1 > TwistVector
A column vector consisting of one twist.
Definition: eigen_types.h:117
Eigen::Transform< T, drake::kSpaceDimension, Eigen::Isometry > transform_to_world
Definition: kinematics_cache.h:29
static const int MAX_NUM_VELOCITIES
Defines the maximum number of velocity states a joint can have.
Definition: drake_joint.h:72
int get_num_positions() const
Definition: kinematics_cache.h:66
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic, 0, DrakeJoint::MAX_NUM_VELOCITIES, DrakeJoint::MAX_NUM_POSITIONS > qdot_to_v
Definition: kinematics_cache.h:41
#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
drake::TwistVector< T > motion_subspace_in_world_dot_times_v
Definition: kinematics_cache.h:61
int num_positions_
Definition: gyroscope_test.cc:71
KinematicsCacheElement(int num_positions_joint, int num_velocities_joint)
Definition: kinematics_cache-inl.h:16
Definition: kinematics_cache.h:22
Eigen::Matrix< Scalar, kTwistSize, kTwistSize > SquareTwistMatrix
A six-by-six matrix.
Definition: eigen_types.h:125
Definition: kinematics_cache.h:74