<commit_before>#include "mex.h"
#include <iostream>
#include <cmath>
#include "drakeMexUtil.h"
#include "RigidBodyManipulator.h"
#include <stdexcept>
#include <RevoluteJoint.h>
#include <PrismaticJoint.h>
#include <HelicalJoint.h>
#include <RollPitchYawFloatingJoint.h>
#include <QuaternionFloatingJoint.h>

using namespace Eigen;
using namespace std;

bool isMxArrayVector( const mxArray* array )
{
  size_t num_rows = mxGetM(array);
  size_t num_cols = mxGetN(array);
  return (num_rows <= 1) || (num_cols <= 1);
}

template <typename Derived>
void setDynamics(const mxArray *pBodies, int i, FixedAxisOneDoFJoint<Derived>* fixed_axis_one_dof_joint) {
  double damping = mxGetScalar(mxGetProperty(pBodies, i, "damping"));
  double coulomb_friction = mxGetScalar(mxGetProperty(pBodies, i, "coulomb_friction"));
  double coulomb_window = mxGetScalar(mxGetProperty(pBodies, i, "coulomb_window"));
  fixed_axis_one_dof_joint->setDynamics(damping, coulomb_friction, coulomb_window);
}

template <typename Derived>
void setLimits(const mxArray *pBodies, int i, FixedAxisOneDoFJoint<Derived>* fixed_axis_one_dof_joint) {
  double joint_limit_min = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_min"));
  double joint_limit_max = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_max"));
  fixed_axis_one_dof_joint->setJointLimits(joint_limit_min,joint_limit_max);
}


void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  //DEBUG
  //cout << "constructModelmex: START" << endl;
  //END_DEBUG
  char buf[100];
  mxArray *pm;

  if (nrhs!=1) {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)");
  }

  if (isa(prhs[0],"DrakeMexPointer")) {  // then it's calling the destructor
    destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]);
    return;
  }

  const mxArray* pRBM = prhs[0];
  RigidBodyManipulator *model=NULL;

//  model->robot_name = get_strings(mxGetProperty(pRBM,0,"name"));

  const mxArray* pBodies = mxGetProperty(pRBM,0,"body");
  if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid");
  int num_bodies = static_cast<int>(mxGetNumberOfElements(pBodies));

  const mxArray* pFrames = mxGetProperty(pRBM,0,"frame");
  if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
  int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames));

  pm = mxGetProperty(pRBM, 0, "num_positions");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","model should have a num_positions field");
  int num_positions = static_cast<int>(*mxGetPrSafe(pm));
  model = new RigidBodyManipulator(num_positions, num_bodies, num_frames);

  for (int i=0; i<model->num_bodies; i++) {
    //DEBUG
    //cout << "constructModelmex: body " << i << endl;
    //END_DEBUG
    model->bodies[i]->body_index = i;

    pm = mxGetProperty(pBodies,i,"linkname");
    mxGetString(pm,buf,100);
    model->bodies[i]->linkname.assign(buf,strlen(buf));

    pm = mxGetProperty(pBodies,i,"robotnum");
    model->bodies[i]->robotnum = (int) mxGetScalar(pm)-1;

    pm = mxGetProperty(pBodies,i,"mass");
    model->bodies[i]->mass = mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"com");
    if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->com.data(),mxGetPrSafe(pm),sizeof(double)*3);

    pm = mxGetProperty(pBodies,i,"I");
    if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->I.data(),mxGetPrSafe(pm),sizeof(double)*6*6);

    pm = mxGetProperty(pBodies,i,"position_num");
    model->bodies[i]->position_num_start = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetProperty(pBodies,i,"velocity_num");
    model->bodies[i]->velocity_num_start = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetProperty(pBodies,i,"parent");
    if (!pm || mxIsEmpty(pm))
      model->bodies[i]->parent = nullptr;
    else {
      int parent_ind = static_cast<int>(mxGetScalar(pm))-1;
      if (parent_ind >= static_cast<int>(model->bodies.size()))
        mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","bad body.parent %d (only have %d bodies)",parent_ind,model->bodies.size());
      if (parent_ind>=0)
        model->bodies[i]->parent = model->bodies[parent_ind];
    }

    {
      mxGetString(mxGetProperty(pBodies, i, "jointname"), buf, 100);
      string joint_name;
      joint_name.assign(buf, strlen(buf));

      pm = mxGetProperty(pBodies, i, "Ttree");
      // todo: check that the size is 4x4
      Isometry3d transform_to_parent_body;
      memcpy(transform_to_parent_body.data(), mxGetPrSafe(pm), sizeof(double) * 4 * 4);

      int floating = (int) mxGetScalar(mxGetProperty(pBodies, i, "floating"));

      Eigen::Vector3d joint_axis;
      pm = mxGetProperty(pBodies, i, "joint_axis");
      memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3);

      double pitch = mxGetScalar(mxGetProperty(pBodies, i, "pitch"));

      if (model->bodies[i]->hasParent()) {
        std::unique_ptr<DrakeJoint> joint;
        switch (floating) {
          case 0: {
            if (pitch == 0.0) {
              RevoluteJoint *revolute_joint = new RevoluteJoint(joint_name, transform_to_parent_body, joint_axis);
              joint = std::unique_ptr<RevoluteJoint>(revolute_joint);
              setLimits(pBodies, i, revolute_joint);
              setDynamics(pBodies, i, revolute_joint);
            } else if (std::isinf<double>(pitch)) {
              PrismaticJoint *prismatic_joint = new PrismaticJoint(joint_name, transform_to_parent_body, joint_axis);
              joint = std::unique_ptr<PrismaticJoint>(prismatic_joint);
              setLimits(pBodies, i, prismatic_joint);
              setDynamics(pBodies, i, prismatic_joint);
            } else {
              joint = std::unique_ptr<HelicalJoint>(new HelicalJoint(joint_name, transform_to_parent_body, joint_axis, pitch));
            }
            break;
          }
          case 1: {
            joint = std::unique_ptr<RollPitchYawFloatingJoint>(new RollPitchYawFloatingJoint(joint_name, transform_to_parent_body));
            break;
          }
          case 2: {
            joint = std::unique_ptr<QuaternionFloatingJoint>(new QuaternionFloatingJoint(joint_name, transform_to_parent_body));
            break;
          }
          default: {
            std::ostringstream stream;
            stream << "floating type " << floating << " not recognized.";
            throw std::runtime_error(stream.str());
          }
        }

        model->bodies[i]->setJoint(move(joint));
      }<commit_msg>help gcc with template args<commit_after>#include "mex.h"
