diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h index 9ce30f0b6..dc0c55510 100644 --- a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h +++ b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h @@ -48,6 +48,10 @@ class OBBTreeNodeImpl; class OBBTree; class Plane; +static const CoordinateAxis TangentAxis = CoordinateAxis::XCoordinateAxis(); +static const CoordinateAxis NormalAxis = CoordinateAxis::YCoordinateAxis(); +static const CoordinateAxis BinormalAxis = CoordinateAxis::ZCoordinateAxis(); + //============================================================================== @@ -118,6 +122,84 @@ class Cylinder; class Brick; class TriangleMesh; +static constexpr int GEODESIC_DOF = 4; + +using FrenetFrame = Transform; +using GeodesicPointVariation = Mat34; +using GeodesicFrameVariation = Mat34; + +// Variation as SpatialVec: +// +// {dR, dx} = {W*R, v} +// +// dx = v +// dt = w % t +// dn = w % n +// db = w % b +// dR = W * R +using GeodesicVariation = std::array; +using GeodesicCorrection = Vec4; + +/* bool analyticFormAvailable() const; */ + +/* void calcGeodesicWithVariationAnalytically( */ +/* Vec3 xGuess, */ +/* Vec3 tGuess, */ +/* Real l, */ +/* FrenetFrame& K_P, */ +/* GeodesicVariation& dK_P, */ +/* FrenetFrame& K_Q, */ +/* GeodesicVariation& dK_Q) const; */ + +/* void resampleGeodesicPointsAnalytically( */ +/* const FrenetFrame& K_P, */ +/* const FrenetFrame& K_Q, */ +/* Real l, */ +/* size_t size, */ +/* std::vector& points) const; */ + +/* void resampleGeodesicFramesAnalytically( */ +/* const FrenetFrame& K_P, */ +/* const FrenetFrame& K_Q, */ +/* Real l, */ +/* size_t size, */ +/* std::vector& frames) const; */ + +/* size_t calcNearestFrenetFrameImplicitlyFast( */ +/* Vec3 xGuess, */ +/* Vec3 tGuess, */ +/* FrenetFrame& K_P, */ +/* size_t maxIter, */ +/* Real eps) const; */ + +/* void calcGeodesicStartFrameVariationImplicitly( */ +/* const FrenetFrame& K_P, */ +/* GeodesicVariation& dK_P) const; */ + +/* void calcGeodesicEndFrameVariationImplicitly( */ +/* const FrenetFrame& KP, */ +/* Real l, */ +/* FrenetFrame& K_Q, */ +/* GeodesicVariation& dK_Q, */ +/* Real initStepSize, */ +/* Real accuracy, */ +/* std::vector& frames) const; */ + +/* bool calcNearestPointOnLineImplicitly( */ +/* Vec3 a, */ +/* Vec3 b, */ +/* Vec3& point, */ +/* size_t maxIter, */ +/* double eps) const; */ + +/* bool calcNearestPointOnLineAnalytically( */ +/* Vec3 a, */ +/* Vec3 b, */ +/* Vec3& point) const; */ + +Real calcNormalCurvature(Vec3 x, UnitVec3 t) const; +Real calcGeodesicTorsion(Vec3 x, UnitVec3 t) const; + // TODO class Cone; diff --git a/SimTKmath/Geometry/src/ContactGeometry.cpp b/SimTKmath/Geometry/src/ContactGeometry.cpp index 22f13b40a..86f215912 100644 --- a/SimTKmath/Geometry/src/ContactGeometry.cpp +++ b/SimTKmath/Geometry/src/ContactGeometry.cpp @@ -181,6 +181,190 @@ void ContactGeometry::calcSurfacePrincipalCurvatures(const Vec3& point, Vec3 ContactGeometry::calcSupportPoint(UnitVec3 direction) const { return getImpl().calcSupportPoint(direction); } +using FrenetFrame = ContactGeometry::FrenetFrame; +using GeodesicVariation = ContactGeometry::GeodesicVariation; +using GeodesicCorrection = ContactGeometry::GeodesicCorrection; + +/* void ContactGeometry::calcGeodesicWithVariationAnalytically( */ +/* Vec3 xGuess, */ +/* Vec3 tGuess, */ +/* Real l, */ +/* FrenetFrame& K_P, */ +/* GeodesicVariation& dK_P, */ +/* FrenetFrame& K_Q, */ +/* GeodesicVariation& dK_Q) const */ +/* { */ +/* getImpl().calcGeodesicWithVariationAnalytically(xGuess, tGuess, l, K_P, dK_P, K_Q, dK_Q); */ +/* } */ + +/* void ContactGeometry::resampleGeodesicPointsAnalytically( */ +/* const FrenetFrame& K_P, */ +/* const FrenetFrame& K_Q, */ +/* Real l, */ +/* size_t size, */ +/* std::vector& points) const */ +/* { */ +/* getImpl().resampleGeodesicPointsAnalytically(K_P, K_Q, l, size, points); */ +/* } */ + +/* void ContactGeometry::resampleGeodesicFramesAnalytically( */ +/* const FrenetFrame& K_P, */ +/* const FrenetFrame& K_Q, */ +/* Real l, */ +/* size_t size, */ +/* std::vector& frames) const */ +/* { */ +/* getImpl().resampleGeodesicFramesAnalytically(K_P, K_Q, l, size, frames); */ +/* } */ + +/* size_t ContactGeometry::calcNearestFrenetFrameImplicitlyFast( */ +/* Vec3 xGuess, */ +/* Vec3 tGuess, */ +/* FrenetFrame& K_P, */ +/* size_t maxIter, */ +/* Real eps) const */ +/* { */ +/* return getImpl().calcNearestFrenetFrameImplicitlyFast(xGuess, tGuess, K_P, maxIter, eps); */ +/* } */ + +/* void ContactGeometry::calcGeodesicStartFrameVariationImplicitly( */ +/* const FrenetFrame& K_P, */ +/* GeodesicVariation& dK_P) const */ +/* { */ +/* getImpl().calcGeodesicStartFrameVariationImplicitly(K_P, dK_P); */ +/* } */ + +/* void ContactGeometry::calcGeodesicEndFrameVariationImplicitly( */ +/* const FrenetFrame& K_P, */ +/* Real l, */ +/* FrenetFrame& K_Q, */ +/* GeodesicVariation& dK_Q, */ +/* Real initStepSize, */ +/* Real accuracy, */ +/* std::vector& frames) const */ +/* { */ +/* getImpl().calcGeodesicEndFrameVariationImplicitly(K_P, l, K_Q, dK_Q, initStepSize, accuracy, frames); */ +/* } */ + +/* bool ContactGeometry::calcNearestPointOnLineImplicitly( */ +/* Vec3 a, */ +/* Vec3 b, */ +/* Vec3& point, */ +/* size_t maxIter, */ +/* double eps) const */ +/* { */ +/* return getImpl().calcNearestPointOnLineImplicitly(a, b, point, maxIter, eps); */ +/* } */ + +/* bool ContactGeometry::calcNearestPointOnLineAnalytically( */ +/* Vec3 a, */ +/* Vec3 b, */ +/* Vec3& point) const */ +/* { */ +/* return getImpl().calcNearestPointOnLineAnalytically(a, b, point); */ +/* } */ + +//============================================================================== +// IMPLICIT METHODS +//============================================================================== + +// TODO is this right? +/* size_t ContactGeometryImpl::calcNearestFrenetFrameImplicitlyFast( */ +/* Vec3 xGuess, */ +/* Vec3 tGuess, */ +/* FrenetFrame& K_P, */ +/* size_t maxIter, */ +/* Real eps) const */ +/* { */ +/* SimTK_THROW2(Exception::UnimplementedVirtualMethod, */ +/* "ContactGeometryImpl", "calcNearestFrenetFrameImplicitlyFast"); */ +/* /1* Vec3& x = xGuess; *1/ */ +/* /1* size_t it = 0; *1/ */ +/* /1* for (; it < maxIter; ++it) { *1/ */ +/* /1* const double c = calcSurfaceValue(x); *1/ */ + +/* /1* const double error = std::abs(c); *1/ */ + +/* /1* if (error < eps) { *1/ */ +/* /1* break; *1/ */ +/* /1* } *1/ */ + +/* /1* const Vec3 g = calcSurfaceGradient(x); *1/ */ + +/* /1* x += -g * c / dot(g,g); *1/ */ +/* /1* } *1/ */ + +/* /1* UnitVec3 n (calcSurfaceGradient(x)); *1/ */ +/* /1* Vec3& t = tGuess; *1/ */ +/* /1* if (!(abs(t % n) > 1e-13)) { *1/ */ +/* /1* // TODO split NaN detection. *1/ */ +/* /1* throw std::runtime_error("Surface projection failed: Tangent guess is parallel to surface normal, or there are NaNs..."); *1/ */ +/* /1* } *1/ */ + +/* /1* t = t - dot(n, t) * n; *1/ */ + +/* /1* K_P.updR().setRotationFromTwoAxes(n, NormalAxis, t, TangentAxis); *1/ */ +/* /1* K_P.setP(x); *1/ */ + +/* /1* return it; *1/ */ +/* } */ + +/* void ContactGeometryImpl::calcGeodesicStartFrameVariationImplicitly( */ +/* const FrenetFrame& K_P, */ +/* ContactGeometry::GeodesicVariation& dK_P) const */ +/* { */ +/* SimTK_THROW2(Exception::UnimplementedVirtualMethod, */ +/* "ContactGeometryImpl", "calcGeodesicStartFrameVariationImplicitly"); */ +/* /1* const UnitVec3& t = K_P.R().getAxisUnitVec(TangentAxis); *1/ */ +/* /1* const UnitVec3& n = K_P.R().getAxisUnitVec(NormalAxis); *1/ */ +/* /1* const UnitVec3& b = K_P.R().getAxisUnitVec(BinormalAxis); *1/ */ + +/* /1* const Vec3& x = K_P.p(); *1/ */ + +/* /1* dK_P.at(0)[1] = t; *1/ */ +/* /1* dK_P.at(1)[1] = b; // * q.a; // a = 1 *1/ */ +/* /1* dK_P.at(2)[1] = Vec3{0.}; // b * q.r; // r = 0 *1/ */ +/* /1* dK_P.at(3)[1] = Vec3{0.}; // isEnd ? t : Vec3{0., 0., 0.}; *1/ */ + +/* /1* const double tau_g = calcGeodesicTorsion(x, t); *1/ */ +/* /1* const double kappa_n = calcNormalCurvature(x, t); *1/ */ +/* /1* const double kappa_a = calcNormalCurvature(x, b); *1/ */ + +/* /1* dK_P.at(0)[0] = tau_g * t + kappa_n * b; *1/ */ +/* /1* dK_P.at(1)[0] = -kappa_a * t - tau_g * b; *1/ */ +/* /1* dK_P.at(2)[0] = -n; *1/ */ +/* /1* dK_P.at(3)[0] = Vec3{0.}; *1/ */ +/* } */ + +/* void ContactGeometryImpl::calcGeodesicEndFrameVariationImplicitly( */ +/* const FrenetFrame& KP, */ +/* Real l, */ +/* FrenetFrame& K_Q, */ +/* GeodesicVariation& dK_Q, */ +/* Real initStepSize, */ +/* Real accuracy, */ +/* std::vector& frames) const */ +/* { SimTK_THROW2(Exception::UnimplementedVirtualMethod, */ +/* "ContactGeometryImpl", "calcGeodesicEndFrameVariationImplicitly"); } */ + +/* bool ContactGeometryImpl::calcNearestPointOnLineImplicitly( */ +/* Vec3 a, */ +/* Vec3 b, */ +/* Vec3& point, */ +/* size_t maxIter, */ +/* double eps) const */ +/* { SimTK_THROW2(Exception::UnimplementedVirtualMethod, */ +/* "ContactGeometryImpl", "calcNearestPointOnLineImplicitly"); } */ + +Real ContactGeometry::calcNormalCurvature(Vec3 x, UnitVec3 t) const +{ + return getImpl().calcNormalCurvature(x, t); +} + +Real ContactGeometry::calcGeodesicTorsion(Vec3 x, UnitVec3 t) const +{ + return getImpl().calcGeodesicTorsion(x, t); +} //------------------------------------------------------------------------------ // EVAL PARAMETRIC CURVATURE diff --git a/SimTKmath/Geometry/src/ContactGeometryImpl.h b/SimTKmath/Geometry/src/ContactGeometryImpl.h index 23bbf3576..5142b63a9 100644 --- a/SimTKmath/Geometry/src/ContactGeometryImpl.h +++ b/SimTKmath/Geometry/src/ContactGeometryImpl.h @@ -89,6 +89,82 @@ class SimTK_SIMMATH_EXPORT ContactGeometryImpl { virtual bool isConvex() const = 0; virtual bool isFinite() const = 0; + using FrenetFrame = ContactGeometry::FrenetFrame; + using GeodesicVariation = ContactGeometry::GeodesicVariation; + using GeodesicCorrection = ContactGeometry::GeodesicCorrection; + + /* virtual bool analyticFormAvailable() const {return false;} */ + + /* virtual void calcGeodesicWithVariationAnalytically( */ + /* Vec3 xGuess, */ + /* Vec3 tGuess, */ + /* Real l, */ + /* FrenetFrame& K_P, */ + /* GeodesicVariation& dK_P, */ + /* FrenetFrame& K_Q, */ + /* GeodesicVariation& dK_Q) const */ + /* { SimTK_THROW2(Exception::UnimplementedVirtualMethod, */ + /* "ContactGeometryImpl", "calcGeodesicWithVariationAnalytically"); } */ + + /* virtual void resampleGeodesicPointsAnalytically( */ + /* const FrenetFrame& K_P, */ + /* const FrenetFrame& K_Q, */ + /* Real l, */ + /* size_t size, */ + /* std::vector& points) const */ + /* { SimTK_THROW2(Exception::UnimplementedVirtualMethod, */ + /* "ContactGeometryImpl", "resampleGeodesicPointsAnalytically"); } */ + + /* virtual void resampleGeodesicFramesAnalytically( */ + /* const FrenetFrame& K_P, */ + /* const FrenetFrame& K_Q, */ + /* Real l, */ + /* size_t size, */ + /* std::vector& frames) const */ + /* { SimTK_THROW2(Exception::UnimplementedVirtualMethod, */ + /* "ContactGeometryImpl", "resampleGeodesicFramesAnalytically"); } */ + + /* virtual size_t calcNearestFrenetFrameImplicitlyFast( */ + /* Vec3 xGuess, */ + /* Vec3 tGuess, */ + /* FrenetFrame& K_P, */ + /* size_t maxIter, */ + /* Real eps) const; */ + + /* virtual void calcGeodesicStartFrameVariationImplicitly( */ + /* const FrenetFrame& K_P, */ + /* GeodesicVariation& dK_P) const; */ + + /* virtual void calcGeodesicEndFrameVariationImplicitly( */ + /* const FrenetFrame& KP, */ + /* Real l, */ + /* FrenetFrame& K_Q, */ + /* GeodesicVariation& dK_Q, */ + /* Real initStepSize, */ + /* Real accuracy, */ + /* std::vector& frames) const; */ + + /* virtual bool calcNearestPointOnLineImplicitly( */ + /* Vec3 a, */ + /* Vec3 b, */ + /* Vec3& point, */ + /* size_t maxIter, */ + /* double eps) const; */ + + /* virtual bool calcNearestPointOnLineAnalytically( */ + /* Vec3 a, */ + /* Vec3 b, */ + /* Vec3& point) const */ + /* { SimTK_THROW2(Exception::UnimplementedVirtualMethod, */ + /* "ContactGeometryImpl", "calcNearestPointOnLineAnalytically"); } */ + + Real calcNormalCurvature(Vec3 x, UnitVec3 t) const + { SimTK_THROW2(Exception::UnimplementedVirtualMethod, + "ContactGeometryImpl", "calcNormalCurvature"); } + Real calcGeodesicTorsion(Vec3 x, UnitVec3 t) const + { SimTK_THROW2(Exception::UnimplementedVirtualMethod, + "ContactGeometryImpl", "calcGeodesicTorsion"); } + // Smooth surfaces only. virtual void calcCurvature(const Vec3& point, Vec2& curvature, Rotation& orientation) const diff --git a/SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp b/SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp index 8bddb48b6..c00166419 100644 --- a/SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp +++ b/SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp @@ -47,6 +47,8 @@ using std::cout; using std::endl; // CONTACT GEOMETRY :: SPHERE & IMPL //============================================================================== +/* bool ContactGeometryImpl::Sphere::analyticFormAvailable() const {return true;} */ + ContactGeometry::Sphere::Sphere(Real radius) : ContactGeometry(new Sphere::Impl(radius)) {} diff --git a/Simbody/include/SimTKsimbody.h b/Simbody/include/SimTKsimbody.h index fd1dfc159..c59947a07 100644 --- a/Simbody/include/SimTKsimbody.h +++ b/Simbody/include/SimTKsimbody.h @@ -77,6 +77,7 @@ will include this one). **/ #include "simbody/internal/CompliantContactSubsystem.h" #include "simbody/internal/CableTrackerSubsystem.h" #include "simbody/internal/CablePath.h" +#include "simbody/internal/Wrapping.h" #include "simbody/internal/CableSpring.h" #include "simbody/internal/Visualizer.h" #include "simbody/internal/Visualizer_InputListener.h" diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h new file mode 100644 index 000000000..262fdd148 --- /dev/null +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -0,0 +1,314 @@ +#ifndef SimTK_SIMBODY_WRAPPING_PATH_SUBSYSTEM_H_ +#define SimTK_SIMBODY_WRAPPING_PATH_SUBSYSTEM_H_ + +#include "SimTKcommon/internal/State.h" +#include "SimTKmath.h" +#include "simbody/internal/MobilizedBody.h" +#include "simbody/internal/MultibodySystem.h" +#include "simbody/internal/SimbodyMatterSubsystem.h" +#include "simbody/internal/common.h" +#include "simmath/internal/ContactGeometry.h" +#include +#include + +namespace SimTK +{ + +SimTK_DEFINE_UNIQUE_INDEX_TYPE(CurveSegmentIndex); +SimTK_DEFINE_UNIQUE_INDEX_TYPE(CableSpanIndex); + +class MultibodySystem; +class CableSubsystem; +class CurveSegment; +class CableSpan; + +//============================================================================== +// A curved cable segment on a surface that is part of a `CableSpan`. +class SimTK_SIMBODY_EXPORT CurveSegment final +{ +private: + CurveSegment() = default; + +public: + CurveSegment(const CurveSegment&) = default; + CurveSegment& operator=(const CurveSegment&) = default; + CurveSegment(CurveSegment&&) noexcept = default; + CurveSegment& operator=(CurveSegment&&) noexcept = default; + ~CurveSegment() = default; + + /** Construct a CurveSegment representing a segment of a CableSpan that + wraps over a ContactGeometry. + @param cable The cable this segment belongs to. + @param mobod The body that the contact geometry is rigidly attached to. + @param X_BS Transform specifying the location and orientation of the + contact geometry's origin frame with respect to the mobilized body. + @param geometry The contact geometry over which this segment wraps. + @param initialContactPointHint A guess of the contact point of the cable + span and the contact geometry to compute the initial cable path. This point + is defined in the local contact geometry's frame. The point will be used as + a starting point when computing the initial cable path. As such, it does + not have to lie on the contact geometry's surface, nor does it have to + belong to a valid cable path.*/ + CurveSegment( + CableSpan cable, + MobilizedBodyIndex body, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 initialContactPointHint); + + /** A helper class, representing the wrapping status of this segment in + relation to the contact geometry.*/ + enum class WrappingStatus + { + InContactWithSurface, + LiftedFromSurface, + Disabled, + }; + + /** Get the CableSpan that this segment is a part of. */ + const CableSpan& getCable() const; + + /** Get the ContactGeometry that this segment wraps over. */ + const ContactGeometry& getContactGeometry() const; + + /** Get the MobilizedBody that the contact geometry is rigidly attached to. */ + const Mobod& getMobilizedBody() const; + + /** Get the transform representing the orientation and postion of the + contact geometry's origin with respect to the body fixed frame. */ + const Transform& getXformSurfaceToBody() const; + + /** Set the transform representing the orientation and postion of the + contact geometry's origin with respect to the body fixed frame. */ + void setXformSurfaceToBody(Transform X_BS); + + /** Get the length of this segment. + The system must be realiezd to Stage::Position. */ + Real getSegmentLength(const State& state) const; + + /** Get the frenet frame at the start (first contact point) of this curve segment. + TODO describe the frame axes. + The system must be realiezd to Stage::Position. */ + const Transform& getFrenetFrameStart(const State& state) const; + + /** Get the frenet frame at the end (last contact point) of this curve segment. + TODO describe the frame axes. + The system must be realiezd to Stage::Position. */ + const Transform& getFrenetFrameEnd(const State& state) const; + + /** Get the wrapping status of this segment. + The system must be realiezd to Stage::Position. */ + WrappingStatus getStatus(const State& state) const; + + /** Get the number of steps taken by the GeodesicIntegrator to compute this segment during the last realization. + The system must be realiezd to Stage::Position. */ + int getNumberOfIntegratorStepsTaken(const State& state); + + /** Get the initial step size that the GeodesicIntegrator will use for the next path computation. + The system must be realiezd to Stage::Position. */ + Real getInitialIntegratorStepSize(const State& state); + + // TODO useful? + /* void setDisabled(const State& state) const; */ + /* void setEnabled(const State& state) const; */ + + /** Compute the unit force vector in Ground frame that this segment exerts + on the MobilizedBody. The actual applied force can be found by multiplication with the cable tension. + The system must be realiezd to Stage::Position. */ + void calcUnitForce(const State& state, SpatialVec& unitForce_G) const; + + /** Compute points along this segment in Ground frame. + The system must be realiezd to Stage::Position. + + @param state State of the system. + @param points_G The output buffer to which the points are written. + @param Controls the number of points that are computed. These points are + resampled from the computed curve at equal length intervals. Optionally, + use `nPoints=0` to disable interpolation, and the original points from the + GeodesicIntegrator will be used. If the curve length is zero, this + parameter is ignored, and a single point is written. If the curve is not + active (Status::Disabled or Status::Lifted), no points are written. Note + that if nPoints=1, and the curve length is not zero, an exception is + thrown. + @return The number of points written. */ + int calcPoints(const State& state, std::function& sink, int nPoints) const; + + /** Comute points along the curve in ground frame. + The system must be realiezd to Stage::Position. + TODO describe behavior + */ + int calcPoints(const State& state, std::function& sink) const; + + bool isActive(const State& state) const + { + return getStatus(state) == WrappingStatus::InContactWithSurface; + } + +//------------------------------------------------------------------------------ + // TODO public? private? + class Impl; + +private: + friend CableSpan; + const Impl& getImpl() const + { + return *m_Impl; + } + Impl& updImpl() + { + return *m_Impl; + } + + std::shared_ptr m_Impl = nullptr; +}; + +//============================================================================== +// CABLE SPAN +//============================================================================== +// Representation of a cable spanning from one body to another. +// The cable can adopt obstacles that must be wrapped over. +// +// NOTE: The interaction with the obstacles is **ordered**. The cable can wrap +// over the obstacles in the order that they are stored. This means that the +// cable can not wrap over the first obstacle twice, even though it might +// spatially intersect it twice. This greatly simplifies the implementation, +// while covering many use-cases. +class SimTK_SIMBODY_EXPORT CableSpan final +{ +public: + +//------------------------------------------------------------------------------ + + // Helper struct representing the cable segments that do not lie on a surface. + struct LineSegment final + { + LineSegment() = default; + + LineSegment(Vec3 a, Vec3 b) : l((b - a).norm()), d((b - a) / l) + {} + + Real l = NaN; + UnitVec3 d{NaN, NaN, NaN}; + }; + +//------------------------------------------------------------------------------ + CableSpan() = default; + ~CableSpan() = default; + CableSpan(const CableSpan&) = default; + CableSpan& operator=(const CableSpan&) = default; + CableSpan(CableSpan&&) noexcept = default; + CableSpan& operator=(CableSpan&&) noexcept = default; + +//------------------------------------------------------------------------------ +// Parameter interface +//------------------------------------------------------------------------------ + CableSpan( + CableSubsystem& subsystem, + MobilizedBodyIndex originBody, + const Vec3& defaultOriginPoint, + MobilizedBodyIndex terminationBody, + const Vec3& defaultTerminationPoint); + + void adoptWrappingObstacle( + MobilizedBodyIndex mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 contactPointHint = {1., 0., 0.}); + + int getNumCurveSegments() const; + + const CurveSegment& getCurveSegment(CurveSegmentIndex ix) const; + +//------------------------------------------------------------------------------ +// State dependent interface +//------------------------------------------------------------------------------ + Real getLength(const State& state) const; + + Real getLengthDot(const State& state) const; + + size_t countActive(const State& state) const; + +//------------------------------------------------------------------------------ +// State dependent calculations +//------------------------------------------------------------------------------ + + /** Compute the unit force vector in Ground frame that this cable exerts on + the cable's origin body. The actual applied force can be found by + multiplication with the cable tension. + The system must be realiezd to Stage::Position. */ + void calcUnitForceAtOrigin(const State& state, SpatialVec& unitForce_G) const; + + /** Compute the unit force vector in Ground frame that this cable exerts on + the cable's termination body. The actual applied force can be found by + multiplication with the cable tension. + The system must be realiezd to Stage::Position. */ + void calcUnitForceAtTermination(const State& state, SpatialVec& unitForce_G) const; + + void applyBodyForces( + const State& state, + Real tension, + Vector_& bodyForcesInG) const; + + Real calcCablePower(const State& state, Real tension) const; + + // Calls `CurveSegment::calcPoints` on each active curve segment. + int calcPoints(const State& state, std::function& sink, int nPointsPerCurveSegment = 0) const; + +//------------------------------------------------------------------------------ +// TODO TEMPORARY FOR UNIT TESTING +//------------------------------------------------------------------------------ + // TODO this is useful for unit testing, but we really really dont want to expose it... + void calcPathErrorJacobian( + const State& state, + Vector& pathError, + Matrix& pathErrorJacobian) const; + + // TODO this is useful for unit testing, but we really really dont want to expose it... + void applyCorrection(const State& state, const Vector& correction) const; + +//------------------------------------------------------------------------------ + class Impl; // TODO private? public? + +private: + const Impl& getImpl() const + { + return *m_Impl; + } + + Impl& updImpl() + { + return *m_Impl; + } + + std::shared_ptr m_Impl = nullptr; + + friend CurveSegment; + friend CurveSegment::Impl; + friend CableSubsystem; +}; + +//============================================================================== +// SUBSYSTEM +//============================================================================== + +// TODO Alternative names: CableTrackerSubSystem, WrappingPathSubsystem. +class SimTK_SIMBODY_EXPORT CableSubsystem : public Subsystem +{ +public: + CableSubsystem(); + explicit CableSubsystem(MultibodySystem&); + + int getNumPaths() const; + const CableSpan& getPath(CableSpanIndex idx) const; + CableSpan& updPath(CableSpanIndex idx); + + /* private: */ + SimTK_PIMPL_DOWNCAST(CableSubsystem, Subsystem); + class Impl; + Impl& updImpl(); + const Impl& getImpl() const; +}; + +} // namespace SimTK + +#endif diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp new file mode 100644 index 000000000..d0ccda156 --- /dev/null +++ b/Simbody/src/Wrapping.cpp @@ -0,0 +1,1953 @@ +#include "SimTKcommon/internal/CoordinateAxis.h" +#include "SimTKcommon/internal/ExceptionMacros.h" +#include "SimTKmath.h" +#include "WrappingImpl.h" +#include "simmath/internal/ContactGeometry.h" +#include +#include +#include + +using namespace SimTK; + +using Correction = ContactGeometry::GeodesicCorrection; +using FrameVariation = ContactGeometry::GeodesicFrameVariation; +using FrenetFrame = ContactGeometry::FrenetFrame; +using GeodesicInfo = CurveSegment::Impl::PosInfo; +using GeodesicJacobian = Vec4; +using LineSegment = CableSpan::LineSegment; +using LocalGeodesicInfo = CurveSegment::Impl::InstanceEntry; +using LocalGeodesicSample = CurveSegment::Impl::LocalGeodesicSample; +using PointVariation = ContactGeometry::GeodesicPointVariation; +using SolverData = CableSubsystem::Impl::SolverData; +using Status = CurveSegment::WrappingStatus; +using Variation = ContactGeometry::GeodesicVariation; + +//============================================================================== +// CURVE SEGMENT +//============================================================================== + +CurveSegment::CurveSegment( + CableSpan cable, + MobilizedBodyIndex body, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 xHint) : + m_Impl(std::shared_ptr( + new CurveSegment::Impl(cable, body, X_BS, geometry, xHint))) +{ + // TODO bit awkward to set the index later. + updImpl().setIndex(cable.updImpl().adoptSegment(*this)); +} + +const CableSpan& CurveSegment::getCable() const +{ + return getImpl().getCable(); +} + +Real CurveSegment::getSegmentLength(const State& s) const +{ + getImpl().realizeCablePosition(s); + return getImpl().getInstanceEntry(s).length; +} + +const ContactGeometry& CurveSegment::getContactGeometry() const +{ + return getImpl().getContactGeometry(); +} + +const Mobod& CurveSegment::getMobilizedBody() const +{ + return getImpl().getMobilizedBody(); +} + +void CurveSegment::setXformSurfaceToBody(Transform X_BS) +{ + updImpl().setXformSurfaceToBody(std::move(X_BS)); +} + +const Transform& CurveSegment::getXformSurfaceToBody() const +{ + return getImpl().getXformSurfaceToBody(); +} + +CurveSegment::WrappingStatus CurveSegment::getStatus(const State& s) const +{ + getImpl().realizeCablePosition(s); + return getImpl().getInstanceEntry(s).status; +} + +const Transform& CurveSegment::getFrenetFrameStart(const State& state) const +{ + getImpl().realizeCablePosition(state); + return getImpl().getPosInfo(state).KP; +} + +const Transform& CurveSegment::getFrenetFrameEnd(const State& state) const +{ + getImpl().realizeCablePosition(state); + return getImpl().getPosInfo(state).KQ; +} + +int CurveSegment::getNumberOfIntegratorStepsTaken(const State& state) +{ + getImpl().realizeCablePosition(state); + return getImpl().getInstanceEntry(state).samples.size(); +} + +Real CurveSegment::getInitialIntegratorStepSize(const State& state) +{ + getImpl().realizeCablePosition(state); + return getImpl().getInstanceEntry(state).sHint; +} + +void CurveSegment::calcUnitForce(const State& state, SpatialVec& unitForce_G) + const +{ + getImpl().realizeCablePosition(state); + getImpl().calcUnitForce(state, unitForce_G); +} + +int CurveSegment::calcPoints(const State& state, std::function& sink, int nPoints) const +{ + getImpl().realizeCablePosition(state); + return getImpl().calcPathPoints(state, sink, nPoints); +} + +//============================================================================== +// CABLE SPAN +//============================================================================== + +CableSpan::CableSpan( + CableSubsystem& subsystem, + MobilizedBodyIndex originBody, + const Vec3& defaultOriginPoint, + MobilizedBodyIndex terminationBody, + const Vec3& defaultTerminationPoint) : + m_Impl(std::shared_ptr(new Impl( + subsystem, + originBody, + defaultOriginPoint, + terminationBody, + defaultTerminationPoint))) +{ + subsystem.updImpl().adoptCablePath(*this); +} + +void CableSpan::adoptWrappingObstacle( + MobilizedBodyIndex body, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 contactPointHint) +{ + CurveSegment(*this, body, X_BS, geometry, contactPointHint); +} + +int CableSpan::getNumCurveSegments() const +{ + return getImpl().getNumCurveSegments(); +} + +const CurveSegment& CableSpan::getCurveSegment(CurveSegmentIndex ix) const +{ + return getImpl().getCurveSegment(ix); +} + +Real CableSpan::getLength(const State& s) const +{ + return getImpl().getPosInfo(s).l; +} + +Real CableSpan::getLengthDot(const State& s) const +{ + return getImpl().getVelInfo(s).lengthDot; +} + +void CableSpan::applyBodyForces( + const State& s, + Real tension, + Vector_& bodyForcesInG) const +{ + return getImpl().applyBodyForces(s, tension, bodyForcesInG); +} + +int CableSpan::calcPoints(const State& state, std::function& sink, + int nPointsPerCurveSegment) const +{ + return getImpl().calcPathPoints(state, sink, nPointsPerCurveSegment); +} + +void CableSpan::calcUnitForceAtOrigin( + const State& state, + SpatialVec& unitForce_G) const +{ + getImpl().calcUnitForceAtOrigin(state, unitForce_G); +} + +void CableSpan::calcUnitForceAtTermination( + const State& state, + SpatialVec& unitForce_G) const +{ + getImpl().calcUnitForceAtTermination(state, unitForce_G); +} + +Real CableSpan::calcCablePower(const State& state, Real tension) const +{ + return getImpl().calcCablePower(state, tension); +} + +//============================================================================== +// CONSTANTS +//============================================================================== +namespace +{ +static const int GeodesicDOF = 4; +} // namespace + +//============================================================================== +// GEODESIC MATH TODO move to contact geometry +//============================================================================== + +namespace +{ + +// TODO Sign of implicit function was flipped. +Real calcSurfaceConstraintValue(const ContactGeometry& geometry, Vec3 point) +{ + return -geometry.calcSurfaceValue(point); +} + +// TODO Sign of implicit function was flipped. +Vec3 calcSurfaceConstraintGradient(const ContactGeometry& geometry, Vec3 point) +{ + return -geometry.calcSurfaceGradient(point); +} + +// TODO Sign of implicit function was flipped. +Mat33 calcSurfaceConstraintHessian(const ContactGeometry& geometry, Vec3 point) +{ + return -geometry.calcSurfaceHessian(point); +} + +struct ImplicitGeodesicState +{ + ImplicitGeodesicState() = default; + + explicit ImplicitGeodesicState( + Vec<10, Real>&& implicitGeodesicStateAsVector) + { + asVecMut() = implicitGeodesicStateAsVector; + } + + ImplicitGeodesicState(Vec3 point, Vec3 tangent) : x(point), t(tangent){}; + + const Vec<10, Real>& asVec() const + { + return reinterpret_cast&>(x[0]); + } + + Vec<10, Real>& asVecMut() + { + return reinterpret_cast&>(x[0]); + } + + Vec<10, Real> calcDerivativeVector( + const Vec3& acceleration, + Real gaussianCurvature) const + { + ImplicitGeodesicState dy; + dy.x = t; + dy.t = acceleration; + dy.a = aDot; + dy.aDot = -a * gaussianCurvature; + dy.r = rDot; + dy.rDot = -r * gaussianCurvature; + return {dy.asVec()}; + ; + } + + Vec3 x = {NaN, NaN, NaN}; + Vec3 t = {NaN, NaN, NaN}; + Real a = 1.; + Real aDot = 0.; + Real r = 0.; + Real rDot = 1.; +}; + +void calcSurfaceProjectionFast( + const ContactGeometry& geometry, + Vec3& x, + Vec3& t, + size_t maxIter, + Real eps) +{ + size_t it = 0; + for (; it < maxIter; ++it) { + const Real c = calcSurfaceConstraintValue(geometry, x); + + if (std::abs(c) < eps) { + break; + } + + const Vec3 g = calcSurfaceConstraintGradient(geometry, x); + x += -g * c / dot(g, g); + } + + if (it >= maxIter) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Surface projection failed: Reached max iterations"); + } + + if (std::abs(geometry.calcSurfaceValue(x)) > eps) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Surface projection failed: no longer on surface"); + } + + UnitVec3 n(calcSurfaceConstraintGradient(geometry, x)); + while (true) { + t = t - dot(n, t) * n; + Real norm = t.norm(); + if (isNaN(norm)) + // TODO use SimTK_ASSERT + throw std::runtime_error("Surface projection failed: Detected NaN"); + if (norm < 1e-13) { + std::cout << "WARNING: Tangent guess is parallel to surface " + "normal: perturbing\n"; + + Random::Gaussian random; + t += Vec3{ + random.getValue(), + random.getValue(), + random.getValue(), + }; + std::cout << "t = " << t << "\n"; + } else { + t = t / norm; + break; + } + } +} + +bool calcNearestPointOnLineImplicitly( + const ContactGeometry& geometry, + Vec3 a, + Vec3 b, + Vec3& point, + size_t maxIter, + Real eps) +{ + // Initial guess. + const Vec3 d = b - a; + Real alpha = -dot(d, a - point) / dot(d, d); + alpha = std::max(0., std::min(alpha, 1.)); + + size_t iter = 0; + + for (; iter < maxIter; ++iter) { + // Touchdown point on line. + const Vec3 pl = a + d * alpha; + + // Constraint evaluation at touchdown point. + const Real c = calcSurfaceConstraintValue(geometry, pl); + + // Break on touchdown, TODO or not? + if (std::abs(c) < eps) + break; + + // Gradient at point on line. + const Vec3 g = calcSurfaceConstraintGradient(geometry, pl); + const Mat33 H = calcSurfaceConstraintHessian(geometry, pl); + + // Add a weight to the newton step to avoid large steps. + constexpr Real w = 0.5; + + // Update alpha. + const Real step = dot(g, d) / (dot(d, H * d) + w); + + // Stop when converged. + if (std::abs(step) < eps) + break; + + // Clamp the stepsize. + constexpr Real maxStep = 0.25; + alpha -= std::min(std::max(-maxStep, step), maxStep); + + // Stop when leaving bounds. + if (alpha < 0. || alpha > 1.) + break; + } + + // Write the point on line nearest the surface. + alpha = std::max(0., std::min(alpha, 1.)); + point = a + d * alpha; + + // Assumes a negative constraint evaluation means touchdown. + const bool contact = calcSurfaceConstraintValue(geometry, point) < eps; + + // TODO handle here? + if (iter >= maxIter) { + // TODO use SimTK_ASSERT + throw std::runtime_error("Failed to compute point on line nearest " + "surface: Reached max iterations"); + } + + // Return number of iterations required. + return contact; +} + +ImplicitGeodesicState operator-( + const ImplicitGeodesicState& lhs, + const ImplicitGeodesicState& rhs) +{ + ImplicitGeodesicState out; + out.asVecMut() = lhs.asVec() - rhs.asVec(); + return out; +} + +ImplicitGeodesicState operator+( + const ImplicitGeodesicState& lhs, + const Vec<10, Real>& rhs) +{ + ImplicitGeodesicState out; + out.asVecMut() = lhs.asVec() + rhs; + return out; +} + +Real calcInfNorm(const ImplicitGeodesicState& q) +{ + Real infNorm = 0.; + const Vec<10, Real>& v = q.asVec(); + for (size_t r = 0; r < v.nrow(); ++r) { + infNorm = std::max(infNorm, std::abs(v[r])); + } + return infNorm; +} + +class RKM +{ + using Y = ImplicitGeodesicState; + using DY = Vec<10, Real>; + +public: + RKM() = default; + + // Integrate y0, populating y1, y2. + Real step(Real h, std::function& f); + + struct Sample + { + Real l; + FrenetFrame K; + }; + + Y stepTo( + Y y0, + Real x1, + Real& h0, + std::function& f, // Dynamics + std::function& g, // Surface projection + std::function& m, // State log + Real accuracy); + + size_t getNumberOfFailedSteps() const + { + return _failedCount; + } + +private: + static constexpr size_t ORDER = 5; + + std::array _k{}; + // [yInit, ySave, yStep] + std::array _y{}; + + Real _hMin = 1e-10; + Real _hMax = 1e-1; + Real _h = NaN; + Real _h0 = NaN; + Real _e = NaN; + + size_t _failedCount = 0; +}; + +Real RKM::step(Real h, std::function& f) +{ + Y& yk = _y.at(1); + + for (size_t i = 0; i < 5; ++i) { + yk = _y.at(0) + (h / 3.) * _k.at(0); + } + + // k1 + _k.at(0) = f(_y.at(0)); + + // k2 + { + yk = _y.at(0) + (h / 3.) * _k.at(0); + _k.at(1) = f(yk); + } + + // k3 + { + yk = _y.at(0) + (h / 6.) * _k.at(0) + (h / 6.) * _k.at(1); + _k.at(2) = f(yk); + } + + // k4 + { + yk = _y.at(0) + (1. / 8. * h) * _k.at(0) + (3. / 8. * h) * _k.at(2); + _k.at(3) = f(yk); + } + + // k5 + { + yk = _y.at(0) + (1. / 2. * h) * _k.at(0) + (-3. / 2. * h) * _k.at(2) + + (2. * h) * _k.at(3); + _k.at(4) = f(yk); + } + + // y1: Auxiliary --> Already updated in k5 computation. + + // y2: Final state. + _y.at(2) = _y.at(0) + (1. / 6. * h) * _k.at(0) + (2. / 3. * h) * _k.at(3) + + (1. / 6. * h) * _k.at(4); + + return calcInfNorm(_y.at(1) - _y.at(2)) * 0.2; +} + +RKM::Y RKM::stepTo( + Y y0, + Real x1, + Real& h0, + std::function& f, + std::function& g, + std::function& m, + Real accuracy) +{ + g(y0); + m(0., y0); + + _y.at(0) = std::move(y0); + _h = std::min(h0 > _hMin ? h0 : _hMin, _hMax); + _e = 0.; + _failedCount = 0; + + Real x = 0.; + while (x < x1 - 1e-13) { + const bool init = x == 0.; + + _h = x + _h > x1 ? x1 - x : _h; + + // Attempt step. + Real err = step(_h, f); + + // Reject if accuracy was not met. + if (err > accuracy) { // Rejected + // Decrease stepsize. + _h /= 2.; + _y.at(1) = _y.at(0); + _y.at(2) = _y.at(0); + ++_failedCount; + } else { // Accepted + g(_y.at(2)); // Enforce constraints. + _y.at(0) = _y.at(2); + _y.at(1) = _y.at(2); + x += _h; + m(x, _y.at(0)); + } + + // Potentially increase stepsize. + if (err < accuracy / 64.) { + _h *= 2.; + } + + _e = std::max(_e, err); + + if (_h < _hMin) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Geodesic Integrator failed: Reached very small stepsize"); + } + + if (init) { + h0 = _h; + } + } + if (std::abs(x - x1) > 1e-13) { + // TODO use SimTK_ASSERT + throw std::runtime_error("failed to integrate"); + } + return _y.at(0); +} + +Real calcNormalCurvature( + const ContactGeometry& geometry, + Vec3 point, + Vec3 tangent) +{ + const Vec3& p = point; + const Vec3& v = tangent; + const Vec3 g = calcSurfaceConstraintGradient(geometry, p); + const Vec3 h_v = calcSurfaceConstraintHessian(geometry, p) * v; + // Sign flipped compared to thesis: kn = negative, see eq 3.63 + return -dot(v, h_v) / g.norm(); +} + +Real calcGeodesicTorsion( + const ContactGeometry& geometry, + Vec3 point, + Vec3 tangent) +{ + // TODO verify this! + const Vec3& p = point; + const Vec3& v = tangent; + const Vec3 g = calcSurfaceConstraintGradient(geometry, p); + const Vec3 h_v = calcSurfaceConstraintHessian(geometry, p) * v; + const Vec3 gxv = cross(g, v); + return -dot(h_v, gxv) / dot(g, g); +} + +UnitVec3 calcSurfaceNormal(const ContactGeometry& geometry, Vec3 point) +{ + const Vec3& p = point; + const Vec3 gradient = calcSurfaceConstraintGradient(geometry, p); + return UnitVec3{gradient}; +} + +Vec3 calcAcceleration(const ContactGeometry& geometry, Vec3 point, Vec3 tangent) +{ + // TODO Writing it out saves a root, but optimizers are smart. + // Sign flipped compared to thesis: kn = negative, see eq 3.63 + return calcNormalCurvature(geometry, point, std::move(tangent)) * + calcSurfaceNormal(geometry, point); +} + +Mat33 calcAdjoint(const Mat33& mat) +{ + Real fxx = mat(0, 0); + Real fyy = mat(1, 1); + Real fzz = mat(2, 2); + + Real fxy = mat(0, 1); + Real fxz = mat(0, 2); + Real fyz = mat(1, 2); + + std::array elements = { + fyy * fzz - fyz * fyz, + fyz * fxz - fxy * fzz, + fxy * fyz - fyy * fxz, + fxz * fyz - fxy * fzz, + fxx * fzz - fxz * fxz, + fxy * fxz - fxx * fyz, + fxy * fyz - fxz * fyy, + fxy * fxz - fxx * fyz, + fxx * fyy - fxy * fxy}; + Mat33 adj; + size_t i = 0; + for (size_t r = 0; r < 3; ++r) { + for (size_t c = 0; c < 3; ++c) { + adj(r, c) = elements[i]; + ++i; + } + } + return adj; +} + +Real calcGaussianCurvature(const ContactGeometry& geometry, Vec3 point) +{ + const Vec3& p = point; + Vec3 g = calcSurfaceConstraintGradient(geometry, p); + Real gDotg = dot(g, g); + Mat33 adj = calcAdjoint(calcSurfaceConstraintHessian(geometry, p)); + + if (gDotg * gDotg < 1e-13) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Gaussian curvature inaccurate: are we normal to surface?"); + } + + return (dot(g, adj * g)) / (gDotg * gDotg); +} + +void calcFrenetFrame( + const ContactGeometry& geometry, + const ImplicitGeodesicState& q, + FrenetFrame& K) +{ + K.setP(q.x); + K.updR().setRotationFromTwoAxes( + calcSurfaceNormal(geometry, q.x), + NormalAxis, + q.t, + TangentAxis); +} + +void calcGeodesicBoundaryState( + const ContactGeometry& geometry, + const ImplicitGeodesicState& q, + bool isEnd, + FrenetFrame& K, + Mat34& v, + Mat34& w) +{ + calcFrenetFrame(geometry, q, K); + + const UnitVec3& t = K.R().getAxisUnitVec(TangentAxis); + const UnitVec3& n = K.R().getAxisUnitVec(NormalAxis); + const UnitVec3& b = K.R().getAxisUnitVec(BinormalAxis); + + // TODO remove fourth element? + v.col(0) = Vec3{t}; + v.col(1) = b * q.a; + v.col(2) = b * q.r; + v.col(3) = isEnd ? v.col(0) : Vec3{0.}; + + const Real tau_g = calcGeodesicTorsion(geometry, q.x, t); + const Real kappa_n = calcNormalCurvature(geometry, q.x, t); + const Real kappa_a = calcNormalCurvature(geometry, q.x, b); + + w.col(0) = tau_g * t + kappa_n * b; + w.col(1) = -q.a * kappa_a * t - q.aDot * n - q.a * tau_g * b; + w.col(2) = -q.r * kappa_a * t - q.rDot * n - q.r * tau_g * b; + w.col(3) = isEnd ? w.col(0) : Vec3{0.}; +} + +void calcGeodesicAndVariationImplicitly( + const ContactGeometry& geometry, + Vec3 x, + Vec3 t, + Real l, + Real& ds, + FrenetFrame& K_P, + Variation& dK_P, + FrenetFrame& K_Q, + Variation& dK_Q, + Real accuracy, + size_t prjMaxIter, + Real prjAccuracy, + std::vector& log) +{ + using Y = ImplicitGeodesicState; + using DY = Vec<10, Real>; + + std::function f = [&](const Y& q) -> DY { + return q.calcDerivativeVector( + calcAcceleration(geometry, q.x, q.t), + calcGaussianCurvature(geometry, q.x)); + }; + std::function g = [&](Y& q) { + calcSurfaceProjectionFast(geometry, q.x, q.t, prjMaxIter, prjAccuracy); + }; + std::function m = [&](Real l, const Y& q) { + FrenetFrame frame; + calcFrenetFrame(geometry, q, frame); + log.push_back(LocalGeodesicSample(l, frame)); + }; + + Y y0(x, t); + g(y0); + + RKM rkm{}; + + Y y1 = rkm.stepTo(y0, l, ds, f, g, m, accuracy); + + SimTK_ASSERT(log.size() > 0, "Failed to integrate geodesic: Log is empty"); + calcGeodesicBoundaryState(geometry, y0, false, K_P, dK_P[1], dK_P[0]); + calcGeodesicBoundaryState(geometry, y1, true, K_Q, dK_Q[1], dK_Q[0]); +} + +} // namespace + +//============================================================================== +// Resampling geodesic +//============================================================================== + +namespace +{ +Vec3 calcHermiteInterpolation( + Real x0, + const Vec3& y0, + const Vec3& y0Dot, + Real x1, + const Vec3& y1, + const Vec3& y1Dot, + Real x) +{ + const Real dx = x1 - x0; + const Vec3 dy = y1 - y0; + const Vec3 dyDot = y1Dot - y0Dot; + + const Vec3& c0 = y0; + const Vec3& c1 = y0Dot; + const Vec3 c3 = + -2. * (dy - y0Dot * dx - 0.5 * dx * dyDot) / std::pow(dx, 3); + const Vec3 c2 = (dyDot / dx - 3. * c3 * dx) / 2.; + + const Real h = x - x0; + return c0 + h * (c1 + h * (c2 + h * c3)); +} + +Vec3 calcHermiteInterpolation( + const LocalGeodesicSample& a, + const LocalGeodesicSample& b, + Real l) +{ + return calcHermiteInterpolation( + a.length, + a.frame.p(), + a.frame.R().getAxisUnitVec(TangentAxis), + b.length, + b.frame.p(), + b.frame.R().getAxisUnitVec(TangentAxis), + l); +} + +size_t calcResampledGeodesicPoints( + const std::vector& geodesic, + const Transform& X_GS, + int nSamples, + std::function& sink) +{ + // Some sanity checks. + if (geodesic.empty()) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Resampling of geodesic failed: Provided geodesic is empty."); + } + if (geodesic.front().length != 0.) { + // TODO use SimTK_ASSERT + throw std::runtime_error("Resampling of geodesic failed: First frame " + "must be at length = zero"); + } + if (geodesic.front().length < 0.) { + // TODO use SimTK_ASSERT + throw std::runtime_error("Resampling of geodesic failed: Last frame " + "must be at length > zero"); + } + if (nSamples == 1 && geodesic.size() != 1) { + // TODO use SimTK_ASSERT + throw std::runtime_error("Resampling of geodesic failed: Requested " + "number of samples must be unequal to 1"); + } + + // Capture the start of the geodesic. + sink( + X_GS.shiftFrameStationToBase(geodesic.front().frame.p())); + + // If there is but one sample in the geodesic, write that sample and exit. + if (geodesic.size() == 1) { + return 1; + } + + // Seperate the interpolation points by equal length increments. + const Real dl = geodesic.back().length / static_cast(nSamples - 1); + + // Compute the interpolated points from the geodesic. + auto itGeodesic = geodesic.begin(); + // We can skip the first and last samples, because these are pushed + // manually before and after this loop respectively (we start at i=1 + // and stop at i < nSamples-1). + for (size_t i = 1; i < nSamples - 1; ++i) { + + // Length at the current interpolation point. + const Real length = dl * static_cast(i); + + // Find the two samples (lhs, rhs) of the geodesic such that the + // length of the interpolation point lies between them. + // i.e. find: lhs.length <= length < rhs.length + while (true) { + // Sanity check: We should stay within range. + if ((itGeodesic + 1) == geodesic.end()) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Resampling of geodesic failed: Attempted to read out of " + "array range"); + } + + // The candidate samples to use for interpolation. + const LocalGeodesicSample& lhs = *itGeodesic; + const LocalGeodesicSample& rhs = *(itGeodesic + 1); + + // Sanity check: Samples are assumed to be monotonically increasing + // in length. + if (lhs.length > rhs.length) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Resampling of geodesic failed: Samples are not " + "monotonically increasing in length."); + } + + // Check that the interpolation point lies between these samples: + // lhs.length <= length < rhs.length + if (length >= rhs.length) { + // Try the next two samples. + ++itGeodesic; + continue; + } + + // Do the interpolation, and write to the output buffer. + const Vec3 point_S = calcHermiteInterpolation(lhs, rhs, length); + // Transform to ground frame. + const Vec3 point_G = X_GS.shiftFrameStationToBase(point_S); + // Write interpolated point to the output buffer. + sink(point_G); + + break; + } + } + + // Capture the last point of the geodesic. + sink(X_GS.shiftFrameStationToBase(geodesic.back().frame.p())); + + return nSamples; +} +} // namespace + +int CurveSegment::Impl::calcPathPoints(const State& state, std::function& sink, int nSamples) const +{ + const Transform& X_GS = getPosInfo(state).X_GS; + const InstanceEntry& geodesic_S = getInstanceEntry(state); + if (!geodesic_S.isActive()) { + return 0; + } + + // Do not do any resampling if nSamples==0, simply write the points from the + // integrator to the output buffer. + if (nSamples == 0) { + for (const LocalGeodesicSample& sample : geodesic_S.samples) { + sink(X_GS.shiftFrameStationToBase(sample.frame.p())); + } + return geodesic_S.samples.size(); + } + + // Resample the points from the integrator by interpolating at equal + // intervals. + return calcResampledGeodesicPoints( + geodesic_S.samples, + X_GS, + nSamples, + sink); +} + +//============================================================================== +// ??? +//============================================================================== + +namespace +{ + +void calcPathCorrections(SolverData& data, Real weight) +{ + // Add a cost to changing the length. + static constexpr int NUMBER_OF_CONSTRAINTS = 4; + for (int i = 0; i < data.nCurves; ++i) { + int r = data.nCurves * NUMBER_OF_CONSTRAINTS + i; + int c = 4 * i + 3; + data.pathErrorJacobian.set(r, c, weight); + } + + data.matInv = data.pathErrorJacobian; + data.matInv.solve(data.pathError, data.pathCorrection); + data.pathCorrection *= -1.; +} + +Real calcMaxRelativeDeviationFromLinear(SolverData& data) +{ + data.predictedPathError = data.pathErrorJacobian * data.pathCorrection + data.prevPathError; + if (data.predictedPathError.nrow() != data.pathError.nrow()) { + throw std::runtime_error("Incompatible vector sizes"); + } + const Real deviationFromLinear = + (data.predictedPathError - data.pathError).norm(); + const Real maxRelativeDeviation = + deviationFromLinear / data.pathCorrectionNorm; + return maxRelativeDeviation; +} + +const Correction* getPathCorrections(SolverData& data) +{ + static_assert( + sizeof(Correction) == sizeof(Real) * GeodesicDOF, + "Invalid size of geodesic correction."); + if (data.pathCorrection.size() * sizeof(Real) != + data.nCurves * sizeof(Correction)) { + throw std::runtime_error("Invalid size of path corrections vector."); + } + return reinterpret_cast(&data.pathCorrection[0]); +} + +} // namespace + +//============================================================================== +// SURFACE IMPL +//============================================================================== + +void CurveSegment::Impl::calcLiftoffIfNeeded( + const Vec3& prevPoint_S, + const Vec3& nextPoint_S, + CurveSegment::Impl::InstanceEntry& cache) const +{ + // Only attempt liftoff when currently wrapping the surface. + LocalGeodesicInfo& g = cache; + if (g.status != WrappingStatus::InContactWithSurface) { + return; + } + + // The curve length must have shrunk completely before lifting off. + if (g.length < 0.) { + return; + } + + // For a zero-length curve, trigger liftoff when the prev and next points + // lie above the surface plane. + if (dot(prevPoint_S - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= + 0. || + dot(nextPoint_S - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= + 0.) { + // No Lifted. + return; + } + + // Liftoff detected: update status. + g.status = WrappingStatus::LiftedFromSurface; + // Initialize the tracking point from the last geodesic start point. + cache.trackingPointOnLine = g.K_P.p(); +} + +void CurveSegment::Impl::calcTouchdownIfNeeded( + const Vec3& prevPoint_S, + const Vec3& nextPoint_S, + CurveSegment::Impl::InstanceEntry& cache) const +{ + // Only attempt touchdown when lifted. + LocalGeodesicInfo& g = cache; + if (g.status != WrappingStatus::LiftedFromSurface) { + return; + } + + // Detect touchdown by computing the point on the line from x_QS to x_PS + // that is nearest to the surface. + const bool touchdownDetected = calcNearestPointOnLineImplicitly( + m_Geometry, + prevPoint_S, + nextPoint_S, + cache.trackingPointOnLine, + m_TouchdownIter, + m_TouchdownAccuracy); + if (!touchdownDetected) { + return; + } + + // Touchdown detected: Remove the Lifted status flag. + g.status = WrappingStatus::InContactWithSurface; + // Shoot a zero length geodesic at the touchdown point. + shootNewGeodesic( + cache.trackingPointOnLine, + nextPoint_S - prevPoint_S, + 0., + cache.sHint, + cache); +} + +void CurveSegment::Impl::assertSurfaceBounds( + const Vec3& prevPoint_S, + const Vec3& nextPoint_S) const +{ + // Make sure that the previous point does not lie inside the surface. + if (calcSurfaceConstraintValue(m_Geometry, prevPoint_S) < 0.) { + // TODO use SimTK_ASSERT + throw std::runtime_error("Unable to wrap over surface: Preceding point " + "lies inside the surface"); + } + if (calcSurfaceConstraintValue(m_Geometry, nextPoint_S) < 0.) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Unable to wrap over surface: Next point lies inside the surface"); + } +} + +void CurveSegment::Impl::shootNewGeodesic( + Vec3 x, + Vec3 t, + Real l, + Real sHint, + InstanceEntry& cache) const +{ + cache.samples.clear(); + cache.sHint = sHint; + + calcGeodesicAndVariationImplicitly( + m_Geometry, + x, + t, + l, + cache.sHint, + cache.K_P, + cache.dK_P, + cache.K_Q, + cache.dK_Q, + m_IntegratorAccuracy, + m_ProjectionMaxIter, + m_ProjectionAccuracy, + cache.samples); + + cache.length = l; +} + +void CurveSegment::Impl::calcMaxCorrectionStepSize( + const State& s, + const Correction& c, + Real maxCorrectionStepDeg, + Real& maxStepSize) const +{ + const Real angle = maxCorrectionStepDeg / 180. * M_PI; + + const InstanceEntry& cache = getInstanceEntry(s); + + auto UpdateMaxStepSize = + [&](const FrenetFrame& K, const Variation& dK, CoordinateAxis axis) { + const UnitVec3& a = cache.K_P.R().getAxisUnitVec(axis); + + const Vec4 dx = ~dK[1] * a; + const Real maxDisplacementEstimate = dot(abs(dx), abs(c)); + const Real curvature = + calcNormalCurvature(getContactGeometry(), K.p(), a); + const Real maxAllowedDisplacement = std::abs(angle / curvature); + if (std::abs(maxDisplacementEstimate) > maxAllowedDisplacement) { + const Real s = + std::abs(maxAllowedDisplacement / maxDisplacementEstimate); + maxStepSize = s < maxStepSize ? s : maxStepSize; + } + }; + + UpdateMaxStepSize(cache.K_P, cache.dK_P, TangentAxis); + UpdateMaxStepSize(cache.K_P, cache.dK_P, BinormalAxis); + UpdateMaxStepSize(cache.K_Q, cache.dK_Q, TangentAxis); + UpdateMaxStepSize(cache.K_Q, cache.dK_Q, BinormalAxis); +} + +//============================================================================== +// CURVE SEGMENT IMPL +//============================================================================== + +CurveSegment::Impl::Impl( + CableSpan path, + MobilizedBodyIndex body, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess) : + m_Subsystem(&path.updImpl().updSubsystem()), + m_Path(path), m_Index(-1), // TODO what to do with this index, and when + m_Body(body), m_X_BS(X_BS), m_Geometry(geometry), + m_ContactPointHint_S(initPointGuess), + m_Decoration(geometry.createDecorativeGeometry() + .setColor(Orange) + .setOpacity(.75) + .setResolution(3)) +{ + SimTK_ASSERT(body.isValid(), "Failed to create new CurveSegment: Invalid MobilizedBodyIndex."); +} + +const MobilizedBody& CurveSegment::Impl::getMobilizedBody() const +{ + return getSubsystem().getImpl().getMultibodySystem(). + getMatterSubsystem().getMobilizedBody(m_Body); +} + +void CurveSegment::Impl::realizeCablePosition(const State& s) const +{ + m_Path.getImpl().realizePosition(s); +} + +//============================================================================== +// PATH HELPERS +//============================================================================== + +namespace +{ + +static const int N_PATH_CONSTRAINTS = 4; + +void addDirectionJacobian( + const LineSegment& e, + const UnitVec3& axis, + const PointVariation& dx, + std::function& AddBlock, + bool invert = false) +{ + Vec3 y = axis - e.d * dot(e.d, axis); + y /= e.l * (invert ? -1. : 1); + AddBlock((~dx) * y); +} + +Real calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) +{ + return dot(e.d, R.getAxisUnitVec(axis)); +} + +void addPathErrorJacobian( + const LineSegment& e, + const UnitVec3& axis, + const Variation& dK, + std::function& AddBlock, + bool invertV = false) +{ + addDirectionJacobian(e, axis, dK[1], AddBlock, invertV); + AddBlock((~dK[0]) * cross(axis, e.d)); +} + +} // namespace + +//============================================================================== +// CABLE SPAN IMPL +//============================================================================== + +const Mobod& CableSpan::Impl::getOriginBody() const +{ + return getSubsystem().getImpl().getMultibodySystem(). + getMatterSubsystem().getMobilizedBody(m_OriginBody); +} + +const Mobod& CableSpan::Impl::getTerminationBody() const +{ + return getSubsystem().getImpl().getMultibodySystem(). + getMatterSubsystem().getMobilizedBody(m_TerminationBody); +} + +void CableSpan::Impl::realizeTopology(State& s) +{ + for (CurveSegment segment : m_CurveSegments) { + segment.updImpl().realizeTopology(s); + } + + PosInfo posInfo{}; + m_PosInfoIx = updSubsystem().allocateCacheEntry( + s, + Stage::Position, + Stage::Infinity, + new Value(posInfo)); + + getSubsystem().markCacheValueNotRealized(s, m_PosInfoIx); + + VelInfo velInfo{}; + m_VelInfoIx = updSubsystem().allocateCacheEntry( + s, + Stage::Velocity, + Stage::Infinity, + new Value(velInfo)); + + getSubsystem().markCacheValueNotRealized(s, m_VelInfoIx); +} + +void CableSpan::Impl::realizePosition(const State& s) const +{ + if (getSubsystem().isCacheValueRealized(s, m_PosInfoIx)) { + return; + } + calcPosInfo(s, updPosInfo(s)); + getSubsystem().markCacheValueRealized(s, m_PosInfoIx); +} + +void CableSpan::Impl::realizeVelocity(const State& s) const +{ + realizePosition(s); + if (getSubsystem().isCacheValueRealized(s, m_VelInfoIx)) { + return; + } + calcVelInfo(s, updVelInfo(s)); + getSubsystem().markCacheValueRealized(s, m_VelInfoIx); +} + +const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo( + const State& s) const +{ + realizePosition(s); + return Value::downcast( + getSubsystem().getCacheEntry(s, m_PosInfoIx)); +} + +CableSpan::Impl::PosInfo& CableSpan::Impl::updPosInfo(const State& s) const +{ + return Value::updDowncast( + getSubsystem().updCacheEntry(s, m_PosInfoIx)); +} + +const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo( + const State& s) const +{ + realizeVelocity(s); + return Value::downcast( + getSubsystem().getCacheEntry(s, m_VelInfoIx)); +} + +CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const +{ + return Value::updDowncast( + getSubsystem().updCacheEntry(s, m_VelInfoIx)); +} + +const CurveSegment* CableSpan::Impl::findPrevActiveCurveSegment( + const State& s, + CurveSegmentIndex ix) const +{ + for (int i = ix - 1; i >= 0; --i) { + // Find the active segment before the current. + if (m_CurveSegments.at(CurveSegmentIndex(i)) + .getImpl() + .getInstanceEntry(s) + .isActive()) { + return &m_CurveSegments.at(CurveSegmentIndex(i)); + } + } + return nullptr; +} + +const CurveSegment* CableSpan::Impl::findNextActiveCurveSegment( + const State& s, + CurveSegmentIndex ix) const +{ + // Find the active segment after the current. + for (int i = ix + 1; i < m_CurveSegments.size(); ++i) { + if (m_CurveSegments.at(CurveSegmentIndex(i)) + .getImpl() + .getInstanceEntry(s) + .isActive()) { + return &m_CurveSegments.at(CurveSegmentIndex(i)); + } + } + return nullptr; +} + +Vec3 CableSpan::Impl::findPrevPoint(const State& s, CurveSegmentIndex ix) const +{ + const CurveSegment* segment = findPrevActiveCurveSegment(s, ix); + return segment ? segment->getImpl().calcFinalContactPoint(s) + : getOriginBody().getBodyTransform(s).shiftFrameStationToBase( + m_OriginPoint); +} + +Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) const +{ + const CurveSegment* segment = findNextActiveCurveSegment(s, ix); + return segment + ? segment->getImpl().calcInitialContactPoint(s) + : getTerminationBody().getBodyTransform(s).shiftFrameStationToBase( + m_TerminationPoint); +} + +template +void CableSpan::Impl::calcPathErrorVector( + const State& s, + const std::vector& lines, + std::array axes, + Vector& pathError) const +{ + size_t lineIx = 0; + ptrdiff_t row = -1; + pathError *= 0; + + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().getInstanceEntry(s).isActive()) { + continue; + } + + const GeodesicInfo& g = segment.getImpl().getPosInfo(s); + for (CoordinateAxis axis : axes) { + pathError(++row) = calcPathError(lines.at(lineIx), g.KP.R(), axis); + } + ++lineIx; + for (CoordinateAxis axis : axes) { + pathError(++row) = calcPathError(lines.at(lineIx), g.KQ.R(), axis); + } + } +} + +template +void CableSpan::Impl::calcPathErrorJacobian( + const State& s, + const std::vector& lines, + std::array axes, + Matrix& J) const +{ + constexpr size_t Nq = GeodesicDOF; + + // TODO perhaps just not make method static. + const size_t n = lines.size() - 1; + J *= 0.; + + SimTK_ASSERT( + J.rows() == n * N, + "Invalid number of rows in jacobian matrix"); + SimTK_ASSERT( + J.cols() == n * Nq, + "Invalid number of columns in jacobian matrix"); + + size_t row = 0; + size_t col = 0; + size_t activeIx = 0; + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().getInstanceEntry(s).isActive()) { + continue; + } + const GeodesicInfo& g = segment.getImpl().getPosInfo(s); + + const LineSegment& l_P = lines.at(activeIx); + const LineSegment& l_Q = lines.at(++activeIx); + + const CurveSegmentIndex ix = segment.getImpl().getIndex(); + const CurveSegment* prev = findPrevActiveCurveSegment(s, ix); + const CurveSegment* next = findNextActiveCurveSegment(s, ix); + + int blkCol = col; + std::function AddBlock = [&](const Vec4& block) { + for (int ix = 0; ix < 4; ++ix) { + J.row(row).col(blkCol + ix) += block[ix]; + } + }; + + const Variation& dK_P = g.dKP; + for (CoordinateAxis axis : axes) { + const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); + + blkCol = col; + addPathErrorJacobian(l_P, a_P, dK_P, AddBlock, false); + + if (prev) { + const Variation& prev_dK_Q = prev->getImpl().getPosInfo(s).dKQ; + blkCol = col - Nq; + addDirectionJacobian(l_P, a_P, prev_dK_Q[1], AddBlock, true); + } + ++row; + } + + const Variation& dK_Q = g.dKQ; + for (CoordinateAxis axis : axes) { + const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); + + blkCol = col; + addPathErrorJacobian(l_Q, a_Q, dK_Q, AddBlock, true); + + if (next) { + const Variation& next_dK_P = next->getImpl().getPosInfo(s).dKP; + blkCol = col + Nq; + addDirectionJacobian(l_Q, a_Q, next_dK_P[1], AddBlock, false); + } + ++row; + } + + col += Nq; + }; +} + +Real CableSpan::Impl::calcLineSegments( + const State& s, + Vec3 p_O, + Vec3 p_I, + std::vector& lines) const +{ + lines.resize(m_CurveSegments.size() + 1); + lines.clear(); + + Real totalCableLength = 0.; + Vec3 lineStart = std::move(p_O); + for (const CurveSegment& curve : m_CurveSegments) { + const CurveSegment::Impl& c = curve.getImpl(); + if (!c.getInstanceEntry(s).isActive()) { + continue; + } + + totalCableLength += c.getInstanceEntry(s).length; + + const GeodesicInfo& g = curve.getImpl().getPosInfo(s); + const Vec3 lineEnd = g.KP.p(); + lines.push_back(LineSegment(lineStart, lineEnd)); + totalCableLength += lines.back().l; + + lineStart = g.KQ.p(); + } + lines.emplace_back(lineStart, p_I); + return totalCableLength; +} + +size_t CableSpan::countActive(const State& s) const +{ + return getImpl().countActive(s); +} + +size_t CableSpan::Impl::countActive(const State& s) const +{ + size_t count = 0; + for (const CurveSegment& segment : m_CurveSegments) { + if (segment.getImpl().getInstanceEntry(s).isActive()) { + ++count; + } + } + return count; +} + +/* template */ +void CableSpan::calcPathErrorJacobian( + const State& s, + /* const std::array& axes, */ + Vector& e, + Matrix& J) const +{ + getImpl().calcPathErrorJacobianUtility(s, e, J); +} + +void CableSpan::applyCorrection(const State& s, const Vector& c) const +{ + getImpl().applyCorrection(s, c); +} + +void CableSpan::Impl::applyCorrection(const State& s, const Vector& c) const +{ + const size_t nActive = countActive(s); + if (nActive * GeodesicDOF != c.size()) { + // TODO use SimTK_ASSERT + throw std::runtime_error("invalid size of corrections vector"); + } + + const Correction* corrIt = reinterpret_cast(&c[0]); + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.getImpl().getInstanceEntry(s).isActive()) { + continue; + } + curve.getImpl().applyGeodesicCorrection(s, *corrIt); + ++corrIt; + } + + // Path has changed: invalidate each segment's cache. + invalidatePositionLevelCache(s); +} + +void CableSpan::Impl::invalidatePositionLevelCache(const State& s) const +{ + for (const CurveSegment& curve : m_CurveSegments) { + curve.getImpl().invalidatePosEntry(s); + } + getSubsystem().markCacheValueNotRealized(s, m_PosInfoIx); +} + +/* template */ +void CableSpan::Impl::calcPathErrorJacobianUtility( + const State& s, + /* const std::array& axes, */ + Vector& e, + Matrix& J) const +{ + const std::array axes = {NormalAxis, BinormalAxis}; + // Force re-realizing the cache. + invalidatePositionLevelCache(s); + for (const CurveSegment& curve : m_CurveSegments) { + curve.getImpl().realizePosition( + s, + findPrevPoint(s, curve), + findNextPoint(s, curve)); + } + + const size_t nActive = countActive(s); + + if (nActive == 0) { + J.resize(0, 0); + return; + } + + // Grab the shared data cache for computing the matrices. + SolverData& data = + getSubsystem().getImpl().updCachedScratchboard(s).updOrInsert(nActive); + + // Compute the straight-line segments. + const Vec3 x_O = + getOriginBody().getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + const Vec3 x_I = + getTerminationBody().getBodyTransform(s).shiftFrameStationToBase( + m_TerminationPoint); + calcLineSegments(s, x_O, x_I, data.lineSegments); + + // Evaluate path error. + calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); + + // Evaluate the path error jacobian. + calcPathErrorJacobian<2>( + s, + data.lineSegments, + axes, + data.pathErrorJacobian); + + e = data.pathError; + J = data.pathErrorJacobian; +} + +void CableSpan::Impl::callForEachActiveCurveSegment( + const State& s, + std::function f) const +{ + for (const CurveSegment& curve : m_CurveSegments) { + // Skip non-active segments. + if (!curve.getImpl().getInstanceEntry(s).isActive()) { + continue; + } + // Call provided function for active segments. + f(curve.getImpl()); + } +} + +void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const +{ + // Path origin and termination point. + const Vec3 x_O = + getOriginBody().getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + const Vec3 x_I = + getTerminationBody().getBodyTransform(s).shiftFrameStationToBase( + m_TerminationPoint); + + posInfo.xO = x_O; + posInfo.xI = x_I; + + // Axes considered when computing the path error. + const std::array axes{NormalAxis, BinormalAxis}; + + posInfo.loopIter = 0; + Real safetyFactor = 1.; + while (true) { + // Make sure all curve segments are realized to position stage. + // This will transform all last computed geodesics to Ground frame, and + // will update each curve's Status. + for (const CurveSegment& curve : m_CurveSegments) { + curve.getImpl().realizePosition( + s, + findPrevPoint(s, curve), + findNextPoint(s, curve)); + } + + // Count the number of active curve segments. + const size_t nActive = countActive(s); + + // If the path contains no curved segments it is a straight line. + if (nActive == 0) { + // Update the path length, and exit. + posInfo.l = (x_I - x_O).norm(); + break; + } + + // Grab the shared data cache for helping with computing the path + // corrections. This data is only used as an intermediate variable, and + // will be discarded after each iteration. + SolverData& data = + getSubsystem().getImpl().updCachedScratchboard(s).updOrInsert( + nActive); + + // Compute the straight-line segments of this cable span, and set the + // total length. Note that there is one more straight line segment, + // than there are active curve segments. + posInfo.l = calcLineSegments(s, x_O, x_I, data.lineSegments); + + // Because of the nonlinearity of the system, it is hard to predict the + // step size for which the path error behaves locally linear. + // We can compare the previous path error to the current one, and see if + // it matches what we would expect given the previous jacobian and + // applied correction. If there is a large deviation, we will try a + // smaller step next time. First determine if we can do the comparison + // at all: + bool evaluateCorrectionEffect = false; + { + // We need to have completed atleast one iteration to see the + // effect of the applied correction. + bool evaluateCorrectionEffect = posInfo.loopIter > 0; + + // We must check that there was no change in the wrapping status of + // each of the obstacles, otherwise the jacobian will not predict + // the change in path error. + for (const CurveSegment& curve : m_CurveSegments) { + evaluateCorrectionEffect &= + curve.getImpl().getInstanceEntry(s).status == + curve.getImpl().getInstanceEntry(s).prev_status; + } + + // Store the previous path error for the evaluation. + if (evaluateCorrectionEffect) { + data.prevPathError = data.pathError; + } + } + + // Evaluate the current path error as the misalignment of the straight + // line segments with the curve segment's tangent vectors at the + // contact points. + calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); + const Real maxPathError = data.pathError.normInf(); + + // Evaluate local linearity of the path error. + if (evaluateCorrectionEffect) { + // Compute the deviation from the expected path error. If the + // deviation was too large, use the safetyFactor to enfore a + // smaller step the next time. + if (calcMaxRelativeDeviationFromLinear(data) > m_PathPredErrBound) { + safetyFactor /= 2.; + std::cout << "SAFETY FACTOR = " << safetyFactor << "\n"; + } + + // We should see the deviation vanish for smaller and smaller + // correction steps. If not, there might be a bug in computing the + // errors or the jacobian. + if (!(safetyFactor > 1e-3)) { + throw std::runtime_error( + "Deviation of the path error from its linear approximation " + "did not vanish for small correction."); + } + } + + // Stop iterating if max path error is small, or max iterations has been + // reached. + if (maxPathError < m_PathErrorBound || + posInfo.loopIter >= m_PathMaxIter) { + break; + } + + // Evaluate the path error jacobian to the natural geodesic corrections + // of each curve segment. + calcPathErrorJacobian<2>( + s, + data.lineSegments, + axes, + data.pathErrorJacobian); + + // Compute the geodesic corrections for each curve segment: This gives + // us a correction vector in a direction that lowers the path error. + calcPathCorrections(data, maxPathError); + + // Compute the step size that we take along the correction vector. + Real stepSize = 1.; + const Correction* corrIt = getPathCorrections(data); + callForEachActiveCurveSegment(s, [&](const CurveSegment::Impl& curve) { + // Compute the max allowed stepsize for each active segment. + curve.calcMaxCorrectionStepSize( + s, + *corrIt, + m_MaxCorrectionStepDeg, + stepSize); + ++corrIt; + }); + + // Compute the correction with the proper step size. + data.pathCorrection *= stepSize * safetyFactor; + + // If the applied correction converges to zero before the path error + // converges to zero, unfortunately, there is nothing more to do. + { + const Real cNorm = + (data.pathCorrectionNorm = data.pathCorrection.norm()); + if (cNorm < 1e-10) { + // TODO use proper warning channel. + std::cout << "WARNING: Local minimum found\n"; + break; + } + } + + // Apply corrections to the curve segments. + corrIt = getPathCorrections(data); + callForEachActiveCurveSegment(s, [&](const CurveSegment::Impl& curve) { + curve.applyGeodesicCorrection(s, *corrIt); + ++corrIt; + }); + + // The applied corrections have changed the path: invalidate each + // segment's cache. + for (const CurveSegment& curve : m_CurveSegments) { + // Also invalidate non-active segments: They might touchdown + // again. + curve.getImpl().invalidatePosEntry(s); + } + + ++posInfo.loopIter; + } + + // TODO throw? + if (posInfo.loopIter >= m_PathMaxIter) { + std::cout << "CableSpan::calcPosInfo(): Reached max iterations!\n"; + } +} + +void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const +{ + auto CalcPointVelocityInGround = [&](const MobilizedBody& mobod, + const Vec3& point_G) -> Vec3 { + // Not using MobilizedBody::findStationVelocityInGround because the + // point_G is in ground frame. The following computation is the same + // though (minus transforming the point to the ground frame). + + // Get body kinematics in ground frame. + const Vec3& x_BG = mobod.getBodyOriginLocation(s); + const Vec3& w_BG = mobod.getBodyAngularVelocity(s); + const Vec3& v_BG = mobod.getBodyOriginVelocity(s); + + // Compute surface point velocity in ground frame. + return v_BG + w_BG % (point_G - x_BG); + }; + + Real& lengthDot = (velInfo.lengthDot = 0.); + + Vec3 v_GQ = getOriginBody().findStationVelocityInGround(s, m_OriginPoint); + const CurveSegment* lastActive = nullptr; + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.getImpl().getInstanceEntry(s).isActive()) { + continue; + } + + const MobilizedBody& mobod = curve.getImpl().getMobilizedBody(); + // TODO odd name: "g" + const CurveSegment::Impl::PosInfo& g = curve.getImpl().getPosInfo(s); + const UnitVec3 e_G = g.KP.R().getAxisUnitVec(TangentAxis); + + const Vec3 v_GP = CalcPointVelocityInGround(mobod, g.KP.p()); + + lengthDot += dot(e_G, v_GP - v_GQ); + + v_GQ = CalcPointVelocityInGround(mobod, g.KQ.p()); + + lastActive = &curve; + } + + const Vec3 v_GP = + getTerminationBody().findStationVelocityInGround(s, m_TerminationPoint); + + const PosInfo& pos = getPosInfo(s); + const UnitVec3 e_G = + lastActive ? lastActive->getImpl().getPosInfo(s).KQ.R().getAxisUnitVec( + TangentAxis) + : UnitVec3(pos.xI - pos.xO); + + lengthDot += dot(e_G, v_GP - v_GQ); +} + +void CableSpan::Impl::applyBodyForces( + const State& s, + Real tension, + Vector_& bodyForcesInG) const +{ + realizePosition(s); + if (tension < 0.) { + // TODO throw? or skip? + throw std::runtime_error("Cable tension should be nonnegative."); + } + + SpatialVec unitForce_G; + + { + calcUnitForceAtOrigin(s, unitForce_G); + getOriginBody().applyBodyForce(s, unitForce_G * tension, bodyForcesInG); + } + + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.isActive(s)) { + return; + } + + curve.calcUnitForce(s, unitForce_G); + curve.getMobilizedBody().applyBodyForce( + s, + unitForce_G * tension, + bodyForcesInG); + } + + { + calcUnitForceAtTermination(s, unitForce_G); + getTerminationBody().applyBodyForce( + s, + unitForce_G * tension, + bodyForcesInG); + } +} + +int CableSpan::Impl::calcDecorativeGeometryAndAppend( + const State& s, + Stage stage, + Array_& decorations) const +{ + static constexpr int LINE_THICKNESS = 3; + + const PosInfo& ppe = getPosInfo(s); + Vec3 prevPoint = ppe.xO; + + for (const CurveSegment& curveSegment : m_CurveSegments) { + const CurveSegment::Impl& curve = curveSegment.getImpl(); + + if (curve.getInstanceEntry(s).isActive()) { + const CurveSegment::Impl::PosInfo cppe = curve.getPosInfo(s); + + const Vec3 nextPoint = cppe.KP.p(); + decorations.push_back(DecorativeLine(prevPoint, nextPoint) + .setColor(Purple) + .setLineThickness(LINE_THICKNESS)); + prevPoint = cppe.KQ.p(); + } + + curve.calcDecorativeGeometryAndAppend(s, decorations); + } + + // TODO choose colors. + decorations.push_back(DecorativeLine(prevPoint, ppe.xI) + .setColor(Purple) + .setLineThickness(3)); + + return 0; +} + +//============================================================================== +// SUBSYSTEM +//============================================================================== + +bool CableSubsystem::isInstanceOf(const Subsystem& s) +{ + return Impl::isA(s.getSubsystemGuts()); +} + +const CableSubsystem& CableSubsystem::downcast(const Subsystem& s) +{ + assert(isInstanceOf(s)); + return static_cast(s); +} +CableSubsystem& CableSubsystem::updDowncast(Subsystem& s) +{ + assert(isInstanceOf(s)); + return static_cast(s); +} + +const CableSubsystem::Impl& CableSubsystem::getImpl() const +{ + return SimTK_DYNAMIC_CAST_DEBUG(getSubsystemGuts()); +} +CableSubsystem::Impl& CableSubsystem::updImpl() +{ + return SimTK_DYNAMIC_CAST_DEBUG(updSubsystemGuts()); +} + +// Create Subsystem but don't associate it with any System. This isn't much use +// except for making std::vectors, which require a default constructor to be +// available. +CableSubsystem::CableSubsystem() +{ + adoptSubsystemGuts(new Impl()); +} + +CableSubsystem::CableSubsystem(MultibodySystem& mbs) +{ + adoptSubsystemGuts(new Impl()); + mbs.adoptSubsystem(*this); +} // steal ownership + +int CableSubsystem::getNumPaths() const +{ + return getImpl().getNumPaths(); +} + +const CableSpan& CableSubsystem::getPath(CableSpanIndex ix) const +{ + return getImpl().getCablePath(ix); +} + +CableSpan& CableSubsystem::updPath(CableSpanIndex ix) +{ + return updImpl().updCablePath(ix); +} diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h new file mode 100644 index 000000000..24e2bf01f --- /dev/null +++ b/Simbody/src/WrappingImpl.h @@ -0,0 +1,1116 @@ +#ifndef SimTK_SIMBODY_WRAPPING_PATH_IMPL_H_ +#define SimTK_SIMBODY_WRAPPING_PATH_IMPL_H_ + +#include "SimTKcommon/internal/State.h" +#include "SimTKmath.h" +#include "simbody/internal/MobilizedBody.h" +#include "simbody/internal/MultibodySystem.h" +#include "simbody/internal/SimbodyMatterSubsystem.h" +#include "simbody/internal/Wrapping.h" +#include "simbody/internal/common.h" +#include "simmath/internal/ContactGeometry.h" +#include +#include +#include + +namespace SimTK +{ + +//============================================================================== +// ??? IMPL +//============================================================================== +class CurveSegment::Impl +{ + +//------------------------------------------------------------------------------ +// Some public types and aliases +//------------------------------------------------------------------------------ +public: + using FrenetFrame = ContactGeometry::FrenetFrame; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; + + // Instance level cache entry. + struct LocalGeodesicSample + { + LocalGeodesicSample(Real l, FrenetFrame K) : length(l), frame(K) + {} + + Real length; + FrenetFrame frame; + }; + + // Instance level cache entry. + struct InstanceEntry + { + bool isActive() const + { + return status == WrappingStatus::InContactWithSurface; + } + + FrenetFrame K_P{}; + FrenetFrame K_Q{}; + + Real length = NaN; + + Variation dK_P{}; + Variation dK_Q{}; + + std::vector samples; + Real sHint = NaN; + + Vec3 trackingPointOnLine{NaN, NaN, NaN}; + WrappingStatus status = WrappingStatus::InContactWithSurface; + + // TODO experimental, might remove later. + FrenetFrame prev_K_P; + FrenetFrame prev_K_Q; + Variation prev_dK_P; + Variation prev_dK_Q; + Correction prev_correction; + WrappingStatus prev_status = WrappingStatus::Disabled; + }; + + // Position level cache: Curve in ground frame. + struct PosInfo + { + Transform X_GS{}; + + FrenetFrame KP{}; + FrenetFrame KQ{}; + + Variation dKP{}; + Variation dKQ{}; + }; +//------------------------------------------------------------------------------ + +public: + Impl() = delete; + Impl(const Impl&) = delete; + Impl& operator=(const Impl&) = delete; + + ~Impl() = default; + Impl(Impl&&) noexcept = default; + Impl& operator=(Impl&&) noexcept = default; + + // TODO you would expect the constructor to take the index as well here? + Impl( + CableSpan path, + MobilizedBodyIndex body, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess); + +//------------------------------------------------------------------------------ +// Stage realizations +//------------------------------------------------------------------------------ + // Allocate state variables and cache entries. + void realizeTopology(State& s) + { + // Allocate an auto-update discrete variable for the last computed + // geodesic. + Value* cache = new Value(); + + cache->upd().length = 0.; + cache->upd().status = WrappingStatus::LiftedFromSurface; + cache->upd().trackingPointOnLine = getContactPointHint(); + + m_InstanceIx = updSubsystem().allocateAutoUpdateDiscreteVariable( + s, + Stage::Report, + cache, + Stage::Position); + + PosInfo posInfo{}; + m_PosIx = updSubsystem().allocateCacheEntry( + s, + Stage::Position, + Stage::Infinity, + new Value(posInfo)); + } + + void realizePosition(const State& s, Vec3 prevPointG, Vec3 nextPointG) const + { + if (getSubsystem().isCacheValueRealized(s, m_PosIx)) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "expected not realized when calling realizePosition"); + } + + if (getInstanceEntry(s).status == WrappingStatus::Disabled) { + return; + } + + // Compute tramsform from local surface frame to ground. + const Transform& X_GS = calcSurfaceFrameInGround(s); + + { + // Transform the prev and next path points to the surface frame. + const Vec3 prevPoint_S = X_GS.shiftBaseStationToFrame(prevPointG); + const Vec3 nextPoint_S = X_GS.shiftBaseStationToFrame(nextPointG); + + // Detect liftoff, touchdown and potential invalid configurations. + // TODO this doesnt follow the regular invalidation scheme... + // Grab the last geodesic that was computed. + assertSurfaceBounds(prevPoint_S, nextPoint_S); + + calcInitialPathIfNeeded( + prevPoint_S, + nextPoint_S, + updInstanceEntry(s)); + + calcTouchdownIfNeeded( + prevPoint_S, + nextPoint_S, + updInstanceEntry(s)); + calcLiftoffIfNeeded(prevPoint_S, nextPoint_S, updInstanceEntry(s)); + + getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); + } + + // At this point we have a valid geodesic in surface frame. + const InstanceEntry& ie = getInstanceEntry(s); + + // Start updating the position level cache. + PosInfo& pos = updPosInfo(s); + + // Transform geodesic in local surface coordinates to ground. + { + // Store the local geodesic in ground frame. + pos.X_GS = X_GS; + + // Store the the local geodesic in ground frame. + pos.KP = X_GS.compose(ie.K_P); + pos.KQ = X_GS.compose(ie.K_Q); + + pos.dKP[0] = X_GS.R() * ie.dK_P[0]; + pos.dKP[1] = X_GS.R() * ie.dK_P[1]; + + pos.dKQ[0] = X_GS.R() * ie.dK_Q[0]; + pos.dKQ[1] = X_GS.R() * ie.dK_Q[1]; + } + + getSubsystem().markCacheValueRealized(s, m_PosIx); + } + + void realizeCablePosition(const State& s) const; + + void invalidatePosEntry(const State& state) const + { + getSubsystem().markCacheValueNotRealized(state, m_PosIx); + } + +//------------------------------------------------------------------------------ +// Parameter & model components access +//------------------------------------------------------------------------------ + const CableSpan& getCable() const + { + return m_Path; + } + + CurveSegmentIndex getIndex() const + { + return m_Index; + } + + void setIndex(CurveSegmentIndex ix) + { + m_Index = ix; + } + + // Set the user defined point that controls the initial wrapping path. + // Point is in surface coordinates. + void setContactPointHint(Vec3 contactPointHint_S) + { + m_ContactPointHint_S = contactPointHint_S; + } + + // Get the user defined point that controls the initial wrapping path. + Vec3 getContactPointHint() const + { + return m_ContactPointHint_S; + } + + const ContactGeometry& getContactGeometry() const + { + return m_Geometry; + } + + const DecorativeGeometry& getDecoration() const + { + return m_Decoration; + } + + const MobilizedBody& getMobilizedBody() const; + + const Transform& getXformSurfaceToBody() const + { + return m_X_BS; + } + void setXformSurfaceToBody(Transform X_BS) + { + m_X_BS = std::move(X_BS); + } + + const CableSubsystem& getSubsystem() const + { + return *m_Subsystem; + } + CableSubsystem& updSubsystem() + { + return *m_Subsystem; + } + +//------------------------------------------------------------------------------ +// Cache entry access +//------------------------------------------------------------------------------ + const InstanceEntry& getInstanceEntry(const State& s) const + { + const CableSubsystem& subsystem = getSubsystem(); + if (!subsystem.isDiscreteVarUpdateValueRealized(s, m_InstanceIx)) { + updInstanceEntry(s) = getPrevInstanceEntry(s); + subsystem.markDiscreteVarUpdateValueRealized(s, m_InstanceIx); + } + return Value::downcast( + subsystem.getDiscreteVarUpdateValue(s, m_InstanceIx)); + } + + const PosInfo& getPosInfo(const State& s) const + { + return Value::downcast( + getSubsystem().getCacheEntry(s, m_PosIx)); + } + + Transform calcSurfaceFrameInGround(const State& s) const + { + return getMobilizedBody().getBodyTransform(s).compose(m_X_BS); + } + + int calcPathPoints(const State& state, std::function& sink, int nSamples = 0) const; + + void calcUnitForce(const State& s, SpatialVec& unitForce_G) const + { + const PosInfo& posInfo = getPosInfo(s); + const Vec3& x_BG = getMobilizedBody().getBodyOriginLocation(s); + + // Contact point moment arms in ground. + const Vec3 r_P = posInfo.KP.p() - x_BG; + const Vec3 r_Q = posInfo.KQ.p() - x_BG; + + // Tangent directions at contact points in ground. + const UnitVec3& t_P = posInfo.KP.R().getAxisUnitVec(TangentAxis); + const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); + + unitForce_G[0] = r_Q % t_Q - r_P % t_P; + unitForce_G[1] = t_Q - t_P; + } + + void calcMaxCorrectionStepSize( + const State& s, + const Correction& c, + Real maxCorrectionStepDeg, + Real& maxStepSize) const; + + // TODO Below code should be moved to a unit test. + void assertLastCorrection(const State& s) const + { + std::ostream& oss = std::cout; + + const InstanceEntry& cache = getInstanceEntry(s); + + const Correction& c = cache.prev_correction; + const Real delta = c.norm(); + const Real eps = 0.05; + + const Real smallAngle = 20. / 180. * Pi; + + if (cache.prev_status != cache.status) { + return; + } + + auto AssertAxis = [&](const Rotation& R0, + const Rotation& R1, + const Vec3& w, + CoordinateAxis axis) -> bool { + const UnitVec3 a0 = R0.getAxisUnitVec(axis); + const UnitVec3 a1 = R1.getAxisUnitVec(axis); + + const Vec3 exp_diff = cross(w, a0); + const Vec3 got_diff = a1 - a0; + + const Real factor = got_diff.norm() / exp_diff.norm(); + bool isOk = std::abs(factor - 1.) < eps; + if (!isOk) { + oss << " a0 = " << R0.transpose() * a0 << "\n"; + oss << " a1 = " << R0.transpose() * a1 << "\n"; + oss << " expected diff = " + << R0.transpose() * exp_diff / delta << "\n"; + oss << " got dt_Q = " + << R0.transpose() * got_diff / delta << "\n"; + oss << " err = " + << (exp_diff - got_diff).norm() / delta << "\n"; + oss << " factor = " << factor << "\n"; + } + return isOk; + }; + + auto AssertFrame = [&](const Transform& K0, + const Transform& K1, + const Variation& dK) -> bool { + const Vec3 got_diff = K1.p() - K0.p(); + const Vec3 exp_diff = dK[1] * c; + + const Real factor = got_diff.norm() / exp_diff.norm(); + bool isOk = std::abs(factor - 1.) < eps; + if (!isOk) { + oss << "Apply variation c = " << c << ".norm() = " << delta + << "\n"; + oss << " x0 = " << K0.p() << "\n"; + oss << " x1 = " << K1.p() << "\n"; + oss << " expected dx_Q = " + << K0.R().transpose() * exp_diff / delta << "\n"; + oss << " got dx_Q = " + << K0.R().transpose() * got_diff / delta << "\n"; + oss << " err = " + << (exp_diff - got_diff).norm() / delta << "\n"; + oss << " factor = " << factor << "\n"; + oss << "FAILED position correction\n"; + } + + const Vec3 w = dK[0] * c; + + std::array axes = { + TangentAxis, + NormalAxis, + BinormalAxis}; + for (size_t i = 0; i < 3; ++i) { + if (!AssertAxis(K0.R(), K1.R(), w, axes.at(i))) { + isOk = false; + std::cout << "FAILED axis " << i << " correction\n"; + } + } + + return isOk; + }; + + if (false && (delta > 1e-10)) { + const bool assertStart = + !AssertFrame(cache.prev_K_P, cache.K_P, cache.prev_dK_P); + const bool assertEnd = + !AssertFrame(cache.prev_K_Q, cache.K_Q, cache.prev_dK_Q); + if (assertStart || assertEnd) { + // TODO use SimTK_ASSERT + throw std::runtime_error("FAILED GEODESIC CORRECTION TEST"); + } + } + } + + // Apply the correction to the initial condition of the geodesic, and + // shoot a new geodesic, updating the cache variable. + void applyGeodesicCorrection(const State& s, const Correction& c) const + { + + // TODO This is experimental, should become a unit test probably. + { + // Test the last correction. + assertLastCorrection(s); + // Setup data for testing next correction. + InstanceEntry& cache = updInstanceEntry(s); + cache.prev_K_P = cache.K_P; + cache.prev_K_Q = cache.K_Q; + cache.prev_dK_P = cache.dK_P; + cache.prev_dK_Q = cache.dK_Q; + cache.prev_correction = c; + cache.prev_status = cache.status; + } + + // Get the previous geodesic. + const InstanceEntry& cache = getInstanceEntry(s); + const Variation& dK_P = cache.dK_P; + const FrenetFrame& K_P = cache.K_P; + + // Get corrected initial conditions. + const Vec3 v = dK_P[1] * c; + const Vec3 point = K_P.p() + v; + + const Vec3 w = dK_P[0] * c; + const UnitVec3 t = K_P.R().getAxisUnitVec(TangentAxis); + const Vec3 tangent = t + cross(w, t); + + // Take the length correction, and add to the current length. + const Real dl = c[3]; // Length increment is the last element. + const Real length = + std::max(cache.length + dl, 0.); // Clamp length to be nonnegative. + + // Shoot the new geodesic. + shootNewGeodesic( + point, + tangent, + length, + cache.sHint, + updInstanceEntry(s)); + + getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); + + invalidatePosEntry(s); + } + + WrappingStatus getPrevStatus(const State& s) const + { + return getInstanceEntry(s).prev_status; + } + + Vec3 calcInitialContactPoint(const State& s) const + { + const InstanceEntry& ic = getInstanceEntry(s); + if (!ic.isActive()) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Invalid contact point: Curve is not active"); + } + const Vec3& x_PS = ic.K_P.p(); + return calcSurfaceFrameInGround(s).shiftFrameStationToBase(x_PS); + } + + Vec3 calcFinalContactPoint(const State& s) const + { + const InstanceEntry& ic = getInstanceEntry(s); + if (!ic.isActive()) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Invalid contact point: Curve is not active"); + } + const Vec3& x_QS = ic.K_Q.p(); + return calcSurfaceFrameInGround(s).shiftFrameStationToBase(x_QS); + } + + void calcDecorativeGeometryAndAppend( + const State& s, + Array_& decorations) const + { + static constexpr int LINE_THICKNESS = 3; + static constexpr Real INACTIVE_OPACITY = 0.25; + + const InstanceEntry& cache = getInstanceEntry(s); + const PosInfo& ppe = getPosInfo(s); + + // Draw the surface (TODO since we do not own it, should it be done here at all?). + { + DecorativeGeometry geo = getDecoration(); // TODO clone it? + const Transform& X_GS = ppe.X_GS; + const Transform& X_SD = geo.getTransform(); + // Inactive surfaces are dimmed. + const Vec3 color = cache.isActive() ? + geo.getColor() : geo.getColor() * INACTIVE_OPACITY; + + decorations.push_back(geo.setTransform(X_GS * X_SD) + .setColor(color)); + } + + // Check wrapping status to see if there is a curve to draw. + if (!cache.isActive() || cache.length <= 0.) { + return; + } + + // Draw the curve segment as straight lines between prevPoint and nextPoint. + { + bool isFirstSample = true; + Vec3 prevPoint {NaN}; + std::function drawLine = [&](Vec3 nextPoint) { + if (!isFirstSample) { + decorations.push_back( + DecorativeLine(prevPoint, nextPoint).setColor(Purple).setLineThickness(3)); + } + isFirstSample = false; + prevPoint = nextPoint; + }; + + calcPathPoints(s, drawLine); + } + + // Draw the Frenet frame at curve start and end. + // TODO this is for debugging should be removed. + { + static constexpr int FRENET_FRAME_LINE_THICKNESS = 5; + static constexpr Real FRENET_FRAME_LINE_LENGTH = 0.5; + + const std::array axes = { + TangentAxis, + NormalAxis, + BinormalAxis}; + const std::array colors = {Red, Green, Blue}; + + std::function DrawFrenetFrame = [&] + (const FrenetFrame& K) { + for (size_t i = 0; i < 3; ++i) { + decorations.push_back( + DecorativeLine( + K.p(), + K.p() + FRENET_FRAME_LINE_LENGTH * K.R().getAxisUnitVec(axes.at(i))) + .setColor(colors.at(i)) + .setLineThickness(FRENET_FRAME_LINE_THICKNESS)); + + } + }; + + DrawFrenetFrame(ppe.KP); + DrawFrenetFrame(ppe.KQ); + } + } + +private: + InstanceEntry& updInstanceEntry(const State& state) const + { + return Value::updDowncast( + getSubsystem().updDiscreteVarUpdateValue(state, m_InstanceIx)); + } + + const InstanceEntry& getPrevInstanceEntry(const State& state) const + { + return Value::downcast( + getSubsystem().getDiscreteVariable(state, m_InstanceIx)); + } + + InstanceEntry& updPrevInstanceEntry(State& state) const + { + return Value::updDowncast( + getSubsystem().updDiscreteVariable(state, m_InstanceIx)); + } + + PosInfo& updPosInfo(const State& state) const + { + return Value::updDowncast( + getSubsystem().updCacheEntry(state, m_PosIx)); + } + +//------------------------------------------------------------------------------ +// Local geodesic helpers +//------------------------------------------------------------------------------ + // Assert that previous and next point lie above the surface. Points are in + // surface coordinates. + void assertSurfaceBounds(const Vec3& prevPoint_S, const Vec3& nextPoint_S) + const; + + void calcInitialPathIfNeeded( + const Vec3& prevPoint_S, + const Vec3& nextPoint_S, + InstanceEntry& cache) const + { + // Initialize the path when enabling the segment. + const bool enableSwitchDetected = + cache.status == WrappingStatus::InContactWithSurface && + cache.prev_status == WrappingStatus::Disabled; + + if (!enableSwitchDetected) { + return; + } + + // Shoot zero length geodesic at configured initial contact point. + shootNewGeodesic( + m_ContactPointHint_S, + nextPoint_S - prevPoint_S, + 0., + 0., + cache); + } + + // Attempt to compute the point of touchdown on the surface. + void calcTouchdownIfNeeded( + const Vec3& prevPoint_S, + const Vec3& nextPoint_S, + InstanceEntry& cache) const; + + void calcLiftoffIfNeeded( + const Vec3& prevPoint_S, + const Vec3& nextPoint_S, + InstanceEntry& cache) const; + + // Compute a new geodesic in surface frame coordinates. + void shootNewGeodesic( + Vec3 point_S, + Vec3 tangent_S, + Real length, + Real stepSizeHint, + InstanceEntry& cache) const; +//------------------------------------------------------------------------------ + + // TODO Required for accessing the cache variable? + CableSubsystem* m_Subsystem; // The subsystem this segment belongs to. + CableSpan m_Path; // The path this segment belongs to. + CurveSegmentIndex m_Index; // The index in its path. + + // MobilizedBody that surface is attached to. + MobilizedBodyIndex m_Body; + // Surface to body transform. + Transform m_X_BS; + + // Decoration TODO should this be here? + ContactGeometry m_Geometry; + DecorativeGeometry m_Decoration; + + // TOPOLOGY CACHE + CacheEntryIndex m_PosIx; + DiscreteVariableIndex m_InstanceIx; + + Vec3 m_ContactPointHint_S{NaN, NaN, NaN}; + + // TODO expose getters and setters. + size_t m_ProjectionMaxIter = 50; // TODO set to reasonable value + Real m_ProjectionAccuracy = 1e-10; + + // TODO expose getters and setters. + Real m_IntegratorAccuracy = 1e-8; + + // TODO expose getters and setters. + Real m_TouchdownAccuracy = 1e-4; // TODO set to reasonable value + size_t m_TouchdownIter = 50; // TODO set to reasonable value +}; + +//============================================================================== +// PATH :: IMPL +//============================================================================== +// TODO +// - handout CurveSegment index +// - add other cache variables: velocity, acceleration, force +// - names: isValid, ContactStationOnBody, Entry (not info/cache) +class CableSpan::Impl +{ +public: + Impl( + CableSubsystem& subsystem, + MobilizedBodyIndex originBody, + Vec3 originPoint, + MobilizedBodyIndex terminationBody, + Vec3 terminationPoint) : + m_Subsystem(&subsystem), + m_OriginBody(originBody), m_OriginPoint(originPoint), + m_TerminationBody(terminationBody), m_TerminationPoint(terminationPoint) + {} + + int getNumCurveSegments() const + { + return m_CurveSegments.size(); + } + + const CurveSegment& getCurveSegment(CurveSegmentIndex ix) const + { + return m_CurveSegments[ix]; + } + + CurveSegmentIndex adoptSegment(const CurveSegment& segment) + { + m_CurveSegments.push_back(segment); + return CurveSegmentIndex(m_CurveSegments.size() - 1); + } + + // Position level cache entry. + struct PosInfo + { + Vec3 xO{NaN, NaN, NaN}; + Vec3 xI{NaN, NaN, NaN}; + + Real l = NaN; + + size_t loopIter = 0; + }; + + // Velocity level cache entry. + struct VelInfo + { + Real lengthDot = NaN; + }; + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void invalidateTopology() + { + getSubsystem().invalidateSubsystemTopologyCache(); + } + void invalidatePositionLevelCache(const State& state) const; + + const PosInfo& getPosInfo(const State& state) const; + const VelInfo& getVelInfo(const State& state) const; + + void applyBodyForces( + const State& state, + Real tension, + Vector_& bodyForcesInG) const; + + int calcDecorativeGeometryAndAppend( + const State& state, + Stage stage, + Array_& decorations) const; + + int calcPathPoints(const State& state, std::function& sink, int nPointsPerCurveSegment) const + { + // Write the initial point. + const PosInfo& pos = getPosInfo(state); + sink(pos.xO); + + // Write points along each of the curves. + int count = 0; // Count number of points written. + for (const CurveSegment& curve : m_CurveSegments) { + count += curve.getImpl().calcPathPoints( + state, + sink, + nPointsPerCurveSegment); + } + + // Write the termination point. + sink(pos.xI); + + // Return number of points written. + return count + 2; + } + + void calcUnitForceAtOrigin(const State& s, SpatialVec& unitForce_G) const + { + const PosInfo& posInfo = getPosInfo(s); + const Vec3& x_BG = getOriginBody().getBodyOriginLocation(s); + + // Origin contact point moment arm in ground. + const Vec3& r = m_OriginPoint; + + Vec3 nextPointG = posInfo.xI; + for (const CurveSegment& curve : m_CurveSegments) { + if (curve.getImpl().getInstanceEntry(s).isActive()) { + nextPointG = curve.getImpl().getPosInfo(s).KP.p(); + break; + } + } + // Tangent direction at origin contact point in ground. + const UnitVec3& t = UnitVec3(nextPointG - posInfo.xO); + + unitForce_G[0] = -r % t; + unitForce_G[1] = -Vec3(t); + } + + void calcUnitForceAtTermination(const State& s, SpatialVec& unitForce_G) + const + { + const PosInfo& posInfo = getPosInfo(s); + const Vec3& x_BG = getTerminationBody().getBodyOriginLocation(s); + + // Termination contact point moment arm in ground. + const Vec3& r = m_TerminationPoint; + + Vec3 prevPointG = posInfo.xO; + // TODO fix this loop. + for (const CurveSegment& curve : m_CurveSegments) { + if (curve.getImpl().getInstanceEntry(s).isActive()) { + prevPointG = curve.getImpl().getPosInfo(s).KP.p(); + } + } + + // Tangent directions at termination contact point in ground. + const UnitVec3& t = UnitVec3(posInfo.xI - prevPointG); + + unitForce_G[0] = r % t; + unitForce_G[1] = Vec3(t); + } + + Real calcCablePower(const State& state, Real tension) const + { + if (tension < 0.) { + // TODO use SimTK_ASSERT + throw std::runtime_error("Cable tension must be nonnegative"); + } + SpatialVec unitForce; + + Real unitPower = 0.; + + { + calcUnitForceAtOrigin(state, unitForce); + SpatialVec v = getOriginBody().getBodyVelocity(state); + unitPower += ~unitForce * v; + } + + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.isActive(state)) { + continue; + } + + curve.calcUnitForce(state, unitForce); + SpatialVec v = curve.getMobilizedBody().getBodyVelocity(state); + unitPower += ~unitForce * v; + } + + { + calcUnitForceAtTermination(state, unitForce); + SpatialVec v = getTerminationBody().getBodyVelocity(state); + unitPower += ~unitForce * v; + } + + return unitPower * tension; + } + + void calcPathErrorJacobianUtility( + const State& state, + Vector& pathError, + Matrix& pathErrorJacobian) const; + + void applyCorrection(const State& state, const Vector& correction) const; + + size_t countActive(const State& s) const; + +private: + PosInfo& updPosInfo(const State& s) const; + VelInfo& updVelInfo(const State& state) const; + + void calcPosInfo(const State& s, PosInfo& posInfo) const; + void calcVelInfo(const State& s, VelInfo& velInfo) const; + + Vec3 findPrevPoint(const State& state, CurveSegmentIndex ix) const; + Vec3 findPrevPoint(const State& state, const CurveSegment& curve) const + { + return findPrevPoint(state, curve.getImpl().getIndex()); + } + + Vec3 findNextPoint(const State& state, CurveSegmentIndex ix) const; + Vec3 findNextPoint(const State& state, const CurveSegment& curve) const + { + return findNextPoint(state, curve.getImpl().getIndex()); + } + + const CurveSegment* findPrevActiveCurveSegment( + const State& s, + CurveSegmentIndex ix) const; + const CurveSegment* findNextActiveCurveSegment( + const State& s, + CurveSegmentIndex ix) const; + + template + void calcPathErrorVector( + const State& state, + const std::vector& lines, + std::array axes, + Vector& pathError) const; + + template + void calcPathErrorJacobian( + const State& state, + const std::vector& lines, + std::array axes, + Matrix& J) const; + + // Make static or not? + Real calcLineSegments( + const State& s, + Vec3 p_O, + Vec3 p_I, + std::vector& lines) const; + + const Mobod& getOriginBody() const; + const Mobod& getTerminationBody() const; + + const CableSubsystem& getSubsystem() const + { + return *m_Subsystem; + } + + CableSubsystem& updSubsystem() + { + return *m_Subsystem; + } + + void callForEachActiveCurveSegment( + const State& s, + std::function f) const; + + // Reference back to the subsystem. + CableSubsystem* m_Subsystem; // TODO just a pointer? + + MobilizedBodyIndex m_OriginBody; + Vec3 m_OriginPoint; + + MobilizedBodyIndex m_TerminationBody; + Vec3 m_TerminationPoint; + + Array_ m_CurveSegments{}; + + // TODO expose getters and setters. + Real m_PathErrorBound = 1e-6; + size_t m_PathMaxIter = 50; // TODO set to something reasonable. + + // For each curve segment the max allowed radial curvature. + Real m_MaxCorrectionStepDeg = 20.; // TODO describe + Real m_PathPredErrBound = 0.05; // TODO describe. + + // TOPOLOGY CACHE (set during realizeTopology()) + CacheEntryIndex m_PosInfoIx; + CacheEntryIndex m_VelInfoIx; + + friend CurveSegment::Impl; +}; + +//============================================================================== +// SUBSYSTEM :: IMPL +//============================================================================== +class CableSubsystem::Impl : public Subsystem::Guts +{ +public: + /** This is a helper struct that is used by a CableSpan to compute the + position level cache entry. + After computing the position level cache, this data is discarded. */ + struct SolverData + { + SolverData(int nActive) : nCurves(nActive) + { + static constexpr int Q = 4; + // 4 for the path error, and 1 for the weighting of the length. + static constexpr int C = 5; + const int n = nActive; + + lineSegments.resize(n + 1); + pathErrorJacobian = Matrix(C * n, Q * n, 0.); + pathCorrection = Vector(Q * n, 0.); + pathError = Vector(C * n, 0.); + prevPathError = Vector(C * n, NaN); + predictedPathError = Vector(C * n, NaN); + } + + std::vector lineSegments; + + Matrix pathErrorJacobian; + Vector pathCorrection; + Vector pathError; + // TODO remove prev, and cycle pathError + Vector prevPathError; + // TODO Best solver? + FactorQTZ matInv; // TODO rename to inv + Vector predictedPathError; + Real pathCorrectionNorm = NaN; + int nCurves = -1; + }; + + struct CacheEntry + { + CacheEntry() = default; + SolverData& updOrInsert(int nActive) + { + if (nActive <= 0) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Cannot produce solver data of zero dimension"); + } + + for (int i = m_Data.size(); i < nActive; ++i) { + m_Data.emplace_back(i + 1); + } + + return m_Data.at(nActive - 1); + } + + std::vector m_Data; + }; + + Impl() + {} + ~Impl() + {} + + int getNumPaths() const + { + return cables.size(); + } + + const CableSpan& getCablePath(CableSpanIndex index) const + { + return cables[index]; + } + + CableSpan& updCablePath(CableSpanIndex index) + { + return cables[index]; + } + + CableSpanIndex adoptCablePath(CableSpan& path) + { + invalidateSubsystemTopologyCache(); + cables.push_back(path); + return CableSpanIndex(cables.size() - 1); + } + + // Return the MultibodySystem which owns this WrappingPathSubsystem. + const MultibodySystem& getMultibodySystem() const + { + return MultibodySystem::downcast(getSystem()); + } + + // Return the SimbodyMatterSubsystem from which this WrappingPathSubsystem + // gets the bodies to track. + const SimbodyMatterSubsystem& getMatterSubsystem() const + { + return getMultibodySystem().getMatterSubsystem(); + } + + void realizeTopology(State& state) + { + CacheEntry cache{}; + m_CacheIx = allocateCacheEntry( + state, + Stage::Instance, + Stage::Infinity, + new Value(cache)); + } + + CacheEntry& updCachedScratchboard(const State& state) const + { + return Value::updDowncast(updCacheEntry(state, m_CacheIx)); + } + + SimTK_DOWNCAST(Impl, Subsystem::Guts); + +private: + Impl* cloneImpl() const override + { + return new Impl(*this); + } + + int calcDecorativeGeometryAndAppendImpl( + const State& state, + Stage stage, + Array_& decorations) const override + { + if (stage != Stage::Position) + return 0; + + for (const CableSpan& cable : cables) { + int returnValue = cable.getImpl().calcDecorativeGeometryAndAppend( + state, + stage, + decorations); + if (returnValue != 0) { + return returnValue; + } + } + return 0; + } + + // Allocate state variables. + int realizeSubsystemTopologyImpl(State& state) const override + { + // Briefly allow writing into the Topology cache; after this the + // Topology cache is const. + Impl* wThis = const_cast(this); + + wThis->realizeTopology(state); + for (CableSpanIndex ix(0); ix < cables.size(); ++ix) { + CableSpan& path = wThis->updCablePath(ix); + path.updImpl().realizeTopology(state); + } + + return 0; + } + + // TOPOLOGY STATE + Array_ cables; + + CacheEntryIndex m_CacheIx; +}; + +} // namespace SimTK + +#endif diff --git a/examples/ExampleCableSpan.cpp b/examples/ExampleCableSpan.cpp new file mode 100644 index 000000000..f01a80bb3 --- /dev/null +++ b/examples/ExampleCableSpan.cpp @@ -0,0 +1,396 @@ +#include "Simbody.h" +#include "simbody/internal/Wrapping.h" +#include +#include +using std::cout; +using std::endl; + +using namespace SimTK; + +// This force element implements an elastic cable of a given nominal length, +// and a stiffness k that generates a k*x force opposing stretch beyond +// the slack length. There is also a dissipation term (k*x)*c*xdot. We keep +// track of dissipated power here so we can use conservation of energy to check +// that the cable and force element aren't obviously broken. +class MyCableSpringImpl : public Force::Custom::Implementation +{ +public: + MyCableSpringImpl( + const GeneralForceSubsystem& forces, + const CableSpan& cable, + Real stiffness, + Real nominal, + Real damping) : + forces(forces), + m_Cable(cable), k(stiffness), x0(nominal), c(damping) + { + assert(stiffness >= 0 && nominal >= 0 && damping >= 0); + } + + const CableSpan& getCable() const + { + return m_Cable; + } + + // Must be at stage Velocity. Evalutes tension if necessary. + Real getTension(const State& s) const + { + ensureTensionCalculated(s); + return Value::downcast(forces.getCacheEntry(s, tensionx)); + } + + // Must be at stage Velocity. + Real getPowerDissipation(const State& s) const + { + const Real stretch = calcStretch(s); + if (stretch == 0) + return 0; + const Real rate = m_Cable.getLengthDot(s); + return k * stretch * std::max(c * rate, -1.) * rate; + } + + // This integral is always available. + Real getDissipatedEnergy(const State& s) const + { + return forces.getZ(s)[workx]; + } + + //-------------------------------------------------------------------------- + // Custom force virtuals + + // Ask the cable to apply body forces given the tension calculated here. + void calcForce( + const State& s, + Vector_& bodyForces, + Vector_& particleForces, + Vector& mobilityForces) const override + { + m_Cable.applyBodyForces(s, getTension(s), bodyForces); + } + + // Return the potential energy currently stored by the stretch of the cable. + Real calcPotentialEnergy(const State& s) const override + { + const Real stretch = calcStretch(s); + if (stretch == 0) + return 0; + return k * square(stretch) / 2; + } + + // Allocate the s variable for tracking dissipated energy, and a + // cache entry to hold the calculated tension. + void realizeTopology(State& s) const override + { + Vector initWork(1, 0.); + workx = forces.allocateZ(s, initWork); + tensionx = forces.allocateLazyCacheEntry( + s, + Stage::Velocity, + new Value(NaN)); + } + + // Report power dissipation as the derivative for the work variable. + void realizeAcceleration(const State& s) const override + { + Real& workDot = forces.updZDot(s)[workx]; + workDot = getPowerDissipation(s); + } + //-------------------------------------------------------------------------- + +private: + // Return the amount by which the cable is stretched beyond its nominal + // length or zero if the cable is slack. Must be at stage Position. + Real calcStretch(const State& s) const + { + const Real stretch = m_Cable.getLength(s) - x0; + return std::max(stretch, 0.); + } + + // Must be at stage Velocity to calculate tension. + Real calcTension(const State& s) const + { + const Real stretch = calcStretch(s); + if (stretch == 0) + return 0; + const Real rate = m_Cable.getLengthDot(s); + if (c * rate < -1) + cout << "c*rate=" << c * rate << "; limited to -1\n"; + const Real tension = k * stretch * (1 + std::max(c * rate, -1.)); + return tension; + } + + // If s is at stage Velocity, we can calculate and store tension + // in the cache if it hasn't already been calculated. + void ensureTensionCalculated(const State& s) const + { + if (forces.isCacheValueRealized(s, tensionx)) + return; + Value::updDowncast(forces.updCacheEntry(s, tensionx)) = + calcTension(s); + forces.markCacheValueRealized(s, tensionx); + } + + const GeneralForceSubsystem& forces; + CableSpan m_Cable; + Real k, x0, c; + mutable ZIndex workx; + mutable CacheEntryIndex tensionx; +}; + +// A nice handle to hide most of the cable spring implementation. This defines +// a user's API. +class MyCableSpring : public Force::Custom +{ +public: + MyCableSpring( + GeneralForceSubsystem& forces, + const CableSpan& cable, + Real stiffness, + Real nominal, + Real damping) : + Force::Custom( + forces, + new MyCableSpringImpl(forces, cable, stiffness, nominal, damping)) + {} + + // Expose some useful methods. + const CableSpan& getCable() const + { + return getImpl().getCable(); + } + Real getTension(const State& s) const + { + return getImpl().getTension(s); + } + Real getPowerDissipation(const State& s) const + { + return getImpl().getPowerDissipation(s); + } + Real getDissipatedEnergy(const State& s) const + { + return getImpl().getDissipatedEnergy(s); + } + +private: + const MyCableSpringImpl& getImpl() const + { + return dynamic_cast(getImplementation()); + } +}; + +// This gets called periodically to dump out interesting things about +// the cables and the system as a whole. It also saves states so that we +// can play back at the end. +static Array_ saveStates; +class ShowStuff : public PeriodicEventReporter +{ +public: + ShowStuff( + const MultibodySystem& mbs, + const MyCableSpring& cable1, + /* const MyCableSpring& cable2, */ + Real interval) : + PeriodicEventReporter(interval), + mbs(mbs), cable1(cable1) + {} + + static void showHeading(std::ostream& o) + { + printf( + "%8s %10s %10s %10s %10s %10s %10s %10s %10s %12s\n", + "time", + "length", + "rate", + "integ-rate", + "unitpow", + "tension", + "disswork", + "KE", + "PE", + "KE+PE-W"); + } + + /** This is the implementation of the EventReporter virtual. **/ + void handleEvent(const State& s) const override + { + const CableSpan& path1 = cable1.getCable(); + /* const CableSpan& path2 = cable2.getCable(); */ + printf( + "%8g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g CPU=%g\n", + s.getTime(), + path1.getLength(s), + path1.getLengthDot(s), + path1.calcCablePower(s, 1), // unit power + cable1.getTension(s), + cable1.getDissipatedEnergy(s), + cpuTime()); + /* printf( */ + /* "%8s %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g " */ + /* "%12.6g\n", */ + /* "", */ + /* path2.getLength(s), */ + /* path2.getLengthDot(s), */ + /* path2.calcCablePower(s, 1), // unit power */ + /* cable2.getTension(s), */ + /* cable2.getDissipatedEnergy(s), */ + /* mbs.calcKineticEnergy(s), */ + /* mbs.calcPotentialEnergy(s), */ + /* mbs.calcEnergy(s) + cable1.getDissipatedEnergy(s) + */ + /* cable2.getDissipatedEnergy(s)); */ + saveStates.push_back(s); + } + +private: + const MultibodySystem& mbs; + MyCableSpring cable1; +}; + +int main() +{ + try { + // Create the system. + MultibodySystem system; + SimbodyMatterSubsystem matter(system); + CableSubsystem cables(system); + GeneralForceSubsystem forces(system); + + system.setUseUniformBackground(true); // no ground plane in display + + Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0)); + // Force::GlobalDamper(forces, matter, 5); + + Body::Rigid someBody(MassProperties(1.0, Vec3(0), Inertia(1))); + const Real Rad = .25; + + Body::Rigid biggerBody(MassProperties(1.0, Vec3(0), Inertia(1))); + const Real BiggerRad = .5; + + const Vec3 radii(.4, .25, .15); + Body::Rigid ellipsoidBody( + MassProperties(1.0, Vec3(0), 1. * UnitInertia::ellipsoid(radii))); + + const Real CylRad = .3, HalfLen = .5; + Body::Rigid cylinderBody(MassProperties( + 1.0, + Vec3(0), + 1. * UnitInertia::cylinderAlongX(Rad, HalfLen))); + + Body::Rigid fancyBody = biggerBody; // NOT USING ELLIPSOID + + MobilizedBody Ground = matter.Ground(); + + MobilizedBody::Ball body1( + Ground, + Transform(Vec3(0)), + someBody, + Transform(Vec3(0, 1, 0))); + MobilizedBody::Ball body2( + body1, + Transform(Vec3(0)), + someBody, + Transform(Vec3(0, 1, 0))); + MobilizedBody::Ball body3( + body2, + Transform(Vec3(0)), + someBody, + Transform(Vec3(0, 1, 0))); + MobilizedBody::Ball body4( + body3, + Transform(Vec3(0)), + fancyBody, + Transform(Vec3(0, 1, 0))); + MobilizedBody::Ball body5( + body4, + Transform(Vec3(0)), + someBody, + Transform(Vec3(0, 1, 0))); + + CableSpan path1( + cables, + body1, + Vec3(-2*Rad, 0, 0), // origin + body5, + Vec3(2*Rad, 0, 0)); // termination + + /* CableObstacle::ViaPoint p1(path1, body2, Rad * UnitVec3(1, 1, 0)); */ + + // obs4 + path1.adoptWrappingObstacle( + body3, + Transform(), + ContactGeometry::Sphere(Rad), + {0., 1., 0.}); + + // obs5 + path1.adoptWrappingObstacle( + body4, + Transform(), + ContactGeometry::Sphere(BiggerRad), + {-1., 0., -2.}); + + MyCableSpring cable1(forces, path1, 100., 3.5, 0.1); + + /* CableSpan path2( */ + /* cables, */ + /* body3, */ + /* 2 * Rad * UnitVec3(1, 1, 1), */ + /* Ground, */ + /* Vec3(-2.5, 1, 0)); */ + /* MyCableSpring cable2(forces, path2, 100., 2, 0.1); */ + + // obs1.setPathPreferencePoint(Vec3(2,3,4)); + // obs1.setDecorativeGeometry(DecorativeSphere(0.25).setOpacity(.5)); + + Visualizer viz(system); + viz.setShowFrameNumber(true); + system.addEventReporter(new Visualizer::Reporter(viz, 0.1 * 1. / 30)); + system.addEventReporter( + new ShowStuff(system, cable1, 0.1 * 0.1)); + // Initialize the system and s. + + system.realizeTopology(); + State s = system.getDefaultState(); + Random::Gaussian random; + for (int i = 0; i < s.getNQ(); ++i) + s.updQ()[i] = random.getValue(); + for (int i = 0; i < s.getNU(); ++i) + s.updU()[i] = 0.1 * random.getValue(); + + system.realize(s, Stage::Position); + viz.report(s); + cout << "path1 init length=" << path1.getLength(s) << endl; + /* cout << "path2 init length=" << path2.getLength(s) << endl; */ + cout << "Hit ENTER ..."; + getchar(); + + // Simulate it. + saveStates.clear(); + saveStates.reserve(2000); + + RungeKuttaMersonIntegrator integ(system); + // CPodesIntegrator integ(system); + integ.setAccuracy(1e-3); + // integ.setAccuracy(1e-6); + TimeStepper ts(system, integ); + ts.initialize(s); + ShowStuff::showHeading(cout); + + const Real finalTime = 1.; + const double startTime = realTime(); + ts.stepTo(finalTime); + cout << "DONE with " << finalTime << "s simulated in " + << realTime() - startTime << "s elapsed.\n"; + + while (true) { + cout << "Hit ENTER FOR REPLAY, Q to quit ..."; + const char ch = getchar(); + if (ch == 'q' || ch == 'Q') + break; + for (unsigned i = 0; i < saveStates.size(); ++i) + viz.report(saveStates[i]); + } + + } catch (const std::exception& e) { + cout << "EXCEPTION: " << e.what() << "\n"; + } +} diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp new file mode 100644 index 000000000..9bd957ecf --- /dev/null +++ b/examples/ExampleCableSpanSimple.cpp @@ -0,0 +1,417 @@ +#include "Simbody.h" +#include "simbody/internal/Wrapping.h" +#include +#include +using std::cout; +using std::endl; + +using namespace SimTK; + +int testJacobian( + std::function f, + int qDim, + Real delta, + Real eps, + std::ostream& os) +{ + int returnValue = 0; + for (size_t i = 0; i < qDim; ++i) { + for (Real d : std::array{delta, -delta}) { + const Vector q0(qDim, 0.); + Vector y0; + Matrix J0; + f(q0, y0, J0); + + if (J0.nrow() != y0.nrow()) { + throw std::runtime_error("incompatible row dimensions"); + } + + Vector q1(qDim, 0.); + q1[i] = d; + + Vector y1; + Matrix J1; + f(q1, y1, J1); + + const Vector dy = (y1 - y0) / d; + const Vector dyExpected = (J0 * q1) / d; + + const Vector e = dy - dyExpected; + const Real diff = e.norm(); + if (e.norm() > eps) { + os << "FAILED Perturbation test " << i << "\n"; + returnValue = 1; + } else { + os << "PASSED Perturbation test " << i << "\n"; + } + os << " q1 = " << q1 << "\n"; + os << " dy = " << dy << "\n"; + os << " dyExp = " << dyExpected << "\n"; + os << " e = " << e << ".norm() = " << e.norm() << "\n"; + } + } + return returnValue; +} + +// This force element implements an elastic cable of a given nominal length, +// and a stiffness k that generates a k*x force opposing stretch beyond +// the slack length. There is also a dissipation term (k*x)*c*xdot. We keep +// track of dissipated power here so we can use conservation of energy to check +// that the cable and force element aren't obviously broken. +class MyCableSpringImpl : public Force::Custom::Implementation +{ +public: + MyCableSpringImpl( + const GeneralForceSubsystem& forces, + const CableSpan& cable, + Real stiffness, + Real nominal, + Real damping) : + forces(forces), + m_Cable(cable), k(stiffness), x0(nominal), c(damping) + { + assert(stiffness >= 0 && nominal >= 0 && damping >= 0); + } + + const CableSpan& getCable() const + { + return m_Cable; + } + + // Must be at stage Velocity. Evalutes tension if necessary. + Real getTension(const State& s) const + { + ensureTensionCalculated(s); + return Value::downcast(forces.getCacheEntry(s, tensionx)); + } + + // Must be at stage Velocity. + Real getPowerDissipation(const State& s) const + { + const Real stretch = calcStretch(s); + if (stretch == 0) + return 0; + const Real rate = m_Cable.getLengthDot(s); + return k * stretch * std::max(c * rate, -1.) * rate; + } + + // This integral is always available. + Real getDissipatedEnergy(const State& s) const + { + return forces.getZ(s)[workx]; + } + + //-------------------------------------------------------------------------- + // Custom force virtuals + + // Ask the cable to apply body forces given the tension calculated here. + void calcForce( + const State& s, + Vector_& bodyForces, + Vector_& particleForces, + Vector& mobilityForces) const override + { + m_Cable.applyBodyForces(s, getTension(s), bodyForces); + } + + // Return the potential energy currently stored by the stretch of the cable. + Real calcPotentialEnergy(const State& s) const override + { + const Real stretch = calcStretch(s); + if (stretch == 0) + return 0; + return k * square(stretch) / 2; + } + + // Allocate the s variable for tracking dissipated energy, and a + // cache entry to hold the calculated tension. + void realizeTopology(State& s) const override + { + Vector initWork(1, 0.); + workx = forces.allocateZ(s, initWork); + tensionx = forces.allocateLazyCacheEntry( + s, + Stage::Velocity, + new Value(NaN)); + } + + // Report power dissipation as the derivative for the work variable. + void realizeAcceleration(const State& s) const override + { + Real& workDot = forces.updZDot(s)[workx]; + workDot = getPowerDissipation(s); + } + //-------------------------------------------------------------------------- + +private: + // Return the amount by which the cable is stretched beyond its nominal + // length or zero if the cable is slack. Must be at stage Position. + Real calcStretch(const State& s) const + { + const Real stretch = m_Cable.getLength(s) - x0; + return std::max(stretch, 0.); + } + + // Must be at stage Velocity to calculate tension. + Real calcTension(const State& s) const + { + const Real stretch = calcStretch(s); + if (stretch == 0) + return 0; + const Real rate = m_Cable.getLengthDot(s); + if (c * rate < -1) + cout << "c*rate=" << c * rate << "; limited to -1\n"; + const Real tension = k * stretch * (1 + std::max(c * rate, -1.)); + return tension; + } + + // If s is at stage Velocity, we can calculate and store tension + // in the cache if it hasn't already been calculated. + void ensureTensionCalculated(const State& s) const + { + if (forces.isCacheValueRealized(s, tensionx)) + return; + Value::updDowncast(forces.updCacheEntry(s, tensionx)) = + calcTension(s); + forces.markCacheValueRealized(s, tensionx); + } + + const GeneralForceSubsystem& forces; + CableSpan m_Cable; + Real k, x0, c; + mutable ZIndex workx; + mutable CacheEntryIndex tensionx; +}; + +// A nice handle to hide most of the cable spring implementation. This defines +// a user's API. +class MyCableSpring : public Force::Custom +{ +public: + MyCableSpring( + GeneralForceSubsystem& forces, + const CableSpan& cable, + Real stiffness, + Real nominal, + Real damping) : + Force::Custom( + forces, + new MyCableSpringImpl(forces, cable, stiffness, nominal, damping)) + {} + + // Expose some useful methods. + const CableSpan& getCable() const + { + return getImpl().getCable(); + } + Real getTension(const State& s) const + { + return getImpl().getTension(s); + } + Real getPowerDissipation(const State& s) const + { + return getImpl().getPowerDissipation(s); + } + Real getDissipatedEnergy(const State& s) const + { + return getImpl().getDissipatedEnergy(s); + } + +private: + const MyCableSpringImpl& getImpl() const + { + return dynamic_cast(getImplementation()); + } +}; + +// This gets called periodically to dump out interesting things about +// the cables and the system as a whole. It also saves states so that we +// can play back at the end. +static Array_ saveStates; +class ShowStuff : public PeriodicEventReporter +{ +public: + ShowStuff( + const MultibodySystem& mbs, + const MyCableSpring& cable1, + const MyCableSpring& cable2, + Real interval) : + PeriodicEventReporter(interval), + mbs(mbs), cable1(cable1), cable2(cable2) + {} + + static void showHeading(std::ostream& o) + { + printf( + "%8s %10s %10s %10s %10s %10s %10s %10s %10s %12s\n", + "time", + "length", + "rate", + "integ-rate", + "unitpow", + "tension", + "disswork", + "KE", + "PE", + "KE+PE-W"); + } + + /** This is the implementation of the EventReporter virtual. **/ + void handleEvent(const State& s) const override + { + const CableSpan& path1 = cable1.getCable(); + const CableSpan& path2 = cable2.getCable(); + printf( + "%8g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g CPU=%g\n", + s.getTime(), + path1.getLength(s), + path1.getLengthDot(s), + path1.calcCablePower(s, 1), // unit power + cable1.getTension(s), + cable1.getDissipatedEnergy(s), + cpuTime()); + printf( + "%8s %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g " + "%12.6g\n", + "", + path2.getLength(s), + path2.getLengthDot(s), + path2.calcCablePower(s, 1), // unit power + cable2.getTension(s), + cable2.getDissipatedEnergy(s), + mbs.calcKineticEnergy(s), + mbs.calcPotentialEnergy(s), + mbs.calcEnergy(s) + cable1.getDissipatedEnergy(s) + + cable2.getDissipatedEnergy(s)); + saveStates.push_back(s); + } + +private: + const MultibodySystem& mbs; + MyCableSpring cable1, cable2; +}; + +int main() +{ + try { + // Create the system. + MultibodySystem system; + SimbodyMatterSubsystem matter(system); + CableSubsystem cables(system); + GeneralForceSubsystem forces(system); + + system.setUseUniformBackground(true); // no ground plane in display + + Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0)); + // Force::GlobalDamper(forces, matter, 5); + + MobilizedBody Ground = matter.Ground(); + + CableSpan path1( + cables, + Ground, + Vec3(-5., 0.1, 0.), + Ground, + Vec3(10., 0.0, 0.1)); + + Body::Rigid ballBody(MassProperties(1.0, Vec3(0), Inertia(1))); + const Real Rad = 1.0; + + Vec3 offset = {-1., 0., 0.}; + Vec3 arm = {0.25, 0., 0.}; + + /* UnitVec3 axis0(Vec3{1., 1., 1.}); */ + /* Real angle0 = 0.5; */ + /* UnitVec3 axis1(Vec3{1., -2., 3.}); */ + /* Real angle1 = 0.22; */ + MobilizedBody::Ball ball( + Ground, + Transform(Vec3{offset + arm}), + ballBody, + Transform(Vec3{offset - arm})); + + // obs4 + path1.adoptWrappingObstacle( + ball, + Transform(), + ContactGeometry::Sphere(Rad), + {0., 1., 0.}); + + Body::Rigid ball2Body(MassProperties(1.0, Vec3(0), Inertia(1))); + const Real Rad2 = 1.5; + Vec3 offset2 = {5., 0., 0.}; + Vec3 arm2 = {0.25, 0., 0.}; + + MobilizedBody::Ball ball2( + Ground, + Transform(Vec3{offset2 + arm2}), + ball2Body, + Transform(Vec3{-arm2})); + + // obs4 + path1.adoptWrappingObstacle( + ball2, + Transform(), + ContactGeometry::Ellipsoid({Rad2, Rad2 * 2., Rad2 * 0.9}), + /* ContactGeometry::Sphere(Rad2), */ + {0., 1., 0.}); + + MyCableSpring cable1(forces, path1, 100., 3.5, 0.1); + + Visualizer viz(system); + viz.setShowFrameNumber(true); + system.addEventReporter(new Visualizer::Reporter(viz, 0.1 * 1. / 30)); + // Initialize the system and s. + + system.realizeTopology(); + State s = system.getDefaultState(); + + Real v = 0.; + bool continuous = false; + Random::Gaussian random; + Real phi = 0.; + while (true) { + system.realize(s, Stage::Position); + viz.report(s); + const Real l = path1.getLength(s); + cout << "path1 init length=" << l << endl; + + std::array axes = {NormalAxis, BinormalAxis}; + std::function f = + [&](const Vector& q, Vector& y, Matrix& J) { + path1.applyCorrection(s, q); + path1.calcPathErrorJacobian(s, y, J); + }; + int qDim = path1.countActive(s) * 4; + + std::ostringstream oss; + testJacobian(f, qDim, 1e-4, 1e-3, oss); + std::cout << oss.str() << "\n"; + + { + /* Random::Gaussian random; */ + v += random.getValue() * 1e-3; + v = std::max(-1e-1, v); + v = std::min(1e-1, v); + } + + phi += 0.01; + for (int i = 0; i < s.getNQ(); ++i) { + /* Random::Gaussian random; */ + s.updQ()[i] = sin(phi * static_cast(i) + random.getValue()*1e-3); + } + + if (continuous) { + sleepInSec(0.1); + } else { + cout << "Hit ENTER ..., or q\n"; + const char ch = getchar(); + if (ch == 'Q' || ch == 'q') + break; + continuous = ch == 'c'; + } + } + } catch (const std::exception& e) { + cout << "EXCEPTION: " << e.what() << "\n"; + } +} diff --git a/install-simbody b/install-simbody new file mode 100755 index 000000000..3106f97c0 --- /dev/null +++ b/install-simbody @@ -0,0 +1,25 @@ +#!/usr/bin/bash +set -xeuo pipefail + +WORKSPACE=$(dirname $(dirname $(realpath "$0"))) +echo $WORKSPACE + +if [ ! -d "$WORKSPACE/simbody-build" ]; then + mkdir -p "$WORKSPACE/simbody-build" + mkdir -p "$WORKSPACE/simbody-install" + + env -C $WORKSPACE cmake -B "simbody-build" -S "simbody" -DCMAKE_INSTALL_PREFIX="simbody-install" -DCMAKE_EXPORT_COMPILE_COMMANDS="ON" -DCMAKE_BUILD_TYPE="RelWithDebInfo" +fi + +env -C $WORKSPACE cmake --build "simbody-build" -j$(nproc) --target "install" + +ln -sf "$WORKSPACE/simbody-build/compile_commands.json" "$WORKSPACE/simbody/compile_commands.json" + +# env -C "$WORKSPACE/simbody-build" ctest --parallel -j$(nproc) --output-on-failure + +LD_LIBRARY_PATH="$WORKSPACE/simbody-install/lib" +# env -C "$WORKSPACE/simbody-build" ./ExampleCableTest +# env -C "$WORKSPACE/simbody-build" ./ExampleCablePath +env -C "$WORKSPACE/simbody-build" ./ExampleCableSpan +# env -C "$WORKSPACE/simbody-build" ./ExampleCableSpanSimple +# env -C "$WORKSPACE/simbody-build" gdb --args ExampleCableSpan