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;
}
};
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;
}
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;
}
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;
};
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;
}
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());
}
}
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);
}
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);
// }
// }
// }
}
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;
}
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);
}
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);
}
// 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;
}
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);
}
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)
}
}
}
}
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;
}
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;
}
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;
}
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);
}
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;
}
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;
}
};
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;
}