#include <iostream>
#include <cmath>
#include "drakeMexUtil.h"
#include "RigidBodyManipulator.h"
#include <stdexcept>
#include <RevoluteJoint.h>
#include <PrismaticJoint.h>
#include <HelicalJoint.h>
#include <RollPitchYawFloatingJoint.h>
#include <QuaternionFloatingJoint.h>

using namespace Eigen;
using namespace std;

bool isMxArrayVector( const mxArray* array )
{
  size_t num_rows = mxGetM(array);
  size_t num_cols = mxGetN(array);
  return (num_rows <= 1) || (num_cols <= 1);
}

template <typename Derived>
void setDynamics(const mxArray *pBodies, int i, FixedAxisOneDoFJoint<Derived>* fixed_axis_one_dof_joint) {
  double damping = mxGetScalar(mxGetProperty(pBodies, i, "damping"));
  double coulomb_friction = mxGetScalar(mxGetProperty(pBodies, i, "coulomb_friction"));
  double coulomb_window = mxGetScalar(mxGetProperty(pBodies, i, "coulomb_window"));
  fixed_axis_one_dof_joint->setDynamics(damping, coulomb_friction, coulomb_window);
}

template <typename Derived>
void setLimits(const mxArray *pBodies, int i, FixedAxisOneDoFJoint<Derived>* fixed_axis_one_dof_joint) {
  double joint_limit_min = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_min"));
  double joint_limit_max = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_max"));
  fixed_axis_one_dof_joint->setJointLimits(joint_limit_min,joint_limit_max);
}


void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  //DEBUG
  //cout << "constructModelmex: START" << endl;
  //END_DEBUG
  char buf[100];
  mxArray *pm;

  if (nrhs!=1) {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)");
  }

  if (isa(prhs[0],"DrakeMexPointer")) {  // then it's calling the destructor
    destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]);
    return;
  }

  const mxArray* pRBM = prhs[0];
  RigidBodyManipulator *model=NULL;

