<commit_before>#include "trajopt/configuration_space.hpp"
#include <boost/foreach.hpp>
#include "trajopt/rave_utils.hpp"
#include "utils/math.hpp"
using namespace OpenRAVE;
using namespace util;

// TODO: configuration should know something about what dofs are part of SO(1), SO(2) etc for functions like RandomDOFValues

namespace trajopt {

void RobotAndDOF::SetDOFValues(const DblVec& dofs) {
  if (affinedofs != 0) {
    OR::Transform T;
    OR::RaveGetTransformFromAffineDOFValues(T, dofs.begin()+joint_inds.size(), affinedofs, rotationaxis, true);
    robot->SetTransform(T);
    robot->SetDOFValues(dofs, false, joint_inds);
  }
  else {
    robot->SetDOFValues(dofs, false, joint_inds);
  }
}

DblVec RobotAndDOF::GetDOFValues() {
  DblVec out;
  robot->GetDOFValues(out, joint_inds);
  if (affinedofs != 0) {
    out.resize(GetDOF());
    OR::Transform T = robot->GetTransform();
    OR::RaveGetAffineDOFValuesFromTransform(out.begin() + joint_inds.size(), T, affinedofs, rotationaxis);
  }
  return out;
}

void RobotAndDOF::SetRobotActiveDOFs()  {
  RobotBasePtr robot1 = boost::dynamic_pointer_cast<RobotBase>(robot); // since robot is just a kinbody
  vector<int> current_active = robot1->GetActiveDOFIndices();
  if (robot1->GetActiveDOF() != GetDOF() || !std::equal(current_active.begin(), current_active.end(), joint_inds.begin()))
    robot1->SetActiveDOFs(joint_inds, affinedofs);
}

void RobotAndDOF::GetDOFLimits(DblVec& lower, DblVec& upper) const {
  robot->GetDOFLimits(lower, upper, joint_inds);
  const int translation_dofs[3] = {DOF_X, DOF_Y, DOF_Z};
  for (int i=0; i < 3; ++i) {
    if (affinedofs & translation_dofs[i]) {
      lower.push_back(-INFINITY);
      upper.push_back(INFINITY);
    }
  }
  if (affinedofs & DOF_RotationMask) {
    if (affinedofs & DOF_RotationAxis) {
      lower.push_back(-INFINITY);
      upper.push_back(INFINITY);
    }<commit_msg>Fixed bug that broke base only planning<commit_after>#include "trajopt/configuration_space.hpp"
#include <boost/foreach.hpp>
#include "trajopt/rave_utils.hpp"
#include "utils/math.hpp"
using namespace OpenRAVE;
using namespace util;

// TODO: configuration should know something about what dofs are part of SO(1), SO(2) etc for functions like RandomDOFValues

namespace trajopt {

void RobotAndDOF::SetDOFValues(const DblVec& dofs) {
  if (affinedofs != 0) {
    OR::Transform T;
    OR::RaveGetTransformFromAffineDOFValues(T, dofs.begin()+joint_inds.size(), affinedofs, rotationaxis, true);
    robot->SetTransform(T);
    if (joint_inds.size() > 0)
      robot->SetDOFValues(dofs, false, joint_inds);
  }
  else {
    robot->SetDOFValues(dofs, false, joint_inds);
  }
}

DblVec RobotAndDOF::GetDOFValues() {
  DblVec out;
  if (joint_inds.size() > 0):
    robot->GetDOFValues(out, joint_inds);
  if (affinedofs != 0) {
    out.resize(GetDOF());
    OR::Transform T = robot->GetTransform();
    OR::RaveGetAffineDOFValuesFromTransform(out.begin() + joint_inds.size(), T, affinedofs, rotationaxis);
  }
  return out;
}

void RobotAndDOF::SetRobotActiveDOFs()  {
  RobotBasePtr robot1 = boost::dynamic_pointer_cast<RobotBase>(robot); // since robot is just a kinbody
  vector<int> current_active = robot1->GetActiveDOFIndices();
  if (robot1->GetActiveDOF() != GetDOF() || !std::equal(current_active.begin(), current_active.end(), joint_inds.begin()))
    robot1->SetActiveDOFs(joint_inds, affinedofs);
}

void RobotAndDOF::GetDOFLimits(DblVec& lower, DblVec& upper) const {
  if (joint_inds.size() > 0)
    robot->GetDOFLimits(lower, upper, joint_inds);
  const int translation_dofs[3] = {DOF_X, DOF_Y, DOF_Z};
  for (int i=0; i < 3; ++i) {
    if (affinedofs & translation_dofs[i]) {
      lower.push_back(-INFINITY);
      upper.push_back(INFINITY);
    }
  }
  if (affinedofs & DOF_RotationMask) {
    if (affinedofs & DOF_RotationAxis) {
      lower.push_back(-INFINITY);
      upper.push_back(INFINITY);
    }
{'fraction_non_alphanumeric': 0.13850486431131592, 'fraction_numerical': 0.006400409626216078, 'mean_word_length': 4.79673590504451, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 30, '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': '6183706', 'n_tokens_mistral': 1450, 'n_tokens_neox': 1449, 'n_words': 279}