Matrix6x6 SpatialMotionVector::asCrossProductMatrixWrench() const
{
    Matrix6x6 res;

    Eigen::Map< Eigen::Matrix<double,6,6,Eigen::RowMajor> > retEigen(res.data());

    retEigen.block<3,3>(0,0) = skew(toEigen(this->angularVec3));
    retEigen.block<3,3>(0,3).setZero();
    retEigen.block<3,3>(3,0) = skew(toEigen(this->linearVec3));
    retEigen.block<3,3>(3,3) = skew(toEigen(this->angularVec3));

    return res;
}
Esempio n. 2
0
ArticulatedBodyInertia TransformDerivative::transform(const Transform& transform,
                                                      ArticulatedBodyInertia& other)
{
    // Inefficient implementation for the time being, this should not be a bottleneck
    Matrix6x6 oldABI = other.asMatrix();
    Matrix6x6 a_X_b_wrench  = transform.asAdjointTransformWrench();
    Matrix6x6 a_dX_b_wrench = this->asAdjointTransformWrenchDerivative(transform);
    Eigen::Matrix<double,6,6,Eigen::RowMajor> b_X_a = toEigen(a_X_b_wrench).transpose();
    Eigen::Matrix<double,6,6,Eigen::RowMajor> b_dX_a = toEigen(a_dX_b_wrench).transpose();

    Matrix6x6 newABI;

    toEigen(newABI) =  toEigen(a_dX_b_wrench)*toEigen(oldABI)*(b_X_a)
                      +toEigen(a_X_b_wrench)*toEigen(oldABI)*(b_dX_a);

    return ArticulatedBodyInertia(newABI.data(),6,6);
}
Esempio n. 3
0
Matrix6x6 TransformDerivative::asAdjointTransformDerivative(const Transform& transform) const
{
    Matrix6x6 ret;

    Eigen::Map< Eigen::Matrix<double,6,6,Eigen::RowMajor> > retEigen(ret.data());

    Eigen::Map<const Eigen::Vector3d> dp(this->posDerivative.data());
    Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor> > dR(this->rotDerivative.data());

    Eigen::Map<const Eigen::Vector3d> p(transform.getPosition().data());
    Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor> > R(transform.getRotation().data());

    retEigen.block<3,3>(0,0) = dR;
    retEigen.block<3,3>(0,3) = mySkeww(dp)*R+mySkeww(p)*dR;
    retEigen.block<3,3>(3,0).setZero();
    retEigen.block<3,3>(3,3) = dR;

    return ret;
}