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);
}
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);
}
Exemple #3
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;
}
Exemple #4
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;
}
Exemple #5
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;
}
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;
}
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);
}
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)
      }
    }
  }
}
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);
}
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)
    }
  }
}
Exemple #11
0
Vector6d bodySpatialMotionPD(
    const RigidBodyTree &r, const DrakeRobotState &robot_state,
    const int body_index, const Isometry3d &body_pose_des,
    const Ref<const Vector6d> &body_v_des,
    const Ref<const Vector6d> &body_vdot_des, const Ref<const Vector6d> &Kp,
    const Ref<const Vector6d> &Kd, const Isometry3d &T_task_to_world) {
  // @param body_pose_des  desired pose in the task frame, this is the
  // homogeneous transformation from desired body frame to task frame
  // @param body_v_des    desired [xyzdot;angular_velocity] in task frame
  // @param body_vdot_des    desired [xyzddot;angular_acceleration] in task
  // frame
  // @param Kp     The gain in task frame
  // @param Kd     The gain in task frame
  // @param T_task_to_world  The homogeneous transform from task to world
  // @retval twist_dot, [angular_acceleration, xyz_acceleration] in body frame

  Isometry3d T_world_to_task = T_task_to_world.inverse();
  KinematicsCache<double> cache = r.doKinematics(robot_state.q, robot_state.qd);

  auto body_pose = r.relativeTransform(cache, 0, body_index);
  const auto &body_xyz = body_pose.translation();
  Vector3d body_xyz_task = T_world_to_task * body_xyz;
  Vector4d body_quat = rotmat2quat(body_pose.linear());
  std::vector<int> v_indices;
  auto J_geometric =
      r.geometricJacobian(cache, 0, body_index, body_index, true, &v_indices);
  VectorXd v_compact(v_indices.size());
  for (size_t i = 0; i < v_indices.size(); i++) {
    v_compact(i) = robot_state.qd(v_indices[i]);
  }
  Vector6d body_twist = J_geometric * v_compact;
  Matrix3d R_body_to_world = quat2rotmat(body_quat);
  Matrix3d R_world_to_body = R_body_to_world.transpose();
  Matrix3d R_body_to_task = T_world_to_task.linear() * R_body_to_world;
  Vector3d body_angular_vel =
      R_body_to_world *
      body_twist.head<3>();  // body_angular velocity in world frame
  Vector3d body_xyzdot =
      R_body_to_world * body_twist.tail<3>();  // body_xyzdot in world frame
  Vector3d body_angular_vel_task = T_world_to_task.linear() * body_angular_vel;
  Vector3d body_xyzdot_task = T_world_to_task.linear() * body_xyzdot;

  Vector3d body_xyz_des = body_pose_des.translation();
  Vector3d body_angular_vel_des = body_v_des.tail<3>();
  Vector3d body_angular_vel_dot_des = body_vdot_des.tail<3>();

  Vector3d xyz_err_task = body_xyz_des - body_xyz_task;

  Matrix3d R_des = body_pose_des.linear();
  Matrix3d R_err_task = R_des * R_body_to_task.transpose();
  Vector4d angleAxis_err_task = rotmat2axis(R_err_task);
  Vector3d angular_err_task =
      angleAxis_err_task.head<3>() * angleAxis_err_task(3);

  Vector3d xyzdot_err_task = body_v_des.head<3>() - body_xyzdot_task;
  Vector3d angular_vel_err_task = body_angular_vel_des - body_angular_vel_task;

  Vector3d Kp_xyz = Kp.head<3>();
  Vector3d Kd_xyz = Kd.head<3>();
  Vector3d Kp_angular = Kp.tail<3>();
  Vector3d Kd_angular = Kd.tail<3>();
  Vector3d body_xyzddot_task =
      (Kp_xyz.array() * xyz_err_task.array()).matrix() +
      (Kd_xyz.array() * xyzdot_err_task.array()).matrix() +
      body_vdot_des.head<3>();
  Vector3d body_angular_vel_dot_task =
      (Kp_angular.array() * angular_err_task.array()).matrix() +
      (Kd_angular.array() * angular_vel_err_task.array()).matrix() +
      body_angular_vel_dot_des;

  Vector6d twist_dot = Vector6d::Zero();
  Vector3d body_xyzddot = T_task_to_world.linear() * body_xyzddot_task;
  Vector3d body_angular_vel_dot =
      T_task_to_world.linear() * body_angular_vel_dot_task;
  twist_dot.head<3>() = R_world_to_body * body_angular_vel_dot;
  twist_dot.tail<3>() = R_world_to_body * body_xyzddot -
                        body_twist.head<3>().cross(body_twist.tail<3>());
  return twist_dot;
}
/**
 * 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;
}
Exemple #13
0
int main()
{
  RigidBodyTree rbm("examples/Atlas/urdf/atlas_minimal_contact.urdf");
  RigidBodyTree * model = &rbm;

  if(!model)
  {
    cerr<<"ERROR: Failed to load model"<<endl;
  }
  Vector2d tspan;
  tspan<<0,1;
  int l_hand;
  int r_hand;
  //int l_foot;
  //int r_foot;
  for(int i = 0;i<model->bodies.size();i++)
  {
    if(model->bodies[i]->linkname.compare(string("l_hand")))
    {
      l_hand = i;
    }
    else if(model->bodies[i]->linkname.compare(string("r_hand")))
    {
      r_hand = i;
    }
    //else if(model->bodies[i].linkname.compare(string("l_foot")))
    //{
    //  l_foot = i;
    //}
    //else if(model->bodies[i].linkname.compare(string("r_foot")))
    //{
    //  r_foot = i;
    //}
  }
  int nq = model->num_positions;
  VectorXd qstar = VectorXd::Zero(nq);
  qstar(3) = 0.8;
  KinematicsCache<double> cache = model->doKinematics(qstar);
  Vector3d com0 = model->centerOfMass(cache);

  Vector3d r_hand_pt = Vector3d::Zero();
  Vector3d rhand_pos0 = model->forwardKin(cache, r_hand_pt, r_hand, 0, 0);

  int nT = 4;
  double* t = new double[nT];
  double dt = 1.0/(nT-1);
  for(int i = 0;i<nT;i++)
  {
    t[i] = dt*i;
  }
  MatrixXd q0 = qstar.replicate(1,nT);
  VectorXd qdot0 = VectorXd::Zero(model->num_velocities);
  Vector3d com_lb = com0;
  com_lb(0) = std::numeric_limits<double>::quiet_NaN();
  com_lb(1) = std::numeric_limits<double>::quiet_NaN();
  Vector3d com_ub = com0;
  com_ub(0) = std::numeric_limits<double>::quiet_NaN();
  com_ub(1) = std::numeric_limits<double>::quiet_NaN();
  com_ub(2) = com0(2)+0.5;
  WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub);
  Vector3d rhand_pos_lb = rhand_pos0;
  rhand_pos_lb(0) +=0.1;
  rhand_pos_lb(1) +=0.05;
  rhand_pos_lb(2) +=0.25;
  Vector3d rhand_pos_ub = rhand_pos_lb;
  rhand_pos_ub(2) += 0.25;
  Vector2d tspan_end;
  tspan_end<<t[nT-1],t[nT-1];
  WorldPositionConstraint* kc_rhand = new WorldPositionConstraint(model,r_hand,r_hand_pt,rhand_pos_lb,rhand_pos_ub,tspan_end);
  int num_constraints = 2;
  RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
  constraint_array[0] = com_kc;
  constraint_array[1] = kc_rhand;
  IKoptions ikoptions(model);
  MatrixXd q_sol(model->num_positions,nT);
  MatrixXd qdot_sol(model->num_velocities,nT);
  MatrixXd qddot_sol(model->num_positions,nT);
  int info = 0;
  vector<string> infeasible_constraint;
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  ikoptions.setFixInitialState(false);
  ikoptions.setMajorIterationsLimit(500);
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  RowVectorXd t_inbetween(5);
  t_inbetween << 0.1,0.15,0.3,0.4,0.6;
  ikoptions.setAdditionaltSamples(t_inbetween);
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  delete com_kc;
  delete[] constraint_array;
  delete[] t;
  return 0;
}
void Pass::DoCollisionCheck(){
  
  Eigen::Vector3d robot_pos(0,0,0);

  // 1. create the list of points to be considered:
  vector< Vector3d > points;
  std::vector<unsigned int> possible_indices; // the indices of points that could possibly be in intersection: not to near and not too far
  if (which_ ==1){
    for( unsigned int i = 0; i < 10000; i++ ){
      Vector3d point(robot_pos(0) +  -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0,
                    robot_pos(1) + -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0,
                    robot_pos(2) +  -1.0 + 2.0 * ( double )( rand() % 1000 ) / 1000.0 );
      points.push_back( point );
      possible_indices.push_back( points.size() - 1 );
    }
  }else if(which_ ==2){
    for( unsigned int i = 0; i < 500; i++ ){
      Vector3d point(-0.0, -1 + 0.005*i,1.4);
      points.push_back( point );
      possible_indices.push_back( points.size() - 1 );
    }
  }else if(which_ ==3){ // useful for valkyrie (v2)
    Vector3d offset( 0.,0.,0.3);
    for( float i = -0.5; i < 0.5; i=i+0.03 ){
      for( float j = -0.5; j < 0.5; j=j+0.03 ){
        Vector3d point =  Vector3d(i,j,0.) + robot_pos + offset;
        points.push_back( point );
        possible_indices.push_back( points.size() - 1 );
      }
    }
  }else if(which_ ==4){ // useful for atlas (v5)
    Vector3d offset( 0.,0.,0.45);
    for( float i = -0.5; i < 0.5; i=i+0.03 ){
      for( float j = -0.5; j < 0.5; j=j+0.03 ){
        Vector3d point =  Vector3d(i,j,0.) + robot_pos + offset;
        points.push_back( point );
        possible_indices.push_back( points.size() - 1 );
      }
    }
  }
  
  
  // 2. Extract the indices of the points in collision:
  VectorXd q = VectorXd::Zero(drake_model_.num_positions, 1);
  KinematicsCache<double> cache = drake_model_.doKinematics(q);
  vector<size_t> filtered_point_indices = drake_model_.collidingPoints(cache, points, collision_threshold_);
  ///////////////////////////////////////////////////////////////////////////
  
  vector< Vector3d > free_points;
  vector< Vector3d > colliding_points;     
  if (verbose_){
    for( unsigned int j = 0; j < points.size(); j++ ){
      bool point_filtered = false;
      for( unsigned int k = 0; k < filtered_point_indices.size(); k++ ){
        if( j == filtered_point_indices[ k ] ){
          point_filtered = true;
        }
      }
      if( point_filtered ){
        colliding_points.push_back( points[ j ] );
      } else {
        free_points.push_back( points[ j ] );
      }
    }    
    
    cout << 0 << " | total returns "<< 0
        << " | colliding " << colliding_points.size() << " | free " << free_points.size() << endl;
    bot_lcmgl_point_size(lcmgl_, 4.5f);
    bot_lcmgl_color3f(lcmgl_, 0, 1, 0);
    for (int i = 0; i < free_points.size(); ++i) {
      bot_lcmgl_begin(lcmgl_, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl_, free_points[i][0], free_points[i][1], free_points[i][2]);
      bot_lcmgl_end(lcmgl_);
    }

    bot_lcmgl_point_size(lcmgl_, 10.5f);
    bot_lcmgl_color3f(lcmgl_, 1, 0, 0);
    for (int i = 0; i < colliding_points.size(); ++i) {
      bot_lcmgl_begin(lcmgl_, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl_, colliding_points[i][0], colliding_points[i][1], colliding_points[i][2]);
      bot_lcmgl_end(lcmgl_);
    }
    bot_lcmgl_switch_buffer(lcmgl_);  
  }
}