InstantaneousQPController memory debug attempt

gistfile1.txt

#include <array>
#include <iostream>
#include <map>
#include <memory>
#include <string>

#include "drake/common/drake_assert.h"
#include "drake/common/drake_path.h"
#include "drake/systems/plants/parser_urdf.h"
#include "drake/systems/plants/RigidBodyTree.h"
#include "drake/util/yaml/yamlUtil.h"

void applyURDFModifications(std::unique_ptr<RigidBodyTree>& robot,
                            const KinematicModifications& modifications) {
  for (auto it = modifications.attachments.begin();
       it != modifications.attachments.end(); ++it) {
    std::shared_ptr<RigidBodyFrame> attach_to_frame =
        robot->findFrame(it->attach_to_frame);
    if (!attach_to_frame) {
      std::cerr << "frame name: " << it->attach_to_frame << std::endl;
      throw std::runtime_error(
          "Could not find attachment frame when handling urdf modifications");
    }
    drake::parsers::urdf::AddModelInstanceFromURDF(
        drake::GetDrakePath() + "/" + it->urdf_filename, it->joint_type,
        attach_to_frame, robot.get());
  }

  auto filter = [&](const std::string& group_name) {
    return modifications.collision_groups_to_keep.find(group_name) ==
           modifications.collision_groups_to_keep.end();
  };
  robot->removeCollisionGroupsIf(filter);
  robot->compile();
}

void applyURDFModifications(std::unique_ptr<RigidBodyTree>& robot,
                            const std::string& urdf_modifications_filename) {
  KinematicModifications modifications =
      parseKinematicModifications(YAML::LoadFile(urdf_modifications_filename));
  applyURDFModifications(robot, modifications);
}

std::unordered_map<std::string, int>
ComputeBodyOrFrameNameToIdMap(const RigidBodyTree& robot) {
  std::unordered_map<std::string, int> id_map;

  const int num_bodies = static_cast<int>(robot.bodies.size());
  for (int i = 0; i < num_bodies; ++i) {
    const int id = i;
    DRAKE_ASSERT(robot.bodies[i] != nullptr);
    const std::string& name = robot.bodies[i]->get_name();
    id_map[name] = id;
    DRAKE_ASSERT(robot.getBodyOrFrameName(id) == name);
  }

  const int num_frames = static_cast<int>(robot.frames.size());
  for (int i = 0; i < num_frames; ++i) {
    const int id = -i - 2;
    DRAKE_ASSERT(robot.frames[i] != nullptr);
    const std::string& name = robot.frames[i]->get_name();
    id_map[name] = id;
    DRAKE_ASSERT(robot.getBodyOrFrameName(id) == name);
  }
  return id_map;
}

int main(int argc, char* argv[]) {
  // constructQPDataPointerMex
  const std::string control_config_filename(drake::GetDrakePath() + "/examples/Atlas/config/control_config_sim.yaml");
  const std::string urdf_filename(drake::GetDrakePath() + "/examples/Atlas/urdf/atlas_minimal_contact.urdf");
  const std::string urdf_modifications_filename(drake::GetDrakePath() + "/examples/Atlas/config/urdf_modifications_robotiq_weight.yaml");
  auto robot = std::make_unique<RigidBodyTree>(urdf_filename);
  applyURDFModifications(robot, urdf_modifications_filename);

  // The InstantaneousQPController constructor.

  KinematicsCache<double> cache(robot->bodies);
  std::cout << "**Number of positions in cache: " << cache.getNumPositions() << std::endl;

  // loadConfigurationFromYAML(control_config_filename)
  YAML::Node control_config = YAML::LoadFile(control_config_filename);
  std::ofstream debug_file(control_config_filename + ".debug.yaml");
  auto param_sets = loadAllParamSets(control_config["qp_controller_params"], *robot,
                                debug_file);
  auto rpc = parseKinematicTreeMetadata(control_config["kinematic_tree_metadata"],
                                   *robot);

  //initialize()
  auto id_map = ComputeBodyOrFrameNameToIdMap(*robot);

  std::cout << "**Param Sets" << std::endl;
  for (const auto& kv : param_sets) {
    std::cout << kv.first << std::endl;
  }

  std::cout << "**ID Map" << std::endl;
  for (const auto& kv : id_map) {
    std::cout << kv.first << " : " << kv.second << std::endl;
  }

  std::cout << "**RPC" << std::endl;
  for (const auto& kv : rpc.foot_ids) {
    std::cout << kv.first.toString() << " : " << kv.second << std::endl;
  }
  return 0;
}