<commit_before>#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unordered_map>
#include <vector>
#include <cassert>
#include <numeric>
#include <type_traits>
#include <stdexcept>
#include <utility>
#include "drake/util/drakeGradientUtil.h"
#include "RigidBody.h"

template <typename Scalar>
class KinematicsCacheElement {
 public:
  /*
   * Configuration dependent
   */
  Eigen::Transform<Scalar, SPACE_DIMENSION, Eigen::Isometry> transform_to_world;
  Eigen::Matrix<Scalar, TWIST_SIZE, Eigen::Dynamic, 0, TWIST_SIZE,
                DrakeJoint::MAX_NUM_VELOCITIES>
      motion_subspace_in_body;  // gradient w.r.t. q_i only
  Eigen::Matrix<Scalar, TWIST_SIZE, Eigen::Dynamic, 0, TWIST_SIZE,
                DrakeJoint::MAX_NUM_VELOCITIES>
      motion_subspace_in_world;  // gradient w.r.t. q
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0,
                DrakeJoint::MAX_NUM_VELOCITIES,
                DrakeJoint::MAX_NUM_POSITIONS> qdot_to_v;  // gradient w.r.t. q
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0,
                DrakeJoint::MAX_NUM_POSITIONS,
                DrakeJoint::MAX_NUM_VELOCITIES> v_to_qdot;  // gradient w.r.t. q
  Eigen::Matrix<Scalar, TWIST_SIZE, TWIST_SIZE> inertia_in_world;
  Eigen::Matrix<Scalar, TWIST_SIZE, TWIST_SIZE> crb_in_world;

  /*
   * Configuration and velocity dependent
   */
  Eigen::Matrix<Scalar, TWIST_SIZE, 1>
      twist_in_world;  // gradient w.r.t. q only; gradient w.r.t. v is
                       // motion_subspace_in_world
  Eigen::Matrix<Scalar, TWIST_SIZE, 1>
      motion_subspace_in_body_dot_times_v;  // gradient w.r.t. q_i and v_i only
  Eigen::Matrix<Scalar, TWIST_SIZE, 1>
      motion_subspace_in_world_dot_times_v;  // gradient w.r.t. q and v

 public:
  KinematicsCacheElement(int num_positions_joint, int num_velocities_joint)
      : motion_subspace_in_body(TWIST_SIZE, num_velocities_joint),
        motion_subspace_in_world(TWIST_SIZE, num_velocities_joint),
        qdot_to_v(num_velocities_joint, num_positions_joint),
        v_to_qdot(num_positions_joint, num_velocities_joint) {
    // empty
  }

 public:
#ifndef SWIG
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

template <typename Scalar>
class KinematicsCache {
 private:
  std::unordered_map<
      RigidBody const*, KinematicsCacheElement<Scalar>,
      std::hash<RigidBody const*>, std::equal_to<RigidBody const*>,
      Eigen::aligned_allocator<std::pair<
          RigidBody const* const, KinematicsCacheElement<Scalar>>>> elements;
  std::vector<RigidBody const*> bodies;
  const int num_positions;
  const int num_velocities;
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> q;
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> v;
  bool velocity_vector_valid;
  bool position_kinematics_cached;
  bool jdotV_cached;
  bool inertias_cached;

