<commit_before>#include <iostream>
#include <map>

#include "drakeGeometryUtil.h"
#include "RigidBodyManipulator.h"
#include "DrakeJoint.h"
#include "FixedJoint.h"
#include "drakeUtil.h"

#include <algorithm>
#include <string>
#include <regex>
#include <stdexcept>
#include <limits>
#include "RigidBodyConstraint.h"
//DEBUG
//#include <stdexcept>
//END_DEBUG

using namespace std;

std::set<int> emptyIntSet;


template <typename T>
void getFiniteIndexes(T const & v, std::vector<int> &finite_indexes)
{
  finite_indexes.clear();
  const size_t n = v.size();
  for (int x = 0; x < n; x++)
  {
    if (std::isfinite(static_cast<double>(v[x]))) {
      finite_indexes.push_back(x);
    }
  }
}

std::ostream& operator<<(std::ostream& os, const RigidBodyLoop& obj)
{
  os << "loop connects pt " << obj.frameA->Ttree.topRightCorner(3,1).transpose() << " on " << obj.frameA->body->linkname << " to pt " << obj.frameB->Ttree.topRightCorner(3,1).transpose() << " on " << obj.frameB->body->linkname << std::endl;
  return os;
}

RigidBodyManipulator::RigidBodyManipulator(const std::string &urdf_filename, const DrakeJoint::FloatingBaseType floating_base_type)
  :  collision_model(DrakeCollision::newModel())
{
  a_grav << 0, 0, 0, 0, 0, -9.81;

  shared_ptr<RigidBody> b = make_shared<RigidBody>();
  b->linkname = "world";
  b->robotnum = 0;
  b->body_index = 0;
  bodies.push_back(b);

  addRobotFromURDF(urdf_filename,floating_base_type);
}

RigidBodyManipulator::RigidBodyManipulator(void) :
    collision_model(DrakeCollision::newModel())
{
  a_grav << 0, 0, 0, 0, 0, -9.81;

  shared_ptr<RigidBody> b = make_shared<RigidBody>();
  b->linkname = "world";
  b->robotnum = 0;
  b->body_index = 0;
  bodies.push_back(b);
}

RigidBodyManipulator::~RigidBodyManipulator(void)
{
}


