C++ Isometry3d::data方法代码示例

DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs != 6 || nlhs != 7) {
    mexErrMsgIdAndTxt("Drake:testGeometryGradientsmex:BadInputs",
                      "Usage [dT, dTInv, dAdT, dAdT_transpose, x_norm, "
                      "dx_norm, ddx_norm] = testGeometryGradientsmex(T, S, "
                      "qdot_to_v, X, dX, x)");
  }

  int argnum = 0;
  Isometry3d T;
  memcpy(T.data(), mxGetPr(prhs[argnum++]),
         sizeof(double) * drake::kHomogeneousTransformSize);
  auto S = matlabToEigen<drake::kTwistSize, Eigen::Dynamic>(prhs[argnum++]);
  auto qdot_to_v =
      matlabToEigen<Eigen::Dynamic, Eigen::Dynamic>(prhs[argnum++]);
  auto X = matlabToEigen<drake::kTwistSize, Eigen::Dynamic>(prhs[argnum++]);
  auto dX = matlabToEigen<Eigen::Dynamic, Eigen::Dynamic>(prhs[argnum++]);
  auto x = matlabToEigen<4, 1>(prhs[argnum++]);

  auto dT = dHomogTrans(T, S, qdot_to_v).eval();
  auto dTInv = dHomogTransInv(T, dT).eval();
  auto dAdT = dTransformSpatialMotion(T, X, dT, dX).eval();
  auto dAdTInv_transpose = dTransformSpatialForce(T, X, dT, dX).eval();

  Vector4d x_norm;
  Gradient<Vector4d, 4, 1>::type dx_norm;
  Gradient<Vector4d, 4, 2>::type ddx_norm;
  drake::math::NormalizeVector(x, x_norm, &dx_norm, &ddx_norm);

  int outnum = 0;
  plhs[outnum++] = eigenToMatlab(dT);
  plhs[outnum++] = eigenToMatlab(dTInv);
  plhs[outnum++] = eigenToMatlab(dAdT);
  plhs[outnum++] = eigenToMatlab(dAdTInv_transpose);
  plhs[outnum++] = eigenToMatlab(x_norm);
  plhs[outnum++] = eigenToMatlab(dx_norm);
  plhs[outnum++] = eigenToMatlab(ddx_norm);
}

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 jointname;
      jointname.assign(buf, strlen(buf));

      pm = mxGetProperty(pBodies, i, "Ttree");
      // todo: check that the size is 4x4
      Isometry3d Ttree;
      memcpy(Ttree.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()) {
        unique_ptr<DrakeJoint> joint = createJoint(jointname, Ttree, floating, joint_axis, pitch);

//        mexPrintf((model->bodies[i]->getJoint().getName() + ": " + std::to_string(model->bodies[i]->getJoint().getNumVelocities()) + "\n").c_str());

        FixedAxisOneDoFJoint* fixed_axis_one_dof_joint = dynamic_cast<FixedAxisOneDoFJoint*>(joint.get());
        if (fixed_axis_one_dof_joint != nullptr) {
          double joint_limit_min = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_min"));
//.........这里部分代码省略.........

void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  // DEBUG
  // mexPrintf("constructModelmex: START\n");
  // END_DEBUG
  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<RigidBodyTree*>(prhs[0]);
    return;
  }

  const mxArray* pRBM = prhs[0];
  RigidBodyTree* model = new RigidBodyTree();
  model->bodies.clear();  // a little gross:  the default constructor makes a
                          // body "world".  zap it because we will construct one
                          // again below

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

  const mxArray* pBodies = mxGetPropertySafe(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 = mxGetPropertySafe(pRBM, 0, "frame");
  if (!pFrames)
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs",
                      "the frame array is invalid");
  int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames));

  for (int i = 0; i < num_bodies; i++) {
    // DEBUG
    // mexPrintf("constructModelmex: body %d\n", i);
    // END_DEBUG
    std::unique_ptr<RigidBody> b(new RigidBody());
    b->set_body_index(i);

    b->set_name(mxGetStdString(mxGetPropertySafe(pBodies, i, "linkname")));

    pm = mxGetPropertySafe(pBodies, i, "robotnum");
    b->set_model_instance_id((int)mxGetScalar(pm) - 1);

    pm = mxGetPropertySafe(pBodies, i, "mass");
    b->set_mass(mxGetScalar(pm));

    pm = mxGetPropertySafe(pBodies, i, "com");
    Eigen::Vector3d com;
    if (!mxIsEmpty(pm)) {
      memcpy(com.data(), mxGetPrSafe(pm), sizeof(double) * 3);
      b->set_center_of_mass(com);
    }

    pm = mxGetPropertySafe(pBodies, i, "I");
    if (!mxIsEmpty(pm)) {
      drake::SquareTwistMatrix<double> I;
      memcpy(I.data(), mxGetPrSafe(pm), sizeof(double) * 6 * 6);
      b->set_spatial_inertia(I);
    }

    pm = mxGetPropertySafe(pBodies, i, "position_num");
    b->set_position_start_index((int)mxGetScalar(pm) - 1);  // zero-indexed

    pm = mxGetPropertySafe(pBodies, i, "velocity_num");
    b->set_velocity_start_index((int)mxGetScalar(pm) - 1);  // zero-indexed

    pm = mxGetPropertySafe(pBodies, i, "parent");
    if (!pm || mxIsEmpty(pm)) {
      b->set_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) b->set_parent(model->bodies[parent_ind].get());
    }

    if (b->has_parent_body()) {
      string joint_name =
          mxGetStdString(mxGetPropertySafe(pBodies, i, "jointname"));
      // mexPrintf("adding joint %s\n", joint_name.c_str());

      pm = mxGetPropertySafe(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(mxGetPropertySafe(pBodies, i, "floating"));

      Eigen::Vector3d joint_axis;
      pm = mxGetPropertySafe(pBodies, i, "joint_axis");
      memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3);
//.........这里部分代码省略.........