SpatialForceVector SpatialMotionVector::cross(const SpatialForceVector& other) const { SpatialForceVector res; res.getLinearVec3() = this->angularVec3.cross(other.getLinearVec3()); res.getAngularVec3() = this->linearVec3.cross(other.getLinearVec3()) + this->angularVec3.cross(other.getAngularVec3()); return res; }
void checkInertiaAccProduct(const ArticulatedBodyInertia & inertia, const SpatialMotionVector & twist) { SpatialForceVector momentum = inertia*twist; Vector6 momentumCheck; Matrix6x6 I = inertia.asMatrix(); Vector6 twistPlain = twist.asVector(); toEigen(momentumCheck) = toEigen(I)*toEigen(twistPlain); ASSERT_EQUAL_VECTOR(momentum.asVector(),momentumCheck); }
SpatialForceVector PositionRaw::changePointOf(const SpatialForceVector & other) const { SpatialForceVector result; Eigen::Map<const Eigen::Vector3d> thisPos(this->data()); Eigen::Map<const Eigen::Vector3d> otherLinear(other.getLinearVec3().data()); Eigen::Map<const Eigen::Vector3d> otherAngular(other.getAngularVec3().data()); Eigen::Map<Eigen::Vector3d> resLinear(result.getLinearVec3().data()); Eigen::Map<Eigen::Vector3d> resAngular(result.getAngularVec3().data()); resLinear = otherLinear; resAngular = thisPos.cross(otherLinear) + otherAngular; return result; }
SpatialForceVector TransformDerivative::transform(const Transform& transform, SpatialForceVector& other) { SpatialForceVector ret; Eigen::Map<const Eigen::Vector3d> p(transform.getPosition().data()); Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor> > R(transform.getRotation().data()); Eigen::Map<const Eigen::Vector3d> dp(this->posDerivative.data()); Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor> > dR(this->rotDerivative.data()); toEigen(ret.getLinearVec3()) = dR*toEigen(other.getLinearVec3()); toEigen(ret.getAngularVec3()) = dR*toEigen(other.getAngularVec3()) + p.cross(toEigen(ret.getLinearVec3())) + dp.cross(R*toEigen(other.getLinearVec3())); return ret; }