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;
};
Example #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;
}