Пример #1
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->number_of_velocities());
  int i;

  if (argc >= 2 + model->number_of_positions()) {
    for (i = 0; i < model->number_of_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 < static_cast<int>(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].name_ << " is at
    //    " << pt.transpose() << endl;
    cout << model->bodies[i]->name_ << " ";
    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;
}
int main(int argc, char* argv[]) {
  RigidBodyTree tree;

  for (int i = 0; i < 10; ++i) {
    drake::parsers::urdf::AddModelInstanceFromURDF(
        drake::GetDrakePath() + "/systems/plants/test/PointMass.urdf",
            DrakeJoint::ROLLPITCHYAW, &tree);
  }

  drake::parsers::urdf::AddModelInstanceFromURDF(
      drake::GetDrakePath() + "/systems/plants/test/FallingBrick.urdf",
          DrakeJoint::FIXED, &tree);

  VectorXd q = VectorXd::Random(tree.number_of_positions());
  VectorXd v = VectorXd::Random(tree.number_of_velocities());
  auto kinsol = tree.doKinematics(q, v);

  VectorXd phi;
  Matrix3Xd normal, xA, xB;
  std::vector<int> bodyA_idx, bodyB_idx;
  tree.collisionDetect(kinsol, phi, normal, xA, xB, bodyA_idx, bodyB_idx,
                       false);
}
Пример #3
0
void testScenario2(const RigidBodyTree& model) {
  int ntests = 1000;

  vector<pair<VectorXd, VectorXd>> states_double;
  vector<pair<Matrix<AutoDiffFixedMaxSize, Dynamic, 1>,
              Matrix<AutoDiffFixedMaxSize, Dynamic, 1>>>
      states_autodiff_fixed;
  vector<pair<Matrix<AutoDiffDynamicSize, Dynamic, 1>,
              Matrix<AutoDiffDynamicSize, Dynamic, 1>>>
      states_autodiff_dynamic;
  default_random_engine generator;

  for (int i = 0; i < ntests; i++) {
    VectorXd q = model.getRandomConfiguration(generator);
    VectorXd v = VectorXd::Random(model.number_of_velocities());
    VectorXd x(q.rows() + v.rows());
    x << q, v;
    states_double.push_back(make_pair(q, v));

    MatrixXd grad = MatrixXd::Identity(x.size(), x.size());

    auto x_autodiff_fixed = x.cast<AutoDiffFixedMaxSize>().eval();
    gradientMatrixToAutoDiff(grad, x_autodiff_fixed);
    Matrix<AutoDiffFixedMaxSize, Dynamic, 1> q_autodiff_fixed =
        x_autodiff_fixed.topRows(model.number_of_positions());
    Matrix<AutoDiffFixedMaxSize, Dynamic, 1> v_autodiff_fixed =
        x_autodiff_fixed.bottomRows(model.number_of_velocities());
    states_autodiff_fixed.push_back(
        make_pair(q_autodiff_fixed, v_autodiff_fixed));

    auto x_autodiff_dynamic = x.cast<AutoDiffDynamicSize>().eval();
    gradientMatrixToAutoDiff(grad, x_autodiff_dynamic);
    Matrix<AutoDiffDynamicSize, Dynamic, 1> q_autodiff_dynamic =
        x_autodiff_dynamic.topRows(model.number_of_positions());
    Matrix<AutoDiffDynamicSize, Dynamic, 1> v_autodiff_dynamic =
        x_autodiff_dynamic.bottomRows(model.number_of_velocities());
    states_autodiff_dynamic.push_back(
        make_pair(q_autodiff_dynamic, v_autodiff_dynamic));
  }

  KinematicsCache<double> cache_double(model.bodies);
  KinematicsCache<AutoDiffFixedMaxSize> cache_autodiff_fixed(model.bodies);
  KinematicsCache<AutoDiffDynamicSize> cache_autodiff_dynamic(model.bodies);

  cout << "scenario 2:" << endl;
  cout << "no gradients: "
       << measure<>::execution(scenario2<double>, model, cache_double,
                               states_double) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << "autodiff fixed max size: "
       << measure<>::execution(scenario2<AutoDiffFixedMaxSize>, model,
                               cache_autodiff_fixed, states_autodiff_fixed) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << "autodiff dynamic size: "
       << measure<>::execution(scenario2<AutoDiffDynamicSize>, model,
                               cache_autodiff_dynamic,
                               states_autodiff_dynamic) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << endl;
}
Пример #4
0
// [z, Mvn, wvn] = setupLCPmex(mex_model_ptr, cache_ptr, u, phiC, n, D, h,
//   z_inactive_guess_tol)
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nlhs != 5 || nrhs != 13) {
    mexErrMsgIdAndTxt("Drake:setupLCPmex:InvalidUsage",
                      "Usage: [z, Mvn, wvn, zqp] = setupLCPmex(mex_model_ptr, "
                      "cache_ptr, u, phiC, n, D, h, z_inactive_guess_tol, "
                      "z_cached, H, C, B)");
  }
  static unique_ptr<MexWrapper> lcp_mex =
      unique_ptr<MexWrapper>(new MexWrapper(PATHLCP_MEXFILE));

  int arg_num = 0;
  RigidBodyTree* model =
      static_cast<RigidBodyTree*>(getDrakeMexPointer(prhs[arg_num++]));
  KinematicsCache<double>& cache =
      fromMex(prhs[arg_num++], static_cast<KinematicsCache<double>*>(nullptr));
  cache.checkCachedKinematicsSettings(true, true, "solveLCPmex");

  const int nq = model->number_of_positions();
  const int nv = model->number_of_velocities();

  // input mappings
  const mxArray* u_array = prhs[arg_num++];
  const mxArray* phiC_array = prhs[arg_num++];
  const mxArray* n_array = prhs[arg_num++];
  const mxArray* D_array = prhs[arg_num++];
  const mxArray* h_array = prhs[arg_num++];
  const mxArray* inactive_guess_array = prhs[arg_num++];
  const mxArray* z_cached_array = prhs[arg_num++];
  const mxArray* H_array = prhs[arg_num++];
  const mxArray* C_array = prhs[arg_num++];
  const mxArray* B_array = prhs[arg_num++];
  const mxArray* enable_fastqp_array = prhs[arg_num++];

  const size_t num_z_cached = mxGetNumberOfElements(z_cached_array);
  const size_t num_contact_pairs = mxGetNumberOfElements(phiC_array);
  const size_t mC = mxGetNumberOfElements(D_array);
  const double z_inactive_guess_tol = mxGetScalar(inactive_guess_array);
  const double h = mxGetScalar(h_array);
  const auto& q = cache.getQ();
  const auto& v = cache.getV();
  const Map<VectorXd> u(mxGetPrSafe(u_array), mxGetNumberOfElements(u_array));
  const Map<VectorXd> phiC(mxGetPrSafe(phiC_array), num_contact_pairs);
  const Map<MatrixXd> n(mxGetPrSafe(n_array), num_contact_pairs, nq);
  const Map<VectorXd> z_cached(mxGetPrSafe(z_cached_array), num_z_cached);
  const Map<MatrixXd> H(mxGetPrSafe(H_array), nv, nv);
  const Map<VectorXd> C(mxGetPrSafe(C_array), nv);
  const Map<MatrixXd> B(mxGetPrSafe(B_array), mxGetM(B_array), mxGetN(B_array));

  const bool enable_fastqp = mxIsLogicalScalarTrue(enable_fastqp_array);

  VectorXd phiL, phiL_possible, phiC_possible, phiL_check, phiC_check;
  MatrixXd JL, JL_possible, n_possible, JL_check, n_check;

  auto phiP = model->positionConstraints(cache);
  auto JP = model->positionConstraintsJacobian(cache);
  model->jointLimitConstraints(q, phiL, JL);

  const size_t nP = phiP.size();

  // Convert jacobians to velocity mappings
  const MatrixXd n_velocity =
      cache.transformPositionDotMappingToVelocityMapping(n);
  const MatrixXd JL_velocity =
      cache.transformPositionDotMappingToVelocityMapping(JL);
  const auto JP_velocity =
      cache.transformPositionDotMappingToVelocityMapping(JP);

  plhs[2] = mxCreateDoubleMatrix(nv, 1, mxREAL);
  Map<VectorXd> wvn(mxGetPrSafe(plhs[2]), nv);

  LLT<MatrixXd> H_cholesky(H);  // compute the Cholesky decomposition of H
  wvn = H_cholesky.solve(B * u - C);
  wvn *= h;
  wvn += v;

  // use forward euler step in joint space as
  // initial guess for active constraints
  vector<bool> possible_contact;
  vector<bool> possible_jointlimit;
  vector<bool> z_inactive;

  getThresholdInclusion((phiC + h * n_velocity * v).eval(),
                        z_inactive_guess_tol, possible_contact);
  getThresholdInclusion((phiL + h * JL_velocity * v).eval(),
                        z_inactive_guess_tol, possible_jointlimit);

  while (true) {
    // continue from here if our inactive guess fails
    const size_t nC = getNumTrue(possible_contact);
    const size_t nL = getNumTrue(possible_jointlimit);
    const size_t lcp_size = nL + nP + (mC + 2) * nC;

    plhs[0] = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(nv, lcp_size, mxREAL);

    Map<VectorXd> z(mxGetPrSafe(plhs[0]), lcp_size);
    Map<MatrixXd> Mvn(mxGetPrSafe(plhs[1]), nv, lcp_size);

    if (lcp_size == 0) {
      plhs[3] = mxCreateDoubleMatrix(1, possible_contact.size(), mxREAL);
      plhs[4] = mxCreateDoubleMatrix(1, possible_jointlimit.size(), mxREAL);
      return;
    }
    z = VectorXd::Zero(lcp_size);

    vector<size_t> possible_contact_indices;
    getInclusionIndices(possible_contact, possible_contact_indices, true);

    partitionVector(possible_contact, phiC, phiC_possible, phiC_check);
    partitionVector(possible_jointlimit, phiL, phiL_possible, phiL_check);
    partitionMatrix(possible_contact, n_velocity, n_possible, n_check);
    partitionMatrix(possible_jointlimit, JL_velocity, JL_possible, JL_check);

    MatrixXd D_possible(mC * nC, nv);
    for (size_t i = 0; i < mC; i++) {
      Map<MatrixXd> D_i(mxGetPrSafe(mxGetCell(D_array, i)), num_contact_pairs,
                        nq);
      MatrixXd D_i_possible, D_i_exclude;
      filterByIndices(possible_contact_indices, D_i, D_i_possible);
      D_possible.block(nC * i, 0, nC, nv) =
          cache.transformPositionDotMappingToVelocityMapping(D_i_possible);
    }

    // J in velocity coordinates
    MatrixXd J(lcp_size, nv);
    J << JL_possible, JP_velocity, n_possible, D_possible,
        MatrixXd::Zero(nC, nv);
    Mvn = H_cholesky.solve(J.transpose());

    // solve LCP problem
    // TODO(psiorx): call path from C++ (currently only 32-bit C libraries
    // available)
    mxArray* mxw = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);
    mxArray* mxlb = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);
    mxArray* mxub = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);

    Map<VectorXd> lb(mxGetPrSafe(mxlb), lcp_size);
    Map<VectorXd> ub(mxGetPrSafe(mxub), lcp_size);
    lb << VectorXd::Zero(nL), -BIG * VectorXd::Ones(nP),
        VectorXd::Zero(nC + mC * nC + nC);
    ub = BIG * VectorXd::Ones(lcp_size);

    MatrixXd M(lcp_size, lcp_size);
    Map<VectorXd> w(mxGetPrSafe(mxw), lcp_size);

    // build LCP matrix
    M << h* JL_possible* Mvn, h* JP_velocity* Mvn, h* n_possible* Mvn,
        D_possible* Mvn, MatrixXd::Zero(nC, lcp_size);

    if (nC > 0) {
      for (size_t i = 0; i < mC; i++) {
        M.block(nL + nP + nC + nC * i, nL + nP + nC + mC * nC, nC, nC) =
            MatrixXd::Identity(nC, nC);
        M.block(nL + nP + nC + mC * nC, nL + nP + nC + nC * i, nC, nC) =
            -MatrixXd::Identity(nC, nC);
      }
      double mu = 1.0;  // TODO(psiorx): pull this from contactConstraints
      M.block(nL + nP + nC + mC * nC, nL + nP, nC, nC) =
          mu * MatrixXd::Identity(nC, nC);
    }

    // build LCP vector
    w << phiL_possible + h* JL_possible* wvn, phiP + h* JP_velocity* wvn,
        phiC_possible + h* n_possible* wvn, D_possible* wvn, VectorXd::Zero(nC);

    // try fastQP first
    bool qp_failed = true;

    if (enable_fastqp) {
      if (num_z_cached != lcp_size) {
        z_inactive.clear();
        for (int i = 0; i < lcp_size; i++) {
          z_inactive.push_back(true);
        }
      } else {
        getThresholdInclusion((lb - z_cached).eval(), -SMALL, z_inactive);
      }
      qp_failed = !callFastQP(M, w, lb, z_inactive, nL + nP + nC, z);
    }

    int nnz;
    mxArray* mxM_sparse = eigenToMatlabSparse(M, nnz);
    mxArray* mxnnzJ = mxCreateDoubleScalar(static_cast<double>(nnz));
    mxArray* mxn = mxCreateDoubleScalar(static_cast<double>(lcp_size));
    mxArray* mxz = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);
    mxArray* lhs[2];
    mxArray* rhs[] = {mxn, mxnnzJ, mxz, mxlb, mxub, mxM_sparse, mxw};

    Map<VectorXd> z_path(mxGetPr(mxz), lcp_size);
    z_path = VectorXd::Zero(lcp_size);
    // fall back to pathlcp
    if (qp_failed) {
      lcp_mex->mexFunction(2, lhs, 7, const_cast<const mxArray**>(rhs));
      z = z_path;
      mxDestroyArray(lhs[0]);
      mxDestroyArray(lhs[1]);
    }

    mxDestroyArray(mxz);
    mxDestroyArray(mxn);
    mxDestroyArray(mxnnzJ);
    mxDestroyArray(mxub);
    mxDestroyArray(mxlb);
    mxDestroyArray(mxw);
    mxDestroyArray(mxM_sparse);

    VectorXd vn = Mvn * z + wvn;

    vector<size_t> impossible_contact_indices, impossible_limit_indices;
    getInclusionIndices(possible_contact, impossible_contact_indices, false);
    getInclusionIndices(possible_jointlimit, impossible_limit_indices, false);

    vector<bool> penetrating_joints, penetrating_contacts;
    getThresholdInclusion((phiL_check + h * JL_check * vn).eval(), 0.0,
                          penetrating_joints);
    getThresholdInclusion((phiC_check + h * n_check * vn).eval(), 0.0,
                          penetrating_contacts);

    const size_t num_penetrating_joints = getNumTrue(penetrating_joints);
    const size_t num_penetrating_contacts = getNumTrue(penetrating_contacts);
    const size_t penetrations =
        num_penetrating_joints + num_penetrating_contacts;
    // check nonpenetration assumptions
    if (penetrations > 0) {
      // revise joint limit active set
      for (size_t i = 0; i < impossible_limit_indices.size(); i++) {
        if (penetrating_joints[i]) {
          possible_jointlimit[impossible_limit_indices[i]] = true;
        }
      }

      // revise contact constraint active set
      for (size_t i = 0; i < impossible_contact_indices.size(); i++) {
        if (penetrating_contacts[i]) {
          possible_contact[impossible_contact_indices[i]] = true;
        }
      }
      // throw away our old solution and try again
      mxDestroyArray(plhs[0]);
      mxDestroyArray(plhs[1]);
      continue;
    }
    // our initial guess was correct. we're done
    break;
  }

  plhs[3] = mxCreateDoubleMatrix(1, possible_contact.size(), mxREAL);
  plhs[4] = mxCreateDoubleMatrix(1, possible_jointlimit.size(), mxREAL);

  Map<VectorXd> possible_contact_map(mxGetPrSafe(plhs[3]),
                                     possible_contact.size());
  Map<VectorXd> possible_jointlimit_map(mxGetPrSafe(plhs[4]),
                                        possible_jointlimit.size());

  for (size_t i = 0; i < possible_contact.size(); i++) {
    possible_contact_map[i] = static_cast<double>(possible_contact[i]);
  }

  for (size_t i = 0; i < possible_jointlimit.size(); i++) {
    possible_jointlimit_map[i] = static_cast<double>(possible_jointlimit[i]);
  }
}