bool BulletModel::collisionRaycast(const Matrix3Xd &origins, const Matrix3Xd &ray_endpoints, bool use_margins, VectorXd &distances)
{
distances.resize(origins.cols());
BulletCollisionWorldWrapper& bt_world = getBulletWorld(use_margins);
for (int i = 0; i < origins.cols(); i ++)
{
btVector3 ray_from_world(origins(0,i), origins(1,i), origins(2,i));
btVector3 ray_to_world(ray_endpoints(0,i), ray_endpoints(1,i), ray_endpoints(2,i));
btCollisionWorld::ClosestRayResultCallback ray_callback(ray_from_world, ray_to_world);
bt_world.bt_collision_world->rayTest(ray_from_world, ray_to_world, ray_callback);
if (ray_callback.hasHit()) {
// compute distance to hit
btVector3 end = ray_callback.m_hitPointWorld;
Vector3d end_eigen(end.getX(), end.getY(), end.getZ());
distances(i) = (end_eigen - origins.col(i)).norm();
} else {
distances(i) = -1;
}
}
return true;
}
int main(int, char**)
{
cout.precision(3);
typedef Matrix<double,3,Dynamic> Matrix3Xd;
Matrix3Xd M = Matrix3Xd::Random(3,5);
Projective3d P(Matrix4d::Random());
cout << "The matrix M is:" << endl << M << endl << endl;
cout << "M.colwise().homogeneous():" << endl << M.colwise().homogeneous() << endl << endl;
cout << "P * M.colwise().homogeneous():" << endl << P * M.colwise().homogeneous() << endl << endl;
cout << "P * M.colwise().homogeneous().hnormalized(): " << endl << (P * M.colwise().homogeneous()).colwise().hnormalized() << endl << endl;
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());
}
}
// Gets a concatenated list of contact points corresponding to a single body regardless of whether it is body A or body B
// This allows all the points to be transformed into world space using a single forwardKin call.
// INPUTS:
// cindA: list of indexes into the original list of m contact pairs where the body appeared as body A
// cindB: list of iindexes into the original list of m contact pairs where the body appeared as body B
// xA: (3 x m) matrix where each column represents a contact point on body A
// xB: (3 x m) matrix where each column represents a contact point on body B
// OUTPUTS:
// bodyPoints: (4 x n) matrix of contact points containing occurrences of body A contact points followed
// by body B contact points where n = size(cindA) + size(cindB)
// NOTE: the output is a matrix of 4-vector columns in homogeneous coordinates (x,y,z,1)'
void getBodyPoints(std::vector<size_t> const & cindA, std::vector<size_t> const & cindB, Matrix3Xd const & xA, Matrix3Xd const & xB, Matrix3Xd & bodyPoints)
{
size_t i = 0;
size_t numPtsA = cindA.size();
size_t numPtsB = cindB.size();
bodyPoints.resize(3, numPtsA + numPtsB);
for (i = 0 ; i < numPtsA ; i++ ) {
bodyPoints.col(i) = xA.col(cindA[i]);
}
for (i = 0 ; i < numPtsB ; i++ ) {
bodyPoints.col(numPtsA + i) = xB.col(cindB[i]);
}
}
unique_ptr<btCollisionShape> BulletModel::newBulletMeshShape(const DrakeShapes::Mesh& geometry, bool use_margins)
{
unique_ptr<btCollisionShape> bt_shape(new btConvexHullShape());
Matrix3Xd vertices;
if(geometry.extractMeshVertices(vertices)) {
if (use_margins)
bt_shape->setMargin(BulletModel::large_margin);
else
bt_shape->setMargin(BulletModel::small_margin);
for (int i=0; i<vertices.cols(); i++){
dynamic_cast<btConvexHullShape*>(bt_shape.get())->addPoint(btVector3(vertices(0,i),vertices(1,i),vertices(2,i)));
}
return bt_shape;
} else {
return nullptr;
}
}
// Gets a concatenated list of contact points corresponding to a single body
// regardless of whether it is body A or body B
// This allows all the points to be transformed into world space using a single
// forwardKin call.
// INPUTS:
// cindA: list of indexes into the original list of m contact pairs where the
// body appeared as body A
// cindB: list of iindexes into the original list of m contact pairs where the
// body appeared as body B
// xA: (3 x m) matrix where each column represents a contact point on body A
// xB: (3 x m) matrix where each column represents a contact point on body B
// OUTPUTS:
// bodyPoints: (4 x n) matrix of contact points containing occurrences of body
// A contact points followed
// by body B contact points where n = size(cindA) + size(cindB)
// NOTE: the output is a matrix of 4-vector columns in homogeneous coordinates
// (x, y, z, 1)'
void getBodyPoints(std::vector<size_t> const &cindA,
std::vector<size_t> const &cindB, Matrix3Xd const &xA,
Matrix3Xd const &xB,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
Matrix3Xd &bodyPoints) {
size_t i = 0;
size_t numPtsA = cindA.size();
size_t numPtsB = cindB.size();
bodyPoints.resize(3, numPtsA + numPtsB);
for (i = 0; i < numPtsA; i++) {
bodyPoints.col(i) = xA.col(cindA[i]);
}
for (i = 0; i < numPtsB; i++) {
bodyPoints.col(numPtsA + i) = xB.col(cindB[i]);
}
}
drake::lcmt_qp_controller_input QPLocomotionPlan::createQPControllerInput(
double t_global, const MatrixBase<DerivedQ>& q, const MatrixBase<DerivedV>& v, const std::vector<bool>& contact_force_detected)
{
if (std::isnan(start_time))
start_time = t_global;
double t_plan = t_global - start_time;
if (t_plan < 0)
throw std::runtime_error("t_plan is negative!"); // TODO: decide how to handle this case; fail fast for now
t_plan = std::min(t_plan, settings.duration);
// find index into supports vector
size_t support_index = 0;
while (support_index < settings.support_times.size() - 1 && t_plan >= settings.support_times[support_index + 1])
support_index++;
// do kinematics
KinematicsCache<double> cache = robot.doKinematics(q, v);
RigidBodySupportState& support_state = settings.supports[support_index];
bool is_last_support = support_index == settings.supports.size() - 1;
const RigidBodySupportState& next_support = is_last_support ? settings.supports[support_index] : settings.supports[support_index + 1];
if (settings.use_plan_shift) {
updatePlanShift(cache, t_plan, contact_force_detected, support_index);
}
drake::lcmt_qp_controller_input qp_input;
qp_input.be_silent = false;
qp_input.timestamp = static_cast<int64_t>(t_global * 1e6);
qp_input.num_support_data = 0;
qp_input.num_tracked_bodies = 0;
qp_input.num_external_wrenches = 0;
qp_input.num_joint_pd_overrides = 0;
// whole body data
auto q_des = settings.q_traj.value(t_plan);
qp_input.whole_body_data.timestamp = 0;
qp_input.whole_body_data.num_positions = robot.num_positions;
eigenVectorToStdVector(q_des, qp_input.whole_body_data.q_des);
qp_input.whole_body_data.constrained_dofs = settings.constrained_position_indices;
addOffset(qp_input.whole_body_data.constrained_dofs, 1); // use 1-indexing in LCM
qp_input.whole_body_data.num_constrained_dofs = qp_input.whole_body_data.constrained_dofs.size();
// apply plan shift
if (settings.use_plan_shift) {
for (auto direction_it = settings.plan_shift_body_motion_indices.begin(); direction_it != settings.plan_shift_body_motion_indices.end(); ++direction_it) {
qp_input.whole_body_data.q_des[*direction_it] -= plan_shift[*direction_it];
}
}
// zmp data:
qp_input.zmp_data = createZMPData(t_plan);
bool pelvis_has_tracking = false;
for (int j = 0; j < settings.body_motions.size(); ++j) {
BodyMotionData& body_motion = settings.body_motions[j];
int body_or_frame_id = body_motion.getBodyOrFrameId();
int body_id = robot.parseBodyOrFrameID(body_or_frame_id);
int body_motion_segment_index = body_motion.findSegmentIndex(t_plan);
auto side_it = foot_body_ids.begin();
for (; side_it != foot_body_ids.end(); side_it++) {
if (side_it->second == body_id) {
break;
}
}
bool is_foot = side_it != foot_body_ids.end();
if (is_foot) {
// toe off active switching logic
Side side = side_it->first;
int knee_index = knee_indices.at(side);
int akx_index = akx_indices.at(side);
int aky_index = aky_indices.at(side);
bool knee_close_to_singularity = q[knee_index] < settings.knee_settings.min_knee_angle;
if (!toe_off_active[side]) {
bool is_support_foot = isSupportingBody(body_id, support_state);
bool ankle_close_to_limit = ankleCloseToLimits(q[akx_index],q[aky_index], settings.ankle_limits_tolerance);
Matrix<double, 2, Dynamic> reduced_support_pts(2, 0);
for (int i=0; i < support_state.size(); ++i) {
RigidBodySupportStateElement& support_state_element = support_state[i];
Matrix3Xd pts;
if (support_state_element.body == body_id) {
// pts = settings.contact_groups[body_id].at("toe");
pts = getFrontTwoContactPoints(support_state_element.contact_points);
} else {
pts = support_state_element.contact_points;
}
Matrix3Xd pts_in_world = robot.transformPoints(cache, pts, support_state_element.body, 0);
reduced_support_pts.conservativeResize(2, reduced_support_pts.cols() + pts.cols());
reduced_support_pts.block(0, reduced_support_pts.cols() - pts_in_world.cols(), 2, pts_in_world.cols()) = pts_in_world.topRows<2>();
}
bool zmp_is_safely_within_reduced_support_polygon = signedDistanceInsideConvexHull(reduced_support_pts, shifted_zmp_trajectory.value(t_plan)) > settings.zmp_safety_margin;
bool toe_off_allowed = body_motion.isToeOffAllowed(body_motion_segment_index);
if (toe_off_allowed && is_support_foot && zmp_is_safely_within_reduced_support_polygon && (knee_close_to_singularity || ankle_close_to_limit)) {
std::cout << "body: " << body_id << std::endl;
std::cout << "knee q: " << q[knee_index] << " aky q: " << q[aky_index] << std::endl;
std::cout << "reduced support pts: " << std::endl << reduced_support_pts << std::endl;
std::cout << "zmp: " << shifted_zmp_trajectory.value(t_plan).transpose() << std::endl;
std::cout << "is_support: " << is_support_foot << " zmp in margin: " << zmp_is_safely_within_reduced_support_polygon << " knee: " << knee_close_to_singularity << " ankle: " << ankle_close_to_limit << std::endl;
if (is_last_support)
toe_off_active[side] = false;
else {
toe_off_active[side] = !isSupportingBody(body_id, next_support);
}
std::cout << "toe off active: " << toe_off_active[side] << std::endl;
}
}
else {
if (!isSupportingBody(body_id, support_state)) {
toe_off_active[side] = false;
knee_pd_active[side] = false;
updateSwingTrajectory(t_plan, body_motion, body_motion_segment_index - 1, cache);
}
}
if (toe_off_active[side]) {
for (int i = 0; i < support_state.size(); ++i) {
RigidBodySupportStateElement& support_state_element = support_state[i];
if (support_state_element.body == body_id)
support_state_element.contact_points = getFrontTwoContactPoints(support_state_element.contact_points);
// support_state_element.contact_points = settings.contact_groups[body_id].at("toe");
}
knee_pd_active[side] = true;
if (knee_close_to_singularity) {
knee_pd_settings[side] = settings.knee_settings;
} else {
knee_pd_settings[side] = settings.knee_settings;
knee_pd_settings[side].knee_kp = 0;
knee_pd_settings[side].knee_kd = 0;
}
}
if (knee_pd_active.at(side)) {
this->applyKneePD(side, qp_input);
}
if (body_motion.isToeOffAllowed(body_motion_segment_index)) {
updateSwingTrajectory(t_plan, body_motion, body_motion_segment_index, cache);
}
}
// extract out current and next polynomial segments
PiecewisePolynomial<double> body_motion_trajectory_slice = body_motion.getTrajectory().slice(body_motion_segment_index, std::min(2, body_motion.getTrajectory().getNumberOfSegments() - body_motion_segment_index));
// convert to global time
body_motion_trajectory_slice.shiftRight(start_time);
// apply plan shift
PiecewisePolynomial<double>::CoefficientMatrix trajectory_shift(body_motion_trajectory_slice.rows(), body_motion_trajectory_slice.cols());
trajectory_shift.setZero();
if (settings.use_plan_shift) {
for (auto direction_it = settings.plan_shift_body_motion_indices.begin(); direction_it != settings.plan_shift_body_motion_indices.end(); ++direction_it) {
trajectory_shift(*direction_it) = plan_shift(*direction_it);
}
body_motion_trajectory_slice -= trajectory_shift;
}
drake::lcmt_body_motion_data body_motion_data_for_support_lcm;
body_motion_data_for_support_lcm.timestamp = 0;
body_motion_data_for_support_lcm.body_id = body_or_frame_id + 1; // use 1-indexing in LCM
encodePiecewisePolynomial(body_motion_trajectory_slice, body_motion_data_for_support_lcm.spline);
body_motion_data_for_support_lcm.in_floating_base_nullspace = body_motion.isInFloatingBaseNullSpace(body_motion_segment_index);
body_motion_data_for_support_lcm.control_pose_when_in_contact = body_motion.isPoseControlledWhenInContact(body_motion_segment_index);
const Isometry3d& transform_task_to_world = body_motion.getTransformTaskToWorld();
Vector4d quat_task_to_world = rotmat2quat(transform_task_to_world.linear());
Vector3d translation_task_to_world = transform_task_to_world.translation();
eigenVectorToCArray(quat_task_to_world, body_motion_data_for_support_lcm.quat_task_to_world);
eigenVectorToCArray(translation_task_to_world, body_motion_data_for_support_lcm.translation_task_to_world);
eigenVectorToCArray(body_motion.getXyzProportionalGainMultiplier(), body_motion_data_for_support_lcm.xyz_kp_multiplier);
eigenVectorToCArray(body_motion.getXyzDampingRatioMultiplier(), body_motion_data_for_support_lcm.xyz_damping_ratio_multiplier);
body_motion_data_for_support_lcm.expmap_kp_multiplier = body_motion.getExponentialMapProportionalGainMultiplier();
body_motion_data_for_support_lcm.expmap_damping_ratio_multiplier = body_motion.getExponentialMapDampingRatioMultiplier();
eigenVectorToCArray(body_motion.getWeightMultiplier(), body_motion_data_for_support_lcm.weight_multiplier);
qp_input.body_motion_data.push_back(body_motion_data_for_support_lcm);
qp_input.num_tracked_bodies++;
if (body_id == pelvis_id)
pelvis_has_tracking = true;
// If this body is not currently in support, but will be soon (within
// early_contact_allowed_fraction of the duration of the current support),
// then generate a new support data for that body which will only be
// active if that body senses force.
bool last_support = support_index == settings.supports.size() - 1;
if (!last_support) {
if (t_plan >= settings.early_contact_allowed_fraction * (settings.support_times[support_index + 1] - settings.support_times[support_index]) + settings.support_times[support_index]) {
if (!isSupportingBody(body_id, support_state)) {
for (auto it = next_support.begin(); it != next_support.end(); ++it) {
if (it->body == body_id) {
qp_input.support_data.push_back(createSupportDataElement(*it, support_logic_maps.at(ONLY_IF_FORCE_SENSED)));
qp_input.num_support_data++;
break;
}
}
}
}
}
}
if (!pelvis_has_tracking)
throw runtime_error("Expecting a motion_motion_data element for the pelvis");
// set support data
for (auto it = support_state.begin(); it != support_state.end(); ++it) {
qp_input.support_data.push_back(createSupportDataElement(*it, settings.planned_support_command));
qp_input.num_support_data++;
}
qp_input.param_set_name = settings.gain_set;
for (auto it = settings.untracked_position_indices.begin(); it != settings.untracked_position_indices.end(); ++it) {
drake::lcmt_joint_pd_override joint_pd_override;
joint_pd_override.timestamp = 0;
joint_pd_override.position_ind = *it + 1; // use 1-indexing in LCM
joint_pd_override.qi_des = q[*it];
joint_pd_override.qdi_des = v[*it];
joint_pd_override.kp = 0.0;
joint_pd_override.kd = 0.0;
joint_pd_override.weight = 0.0;
qp_input.joint_pd_override.push_back(joint_pd_override);
qp_input.num_joint_pd_overrides++;
qp_input.whole_body_data.q_des[*it] = q[*it];
auto constrained_dofs_it = std::find(qp_input.whole_body_data.constrained_dofs.begin(), qp_input.whole_body_data.constrained_dofs.end(), *it + 1); // use 1-indexing in LCM
if (constrained_dofs_it != qp_input.whole_body_data.constrained_dofs.end()) {
qp_input.whole_body_data.constrained_dofs.erase(constrained_dofs_it);
qp_input.whole_body_data.num_constrained_dofs = qp_input.whole_body_data.constrained_dofs.size();
}
}
last_qp_input = qp_input;
verifySubtypeSizes(qp_input);
return qp_input;
}
