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