 public:
  explicit KinematicsCache(
      const std::vector<std::unique_ptr<RigidBody>>& bodies)
      : num_positions(getNumPositions(bodies)),
        num_velocities(getNumVelocities(bodies)),
        q(Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(num_positions)),
        v(Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(num_velocities)),
        velocity_vector_valid(false) {
    for (const auto& body_unique_ptr : bodies) {
      const RigidBody& body = *body_unique_ptr;
      int num_positions_joint =
          body.hasParent() ? body.getJoint().getNumPositions() : 0;
      int num_velocities_joint =<commit_msg>Cleanup nested template in drake/systems/plants/KinematicsCache.h<commit_after>#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unordered_map>
#include <vector>
#include <cassert>
#include <numeric>
#include <type_traits>
#include <stdexcept>
#include <utility>
#include "drake/util/drakeGradientUtil.h"
#include "RigidBody.h"

template <typename Scalar>
class KinematicsCacheElement {
 public:
  /*
   * Configuration dependent
   */
  Eigen::Transform<Scalar, SPACE_DIMENSION, Eigen::Isometry> transform_to_world;
  Eigen::Matrix<Scalar, TWIST_SIZE, Eigen::Dynamic, 0, TWIST_SIZE,
                DrakeJoint::MAX_NUM_VELOCITIES>
      motion_subspace_in_body;  // gradient w.r.t. q_i only
  Eigen::Matrix<Scalar, TWIST_SIZE, Eigen::Dynamic, 0, TWIST_SIZE,
                DrakeJoint::MAX_NUM_VELOCITIES>
      motion_subspace_in_world;  // gradient w.r.t. q
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0,
                DrakeJoint::MAX_NUM_VELOCITIES,
                DrakeJoint::MAX_NUM_POSITIONS> qdot_to_v;  // gradient w.r.t. q
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0,
                DrakeJoint::MAX_NUM_POSITIONS,
                DrakeJoint::MAX_NUM_VELOCITIES> v_to_qdot;  // gradient w.r.t. q
  Eigen::Matrix<Scalar, TWIST_SIZE, TWIST_SIZE> inertia_in_world;
  Eigen::Matrix<Scalar, TWIST_SIZE, TWIST_SIZE> crb_in_world;

  /*
   * Configuration and velocity dependent
   */
  Eigen::Matrix<Scalar, TWIST_SIZE, 1>
      twist_in_world;  // gradient w.r.t. q only; gradient w.r.t. v is
                       // motion_subspace_in_world
  Eigen::Matrix<Scalar, TWIST_SIZE, 1>
      motion_subspace_in_body_dot_times_v;  // gradient w.r.t. q_i and v_i only
  Eigen::Matrix<Scalar, TWIST_SIZE, 1>
      motion_subspace_in_world_dot_times_v;  // gradient w.r.t. q and v

 public:
  KinematicsCacheElement(int num_positions_joint, int num_velocities_joint)
      : motion_subspace_in_body(TWIST_SIZE, num_velocities_joint),
        motion_subspace_in_world(TWIST_SIZE, num_velocities_joint),
        qdot_to_v(num_velocities_joint, num_positions_joint),
        v_to_qdot(num_positions_joint, num_velocities_joint) {
    // empty
  }

 public:
#ifndef SWIG
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

template <typename Scalar>
class KinematicsCache {
 private:
  typedef KinematicsCacheElement<Scalar> KinematicsCacheElementScalar;
  typedef std::pair<RigidBody const* const, KinematicsCacheElementScalar>
      RigidBodyKCacheElementPair;
  typedef Eigen::aligned_allocator<RigidBodyKCacheElementPair>
      RigidBodyKCacheElementPairAllocator;
  typedef std::unordered_map<
      RigidBody const*, KinematicsCacheElementScalar,
      std::hash<RigidBody const*>, std::equal_to<RigidBody const*>,
      RigidBodyKCacheElementPairAllocator> RigidBodyToKCacheElementMap;

  RigidBodyToKCacheElementMap elements;
  std::vector<RigidBody const*> bodies;
  const int num_positions;
  const int num_velocities;
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> q;
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> v;
  bool velocity_vector_valid;
  bool position_kinematics_cached;
  bool jdotV_cached;
  bool inertias_cached;

 public:
  explicit KinematicsCache(
      const std::vector<std::unique_ptr<RigidBody>>& bodies)
      : num_positions(getNumPositions(bodies)),
        num_velocities(getNumVelocities(bodies)),
        q(Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(num_positions)),
        v(Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(num_velocities)),
        velocity_vector_valid(false) {
    for (const auto& body_unique_ptr : bodies) {
      const RigidBody& body = *body_unique_ptr;
      int num_positions_joint =
          body.hasParent() ? body.getJoint().getNumPositions() : 0;
      int num_velocities_joint =
{'fraction_non_alphanumeric': 0.10650069156293222, 'fraction_numerical': 0.0033195020746887966, 'mean_word_length': 4.274252370532458, 'pattern_counts': {'":': 0, '<': 72, '<?xml version=': 0, '>': 72, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3732291', 'n_tokens_mistral': 2641, 'n_tokens_neox': 2503, 'n_words': 447}