예제 #1
0
int main(int argc, char* argv[])
{
	if (argc<2) {
		cerr << "Usage: urdfCollisionTest urdf_filename" << endl;
		exit(-1);
	}
  RigidBodyTree * model = new RigidBodyTree(argv[1]);
  if (!model) {
  	cerr << "ERROR: Failed to load model from " << argv[1] << endl;
  	return -1;
  }

  // run kinematics with second derivatives 100 times
  Eigen::VectorXd q = model->getZeroConfiguration();
  int i;

  if (argc>=2+model->num_positions) {
  	for (i=0; i<model->num_positions; i++)
  		sscanf(argv[2+i],"%lf",&q(i));
  }

// for (i=0; i<model->num_dof; i++)
// 	 q(i)=(double)rand() / RAND_MAX;
  KinematicsCache<double> cache = model->doKinematics(q);
//  }

  Eigen::VectorXd phi;
  Eigen::Matrix3Xd normal, xA, xB;
  vector<int> bodyA_idx, bodyB_idx;

  model->collisionDetect(cache, phi,normal,xA,xB,bodyA_idx,bodyB_idx);

  cout << "=======" << endl;
  for (int j=0; j<phi.rows(); ++j) {
    cout << phi(j) << " ";
    for (int i=0; i<3; ++i) {
      cout << normal(i,j) << " ";
    }
    for (int i=0; i<3; ++i) {
      cout << xA(i,j) << " ";
    }
    for (int i=0; i<3; ++i) {
      cout << xB(i,j) << " ";
    }
    cout << model->bodies[bodyA_idx.at(j)]->linkname << " ";
    cout << model->bodies[bodyB_idx.at(j)]->linkname << endl;
  }

  delete model;
  return 0;
}
예제 #2
0
int main(int argc, char* argv[]) {
  if (argc < 2) {
    cerr << "Usage: urdfKinTest urdf_filename" << endl;
    exit(-1);
  }
  RigidBodyTree* model = new RigidBodyTree(argv[1]);
  cout << "=======" << endl;

  // run kinematics with second derivatives 100 times
  Eigen::VectorXd q = model->getZeroConfiguration();
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model->num_velocities);
  int i;

  if (argc >= 2 + model->num_positions) {
    for (i = 0; i < model->num_positions; i++)
      sscanf(argv[2 + i], "%lf", &q(i));
  }

  // for (i=0; i<model->num_dof; i++)
  // 	 q(i)=(double)rand() / RAND_MAX;
  KinematicsCache<double> cache = model->doKinematics(q, v);
  //  }

  //  const Vector4d zero(0,0,0,1);
  Eigen::Vector3d zero = Eigen::Vector3d::Zero();
  Eigen::Matrix<double, 6, 1> pt;

  for (i = 0; i < model->bodies.size(); i++) {
    //    model->forwardKin(i,zero,1,pt);
    auto pt = model->transformPoints(cache, zero, i, 0);
    auto rpy = model->relativeRollPitchYaw(cache, i, 0);
    Eigen::Matrix<double, 6, 1> x;
    x << pt, rpy;
    //    cout << i << ": forward kin: " << model->bodies[i].linkname << " is at
    //    " << pt.transpose() << endl;
    cout << model->bodies[i]->linkname << " ";
    cout << x.transpose() << endl;
    //    for (int j=0; j<pt.size(); j++)
    //    	cout << pt(j) << " ";
  }

  auto phi = model->positionConstraints<double>(cache);
  cout << "phi = " << phi.transpose() << endl;

  delete model;
  return 0;
}
예제 #3
0
/**
 * Use Frank's fastQP code (mexed)
 * [q, info] = approximateIKEIQPmex(objgetMexModelPtr, q0, q_nom, Q, varargin)
 * info = 0 on success, 1 on failure
 **/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs < 4) {
    mexErrMsgIdAndTxt("Drake:approximateIKmex:NotEnoughInputs",
                      "Usage approximateIKmex(model_ptr, q0, q_nom, Q,...)");
  }

  if (nlhs < 1) return;

  // first get the model_ptr back from matlab
  RigidBodyTree *model = (RigidBodyTree *)getDrakeMexPointer(prhs[0]);

  int i, j, error, nq = model->num_positions;

  static RigidBodyTree *lastModel = NULL;
  static int lastNumJointLimits = 0;

  int equality_ind = 0;
  int inequality_ind = 0;

  // Constraint preallocation
  Matrix<double, -1, 1, 0, MAX_CONSTRS, 1> beq(MAX_CONSTRS);
  Matrix<double, -1, 1, 0, MAX_CONSTRS, 1> bin(MAX_CONSTRS);
  Matrix<double, -1, -1, RowMajor, MAX_CONSTRS, -1> Aeq(MAX_CONSTRS, nq);
  Matrix<double, -1, -1, RowMajor, MAX_CONSTRS, -1> Ain(MAX_CONSTRS, nq);

  // Add joint limits
  if (lastModel != model || true) {
    lastModel = model;
    for (i = 0; i < nq; i++) {
      if (!mxIsInf(model->joint_limit_max[i])) {
        bin(inequality_ind) = model->joint_limit_max[i];
        Ain.row(inequality_ind) = MatrixXd::Zero(1, nq);
        Ain(inequality_ind, i) = 1;

        //         cout << bin(inequality_ind) << endl << endl;
        inequality_ind++;
      }

      if (!mxIsInf(model->joint_limit_min[i])) {
        bin(inequality_ind) = -model->joint_limit_min[i];
        Ain.row(inequality_ind) << MatrixXd::Zero(1, nq);
        Ain(inequality_ind, i) = -1;
        //         cout << bin(inequality_ind) << endl << endl;
        inequality_ind++;
      }
    }
    lastNumJointLimits = inequality_ind;
  } else {
    inequality_ind = lastNumJointLimits;
  }

  // cost:  (q-q_nom)'*Q*(q-q_nom)  \equiv q'*Q*q - 2*q_nom'*Q*q  (const term
  // doesn't matter)
  Map<VectorXd> q_nom(mxGetPrSafe(prhs[2]), nq);
  Map<MatrixXd> Q(mxGetPrSafe(prhs[3]), nq, nq);
  VectorXd c = -Q * q_nom;

  double *q0 = mxGetPrSafe(prhs[1]);
  model->doKinematics(q0);

  VectorXd q0vec = Map<VectorXd>(q0, nq);

  i = 4;
  while (i < nrhs) {
    MatrixXd body_pos;
    Map<MatrixXd> *world_pos = NULL;
    int rows;
    int body_ind = mxGetScalar(prhs[i]) - 1;
    mxArray *min = NULL;
    mxArray *max = NULL;
    int n_pts;
    if (body_ind == -1) {
      int n_pts = mxGetN(prhs[i + 1]);
      body_pos.resize(4, 1);
      body_pos << 0, 0, 0, 1;
      world_pos = new Map<MatrixXd>(mxGetPrSafe(prhs[i + 1]), 3, n_pts);

      rows = 3;
      i += 2;
    } else {
      if (mxIsClass(prhs[i + 2], "struct")) {  // isstruct(worldpos)
        min = mxGetField(prhs[i + 2], 0, "min");
        max = mxGetField(prhs[i + 2], 0, "max");

        if (min == NULL || max == NULL) {
          mexErrMsgIdAndTxt(
              "Drake:approximateIKMex:BadInputs",
              "if world_pos is a struct, it must have fields .min and .max");
        }

        rows = mxGetM(min);

        if (rows != 3 && rows != 6) {
          mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
                            "world_pos.min must have 3 or 6 rows");
        }

        if (mxGetM(max) != rows) {
          mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
                            "world_pos.max must have the same number of rows "
                            "as world_pos.min");
        }
      } else {
        rows = mxGetM(prhs[i + 2]);
        int n_pts = mxGetN(prhs[i + 2]);
        world_pos = new Map<MatrixXd>(mxGetPrSafe(prhs[i + 2]), rows, n_pts);
      }

      if (mxIsClass(prhs[i + 1], "char") || mxGetM(prhs[i + 1]) == 1) {
        mexErrMsgIdAndTxt("Drake:approximateIKMex:NotImplemented",
                          "collision group not implemented in mex (mposa)");
      } else {
        if (mxGetM(prhs[i + 1]) != 3) {
          mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
                            "bodypos must be 3xmi");
        }
        n_pts = mxGetN(prhs[i + 1]);

        Map<MatrixXd> pts_tmp(mxGetPrSafe(prhs[i + 1]), 3, n_pts);
        body_pos.resize(4, n_pts);
        body_pos << pts_tmp, MatrixXd::Ones(1, n_pts);
      }
      i += 3;
    }

    MatrixXd x;
    MatrixXd J;
    if (body_ind == -1) {
      x = VectorXd::Zero(3, 1);
      Vector3d x_com;

      model->getCOM(x_com);
      model->getCOMJac(J);
      x.resize(3, 1);
      x << x_com;
    } else {
      J.resize(rows * n_pts, nq);

      model->forwardKin(body_ind, body_pos, (int)rows == 6, x);
      model->forwardJac(body_ind, body_pos, (int)rows == 6, J);

      if (rows == 6 && min == NULL && max == NULL) {
        VectorXd delta;
        angleDiff(x.block(3, 0, 3, n_pts), (*world_pos).block(3, 0, 3, n_pts),
                  &delta);
        (*world_pos).block(3, 0, 3, n_pts) = x.block(3, 0, 3, n_pts) + delta;
      }
    }

    if (max != NULL) {
      double *val = mxGetPrSafe(max);

      // add inequality constraint
      for (j = 0; j < rows; j++) {
        if (!mxIsNaN(val[j])) {
          VectorXd rowVec = J.row(j);
          double rhs = val[j] - x(j) + J.row(j) * q0vec;
          if (mxIsInf(rhs)) {
            if (rhs < 0) {
              mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
                                "RHS of a constraint evaluates to -inf");
            }
          } else {
            Ain.row(inequality_ind) << J.row(j);
            bin(inequality_ind) = rhs;
            inequality_ind++;
          }
        }
      }
    }

    if (min != NULL) {
      double *val = mxGetPrSafe(min);
      // add inequality constraint
      for (j = 0; j < rows; j++) {
        if (!mxIsNaN(val[j])) {
          VectorXd rowVec = J.row(j);
          double rhs = -(val[j] - x(j) + J.row(j) * q0vec);
          if (mxIsInf(rhs)) {
            if (rhs < 0) {
              mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
                                "RHS of a constraint evaluates to -inf");
            }
          } else {
            Ain.row(inequality_ind) << J.row(j);
            bin(inequality_ind) = rhs;
            inequality_ind++;
          }
        }
      }
    }

    if (min == NULL && max == NULL) {
      for (j = 0; j < rows; j++) {
        if (!mxIsNaN((*world_pos)(j))) {
          double rhs = (*world_pos)(j)-x(j) + J.row(j) * q0vec;

          Aeq.row(equality_ind) << J.row(j);
          beq(equality_ind) = rhs;
          equality_ind++;
        }
      }
    }
    delete world_pos;
    if (min) mxDestroyArray(min);
    if (max) mxDestroyArray(max);
  }

  Aeq.conservativeResize(equality_ind, nq);
  Ain.conservativeResize(inequality_ind, nq);
  beq.conservativeResize(equality_ind);
  bin.conservativeResize(inequality_ind);

  cout << "c is " << c.rows() << endl;
  cout << "Aeq is " << Aeq.rows() << " by " << Aeq.cols() << endl;

  VectorXd q = model->getZeroConfiguration();
  //   double result = solve_quadprog(Q, c, -Aeq, beq, -Ain, bin, q);

  VectorXd Qdiag = Q.diagonal();
  vector<MatrixBase<VectorXd> > blkQ;
  blkQ.push_back(Qdiag);
  set<int> active;
  double result = fastQP(blkQ, c, Aeq, beq, Ain, bin, active, q);

// Enable the block below to use stephens' qp code
#if USE_EIQUADPROG_BACKUP
  if (result == 1) {
    Q += 1e-8 * MatrixXd::Identity(nq, nq);
    result =
        solve_quadprog(Q, c, -Aeq.transpose(), beq, -Ain.transpose(), bin, q);

    if (mxIsInf(result)) {
      result = 1;
    } else {
      result = 0;
    }
  }
#endif

  plhs[0] = mxCreateDoubleMatrix(nq, 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[0]), q.data(), sizeof(double) * nq);

  if (nlhs > 1) {
    plhs[1] = mxCreateDoubleScalar(result);
  }

  return;
}