Matrix<Scalar, Dynamic, DerivedPoints::ColsAtCompileTime> forwardJacDotTimesVTemp(
    const RigidBodyTree &tree, const KinematicsCache<Scalar> &cache, const MatrixBase<DerivedPoints> &points, int current_body_or_frame_ind, int new_body_or_frame_ind, int rotation_type) {

  auto Jtransdot_times_v = tree.transformPointsJacobianDotTimesV(cache, points, current_body_or_frame_ind, new_body_or_frame_ind);
  if (rotation_type == 0) {
    return Jtransdot_times_v;
  }
  else {
    Matrix<Scalar, Dynamic, 1> Jrotdot_times_v(rotationRepresentationSize(rotation_type));
    if (rotation_type == 1) {
      Jrotdot_times_v = tree.relativeRollPitchYawJacobianDotTimesV(cache, current_body_or_frame_ind, new_body_or_frame_ind);
    }
    else if (rotation_type == 2) {
      Jrotdot_times_v = tree.relativeQuaternionJacobianDotTimesV(cache, current_body_or_frame_ind, new_body_or_frame_ind);
    }
    else {
      throw runtime_error("rotation type not recognized");
    }

    Matrix<Scalar, Dynamic, 1> Jdot_times_v((3 + rotationRepresentationSize(rotation_type)) * points.cols(), 1);

    int row_start = 0;
    for (int i = 0; i < points.cols(); i++) {
      Jdot_times_v.template middleRows<3>(row_start) = Jtransdot_times_v.template middleRows<3>(3 * i);
      row_start += 3;

      Jdot_times_v.middleRows(row_start, Jrotdot_times_v.rows()) = Jrotdot_times_v;
      row_start += Jrotdot_times_v.rows();
    }
    return Jdot_times_v;
  }
};
Esempio n. 2
0
int contactConstraintsBV(
    const RigidBodyTree<double>& r,
    const KinematicsCache<double>& cache, int nc,
    std::vector<double> support_mus,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    drake::eigen_aligned_std_vector<SupportStateElement>& supp, MatrixXd& B,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    MatrixXd& JB, MatrixXd& Jp, VectorXd& Jpdotv, MatrixXd& normals) {
  int j, k = 0, nq = r.get_num_positions();

  B.resize(3, nc * 2 * m_surface_tangents);
  JB.resize(nq, nc * 2 * m_surface_tangents);
  Jp.resize(3 * nc, nq);
  Jpdotv.resize(3 * nc);
  normals.resize(3, nc);

  Vector3d contact_pos, pos, normal;
  MatrixXd J(3, nq);
  Matrix<double, 3, m_surface_tangents> d;

  for (auto iter = supp.begin(); iter != supp.end(); iter++) {
    double mu = support_mus[iter - supp.begin()];
    double norm = sqrt(1 + mu * mu);  // because normals and ds are orthogonal,
                                      // the norm has a simple form
    if (nc > 0) {
      for (auto pt_iter = iter->contact_pts.begin();
           pt_iter != iter->contact_pts.end(); pt_iter++) {
        contact_pos = r.transformPoints(cache, *pt_iter, iter->body_idx, 0);
        J = r.transformPointsJacobian(cache, *pt_iter, iter->body_idx, 0, true);

        normal = iter->support_surface.head(3);
        surfaceTangents(normal, d);
        for (j = 0; j < m_surface_tangents; j++) {
          B.col(2 * k * m_surface_tangents + j) =
              (normal + mu * d.col(j)) / norm;
          B.col((2 * k + 1) * m_surface_tangents + j) =
              (normal - mu * d.col(j)) / norm;

          JB.col(2 * k * m_surface_tangents + j) =
              J.transpose() * B.col(2 * k * m_surface_tangents + j);
          JB.col((2 * k + 1) * m_surface_tangents + j) =
              J.transpose() * B.col((2 * k + 1) * m_surface_tangents + j);
        }

        // store away kin sols into Jp and Jpdotv
        // NOTE: I'm cheating and using a slightly different ordering of J and
        // Jdot here
        Jp.block(3 * k, 0, 3, nq) = J;
        Vector3d pt = (*pt_iter).head(3);
        Jpdotv.block(3 * k, 0, 3, 1) =
            r.transformPointsJacobianDotTimesV(cache, pt, iter->body_idx, 0);
        normals.col(k) = normal;

        k++;
      }
    }
  }

  return k;
}
Esempio n. 3
0
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs < 5) {
    mexErrMsgIdAndTxt("Drake:inverseKinmex:NotEnoughInputs",
                      "Usage "
                      "inverseKinmex(model_ptr, q_seed, q_nom, constraint1,"
                      "constraint2,..., ikoptions");
  }
  RigidBodyTree* model = (RigidBodyTree*)getDrakeMexPointer(prhs[0]);
  int nq = model->get_num_positions();
  Map<VectorXd> q_seed(mxGetPrSafe(prhs[1]), nq);
  Map<VectorXd> q_nom(mxGetPrSafe(prhs[2]), nq);
  int num_constraints = nrhs - 4;
  RigidBodyConstraint** constraint_array =
      new RigidBodyConstraint* [num_constraints];
  for (int i = 0; i < num_constraints; i++) {
    constraint_array[i] = (RigidBodyConstraint*)getDrakeMexPointer(prhs[3 + i]);
  }
  IKoptions* ikoptions = (IKoptions*)getDrakeMexPointer(prhs[nrhs - 1]);
  plhs[0] = mxCreateDoubleMatrix(nq, 1, mxREAL);
  Map<VectorXd> q_sol(mxGetPrSafe(plhs[0]), nq);
  int info;
  vector<string> infeasible_constraint;
  inverseKin(model, q_seed, q_nom, num_constraints, constraint_array,
             *ikoptions, &q_sol, &info, &infeasible_constraint);
  plhs[1] = mxCreateDoubleScalar((double)info);
  mwSize name_dim[1] = {static_cast<mwSize>(infeasible_constraint.size())};
  plhs[2] = mxCreateCellArray(1, name_dim);
  for (int i = 0; i < infeasible_constraint.size(); i++) {
    mxArray* name_ptr = mxCreateString(infeasible_constraint[i].c_str());
    mxSetCell(plhs[2], i, name_ptr);
  }
  delete[] constraint_array;
}
Esempio n. 4
0
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    if (nrhs < 2 || nlhs < 2) {
        mexErrMsgIdAndTxt(
            "Drake:positionConstraintsmex:InvalidCall",
            "Usage: [phi, J] = positionConstraintsmex(mex_model_ptr, q) ");
    }

    RigidBodyTree<double> *model =
        (RigidBodyTree<double> *)getDrakeMexPointer(prhs[0]);

    const size_t nq = model->get_num_positions();

    if (mxGetNumberOfElements(prhs[1]) != nq) {
        mexErrMsgIdAndTxt(
            "Drake:positionConstraintsmex:InvalidPositionVectorLength",
            "q contains the wrong number of elements");
    }

    Map<VectorXd> q(mxGetPrSafe(prhs[1]), nq);

    KinematicsCache<double> cache =
        model->doKinematics(q);  // FIXME: KinematicsCache should be passed in!

    auto phi = model->positionConstraints(cache);
    plhs[0] = eigenToMatlab(phi);

    auto dphi = model->positionConstraintsJacobian(cache);
    plhs[1] = eigenToMatlab(dphi);
}
Matrix<Scalar, Dynamic, DerivedPoints::ColsAtCompileTime> forwardKinTemp(const RigidBodyTree& tree,
    const KinematicsCache<Scalar> &cache, const MatrixBase<DerivedPoints> &points, int current_body_or_frame_ind, int new_body_or_frame_ind, int rotation_type) {

  Matrix<Scalar, Dynamic, DerivedPoints::ColsAtCompileTime> ret(3 + rotationRepresentationSize(rotation_type), points.cols());
  ret.template topRows<3>() = tree.transformPoints(cache, points, current_body_or_frame_ind, new_body_or_frame_ind);
  if (rotation_type == 1) {
    ret.template bottomRows<3>().colwise() = tree.relativeRollPitchYaw(cache, current_body_or_frame_ind, new_body_or_frame_ind);
  }
  else if (rotation_type == 2) {
    ret.template bottomRows<4>().colwise() = tree.relativeQuaternion(cache, current_body_or_frame_ind, new_body_or_frame_ind);
  }
  return ret;
};
Esempio n. 6
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;
}
Esempio n. 7
0
void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) {
  
  if (nrhs < 5) {
    mexErrMsgIdAndTxt("Drake:collisionRaycastmex:NotEnoughInputs","Usage collisionRaycastmex(model_ptr, cache_ptr, origin_vector, ray_endpoint, use_margins)");
  }

  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));

  const mxArray* origin_vector_mex = prhs[arg_num++];
  const mxArray* ray_endpoint_mex = prhs[arg_num++];
  bool use_margins = (mxGetScalar(prhs[arg_num++])!=0.0);

  Matrix3Xd origins(3, mxGetN(origin_vector_mex)), ray_endpoints(3, mxGetN(ray_endpoint_mex));
  
  if (!mxIsNumeric(origin_vector_mex)) {
      mexErrMsgIdAndTxt("Drake:collisionRaycastmex:InputNotNumeric","Expected a numeric value, got something else.");
  }
  
  if (!mxIsNumeric(ray_endpoint_mex)) {
      mexErrMsgIdAndTxt("Drake:collisionRaycastmex:InputNotNumeric","Expected a numeric value, got something else.");
  }
  
  
  if (mxGetM(origin_vector_mex) != 3) {
    mexErrMsgIdAndTxt("Drake:collisionRaycastmex:InputSizeWrong","Expected a 3 x N matrix, got %d x %d.", mxGetM(origin_vector_mex), mxGetN(origin_vector_mex));
  }
  
  if (mxGetM(ray_endpoint_mex) != 3) {
    mexErrMsgIdAndTxt("Drake:collisionRaycastmex:InputSizeWrong","Expected a 3-element vector for ray_endpoint, got %d elements", mxGetNumberOfElements(ray_endpoint_mex));
  }
  
  
  memcpy(origins.data(), mxGetPrSafe(origin_vector_mex), sizeof(double)*mxGetNumberOfElements(origin_vector_mex));
  memcpy(ray_endpoints.data(), mxGetPrSafe(ray_endpoint_mex), sizeof(double)*mxGetNumberOfElements(ray_endpoint_mex));
  VectorXd distances;
  Matrix3Xd normals;
  model->collisionRaycast(cache, origins, ray_endpoints, distances, normals, use_margins);
  if (nlhs>=1) {
    plhs[0] = mxCreateDoubleMatrix(static_cast<int>(distances.size()),1,mxREAL);
    memcpy(mxGetPrSafe(plhs[0]), distances.data(), sizeof(double)*distances.size());
  }
  if (nlhs>=2) {
    plhs[1] = mxCreateDoubleMatrix(3, static_cast<int>(normals.size())/3, mxREAL);
    memcpy(mxGetPrSafe(plhs[1]), normals.data(), sizeof(double)*normals.size());
  }
  
}
Esempio n. 8
0
QPLocomotionPlan::QPLocomotionPlan(RigidBodyTree & robot, const QPLocomotionPlanSettings& settings, const std::string& lcm_channel) :
    robot(robot),
    settings(settings),
    start_time(std::numeric_limits<double>::quiet_NaN()),
    pelvis_id(robot.findLinkId(settings.pelvis_name)),
    foot_body_ids(createFootBodyIdMap(robot, settings.foot_names)),
    knee_indices(createJointIndicesMap(robot, settings.knee_names)),
    akx_indices(createJointIndicesMap(robot, settings.akx_names)),
    aky_indices(createJointIndicesMap(robot, settings.aky_names)),
    plan_shift(Vector3d::Zero()),
    shifted_zmp_trajectory(settings.zmp_trajectory),
    last_foot_shift_time(0.0)
{
  for (int i = 1; i < settings.support_times.size(); i++) {
    if (settings.support_times[i] < settings.support_times[i - 1])
      throw std::runtime_error("support times must be increasing");
  }

  for (auto it = Side::values.begin(); it != Side::values.end(); ++it) {
    toe_off_active[*it] = false;
    knee_pd_active[*it] = false;
    knee_pd_settings[*it] = QPLocomotionPlanSettings::createDefaultKneeSettings();
    foot_shifts[*it] = Vector3d::Zero();
  }

  if (!lcm.good())
  {
    cerr << "ERROR: lcm is not good()" << endl;
  }
}
pair<MatrixX<Scalar>, vector<MatrixX<Scalar>>> contactConstraints(
    const RigidBodyTree &model, const KinematicsCache<Scalar> &cache,
    const Map<const Matrix3Xd> &normals, const Map<const VectorXi> &idxA,
    const Map<const VectorXi> &idxB, const Map<const Matrix3Xd> &xA,
    const Map<const Matrix3Xd> &xB, const vector<Map<const Matrix3Xd>> &d) {
  Matrix<Scalar, Dynamic, Dynamic> J;
  model.computeContactJacobians(cache, idxA, idxB, xA, xB, J);

  SparseMatrix<double> sparseNormals;
  buildSparseMatrixForContactConstraints(normals, sparseNormals);
  auto n = (sparseNormals.cast<Scalar>() * J).eval();  // dphi/dq

  size_t num_tangent_vectors = d.size();
  vector<MatrixX<Scalar>> D(2 * num_tangent_vectors,
                            MatrixX<Scalar>(d.at(0).rows(), J.cols()));
  for (int k = 0; k < num_tangent_vectors;
       k++) {  // for each friction cone basis vector
    const auto &dk = d.at(k);
    SparseMatrix<double> sparseTangents;
    buildSparseMatrixForContactConstraints(dk, sparseTangents);
    auto sparseTangents_cast = (sparseTangents.cast<Scalar>()).eval();
    D.at(k) = (sparseTangents_cast * J).eval();  // dd/dq;
    D.at(k + num_tangent_vectors) = -D.at(k);
  }

  return make_pair(n, D);
}
Esempio n. 10
0
void loadJointParams(QPControllerParams& params, const YAML::Node& config,
                     const RigidBodyTree& robot) {
  std::map<std::string, int> position_name_to_index =
      robot.computePositionNameToIndexMap();
  for (auto position_it = position_name_to_index.begin();
       position_it != position_name_to_index.end(); ++position_it) {
    try {
      loadSingleJointParams(params, position_it->second,
                            get(config, position_it->first), robot);
    } catch (...) {
      std::cerr << "error loading joint params from node: "
                << get(config, position_it->first) << std::endl;
      std::cerr << "config: " << config << std::endl;
      std::cerr << "position: " << position_it->first << std::endl;
      throw;
    }
  }

  // for (auto config_it = config.begin(); config_it != config.end();
  // ++config_it) {
  //   std::regex joint_regex = globToRegex(get(*config_it,
  //   "name").as<std::string>());
  //   for (auto position_it = position_name_to_index.begin(); position_it !=
  //   position_name_to_index.end(); ++position_it) {
  //     if (std::regex_match(position_it->first, joint_regex)) {
  //       // std::cout << get(*config_it, "name").as<std::string>() << "
  //       matches " << position_it->first << std::endl;
  //       loadSingleJointParams(params, position_it->second, get(*config_it,
  //       "params"), robot);
  //     }
  //   }
  // }
}
Esempio n. 11
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;
}
Esempio n. 12
0
const std::map<Side, int> QPLocomotionPlan::createFootBodyIdMap(RigidBodyTree & robot, const std::map<Side, std::string>& foot_names)
{
  std::map<Side, int> foot_body_ids;
  for (auto it = Side::values.begin(); it != Side::values.end(); ++it) {
    foot_body_ids[*it] = robot.findLinkId(foot_names.at(*it));
  }
  return foot_body_ids;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  if(nrhs!= 3 && nrhs != 4)
  {
    mexErrMsgIdAndTxt("Drake:testQuasiStaticConstraintmex:BadInputs","Usage [active,num_weights,c,dc] = testQuasiStaticConstraintmex(qsc_ptr,q,weights,t)");
  }
  double t;
  double* t_ptr;
  if(nrhs == 4&&mxGetNumberOfElements(prhs[3])== 1)
  {
    t = mxGetScalar(prhs[3]);
    t_ptr = &t;
  }
  else
  {
    t_ptr = nullptr;
  }
  QuasiStaticConstraint* qsc = (QuasiStaticConstraint*) getDrakeMexPointer(prhs[0]);
  bool active = qsc->isActive();
  RigidBodyTree * model = qsc->getRobotPointer();
  int nq = model->num_positions;
  Map<VectorXd> q(mxGetPrSafe(prhs[1]), nq);
  int num_weights = qsc->getNumWeights();
  double* weights = new double[num_weights];
  memcpy(weights,mxGetPrSafe(prhs[2]),sizeof(double)*num_weights);
  int num_qsc_cnst = qsc->getNumConstraint(t_ptr);
  VectorXd c(num_qsc_cnst-1);
  MatrixXd dc = MatrixXd::Zero(num_qsc_cnst-1,nq+num_weights);
  KinematicsCache<double> cache = model->doKinematics(q);
  qsc->eval(t_ptr, cache, weights, c, dc);
  VectorXd lb,ub;
  lb.resize(num_qsc_cnst-1);
  ub.resize(num_qsc_cnst-1);
  qsc->bounds(t_ptr,lb,ub);
  plhs[0] = mxCreateLogicalScalar(active);
  plhs[1] = mxCreateDoubleScalar((double) num_weights);
  plhs[2] = mxCreateDoubleMatrix(num_qsc_cnst-1,1,mxREAL);
  memcpy(mxGetPrSafe(plhs[2]),c.data(),sizeof(double)*(num_qsc_cnst-1));
  plhs[3] = mxCreateDoubleMatrix(num_qsc_cnst-1,nq+num_weights,mxREAL);
  memcpy(mxGetPrSafe(plhs[3]),dc.data(),sizeof(double)*dc.size());
  plhs[4] = mxCreateDoubleMatrix(num_qsc_cnst-1,1,mxREAL);
  memcpy(mxGetPrSafe(plhs[4]),lb.data(),sizeof(double)*(num_qsc_cnst-1));
  plhs[5] = mxCreateDoubleMatrix(num_qsc_cnst-1,1,mxREAL);
  memcpy(mxGetPrSafe(plhs[5]),ub.data(),sizeof(double)*(num_qsc_cnst-1));
  delete[] weights;
}
void doKinematicsTemp(const RigidBodyTree &model, KinematicsCache<typename DerivedQ::Scalar> &cache, const MatrixBase<DerivedQ> &q, const MatrixBase<DerivedV> &v, bool compute_JdotV) {
  // temporary solution. Explicit doKinematics calls will not be necessary in the near future.
  if (v.size() == 0 && model.num_velocities != 0)
    cache.initialize(q);
  else
    cache.initialize(q, v);
  model.doKinematics(cache, compute_JdotV);
}
Esempio n. 15
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs < 2 || nlhs < 2) {
    mexErrMsgIdAndTxt(
        "Drake:jointLimitConstraintsmex:InvalidCall",
        "Usage: [phi, J] = jointLimitConstraintsmex(mex_model_ptr, q) ");
  }

  RigidBodyTree *model = (RigidBodyTree *)getDrakeMexPointer(prhs[0]);

  if (mxGetNumberOfElements(prhs[1]) != model->number_of_positions()) {
    mexErrMsgIdAndTxt(
        "Drake:jointLimitConstraintsmex:InvalidPositionVectorLength",
        "q contains the wrong number of elements");
  }

  Map<VectorXd> q(mxGetPrSafe(prhs[1]), model->number_of_positions());

  size_t numJointConstraints = model->getNumJointLimitConstraints();

  plhs[0] = mxCreateDoubleMatrix(numJointConstraints, 1, mxREAL);
  plhs[1] =
      mxCreateDoubleMatrix(numJointConstraints, model->number_of_positions(),
        mxREAL);

  Map<VectorXd> phi(mxGetPrSafe(plhs[0]), numJointConstraints);
  Map<MatrixXd> J(mxGetPrSafe(plhs[1]), numJointConstraints,
                  model->number_of_positions());

  model->jointLimitConstraints(q, phi, J);
}
Esempio n. 16
0
// Find the joint position indices corresponding to 'name'
vector<int> getJointPositionVectorIndices(const RigidBodyTree &model, const std::string &name) {
  shared_ptr<RigidBody> joint_parent_body = model.findJoint(name);
  int num_positions = joint_parent_body->getJoint().getNumPositions();
  vector<int> ret(static_cast<size_t>(num_positions));

  // fill with sequentially increasing values, starting at joint_parent_body->position_num_start:
  iota(ret.begin(), ret.end(), joint_parent_body->position_num_start);
  return ret;
}
Esempio n. 17
0
const std::map<Side, int> QPLocomotionPlan::createJointIndicesMap(RigidBodyTree & robot, const std::map<Side, std::string>& joint_names)
{
  std::map<Side, int> joint_indices;
  for (auto it = Side::values.begin(); it != Side::values.end(); ++it) {
    int joint_id = robot.findJointId(joint_names.at(*it));
    joint_indices[*it] = robot.bodies[joint_id]->position_num_start;
  }
  return joint_indices;
}
Matrix<Scalar, TWIST_SIZE, Dynamic> geometricJacobianTemp(
    const RigidBodyTree &model, const KinematicsCache<Scalar> &cache,
    int base_body_or_frame_ind, int end_effector_body_or_frame_ind,
    int expressed_in_body_or_frame_ind, bool in_terms_of_qdot) {
    // temporary solution. Gross v_or_qdot_indices pointer will be gone soon.
    return model.geometricJacobian(
               cache, base_body_or_frame_ind, end_effector_body_or_frame_ind,
               expressed_in_body_or_frame_ind, in_terms_of_qdot, nullptr);
};
int main(int argc, char* argv[])
{
  RigidBodyTree tree;

  for (int i=0; i<10; i++) {
    tree.addRobotFromURDF(getDrakePath()+"/systems/plants/test/PointMass.urdf",DrakeJoint::ROLLPITCHYAW);
  }
  tree.addRobotFromURDF(getDrakePath()+"/systems/plants/test/FallingBrick.urdf",DrakeJoint::FIXED);

  VectorXd q = VectorXd::Random(tree.num_positions);
  VectorXd v = VectorXd::Random(tree.num_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);
}
Esempio n. 20
0
void scenario1(const RigidBodyTree& model, KinematicsCache<Scalar>& cache,
               const vector<Matrix<Scalar, Dynamic, 1>>& qs,
               const map<int, Matrix3Xd>& body_fixed_points) {
  for (const auto& q : qs) {
    cache.initialize(q);
    model.doKinematics(cache, false);
    for (const auto& pair : body_fixed_points) {
      auto J = model.transformPointsJacobian(cache, pair.second, pair.first, 0,
                                             false);
      if (uniform(generator) < 1e-15) {
        // print with some probability to avoid optimizing away
        printMatrix<decltype(J)::RowsAtCompileTime,
                    decltype(J)::ColsAtCompileTime>(
            J);  // MSVC 2013 can't infer rows and cols (ICE)
      }
    }
  }
}
Esempio n. 21
0
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs < 6) {
    mexErrMsgIdAndTxt("Drake:inverseKinPointwisemex:NotEnoughInputs",
                      "Usage "
                      "inverseKinPointwisemex(model_ptr, t, q_seed, q_nom,"
                      "constraint1, constraint2,..., ikoptions");
  }
  RigidBodyTree* model = (RigidBodyTree*)getDrakeMexPointer(prhs[0]);
  int nq = model->get_num_positions();
  int nT = static_cast<int>(mxGetNumberOfElements(prhs[1]));
  double* t = mxGetPrSafe(prhs[1]);
  Map<MatrixXd> q_seed(mxGetPrSafe(prhs[2]), nq, nT);
  Map<MatrixXd> q_nom(mxGetPrSafe(prhs[3]), nq, nT);
  int num_constraints = nrhs - 5;
  RigidBodyConstraint** constraint_array =
      new RigidBodyConstraint* [num_constraints];
  for (int i = 0; i < num_constraints; i++) {
    constraint_array[i] = (RigidBodyConstraint*)getDrakeMexPointer(prhs[4 + i]);
  }
  IKoptions* ikoptions = (IKoptions*)getDrakeMexPointer(prhs[nrhs - 1]);
  plhs[0] = mxCreateDoubleMatrix(nq, nT, mxREAL);
  Map<MatrixXd> q_sol(mxGetPrSafe(plhs[0]), nq, nT);
  int* info = new int[nT];
  vector<string> infeasible_constraint;
  inverseKinPointwise(model, nT, t, q_seed, q_nom, num_constraints,
                      constraint_array, *ikoptions,
                      &q_sol, info, &infeasible_constraint);

  plhs[1] = mxCreateDoubleMatrix(1, nT, mxREAL);
  for (int i = 0; i < nT; i++) {
    *(mxGetPrSafe(plhs[1]) + i) = (double)info[i];
  }
  mwSize name_dim[1] = {static_cast<mwSize>(infeasible_constraint.size())};
  plhs[2] = mxCreateCellArray(1, name_dim);
  for (int i = 0; i < infeasible_constraint.size(); i++) {
    mxArray* name_ptr = mxCreateString(infeasible_constraint[i].c_str());
    mxSetCell(plhs[2], i, name_ptr);
  }
  delete[] info;
  delete[] constraint_array;
}
Esempio n. 22
0
RobotPropertyCache parseKinematicTreeMetadata(
    const YAML::Node& metadata,
    const RigidBodyTree<double>& robot) {
  RobotPropertyCache ret;
  std::map<Side, string> side_identifiers = {{Side::RIGHT, "r"},
                                             {Side::LEFT, "l"}};

  YAML::Node body_names = metadata["body_names"];
  YAML::Node feet = body_names["feet"];
  YAML::Node hands = body_names["hands"];
  for (const auto& side : Side::values) {
    ret.foot_ids[side] =
        robot.FindBodyIndex(feet[side_identifiers[side]].as<string>());
  }

  YAML::Node joint_group_names = metadata["joint_group_names"];
  for (const auto& side : Side::values) {
    auto side_id = side_identifiers.at(side);
    ret.position_indices.legs[side] = findPositionIndices(
        robot, joint_group_names["legs"][side_id].as<vector<string>>());
    ret.position_indices.knees[side] =
        robot
            .FindChildBodyOfJoint(
                joint_group_names["knees"][side_id].as<string>())
            ->get_position_start_index();
    ret.position_indices.ankles[side] = findPositionIndices(
        robot, joint_group_names["ankles"][side_id].as<vector<string>>());
    ret.position_indices.arms[side] = findPositionIndices(
        robot, joint_group_names["arms"][side_id].as<vector<string>>());
  }
  ret.position_indices.neck = findPositionIndices(
      robot, joint_group_names["neck"].as<vector<string>>());
  ret.position_indices.back_bkz =
      robot.FindChildBodyOfJoint(joint_group_names["back_bkz"].as<string>())
          ->get_position_start_index();
  ret.position_indices.back_bky =
      robot.FindChildBodyOfJoint(joint_group_names["back_bky"].as<string>())
          ->get_position_start_index();

  return ret;
}
Esempio n. 23
0
vector<int> findPositionIndices(const RigidBodyTree& robot,
                                const vector<string>& joint_names) {
  vector<int> position_indices;
  position_indices.reserve(joint_names.size());
  for (const auto& joint_name : joint_names) {
    const RigidBody& body = *robot.FindChildBodyOfJoint(joint_name);
    for (int i = 0; i < body.getJoint().getNumPositions(); i++) {
      position_indices.push_back(body.get_position_start_index() + i);
    }
  }
  return position_indices;
}
Esempio n. 24
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  std::string usage =
      "Usage [phi, n, x, body_x, body_idx] = "
      "collisionDetectFromPointsmex(model_ptr, cache_ptr, points, use_margins)";
  if (nrhs != 4) {
    mexErrMsgIdAndTxt("Drake:collisionDetectFromPointsmex:WrongNumberOfInputs",
                      usage.c_str());
    printf("Had %d inputs\n", nrhs);
  }

  if (nlhs != 5) {
    mexErrMsgIdAndTxt("Drake:collisionDetectFromPointsmex:WrongNumberOfOutputs",
                      usage.c_str());
    printf("Had %d outputs\n", nlhs);
  }

  int arg_num = 0;
  RigidBodyTree *model =
      static_cast<RigidBodyTree *>(getDrakeMexPointer(prhs[arg_num++]));
  KinematicsCache<double> *cache = static_cast<KinematicsCache<double> *>(
      getDrakeMexPointer(prhs[arg_num++]));

  auto points = matlabToEigen<SPACE_DIMENSION, Eigen::Dynamic>(prhs[arg_num++]);
  bool use_margins = (bool)(mxGetLogicals(prhs[arg_num++]))[0];

  VectorXd phi;
  Matrix3Xd normal;
  Matrix3Xd x;
  Matrix3Xd body_x;
  vector<int> body_idx;

  model->collisionDetectFromPoints(*cache, points, phi, normal, x, body_x,
                                   body_idx, use_margins);

  plhs[0] = eigenToMatlab(phi);
  plhs[1] = eigenToMatlab(normal);
  plhs[2] = eigenToMatlab(x);
  plhs[3] = eigenToMatlab(body_x);
  plhs[4] = stdVectorToMatlab(body_idx);
}
Esempio n. 25
0
Matrix<bool, Dynamic, 1> getActiveSupportMask(
    const RigidBodyTree &r, VectorXd q, VectorXd qd,
    std::vector<SupportStateElement,
                Eigen::aligned_allocator<SupportStateElement>> &
        available_supports,
    const Ref<const Matrix<bool, Dynamic, 1>> &contact_force_detected,
    double contact_threshold) {
  KinematicsCache<double> cache = r.doKinematics(q, qd);

  size_t nsupp = available_supports.size();
  Matrix<bool, Dynamic, 1> active_supp_mask =
      Matrix<bool, Dynamic, 1>::Zero(nsupp);
  VectorXd phi;
  SupportStateElement se;
  bool needs_kin_check;
  bool kin_contact;
  bool force_contact;

  for (size_t i = 0; i < nsupp; i++) {
    // mexPrintf("evaluating support: %d\n", i);
    se = available_supports[i];

    force_contact = (contact_force_detected(se.body_idx) != 0);
    // Determine if the body needs to be checked for kinematic contact. We only
    // need to check for kin contact if the logic map indicates that the
    // presence or absence of such contact would  affect the decision about
    // whether to use that body as a support.
    needs_kin_check = (((se.support_logic_map[1] != se.support_logic_map[0]) &&
                        (contact_force_detected(se.body_idx) == 0)) ||
                       ((se.support_logic_map[3] != se.support_logic_map[2]) &&
                        (contact_force_detected(se.body_idx) == 1)));

    if (needs_kin_check) {
      if (contact_threshold == -1) {
        kin_contact = true;
      } else {
        contactPhi(r, cache, se, phi);
        kin_contact = (phi.minCoeff() <= contact_threshold);
      }
    } else {
      kin_contact = false;  // we've determined already that kin contact doesn't
                            // matter for this support element
    }

    active_supp_mask(i) =
        isSupportElementActive(&se, force_contact, kin_contact);
    // mexPrintf("needs check: %d force contact: %d kin_contact: %d is_active:
    // %d\n", needs_kin_check, force_contact, kin_contact, active_supp_mask(i));
  }
  return active_supp_mask;
}
Esempio n. 26
0
void scenario2(
    const RigidBodyTree& model, KinematicsCache<Scalar>& cache,
    const vector<pair<Matrix<Scalar, Dynamic, 1>, Matrix<Scalar, Dynamic, 1>>>&
        states) {
  const eigen_aligned_unordered_map<RigidBody const*,
                                    Matrix<Scalar, TWIST_SIZE, 1>>
      f_ext;
  for (const auto& state : states) {
    cache.initialize(state.first, state.second);
    model.doKinematics(cache, true);
    auto H = model.massMatrix(cache);
    auto C = model.dynamicsBiasTerm(cache, f_ext);
    if (uniform(generator) <
        1e-15) {  // print with some probability to avoid optimizing away
      printMatrix<decltype(H)::RowsAtCompileTime,
                  decltype(H)::ColsAtCompileTime>(
          H);  // MSVC 2013 can't infer rows and cols (ICE)
      printMatrix<decltype(C)::RowsAtCompileTime,
                  decltype(C)::ColsAtCompileTime>(
          C);  // MSVC 2013 can't infer rows and cols (ICE)
    }
  }
}
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);
}
Matrix<Scalar, Dynamic, 1>  dynamicsBiasTermTemp(const RigidBodyTree &model, KinematicsCache<Scalar> &cache, const MatrixBase<DerivedF> &f_ext_value) {
  // temporary solution.

  eigen_aligned_unordered_map<const RigidBody *, Matrix<Scalar, 6, 1> > f_ext;

  if (f_ext_value.size() > 0) {
    assert(f_ext_value.cols() == model.bodies.size());
    for (DenseIndex i = 0; i < f_ext_value.cols(); i++) {
      f_ext.insert({model.bodies[i].get(), f_ext_value.col(i)});
    }
  }

  return model.dynamicsBiasTerm(cache, f_ext);
};
Matrix<Scalar, Dynamic, Dynamic> forwardKinJacobianTemp(
    const RigidBodyTree &tree, const KinematicsCache<Scalar> &cache,
    const MatrixBase<DerivedPoints> &points, int current_body_or_frame_ind,
    int new_body_or_frame_ind, int rotation_type, bool in_terms_of_qdot) {
    auto Jtrans =
        tree.transformPointsJacobian(cache, points, current_body_or_frame_ind,
                                     new_body_or_frame_ind, in_terms_of_qdot);
    if (rotation_type == 0)
        return Jtrans;
    else {
        Matrix<Scalar, Dynamic, Dynamic> Jrot(
            rotationRepresentationSize(rotation_type), Jtrans.cols());
        if (rotation_type == 1)
            Jrot = tree.relativeRollPitchYawJacobian(cache, current_body_or_frame_ind,
                    new_body_or_frame_ind,
                    in_terms_of_qdot);
        else if (rotation_type == 2)
            Jrot = tree.relativeQuaternionJacobian(cache, current_body_or_frame_ind,
                                                   new_body_or_frame_ind,
                                                   in_terms_of_qdot);
        else
            throw runtime_error("rotation_type not recognized");
        Matrix<Scalar, Dynamic, Dynamic> J((3 + Jrot.rows()) * points.cols(),
                                           Jtrans.cols());
        int row_start = 0;
        for (int i = 0; i < points.cols(); i++) {
            J.template middleRows<3>(row_start) =
                Jtrans.template middleRows<3>(3 * i);
            row_start += 3;

            J.middleRows(row_start, Jrot.rows()) = Jrot;
            row_start += Jrot.rows();
        }
        return J;
    }
};
Esempio n. 30
0
int contactPhi(const RigidBodyTree &r, const KinematicsCache<double> &cache,
               SupportStateElement &supp, VectorXd &phi) {
  int nc = static_cast<int>(supp.contact_pts.size());
  phi.resize(nc);

  if (nc < 1) return nc;

  int i = 0;
  for (auto pt_iter = supp.contact_pts.begin();
       pt_iter != supp.contact_pts.end(); pt_iter++) {
    Vector3d contact_pos = r.transformPoints(cache, *pt_iter, supp.body_idx, 0);
    phi(i) = supp.support_surface.head<3>().dot(contact_pos) +
             supp.support_surface(3);
    i++;
  }
  return nc;
}