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:
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 
119  void CreateCacheElement(int num_positions, int num_velocities);
120 
122  const KinematicsCacheElement<T>& get_element(int body_id) const;
123 
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 
140  const Eigen::Matrix<T, Eigen::Dynamic, 1>& getQ() const;
141 
144  const Eigen::Matrix<T, Eigen::Dynamic, 1>& getV() const;
145 
151  Eigen::Matrix<T, Eigen::Dynamic, 1> getX() const;
152 
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
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
int v
Definition: rgbd_camera_test.cc:165
constexpr int kTwistSize
https://en.wikipedia.org/wiki/Screw_theory#Twist
Definition: constants.h:14
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:14
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