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