C++ (Cpp) SpatialForceVectorRaw Examples

Example #1

File: SpatialForceVectorRaw.cpp Project: nunoguedelha/idyntree

SpatialForceVectorRaw SpatialForceVectorRaw::inverse(const SpatialForceVectorRaw& op)
{
    SpatialForceVectorRaw result;

    Eigen::Map<const Vector6d> opData(op.data());
    Eigen::Map<Vector6d> resultData(result.data());

    resultData = -opData;

    return result;
}

Example #2

File: SpatialForceVectorRaw.cpp Project: nunoguedelha/idyntree

SpatialForceVectorRaw SpatialForceVectorRaw::compose(const SpatialForceVectorRaw& op1, const SpatialForceVectorRaw& op2)
{
    SpatialForceVectorRaw result;

    Eigen::Map<const Vector6d> op1Data(op1.data());
    Eigen::Map<const Vector6d> op2Data(op2.data());
    Eigen::Map<Vector6d> resultData(result.data());

    resultData = op1Data + op2Data;

    return result;
}

Example #3

File: PositionRaw.cpp Project: arocchi/idyntree

    SpatialForceVectorRaw PositionRaw::changePointOf(const SpatialForceVectorRaw & other) const
    {
        SpatialForceVectorRaw result;

        Eigen::Map<const Eigen::Vector3d> thisPos(this->data());
        Eigen::Map<const Vector6d> otherWrench(other.data());
        Eigen::Map<Vector6d> resWrench(result.data());

        resWrench.segment<3>(0) = otherWrench.segment<3>(0);
        resWrench.segment<3>(3) = thisPos.cross(otherWrench.segment<3>(0))+otherWrench.segment<3>(3);

        return result;
    }

Example #4

File: SpatialMotionVectorRaw.cpp Project: nunoguedelha/idyntree

SpatialForceVectorRaw SpatialMotionVectorRaw::cross(const SpatialForceVectorRaw& other) const
{
    SpatialForceVectorRaw resForce;
    Eigen::Map<const Eigen::Vector3d> op1VelLinear(this->data());
    Eigen::Map<const Eigen::Vector3d> op1VelAngular(this->data()+3);
    Eigen::Map<const Eigen::Vector3d> op2ForceLinear(other.data());
    Eigen::Map<const Eigen::Vector3d> op2ForceAngular(other.data()+3);
    Eigen::Map<Eigen::Vector3d> resForceLinear(resForce.data());
    Eigen::Map<Eigen::Vector3d> resForceAngular(resForce.data()+3);

    resForceLinear  = op1VelAngular.cross(op2ForceLinear);
    resForceAngular = op1VelLinear.cross(op2ForceLinear) + op1VelAngular.cross(op2ForceAngular);

    return resForce;
}

Example #5

File: SpatialMotionVectorRaw.cpp Project: nunoguedelha/idyntree

double SpatialMotionVectorRaw::dot(const SpatialForceVectorRaw& other) const
{
    Eigen::Map<const Vector6d> otherData(other.data());
    Eigen::Map<const Vector6d> thisData(this->data());

    return thisData.dot(otherData);
}

Example #6

File: SpatialForceVectorRaw.cpp Project: nunoguedelha/idyntree

SpatialForceVectorRaw::SpatialForceVectorRaw(const SpatialForceVectorRaw& other): Vector6(other.data(),6)
{

}