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;
}
Matrix4x4 TransformDerivative::asHomogeneousTransformDerivative() const
{
Matrix4x4 ret;
Eigen::Map< Eigen::Matrix<double,4,4,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());
retEigen.block<3,3>(0,0) = dR;
retEigen.block<3,1>(0,3) = dp;
retEigen.block<1,4>(3,0).setZero();
return ret;
}
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;
}
