<commit_before>#include <memory>
#include <stdexcept>
#include <string>

#include "drake/Path.h"
#include "drake/systems/plants/RigidBodyTree.h"
#include "drake/util/testUtil.h"

//#include "drake/systems/plants/BotVisualizer.h"
//#include "drake/systems/plants/RigidBodySystem.h"

#include "gtest/gtest.h"
#include "gmock/gmock.h"

#include <iostream>
#define PRINT_VAR(x) std::cout <<  #x ": " << x << std::endl;

namespace drake {
namespace {

using Eigen::Vector3d;
using Eigen::VectorXd;
using Eigen::Matrix3Xd;
//using Drake::BotVisualizer;
//using Drake::RigidBodySystem;

class RBTCollisionTest: public ::testing::Test {
 protected:
  void SetUp() override {
    tree_.addRobotFromSDF(
        Drake::getDrakePath() + "/systems/plants/test/collision_test.sdf",
        DrakeJoint::QUATERNION);
  }

  RigidBodyTree tree_;
};

TEST_F(RBTCollisionTest, FindAndComputeContactPoints) {
  // Numerical precision tolerance to perform floating point comparisons.
  // Its magnitude was chosen to be the minimum value for which these tests can
  // successfully pass.
  const double tolerance = 2.0e-9;

  int nq = tree_.number_of_positions();
  int nv = tree_.number_of_velocities();
  int num_states = nq + nv;
  VectorXd x = VectorXd::Zero(num_states);
  x.head(nq) = tree_.getZeroConfiguration();

  auto q = x.topRows(nq);
  auto v = x.bottomRows(nv);

  auto kinsol = tree_.doKinematics(q, v);

  VectorXd phi;
  Matrix3Xd normal, xA, xB;
  std::vector<int> bodyA_idx, bodyB_idx;

  tree_.FindAndComputeContactPoints(
      kinsol, phi, normal, xA, xB, bodyA_idx, bodyB_idx, false);
<commit_msg>Updated rbt_compute_collisions.cc to use tree_.ComputeMaximumDepthCollisionPoints<commit_after>#include <memory>
#include <stdexcept>
#include <string>

#include "drake/Path.h"
#include "drake/systems/plants/RigidBodyTree.h"
#include "drake/util/testUtil.h"

//#include "drake/systems/plants/BotVisualizer.h"
//#include "drake/systems/plants/RigidBodySystem.h"

#include "gtest/gtest.h"
#include "gmock/gmock.h"

#include <iostream>
#define PRINT_VAR(x) std::cout <<  #x ": " << x << std::endl;

namespace drake {
namespace {

using Eigen::Vector3d;
using Eigen::VectorXd;
using Eigen::Matrix3Xd;
//using Drake::BotVisualizer;
//using Drake::RigidBodySystem;

class RBTCollisionTest: public ::testing::Test {
 protected:
  void SetUp() override {
    tree_.addRobotFromSDF(
        Drake::getDrakePath() + "/systems/plants/test/collision_test.sdf",
        DrakeJoint::QUATERNION);
  }

  RigidBodyTree tree_;
};

TEST_F(RBTCollisionTest, FindAndComputeContactPoints) {
  // Numerical precision tolerance to perform floating point comparisons.
  // Its magnitude was chosen to be the minimum value for which these tests can
  // successfully pass.
  const double tolerance = 2.0e-9;

  int nq = tree_.number_of_positions();
  int nv = tree_.number_of_velocities();
  int num_states = nq + nv;
  VectorXd x = VectorXd::Zero(num_states);
  x.head(nq) = tree_.getZeroConfiguration();

  auto q = x.topRows(nq);
  auto v = x.bottomRows(nv);

  auto kinsol = tree_.doKinematics(q, v);

  VectorXd phi;
  Matrix3Xd normal, xA, xB;
  std::vector<int> bodyA_idx, bodyB_idx;

  tree_.ComputeMaximumDepthCollisionPoints(
      kinsol, phi, normal, xA, xB, bodyA_idx, bodyB_idx, false);

{'fraction_non_alphanumeric': 0.13060726274031126, 'fraction_numerical': 0.00366188587122368, 'mean_word_length': 4.641996557659208, 'pattern_counts': {'":': 2, '<': 25, '<?xml version=': 0, '>': 13, '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': '3626420', 'n_tokens_mistral': 1231, 'n_tokens_neox': 1177, 'n_words': 260}