Ejemplo n.º 1
0
std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(const Eigen::MatrixBase<DerivedTorque> & torque, const Eigen::MatrixBase<DerivedForce> & force, const Eigen::MatrixBase<DerivedNormal> & normal, const Eigen::MatrixBase<DerivedPoint> & point_on_contact_plane)
{
    // TODO: implement multi-column version
    using namespace Eigen;

    if (abs(normal.squaredNorm() - 1.0) > 1e-12) {
        throw std::runtime_error("Drake:resolveCenterOfPressure:BadInputs: normal should be a unit vector");
    }

    Vector3d cop;
    double normal_torque_at_cop;

    double fz = normal.dot(force);
    bool cop_exists = abs(fz) > 1e-12;

    if (cop_exists) {
        auto torque_at_point_on_contact_plane = torque - point_on_contact_plane.cross(force);
        double normal_torque_at_point_on_contact_plane = normal.dot(torque_at_point_on_contact_plane);
        auto tangential_torque = torque_at_point_on_contact_plane - normal * normal_torque_at_point_on_contact_plane;
        cop = normal.cross(tangential_torque) / fz + point_on_contact_plane;
        auto torque_at_cop = torque - cop.cross(force);
        normal_torque_at_cop = normal.dot(torque_at_cop);
    }
    else {
        cop.setConstant(std::numeric_limits<double>::quiet_NaN());
        normal_torque_at_cop = std::numeric_limits<double>::quiet_NaN();
    }
    return std::pair<Vector3d, double>(cop, normal_torque_at_cop);
}
Ejemplo n.º 2
0
Archivo: main2.cpp Proyecto: Daiver/jff
float residualWithoutKWeight(
        const Eigen::MatrixBase<Derived> &weights,
        const Eigen::MatrixBase<Derived2> &sample,
        const Scalar output,
        const int k)
{
    return weights.dot(sample) - output - weights(k, 0)*sample(k, 0);
}
Ejemplo n.º 3
0
inline
typename Eigen::internal::scalar_product_traits<
    typename Eigen::internal::traits<Derived>::Scalar,
    typename Eigen::internal::traits<OtherDerived>::Scalar
>::ReturnType
static dot(
    const Eigen::MatrixBase<Derived>& mat,
    const Eigen::MatrixBase<OtherDerived>& other) {
    return mat.dot(other);
}
Ejemplo n.º 4
0
auto pardot(Eigen::MatrixBase<V1> const& A, Eigen::MatrixBase<V2> const& B, int chunkSize = 1024) {

    if(A.rows() < 10*chunkSize) {
        return A.dot(B);
    }

    using T = std::decay_t<decltype(A(0))>;
    struct r_struct{T val{0};};

    SmallVector<ReductionVariable<r_struct>, 8> S(yafel::config::num_cores, r_struct{0});

    parfor(0, A.rows(), [&](auto i) {
        auto id = worker_global::worker_id;
        S[id].val += A(i)*B(i);
    },getGlobalScheduler(), chunkSize);

    T total{0};
    for(auto &s : S){
        total += s.val;
    }
    return total;
}
Ejemplo n.º 5
0
typename T1::Scalar inner_prod(const Eigen::MatrixBase<T1> &x, const Eigen::MatrixBase<T2> &y) {
    return x.dot(y);
}