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