//  model->robot_name = get_strings(mxGetProperty(pRBM,0,"name"));

  const mxArray* pBodies = mxGetProperty(pRBM,0,"body");
  if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid");
  int num_bodies = static_cast<int>(mxGetNumberOfElements(pBodies));

  const mxArray* pFrames = mxGetProperty(pRBM,0,"frame");
  if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
  int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames));

  pm = mxGetProperty(pRBM, 0, "num_positions");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","model should have a num_positions field");
  int num_positions = static_cast<int>(*mxGetPrSafe(pm));
  model = new RigidBodyManipulator(num_positions, num_bodies, num_frames);

  for (int i=0; i<model->num_bodies; i++) {
    //DEBUG
    //cout << "constructModelmex: body " << i << endl;
    //END_DEBUG
    model->bodies[i]->body_index = i;

    pm = mxGetProperty(pBodies,i,"linkname");
    mxGetString(pm,buf,100);
    model->bodies[i]->linkname.assign(buf,strlen(buf));

    pm = mxGetProperty(pBodies,i,"robotnum");
    model->bodies[i]->robotnum = (int) mxGetScalar(pm)-1;

    pm = mxGetProperty(pBodies,i,"mass");
    model->bodies[i]->mass = mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"com");
    if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->com.data(),mxGetPrSafe(pm),sizeof(double)*3);

    pm = mxGetProperty(pBodies,i,"I");
    if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->I.data(),mxGetPrSafe(pm),sizeof(double)*6*6);

    pm = mxGetProperty(pBodies,i,"position_num");
    model->bodies[i]->position_num_start = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetProperty(pBodies,i,"velocity_num");
    model->bodies[i]->velocity_num_start = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetProperty(pBodies,i,"parent");
    if (!pm || mxIsEmpty(pm))
      model->bodies[i]->parent = nullptr;
    else {
      int parent_ind = static_cast<int>(mxGetScalar(pm))-1;
      if (parent_ind >= static_cast<int>(model->bodies.size()))
        mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","bad body.parent %d (only have %d bodies)",parent_ind,model->bodies.size());
      if (parent_ind>=0)
        model->bodies[i]->parent = model->bodies[parent_ind];
    }

    {
      mxGetString(mxGetProperty(pBodies, i, "jointname"), buf, 100);
      string joint_name;
      joint_name.assign(buf, strlen(buf));

      pm = mxGetProperty(pBodies, i, "Ttree");
      // todo: check that the size is 4x4
      Isometry3d transform_to_parent_body;
      memcpy(transform_to_parent_body.data(), mxGetPrSafe(pm), sizeof(double) * 4 * 4);

      int floating = (int) mxGetScalar(mxGetProperty(pBodies, i, "floating"));

      Eigen::Vector3d joint_axis;
      pm = mxGetProperty(pBodies, i, "joint_axis");
      memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3);

      double pitch = mxGetScalar(mxGetProperty(pBodies, i, "pitch"));

      if (model->bodies[i]->hasParent()) {
        std::unique_ptr<DrakeJoint> joint;
        switch (floating) {
          case 0: {
            if (pitch == 0.0) {
              RevoluteJoint *revolute_joint = new RevoluteJoint(joint_name, transform_to_parent_body, joint_axis);
              joint = std::unique_ptr<RevoluteJoint>(revolute_joint);
              setLimits(pBodies, i, revolute_joint);
              setDynamics(pBodies, i, revolute_joint);
            } else if (std::isinf(static_cast<double>(pitch))) {
              PrismaticJoint *prismatic_joint = new PrismaticJoint(joint_name, transform_to_parent_body, joint_axis);
              joint = std::unique_ptr<PrismaticJoint>(prismatic_joint);
              setLimits(pBodies, i, prismatic_joint);
              setDynamics(pBodies, i, prismatic_joint);
            } else {
              joint = std::unique_ptr<HelicalJoint>(new HelicalJoint(joint_name, transform_to_parent_body, joint_axis, pitch));
            }
            break;
          }
          case 1: {
            joint = std::unique_ptr<RollPitchYawFloatingJoint>(new RollPitchYawFloatingJoint(joint_name, transform_to_parent_body));
            break;
          }
          case 2: {
            joint = std::unique_ptr<QuaternionFloatingJoint>(new QuaternionFloatingJoint(joint_name, transform_to_parent_body));
            break;
          }
          default: {
            std::ostringstream stream;
            stream << "floating type " << floating << " not recognized.";
            throw std::runtime_error(stream.str());
          }
        }

        model->bodies[i]->setJoint(move(joint));
      }
{'fraction_non_alphanumeric': 0.13899672680216182, 'fraction_numerical': 0.006089670396589785, 'mean_word_length': 4.142074363992172, 'pattern_counts': {'":': 0, '<': 91, '<?xml version=': 0, '>': 119, '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': '3736681', 'n_tokens_mistral': 4794, 'n_tokens_neox': 4605, 'n_words': 768}