void RigidBodyManipulator::compile(void)
{
  // reorder body list to make sure that parents before children in the list
  size_t i = 0;
  while (i < bodies.size() - 1) {
    if (bodies[i]->hasParent()) {
      auto iter = find(bodies.begin() + i + 1, bodies.end(), bodies[i]->parent);
      if (iter != bodies.end()) {
        bodies.erase(iter);
        bodies.insert(bodies.begin() + i, bodies[i]->parent);
        i--;
      }
    }
    i++;
  }

  // weld joints for links that have zero inertia and no children (as seen in pr2.urdf)
  for (size_t i=0; i<bodies.size(); i++) {
    if (bodies[i]->hasParent() && bodies[i]->I.isConstant(0)) {
      bool hasChild = false;
      for (size_t j=i+1; j<bodies.size(); j++) {
        if (bodies[j]->parent == bodies[i]) {
          hasChild = true;
          break;
        }
      }
      if (!hasChild) {
        // now check if this body is attached by a loop joint
        for (const auto &loop : loops) {
          if ((loop.frameA->body == bodies[i]) || (loop.frameB->body == bodies[i])) {
            hasChild = true;
            break;
          }
        }
      }
      if (!hasChild) {
      	cout << "welding " << bodies[i]->getJoint().getName() << " because it has no inertia beneath it" << endl;
        unique_ptr<DrakeJoint> joint_unique_ptr(new FixedJoint(bodies[i]->getJoint().getName(), bodies[i]->getJoint().getTransformToParentBody()));
        bodies[i]->setJoint(move(joint_unique_ptr));
      }
    }
  }

  int _num_positions = 0;
  num_velocities = 0;
  for (auto it = bodies.begin(); it != bodies.end(); ++it) {
    RigidBody& body = **it;
    if (body.hasParent()) {
      body.position_num_start = _num_positions;
      _num_positions += body.getJoint().getNumPositions();
      body.velocity_num_start = num_velocities;
      num_velocities += body.getJoint().getNumVelocities();
    }
    else {
      body.position_num_start = 0;
      body.velocity_num_start = 0;
    }
  }

  for (size_t i=0; i<bodies.size(); i++) {
    bodies[i]->body_index = i;
    bodies[i]->setN(_num_positions, num_velocities);
  }

  B.resize(num_velocities,actuators.size());
  B = MatrixXd::Zero(num_velocities,actuators.size());
  for (size_t ia=0; ia<actuators.size(); ia++)
    for (int i=0; i<actuators[ia].body->getJoint().getNumVelocities(); i++)
      B(actuators[ia].body->velocity_num_start+i,ia) = actuators[ia].reduction;
<commit_msg>fix num_positions<commit_after>#include <iostream>
#include <map>

#include "drakeGeometryUtil.h"
#include "RigidBodyManipulator.h"
#include "DrakeJoint.h"
#include "FixedJoint.h"
#include "drakeUtil.h"

#include <algorithm>
#include <string>
#include <regex>
#include <stdexcept>
#include <limits>
#include "RigidBodyConstraint.h"
//DEBUG
//#include <stdexcept>
//END_DEBUG

using namespace std;

std::set<int> emptyIntSet;


template <typename T>
void getFiniteIndexes(T const & v, std::vector<int> &finite_indexes)
{
  finite_indexes.clear();
  const size_t n = v.size();
  for (int x = 0; x < n; x++)
  {
    if (std::isfinite(static_cast<double>(v[x]))) {
      finite_indexes.push_back(x);
    }
  }
}

std::ostream& operator<<(std::ostream& os, const RigidBodyLoop& obj)
{
  os << "loop connects pt " << obj.frameA->Ttree.topRightCorner(3,1).transpose() << " on " << obj.frameA->body->linkname << " to pt " << obj.frameB->Ttree.topRightCorner(3,1).transpose() << " on " << obj.frameB->body->linkname << std::endl;
  return os;
}

RigidBodyManipulator::RigidBodyManipulator(const std::string &urdf_filename, const DrakeJoint::FloatingBaseType floating_base_type)
  :  collision_model(DrakeCollision::newModel())
{
  a_grav << 0, 0, 0, 0, 0, -9.81;

  shared_ptr<RigidBody> b = make_shared<RigidBody>();
  b->linkname = "world";
  b->robotnum = 0;
  b->body_index = 0;
  bodies.push_back(b);

  addRobotFromURDF(urdf_filename,floating_base_type);
}

RigidBodyManipulator::RigidBodyManipulator(void) :
    collision_model(DrakeCollision::newModel())
{
  a_grav << 0, 0, 0, 0, 0, -9.81;

  shared_ptr<RigidBody> b = make_shared<RigidBody>();
  b->linkname = "world";
  b->robotnum = 0;
  b->body_index = 0;
  bodies.push_back(b);
}

RigidBodyManipulator::~RigidBodyManipulator(void)
{
}


void RigidBodyManipulator::compile(void)
{
  // reorder body list to make sure that parents before children in the list
  size_t i = 0;
  while (i < bodies.size() - 1) {
    if (bodies[i]->hasParent()) {
      auto iter = find(bodies.begin() + i + 1, bodies.end(), bodies[i]->parent);
      if (iter != bodies.end()) {
        bodies.erase(iter);
        bodies.insert(bodies.begin() + i, bodies[i]->parent);
        i--;
      }
    }
    i++;
  }

  // weld joints for links that have zero inertia and no children (as seen in pr2.urdf)
  for (size_t i=0; i<bodies.size(); i++) {
    if (bodies[i]->hasParent() && bodies[i]->I.isConstant(0)) {
      bool hasChild = false;
      for (size_t j=i+1; j<bodies.size(); j++) {
        if (bodies[j]->parent == bodies[i]) {
          hasChild = true;
          break;
        }
      }
      if (!hasChild) {
        // now check if this body is attached by a loop joint
        for (const auto &loop : loops) {
          if ((loop.frameA->body == bodies[i]) || (loop.frameB->body == bodies[i])) {
            hasChild = true;
            break;
          }
        }
      }
      if (!hasChild) {
      	cout << "welding " << bodies[i]->getJoint().getName() << " because it has no inertia beneath it" << endl;
        unique_ptr<DrakeJoint> joint_unique_ptr(new FixedJoint(bodies[i]->getJoint().getName(), bodies[i]->getJoint().getTransformToParentBody()));
        bodies[i]->setJoint(move(joint_unique_ptr));
      }
    }
  }

  num_positions = 0;
  num_velocities = 0;
  for (auto it = bodies.begin(); it != bodies.end(); ++it) {
    RigidBody& body = **it;
    if (body.hasParent()) {
      body.position_num_start = num_positions;
      num_positions += body.getJoint().getNumPositions();
      body.velocity_num_start = num_velocities;
      num_velocities += body.getJoint().getNumVelocities();
    }
    else {
      body.position_num_start = 0;
      body.velocity_num_start = 0;
    }
  }

  for (size_t i=0; i<bodies.size(); i++) {
    bodies[i]->body_index = i;
    bodies[i]->setN(num_positions, num_velocities);
  }

  B.resize(num_velocities,actuators.size());
  B = MatrixXd::Zero(num_velocities,actuators.size());
  for (size_t ia=0; ia<actuators.size(); ia++)
    for (int i=0; i<actuators[ia].body->getJoint().getNumVelocities(); i++)
      B(actuators[ia].body->velocity_num_start+i,ia) = actuators[ia].reduction;

{'fraction_non_alphanumeric': 0.16834291187739464, 'fraction_numerical': 0.009339080459770116, 'mean_word_length': 3.4596903363587828, 'pattern_counts': {'":': 0, '<': 115, '<?xml version=': 0, '>': 93, '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': '2264539', 'n_tokens_mistral': 3211, 'n_tokens_neox': 3083, 'n_words': 600}