From c341560eaa989336634a3b9e3c71a0917bd8f8e6 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 15 Apr 2024 11:38:12 +0200 Subject: [PATCH 001/127] add install script for building and testing --- install-simbody | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100755 install-simbody diff --git a/install-simbody b/install-simbody new file mode 100755 index 000000000..07266b398 --- /dev/null +++ b/install-simbody @@ -0,0 +1,12 @@ +#!/usr/bin/bash +set -xeuo pipefail + +WORKSPACE=$(dirname $(dirname $(realpath "$0"))) +echo $WORKSPACE + +env -C $WORKSPACE cmake -B "simbody-build" -S "simbody" -DCMAKE_INSTALL_PREFIX="simbody-install" -DCMAKE_EXPORT_COMPILE_COMMANDS="ON" +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 From 14c7bb8072ec1c239a997615fa2dec735f3a2725 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 10:12:09 +0200 Subject: [PATCH 002/127] wip: Sketching the WrappingPathSubsystem --- .../simbody/internal/WrappingPathSubsystem.h | 324 ++++++++++++++++++ 1 file changed, 324 insertions(+) create mode 100644 Simbody/include/simbody/internal/WrappingPathSubsystem.h diff --git a/Simbody/include/simbody/internal/WrappingPathSubsystem.h b/Simbody/include/simbody/internal/WrappingPathSubsystem.h new file mode 100644 index 000000000..2838e2ca6 --- /dev/null +++ b/Simbody/include/simbody/internal/WrappingPathSubsystem.h @@ -0,0 +1,324 @@ +#ifndef SimTK_SIMBODY_WRAPPING_PATH_SUBSYSTEM_H_ +#define SimTK_SIMBODY_WRAPPING_PATH_SUBSYSTEM_H_ + +#include "SimTKmath.h" +#include "simbody/internal/MobilizedBody.h" +#include "simbody/internal/common.h" +#include "simmath/internal/ContactGeometry.h" +#include + +namespace SimTK +{ + +SimTK_DEFINE_UNIQUE_INDEX_TYPE(WrappingPathIndex); +SimTK_DEFINE_UNIQUE_INDEX_TYPE(WrapObstacleIndex); + +class MultibodySystem; +class WrappingPathSubsystem; +class WrapObstacle; + +// TODO where to put decorations? + +// WrapObstacle: PIMPL - not copy +// - only one +// - Impl: +// - has virtual methods +// +// AnalyticWrapObstacle: WrapObstacle +// - has cache +// ImplicitWrapObstacle: WrapObstacle +// - has cache +// +// Surface: PIMPL +// - ContactGeometry +// - BodyIndex +// - Transform + +//============================================================================== +// SURFACE +//============================================================================== +// Binds {ContactGeometry, Body, OffsetFrame} +// Cheap to copy, reuseable in the model. +class SimTK_SIMBODY_EXPORT Surface +{ +private: + Surface() = default; +public: + ~Surface() = default; + Surface(Surface&&) noexcept = default; + Surface& operator=(Surface&&) noexcept = default; + Surface(const Surface&) = default; + Surface& operator=(const Surface&) = default; + + Surface(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry); + + const ContactGeometry& getGeometry() const; + const Transform& calcSurfaceToGroundTransform(const State& state) const; + +private: + + //-------------------------------------------------------------------------- + class Impl; + explicit Surface(std::shared_ptr impl); + std::shared_ptr impl = nullptr; +}; + +class Surface::Impl { + typedef Surface::Impl Super; +private: + Impl() = default; +public: + ~Impl() = default; + Impl(Impl&&) noexcept = default; + Impl& operator=(Impl&&) noexcept = default; + Impl(const Impl&) = default; + Impl& operator=(const Impl&) = default; + + Impl(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry); + + const ContactGeometry& getGeometry() const; + const Transform& calcSurfaceToGroundTransform(const State& state) const; + +private: + ContactGeometry geometry; + MobilizedBody mobod; + Transform offset; +}; + +//============================================================================== +// OBSTACLE +//============================================================================== +// Although cheap to copy, we cannot hand them out because they have a cache entry associated with them. +// The surface they hold a pointer to can be reused in the model. +class SimTK_SIMBODY_EXPORT WrapObstacle +{ +public: + using FrenetFrame = ContactGeometry::FrenetFrame; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; + using BoundaryFrameVariation = WrapObstacle::Correction; + +private: + WrapObstacle() = default; + +public: + WrapObstacle(const WrapObstacle& source) = default; + WrapObstacle& operator=(const WrapObstacle& source) = default; + ~WrapObstacle() = default; + + explicit WrapObstacle(Surface surface); + + const FrenetFrame& getKP(const State& state) const; + const FrenetFrame& getKQ(const State& state) const; + + const Variation& getDKP(const State& state) const; + const Variation& getDKQ(const State& state) const; + + double getLength(const State& state) const; + + size_t writeGeodesicPoints(const State& state, std::vector points) const; + + // Solution previously commputed, all in local surface frame coordinates. + struct WarmStartInfo + { + FrenetFrame KP {}; + FrenetFrame KQ {}; + + Real length = NaN; + + BoundaryFrameVariation dKP {}; + BoundaryFrameVariation dKQ {}; + + std::vector points {}; + }; + + // Ground frame solution. + struct PosInfo + { + FrenetFrame KP {}; + FrenetFrame KQ {}; + + Real length = NaN; + + BoundaryFrameVariation dKP {}; + BoundaryFrameVariation dKQ {}; + }; + + size_t writeGeodesicPoints(const State& state, std::vector& points) const; + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizeInstance(const State& state) const; + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void realizeAcceleration(const State& state) const; + + const PosInfo& getPosInfo(const State& state) const; + PosInfo& updPosInfo(const State& state) const; + +private: + const WarmStartInfo& getWarmStartInfo(const State& state) const; + WarmStartInfo& updWarmStartInfo(const State& state) const; + + void calcPosInfo(PosInfo& posInfo) const; + + void invalidateTopology() + { if (subsystem) subsystem->invalidateSubsystemTopologyCache(); } + + // Required for accessing the discrete variable? + std::shared_ptr subsystem = nullptr; + + std::vector obstacles {}; + + // TOPOLOGY CACHE (set during realizeTopology()) + DiscreteVariableIndex warmStartInfoIx; + DiscreteVariableIndex posInfoIx; + DiscreteVariableIndex velInfoIx; +}; + +//============================================================================== +// PATH +//============================================================================== +class SimTK_SIMBODY_EXPORT WrappingPath +{ +public: + WrappingPath( + WrappingPathSubsystem& subsystem, + const MobilizedBody& originBody, + const Vec3& defaultOriginPoint, + const MobilizedBody& terminationBody, + const Vec3& defaultTerminationPoint); + + int getNumObstacles() const; + const WrapObstacle& getObstacle(WrapObstacleIndex obstacleIx) const; + WrapObstacleIndex adoptObstacle(WrapObstacle); + + Real getLength(const State& state) const; + Real getLengthDot(const State& state) const; + void applyBodyForces( const State& state, Real tension, Vector_& bodyForcesInG) const; + Real calcPower(const State& state, Real tension) const; + + WrapObstacle& setDecorativeGeometry(const DecorativeGeometry& viz); + WrapObstacle& setNearPoint(const Vec3& point); + WrapObstacle& setContactPointHints( + const Vec3& startHint, + const Vec3& endHint); + + class Impl; + +private: + std::shared_ptr impl = nullptr; +}; + +//============================================================================== +// SUBSYSTEM +//============================================================================== +class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem +{ +public: + WrappingPathSubsystem(); + explicit WrappingPathSubsystem(MultibodySystem&); + + int getNumCablePaths() const; + const WrappingPath& getPath(WrappingPathIndex idx) const; + WrappingPath& updPath(WrappingPathIndex idx); + + /** @cond **/ // Hide from Doxygen. + SimTK_PIMPL_DOWNCAST(WrappingPathSubsystem, Subsystem); + class Impl; + Impl& updImpl(); + const Impl& getImpl() const; + /** @endcond **/ +}; + +//============================================================================== +// PATH :: IMPL +//============================================================================== + +class WrappingPath::Impl { +public: + + struct LineSegment + { + UnitVec3 d {NaN, NaN, NaN}; + Real l = NaN; + }; + + struct PosInfo + { + Vec3 xO {NaN, NaN, NaN}; + Vec3 xI {NaN, NaN, NaN}; + + Real l = NaN; + + std::vector lines; + + size_t loopIter = 0; + }; + + struct VizInfo + { + std::vector> points {}; + }; + + struct VelInfo + { + Real lDot = NaN; + }; + + int getNumObstacles() const {return obstacles.size();} + const WrapObstacle& getObstacle(WrapObstacleIndex ix) const; + WrapObstacleIndex adoptObstacle(WrapObstacle& obstacle); + + void calcPath(State& state, bool preventLiftOff = false) const; + void calcInitPath(State& state, std::function pointHints); + + Real getPathError(const State& state) const; + Real getLength(const State& state) const; + Real getLengthDot(const State& state) const; + void applyBodyForces(const State& state, Real tension, Vector_& bodyForcesInG) const; + Real calcCablePower(const State& state, Real tension) const; + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizeInstance(const State& state) const; + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void realizeAcceleration(const State& state) const; + /* void calcEventTriggerInfo (const State&, Array_&) const; */ + /* void handleEvents (State&, Event::Cause, const Array_& eventIds, const HandleEventsOptions& options, HandleEventsResults& results) const; */ + + const PosInfo& getPosInfo(const State& state) const; + const VelInfo& getVelInfo(const State& state) const; + const VizInfo& getVizInfo(const State& state) const; + + PosInfo& updPosEntry(const State& state) const; + VelInfo& updVelEntry(const State& state) const; + VizInfo& updVizInfo(const State& state) const; + + void calcPosInfo(PosInfo& posInfo) const; + void calcVelInfo(const PosInfo& posInfo, VelInfo& velInfo) const; + void calcVizInfo(const PosInfo& posInfo, VizInfo& vizInfo) const; + + // Be sure to call this whenever you make a topology-level change to + // the cable definition, like adding an obstacle or modifying one in + // a significant way. + void invalidateTopology() + { if (subsystem) subsystem->invalidateSubsystemTopologyCache(); } + +private: +friend class WrappingPath; + std::shared_ptr subsystem = nullptr; + WrappingPathIndex index {}; + + std::vector obstacles {}; + + // TOPOLOGY CACHE (set during realizeTopology()) + DiscreteVariableIndex posInfoIx; + DiscreteVariableIndex velInfoIx; + DiscreteVariableIndex vizInfoIx; +}; + +} // namespace SimTK + +#endif From 3a4216db3d5105e43efeabc01c4aa9ee761ea71c Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 10:41:00 +0200 Subject: [PATCH 003/127] rename to Wrapping.h --- .../simbody/internal/{WrappingPathSubsystem.h => Wrapping.h} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename Simbody/include/simbody/internal/{WrappingPathSubsystem.h => Wrapping.h} (100%) diff --git a/Simbody/include/simbody/internal/WrappingPathSubsystem.h b/Simbody/include/simbody/internal/Wrapping.h similarity index 100% rename from Simbody/include/simbody/internal/WrappingPathSubsystem.h rename to Simbody/include/simbody/internal/Wrapping.h From 71ff5fa289b7ff1011c49b1e88b4348b4c86a2b0 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 10:55:58 +0200 Subject: [PATCH 004/127] wip: add cpp file --- Simbody/include/simbody/internal/Wrapping.cpp | 16 ++++ Simbody/include/simbody/internal/Wrapping.h | 73 ++++++++----------- 2 files changed, 47 insertions(+), 42 deletions(-) create mode 100644 Simbody/include/simbody/internal/Wrapping.cpp diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp new file mode 100644 index 000000000..5f9fdcbd1 --- /dev/null +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -0,0 +1,16 @@ +#include "SimTKmath.h" +#include "Wrapping.h" + +using namespace SimTK; + +//============================================================================== +// SURFACE +//============================================================================== + +Surface::Surface(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry) +{ + +} + +const ContactGeometry& getGeometry() const; +const Transform& calcSurfaceToGroundTransform(const State& state) const; diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 2838e2ca6..3be6ab3b4 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -6,6 +6,7 @@ #include "simbody/internal/common.h" #include "simmath/internal/ContactGeometry.h" #include +#include namespace SimTK { @@ -17,26 +18,37 @@ class MultibodySystem; class WrappingPathSubsystem; class WrapObstacle; -// TODO where to put decorations? - -// WrapObstacle: PIMPL - not copy -// - only one -// - Impl: -// - has virtual methods -// -// AnalyticWrapObstacle: WrapObstacle -// - has cache -// ImplicitWrapObstacle: WrapObstacle -// - has cache -// -// Surface: PIMPL -// - ContactGeometry -// - BodyIndex -// - Transform - //============================================================================== // SURFACE //============================================================================== +class SurfaceImpl { + +private: + SurfaceImpl() = default; +public: + ~SurfaceImpl() = default; + SurfaceImpl(SurfaceImpl&&) noexcept = default; + SurfaceImpl& operator=(SurfaceImpl&&) noexcept = default; + SurfaceImpl(const SurfaceImpl&) = default; + SurfaceImpl& operator=(const SurfaceImpl&) = default; + + SurfaceImpl(MobilizedBody mobod, Transform X_BS, ContactGeometry geometry) : + m_Mobod(std::move(mobod)), + m_Offset(std::move(X_BS)), + m_Geometry(geometry) {} + + const ContactGeometry& getGeometry() const {return m_Geometry;} + Transform calcSurfaceToGroundTransform(const State& state) const + { throw std::runtime_error("check transform order"); + return + m_Mobod.getBodyTransform(state).compose(m_Offset);} + +private: + MobilizedBody m_Mobod; + Transform m_Offset; + ContactGeometry m_Geometry; +}; + // Binds {ContactGeometry, Body, OffsetFrame} // Cheap to copy, reuseable in the model. class SimTK_SIMBODY_EXPORT Surface @@ -58,31 +70,8 @@ class SimTK_SIMBODY_EXPORT Surface private: //-------------------------------------------------------------------------- - class Impl; - explicit Surface(std::shared_ptr impl); - std::shared_ptr impl = nullptr; -}; - -class Surface::Impl { - typedef Surface::Impl Super; -private: - Impl() = default; -public: - ~Impl() = default; - Impl(Impl&&) noexcept = default; - Impl& operator=(Impl&&) noexcept = default; - Impl(const Impl&) = default; - Impl& operator=(const Impl&) = default; - - Impl(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry); - - const ContactGeometry& getGeometry() const; - const Transform& calcSurfaceToGroundTransform(const State& state) const; - -private: - ContactGeometry geometry; - MobilizedBody mobod; - Transform offset; + explicit Surface(std::shared_ptr impl); + std::shared_ptr impl = nullptr; }; //============================================================================== From 6e73259fff6e369151a9f7f24e9b4274422d77d4 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 11:08:40 +0200 Subject: [PATCH 005/127] cleanup Wrapping.h design --- Simbody/include/simbody/internal/Wrapping.h | 34 ++++++--------------- 1 file changed, 9 insertions(+), 25 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 3be6ab3b4..28e59e9db 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -40,8 +40,7 @@ class SurfaceImpl { const ContactGeometry& getGeometry() const {return m_Geometry;} Transform calcSurfaceToGroundTransform(const State& state) const { throw std::runtime_error("check transform order"); - return - m_Mobod.getBodyTransform(state).compose(m_Offset);} + return m_Mobod.getBodyTransform(state).compose(m_Offset);} private: MobilizedBody m_Mobod; @@ -62,15 +61,13 @@ class SimTK_SIMBODY_EXPORT Surface Surface(const Surface&) = default; Surface& operator=(const Surface&) = default; - Surface(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry); + Surface(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry) + : impl(std::make_shared(mobod, X_BS, geometry)) {} - const ContactGeometry& getGeometry() const; - const Transform& calcSurfaceToGroundTransform(const State& state) const; + const ContactGeometry& getGeometry() const {return impl->getGeometry();} + Transform calcSurfaceToGroundTransform(const State& state) const {return impl->calcSurfaceToGroundTransform(state);} private: - - //-------------------------------------------------------------------------- - explicit Surface(std::shared_ptr impl); std::shared_ptr impl = nullptr; }; @@ -97,16 +94,6 @@ class SimTK_SIMBODY_EXPORT WrapObstacle explicit WrapObstacle(Surface surface); - const FrenetFrame& getKP(const State& state) const; - const FrenetFrame& getKQ(const State& state) const; - - const Variation& getDKP(const State& state) const; - const Variation& getDKQ(const State& state) const; - - double getLength(const State& state) const; - - size_t writeGeodesicPoints(const State& state, std::vector points) const; - // Solution previously commputed, all in local surface frame coordinates. struct WarmStartInfo { @@ -133,27 +120,25 @@ class SimTK_SIMBODY_EXPORT WrapObstacle BoundaryFrameVariation dKQ {}; }; - size_t writeGeodesicPoints(const State& state, std::vector& points) const; - // Allocate state variables and cache entries. void realizeTopology(State& state); void realizeInstance(const State& state) const; void realizePosition(const State& state) const; void realizeVelocity(const State& state) const; void realizeAcceleration(const State& state) const; + void invalidateTopology(); const PosInfo& getPosInfo(const State& state) const; PosInfo& updPosInfo(const State& state) const; + size_t writeGeodesicPoints(const State& state, std::vector points) const; + void applyCorrection(const State& state) const; + private: const WarmStartInfo& getWarmStartInfo(const State& state) const; WarmStartInfo& updWarmStartInfo(const State& state) const; - void calcPosInfo(PosInfo& posInfo) const; - void invalidateTopology() - { if (subsystem) subsystem->invalidateSubsystemTopologyCache(); } - // Required for accessing the discrete variable? std::shared_ptr subsystem = nullptr; @@ -162,7 +147,6 @@ class SimTK_SIMBODY_EXPORT WrapObstacle // TOPOLOGY CACHE (set during realizeTopology()) DiscreteVariableIndex warmStartInfoIx; DiscreteVariableIndex posInfoIx; - DiscreteVariableIndex velInfoIx; }; //============================================================================== From f140921707745950ba4074c65a59c99029ac07c1 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 11:21:55 +0200 Subject: [PATCH 006/127] add WrappingSubsystem::Impl header --- Simbody/include/simbody/internal/Wrapping.h | 296 +++++++++++++------- 1 file changed, 195 insertions(+), 101 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 28e59e9db..50d49e989 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -3,8 +3,11 @@ #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 @@ -149,6 +152,27 @@ class SimTK_SIMBODY_EXPORT WrapObstacle DiscreteVariableIndex posInfoIx; }; +//============================================================================== +// SUBSYSTEM +//============================================================================== +class WrappingPath; + +class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem +{ +public: + WrappingPathSubsystem(); + explicit WrappingPathSubsystem(MultibodySystem&); + + int getNumPaths() const; + const WrappingPath& getPath(WrappingPathIndex idx) const; + WrappingPath& updPath(WrappingPathIndex idx); + + SimTK_PIMPL_DOWNCAST(WrappingPathSubsystem, Subsystem); + class Impl; + Impl& updImpl(); + const Impl& getImpl() const; +}; + //============================================================================== // PATH //============================================================================== @@ -180,116 +204,186 @@ class SimTK_SIMBODY_EXPORT WrappingPath class Impl; private: - std::shared_ptr impl = nullptr; -}; + friend WrappingPathSubsystem::Impl; -//============================================================================== -// SUBSYSTEM -//============================================================================== -class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem -{ -public: - WrappingPathSubsystem(); - explicit WrappingPathSubsystem(MultibodySystem&); - - int getNumCablePaths() const; - const WrappingPath& getPath(WrappingPathIndex idx) const; - WrappingPath& updPath(WrappingPathIndex idx); - - /** @cond **/ // Hide from Doxygen. - SimTK_PIMPL_DOWNCAST(WrappingPathSubsystem, Subsystem); - class Impl; - Impl& updImpl(); const Impl& getImpl() const; - /** @endcond **/ + Impl& updImpl(); + + std::shared_ptr impl = nullptr; }; //============================================================================== // PATH :: IMPL //============================================================================== - class WrappingPath::Impl { -public: - - struct LineSegment - { - UnitVec3 d {NaN, NaN, NaN}; - Real l = NaN; - }; - - struct PosInfo - { - Vec3 xO {NaN, NaN, NaN}; - Vec3 xI {NaN, NaN, NaN}; - - Real l = NaN; - - std::vector lines; - - size_t loopIter = 0; - }; - - struct VizInfo - { - std::vector> points {}; - }; - - struct VelInfo - { - Real lDot = NaN; - }; - - int getNumObstacles() const {return obstacles.size();} - const WrapObstacle& getObstacle(WrapObstacleIndex ix) const; - WrapObstacleIndex adoptObstacle(WrapObstacle& obstacle); - - void calcPath(State& state, bool preventLiftOff = false) const; - void calcInitPath(State& state, std::function pointHints); - - Real getPathError(const State& state) const; - Real getLength(const State& state) const; - Real getLengthDot(const State& state) const; - void applyBodyForces(const State& state, Real tension, Vector_& bodyForcesInG) const; - Real calcCablePower(const State& state, Real tension) const; - - // Allocate state variables and cache entries. - void realizeTopology(State& state); - void realizeInstance(const State& state) const; - void realizePosition(const State& state) const; - void realizeVelocity(const State& state) const; - void realizeAcceleration(const State& state) const; - /* void calcEventTriggerInfo (const State&, Array_&) const; */ - /* void handleEvents (State&, Event::Cause, const Array_& eventIds, const HandleEventsOptions& options, HandleEventsResults& results) const; */ - - const PosInfo& getPosInfo(const State& state) const; - const VelInfo& getVelInfo(const State& state) const; - const VizInfo& getVizInfo(const State& state) const; - - PosInfo& updPosEntry(const State& state) const; - VelInfo& updVelEntry(const State& state) const; - VizInfo& updVizInfo(const State& state) const; - - void calcPosInfo(PosInfo& posInfo) const; - void calcVelInfo(const PosInfo& posInfo, VelInfo& velInfo) const; - void calcVizInfo(const PosInfo& posInfo, VizInfo& vizInfo) const; - - // Be sure to call this whenever you make a topology-level change to - // the cable definition, like adding an obstacle or modifying one in - // a significant way. - void invalidateTopology() - { if (subsystem) subsystem->invalidateSubsystemTopologyCache(); } - -private: -friend class WrappingPath; - std::shared_ptr subsystem = nullptr; - WrappingPathIndex index {}; - - std::vector obstacles {}; + public: + + struct LineSegment + { + UnitVec3 d {NaN, NaN, NaN}; + Real l = NaN; + }; + + struct PosInfo + { + Vec3 xO {NaN, NaN, NaN}; + Vec3 xI {NaN, NaN, NaN}; + + Real l = NaN; + + std::vector lines; + + size_t loopIter = 0; + }; + + struct VizInfo + { + std::vector> points {}; + }; + + struct VelInfo + { + Real lDot = NaN; + }; + + int getNumObstacles() const {return obstacles.size();} + const WrapObstacle& getObstacle(WrapObstacleIndex ix) const; + WrapObstacleIndex adoptObstacle(WrapObstacle& obstacle); + + void calcPath(State& state, bool preventLiftOff = false) const; + void calcInitPath(State& state, std::function pointHints); + + Real getPathError(const State& state) const; + Real getLength(const State& state) const; + Real getLengthDot(const State& state) const; + void applyBodyForces(const State& state, Real tension, Vector_& bodyForcesInG) const; + Real calcCablePower(const State& state, Real tension) const; + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizeInstance(const State& state) const; + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void realizeAcceleration(const State& state) const; + /* void calcEventTriggerInfo (const State&, Array_&) const; */ + /* void handleEvents (State&, Event::Cause, const Array_& eventIds, const HandleEventsOptions& options, HandleEventsResults& results) const; */ + + const PosInfo& getPosInfo(const State& state) const; + const VelInfo& getVelInfo(const State& state) const; + const VizInfo& getVizInfo(const State& state) const; + + PosInfo& updPosEntry(const State& state) const; + VelInfo& updVelEntry(const State& state) const; + VizInfo& updVizInfo(const State& state) const; + + void calcPosInfo(PosInfo& posInfo) const; + void calcVelInfo(const PosInfo& posInfo, VelInfo& velInfo) const; + void calcVizInfo(const PosInfo& posInfo, VizInfo& vizInfo) const; + + // Be sure to call this whenever you make a topology-level change to + // the cable definition, like adding an obstacle or modifying one in + // a significant way. + void invalidateTopology() + { if (subsystem) subsystem->invalidateSubsystemTopologyCache(); } + + private: + friend class WrappingPath; + std::shared_ptr subsystem = nullptr; + WrappingPathIndex index {}; + + std::vector obstacles {}; + + // TOPOLOGY CACHE (set during realizeTopology()) + DiscreteVariableIndex posInfoIx; + DiscreteVariableIndex velInfoIx; + DiscreteVariableIndex vizInfoIx; +}; - // TOPOLOGY CACHE (set during realizeTopology()) - DiscreteVariableIndex posInfoIx; - DiscreteVariableIndex velInfoIx; - DiscreteVariableIndex vizInfoIx; +//============================================================================== +// SUBSYSTEM :: IMPL +//============================================================================== +class WrappingPathSubsystem::Impl : public Subsystem::Guts { + public: + Impl() {} + ~Impl() {} + Impl* cloneImpl() const override + { return new Impl(*this); } + + int getNumPaths() const {return paths.size();} + + const WrappingPath& getCablePath(WrappingPathIndex index) const + { return paths[index]; } + + WrappingPath& updCablePath(WrappingPathIndex index) + { return paths[index]; } + + // Add a cable path to the list, bumping the reference count. + WrappingPathIndex adoptCablePath(WrappingPath& path) { + paths.push_back(path); + return WrappingPathIndex(paths.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(); } + + // 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); + + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + WrappingPath& path = wThis->updCablePath(ix); + path.updImpl().realizeTopology(state); + } + + return 0; + } + + int realizeSubsystemInstanceImpl(const State& state) const override { + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + const WrappingPath& path = getCablePath(ix); + path.getImpl().realizeInstance(state); + } + return 0; + } + + int realizeSubsystemPositionImpl(const State& state) const override { + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + const WrappingPath& path = getCablePath(ix); + path.getImpl().realizePosition(state); + } + return 0; + } + + int realizeSubsystemVelocityImpl(const State& state) const override { + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + const WrappingPath& path = getCablePath(ix); + path.getImpl().realizeVelocity(state); + } + return 0; + } + + + int realizeSubsystemAccelerationImpl(const State& state) const override { + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + const WrappingPath& path = getCablePath(ix); + path.getImpl().realizeAcceleration(state); + } + return 0; + } + + SimTK_DOWNCAST(Impl, Subsystem::Guts); + + private: + // TOPOLOGY STATE + Array_ paths; }; } // namespace SimTK From b438ccec411695dcfe5723ae4d0adcf8b7e72def Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 12:34:32 +0200 Subject: [PATCH 007/127] update function bodies for WrappingPath::Impl --- Simbody/include/simbody/internal/Wrapping.cpp | 30 ++++++++-- Simbody/include/simbody/internal/Wrapping.h | 58 ++++++------------- 2 files changed, 45 insertions(+), 43 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 5f9fdcbd1..54ff437f0 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -4,13 +4,35 @@ using namespace SimTK; //============================================================================== -// SURFACE +// WRAPPING PATH //============================================================================== -Surface::Surface(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry) +void WrappingPath::Impl::realizeTopology(State &state) { + PosInfo posInfo {}; + m_PosInfoIx = m_Subsystem->allocateCacheEntry(state, Stage::Position, new Value(posInfo)); + VelInfo velInfo {}; + m_VelInfoIx = m_Subsystem->allocateCacheEntry(state, Stage::Velocity, new Value(velInfo)); + + VizInfo vizInfo {}; + m_VizInfoIx = m_Subsystem->allocateCacheEntry(state, Stage::Position, new Value(vizInfo)); } -const ContactGeometry& getGeometry() const; -const Transform& calcSurfaceToGroundTransform(const State& state) const; +void WrappingPath::Impl::realizePosition(const State &state) const +{ + if (m_Subsystem->isCacheValueRealized(state, m_PosInfoIx)) {return;} + calcPosInfo(updPosInfo(state)); + m_Subsystem->markCacheValueRealized(state, m_PosInfoIx); +} + +const WrappingPath::Impl::PosInfo& WrappingPath::Impl::getPosInfo(const State &state) const +{ + realizePosition(state); + return Value::downcast(m_Subsystem->getCacheEntry(state, m_PosInfoIx)); +} + +WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &state) const +{ + return Value::updDowncast(m_Subsystem->updCacheEntry(state, m_PosInfoIx)); +} diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 50d49e989..cb360a4bf 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -1,6 +1,7 @@ #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" @@ -191,15 +192,6 @@ class SimTK_SIMBODY_EXPORT WrappingPath WrapObstacleIndex adoptObstacle(WrapObstacle); Real getLength(const State& state) const; - Real getLengthDot(const State& state) const; - void applyBodyForces( const State& state, Real tension, Vector_& bodyForcesInG) const; - Real calcPower(const State& state, Real tension) const; - - WrapObstacle& setDecorativeGeometry(const DecorativeGeometry& viz); - WrapObstacle& setNearPoint(const Vec3& point); - WrapObstacle& setContactPointHints( - const Vec3& startHint, - const Vec3& endHint); class Impl; @@ -246,57 +238,45 @@ class WrappingPath::Impl { Real lDot = NaN; }; - int getNumObstacles() const {return obstacles.size();} - const WrapObstacle& getObstacle(WrapObstacleIndex ix) const; - WrapObstacleIndex adoptObstacle(WrapObstacle& obstacle); - - void calcPath(State& state, bool preventLiftOff = false) const; - void calcInitPath(State& state, std::function pointHints); - - Real getPathError(const State& state) const; - Real getLength(const State& state) const; - Real getLengthDot(const State& state) const; - void applyBodyForces(const State& state, Real tension, Vector_& bodyForcesInG) const; - Real calcCablePower(const State& state, Real tension) const; + std::vector& updObstacles() {return m_Obstacles;} + const std::vector& getObstacles() const {return m_Obstacles;} // Allocate state variables and cache entries. void realizeTopology(State& state); - void realizeInstance(const State& state) const; void realizePosition(const State& state) const; void realizeVelocity(const State& state) const; void realizeAcceleration(const State& state) const; - /* void calcEventTriggerInfo (const State&, Array_&) const; */ - /* void handleEvents (State&, Event::Cause, const Array_& eventIds, const HandleEventsOptions& options, HandleEventsResults& results) const; */ + void invalidateTopology() + { if (m_Subsystem) m_Subsystem->invalidateSubsystemTopologyCache(); } const PosInfo& getPosInfo(const State& state) const; const VelInfo& getVelInfo(const State& state) const; const VizInfo& getVizInfo(const State& state) const; - PosInfo& updPosEntry(const State& state) const; - VelInfo& updVelEntry(const State& state) const; + private: + + PosInfo& updPosInfo(const State& state) const; + VelInfo& updVelInfo(const State& state) const; VizInfo& updVizInfo(const State& state) const; void calcPosInfo(PosInfo& posInfo) const; void calcVelInfo(const PosInfo& posInfo, VelInfo& velInfo) const; void calcVizInfo(const PosInfo& posInfo, VizInfo& vizInfo) const; - // Be sure to call this whenever you make a topology-level change to - // the cable definition, like adding an obstacle or modifying one in - // a significant way. - void invalidateTopology() - { if (subsystem) subsystem->invalidateSubsystemTopologyCache(); } + void calcPath(State& state, bool preventLiftOff = false) const; + void calcInitPath(State& state, std::function pointHints); - private: - friend class WrappingPath; - std::shared_ptr subsystem = nullptr; - WrappingPathIndex index {}; + std::shared_ptr m_Subsystem = nullptr; + WrappingPathIndex m_Index {}; - std::vector obstacles {}; + std::vector m_Obstacles {}; // TOPOLOGY CACHE (set during realizeTopology()) - DiscreteVariableIndex posInfoIx; - DiscreteVariableIndex velInfoIx; - DiscreteVariableIndex vizInfoIx; + CacheEntryIndex m_PosInfoIx; + CacheEntryIndex m_VelInfoIx; + CacheEntryIndex m_VizInfoIx; + + friend class WrappingPath; }; //============================================================================== From 65a240da3bc026d2aed6cf336897bd8ff08120c7 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 12:36:22 +0200 Subject: [PATCH 008/127] remove future cache entries from WrappingPath --- Simbody/include/simbody/internal/Wrapping.cpp | 6 ------ Simbody/include/simbody/internal/Wrapping.h | 16 +--------------- 2 files changed, 1 insertion(+), 21 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 54ff437f0..5fb649cb4 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -11,12 +11,6 @@ void WrappingPath::Impl::realizeTopology(State &state) { PosInfo posInfo {}; m_PosInfoIx = m_Subsystem->allocateCacheEntry(state, Stage::Position, new Value(posInfo)); - - VelInfo velInfo {}; - m_VelInfoIx = m_Subsystem->allocateCacheEntry(state, Stage::Velocity, new Value(velInfo)); - - VizInfo vizInfo {}; - m_VizInfoIx = m_Subsystem->allocateCacheEntry(state, Stage::Position, new Value(vizInfo)); } void WrappingPath::Impl::realizePosition(const State &state) const diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index cb360a4bf..8dc03bdd6 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -226,16 +226,8 @@ class WrappingPath::Impl { std::vector lines; size_t loopIter = 0; - }; - struct VizInfo - { - std::vector> points {}; - }; - - struct VelInfo - { - Real lDot = NaN; + // TODO solver matrices }; std::vector& updObstacles() {return m_Obstacles;} @@ -250,18 +242,12 @@ class WrappingPath::Impl { { if (m_Subsystem) m_Subsystem->invalidateSubsystemTopologyCache(); } const PosInfo& getPosInfo(const State& state) const; - const VelInfo& getVelInfo(const State& state) const; - const VizInfo& getVizInfo(const State& state) const; private: PosInfo& updPosInfo(const State& state) const; - VelInfo& updVelInfo(const State& state) const; - VizInfo& updVizInfo(const State& state) const; void calcPosInfo(PosInfo& posInfo) const; - void calcVelInfo(const PosInfo& posInfo, VelInfo& velInfo) const; - void calcVizInfo(const PosInfo& posInfo, VizInfo& vizInfo) const; void calcPath(State& state, bool preventLiftOff = false) const; void calcInitPath(State& state, std::function pointHints); From c02ce9f7ab19d59b1182c9b53c6a2ee407394345 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 12:51:50 +0200 Subject: [PATCH 009/127] Fill in WrappingPath function bodies --- Simbody/include/simbody/internal/Wrapping.cpp | 16 +++++++---- Simbody/include/simbody/internal/Wrapping.h | 27 ++++++++++++------- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 5fb649cb4..1d6fc622f 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1,5 +1,6 @@ #include "SimTKmath.h" #include "Wrapping.h" +#include using namespace SimTK; @@ -10,23 +11,28 @@ using namespace SimTK; void WrappingPath::Impl::realizeTopology(State &state) { PosInfo posInfo {}; - m_PosInfoIx = m_Subsystem->allocateCacheEntry(state, Stage::Position, new Value(posInfo)); + m_PosInfoIx = m_Subsystem.allocateCacheEntry(state, Stage::Position, new Value(posInfo)); } void WrappingPath::Impl::realizePosition(const State &state) const { - if (m_Subsystem->isCacheValueRealized(state, m_PosInfoIx)) {return;} + if (m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) {return;} calcPosInfo(updPosInfo(state)); - m_Subsystem->markCacheValueRealized(state, m_PosInfoIx); + m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); } const WrappingPath::Impl::PosInfo& WrappingPath::Impl::getPosInfo(const State &state) const { realizePosition(state); - return Value::downcast(m_Subsystem->getCacheEntry(state, m_PosInfoIx)); + return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); } WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &state) const { - return Value::updDowncast(m_Subsystem->updCacheEntry(state, m_PosInfoIx)); + return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); +} + +void WrappingPath::Impl::calcPosInfo(PosInfo& posInfo) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); } diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 8dc03bdd6..3b11f6d59 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -209,6 +209,17 @@ class SimTK_SIMBODY_EXPORT WrappingPath //============================================================================== class WrappingPath::Impl { public: + Impl( + WrappingPathSubsystem subsystem, + MobilizedBody originBody, + Vec3 originPoint, + MobilizedBody terminationBody, + Vec3 terminationPoint): + m_Subsystem(subsystem), + m_OriginBody(originBody), + m_OriginPoint(originPoint), + m_TerminationBody(terminationBody), + m_TerminationPoint(terminationPoint) {} struct LineSegment { @@ -236,24 +247,20 @@ class WrappingPath::Impl { // Allocate state variables and cache entries. void realizeTopology(State& state); void realizePosition(const State& state) const; - void realizeVelocity(const State& state) const; - void realizeAcceleration(const State& state) const; void invalidateTopology() - { if (m_Subsystem) m_Subsystem->invalidateSubsystemTopologyCache(); } + { m_Subsystem.invalidateSubsystemTopologyCache(); } const PosInfo& getPosInfo(const State& state) const; - private: - PosInfo& updPosInfo(const State& state) const; - void calcPosInfo(PosInfo& posInfo) const; - void calcPath(State& state, bool preventLiftOff = false) const; - void calcInitPath(State& state, std::function pointHints); - std::shared_ptr m_Subsystem = nullptr; - WrappingPathIndex m_Index {}; + WrappingPathSubsystem m_Subsystem; + MobilizedBody m_OriginBody; + Vec3 m_OriginPoint; + MobilizedBody m_TerminationBody; + Vec3 m_TerminationPoint; std::vector m_Obstacles {}; From 3d7bf951740476c016f8a4b4b20fd048bf8b265d Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 12:57:47 +0200 Subject: [PATCH 010/127] update WrappingPath --- Simbody/include/simbody/internal/Wrapping.h | 63 ++++++++++----------- 1 file changed, 31 insertions(+), 32 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 3b11f6d59..3bc86fbfa 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -174,42 +174,12 @@ class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem const Impl& getImpl() const; }; -//============================================================================== -// PATH -//============================================================================== -class SimTK_SIMBODY_EXPORT WrappingPath -{ -public: - WrappingPath( - WrappingPathSubsystem& subsystem, - const MobilizedBody& originBody, - const Vec3& defaultOriginPoint, - const MobilizedBody& terminationBody, - const Vec3& defaultTerminationPoint); - - int getNumObstacles() const; - const WrapObstacle& getObstacle(WrapObstacleIndex obstacleIx) const; - WrapObstacleIndex adoptObstacle(WrapObstacle); - - Real getLength(const State& state) const; - - class Impl; - -private: - friend WrappingPathSubsystem::Impl; - - const Impl& getImpl() const; - Impl& updImpl(); - - std::shared_ptr impl = nullptr; -}; - //============================================================================== // PATH :: IMPL //============================================================================== -class WrappingPath::Impl { +class WrappingPathImpl { public: - Impl( + WrappingPathImpl( WrappingPathSubsystem subsystem, MobilizedBody originBody, Vec3 originPoint, @@ -272,6 +242,35 @@ class WrappingPath::Impl { friend class WrappingPath; }; +//============================================================================== +// PATH +//============================================================================== +class SimTK_SIMBODY_EXPORT WrappingPath +{ +public: + using Impl = WrappingPathImpl; + + WrappingPath( + WrappingPathSubsystem& subsystem, + const MobilizedBody& originBody, + const Vec3& defaultOriginPoint, + const MobilizedBody& terminationBody, + const Vec3& defaultTerminationPoint); + + std::vector& updObstacles() {return updImpl().updObstacles();} + const std::vector& getObstacles() const {return getImpl().getObstacles();} + + Real getLength(const State& state) const {return getImpl().getPosInfo(state).l; } + +private: + friend WrappingPathSubsystem::Impl; + + const Impl& getImpl() const { return *impl; } + Impl& updImpl() { return *impl; } + + std::shared_ptr impl = nullptr; +}; + //============================================================================== // SUBSYSTEM :: IMPL //============================================================================== From ddbc6f21300a1ab5d0a7fa5e13d5961b67e9621a Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 13:00:53 +0200 Subject: [PATCH 011/127] add WrappingPathSubsystem functions to cpp --- Simbody/include/simbody/internal/Wrapping.cpp | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 1d6fc622f..92ff3375b 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -4,6 +4,56 @@ using namespace SimTK; +//============================================================================== +// SUBSYSTEM +//============================================================================== + +bool WrappingPathSubsystem::isInstanceOf(const Subsystem& s) { + return Impl::isA(s.getSubsystemGuts()); +} + +const WrappingPathSubsystem& WrappingPathSubsystem:: +downcast(const Subsystem& s) { + assert(isInstanceOf(s)); + return static_cast(s); +} +WrappingPathSubsystem& WrappingPathSubsystem:: +updDowncast(Subsystem& s) { + assert(isInstanceOf(s)); + return static_cast(s); +} + +const WrappingPathSubsystem::Impl& WrappingPathSubsystem:: +getImpl() const { + return SimTK_DYNAMIC_CAST_DEBUG(getSubsystemGuts()); +} +WrappingPathSubsystem::Impl& WrappingPathSubsystem:: +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. +WrappingPathSubsystem::WrappingPathSubsystem() +{ adoptSubsystemGuts(new Impl()); } + +WrappingPathSubsystem::WrappingPathSubsystem(MultibodySystem& mbs) +{ adoptSubsystemGuts(new Impl()); + mbs.adoptSubsystem(*this); } // steal ownership + +int WrappingPathSubsystem::getNumPaths() const +{ return getImpl().getNumPaths(); } + +const WrappingPath& WrappingPathSubsystem:: +getPath(WrappingPathIndex cableIx) const +{ return getImpl().getCablePath(cableIx); } + +WrappingPath& WrappingPathSubsystem:: +updPath(WrappingPathIndex cableIx) +{ return updImpl().updCablePath(cableIx); } + + //============================================================================== // WRAPPING PATH //============================================================================== From 38ae9afed14fab1b120c8cf83da0437da7113947 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 15:22:50 +0200 Subject: [PATCH 012/127] wip --- Simbody/include/simbody/internal/Wrapping.cpp | 42 ++++++++++++++- Simbody/include/simbody/internal/Wrapping.h | 51 ++++++++++--------- 2 files changed, 67 insertions(+), 26 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 92ff3375b..d671a5896 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -55,7 +55,7 @@ updPath(WrappingPathIndex cableIx) //============================================================================== -// WRAPPING PATH +// WRAPPING PATH //============================================================================== void WrappingPath::Impl::realizeTopology(State &state) @@ -86,3 +86,43 @@ void WrappingPath::Impl::calcPosInfo(PosInfo& posInfo) const { throw std::runtime_error("NOTYETIMPLEMENTED"); } + +//============================================================================== +// OBSTACLE +//============================================================================== + +void WrapObstacle::realizeTopology(State &state) +{ + // Allocate an auto-update discrete variable for the last computed geodesic. + WarmStartInfo warmStartInfo {}; + m_WarmStartInfoDIx = m_Subsystem.allocateAutoUpdateDiscreteVariable(state, Stage::Velocity, new Value(warmStartInfo), Stage::Position); + m_WarmStartInfoIx = m_Subsystem.getDiscreteVarUpdateIndex(state, m_WarmStartInfoDIx); + + // Allocate position level cache. + PosInfo posInfo {}; + m_PosInfoIx = m_Subsystem.allocateCacheEntry(state, Stage::Position, new Value(posInfo)); +} + +void WrapObstacle::realizePosition(const State &state) const +{ + if (m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) {return;} + calcPosInfo(updPosInfo(state)); + m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); +} + +const WrapObstacle::PosInfo& WrapObstacle::getPosInfo(const State &state) const +{ + realizePosition(state); + return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); +} + +WrapObstacle::PosInfo& WrapObstacle::updPosInfo(const State &state) const +{ + return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); +} + +void WrapObstacle::calcPosInfo(PosInfo& posInfo) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 3bc86fbfa..b2045f0f6 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -75,6 +75,27 @@ class SimTK_SIMBODY_EXPORT Surface std::shared_ptr impl = nullptr; }; +//============================================================================== +// SUBSYSTEM +//============================================================================== +class WrappingPath; + +class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem +{ +public: + WrappingPathSubsystem(); + explicit WrappingPathSubsystem(MultibodySystem&); + + int getNumPaths() const; + const WrappingPath& getPath(WrappingPathIndex idx) const; + WrappingPath& updPath(WrappingPathIndex idx); + + SimTK_PIMPL_DOWNCAST(WrappingPathSubsystem, Subsystem); + class Impl; + Impl& updImpl(); + const Impl& getImpl() const; +}; + //============================================================================== // OBSTACLE //============================================================================== @@ -141,37 +162,18 @@ class SimTK_SIMBODY_EXPORT WrapObstacle private: const WarmStartInfo& getWarmStartInfo(const State& state) const; WarmStartInfo& updWarmStartInfo(const State& state) const; + void calcPosInfo(PosInfo& posInfo) const; // Required for accessing the discrete variable? - std::shared_ptr subsystem = nullptr; + WrappingPathSubsystem m_Subsystem; std::vector obstacles {}; // TOPOLOGY CACHE (set during realizeTopology()) - DiscreteVariableIndex warmStartInfoIx; - DiscreteVariableIndex posInfoIx; -}; - -//============================================================================== -// SUBSYSTEM -//============================================================================== -class WrappingPath; - -class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem -{ -public: - WrappingPathSubsystem(); - explicit WrappingPathSubsystem(MultibodySystem&); - - int getNumPaths() const; - const WrappingPath& getPath(WrappingPathIndex idx) const; - WrappingPath& updPath(WrappingPathIndex idx); - - SimTK_PIMPL_DOWNCAST(WrappingPathSubsystem, Subsystem); - class Impl; - Impl& updImpl(); - const Impl& getImpl() const; + DiscreteVariableIndex m_WarmStartInfoDIx; + CacheEntryIndex m_WarmStartInfoIx; + CacheEntryIndex m_PosInfoIx; }; //============================================================================== @@ -225,7 +227,6 @@ class WrappingPathImpl { PosInfo& updPosInfo(const State& state) const; void calcPosInfo(PosInfo& posInfo) const; - WrappingPathSubsystem m_Subsystem; MobilizedBody m_OriginBody; Vec3 m_OriginPoint; From 2aa42b4914af16db55b18eb53097db4e32ca61c1 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 17:28:52 +0200 Subject: [PATCH 013/127] autoupdate discrete cache var: LocalGeodesicInfo --- Simbody/include/simbody/internal/Wrapping.cpp | 117 ++++++++++++++++-- Simbody/include/simbody/internal/Wrapping.h | 73 ++++++++--- 2 files changed, 161 insertions(+), 29 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index d671a5896..75a24652c 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1,5 +1,6 @@ #include "SimTKmath.h" #include "Wrapping.h" +#include "simmath/internal/ContactGeometry.h" #include using namespace SimTK; @@ -94,9 +95,8 @@ void WrappingPath::Impl::calcPosInfo(PosInfo& posInfo) const void WrapObstacle::realizeTopology(State &state) { // Allocate an auto-update discrete variable for the last computed geodesic. - WarmStartInfo warmStartInfo {}; - m_WarmStartInfoDIx = m_Subsystem.allocateAutoUpdateDiscreteVariable(state, Stage::Velocity, new Value(warmStartInfo), Stage::Position); - m_WarmStartInfoIx = m_Subsystem.getDiscreteVarUpdateIndex(state, m_WarmStartInfoDIx); + LocalGeodesicInfo warmStartInfo {}; + m_WarmStartInfoDIx = m_Subsystem.allocateAutoUpdateDiscreteVariable(state, Stage::Velocity, new Value(warmStartInfo), Stage::Position); // Allocate position level cache. PosInfo posInfo {}; @@ -105,24 +105,121 @@ void WrapObstacle::realizeTopology(State &state) void WrapObstacle::realizePosition(const State &state) const { - if (m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) {return;} - calcPosInfo(updPosInfo(state)); - m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); + // Set the current local geodesic equal to the previous. + if (!m_Subsystem.isDiscreteVarUpdateValueRealized(state, m_WarmStartInfoDIx)) { + updLocalGeodesicInfo(state) = getPrevLocalGeodesicInfo(state); + m_Subsystem.markDiscreteVarUpdateValueRealized(state, m_WarmStartInfoDIx); + } } const WrapObstacle::PosInfo& WrapObstacle::getPosInfo(const State &state) const { - realizePosition(state); + if (!m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) { + calcPosInfo(state, getLocalGeodesicInfo(state), updPosInfo(state)); + m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); + } return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); } -WrapObstacle::PosInfo& WrapObstacle::updPosInfo(const State &state) const +namespace { - return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); + Vec3 calcCorrectedGeodesicStartPoint( + const ContactGeometry::GeodesicCorrection& c, + const ContactGeometry::GeodesicVariation& dKP, + const ContactGeometry::FrenetFrame& KP + ) + { + Vec3 v = dKP[1] * c; + return KP.p() + v; + } + + Vec3 calcCorrectedGeodesicStartTangent( + const ContactGeometry::GeodesicCorrection& c, + const ContactGeometry::GeodesicVariation& dKP, + const ContactGeometry::FrenetFrame& KP + ) + { + Vec3 w = dKP[0] * c; + const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); + return t + cross(w, t); + } + + double calcCorrectedGeodesicLength( + const ContactGeometry::GeodesicCorrection& c, + Real length) + { + return length + c[3]; + } + + void xformSurfaceGeodesicToBase( + const WrapObstacle::LocalGeodesicInfo& geodesic_S, + WrapObstacle::PosInfo& geodesic_B, + Transform& X_BS) { + + // TODO check transformation order. + geodesic_B.KP = X_BS.compose(geodesic_S.KP); + geodesic_B.KQ = X_BS.compose(geodesic_S.KQ); + + geodesic_B.dKP[0] = X_BS.R() * geodesic_S.dKP[0]; + geodesic_B.dKP[1] = X_BS.R() * geodesic_S.dKP[1]; + + geodesic_B.dKQ[0] = X_BS.R() * geodesic_S.dKQ[0]; + geodesic_B.dKQ[1] = X_BS.R() * geodesic_S.dKQ[1]; + + geodesic_B.length = geodesic_S.length; + + throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); + } } -void WrapObstacle::calcPosInfo(PosInfo& posInfo) const +void WrapObstacle::applyGeodesicCorrection(const State& state, const WrapObstacle::Correction& c) const { + // Get prev geodesic. + const LocalGeodesicInfo& geodesic = getLocalGeodesicInfo(state); + + // Apply geodesic correction. + Vec3 x = calcCorrectedGeodesicStartPoint(c, geodesic.dKP, geodesic.KP); + Vec3 t = calcCorrectedGeodesicStartTangent(c, geodesic.dKP, geodesic.KP); + Real l = calcCorrectedGeodesicLength(c, geodesic.length); + Real sHint = geodesic.sHint; + + // Shoot the new geodesic. + calcLocalGeodesic(x, t, l, sHint, updLocalGeodesicInfo(state)); + + // Invalidate position level cache. + m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); +} + +void WrapObstacle::calcLocalGeodesic( Vec3 x, Vec3 t, Real l, Real sHint, + WrapObstacle::LocalGeodesicInfo& geodesic) const +{ + const ContactGeometry& geometry = m_Surface.getGeometry(); + + // Compute geodesic start boundary frame. + geometry.calcNearestFrenetFrameFast(x, t, geodesic.KP); + geometry.calcGeodesicStartFrameVariation(geodesic.KP, geodesic.dKP); + + // Compute geodesic end boundary frame (shoot new geodesic). + geometry.calcGeodesicEndFrameVariationImplicitly( + geodesic.KP.p(), + geodesic.KP.R().getAxisUnitVec(ContactGeometry::TangentAxis), + l, + sHint, + geodesic.KQ, + geodesic.dKQ, + geodesic.points); + + // TODO update step size. + // TODO update line tracking? throw std::runtime_error("NOTYETIMPLEMENTED"); } +void WrapObstacle::calcPosInfo( + const State& state, + const WrapObstacle::LocalGeodesicInfo& localGeodesic, + PosInfo& posInfo) const +{ + // Transform the local geodesic to ground frame. + Transform X_BS = m_Surface.calcSurfaceToGroundTransform(state); + xformSurfaceGeodesicToBase(localGeodesic, posInfo, X_BS); +} diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index b2045f0f6..3db122de0 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -107,7 +107,6 @@ class SimTK_SIMBODY_EXPORT WrapObstacle using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; - using BoundaryFrameVariation = WrapObstacle::Correction; private: WrapObstacle() = default; @@ -119,30 +118,33 @@ class SimTK_SIMBODY_EXPORT WrapObstacle explicit WrapObstacle(Surface surface); - // Solution previously commputed, all in local surface frame coordinates. - struct WarmStartInfo + // Ground frame solution. + struct PosInfo { FrenetFrame KP {}; FrenetFrame KQ {}; Real length = NaN; - BoundaryFrameVariation dKP {}; - BoundaryFrameVariation dKQ {}; - - std::vector points {}; + Variation dKP {}; + Variation dKQ {}; }; - // Ground frame solution. - struct PosInfo + // Solution previously commputed, all in local surface frame coordinates. + struct LocalGeodesicInfo { FrenetFrame KP {}; FrenetFrame KQ {}; Real length = NaN; - BoundaryFrameVariation dKP {}; - BoundaryFrameVariation dKQ {}; + Variation dKP {}; + Variation dKQ {}; + + std::vector points {}; + double sHint = NaN; + + double lineTracking = NaN; }; // Allocate state variables and cache entries. @@ -154,25 +156,58 @@ class SimTK_SIMBODY_EXPORT WrapObstacle void invalidateTopology(); const PosInfo& getPosInfo(const State& state) const; - PosInfo& updPosInfo(const State& state) const; + + WrapObstacle::PosInfo& updPosInfo(const State &state) const + { + return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); + } size_t writeGeodesicPoints(const State& state, std::vector points) const; - void applyCorrection(const State& state) const; + void applyGeodesicCorrection(const State& state, const WrapObstacle::Correction& c) const; private: - const WarmStartInfo& getWarmStartInfo(const State& state) const; - WarmStartInfo& updWarmStartInfo(const State& state) const; + const LocalGeodesicInfo& getLocalGeodesicInfo(const State& state) const + { + realizePosition(state); + return Value::downcast( + m_Subsystem.getDiscreteVarUpdateValue(state, m_WarmStartInfoDIx) + ); + } - void calcPosInfo(PosInfo& posInfo) const; + LocalGeodesicInfo& updLocalGeodesicInfo(const State& state) const + { + return Value::updDowncast( + m_Subsystem.updDiscreteVarUpdateValue(state, m_WarmStartInfoDIx) + ); + } + + const LocalGeodesicInfo& getPrevLocalGeodesicInfo(const State& state) const + { + return Value::downcast( + m_Subsystem.getDiscreteVariable(state, m_WarmStartInfoDIx) + ); + } + + LocalGeodesicInfo& updPrevLocalGeodesicInfo(State& state) const + { + return Value::updDowncast( + m_Subsystem.updDiscreteVariable(state, m_WarmStartInfoDIx) + ); + } + + void calcLocalGeodesic(Vec3 x, Vec3 t, Real l, Real sHint, + WrapObstacle::LocalGeodesicInfo& geodesic) const; + void calcPosInfo(const State& state, + const WrapObstacle::LocalGeodesicInfo& localGeodesic, + PosInfo& posInfo) const; + + Surface m_Surface; // Required for accessing the discrete variable? WrappingPathSubsystem m_Subsystem; - std::vector obstacles {}; - // TOPOLOGY CACHE (set during realizeTopology()) DiscreteVariableIndex m_WarmStartInfoDIx; - CacheEntryIndex m_WarmStartInfoIx; CacheEntryIndex m_PosInfoIx; }; From ff1b64d9b9a36939560714883b2a58db04b9ea8a Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 18:03:05 +0200 Subject: [PATCH 014/127] wip: add more geodesic math --- .../simmath/internal/ContactGeometry.h | 46 ++++++ Simbody/include/simbody/internal/Wrapping.cpp | 135 +++++++++++------- 2 files changed, 130 insertions(+), 51 deletions(-) diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h index 9ce30f0b6..d8e078457 100644 --- a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h +++ b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h @@ -118,6 +118,52 @@ class Cylinder; class Brick; class TriangleMesh; + +using FrenetFrame = Transform; + +static const CoordinateAxis TangentAxis; +static const CoordinateAxis NormalAxis; +static const CoordinateAxis BinormalAxis; + +static constexpr int GEODESIC_DOF = 4; + +using GeodesicPointVariation = Mat34; +using GeodesicFrameVariation = Mat34; +using GeodesicVariation = std::array; +using GeodesicCorrection = Vec4; + +struct GeodesicBoundaryFrame { + FrenetFrame K_P{}; + FrenetFrame K_Q{}; + + GeodesicVariation v_P{}; + GeodesicVariation v_Q{}; +}; + +void calcNearestFrenetFrameFast(Vec3 x, Vec3 thint, FrenetFrame& X_BP) const; +void calcGeodesicStartFrameVariation(const FrenetFrame& X_BP, GeodesicVariation& dX_BP) const; +void calcGeodesicEndFrameVariationImplicitly( + Vec3 x, + UnitVec3 t, Real l, Real sHint, + FrenetFrame& X_BQ, + GeodesicVariation& dX_BQ, + std::vector& points) const; +void calcGeodesicEndFrameVariationAnalytically( Vec3 x, + UnitVec3 t, Real l, + FrenetFrame& X_BQ, + GeodesicVariation& dX_BQ) const; +void calcGeodesicPointsAnalytically(Vec3 x, UnitVec3 t, Real l, std::vector& points); +bool analyticFormAvailable() const; + +struct PointOnLineResult +{ + Vec3 p {NaN, NaN, NaN}; + bool isInsideSurface = false; + size_t iter = 0; +}; + +PointOnLineResult calcNearestPointOnLine(Vec3 a, Vec3 b, Vec3 hint, size_t maxIter, double eps); + // TODO class Cone; diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 75a24652c..78865b68e 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -5,6 +5,90 @@ using namespace SimTK; +//============================================================================== +// SOME MATH +//============================================================================== +namespace +{ + +using GeodesicJacobian = Vec4; +using LineSegment = WrappingPathImpl::LineSegment; +using PointVariation = ContactGeometry::GeodesicPointVariation; +using FrameVariation = ContactGeometry::GeodesicFrameVariation; +using Variation = ContactGeometry::GeodesicVariation; + + Vec3 calcCorrectedGeodesicStartPoint( + const ContactGeometry::GeodesicCorrection& c, + const ContactGeometry::GeodesicVariation& dKP, + const ContactGeometry::FrenetFrame& KP + ) + { + Vec3 v = dKP[1] * c; + return KP.p() + v; + } + + Vec3 calcCorrectedGeodesicStartTangent( + const ContactGeometry::GeodesicCorrection& c, + const ContactGeometry::GeodesicVariation& dKP, + const ContactGeometry::FrenetFrame& KP + ) + { + Vec3 w = dKP[0] * c; + const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); + return t + cross(w, t); + } + + double calcCorrectedGeodesicLength( + const ContactGeometry::GeodesicCorrection& c, + Real length) + { + return length + c[3]; + } + + void xformSurfaceGeodesicToBase( + const WrapObstacle::LocalGeodesicInfo& geodesic_S, + WrapObstacle::PosInfo& geodesic_B, + Transform& X_BS) { + + // TODO check transformation order. + geodesic_B.KP = X_BS.compose(geodesic_S.KP); + geodesic_B.KQ = X_BS.compose(geodesic_S.KQ); + + geodesic_B.dKP[0] = X_BS.R() * geodesic_S.dKP[0]; + geodesic_B.dKP[1] = X_BS.R() * geodesic_S.dKP[1]; + + geodesic_B.dKQ[0] = X_BS.R() * geodesic_S.dKQ[0]; + geodesic_B.dKQ[1] = X_BS.R() * geodesic_S.dKQ[1]; + + geodesic_B.length = geodesic_S.length; + + throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); + } + +GeodesicJacobian calcDirectionJacobian( + const LineSegment& e, + UnitVec3 axis, + const PointVariation& v) +{ + Vec3 y = axis - e.d * dot(e.d,axis); + y /= e.l; + return ~v * y; +} + +GeodesicJacobian calcPathErrorJacobian( + const LineSegment& line, + UnitVec3 axis, + const PointVariation& v, + const FrameVariation& w, + bool invertV = false) +{ + GeodesicJacobian jacobian = + calcDirectionJacobian(line, axis, v) * (invertV ? -1. : 1.); + jacobian += cross(axis,line.d).transpose() * w; + return jacobian; +} +} + //============================================================================== // SUBSYSTEM //============================================================================== @@ -121,57 +205,6 @@ const WrapObstacle::PosInfo& WrapObstacle::getPosInfo(const State &state) const return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); } -namespace -{ - Vec3 calcCorrectedGeodesicStartPoint( - const ContactGeometry::GeodesicCorrection& c, - const ContactGeometry::GeodesicVariation& dKP, - const ContactGeometry::FrenetFrame& KP - ) - { - Vec3 v = dKP[1] * c; - return KP.p() + v; - } - - Vec3 calcCorrectedGeodesicStartTangent( - const ContactGeometry::GeodesicCorrection& c, - const ContactGeometry::GeodesicVariation& dKP, - const ContactGeometry::FrenetFrame& KP - ) - { - Vec3 w = dKP[0] * c; - const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); - return t + cross(w, t); - } - - double calcCorrectedGeodesicLength( - const ContactGeometry::GeodesicCorrection& c, - Real length) - { - return length + c[3]; - } - - void xformSurfaceGeodesicToBase( - const WrapObstacle::LocalGeodesicInfo& geodesic_S, - WrapObstacle::PosInfo& geodesic_B, - Transform& X_BS) { - - // TODO check transformation order. - geodesic_B.KP = X_BS.compose(geodesic_S.KP); - geodesic_B.KQ = X_BS.compose(geodesic_S.KQ); - - geodesic_B.dKP[0] = X_BS.R() * geodesic_S.dKP[0]; - geodesic_B.dKP[1] = X_BS.R() * geodesic_S.dKP[1]; - - geodesic_B.dKQ[0] = X_BS.R() * geodesic_S.dKQ[0]; - geodesic_B.dKQ[1] = X_BS.R() * geodesic_S.dKQ[1]; - - geodesic_B.length = geodesic_S.length; - - throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); - } -} - void WrapObstacle::applyGeodesicCorrection(const State& state, const WrapObstacle::Correction& c) const { // Get prev geodesic. From b18ff4727eb6ccdaa3bb3395ff56fec863c16fd5 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 22:30:18 +0200 Subject: [PATCH 015/127] wip: add jacobian calculations --- Simbody/include/simbody/internal/Wrapping.cpp | 293 +++++++++++++++++- Simbody/include/simbody/internal/Wrapping.h | 22 +- 2 files changed, 299 insertions(+), 16 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 78865b68e..2b5d1d874 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -17,6 +17,17 @@ using PointVariation = ContactGeometry::GeodesicPointVariation; using FrameVariation = ContactGeometry::GeodesicFrameVariation; using Variation = ContactGeometry::GeodesicVariation; +using Geodesic = WrapObstacle::PosInfo; +using LocalGeodesic = WrapObstacle::LocalGeodesicInfo; + +static const CoordinateAxis TangentAxis = ContactGeometry::TangentAxis; +static const CoordinateAxis NormalAxis = ContactGeometry::NormalAxis; +static const CoordinateAxis BinormalAxis = ContactGeometry::BinormalAxis; +static const int GeodesicDOF = 4; +static const int N_PATH_CONSTRAINTS = 4; + +using Status = WrapObstacle::Status; + Vec3 calcCorrectedGeodesicStartPoint( const ContactGeometry::GeodesicCorrection& c, const ContactGeometry::GeodesicVariation& dKP, @@ -65,28 +76,280 @@ using Variation = ContactGeometry::GeodesicVariation; throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); } -GeodesicJacobian calcDirectionJacobian( +int countActive(const State& s, const std::vector& obstacles) +{ + int n = 0; + for (const WrapObstacle& o : obstacles) { + if (o.isActive(s)) { + ++n; + } + } + return n; +} + +using ActiveLambda = std::function< + void(size_t prev, size_t next, size_t current, bool isActive)>; + +void CallCurrentWithPrevAndNext( + size_t prev, + size_t next, + size_t current, + bool isActive, + ActiveLambda& f) +{ + f(prev, next, current, isActive); +} + +template +void CallCurrentWithPrevAndNext( + size_t prev, + size_t next, + size_t current, + bool isActive, + ActiveLambda& f, + FUNCS&&... fs) +{ + f(prev, next, current, isActive); + CallCurrentWithPrevAndNext( + prev, + next, + current, + isActive, + std::forward(fs)...); +} + +template +void MapWithPrevAndNext( + const State& s, + const std::vector& obs, + ActiveLambda& f, + FUNCS&&... fs) +{ + const ptrdiff_t n = obs.size(); + ptrdiff_t next = 0; + ptrdiff_t prev = -1; + + for (ptrdiff_t i = 0; i < n; ++i) { + // Find the active segment before the current. + if (i > 0) { + if (obs.at(i - 1).isActive(s)) { + prev = i - 1; + } + } + + // Find the active segment after the current. + if (next <= i) { + for (; ++next < n;) { + const WrapObstacle& o = obs.at(next); + if (o.isActive(s)) { + break; + } + } + } + + CallCurrentWithPrevAndNext( + prev < 0 ? n : prev, + next, + i, + obs.at(i).isActive(s), + f, + std::forward(fs)...); + } +} + +void addDirectionJacobian( const LineSegment& e, - UnitVec3 axis, - const PointVariation& v) + const UnitVec3& axis, + const PointVariation& dx, + MatrixView J, + bool invert = false) { Vec3 y = axis - e.d * dot(e.d,axis); - y /= e.l; - return ~v * y; + y /= e.l * (invert ? 1. : -1); + J += ~dx * y; } -GeodesicJacobian calcPathErrorJacobian( - const LineSegment& line, - UnitVec3 axis, - const PointVariation& v, - const FrameVariation& w, +double calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) +{ + return dot(e.d, R.getAxisUnitVec(axis)); +} + +GeodesicJacobian addPathErrorJacobian( + const LineSegment& e, + const UnitVec3& axis, + const Variation& dK, + MatrixView J, bool invertV = false) { - GeodesicJacobian jacobian = - calcDirectionJacobian(line, axis, v) * (invertV ? -1. : 1.); - jacobian += cross(axis,line.d).transpose() * w; - return jacobian; + addDirectionJacobian(e, axis, dK[1], J, invertV); + J += cross(axis,e.d).transpose() * dK[0]; } + +void calcPathError( + const State& state, + const std::vector& obs, + const std::vector& lines, + Vector& pathError) +{ + size_t i = 0; + ptrdiff_t row = -1; + for (const WrapObstacle& o : obs) { + const Geodesic& g = o.getGeodesic(state); + pathError(++row) = calcPathError(lines.at(i), g.KP.R(), NormalAxis); + pathError(++row) = calcPathError(lines.at(i), g.KP.R(), BinormalAxis); + ++i; + pathError(++row) = calcPathError(lines.at(i), g.KQ.R(), NormalAxis); + pathError(++row) = calcPathError(lines.at(i), g.KQ.R(), BinormalAxis); + } +} + +void writePathErrorJaobianBlock( + const Transform& K, + const Variation& dK, + const LineSegment& l, + MatrixView J) +{ + const UnitVec3 nP = K.R().getAxisUnitVec(NormalAxis); + const UnitVec3 bP = K.R().getAxisUnitVec(BinormalAxis); + +} + +void calcPathErrorJacobian( + const State& s, + const std::vector& obs, + const std::vector& lines, + Matrix& J) +{ + constexpr size_t Q = GeodesicDOF; + constexpr size_t C = N_PATH_CONSTRAINTS; + const size_t n = countActive(s, obs); + + J.resize(n * C, n * Q); + + size_t row = 0; + size_t col = 0; + Vec4 block { NaN, NaN, NaN, NaN,}; + + ActiveLambda f = [&](size_t prevIx, size_t nextIx, size_t i, bool isActive) { + if (!isActive) { + return; + } + + const Geodesic& g = obs.at(i).getGeodesic(s); + + { + const LineSegment& l_P = lines.at(i); + + const UnitVec3 nP = g.KP.R().getAxisUnitVec(NormalAxis); + const UnitVec3 bP = g.KP.R().getAxisUnitVec(BinormalAxis); + + addPathErrorJacobian(l_P, nP, g.dKP, J.block(row, col, 1, Q)); + addPathErrorJacobian(l_P, bP, g.dKP, J.block(row + 1, col, 1, Q)); + + if (prevIx != n) { + const Geodesic& prev = obs.at(prevIx).getGeodesic(s); + + addDirectionJacobian(l_P, nP, prev.dKP[1], J.block(row, col-Q, 1, Q), true); + addDirectionJacobian(l_P, bP, prev.dKP[1], J.block(row + 1, col-Q, 1, Q), true); + } + row += 2; + } + + // TODO looked like this + /* calcPathErrorJacobian(l_Q, a_Q, g.v_Q, g.w_Q, true); */ + // dx_P dR_P + // addDirectionJacobian(l_Q, n_Q, prev.dx_P, J.block()) + { + const LineSegment& l_Q = lines.at(i + 1); + + const UnitVec3 nQ = g.KQ.R().getAxisUnitVec(NormalAxis); + const UnitVec3 bQ = g.KQ.R().getAxisUnitVec(BinormalAxis); + + addPathErrorJacobian(l_Q, nQ, g.dKQ, J.block(row, col, 1, Q), true); + addPathErrorJacobian(l_Q, bQ, g.dKQ, J.block(row + 1, col, 1, Q), true); + + if (nextIx != n) { + /* pathErrorJacobian.block<1, Q>(row, col + Q) = */ + /* calcDirectionJacobian(l_Q, a_Q, obs.at(next).getGeodesic().v_P); */ + + const Geodesic& next = obs.at(nextIx).getGeodesic(s); + + addDirectionJacobian(l_Q, nQ, next.dKP[1], J.block(row, col+Q, 1, Q)); + addDirectionJacobian(l_Q, bQ, next.dKP[1], J.block(row + 1, col+Q, 1, Q)); + } + ++row; + } + + col += Q; + }; + + MapWithPrevAndNext(s, obs, f); +} + +double calcPathLength( + const State& s, + const std::vector& obs, + const std::vector& lines) +{ + double lTot = 0.; + for (const LineSegment& line : lines) { + // TODO spell out as length. + lTot += line.l; + } + + for (const WrapObstacle& obstacle : obs) { + if (!obstacle.isActive(s)) + { + continue; + } + lTot += obstacle.getGeodesic(s).length; + } + return lTot; +} + +void calcLineSegments( + const State& s, + Vec3 p_O, + Vec3 p_I, + const std::vector& obs, + std::vector& lines) +{ + const size_t n = obs.size(); + lines.reserve(n + 1); + lines.clear(); + + Vec3 lineStart = std::move(p_O); + for (size_t i = 0; i < n; ++i) { + if (!obs.at(i).isActive(s)) { + continue; + } + + const Geodesic& g = obs.at(i).getGeodesic(s); + const Vec3 lineEnd = g.KP.p(); + lines.emplace_back(lineStart, lineEnd); + + lineStart = g.KQ.p(); + } + lines.emplace_back(lineStart, p_I); +} + +} + +const WrapObstacle::LocalGeodesicInfo& WrapObstacle::calcInitZeroLengthGeodesicGuess(State& s, Vec3 xPrev) const +{ + Vec3 x = getInitialPointGuess(); + Vec3 t = (x - xPrev); + + // Shoot a zero-length geodesic as initial guess. + calcLocalGeodesic(x, t, 0., 0., updPrevLocalGeodesicInfo(s)); +} + +void WrappingPath::Impl::calcInitZeroLengthGeodesicSegmentsGuess(State& s) const +{ + Vec3 prev = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + for (const WrapObstacle& obstacle: m_Obstacles) { + prev = obstacle.calcInitZeroLengthGeodesicGuess(s, prev).KQ.p(); + } } //============================================================================== @@ -196,7 +459,7 @@ void WrapObstacle::realizePosition(const State &state) const } } -const WrapObstacle::PosInfo& WrapObstacle::getPosInfo(const State &state) const +const WrapObstacle::PosInfo& WrapObstacle::getGeodesic(const State &state) const { if (!m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) { calcPosInfo(state, getLocalGeodesicInfo(state), updPosInfo(state)); diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 3db122de0..014f0afa5 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -118,6 +118,14 @@ class SimTK_SIMBODY_EXPORT WrapObstacle explicit WrapObstacle(Surface surface); + enum class Status + { + Ok, + NegativeLength, + Liftoff, + Disabled, + }; + // Ground frame solution. struct PosInfo { @@ -145,6 +153,8 @@ class SimTK_SIMBODY_EXPORT WrapObstacle double sHint = NaN; double lineTracking = NaN; + + Status status = Status::Ok; }; // Allocate state variables and cache entries. @@ -155,7 +165,14 @@ class SimTK_SIMBODY_EXPORT WrapObstacle void realizeAcceleration(const State& state) const; void invalidateTopology(); - const PosInfo& getPosInfo(const State& state) const; + Status getStatus(const State& state) const; + bool isActive(const State& state) const; + + const LocalGeodesicInfo& calcInitZeroLengthGeodesicGuess(State& s, Vec3 xPrev) const; + + const PosInfo& getGeodesic(const State& state) const; + + Vec3 getInitialPointGuess() const; WrapObstacle::PosInfo& updPosInfo(const State &state) const { @@ -258,6 +275,9 @@ class WrappingPathImpl { { m_Subsystem.invalidateSubsystemTopologyCache(); } const PosInfo& getPosInfo(const State& state) const; + + void calcInitZeroLengthGeodesicSegmentsGuess(State& s) const; + private: PosInfo& updPosInfo(const State& state) const; void calcPosInfo(PosInfo& posInfo) const; From 6dba293791a1bffa920fd2b260099618b1854bc0 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 19 Apr 2024 23:54:02 +0200 Subject: [PATCH 016/127] add calcPath function body --- Simbody/include/simbody/internal/Wrapping.cpp | 76 ++++++++++++++++++- Simbody/include/simbody/internal/Wrapping.h | 28 ++++++- 2 files changed, 100 insertions(+), 4 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 2b5d1d874..8bd00d91e 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1,3 +1,4 @@ +#include "SimTKcommon/internal/CoordinateAxis.h" #include "SimTKmath.h" #include "Wrapping.h" #include "simmath/internal/ContactGeometry.h" @@ -16,6 +17,7 @@ using LineSegment = WrappingPathImpl::LineSegment; using PointVariation = ContactGeometry::GeodesicPointVariation; using FrameVariation = ContactGeometry::GeodesicFrameVariation; using Variation = ContactGeometry::GeodesicVariation; +using Correction = ContactGeometry::GeodesicCorrection; using Geodesic = WrapObstacle::PosInfo; using LocalGeodesic = WrapObstacle::LocalGeodesicInfo; @@ -286,6 +288,8 @@ void calcPathErrorJacobian( MapWithPrevAndNext(s, obs, f); } +void calcPathCorrections(const State& s, const std::vector& obs, const Vector& pathError, const Matrix& pathErrorJacobian, Matrix& pathMatrix, Vector& pathCorrections); + double calcPathLength( const State& s, const std::vector& obs, @@ -430,9 +434,77 @@ WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &state) return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); } -void WrappingPath::Impl::calcPosInfo(PosInfo& posInfo) const +bool WrapObstacle::isPointBelowSurface(const State& state, Vec3 point) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + const Transform& X_BS = getSurfaceToBaseTransform(state); + return m_Surface.getGeometry().calcSurfaceValue(X_BS.shiftBaseStationToFrame(point)) < 0.; +} + +void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo, bool preventLiftOff = false) const +{ + // Path oigin and termination points. + Vec3 x_O = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + Vec3 x_I = m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase(m_TerminationPoint); + + // Helper for detecting if start/end points lie inside the surfacce. + std::function DetectInsideSurfaceError = + [&](Vec3 x_O, const WrapObstacle& obstacle, Vec3 x_I) { // TODO also supply the status. + // Check that previous point does not lie inside the surface. + if (obstacle.isPointBelowSurface(s, x_O)) { + throw std::runtime_error("Start point lies inside the surface"); + } + + // Check that next point does not lie inside the surface. + if (obstacle.isPointBelowSurface(s, x_I)) { + throw std::runtime_error("End point lies inside the surface"); + } + + if (preventLiftOff) { + return; + } + + // TODO detect touhdown + // TODO detect liftoff + throw std::runtime_error("NOTYETIMPLEMENTED"); + }; + + for (posInfo.loopIter = 0; posInfo.loopIter < m_MaxIter; ++posInfo.loopIter) { + + // Detect touchdown & liftoff. + // doForEachObjectWithPrevAndNextPoint(UpdateLiftoffAndTouchdown) + callCurrentWithPrevAndNext(s, DetectInsideSurfaceError); + + // Compute the line segments. + calcLineSegments(s, x_O, x_I, m_Obstacles, posInfo.lines); + + calcPathError(s, m_Obstacles, posInfo.lines, posInfo.pathError); + + const Real maxPathError = posInfo.pathError.normInf(); + + // Evaluate path error, and stop when converged. + if (maxPathError < m_PathErrorBound) { + return; + } + + // Evaluate the path error jacobian. + calcPathErrorJacobian(s, m_Obstacles, posInfo.lines, posInfo.pathErrorJacobian); + + // Compute path corrections. + calcPathCorrections(s, m_Obstacles, posInfo.pathError, posInfo.pathErrorJacobian, posInfo.pathMatrix, posInfo.pathCorrections); + + // Apply path corrections. + throw std::runtime_error("NOTYETIMPLEMENTED"); + const Correction* corrIt = nullptr; // TODO + for (const WrapObstacle& obstacle : m_Obstacles) { + if (!obstacle.isActive(s)) { + continue; + } + obstacle.applyGeodesicCorrection(s, *corrIt); + ++corrIt; + } + } + + throw std::runtime_error("Failed to converge"); } //============================================================================== diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 014f0afa5..49ac37bef 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -170,6 +170,12 @@ class SimTK_SIMBODY_EXPORT WrapObstacle const LocalGeodesicInfo& calcInitZeroLengthGeodesicGuess(State& s, Vec3 xPrev) const; + bool isPointBelowSurface(const State& state, Vec3 point) const; + + const Transform& getSurfaceToBaseTransform(const state& state) const; + + Surface getSurface() const; + const PosInfo& getGeodesic(const State& state) const; Vec3 getInitialPointGuess() const; @@ -260,6 +266,11 @@ class WrappingPathImpl { std::vector lines; + Vector pathError {}; + Matrix pathMatrix {}; + Matrix pathErrorJacobian {}; + Vector pathCorrections {}; + size_t loopIter = 0; // TODO solver matrices @@ -278,18 +289,31 @@ class WrappingPathImpl { void calcInitZeroLengthGeodesicSegmentsGuess(State& s) const; + void callCurrentWithPrevAndNext( + const State& s, + std::function f); + + void callCurrentWithPrevAndNext( + const State& s, + std::function f) const; + private: - PosInfo& updPosInfo(const State& state) const; - void calcPosInfo(PosInfo& posInfo) const; + PosInfo& updPosInfo(const State& s) const; + void calcPosInfo(const State& s, PosInfo& posInfo) const; WrappingPathSubsystem m_Subsystem; + MobilizedBody m_OriginBody; Vec3 m_OriginPoint; + MobilizedBody m_TerminationBody; Vec3 m_TerminationPoint; std::vector m_Obstacles {}; + Real m_PathErrorBound = 0.1; + size_t m_MaxIter = 10; + // TOPOLOGY CACHE (set during realizeTopology()) CacheEntryIndex m_PosInfoIx; CacheEntryIndex m_VelInfoIx; From 26da8da8cdd24038d53f4c8f424f17d080fb71e4 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 22 Apr 2024 11:23:48 +0200 Subject: [PATCH 017/127] split Impl from its box --- Simbody/include/simbody/internal/Wrapping.h | 424 +++--------------- .../include/simbody/internal/WrappingImpl.h | 388 ++++++++++++++++ 2 files changed, 459 insertions(+), 353 deletions(-) create mode 100644 Simbody/include/simbody/internal/WrappingImpl.h diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 49ac37bef..fb380804f 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -21,113 +21,33 @@ SimTK_DEFINE_UNIQUE_INDEX_TYPE(WrapObstacleIndex); class MultibodySystem; class WrappingPathSubsystem; class WrapObstacle; +class WrappingPath; //============================================================================== // SURFACE //============================================================================== -class SurfaceImpl { - -private: - SurfaceImpl() = default; -public: - ~SurfaceImpl() = default; - SurfaceImpl(SurfaceImpl&&) noexcept = default; - SurfaceImpl& operator=(SurfaceImpl&&) noexcept = default; - SurfaceImpl(const SurfaceImpl&) = default; - SurfaceImpl& operator=(const SurfaceImpl&) = default; - - SurfaceImpl(MobilizedBody mobod, Transform X_BS, ContactGeometry geometry) : - m_Mobod(std::move(mobod)), - m_Offset(std::move(X_BS)), - m_Geometry(geometry) {} - - const ContactGeometry& getGeometry() const {return m_Geometry;} - Transform calcSurfaceToGroundTransform(const State& state) const - { throw std::runtime_error("check transform order"); - return m_Mobod.getBodyTransform(state).compose(m_Offset);} - -private: - MobilizedBody m_Mobod; - Transform m_Offset; - ContactGeometry m_Geometry; -}; - -// Binds {ContactGeometry, Body, OffsetFrame} -// Cheap to copy, reuseable in the model. +// Represents the local surface wrapping problem. +// Caches last computed geodesic as a warmstart. +// Not exposed outside of simbody. +// Not shared amongst different paths or obstacles. class SimTK_SIMBODY_EXPORT Surface { -private: - Surface() = default; -public: - ~Surface() = default; - Surface(Surface&&) noexcept = default; - Surface& operator=(Surface&&) noexcept = default; - Surface(const Surface&) = default; - Surface& operator=(const Surface&) = default; - - Surface(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry) - : impl(std::make_shared(mobod, X_BS, geometry)) {} - - const ContactGeometry& getGeometry() const {return impl->getGeometry();} - Transform calcSurfaceToGroundTransform(const State& state) const {return impl->calcSurfaceToGroundTransform(state);} - -private: - std::shared_ptr impl = nullptr; -}; - -//============================================================================== -// SUBSYSTEM -//============================================================================== -class WrappingPath; - -class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem -{ -public: - WrappingPathSubsystem(); - explicit WrappingPathSubsystem(MultibodySystem&); - - int getNumPaths() const; - const WrappingPath& getPath(WrappingPathIndex idx) const; - WrappingPath& updPath(WrappingPathIndex idx); - - SimTK_PIMPL_DOWNCAST(WrappingPathSubsystem, Subsystem); - class Impl; - Impl& updImpl(); - const Impl& getImpl() const; -}; - -//============================================================================== -// OBSTACLE -//============================================================================== -// Although cheap to copy, we cannot hand them out because they have a cache entry associated with them. -// The surface they hold a pointer to can be reused in the model. -class SimTK_SIMBODY_EXPORT WrapObstacle -{ -public: using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; -private: - WrapObstacle() = default; - public: - WrapObstacle(const WrapObstacle& source) = default; - WrapObstacle& operator=(const WrapObstacle& source) = default; - ~WrapObstacle() = default; - - explicit WrapObstacle(Surface surface); + Surface() = default; + ~Surface() = default; + Surface(Surface&&) noexcept = default; + Surface& operator=(Surface&&) noexcept = default; + Surface(const Surface&) = delete; + Surface& operator=(const Surface&) = delete; - enum class Status - { - Ok, - NegativeLength, - Liftoff, - Disabled, - }; + Surface(const ContactGeometry& geometry, Vec3 xHint); - // Ground frame solution. - struct PosInfo + // TODO move to impl to hide? + struct LocalGeodesic { FrenetFrame KP {}; FrenetFrame KQ {}; @@ -138,188 +58,49 @@ class SimTK_SIMBODY_EXPORT WrapObstacle Variation dKQ {}; }; - // Solution previously commputed, all in local surface frame coordinates. - struct LocalGeodesicInfo - { - FrenetFrame KP {}; - FrenetFrame KQ {}; - - Real length = NaN; - - Variation dKP {}; - Variation dKQ {}; - - std::vector points {}; - double sHint = NaN; - - double lineTracking = NaN; - - Status status = Status::Ok; - }; - - // Allocate state variables and cache entries. - void realizeTopology(State& state); - void realizeInstance(const State& state) const; - void realizePosition(const State& state) const; - void realizeVelocity(const State& state) const; - void realizeAcceleration(const State& state) const; - void invalidateTopology(); - - Status getStatus(const State& state) const; - bool isActive(const State& state) const; - - const LocalGeodesicInfo& calcInitZeroLengthGeodesicGuess(State& s, Vec3 xPrev) const; - - bool isPointBelowSurface(const State& state, Vec3 point) const; - - const Transform& getSurfaceToBaseTransform(const state& state) const; - - Surface getSurface() const; - - const PosInfo& getGeodesic(const State& state) const; - - Vec3 getInitialPointGuess() const; - - WrapObstacle::PosInfo& updPosInfo(const State &state) const - { - return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); - } - - size_t writeGeodesicPoints(const State& state, std::vector points) const; - void applyGeodesicCorrection(const State& state, const WrapObstacle::Correction& c) const; + const LocalGeodesic& calcGeodesic(State& s, Vec3 x, Vec3 t, Real l); + const LocalGeodesic& applyGeodesicCorrection(const State& s, const Correction& c); + const LocalGeodesic& getGeodesic(const State& s); + Vec3 getPointOnLineNearSurface(const State& s); private: - const LocalGeodesicInfo& getLocalGeodesicInfo(const State& state) const - { - realizePosition(state); - return Value::downcast( - m_Subsystem.getDiscreteVarUpdateValue(state, m_WarmStartInfoDIx) - ); - } - - LocalGeodesicInfo& updLocalGeodesicInfo(const State& state) const - { - return Value::updDowncast( - m_Subsystem.updDiscreteVarUpdateValue(state, m_WarmStartInfoDIx) - ); - } - - const LocalGeodesicInfo& getPrevLocalGeodesicInfo(const State& state) const - { - return Value::downcast( - m_Subsystem.getDiscreteVariable(state, m_WarmStartInfoDIx) - ); - } - - LocalGeodesicInfo& updPrevLocalGeodesicInfo(State& state) const - { - return Value::updDowncast( - m_Subsystem.updDiscreteVariable(state, m_WarmStartInfoDIx) - ); - } - - void calcLocalGeodesic(Vec3 x, Vec3 t, Real l, Real sHint, - WrapObstacle::LocalGeodesicInfo& geodesic) const; - void calcPosInfo(const State& state, - const WrapObstacle::LocalGeodesicInfo& localGeodesic, - PosInfo& posInfo) const; - - Surface m_Surface; + class Impl; + explicit Surface(std::unique_ptr impl); + const Impl& getImpl() const; + Impl& updImpl(); - // Required for accessing the discrete variable? - WrappingPathSubsystem m_Subsystem; + friend Impl; + friend WrapObstacle; - // TOPOLOGY CACHE (set during realizeTopology()) - DiscreteVariableIndex m_WarmStartInfoDIx; - CacheEntryIndex m_PosInfoIx; + std::unique_ptr impl = nullptr; }; //============================================================================== -// PATH :: IMPL +// OBSTACLE //============================================================================== -class WrappingPathImpl { - public: - WrappingPathImpl( - WrappingPathSubsystem subsystem, - MobilizedBody originBody, - Vec3 originPoint, - MobilizedBody terminationBody, - Vec3 terminationPoint): - m_Subsystem(subsystem), - m_OriginBody(originBody), - m_OriginPoint(originPoint), - m_TerminationBody(terminationBody), - m_TerminationPoint(terminationPoint) {} - - struct LineSegment - { - UnitVec3 d {NaN, NaN, NaN}; - Real l = NaN; - }; - - struct PosInfo - { - Vec3 xO {NaN, NaN, NaN}; - Vec3 xI {NaN, NaN, NaN}; - - Real l = NaN; - - std::vector lines; - - Vector pathError {}; - Matrix pathMatrix {}; - Matrix pathErrorJacobian {}; - Vector pathCorrections {}; - - size_t loopIter = 0; - - // TODO solver matrices - }; - - std::vector& updObstacles() {return m_Obstacles;} - const std::vector& getObstacles() const {return m_Obstacles;} - - // Allocate state variables and cache entries. - void realizeTopology(State& state); - void realizePosition(const State& state) const; - void invalidateTopology() - { m_Subsystem.invalidateSubsystemTopologyCache(); } - - const PosInfo& getPosInfo(const State& state) const; - - void calcInitZeroLengthGeodesicSegmentsGuess(State& s) const; - - void callCurrentWithPrevAndNext( - const State& s, - std::function f); - - void callCurrentWithPrevAndNext( - const State& s, - std::function f) const; - - private: - PosInfo& updPosInfo(const State& s) const; - void calcPosInfo(const State& s, PosInfo& posInfo) const; - - WrappingPathSubsystem m_Subsystem; - - MobilizedBody m_OriginBody; - Vec3 m_OriginPoint; - - MobilizedBody m_TerminationBody; - Vec3 m_TerminationPoint; +// Although cheap to copy, we cannot hand them out because they have a cache entry associated with them. +// The surface they hold a pointer to can be reused in the model. +class SimTK_SIMBODY_EXPORT WrapObstacle +{ +public: + WrapObstacle() = default; + WrapObstacle(const WrapObstacle&) = delete; + WrapObstacle& operator = (const WrapObstacle&) = delete; + WrapObstacle(WrapObstacle&&) noexcept = default; + WrapObstacle& operator = (WrapObstacle&&) noexcept = default; + ~WrapObstacle() = default; - std::vector m_Obstacles {}; + WrapObstacle(const MobilizedBody& mobod, Transform X_BS, const ContactGeometry& geometry, Vec3 xHint); - Real m_PathErrorBound = 0.1; - size_t m_MaxIter = 10; +private: + class Impl; + explicit WrapObstacle(std::unique_ptr impl); - // TOPOLOGY CACHE (set during realizeTopology()) - CacheEntryIndex m_PosInfoIx; - CacheEntryIndex m_VelInfoIx; - CacheEntryIndex m_VizInfoIx; + friend WrappingPath; + const Impl& getImpl() const; + Impl& updImpl(); - friend class WrappingPath; + std::unique_ptr impl; }; //============================================================================== @@ -328,8 +109,6 @@ class WrappingPathImpl { class SimTK_SIMBODY_EXPORT WrappingPath { public: - using Impl = WrappingPathImpl; - WrappingPath( WrappingPathSubsystem& subsystem, const MobilizedBody& originBody, @@ -337,14 +116,16 @@ class SimTK_SIMBODY_EXPORT WrappingPath const MobilizedBody& terminationBody, const Vec3& defaultTerminationPoint); - std::vector& updObstacles() {return updImpl().updObstacles();} - const std::vector& getObstacles() const {return getImpl().getObstacles();} + std::vector& updObstacles(); + const std::vector& getObstacles(); - Real getLength(const State& state) const {return getImpl().getPosInfo(state).l; } + Real getLength(const State& state) const; private: - friend WrappingPathSubsystem::Impl; + class Impl; + explicit WrappingPath(std::unique_ptr impl); + friend WrappingPathSubsystem; const Impl& getImpl() const { return *impl; } Impl& updImpl() { return *impl; } @@ -352,90 +133,27 @@ class SimTK_SIMBODY_EXPORT WrappingPath }; //============================================================================== -// SUBSYSTEM :: IMPL +// SUBSYSTEM //============================================================================== -class WrappingPathSubsystem::Impl : public Subsystem::Guts { - public: - Impl() {} - ~Impl() {} - Impl* cloneImpl() const override - { return new Impl(*this); } - - int getNumPaths() const {return paths.size();} - - const WrappingPath& getCablePath(WrappingPathIndex index) const - { return paths[index]; } - - WrappingPath& updCablePath(WrappingPathIndex index) - { return paths[index]; } - - // Add a cable path to the list, bumping the reference count. - WrappingPathIndex adoptCablePath(WrappingPath& path) { - paths.push_back(path); - return WrappingPathIndex(paths.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(); } - - // 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); - - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - WrappingPath& path = wThis->updCablePath(ix); - path.updImpl().realizeTopology(state); - } - - return 0; - } - - int realizeSubsystemInstanceImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - const WrappingPath& path = getCablePath(ix); - path.getImpl().realizeInstance(state); - } - return 0; - } - - int realizeSubsystemPositionImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - const WrappingPath& path = getCablePath(ix); - path.getImpl().realizePosition(state); - } - return 0; - } - - int realizeSubsystemVelocityImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - const WrappingPath& path = getCablePath(ix); - path.getImpl().realizeVelocity(state); - } - return 0; - } - - - int realizeSubsystemAccelerationImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - const WrappingPath& path = getCablePath(ix); - path.getImpl().realizeAcceleration(state); - } - return 0; - } - - SimTK_DOWNCAST(Impl, Subsystem::Guts); - - private: - // TOPOLOGY STATE - Array_ paths; + +class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem +{ +public: + WrappingPathSubsystem(); + explicit WrappingPathSubsystem(MultibodySystem&); + + int getNumPaths() const; + const WrappingPath& getPath(WrappingPathIndex idx) const; + WrappingPath& updPath(WrappingPathIndex idx); + + size_t writePathPoints(std::vector& points) const; + size_t writePathFrames(std::vector& frenetFrames) const; + +/* private: */ + SimTK_PIMPL_DOWNCAST(WrappingPathSubsystem, Subsystem); + class Impl; + Impl& updImpl(); + const Impl& getImpl() const; }; } // namespace SimTK diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h new file mode 100644 index 000000000..43a3b1c83 --- /dev/null +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -0,0 +1,388 @@ +#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/Wrapping.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 +{ + +//============================================================================== +// SURFACE IMPL +//============================================================================== +class Surface::Impl { + using FrenetFrame = ContactGeometry::FrenetFrame; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; + +public: + Impl() = default; + ~Impl() = default; + Impl(Impl&&) noexcept = default; + Impl& operator=(Impl&&) noexcept = default; + + Impl(const Impl&) = delete; + Impl& operator=(const Impl&) = delete; + + struct GeodesicInitialConditions + { + Vec3 x {NaN, NaN, NaN}; + Vec3 t {NaN, NaN, NaN}; + Real l = NaN; + }; + + struct LocalGeodesicInfo : Surface::LocalGeodesic + { + std::vector points {}; + double sHint = NaN; + }; + + struct LineTracking + { + double c = NaN; + }; + + // The virtual methods: + // 1. calcGeodesic() + // 2. calcPointOnLineNearSurface + // 3. isPointAboveSurface + // 4. calcPathPoints + + Impl(ContactGeometry geometry) : m_Geometry(geometry) {} + + // 1. calcGeodesic + const LocalGeodesic& calcGeodesic(State& state) const; + const LocalGeodesic& getGeodesic(const State& state) const; + const LocalGeodesic& applyGeodesicCorrection(const State& state, const Correction& c); + + // 2. calcPointOnLineNearSurface + Vec3 getPointOnLineNearSurface(const State& state) const; + + // 3. isPointAboveSurface + bool isPointAboveSurface(Vec3 point) const; + + // 4. calcPathPoints + size_t calcPathPoints(const State& state, std::vector& points) const; + + // TODO should be in OpenSim? + void setInitialPointGuess(Vec3 pointGuess); + Vec3 getInitialPointGuess() const; + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void realizeAcceleration(const State& state) const; + void invalidateTopology(); + +private: + const LocalGeodesicInfo& getLocalGeodesicInfo(const State& state) const + { + realizePosition(state); + return Value::downcast( + m_Subsystem.getDiscreteVarUpdateValue(state, m_GeodesicInfoIx) + ); + } + + LocalGeodesicInfo& updLocalGeodesicInfo(const State& state) const + { + return Value::updDowncast( + m_Subsystem.updDiscreteVarUpdateValue(state, m_GeodesicInfoIx) + ); + } + + const LocalGeodesicInfo& getPrevLocalGeodesicInfo(const State& state) const + { + return Value::downcast( + m_Subsystem.getDiscreteVariable(state, m_GeodesicInfoIx) + ); + } + + LocalGeodesicInfo& updPrevLocalGeodesicInfo(State& state) const + { + return Value::updDowncast( + m_Subsystem.updDiscreteVariable(state, m_GeodesicInfoIx) + ); + } + + void calcLocalGeodesic(Vec3 x, Vec3 t, Real l, Real sHint, + LocalGeodesicInfo& geodesic) const; + +//------------------------------------------------------------------------------ + Vec3 xHint; + + WrappingPathSubsystem m_Subsystem; + + ContactGeometry m_Geometry; + + DiscreteVariableIndex m_GeodesicInfoIx; +}; + +//============================================================================== +// OBSTACLE IMPL +//============================================================================== +class WrapObstacle::Impl +{ + using FrenetFrame = ContactGeometry::FrenetFrame; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; + +private: + Impl() = default; + +public: + Impl(const Impl& source) = default; + Impl& operator=(const Impl& source) = default; + ~Impl() = default; + + /* Surface(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry) */ + /* : impl(std::make_shared(mobod, X_BS, geometry)) {} */ + /* Transform calcSurfaceToGroundTransform(const State& state) const {return impl->calcSurfaceToGroundTransform(state);} */ + + explicit Impl(Surface surface); + + enum class Status + { + Ok, + NegativeLength, + Liftoff, + Disabled, + }; + + // Ground frame solution. + struct PosInfo + { + FrenetFrame KP {}; + FrenetFrame KQ {}; + + Variation dKP {}; + Variation dKQ {}; + + Real length = NaN; + + Status status = Status::Ok; + }; + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizeInstance(const State& state) const; + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void realizeAcceleration(const State& state) const; + void invalidateTopology(); + + Status getStatus(const State& state) const; + bool isActive(const State& state) const; + + bool isPointBelowSurface(const State& state, Vec3 point) const; + + Surface getSurface() const; + + const PosInfo& getGeodesic(const State& state) const; + + Vec3 getInitialPointGuess() const; + + PosInfo& updPosInfo(const State &state) const + { + return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); + } + + size_t calcPathPoints(const State& state, std::vector& points) const; + void applyGeodesicCorrection(const State& state, const ContactGeometry::GeodesicCorrection& c) const; + +private: + void calcPosInfo(const State& state, PosInfo& posInfo) const; + + Surface m_Surface; + MobilizedBody m_Mobod; + Transform m_Offset; + + // TODO Required for accessing the cache variable? + WrappingPathSubsystem m_Subsystem; + + // TOPOLOGY CACHE + CacheEntryIndex m_PosInfoIx; +}; + + +//============================================================================== +// PATH :: IMPL +//============================================================================== +class WrappingPath::Impl { + public: + Impl( + WrappingPathSubsystem subsystem, + MobilizedBody originBody, + Vec3 originPoint, + MobilizedBody terminationBody, + Vec3 terminationPoint): + m_Subsystem(subsystem), + m_OriginBody(originBody), + m_OriginPoint(originPoint), + m_TerminationBody(terminationBody), + m_TerminationPoint(terminationPoint) {} + + struct LineSegment + { + UnitVec3 d {NaN, NaN, NaN}; + Real l = NaN; + }; + + struct PosInfo + { + Vec3 xO {NaN, NaN, NaN}; + Vec3 xI {NaN, NaN, NaN}; + + Real l = NaN; + + std::vector lines; + + Vector pathError {}; + Matrix pathMatrix {}; + Matrix pathErrorJacobian {}; + Vector pathCorrections {}; + + size_t loopIter = 0; + + // TODO solver matrices + }; + + std::vector& updObstacles() {return m_Obstacles;} + const std::vector& getObstacles() const {return m_Obstacles;} + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void realizeAcceleration(const State& state) const; + void invalidateTopology() + { m_Subsystem.invalidateSubsystemTopologyCache(); } + + const PosInfo& getPosInfo(const State& state) const; + + void calcInitZeroLengthGeodesicSegmentsGuess(State& s) const; + + void callCurrentWithPrevAndNext( + const State& s, + std::function f); + + void callCurrentWithPrevAndNext( + const State& s, + std::function f) const; + + private: + PosInfo& updPosInfo(const State& s) const; + void calcPosInfo(const State& s, PosInfo& posInfo) const; + + WrappingPathSubsystem m_Subsystem; + + MobilizedBody m_OriginBody; + Vec3 m_OriginPoint; + + MobilizedBody m_TerminationBody; + Vec3 m_TerminationPoint; + + std::vector m_Obstacles {}; + + Real m_PathErrorBound = 0.1; + size_t m_MaxIter = 10; + + // TOPOLOGY CACHE (set during realizeTopology()) + CacheEntryIndex m_PosInfoIx; + CacheEntryIndex m_VelInfoIx; + CacheEntryIndex m_VizInfoIx; + + friend class WrappingPath; +}; + +//============================================================================== +// SUBSYSTEM :: IMPL +//============================================================================== +class WrappingPathSubsystem::Impl : public Subsystem::Guts { + public: + Impl() {} + ~Impl() {} + Impl* cloneImpl() const override + { return new Impl(*this); } + + int getNumPaths() const {return paths.size();} + + const WrappingPath& getCablePath(WrappingPathIndex index) const + { return paths[index]; } + + WrappingPath& updCablePath(WrappingPathIndex index) + { return paths[index]; } + + // Add a cable path to the list, bumping the reference count. + WrappingPathIndex adoptCablePath(WrappingPath& path) { + paths.push_back(path); + return WrappingPathIndex(paths.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(); } + + // 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); + + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + WrappingPath& path = wThis->updCablePath(ix); + path.updImpl().realizeTopology(state); + } + + return 0; + } + + int realizeSubsystemPositionImpl(const State& state) const override { + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + const WrappingPath& path = getCablePath(ix); + path.getImpl().realizePosition(state); + } + return 0; + } + + int realizeSubsystemVelocityImpl(const State& state) const override { + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + const WrappingPath& path = getCablePath(ix); + path.getImpl().realizeVelocity(state); + } + return 0; + } + + + int realizeSubsystemAccelerationImpl(const State& state) const override { + for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { + const WrappingPath& path = getCablePath(ix); + path.getImpl().realizeAcceleration(state); + } + return 0; + } + + SimTK_DOWNCAST(Impl, Subsystem::Guts); + + private: + // TOPOLOGY STATE + Array_ paths; +}; + +} // namespace SimTK + +#endif From 399adb7e7dfb3660c0060a98965065ce671ad66a Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 22 Apr 2024 17:11:02 +0200 Subject: [PATCH 018/127] wip: Surface and WrapObstacle impl refactor sketch --- .../simmath/internal/ContactGeometry.h | 2 +- Simbody/include/simbody/internal/Wrapping.cpp | 417 +++++++++++------- Simbody/include/simbody/internal/Wrapping.h | 25 +- .../include/simbody/internal/WrappingImpl.h | 115 ++--- 4 files changed, 351 insertions(+), 208 deletions(-) diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h index d8e078457..06380545f 100644 --- a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h +++ b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h @@ -162,7 +162,7 @@ struct PointOnLineResult size_t iter = 0; }; -PointOnLineResult calcNearestPointOnLine(Vec3 a, Vec3 b, Vec3 hint, size_t maxIter, double eps); +PointOnLineResult calcNearestPointOnLine(Vec3 a, Vec3 b, Vec3 hint, size_t maxIter, double eps) const; // TODO class Cone; diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 8bd00d91e..f828e016e 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1,6 +1,7 @@ #include "SimTKcommon/internal/CoordinateAxis.h" #include "SimTKmath.h" #include "Wrapping.h" +#include "WrappingImpl.h" #include "simmath/internal/ContactGeometry.h" #include @@ -12,71 +13,256 @@ using namespace SimTK; namespace { -using GeodesicJacobian = Vec4; -using LineSegment = WrappingPathImpl::LineSegment; -using PointVariation = ContactGeometry::GeodesicPointVariation; -using FrameVariation = ContactGeometry::GeodesicFrameVariation; -using Variation = ContactGeometry::GeodesicVariation; -using Correction = ContactGeometry::GeodesicCorrection; +namespace { + using FrenetFrame = ContactGeometry::FrenetFrame; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; + using GeodesicInitialConditions = Surface::Impl::GeodesicInitialConditions; -using Geodesic = WrapObstacle::PosInfo; -using LocalGeodesic = WrapObstacle::LocalGeodesicInfo; + GeodesicInitialConditions calcCorrectedGeodesicInitConditions(const FrenetFrame& KP, const Variation& dKP, Real l, const Correction& c) + { + Surface::Impl::GeodesicInitialConditions g0; -static const CoordinateAxis TangentAxis = ContactGeometry::TangentAxis; -static const CoordinateAxis NormalAxis = ContactGeometry::NormalAxis; -static const CoordinateAxis BinormalAxis = ContactGeometry::BinormalAxis; -static const int GeodesicDOF = 4; -static const int N_PATH_CONSTRAINTS = 4; + Vec3 v = dKP[1] * c; + g0.x = KP.p() + v; -using Status = WrapObstacle::Status; + Vec3 w = dKP[0] * c; + const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); + g0.t = t + cross(w,t); - Vec3 calcCorrectedGeodesicStartPoint( - const ContactGeometry::GeodesicCorrection& c, - const ContactGeometry::GeodesicVariation& dKP, - const ContactGeometry::FrenetFrame& KP - ) - { - Vec3 v = dKP[1] * c; - return KP.p() + v; - } + Real dl = c[3]; + g0.l = l + dl; - Vec3 calcCorrectedGeodesicStartTangent( - const ContactGeometry::GeodesicCorrection& c, - const ContactGeometry::GeodesicVariation& dKP, - const ContactGeometry::FrenetFrame& KP - ) - { - Vec3 w = dKP[0] * c; - const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); - return t + cross(w, t); - } + return g0; + } +} + +//============================================================================== +// SURFACE IMPL +//============================================================================== + +//------------------------------------------------------------------------------ +// REALIZE CACHE +//------------------------------------------------------------------------------ +void Surface::Impl::realizeTopology(State &state) +{ + // Allocate an auto-update discrete variable for the last computed geodesic. + LocalGeodesicInfo geodesic {}; + m_GeodesicInfoIx = m_Subsystem.allocateAutoUpdateDiscreteVariable(state, Stage::Velocity, new Value(geodesic), Stage::Position); +} + +void Surface::Impl::realizeInstance(const State &state) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} - double calcCorrectedGeodesicLength( - const ContactGeometry::GeodesicCorrection& c, - Real length) - { - return length + c[3]; +void Surface::Impl::realizePosition(const State &state) const +{ + // Set the current local geodesic equal to the previous. + if (!m_Subsystem.isDiscreteVarUpdateValueRealized(state, m_GeodesicInfoIx)) { + updLocalGeodesicInfo(state) = getPrevLocalGeodesicInfo(state); + m_Subsystem.markDiscreteVarUpdateValueRealized(state, m_GeodesicInfoIx); } +} + +void Surface::Impl::realizeVelocity(const State &state) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +void Surface::Impl::realizeAcceleration(const State &state) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +//------------------------------------------------------------------------------ +// PUBLIC METHODS +//------------------------------------------------------------------------------ +void Surface::Impl::applyGeodesicCorrection(const State& state, const Correction& c) const +{ + // Get the previous geodesic. + const LocalGeodesicInfo& g = getLocalGeodesicInfo(state); + + // Get corrected initial conditions. + const GeodesicInitialConditions g0 = calcCorrectedGeodesicInitConditions(g.KP, g.dKP, g.length, c); + + // Shoot the new geodesic. + calcLocalGeodesicInfo(g0.x, g0.t, g0.l, g.sHint, updLocalGeodesicInfo(state)); +} + +const Surface::WrappingStatus& Surface::Impl::calcWrappingStatus( + const State& state, + Vec3 prevPoint, + Vec3 nextPoint, size_t maxIter, Real eps) const +{ + LocalGeodesicInfo& g = updLocalGeodesicInfo(state); + + if (g.disabled) {return g;} + + // Make sure that the previous point does not lie inside the surface. + if (m_Geometry.calcSurfaceValue(prevPoint)) { + // TODO use proper assert. + throw std::runtime_error("Unable to wrap over surface: Preceding point lies inside the surface"); + } + if (m_Geometry.calcSurfaceValue(nextPoint)) { + // TODO use proper assert. + throw std::runtime_error("Unable to wrap over surface: Next point lies inside the surface"); + } + + // Update the tracking point. + ContactGeometry::PointOnLineResult result = m_Geometry.calcNearestPointOnLine(prevPoint, nextPoint, g.pointOnLine, maxIter, eps); + g.pointOnLine = result.p; // TODO rename to point. + + // Attempt touchdown. + if (g.liftoff) { + const bool touchdown = result.isInsideSurface; + g.liftoff = !touchdown; + } + + // Detect liftoff. + if (!g.liftoff) { + g.liftoff = g.length == 0.; + g.liftoff &= dot(prevPoint - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0.; + g.liftoff &= dot(nextPoint - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0.; + } + + return g; +} + +size_t Surface::Impl::calcPathPoints(const State& state, std::vector& points) const +{ + size_t count = 0; + const LocalGeodesicInfo& g = getLocalGeodesicInfo(state); + for (Vec3 p: g.points) { + points.push_back(p); + } + return count; +} - void xformSurfaceGeodesicToBase( - const WrapObstacle::LocalGeodesicInfo& geodesic_S, - WrapObstacle::PosInfo& geodesic_B, - Transform& X_BS) { +void Surface::Impl::setInitialPointGuess(Vec3 pointGuess) { + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +Vec3 Surface::Impl::getInitialPointGuess() const { + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +//------------------------------------------------------------------------------ +// PRIVATE METHODS +//------------------------------------------------------------------------------ +void Surface::Impl::calcLocalGeodesicInfo(Vec3 x, Vec3 t, Real l, Real sHint, + LocalGeodesicInfo& geodesic) const +{ + // Compute geodesic start boundary frame and variation. + m_Geometry.calcNearestFrenetFrameFast(x, t, geodesic.KP); + m_Geometry.calcGeodesicStartFrameVariation(geodesic.KP, geodesic.dKP); - // TODO check transformation order. - geodesic_B.KP = X_BS.compose(geodesic_S.KP); - geodesic_B.KQ = X_BS.compose(geodesic_S.KQ); + // Compute geodesic end boundary frame amd variation (shoot new geodesic). + m_Geometry.calcGeodesicEndFrameVariationImplicitly( + geodesic.KP.p(), + geodesic.KP.R().getAxisUnitVec(ContactGeometry::TangentAxis), + l, + sHint, + geodesic.KQ, + geodesic.dKQ, + geodesic.points); - geodesic_B.dKP[0] = X_BS.R() * geodesic_S.dKP[0]; - geodesic_B.dKP[1] = X_BS.R() * geodesic_S.dKP[1]; + // TODO update step size. + // TODO update line tracking? + throw std::runtime_error("NOTYETIMPLEMENTED"); +} - geodesic_B.dKQ[0] = X_BS.R() * geodesic_S.dKQ[0]; - geodesic_B.dKQ[1] = X_BS.R() * geodesic_S.dKQ[1]; +//============================================================================== +// OBSTACLE +//============================================================================== - geodesic_B.length = geodesic_S.length; +void WrapObstacle::Impl::realizeTopology(State &state) +{ + // Allocate position level cache. + PosInfo posInfo {}; + m_PosInfoIx = m_Subsystem.allocateCacheEntry(state, Stage::Position, new Value(posInfo)); +} - throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); +void WrapObstacle::Impl::realizePosition(const State &state) const +{ + if (!m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) { + calcPosInfo(state, updPosInfo(state)); + m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); } +} + +const WrapObstacle::Impl::PosInfo& WrapObstacle::Impl::getPosInfo(const State &state) const +{ + return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); +} + +void WrapObstacle::Impl::applyGeodesicCorrection(const State& state, const WrapObstacle::Impl::Correction& c) const +{ + // Apply correction to curve. + m_Surface.getImpl().applyGeodesicCorrection(state, c); + + // Invalidate position level cache. + m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); +} + +void WrapObstacle::Impl::calcPosInfo( + const State& state, + PosInfo& posInfo) const +{ + // Transform the local geodesic to ground frame. + Transform X_GS = m_Mobod.getBodyTransform(state).compose(m_Offset); + const Surface::LocalGeodesic& geodesic_S = m_Surface.getImpl().getGeodesic(state); + + posInfo.KP = X_GS.compose(geodesic_S.KP); + posInfo.KQ = X_GS.compose(geodesic_S.KQ); + + posInfo.dKP[0] = X_GS.R() * geodesic_S.dKP[0]; + posInfo.dKP[1] = X_GS.R() * geodesic_S.dKP[1]; + + posInfo.dKQ[0] = X_GS.R() * geodesic_S.dKQ[0]; + posInfo.dKQ[1] = X_GS.R() * geodesic_S.dKQ[1]; + + posInfo.length = geodesic_S.length; + + throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); +} + +void WrapObstacle::Impl::calcPosInfo( + const State& state, + Vec3 prev, + Vec3 next, + size_t maxIter, Real eps) const +{ + m_Surface.getImpl().calcWrappingStatus(state, prev, next, maxIter, eps); + + if(isDisabled(state)) + { + return; + } + + calcPosInfo(state, updPosInfo(state)); +} + +void WrapObstacle::Impl::calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const +{ + m_Surface.getImpl().calcGeodesic(state, x, t, l); + m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); +} + +//============================================================================== +// PATH HELPERS +//============================================================================== + +namespace +{ + using GeodesicJacobian = Vec4; + using LineSegment = WrappingPath::LineSegment; + using PointVariation = ContactGeometry::GeodesicPointVariation; + using FrameVariation = ContactGeometry::GeodesicFrameVariation; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; + using Geodesic = WrapObstacle::Impl::PosInfo; +/* using LocalGeodesic = WrapObstacle::LocalGeodesicInfo; */ int countActive(const State& s, const std::vector& obstacles) { @@ -89,6 +275,33 @@ int countActive(const State& s, const std::vector& obstacles) return n; } +} + +//============================================================================== +// PATH IMPL +//============================================================================== + +void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& state, std::function GetInitPointGuess) const +{ + Vec3 prev = m_OriginBody.getBodyTransform(state).shiftFrameStationToBase(m_OriginPoint); + size_t i = 0; + for (const WrapObstacle& obstacle: m_Obstacles) { + Vec3 xGuess = GetInitPointGuess(++i); + obstacle.getImpl().calcGeodesic(state, xGuess, xGuess - prev, 0.); + + prev = obstacle.getImpl().getPosInfo(state).KQ.p(); + } +} + +//============================================================================== +// WRAP OBSTACLE IMPL +//============================================================================== + +namespace +{ + +static const int N_PATH_CONSTRAINTS = 4; + using ActiveLambda = std::function< void(size_t prev, size_t next, size_t current, bool isActive)>; @@ -339,23 +552,6 @@ void calcLineSegments( } -const WrapObstacle::LocalGeodesicInfo& WrapObstacle::calcInitZeroLengthGeodesicGuess(State& s, Vec3 xPrev) const -{ - Vec3 x = getInitialPointGuess(); - Vec3 t = (x - xPrev); - - // Shoot a zero-length geodesic as initial guess. - calcLocalGeodesic(x, t, 0., 0., updPrevLocalGeodesicInfo(s)); -} - -void WrappingPath::Impl::calcInitZeroLengthGeodesicSegmentsGuess(State& s) const -{ - Vec3 prev = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); - for (const WrapObstacle& obstacle: m_Obstacles) { - prev = obstacle.calcInitZeroLengthGeodesicGuess(s, prev).KQ.p(); - } -} - //============================================================================== // SUBSYSTEM //============================================================================== @@ -506,88 +702,3 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo, bool prev throw std::runtime_error("Failed to converge"); } - -//============================================================================== -// OBSTACLE -//============================================================================== - -void WrapObstacle::realizeTopology(State &state) -{ - // Allocate an auto-update discrete variable for the last computed geodesic. - LocalGeodesicInfo warmStartInfo {}; - m_WarmStartInfoDIx = m_Subsystem.allocateAutoUpdateDiscreteVariable(state, Stage::Velocity, new Value(warmStartInfo), Stage::Position); - - // Allocate position level cache. - PosInfo posInfo {}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry(state, Stage::Position, new Value(posInfo)); -} - -void WrapObstacle::realizePosition(const State &state) const -{ - // Set the current local geodesic equal to the previous. - if (!m_Subsystem.isDiscreteVarUpdateValueRealized(state, m_WarmStartInfoDIx)) { - updLocalGeodesicInfo(state) = getPrevLocalGeodesicInfo(state); - m_Subsystem.markDiscreteVarUpdateValueRealized(state, m_WarmStartInfoDIx); - } -} - -const WrapObstacle::PosInfo& WrapObstacle::getGeodesic(const State &state) const -{ - if (!m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) { - calcPosInfo(state, getLocalGeodesicInfo(state), updPosInfo(state)); - m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); - } - return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); -} - -void WrapObstacle::applyGeodesicCorrection(const State& state, const WrapObstacle::Correction& c) const -{ - // Get prev geodesic. - const LocalGeodesicInfo& geodesic = getLocalGeodesicInfo(state); - - // Apply geodesic correction. - Vec3 x = calcCorrectedGeodesicStartPoint(c, geodesic.dKP, geodesic.KP); - Vec3 t = calcCorrectedGeodesicStartTangent(c, geodesic.dKP, geodesic.KP); - Real l = calcCorrectedGeodesicLength(c, geodesic.length); - Real sHint = geodesic.sHint; - - // Shoot the new geodesic. - calcLocalGeodesic(x, t, l, sHint, updLocalGeodesicInfo(state)); - - // Invalidate position level cache. - m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); -} - -void WrapObstacle::calcLocalGeodesic( Vec3 x, Vec3 t, Real l, Real sHint, - WrapObstacle::LocalGeodesicInfo& geodesic) const -{ - const ContactGeometry& geometry = m_Surface.getGeometry(); - - // Compute geodesic start boundary frame. - geometry.calcNearestFrenetFrameFast(x, t, geodesic.KP); - geometry.calcGeodesicStartFrameVariation(geodesic.KP, geodesic.dKP); - - // Compute geodesic end boundary frame (shoot new geodesic). - geometry.calcGeodesicEndFrameVariationImplicitly( - geodesic.KP.p(), - geodesic.KP.R().getAxisUnitVec(ContactGeometry::TangentAxis), - l, - sHint, - geodesic.KQ, - geodesic.dKQ, - geodesic.points); - - // TODO update step size. - // TODO update line tracking? - throw std::runtime_error("NOTYETIMPLEMENTED"); -} - -void WrapObstacle::calcPosInfo( - const State& state, - const WrapObstacle::LocalGeodesicInfo& localGeodesic, - PosInfo& posInfo) const -{ - // Transform the local geodesic to ground frame. - Transform X_BS = m_Surface.calcSurfaceToGroundTransform(state); - xformSurfaceGeodesicToBase(localGeodesic, posInfo, X_BS); -} diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index fb380804f..75fc07c03 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -44,7 +44,7 @@ class SimTK_SIMBODY_EXPORT Surface Surface(const Surface&) = delete; Surface& operator=(const Surface&) = delete; - Surface(const ContactGeometry& geometry, Vec3 xHint); + Surface(WrappingPathSubsystem subsystem, const ContactGeometry& geometry, Vec3 xHint); // TODO move to impl to hide? struct LocalGeodesic @@ -58,13 +58,22 @@ class SimTK_SIMBODY_EXPORT Surface Variation dKQ {}; }; + // TODO move to impl to hide? + struct WrappingStatus + { + Vec3 pointOnLine {NaN, NaN, NaN}; + + bool liftoff = false; + bool disabled = false; + }; + const LocalGeodesic& calcGeodesic(State& s, Vec3 x, Vec3 t, Real l); const LocalGeodesic& applyGeodesicCorrection(const State& s, const Correction& c); const LocalGeodesic& getGeodesic(const State& s); Vec3 getPointOnLineNearSurface(const State& s); -private: class Impl; +private: explicit Surface(std::unique_ptr impl); const Impl& getImpl() const; Impl& updImpl(); @@ -91,9 +100,10 @@ class SimTK_SIMBODY_EXPORT WrapObstacle ~WrapObstacle() = default; WrapObstacle(const MobilizedBody& mobod, Transform X_BS, const ContactGeometry& geometry, Vec3 xHint); + bool isActive(const State& state) const; -private: class Impl; +private: explicit WrapObstacle(std::unique_ptr impl); friend WrappingPath; @@ -108,6 +118,13 @@ class SimTK_SIMBODY_EXPORT WrapObstacle //============================================================================== class SimTK_SIMBODY_EXPORT WrappingPath { +public: + struct LineSegment + { + UnitVec3 d {NaN, NaN, NaN}; + Real l = NaN; + }; + public: WrappingPath( WrappingPathSubsystem& subsystem, @@ -121,8 +138,8 @@ class SimTK_SIMBODY_EXPORT WrappingPath Real getLength(const State& state) const; -private: class Impl; +private: explicit WrappingPath(std::unique_ptr impl); friend WrappingPathSubsystem; diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 43a3b1c83..c6d5d3f47 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -23,6 +23,7 @@ class Surface::Impl { using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; + using PointOnLineResult = ContactGeometry::PointOnLineResult; public: Impl() = default; @@ -40,16 +41,32 @@ class Surface::Impl { Real l = NaN; }; - struct LocalGeodesicInfo : Surface::LocalGeodesic + struct LocalGeodesicInfo : Surface::LocalGeodesic, Surface::WrappingStatus { std::vector points {}; double sHint = NaN; }; - struct LineTracking - { - double c = NaN; - }; + Impl( + WrappingPathSubsystem subsystem, + ContactGeometry geometry, + Vec3 initPointGuess + ) : + m_Subsystem(subsystem), + m_Geometry(geometry), + m_InitPointGuess(initPointGuess) + {} + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + // TODO requires all levels? + void realizeInstance(const State& state) const; + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void realizeAcceleration(const State& state) const; + // Requires invalidateTopology? + /* void invalidateTopology() */ + /* { m_Subsystem.invalidateSubsystemTopologyCache(); } */ // The virtual methods: // 1. calcGeodesic() @@ -57,33 +74,26 @@ class Surface::Impl { // 3. isPointAboveSurface // 4. calcPathPoints - Impl(ContactGeometry geometry) : m_Geometry(geometry) {} - // 1. calcGeodesic - const LocalGeodesic& calcGeodesic(State& state) const; - const LocalGeodesic& getGeodesic(const State& state) const; - const LocalGeodesic& applyGeodesicCorrection(const State& state, const Correction& c); - - // 2. calcPointOnLineNearSurface - Vec3 getPointOnLineNearSurface(const State& state) const; + const LocalGeodesic& calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; + /* void calcGeodesic(GeodesicInitialConditions g0, Real sHint, LocalGeodesic& geodesic) const; */ + /* void calcGeodesic(Vec3 x, Vec3 t, Real l, Real sHint, LocalGeodesic& geodesic) const; */ + const LocalGeodesic& getGeodesic(const State& state) const {return getLocalGeodesicInfo(state);} + void applyGeodesicCorrection(const State& state, const Correction& c) const; - // 3. isPointAboveSurface - bool isPointAboveSurface(Vec3 point) const; + // TODO rename + bool isActive(const State& state) const; // 4. calcPathPoints size_t calcPathPoints(const State& state, std::vector& points) const; + // Detect liftoff or touchdown, and compute the point on line near the surface. + const WrappingStatus& calcWrappingStatus(const State& state, Vec3 prev, Vec3 next, size_t maxIter, Real eps) const; + // TODO should be in OpenSim? void setInitialPointGuess(Vec3 pointGuess); Vec3 getInitialPointGuess() const; - // Allocate state variables and cache entries. - void realizeTopology(State& state); - void realizePosition(const State& state) const; - void realizeVelocity(const State& state) const; - void realizeAcceleration(const State& state) const; - void invalidateTopology(); - private: const LocalGeodesicInfo& getLocalGeodesicInfo(const State& state) const { @@ -114,16 +124,16 @@ class Surface::Impl { ); } - void calcLocalGeodesic(Vec3 x, Vec3 t, Real l, Real sHint, + void calcLocalGeodesicInfo(Vec3 x, Vec3 t, Real l, Real sHint, LocalGeodesicInfo& geodesic) const; //------------------------------------------------------------------------------ - Vec3 xHint; - WrappingPathSubsystem m_Subsystem; ContactGeometry m_Geometry; + Vec3 m_InitPointGuess; + DiscreteVariableIndex m_GeodesicInfoIx; }; @@ -148,12 +158,22 @@ class WrapObstacle::Impl /* : impl(std::make_shared(mobod, X_BS, geometry)) {} */ /* Transform calcSurfaceToGroundTransform(const State& state) const {return impl->calcSurfaceToGroundTransform(state);} */ - explicit Impl(Surface surface); + Impl( + WrappingPathSubsystem subsystem, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess + ) : + m_Subsystem(subsystem), + m_Mobod(mobod), + m_Offset(X_BS), + m_Surface(subsystem, geometry, initPointGuess) + {} enum class Status { Ok, - NegativeLength, Liftoff, Disabled, }; @@ -174,22 +194,16 @@ class WrapObstacle::Impl // Allocate state variables and cache entries. void realizeTopology(State& state); - void realizeInstance(const State& state) const; + /* void realizeInstance(const State& state) const; */ void realizePosition(const State& state) const; - void realizeVelocity(const State& state) const; - void realizeAcceleration(const State& state) const; - void invalidateTopology(); - - Status getStatus(const State& state) const; - bool isActive(const State& state) const; - - bool isPointBelowSurface(const State& state, Vec3 point) const; + /* void realizeVelocity(const State& state) const; */ + /* void realizeAcceleration(const State& state) const; */ + /* void invalidateTopology(); */ - Surface getSurface() const; + const PosInfo& getPosInfo(const State& state) const; - const PosInfo& getGeodesic(const State& state) const; - - Vec3 getInitialPointGuess() const; + bool isActive(const State& state) const; + bool isDisabled(const State& state) const; PosInfo& updPosInfo(const State &state) const { @@ -199,15 +213,19 @@ class WrapObstacle::Impl size_t calcPathPoints(const State& state, std::vector& points) const; void applyGeodesicCorrection(const State& state, const ContactGeometry::GeodesicCorrection& c) const; + void calcPosInfo(const State& state, Vec3 prev, Vec3 next, size_t maxIter, Real eps) const; + void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; + private: void calcPosInfo(const State& state, PosInfo& posInfo) const; - Surface m_Surface; + // TODO Required for accessing the cache variable? + WrappingPathSubsystem m_Subsystem; + MobilizedBody m_Mobod; Transform m_Offset; - // TODO Required for accessing the cache variable? - WrappingPathSubsystem m_Subsystem; + Surface m_Surface; // TOPOLOGY CACHE CacheEntryIndex m_PosInfoIx; @@ -231,12 +249,6 @@ class WrappingPath::Impl { m_TerminationBody(terminationBody), m_TerminationPoint(terminationPoint) {} - struct LineSegment - { - UnitVec3 d {NaN, NaN, NaN}; - Real l = NaN; - }; - struct PosInfo { Vec3 xO {NaN, NaN, NaN}; @@ -269,7 +281,10 @@ class WrappingPath::Impl { const PosInfo& getPosInfo(const State& state) const; - void calcInitZeroLengthGeodesicSegmentsGuess(State& s) const; + void calcInitZeroLengthGeodesic(State& state, std::function GetInitPointGuess) const; + + WrapObstacle* findPrevActiveObstacle(const State& s, size_t obsIdx); + WrapObstacle* findNextActiveObstacle(const State& s, size_t obsIdx); void callCurrentWithPrevAndNext( const State& s, From fcf08b1b69871677fa7e84ad4d72080df73c0f7f Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 23 Apr 2024 09:36:41 +0200 Subject: [PATCH 019/127] wip: implement WrappingPath::Impl wip --- Simbody/include/simbody/internal/Wrapping.cpp | 562 +++++++++--------- .../include/simbody/internal/WrappingImpl.h | 59 +- 2 files changed, 340 insertions(+), 281 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index f828e016e..7445e09b1 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1,17 +1,79 @@ #include "SimTKcommon/internal/CoordinateAxis.h" +#include "SimTKcommon/internal/ExceptionMacros.h" #include "SimTKmath.h" #include "Wrapping.h" #include "WrappingImpl.h" #include "simmath/internal/ContactGeometry.h" +#include #include using namespace SimTK; +using GeodesicInfo = WrapObstacle::Impl::PosInfo; + +//============================================================================== +// CONSTANTS +//============================================================================== +namespace { + static const CoordinateAxis TangentAxis = ContactGeometry::TangentAxis; + static const CoordinateAxis NormalAxis = ContactGeometry::NormalAxis; + static const CoordinateAxis BinormalAxis = ContactGeometry::BinormalAxis; + static const int GeodesicDOF = 4; +} + +//============================================================================== +// SOLVER +//============================================================================== + +namespace { + struct SolverData; + + struct SolverData + { + // A * x = b + Matrix A; + Vector x; + Vector b; + }; + + struct SolverDataCache + { + SolverDataCache(std::mutex&& cacheMutex) : + m_guard(cacheMutex) {} + + void setDataPtr(SolverData* ptr) {m_data = ptr;} + + SolverData& updData() {return *m_data;} + + private: + std::lock_guard m_guard; + SolverData* m_data = nullptr; + }; + + SolverDataCache&& updSolverDataCache(size_t nActive) + { + static constexpr int Q = 4; + static constexpr int C = 4; + static std::vector s_GlobalCache {}; + static std::mutex s_GlobalLock {}; + + SolverDataCache data(std::move(s_GlobalLock)); + + for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { + int n = i + 1; + s_GlobalCache.emplace_back( + Matrix{C * n, Q * n, 0.}, + Vector{Q * n, 0.}, + Vector{C * n, 0.}); + } + + return std::move(data); + } +} // namespace + //============================================================================== // SOME MATH //============================================================================== -namespace -{ namespace { using FrenetFrame = ContactGeometry::FrenetFrame; @@ -261,7 +323,6 @@ namespace using FrameVariation = ContactGeometry::GeodesicFrameVariation; using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; - using Geodesic = WrapObstacle::Impl::PosInfo; /* using LocalGeodesic = WrapObstacle::LocalGeodesicInfo; */ int countActive(const State& s, const std::vector& obstacles) @@ -275,100 +336,43 @@ int countActive(const State& s, const std::vector& obstacles) return n; } -} - -//============================================================================== -// PATH IMPL -//============================================================================== - -void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& state, std::function GetInitPointGuess) const -{ - Vec3 prev = m_OriginBody.getBodyTransform(state).shiftFrameStationToBase(m_OriginPoint); - size_t i = 0; - for (const WrapObstacle& obstacle: m_Obstacles) { - Vec3 xGuess = GetInitPointGuess(++i); - obstacle.getImpl().calcGeodesic(state, xGuess, xGuess - prev, 0.); - - prev = obstacle.getImpl().getPosInfo(state).KQ.p(); - } -} - -//============================================================================== -// WRAP OBSTACLE IMPL -//============================================================================== - -namespace -{ - -static const int N_PATH_CONSTRAINTS = 4; - -using ActiveLambda = std::function< - void(size_t prev, size_t next, size_t current, bool isActive)>; - -void CallCurrentWithPrevAndNext( - size_t prev, - size_t next, - size_t current, - bool isActive, - ActiveLambda& f) -{ - f(prev, next, current, isActive); -} - -template -void CallCurrentWithPrevAndNext( - size_t prev, - size_t next, - size_t current, - bool isActive, - ActiveLambda& f, - FUNCS&&... fs) +const WrapObstacle* FindPrevActiveObstacle( + const State& state, + const std::vector& obs, + size_t idx) { - f(prev, next, current, isActive); - CallCurrentWithPrevAndNext( - prev, - next, - current, - isActive, - std::forward(fs)...); + for (ptrdiff_t i = idx - 1; i > 0; --i) { + // Find the active segment before the current. + if (obs.at(i).isActive(state)) { + return &obs.at(i); + } + } + return nullptr; } -template -void MapWithPrevAndNext( - const State& s, +const WrapObstacle* FindNextActiveObstacle( + const State& state, const std::vector& obs, - ActiveLambda& f, - FUNCS&&... fs) + size_t idx) { - const ptrdiff_t n = obs.size(); - ptrdiff_t next = 0; - ptrdiff_t prev = -1; - - for (ptrdiff_t i = 0; i < n; ++i) { - // Find the active segment before the current. - if (i > 0) { - if (obs.at(i - 1).isActive(s)) { - prev = i - 1; - } + // Find the active segment after the current. + for (ptrdiff_t i = idx + 1; i < obs.size(); ++i) { + if (obs.at(i).isActive(state)) { + return &obs.at(i); } + } + return nullptr; +} - // Find the active segment after the current. - if (next <= i) { - for (; ++next < n;) { - const WrapObstacle& o = obs.at(next); - if (o.isActive(s)) { - break; - } - } - } +static const int N_PATH_CONSTRAINTS = 4; - CallCurrentWithPrevAndNext( - prev < 0 ? n : prev, - next, - i, - obs.at(i).isActive(s), - f, - std::forward(fs)...); +// TODO this is awkward +void addBlock( + const Vec4& values, + Matrix& block) +{ + for (int i = 0; i < Vec4::size(); ++i) { + block[0][i] = values[i]; } } @@ -376,12 +380,12 @@ void addDirectionJacobian( const LineSegment& e, const UnitVec3& axis, const PointVariation& dx, - MatrixView J, + Matrix& J, bool invert = false) { Vec3 y = axis - e.d * dot(e.d,axis); y /= e.l * (invert ? 1. : -1); - J += ~dx * y; + addBlock(~dx * y, J); } double calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) @@ -393,118 +397,142 @@ GeodesicJacobian addPathErrorJacobian( const LineSegment& e, const UnitVec3& axis, const Variation& dK, - MatrixView J, + Matrix& J, bool invertV = false) { addDirectionJacobian(e, axis, dK[1], J, invertV); - J += cross(axis,e.d).transpose() * dK[0]; + addBlock(~dK[0] * cross(axis,e.d), J); +} + +} + +//============================================================================== +// PATH IMPL +//============================================================================== + +void WrappingPath::Impl::realizeTopology(State &state) +{ + PosInfo posInfo {}; + m_PosInfoIx = m_Subsystem.allocateCacheEntry(state, Stage::Position, new Value(posInfo)); +} + +void WrappingPath::Impl::realizePosition(const State &state) const +{ + if (m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) {return;} + calcPosInfo(updPosInfo(state)); + m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); +} + +const WrappingPath::Impl::PosInfo& WrappingPath::Impl::getPosInfo(const State &state) const +{ + realizePosition(state); + return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); +} + +WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &state) const +{ + return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); } -void calcPathError( +void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& state, std::function GetInitPointGuess) const +{ + Vec3 prev = m_OriginBody.getBodyTransform(state).shiftFrameStationToBase(m_OriginPoint); + size_t i = 0; + for (const WrapObstacle& obstacle: m_Obstacles) { + Vec3 xGuess = GetInitPointGuess(++i); + obstacle.getImpl().calcGeodesic(state, xGuess, xGuess - prev, 0.); + + prev = obstacle.getImpl().getPosInfo(state).KQ.p(); + } +} + +template +void WrappingPath::Impl::calcPathErrorVector( const State& state, const std::vector& obs, const std::vector& lines, + std::array axes, Vector& pathError) { - size_t i = 0; - ptrdiff_t row = -1; + size_t lineIx = 0; + ptrdiff_t row = -1; + for (const WrapObstacle& o : obs) { - const Geodesic& g = o.getGeodesic(state); - pathError(++row) = calcPathError(lines.at(i), g.KP.R(), NormalAxis); - pathError(++row) = calcPathError(lines.at(i), g.KP.R(), BinormalAxis); - ++i; - pathError(++row) = calcPathError(lines.at(i), g.KQ.R(), NormalAxis); - pathError(++row) = calcPathError(lines.at(i), g.KQ.R(), BinormalAxis); - } -} + if (!o.isActive(state)) { + continue; + } -void writePathErrorJaobianBlock( - const Transform& K, - const Variation& dK, - const LineSegment& l, - MatrixView J) -{ - const UnitVec3 nP = K.R().getAxisUnitVec(NormalAxis); - const UnitVec3 bP = K.R().getAxisUnitVec(BinormalAxis); - + const GeodesicInfo& g = o.getImpl().getPosInfo(state); + 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); + } + } } -void calcPathErrorJacobian( - const State& s, +template +void WrappingPath::Impl::calcPathErrorJacobian( + const State& state, const std::vector& obs, const std::vector& lines, + std::array axes, Matrix& J) { - constexpr size_t Q = GeodesicDOF; - constexpr size_t C = N_PATH_CONSTRAINTS; - const size_t n = countActive(s, obs); + constexpr size_t Nq = GeodesicDOF; + const size_t n = countActive(state, obs); - J.resize(n * C, n * Q); + 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; - Vec4 block { NaN, NaN, NaN, NaN,}; - - ActiveLambda f = [&](size_t prevIx, size_t nextIx, size_t i, bool isActive) { - if (!isActive) { - return; + for (size_t i = 0; i < n; ++i) { + if (!obs.at(i).isActive(state)) { + continue; } + const GeodesicInfo& g = obs.at(i).getImpl().getPosInfo(state); - const Geodesic& g = obs.at(i).getGeodesic(s); + const LineSegment& l_P = lines.at(i); + const LineSegment& l_Q = lines.at(i + 1); - { - const LineSegment& l_P = lines.at(i); - - const UnitVec3 nP = g.KP.R().getAxisUnitVec(NormalAxis); - const UnitVec3 bP = g.KP.R().getAxisUnitVec(BinormalAxis); + const WrapObstacle* prev = FindPrevActiveObstacle(state, obs, i); + const WrapObstacle* next = FindNextActiveObstacle(state, obs, i); - addPathErrorJacobian(l_P, nP, g.dKP, J.block(row, col, 1, Q)); - addPathErrorJacobian(l_P, bP, g.dKP, J.block(row + 1, col, 1, Q)); + for (CoordinateAxis axis: axes) { + const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); + const Variation& dK_P = g.dKP; - if (prevIx != n) { - const Geodesic& prev = obs.at(prevIx).getGeodesic(s); + addPathErrorJacobian(l_P, a_P, dK_P, J.block(row, col, 1, Nq)); - addDirectionJacobian(l_P, nP, prev.dKP[1], J.block(row, col-Q, 1, Q), true); - addDirectionJacobian(l_P, bP, prev.dKP[1], J.block(row + 1, col-Q, 1, Q), true); + if (prev) { + const Variation& prev_dK_Q = prev->getImpl().getPosInfo(state).dKQ; + addDirectionJacobian(l_P, a_P, prev_dK_Q[1], J.block(row, col-Nq, 1, Nq), true); } - row += 2; + ++row; } - // TODO looked like this - /* calcPathErrorJacobian(l_Q, a_Q, g.v_Q, g.w_Q, true); */ - // dx_P dR_P - // addDirectionJacobian(l_Q, n_Q, prev.dx_P, J.block()) - { - const LineSegment& l_Q = lines.at(i + 1); - - const UnitVec3 nQ = g.KQ.R().getAxisUnitVec(NormalAxis); - const UnitVec3 bQ = g.KQ.R().getAxisUnitVec(BinormalAxis); - - addPathErrorJacobian(l_Q, nQ, g.dKQ, J.block(row, col, 1, Q), true); - addPathErrorJacobian(l_Q, bQ, g.dKQ, J.block(row + 1, col, 1, Q), true); + for (CoordinateAxis axis: axes) { + const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); + const Variation& dK_Q = g.dKQ; - if (nextIx != n) { - /* pathErrorJacobian.block<1, Q>(row, col + Q) = */ - /* calcDirectionJacobian(l_Q, a_Q, obs.at(next).getGeodesic().v_P); */ + addPathErrorJacobian(l_Q, a_Q, dK_Q, J.block(row, col, 1, Nq), true); - const Geodesic& next = obs.at(nextIx).getGeodesic(s); - - addDirectionJacobian(l_Q, nQ, next.dKP[1], J.block(row, col+Q, 1, Q)); - addDirectionJacobian(l_Q, bQ, next.dKP[1], J.block(row + 1, col+Q, 1, Q)); + if (next) { + const Variation& next_dK_P = next->getImpl().getPosInfo(state).dKP; + addDirectionJacobian(l_Q, a_Q, next_dK_P[1], J.block(row, col+Nq, 1, Nq)); } ++row; } - col += Q; + col += Nq; }; - - MapWithPrevAndNext(s, obs, f); } -void calcPathCorrections(const State& s, const std::vector& obs, const Vector& pathError, const Matrix& pathErrorJacobian, Matrix& pathMatrix, Vector& pathCorrections); - -double calcPathLength( - const State& s, +double WrappingPath::Impl::calcPathLength( + const State& state, const std::vector& obs, const std::vector& lines) { @@ -515,17 +543,17 @@ double calcPathLength( } for (const WrapObstacle& obstacle : obs) { - if (!obstacle.isActive(s)) + if (!obstacle.isActive(state)) { continue; } - lTot += obstacle.getGeodesic(s).length; + lTot += obstacle.getImpl().getPosInfo(state).length; } return lTot; } -void calcLineSegments( - const State& s, +void WrappingPath::Impl::calcLineSegments( + const State& state, Vec3 p_O, Vec3 p_I, const std::vector& obs, @@ -536,13 +564,13 @@ void calcLineSegments( lines.clear(); Vec3 lineStart = std::move(p_O); - for (size_t i = 0; i < n; ++i) { - if (!obs.at(i).isActive(s)) { + for (const WrapObstacle& o : obs) { + if (!o.isActive(state)) { continue; } - const Geodesic& g = obs.at(i).getGeodesic(s); - const Vec3 lineEnd = g.KP.p(); + const GeodesicInfo& g = o.getImpl().getPosInfo(state); + const Vec3 lineEnd = g.KP.p(); lines.emplace_back(lineStart, lineEnd); lineStart = g.KQ.p(); @@ -550,6 +578,96 @@ void calcLineSegments( lines.emplace_back(lineStart, p_I); } +Vec3 WrappingPath::Impl::FindPrevPoint( + const State& state, + const Vec3& originPoint, + const std::vector& obs, + size_t idx) +{ + const WrapObstacle* prev = FindPrevActiveObstacle(state, obs, idx); + return prev ? prev->getImpl().getPosInfo(state).KQ.p() : originPoint; +} + +Vec3 WrappingPath::Impl::FindNextPoint( + const State& state, + const Vec3& terminationPoint, + const std::vector& obs, + size_t idx) +{ + const WrapObstacle* next = FindNextActiveObstacle(state, obs, idx); + return next ? next->getImpl().getPosInfo(state).KP.p() : terminationPoint; +} + +size_t WrappingPath::Impl::CalcUpdatedObstaclePosInfo( + const State& s, + const Vec3& x_O, + const Vec3& x_I, + const std::vector& obs, size_t maxIter, Real eps) +{ + size_t countActive = 0; + for (size_t i = 0; i < obs.size(); ++i) { + if (!obs.at(i).isActive(s)) { + continue; + } + + Vec3 prev = FindPrevPoint(s, x_O, obs, i); + Vec3 next = FindPrevPoint(s, x_I, obs, i); + obs.at(i).getImpl().calcPosInfo(s, prev, next, maxIter, eps); + } +} + +void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const +{ + // Path oigin and termination points. + const Vec3 x_O = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + const Vec3 x_I = m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase(m_TerminationPoint); + + const std::array axes {NormalAxis, BinormalAxis}; + + for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; ++posInfo.loopIter) { + // Things have changed: recompute the position level cache of the obstacles. + CalcUpdatedObstaclePosInfo(s, x_O, x_I, m_Obstacles, m_ObsMaxIter, m_ObsErrorBound); + + // Compute the straight-line segments. + calcLineSegments(s, x_O, x_I, m_Obstacles, posInfo.lines); + + // Evaluate path error, and stop when converged. + calcPathErrorVector<2>(s, m_Obstacles, posInfo.lines, axes, posInfo.pathError); + const Real maxPathError = posInfo.pathError.normInf(); + if (maxPathError < m_PathErrorBound) { + return; + } + + // Evaluate the path error jacobian. + calcPathErrorJacobian<2>(s, m_Obstacles, posInfo.lines, axes, posInfo.pathErrorJacobian); + + // Compute path corrections. + calcPathCorrections(s, m_Obstacles, posInfo.pathError, posInfo.pathErrorJacobian, posInfo.pathMatrix, posInfo.pathCorrections); + + // Apply path corrections. + throw std::runtime_error("NOTYETIMPLEMENTED"); + const Correction* corrIt = nullptr; // TODO + for (const WrapObstacle& obstacle : m_Obstacles) { + if (!obstacle.isActive(s)) { + continue; + } + obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); + ++corrIt; + } + } + + throw std::runtime_error("Failed to converge"); +} + +//============================================================================== +// WRAP OBSTACLE IMPL +//============================================================================== + +namespace +{ + +void calcPathCorrections(const State& s, const std::vector& obs, const Vector& pathError, const Matrix& pathErrorJacobian, Matrix& pathMatrix, Vector& pathCorrections); + } //============================================================================== @@ -600,105 +718,3 @@ getPath(WrappingPathIndex cableIx) const WrappingPath& WrappingPathSubsystem:: updPath(WrappingPathIndex cableIx) { return updImpl().updCablePath(cableIx); } - - -//============================================================================== -// WRAPPING PATH -//============================================================================== - -void WrappingPath::Impl::realizeTopology(State &state) -{ - PosInfo posInfo {}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry(state, Stage::Position, new Value(posInfo)); -} - -void WrappingPath::Impl::realizePosition(const State &state) const -{ - if (m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) {return;} - calcPosInfo(updPosInfo(state)); - m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); -} - -const WrappingPath::Impl::PosInfo& WrappingPath::Impl::getPosInfo(const State &state) const -{ - realizePosition(state); - return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); -} - -WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &state) const -{ - return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); -} - -bool WrapObstacle::isPointBelowSurface(const State& state, Vec3 point) const -{ - const Transform& X_BS = getSurfaceToBaseTransform(state); - return m_Surface.getGeometry().calcSurfaceValue(X_BS.shiftBaseStationToFrame(point)) < 0.; -} - -void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo, bool preventLiftOff = false) const -{ - // Path oigin and termination points. - Vec3 x_O = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); - Vec3 x_I = m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase(m_TerminationPoint); - - // Helper for detecting if start/end points lie inside the surfacce. - std::function DetectInsideSurfaceError = - [&](Vec3 x_O, const WrapObstacle& obstacle, Vec3 x_I) { // TODO also supply the status. - // Check that previous point does not lie inside the surface. - if (obstacle.isPointBelowSurface(s, x_O)) { - throw std::runtime_error("Start point lies inside the surface"); - } - - // Check that next point does not lie inside the surface. - if (obstacle.isPointBelowSurface(s, x_I)) { - throw std::runtime_error("End point lies inside the surface"); - } - - if (preventLiftOff) { - return; - } - - // TODO detect touhdown - // TODO detect liftoff - throw std::runtime_error("NOTYETIMPLEMENTED"); - }; - - for (posInfo.loopIter = 0; posInfo.loopIter < m_MaxIter; ++posInfo.loopIter) { - - // Detect touchdown & liftoff. - // doForEachObjectWithPrevAndNextPoint(UpdateLiftoffAndTouchdown) - callCurrentWithPrevAndNext(s, DetectInsideSurfaceError); - - // Compute the line segments. - calcLineSegments(s, x_O, x_I, m_Obstacles, posInfo.lines); - - calcPathError(s, m_Obstacles, posInfo.lines, posInfo.pathError); - - const Real maxPathError = posInfo.pathError.normInf(); - - // Evaluate path error, and stop when converged. - if (maxPathError < m_PathErrorBound) { - return; - } - - // Evaluate the path error jacobian. - calcPathErrorJacobian(s, m_Obstacles, posInfo.lines, posInfo.pathErrorJacobian); - - // Compute path corrections. - calcPathCorrections(s, m_Obstacles, posInfo.pathError, posInfo.pathErrorJacobian, posInfo.pathMatrix, posInfo.pathCorrections); - - // Apply path corrections. - throw std::runtime_error("NOTYETIMPLEMENTED"); - const Correction* corrIt = nullptr; // TODO - for (const WrapObstacle& obstacle : m_Obstacles) { - if (!obstacle.isActive(s)) { - continue; - } - obstacle.applyGeodesicCorrection(s, *corrIt); - ++corrIt; - } - } - - throw std::runtime_error("Failed to converge"); -} diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index c6d5d3f47..919b21e41 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -283,20 +283,61 @@ class WrappingPath::Impl { void calcInitZeroLengthGeodesic(State& state, std::function GetInitPointGuess) const; + + private: + PosInfo& updPosInfo(const State& s) const; + void calcPosInfo(const State& s, PosInfo& posInfo) const; + + static Vec3 FindPrevPoint( + const State& state, + const Vec3& originPoint, + const std::vector& obs, + size_t idx); + + static Vec3 FindNextPoint( + const State& state, + const Vec3& terminationPoint, + const std::vector& obs, + size_t idx); + WrapObstacle* findPrevActiveObstacle(const State& s, size_t obsIdx); WrapObstacle* findNextActiveObstacle(const State& s, size_t obsIdx); - void callCurrentWithPrevAndNext( + template + static void calcPathErrorVector( + const State& state, + const std::vector& obs, + const std::vector& lines, + std::array axes, + Vector& pathError); + + template + static void calcPathErrorJacobian( + const State& state, + const std::vector& obs, + const std::vector& lines, + std::array axes, + Matrix& J); + + static void calcLineSegments( const State& s, - std::function f); + Vec3 p_O, + Vec3 p_I, + const std::vector& obs, + std::vector& lines); - void callCurrentWithPrevAndNext( + static size_t CalcUpdatedObstaclePosInfo( const State& s, - std::function f) const; + const Vec3& x_O, + const Vec3& x_I, + const std::vector& obs, + size_t maxIter, + Real eps); - private: - PosInfo& updPosInfo(const State& s) const; - void calcPosInfo(const State& s, PosInfo& posInfo) const; +static double calcPathLength( + const State& state, + const std::vector& obs, + const std::vector& lines); WrappingPathSubsystem m_Subsystem; @@ -309,7 +350,9 @@ class WrappingPath::Impl { std::vector m_Obstacles {}; Real m_PathErrorBound = 0.1; - size_t m_MaxIter = 10; + Real m_ObsErrorBound = 0.1; + size_t m_PathMaxIter = 10; + size_t m_ObsMaxIter = 10; // TOPOLOGY CACHE (set during realizeTopology()) CacheEntryIndex m_PosInfoIx; From c52e5a0fbe23c6cbf50482f675364776c3ec0bcf Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 23 Apr 2024 11:33:23 +0200 Subject: [PATCH 020/127] fix locking the shared solver data --- Simbody/include/simbody/internal/Wrapping.cpp | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 7445e09b1..855727da4 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -36,9 +36,10 @@ namespace { Vector b; }; - struct SolverDataCache + class SolverDataCache { - SolverDataCache(std::mutex&& cacheMutex) : + public: + SolverDataCache(std::mutex& cacheMutex) : m_guard(cacheMutex) {} void setDataPtr(SolverData* ptr) {m_data = ptr;} @@ -50,24 +51,26 @@ namespace { SolverData* m_data = nullptr; }; - SolverDataCache&& updSolverDataCache(size_t nActive) + SolverDataCache&& findDataCache(size_t nActive) { static constexpr int Q = 4; static constexpr int C = 4; static std::vector s_GlobalCache {}; static std::mutex s_GlobalLock {}; - SolverDataCache data(std::move(s_GlobalLock)); + { + std::lock_guard lock(s_GlobalLock); - for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { - int n = i + 1; - s_GlobalCache.emplace_back( - Matrix{C * n, Q * n, 0.}, - Vector{Q * n, 0.}, - Vector{C * n, 0.}); + for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { + int n = i + 1; + s_GlobalCache.emplace_back( + Matrix{C * n, Q * n, 0.}, + Vector{Q * n, 0.}, + Vector{C * n, 0.}); + } } - return std::move(data); + return SolverDataCache(s_GlobalLock); } } // namespace From 84fccc4332e50d54b5773e6d0090c518fb751b48 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 23 Apr 2024 11:34:13 +0200 Subject: [PATCH 021/127] refactor position level staging --- Simbody/include/simbody/internal/Wrapping.cpp | 78 +++++++++---------- Simbody/include/simbody/internal/Wrapping.h | 4 +- .../include/simbody/internal/WrappingImpl.h | 25 +++++- 3 files changed, 63 insertions(+), 44 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 855727da4..8b39520ae 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -258,6 +258,7 @@ void WrapObstacle::Impl::realizePosition(const State &state) const const WrapObstacle::Impl::PosInfo& WrapObstacle::Impl::getPosInfo(const State &state) const { + realizePosition(state); return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); } @@ -274,10 +275,30 @@ void WrapObstacle::Impl::calcPosInfo( const State& state, PosInfo& posInfo) const { - // Transform the local geodesic to ground frame. + if(isDisabled(state)) + { + return; + } + + // Get tramsform from local surface frame to ground. Transform X_GS = m_Mobod.getBodyTransform(state).compose(m_Offset); + + // Get the path points before and after this segment. + Vec3 prev_G = m_Path.getImpl().findPrevPoint(state, m_PathSegmentIx); + Vec3 next_G = m_Path.getImpl().findNextPoint(state, m_PathSegmentIx); + + // Transform the prev and next path points to the surface frame. + Vec3 prev_S = X_GS.shiftBaseStationToFrame(prev_G); + Vec3 next_S = X_GS.shiftBaseStationToFrame(next_G); + + // Detect liftoff, touchdown and potential invalid configurations. + // TODO this doesnt follow the regular invalidation scheme... + m_Surface.getImpl().calcUpdatedStatus(state, prev_S, next_S); + + // Grab the last geodesic that was computed. const Surface::LocalGeodesic& geodesic_S = m_Surface.getImpl().getGeodesic(state); + // Store the the local geodesic in ground frame. posInfo.KP = X_GS.compose(geodesic_S.KP); posInfo.KQ = X_GS.compose(geodesic_S.KQ); @@ -287,25 +308,18 @@ void WrapObstacle::Impl::calcPosInfo( posInfo.dKQ[0] = X_GS.R() * geodesic_S.dKQ[0]; posInfo.dKQ[1] = X_GS.R() * geodesic_S.dKQ[1]; - posInfo.length = geodesic_S.length; - - throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); -} + // TODO use SpatialVec for variation. + /* for (size_t i = 0; i < GeodesicDOF; ++i) { */ + /* posInfo.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) */ + /* posInfo.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) */ -void WrapObstacle::Impl::calcPosInfo( - const State& state, - Vec3 prev, - Vec3 next, - size_t maxIter, Real eps) const -{ - m_Surface.getImpl().calcWrappingStatus(state, prev, next, maxIter, eps); + /* posInfo.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) */ + /* posInfo.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) */ + /* } */ - if(isDisabled(state)) - { - return; - } + posInfo.length = geodesic_S.length; - calcPosInfo(state, updPosInfo(state)); + throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); } void WrapObstacle::Impl::calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const @@ -422,7 +436,7 @@ void WrappingPath::Impl::realizeTopology(State &state) void WrappingPath::Impl::realizePosition(const State &state) const { if (m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) {return;} - calcPosInfo(updPosInfo(state)); + calcPosInfo(state, updPosInfo(state)); m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); } @@ -601,36 +615,15 @@ Vec3 WrappingPath::Impl::FindNextPoint( return next ? next->getImpl().getPosInfo(state).KP.p() : terminationPoint; } -size_t WrappingPath::Impl::CalcUpdatedObstaclePosInfo( - const State& s, - const Vec3& x_O, - const Vec3& x_I, - const std::vector& obs, size_t maxIter, Real eps) -{ - size_t countActive = 0; - for (size_t i = 0; i < obs.size(); ++i) { - if (!obs.at(i).isActive(s)) { - continue; - } - - Vec3 prev = FindPrevPoint(s, x_O, obs, i); - Vec3 next = FindPrevPoint(s, x_I, obs, i); - obs.at(i).getImpl().calcPosInfo(s, prev, next, maxIter, eps); - } -} - void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { - // Path oigin and termination points. + // Path origin and termination point. const Vec3 x_O = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); const Vec3 x_I = m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase(m_TerminationPoint); const std::array axes {NormalAxis, BinormalAxis}; for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; ++posInfo.loopIter) { - // Things have changed: recompute the position level cache of the obstacles. - CalcUpdatedObstaclePosInfo(s, x_O, x_I, m_Obstacles, m_ObsMaxIter, m_ObsErrorBound); - // Compute the straight-line segments. calcLineSegments(s, x_O, x_I, m_Obstacles, posInfo.lines); @@ -657,6 +650,11 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); ++corrIt; } + + // Path has changed: invalidate each segment's cache. + for (const WrapObstacle& obstacle : m_Obstacles) { + obstacle.getImpl().invalidatePositionLevelCache(s); + } } throw std::runtime_error("Failed to converge"); diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 75fc07c03..ca98cbeea 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -142,11 +142,13 @@ class SimTK_SIMBODY_EXPORT WrappingPath private: explicit WrappingPath(std::unique_ptr impl); - friend WrappingPathSubsystem; const Impl& getImpl() const { return *impl; } Impl& updImpl() { return *impl; } std::shared_ptr impl = nullptr; + + friend WrapObstacle::Impl; + friend WrappingPathSubsystem; }; //============================================================================== diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 919b21e41..e1e07ff6c 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -83,12 +83,13 @@ class Surface::Impl { // TODO rename bool isActive(const State& state) const; + bool isDisabled(const State& state) const; // 4. calcPathPoints size_t calcPathPoints(const State& state, std::vector& points) const; // Detect liftoff or touchdown, and compute the point on line near the surface. - const WrappingStatus& calcWrappingStatus(const State& state, Vec3 prev, Vec3 next, size_t maxIter, Real eps) const; + const WrappingStatus& calcUpdatedStatus(const State& state, Vec3 prev, Vec3 next) const; // TODO should be in OpenSim? void setInitialPointGuess(Vec3 pointGuess); @@ -134,6 +135,9 @@ class Surface::Impl { Vec3 m_InitPointGuess; + Real m_TouchdownAccuracy = 1e-3; + size_t m_TouchdownIter = 10; + DiscreteVariableIndex m_GeodesicInfoIx; }; @@ -200,6 +204,11 @@ class WrapObstacle::Impl /* void realizeAcceleration(const State& state) const; */ /* void invalidateTopology(); */ + void invalidatePositionLevelCache(const State& state) const + { + m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); + } + const PosInfo& getPosInfo(const State& state) const; bool isActive(const State& state) const; @@ -220,7 +229,9 @@ class WrapObstacle::Impl void calcPosInfo(const State& state, PosInfo& posInfo) const; // TODO Required for accessing the cache variable? - WrappingPathSubsystem m_Subsystem; + WrappingPathSubsystem m_Subsystem; // The subsystem this segment belongs to. + WrappingPath m_Path; // The path this segment belongs to. + PathSegmentIndex m_PathSegmentIx; // The index in its path. MobilizedBody m_Mobod; Transform m_Offset; @@ -288,6 +299,14 @@ class WrappingPath::Impl { PosInfo& updPosInfo(const State& s) const; void calcPosInfo(const State& s, PosInfo& posInfo) const; + Vec3 findPrevPoint( + const State& state, + PathSegmentIndex idx) const; + + Vec3 findNextPoint( + const State& state, + PathSegmentIndex idx) const; + static Vec3 FindPrevPoint( const State& state, const Vec3& originPoint, @@ -359,7 +378,7 @@ static double calcPathLength( CacheEntryIndex m_VelInfoIx; CacheEntryIndex m_VizInfoIx; - friend class WrappingPath; + friend WrapObstacle::Impl; }; //============================================================================== From 8f40a3c93d546272e9686f4c5f8ae593419307c9 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 23 Apr 2024 13:08:11 +0200 Subject: [PATCH 022/127] add computing the path corrections --- Simbody/include/simbody/internal/Wrapping.cpp | 116 +++++++++++------- .../include/simbody/internal/WrappingImpl.h | 3 + 2 files changed, 78 insertions(+), 41 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 8b39520ae..4e063cf5f 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -10,6 +10,8 @@ using namespace SimTK; using GeodesicInfo = WrapObstacle::Impl::PosInfo; +using LineSegment = WrappingPath::LineSegment; +using Correction = ContactGeometry::GeodesicCorrection; //============================================================================== // CONSTANTS @@ -26,36 +28,58 @@ namespace { //============================================================================== namespace { - struct SolverData; + class SolverDataCache; + SolverDataCache& findDataCache(size_t nActive); struct SolverData { - // A * x = b - Matrix A; - Vector x; - Vector b; + std::vector lineSegments; + + Matrix pathErrorJacobian; + Vector pathCorrection; + Vector pathError; + Matrix mat; + // TODO Cholesky decomposition... + FactorLU matInv; + Vector vec; }; class SolverDataCache { + private: + SolverDataCache(size_t n) { + static constexpr int Q = 4; + static constexpr int C = 4; + + m_Data.lineSegments.resize(n+1); + m_Data.pathErrorJacobian = Matrix(C * n, Q * n, 0.); + m_Data.pathCorrection = Vector(Q * n, 0.); + m_Data.pathError = Vector(C * n, 0.); + m_Data.mat = Matrix(Q*n, Q*n, NaN); + m_Data.vec = Vector(Q*n, NaN); + } + public: - SolverDataCache(std::mutex& cacheMutex) : - m_guard(cacheMutex) {} + void lock() { + m_Mutex.lock(); + } - void setDataPtr(SolverData* ptr) {m_data = ptr;} + void unlock() { + m_Mutex.unlock(); + } - SolverData& updData() {return *m_data;} + SolverData& updData() {return m_Data;} private: - std::lock_guard m_guard; - SolverData* m_data = nullptr; + SolverData m_Data; + std::mutex m_Mutex {}; + + friend SolverDataCache& findDataCache(size_t nActive); }; - SolverDataCache&& findDataCache(size_t nActive) + SolverDataCache& findDataCache(size_t nActive) { - static constexpr int Q = 4; - static constexpr int C = 4; - static std::vector s_GlobalCache {}; + static std::vector s_GlobalCache {}; static std::mutex s_GlobalLock {}; { @@ -63,14 +87,30 @@ namespace { for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { int n = i + 1; - s_GlobalCache.emplace_back( - Matrix{C * n, Q * n, 0.}, - Vector{Q * n, 0.}, - Vector{C * n, 0.}); + s_GlobalCache.emplace_back(nActive); } } - return SolverDataCache(s_GlobalLock); + return s_GlobalCache.at(nActive-1); + } + + const Correction* calcPathCorrections(SolverData& data) { + Real w = data.pathError.normInf(); + + data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; + for (int i = 0; i < data.mat.nrow(); ++i) { + data.mat[i][i] += w; + } + data.matInv = data.mat; + data.vec = data.pathErrorJacobian.transpose() * data.pathError; + data.matInv.solve(data.vec, data.pathCorrection); + + static_assert( + sizeof(Correction) == sizeof(double) * GeodesicDOF, + "Invalid size of corrections vector"); + SimTK_ASSERT(data.pathCorrection.size() * sizeof(double) == n * sizeof(Correction), + "Invalid size of path corrections vector"); + return reinterpret_cast(&data.pathCorrection[0]); } } // namespace @@ -81,7 +121,6 @@ namespace { namespace { using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; - using Correction = ContactGeometry::GeodesicCorrection; using GeodesicInitialConditions = Surface::Impl::GeodesicInitialConditions; GeodesicInitialConditions calcCorrectedGeodesicInitConditions(const FrenetFrame& KP, const Variation& dKP, Real l, const Correction& c) @@ -155,10 +194,10 @@ void Surface::Impl::applyGeodesicCorrection(const State& state, const Correction calcLocalGeodesicInfo(g0.x, g0.t, g0.l, g.sHint, updLocalGeodesicInfo(state)); } -const Surface::WrappingStatus& Surface::Impl::calcWrappingStatus( +const Surface::WrappingStatus& Surface::Impl::calcUpdatedStatus( const State& state, Vec3 prevPoint, - Vec3 nextPoint, size_t maxIter, Real eps) const + Vec3 nextPoint) const { LocalGeodesicInfo& g = updLocalGeodesicInfo(state); @@ -175,7 +214,7 @@ const Surface::WrappingStatus& Surface::Impl::calcWrappingStatus( } // Update the tracking point. - ContactGeometry::PointOnLineResult result = m_Geometry.calcNearestPointOnLine(prevPoint, nextPoint, g.pointOnLine, maxIter, eps); + ContactGeometry::PointOnLineResult result = m_Geometry.calcNearestPointOnLine(prevPoint, nextPoint, g.pointOnLine, m_TouchdownIter, m_TouchdownAccuracy); g.pointOnLine = result.p; // TODO rename to point. // Attempt touchdown. @@ -335,7 +374,6 @@ void WrapObstacle::Impl::calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) cons namespace { using GeodesicJacobian = Vec4; - using LineSegment = WrappingPath::LineSegment; using PointVariation = ContactGeometry::GeodesicPointVariation; using FrameVariation = ContactGeometry::GeodesicFrameVariation; using Variation = ContactGeometry::GeodesicVariation; @@ -624,11 +662,17 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const const std::array axes {NormalAxis, BinormalAxis}; for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; ++posInfo.loopIter) { + const size_t nActive = countActive(s); + + // Grab the shared data cache for computing the matrices, and lock it. + SolverDataCache& data = findDataCache(nActive); + data.lock(); + // Compute the straight-line segments. - calcLineSegments(s, x_O, x_I, m_Obstacles, posInfo.lines); + calcLineSegments(s, x_O, x_I, m_Obstacles, data.updData().lineSegments); // Evaluate path error, and stop when converged. - calcPathErrorVector<2>(s, m_Obstacles, posInfo.lines, axes, posInfo.pathError); + calcPathErrorVector<2>(s, m_Obstacles, posInfo.lines, axes, data.updData().pathError); const Real maxPathError = posInfo.pathError.normInf(); if (maxPathError < m_PathErrorBound) { return; @@ -638,11 +682,9 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const calcPathErrorJacobian<2>(s, m_Obstacles, posInfo.lines, axes, posInfo.pathErrorJacobian); // Compute path corrections. - calcPathCorrections(s, m_Obstacles, posInfo.pathError, posInfo.pathErrorJacobian, posInfo.pathMatrix, posInfo.pathCorrections); + const Correction* corrIt = calcPathCorrections(data.updData()); // Apply path corrections. - throw std::runtime_error("NOTYETIMPLEMENTED"); - const Correction* corrIt = nullptr; // TODO for (const WrapObstacle& obstacle : m_Obstacles) { if (!obstacle.isActive(s)) { continue; @@ -651,6 +693,9 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const ++corrIt; } + // Release the lock on the shared data. + data.unlock(); + // Path has changed: invalidate each segment's cache. for (const WrapObstacle& obstacle : m_Obstacles) { obstacle.getImpl().invalidatePositionLevelCache(s); @@ -660,17 +705,6 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const throw std::runtime_error("Failed to converge"); } -//============================================================================== -// WRAP OBSTACLE IMPL -//============================================================================== - -namespace -{ - -void calcPathCorrections(const State& s, const std::vector& obs, const Vector& pathError, const Matrix& pathErrorJacobian, Matrix& pathMatrix, Vector& pathCorrections); - -} - //============================================================================== // SUBSYSTEM //============================================================================== diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index e1e07ff6c..d3e202656 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -16,6 +16,9 @@ namespace SimTK { +SimTK_DEFINE_UNIQUE_INDEX_TYPE(PathSegmentIndex); +SimTK_DEFINE_UNIQUE_INDEX_TYPE(PathIndex); + //============================================================================== // SURFACE IMPL //============================================================================== From abe2c3ebc921408a774ef9cfdf5f90a6fbeb11c1 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 23 Apr 2024 16:41:24 +0200 Subject: [PATCH 023/127] wip: Move Surface into WrapObstacle::Impl --- .../simmath/internal/ContactGeometry.h | 9 +- Simbody/include/simbody/internal/Wrapping.cpp | 376 ++++++++++-------- Simbody/include/simbody/internal/Wrapping.h | 69 +--- .../include/simbody/internal/WrappingImpl.h | 269 +++++++------ 4 files changed, 352 insertions(+), 371 deletions(-) diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h index 06380545f..237cb5dff 100644 --- a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h +++ b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h @@ -155,14 +155,7 @@ void calcGeodesicEndFrameVariationAnalytically( Vec3 x, void calcGeodesicPointsAnalytically(Vec3 x, UnitVec3 t, Real l, std::vector& points); bool analyticFormAvailable() const; -struct PointOnLineResult -{ - Vec3 p {NaN, NaN, NaN}; - bool isInsideSurface = false; - size_t iter = 0; -}; - -PointOnLineResult calcNearestPointOnLine(Vec3 a, Vec3 b, Vec3 hint, size_t maxIter, double eps) const; +bool calcNearestPointOnLine(Vec3 a, Vec3 b, Vec3& point, size_t maxIter, double eps) const; // TODO class Cone; diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 4e063cf5f..2456b5f00 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -9,9 +9,12 @@ using namespace SimTK; -using GeodesicInfo = WrapObstacle::Impl::PosInfo; -using LineSegment = WrappingPath::LineSegment; using Correction = ContactGeometry::GeodesicCorrection; +using FrenetFrame = ContactGeometry::FrenetFrame; +using GeodesicInitialConditions = WrapObstacle::Impl::Surface::GeodesicInitialConditions; +using LocalGeodesicInfo = WrapObstacle::Impl::Surface::LocalGeodesicInfo; +using GeodesicInfo = WrapObstacle::Impl::GeodesicInfo; +using Variation = ContactGeometry::GeodesicVariation; //============================================================================== // CONSTANTS @@ -115,216 +118,247 @@ namespace { } // namespace //============================================================================== -// SOME MATH +// GEODESIC INITIAL CONDITIONS //============================================================================== -namespace { - using FrenetFrame = ContactGeometry::FrenetFrame; - using Variation = ContactGeometry::GeodesicVariation; - using GeodesicInitialConditions = Surface::Impl::GeodesicInitialConditions; +GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected(const FrenetFrame& KP, const Variation& dKP, Real l, const Correction& c) +{ + GeodesicInitialConditions g0; - GeodesicInitialConditions calcCorrectedGeodesicInitConditions(const FrenetFrame& KP, const Variation& dKP, Real l, const Correction& c) - { - Surface::Impl::GeodesicInitialConditions g0; + Vec3 v = dKP[1] * c; + g0.x = KP.p() + v; + + Vec3 w = dKP[0] * c; + const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); + g0.t = t + cross(w,t); - Vec3 v = dKP[1] * c; - g0.x = KP.p() + v; + Real dl = c[3]; + g0.l = l + dl; - Vec3 w = dKP[0] * c; - const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); - g0.t = t + cross(w,t); + return g0; +} - Real dl = c[3]; - g0.l = l + dl; +GeodesicInitialConditions GeodesicInitialConditions::CreateInSurfaceFrame(const Transform& X_GS, Vec3 x_G, Vec3 t_G, Real l) +{ + GeodesicInitialConditions g0; - return g0; - } + g0.x = X_GS.shiftBaseStationToFrame(x_G); + g0.t = X_GS.xformBaseVecToFrame(t_G); + g0.l = l; + + return g0; +} + +GeodesicInitialConditions GeodesicInitialConditions::CreateZeroLengthGuess(const Transform& X_GS, Vec3 prev_QS, Vec3 xGuess_S) +{ + GeodesicInitialConditions g0; + + g0.x = xGuess_S; + g0.t = g0.x - X_GS.shiftBaseStationToFrame(prev_QS); + g0.l = 0.; + + return g0; +} + +GeodesicInitialConditions GeodesicInitialConditions::CreateAtTouchdown(Vec3 prev_QS, Vec3 next_PS, Vec3 trackingPointOnLine) +{ + GeodesicInitialConditions g0; + + g0.x = trackingPointOnLine; + g0.t = next_PS - prev_QS; + g0.l = 0.; + + return g0; } //============================================================================== // SURFACE IMPL //============================================================================== +using Surface = WrapObstacle::Impl::Surface; //------------------------------------------------------------------------------ // REALIZE CACHE //------------------------------------------------------------------------------ -void Surface::Impl::realizeTopology(State &state) +void Surface::realizeTopology(State &s) { // Allocate an auto-update discrete variable for the last computed geodesic. - LocalGeodesicInfo geodesic {}; - m_GeodesicInfoIx = m_Subsystem.allocateAutoUpdateDiscreteVariable(state, Stage::Velocity, new Value(geodesic), Stage::Position); + CacheEntry cache {}; + m_CacheIx = m_Subsystem.allocateAutoUpdateDiscreteVariable(s, Stage::Velocity, new Value(cache), Stage::Position); } -void Surface::Impl::realizeInstance(const State &state) const +void Surface::realizePosition(const State &s) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); -} - -void Surface::Impl::realizePosition(const State &state) const -{ - // Set the current local geodesic equal to the previous. - if (!m_Subsystem.isDiscreteVarUpdateValueRealized(state, m_GeodesicInfoIx)) { - updLocalGeodesicInfo(state) = getPrevLocalGeodesicInfo(state); - m_Subsystem.markDiscreteVarUpdateValueRealized(state, m_GeodesicInfoIx); + if (!m_Subsystem.isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { + updCacheEntry(s) = getPrevCacheEntry(s); + m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); } } -void Surface::Impl::realizeVelocity(const State &state) const +//------------------------------------------------------------------------------ +// PUBLIC METHODS +//------------------------------------------------------------------------------ +const LocalGeodesicInfo& Surface::calcInitialGeodesic(State& s, const GeodesicInitialConditions& g0) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + // TODO is this correct? + CacheEntry& cache = updPrevCacheEntry(s); + calcGeodesic(g0, cache); + updCacheEntry(s) = cache; + m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); } -void Surface::Impl::realizeAcceleration(const State &state) const +const LocalGeodesicInfo& Surface::calcLocalGeodesic(const State& s, Vec3 prev_QS, Vec3 next_PS) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + realizePosition(s); + CacheEntry& cache = updCacheEntry(s); + calcStatus(prev_QS, next_PS, cache); + return cache; } -//------------------------------------------------------------------------------ -// PUBLIC METHODS -//------------------------------------------------------------------------------ -void Surface::Impl::applyGeodesicCorrection(const State& state, const Correction& c) const +void Surface::applyGeodesicCorrection(const State& s, const Correction& c) const { + realizePosition(s); + // Get the previous geodesic. - const LocalGeodesicInfo& g = getLocalGeodesicInfo(state); + const CacheEntry& g = getCacheEntry(s); // Get corrected initial conditions. - const GeodesicInitialConditions g0 = calcCorrectedGeodesicInitConditions(g.KP, g.dKP, g.length, c); + const GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateCorrected(g.KP, g.dKP, g.length, c); // Shoot the new geodesic. - calcLocalGeodesicInfo(g0.x, g0.t, g0.l, g.sHint, updLocalGeodesicInfo(state)); + calcGeodesic(g0, updCacheEntry(s)); +} + +void Surface::calcGeodesic(const GeodesicInitialConditions& g0, CacheEntry& cache) const +{ + // Compute geodesic start boundary frame and variation. + m_Geometry.calcNearestFrenetFrameFast(g0.x, g0.t, cache.KP); + m_Geometry.calcGeodesicStartFrameVariation(cache.KP, cache.dKP); + + // Compute geodesic end boundary frame amd variation (shoot new geodesic). + m_Geometry.calcGeodesicEndFrameVariationImplicitly( + cache.KP.p(), + cache.KP.R().getAxisUnitVec(ContactGeometry::TangentAxis), + g0.l, + cache.sHint, + cache.KQ, + cache.dKQ, + cache.points); + + // TODO update step size. + // TODO update line tracking? + throw std::runtime_error("NOTYETIMPLEMENTED"); } -const Surface::WrappingStatus& Surface::Impl::calcUpdatedStatus( - const State& state, - Vec3 prevPoint, - Vec3 nextPoint) const +void Surface::calcStatus(const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& cache) const { - LocalGeodesicInfo& g = updLocalGeodesicInfo(state); + LocalGeodesicInfo& g = cache; - if (g.disabled) {return g;} + if (g.status == Status::Disabled) {return;} // Make sure that the previous point does not lie inside the surface. - if (m_Geometry.calcSurfaceValue(prevPoint)) { + if (m_Geometry.calcSurfaceValue(prev_QS)) { // TODO use proper assert. throw std::runtime_error("Unable to wrap over surface: Preceding point lies inside the surface"); } - if (m_Geometry.calcSurfaceValue(nextPoint)) { + if (m_Geometry.calcSurfaceValue(next_PS)) { // TODO use proper assert. throw std::runtime_error("Unable to wrap over surface: Next point lies inside the surface"); } - // Update the tracking point. - ContactGeometry::PointOnLineResult result = m_Geometry.calcNearestPointOnLine(prevPoint, nextPoint, g.pointOnLine, m_TouchdownIter, m_TouchdownAccuracy); - g.pointOnLine = result.p; // TODO rename to point. + Vec3& pTrack = cache.trackingPointOnLine; - // Attempt touchdown. - if (g.liftoff) { - const bool touchdown = result.isInsideSurface; - g.liftoff = !touchdown; + // Detect touchdown. + bool detectedTouchdown = g.status == Status::Liftoff; + detectedTouchdown &= m_Geometry.calcNearestPointOnLine(prev_QS, next_PS, pTrack, m_TouchdownIter, m_TouchdownAccuracy); + if (detectedTouchdown) { + g.status = Status::Ok; + GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateAtTouchdown(prev_QS, next_PS, cache.trackingPointOnLine); + calcGeodesic(g0, cache); } // Detect liftoff. - if (!g.liftoff) { - g.liftoff = g.length == 0.; - g.liftoff &= dot(prevPoint - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0.; - g.liftoff &= dot(nextPoint - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0.; + bool detectedLiftoff = g.status == Status::Ok; + detectedLiftoff &= g.length == 0.; + detectedLiftoff &= dot(prev_QS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) > 0.; + detectedLiftoff &= dot(next_PS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) > 0.; + if (detectedLiftoff) { + g.status = Status::Liftoff; + pTrack = g.KP.p(); } - - return g; } -size_t Surface::Impl::calcPathPoints(const State& state, std::vector& points) const +size_t Surface::calcPathPoints(const State& s, std::vector& points) const { + realizePosition(s); + size_t count = 0; - const LocalGeodesicInfo& g = getLocalGeodesicInfo(state); - for (Vec3 p: g.points) { + const CacheEntry& cache = getCacheEntry(s); + for (Vec3 p: cache.points) { points.push_back(p); + ++count; } + throw std::runtime_error("NOTYETIMPLEMENTED for analytic"); return count; } -void Surface::Impl::setInitialPointGuess(Vec3 pointGuess) { - throw std::runtime_error("NOTYETIMPLEMENTED"); -} - -Vec3 Surface::Impl::getInitialPointGuess() const { - throw std::runtime_error("NOTYETIMPLEMENTED"); -} - //------------------------------------------------------------------------------ // PRIVATE METHODS //------------------------------------------------------------------------------ -void Surface::Impl::calcLocalGeodesicInfo(Vec3 x, Vec3 t, Real l, Real sHint, - LocalGeodesicInfo& geodesic) const -{ - // Compute geodesic start boundary frame and variation. - m_Geometry.calcNearestFrenetFrameFast(x, t, geodesic.KP); - m_Geometry.calcGeodesicStartFrameVariation(geodesic.KP, geodesic.dKP); - - // Compute geodesic end boundary frame amd variation (shoot new geodesic). - m_Geometry.calcGeodesicEndFrameVariationImplicitly( - geodesic.KP.p(), - geodesic.KP.R().getAxisUnitVec(ContactGeometry::TangentAxis), - l, - sHint, - geodesic.KQ, - geodesic.dKQ, - geodesic.points); - - // TODO update step size. - // TODO update line tracking? - throw std::runtime_error("NOTYETIMPLEMENTED"); -} //============================================================================== // OBSTACLE //============================================================================== -void WrapObstacle::Impl::realizeTopology(State &state) +bool WrapObstacle::Impl::isActive(const State& s) const +{ + return getPosInfo(s).status == Status::Ok; +} + +void WrapObstacle::Impl::realizeTopology(State &s) { // Allocate position level cache. PosInfo posInfo {}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry(state, Stage::Position, new Value(posInfo)); + m_PosInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Position, new Value(posInfo)); } -void WrapObstacle::Impl::realizePosition(const State &state) const +void WrapObstacle::Impl::realizePosition(const State &s) const { - if (!m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) { - calcPosInfo(state, updPosInfo(state)); - m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); + if (!m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { + calcPosInfo(s, updPosInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); } } -const WrapObstacle::Impl::PosInfo& WrapObstacle::Impl::getPosInfo(const State &state) const +const WrapObstacle::Impl::PosInfo& WrapObstacle::Impl::getPosInfo(const State &s) const { - realizePosition(state); - return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); + realizePosition(s); + return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); } -void WrapObstacle::Impl::applyGeodesicCorrection(const State& state, const WrapObstacle::Impl::Correction& c) const +void WrapObstacle::Impl::applyGeodesicCorrection(const State& s, const WrapObstacle::Impl::Correction& c) const { // Apply correction to curve. - m_Surface.getImpl().applyGeodesicCorrection(state, c); + m_Surface.getImpl().applyGeodesicCorrection(s, c); // Invalidate position level cache. - m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); + m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } void WrapObstacle::Impl::calcPosInfo( - const State& state, + const State& s, PosInfo& posInfo) const { - if(isDisabled(state)) + if(isDisabled(s)) { return; } // Get tramsform from local surface frame to ground. - Transform X_GS = m_Mobod.getBodyTransform(state).compose(m_Offset); + Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); // Get the path points before and after this segment. - Vec3 prev_G = m_Path.getImpl().findPrevPoint(state, m_PathSegmentIx); - Vec3 next_G = m_Path.getImpl().findNextPoint(state, m_PathSegmentIx); + Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_PathSegmentIx); + Vec3 next_G = m_Path.getImpl().findNextPoint(s, m_PathSegmentIx); // Transform the prev and next path points to the surface frame. Vec3 prev_S = X_GS.shiftBaseStationToFrame(prev_G); @@ -332,10 +366,10 @@ void WrapObstacle::Impl::calcPosInfo( // Detect liftoff, touchdown and potential invalid configurations. // TODO this doesnt follow the regular invalidation scheme... - m_Surface.getImpl().calcUpdatedStatus(state, prev_S, next_S); + m_Surface.getImpl().calcUpdatedStatus(s, prev_S, next_S); // Grab the last geodesic that was computed. - const Surface::LocalGeodesic& geodesic_S = m_Surface.getImpl().getGeodesic(state); + const Surface::LocalGeodesic& geodesic_S = m_Surface.getImpl().getGeodesic(s); // Store the the local geodesic in ground frame. posInfo.KP = X_GS.compose(geodesic_S.KP); @@ -361,10 +395,10 @@ void WrapObstacle::Impl::calcPosInfo( throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); } -void WrapObstacle::Impl::calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const +void WrapObstacle::Impl::calcGeodesic(State& s, Vec3 x, Vec3 t, Real l) const { - m_Surface.getImpl().calcGeodesic(state, x, t, l); - m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); + m_Surface.getImpl().calcGeodesic(s, x, t, l); + m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } //============================================================================== @@ -380,25 +414,14 @@ namespace using Correction = ContactGeometry::GeodesicCorrection; /* using LocalGeodesic = WrapObstacle::LocalGeodesicInfo; */ -int countActive(const State& s, const std::vector& obstacles) -{ - int n = 0; - for (const WrapObstacle& o : obstacles) { - if (o.isActive(s)) { - ++n; - } - } - return n; -} - const WrapObstacle* FindPrevActiveObstacle( - const State& state, + const State& s, const std::vector& obs, size_t idx) { for (ptrdiff_t i = idx - 1; i > 0; --i) { // Find the active segment before the current. - if (obs.at(i).isActive(state)) { + if (obs.at(i).isActive(s)) { return &obs.at(i); } } @@ -406,13 +429,13 @@ const WrapObstacle* FindPrevActiveObstacle( } const WrapObstacle* FindNextActiveObstacle( - const State& state, + const State& s, const std::vector& obs, size_t idx) { // Find the active segment after the current. for (ptrdiff_t i = idx + 1; i < obs.size(); ++i) { - if (obs.at(i).isActive(state)) { + if (obs.at(i).isActive(s)) { return &obs.at(i); } } @@ -465,45 +488,45 @@ GeodesicJacobian addPathErrorJacobian( // PATH IMPL //============================================================================== -void WrappingPath::Impl::realizeTopology(State &state) +void WrappingPath::Impl::realizeTopology(State &s) { PosInfo posInfo {}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry(state, Stage::Position, new Value(posInfo)); + m_PosInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Position, new Value(posInfo)); } -void WrappingPath::Impl::realizePosition(const State &state) const +void WrappingPath::Impl::realizePosition(const State &s) const { - if (m_Subsystem.isCacheValueRealized(state, m_PosInfoIx)) {return;} - calcPosInfo(state, updPosInfo(state)); - m_Subsystem.markCacheValueRealized(state, m_PosInfoIx); + if (m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) {return;} + calcPosInfo(s, updPosInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); } -const WrappingPath::Impl::PosInfo& WrappingPath::Impl::getPosInfo(const State &state) const +const WrappingPath::Impl::PosInfo& WrappingPath::Impl::getPosInfo(const State &s) const { - realizePosition(state); - return Value::downcast(m_Subsystem.getCacheEntry(state, m_PosInfoIx)); + realizePosition(s); + return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); } -WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &state) const +WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &s) const { - return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); + return Value::updDowncast(m_Subsystem.updCacheEntry(s, m_PosInfoIx)); } -void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& state, std::function GetInitPointGuess) const +void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& s, std::function GetInitPointGuess) const { - Vec3 prev = m_OriginBody.getBodyTransform(state).shiftFrameStationToBase(m_OriginPoint); + Vec3 prev = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); size_t i = 0; for (const WrapObstacle& obstacle: m_Obstacles) { Vec3 xGuess = GetInitPointGuess(++i); - obstacle.getImpl().calcGeodesic(state, xGuess, xGuess - prev, 0.); + obstacle.getImpl().calcGeodesic(s, xGuess, xGuess - prev, 0.); - prev = obstacle.getImpl().getPosInfo(state).KQ.p(); + prev = obstacle.getImpl().getPosInfo(s).KQ.p(); } } template void WrappingPath::Impl::calcPathErrorVector( - const State& state, + const State& s, const std::vector& obs, const std::vector& lines, std::array axes, @@ -513,11 +536,11 @@ void WrappingPath::Impl::calcPathErrorVector( ptrdiff_t row = -1; for (const WrapObstacle& o : obs) { - if (!o.isActive(state)) { + if (!o.isActive(s)) { continue; } - const GeodesicInfo& g = o.getImpl().getPosInfo(state); + const GeodesicInfo& g = o.getImpl().getPosInfo(s); for (CoordinateAxis axis: axes) { pathError(++row) = calcPathError(lines.at(lineIx), g.KP.R(), axis); } @@ -530,14 +553,14 @@ void WrappingPath::Impl::calcPathErrorVector( template void WrappingPath::Impl::calcPathErrorJacobian( - const State& state, + const State& s, const std::vector& obs, const std::vector& lines, std::array axes, Matrix& J) { constexpr size_t Nq = GeodesicDOF; - const size_t n = countActive(state, obs); + const size_t n = countActive(s, obs); 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"); @@ -545,16 +568,16 @@ void WrappingPath::Impl::calcPathErrorJacobian( size_t row = 0; size_t col = 0; for (size_t i = 0; i < n; ++i) { - if (!obs.at(i).isActive(state)) { + if (!obs.at(i).isActive(s)) { continue; } - const GeodesicInfo& g = obs.at(i).getImpl().getPosInfo(state); + const GeodesicInfo& g = obs.at(i).getImpl().getPosInfo(s); const LineSegment& l_P = lines.at(i); const LineSegment& l_Q = lines.at(i + 1); - const WrapObstacle* prev = FindPrevActiveObstacle(state, obs, i); - const WrapObstacle* next = FindNextActiveObstacle(state, obs, i); + const WrapObstacle* prev = FindPrevActiveObstacle(s, obs, i); + const WrapObstacle* next = FindNextActiveObstacle(s, obs, i); for (CoordinateAxis axis: axes) { const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); @@ -563,7 +586,7 @@ void WrappingPath::Impl::calcPathErrorJacobian( addPathErrorJacobian(l_P, a_P, dK_P, J.block(row, col, 1, Nq)); if (prev) { - const Variation& prev_dK_Q = prev->getImpl().getPosInfo(state).dKQ; + const Variation& prev_dK_Q = prev->getImpl().getPosInfo(s).dKQ; addDirectionJacobian(l_P, a_P, prev_dK_Q[1], J.block(row, col-Nq, 1, Nq), true); } ++row; @@ -576,7 +599,7 @@ void WrappingPath::Impl::calcPathErrorJacobian( addPathErrorJacobian(l_Q, a_Q, dK_Q, J.block(row, col, 1, Nq), true); if (next) { - const Variation& next_dK_P = next->getImpl().getPosInfo(state).dKP; + const Variation& next_dK_P = next->getImpl().getPosInfo(s).dKP; addDirectionJacobian(l_Q, a_Q, next_dK_P[1], J.block(row, col+Nq, 1, Nq)); } ++row; @@ -587,7 +610,7 @@ void WrappingPath::Impl::calcPathErrorJacobian( } double WrappingPath::Impl::calcPathLength( - const State& state, + const State& s, const std::vector& obs, const std::vector& lines) { @@ -598,17 +621,17 @@ double WrappingPath::Impl::calcPathLength( } for (const WrapObstacle& obstacle : obs) { - if (!obstacle.isActive(state)) + if (!obstacle.isActive(s)) { continue; } - lTot += obstacle.getImpl().getPosInfo(state).length; + lTot += obstacle.getImpl().getPosInfo(s).length; } return lTot; } void WrappingPath::Impl::calcLineSegments( - const State& state, + const State& s, Vec3 p_O, Vec3 p_I, const std::vector& obs, @@ -620,11 +643,11 @@ void WrappingPath::Impl::calcLineSegments( Vec3 lineStart = std::move(p_O); for (const WrapObstacle& o : obs) { - if (!o.isActive(state)) { + if (!o.isActive(s)) { continue; } - const GeodesicInfo& g = o.getImpl().getPosInfo(state); + const GeodesicInfo& g = o.getImpl().getPosInfo(s); const Vec3 lineEnd = g.KP.p(); lines.emplace_back(lineStart, lineEnd); @@ -634,23 +657,34 @@ void WrappingPath::Impl::calcLineSegments( } Vec3 WrappingPath::Impl::FindPrevPoint( - const State& state, + const State& s, const Vec3& originPoint, const std::vector& obs, size_t idx) { - const WrapObstacle* prev = FindPrevActiveObstacle(state, obs, idx); - return prev ? prev->getImpl().getPosInfo(state).KQ.p() : originPoint; + const WrapObstacle* prev = FindPrevActiveObstacle(s, obs, idx); + return prev ? prev->getImpl().getPosInfo(s).KQ.p() : originPoint; } Vec3 WrappingPath::Impl::FindNextPoint( - const State& state, + const State& s, const Vec3& terminationPoint, const std::vector& obs, size_t idx) { - const WrapObstacle* next = FindNextActiveObstacle(state, obs, idx); - return next ? next->getImpl().getPosInfo(state).KP.p() : terminationPoint; + const WrapObstacle* next = FindNextActiveObstacle(s, obs, idx); + return next ? next->getImpl().getPosInfo(s).KP.p() : terminationPoint; +} + +size_t WrappingPath::Impl::countActive(const State& s) const +{ + size_t count = 0; + for (const WrapObstacle& o : m_Obstacles) { + if (o.getImpl().isActive(s)) { + ++count; + } + } + return count; } void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index ca98cbeea..91f478672 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -23,67 +23,6 @@ class WrappingPathSubsystem; class WrapObstacle; class WrappingPath; -//============================================================================== -// SURFACE -//============================================================================== -// Represents the local surface wrapping problem. -// Caches last computed geodesic as a warmstart. -// Not exposed outside of simbody. -// Not shared amongst different paths or obstacles. -class SimTK_SIMBODY_EXPORT Surface -{ - using FrenetFrame = ContactGeometry::FrenetFrame; - using Variation = ContactGeometry::GeodesicVariation; - using Correction = ContactGeometry::GeodesicCorrection; - -public: - Surface() = default; - ~Surface() = default; - Surface(Surface&&) noexcept = default; - Surface& operator=(Surface&&) noexcept = default; - Surface(const Surface&) = delete; - Surface& operator=(const Surface&) = delete; - - Surface(WrappingPathSubsystem subsystem, const ContactGeometry& geometry, Vec3 xHint); - - // TODO move to impl to hide? - struct LocalGeodesic - { - FrenetFrame KP {}; - FrenetFrame KQ {}; - - Real length = NaN; - - Variation dKP {}; - Variation dKQ {}; - }; - - // TODO move to impl to hide? - struct WrappingStatus - { - Vec3 pointOnLine {NaN, NaN, NaN}; - - bool liftoff = false; - bool disabled = false; - }; - - const LocalGeodesic& calcGeodesic(State& s, Vec3 x, Vec3 t, Real l); - const LocalGeodesic& applyGeodesicCorrection(const State& s, const Correction& c); - const LocalGeodesic& getGeodesic(const State& s); - Vec3 getPointOnLineNearSurface(const State& s); - - class Impl; -private: - explicit Surface(std::unique_ptr impl); - const Impl& getImpl() const; - Impl& updImpl(); - - friend Impl; - friend WrapObstacle; - - std::unique_ptr impl = nullptr; -}; - //============================================================================== // OBSTACLE //============================================================================== @@ -103,6 +42,14 @@ class SimTK_SIMBODY_EXPORT WrapObstacle bool isActive(const State& state) const; class Impl; + + enum class Status + { + Ok, + Liftoff, + Disabled, + }; + private: explicit WrapObstacle(std::unique_ptr impl); diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index d3e202656..9980feb6f 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -19,131 +19,6 @@ namespace SimTK SimTK_DEFINE_UNIQUE_INDEX_TYPE(PathSegmentIndex); SimTK_DEFINE_UNIQUE_INDEX_TYPE(PathIndex); -//============================================================================== -// SURFACE IMPL -//============================================================================== -class Surface::Impl { - using FrenetFrame = ContactGeometry::FrenetFrame; - using Variation = ContactGeometry::GeodesicVariation; - using Correction = ContactGeometry::GeodesicCorrection; - using PointOnLineResult = ContactGeometry::PointOnLineResult; - -public: - Impl() = default; - ~Impl() = default; - Impl(Impl&&) noexcept = default; - Impl& operator=(Impl&&) noexcept = default; - - Impl(const Impl&) = delete; - Impl& operator=(const Impl&) = delete; - - struct GeodesicInitialConditions - { - Vec3 x {NaN, NaN, NaN}; - Vec3 t {NaN, NaN, NaN}; - Real l = NaN; - }; - - struct LocalGeodesicInfo : Surface::LocalGeodesic, Surface::WrappingStatus - { - std::vector points {}; - double sHint = NaN; - }; - - Impl( - WrappingPathSubsystem subsystem, - ContactGeometry geometry, - Vec3 initPointGuess - ) : - m_Subsystem(subsystem), - m_Geometry(geometry), - m_InitPointGuess(initPointGuess) - {} - - // Allocate state variables and cache entries. - void realizeTopology(State& state); - // TODO requires all levels? - void realizeInstance(const State& state) const; - void realizePosition(const State& state) const; - void realizeVelocity(const State& state) const; - void realizeAcceleration(const State& state) const; - // Requires invalidateTopology? - /* void invalidateTopology() */ - /* { m_Subsystem.invalidateSubsystemTopologyCache(); } */ - - // The virtual methods: - // 1. calcGeodesic() - // 2. calcPointOnLineNearSurface - // 3. isPointAboveSurface - // 4. calcPathPoints - - // 1. calcGeodesic - const LocalGeodesic& calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; - /* void calcGeodesic(GeodesicInitialConditions g0, Real sHint, LocalGeodesic& geodesic) const; */ - /* void calcGeodesic(Vec3 x, Vec3 t, Real l, Real sHint, LocalGeodesic& geodesic) const; */ - const LocalGeodesic& getGeodesic(const State& state) const {return getLocalGeodesicInfo(state);} - void applyGeodesicCorrection(const State& state, const Correction& c) const; - - // TODO rename - bool isActive(const State& state) const; - bool isDisabled(const State& state) const; - - // 4. calcPathPoints - size_t calcPathPoints(const State& state, std::vector& points) const; - - // Detect liftoff or touchdown, and compute the point on line near the surface. - const WrappingStatus& calcUpdatedStatus(const State& state, Vec3 prev, Vec3 next) const; - - // TODO should be in OpenSim? - void setInitialPointGuess(Vec3 pointGuess); - Vec3 getInitialPointGuess() const; - -private: - const LocalGeodesicInfo& getLocalGeodesicInfo(const State& state) const - { - realizePosition(state); - return Value::downcast( - m_Subsystem.getDiscreteVarUpdateValue(state, m_GeodesicInfoIx) - ); - } - - LocalGeodesicInfo& updLocalGeodesicInfo(const State& state) const - { - return Value::updDowncast( - m_Subsystem.updDiscreteVarUpdateValue(state, m_GeodesicInfoIx) - ); - } - - const LocalGeodesicInfo& getPrevLocalGeodesicInfo(const State& state) const - { - return Value::downcast( - m_Subsystem.getDiscreteVariable(state, m_GeodesicInfoIx) - ); - } - - LocalGeodesicInfo& updPrevLocalGeodesicInfo(State& state) const - { - return Value::updDowncast( - m_Subsystem.updDiscreteVariable(state, m_GeodesicInfoIx) - ); - } - - void calcLocalGeodesicInfo(Vec3 x, Vec3 t, Real l, Real sHint, - LocalGeodesicInfo& geodesic) const; - -//------------------------------------------------------------------------------ - WrappingPathSubsystem m_Subsystem; - - ContactGeometry m_Geometry; - - Vec3 m_InitPointGuess; - - Real m_TouchdownAccuracy = 1e-3; - size_t m_TouchdownIter = 10; - - DiscreteVariableIndex m_GeodesicInfoIx; -}; - //============================================================================== // OBSTACLE IMPL //============================================================================== @@ -153,10 +28,10 @@ class WrapObstacle::Impl using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; -private: + private: Impl() = default; -public: + public: Impl(const Impl& source) = default; Impl& operator=(const Impl& source) = default; ~Impl() = default; @@ -171,7 +46,7 @@ class WrapObstacle::Impl const Transform& X_BS, ContactGeometry geometry, Vec3 initPointGuess - ) : + ) : m_Subsystem(subsystem), m_Mobod(mobod), m_Offset(X_BS), @@ -224,12 +99,142 @@ class WrapObstacle::Impl size_t calcPathPoints(const State& state, std::vector& points) const; void applyGeodesicCorrection(const State& state, const ContactGeometry::GeodesicCorrection& c) const; + void calcInitZeroLengthGeodesic(State& state, Vec3 prev_QS); - void calcPosInfo(const State& state, Vec3 prev, Vec3 next, size_t maxIter, Real eps) const; + private: + void calcPosInfo(const State& state, PosInfo& posInfo) const; void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; -private: - void calcPosInfo(const State& state, PosInfo& posInfo) const; + public: // TODO public? + //============================================================================== + // SURFACE + //============================================================================== + // Represents the local surface wrapping problem. + // Caches last computed geodesic as a warmstart. + // Not exposed outside of simbody. + // Not shared amongst different paths or obstacles. + class Surface + { + using FrenetFrame = ContactGeometry::FrenetFrame; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; + + public: + Surface() = default; + ~Surface() = default; + Surface(Surface&&) noexcept = default; + Surface& operator=(Surface&&) noexcept = default; + Surface(const Surface&) = delete; + Surface& operator=(const Surface&) = delete; + + Surface( + WrappingPathSubsystem subsystem, + ContactGeometry geometry, + Vec3 initPointGuess + ) : + m_Subsystem(subsystem), + m_Geometry(geometry), + m_InitPointGuess(initPointGuess) + {} + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizePosition(const State& state) const; + + struct LocalGeodesicInfo + { + FrenetFrame KP {}; + FrenetFrame KQ {}; + + Real length = NaN; + + Variation dKP {}; + Variation dKQ {}; + + Status status = Status::Ok; + }; + + struct GeodesicInitialConditions + { + private: + GeodesicInitialConditions() = default; + + public: + static GeodesicInitialConditions CreateCorrected(const FrenetFrame& KP, const Variation& dKP, Real l, const Correction& c); + static GeodesicInitialConditions CreateInSurfaceFrame(const Transform& X_GS, Vec3 x_G, Vec3 t_G, Real l); + static GeodesicInitialConditions CreateZeroLengthGuess(const Transform& X_GS, Vec3 prev_QS, Vec3 xGuess_S); + static GeodesicInitialConditions CreateAtTouchdown(Vec3 prev_QS, Vec3 next_PS, Vec3 trackingPointOnLine); + + Vec3 x {NaN, NaN, NaN}; + Vec3 t {NaN, NaN, NaN}; + Real l = NaN; + }; + + const LocalGeodesicInfo& calcInitialGeodesic(State& s, const GeodesicInitialConditions& g0) const; + // TODO weird name... + const LocalGeodesicInfo& calcLocalGeodesic(const State& s, Vec3 prev_QS, Vec3 next_PS) const; + void applyGeodesicCorrection(const State& s, const Correction& c) const; + + // 4. calcPathPoints + size_t calcPathPoints(const State& state, std::vector& points) const; + + void setInitialPointGuess(Vec3 initPointGuess) {m_InitPointGuess = initPointGuess;} + Vec3 getInitialPointGuess() const {return m_InitPointGuess;} + + private: + + struct CacheEntry : LocalGeodesicInfo + { + Vec3 trackingPointOnLine {NaN, NaN, NaN}; + std::vector points {}; + double sHint = NaN; + }; + + const CacheEntry& getCacheEntry(const State& state) const + { + return Value::downcast( + m_Subsystem.getDiscreteVarUpdateValue(state, m_CacheIx) + ); + } + + CacheEntry& updCacheEntry(const State& state) const + { + return Value::updDowncast( + m_Subsystem.updDiscreteVarUpdateValue(state, m_CacheIx) + ); + } + + const CacheEntry& getPrevCacheEntry(const State& state) const + { + return Value::downcast( + m_Subsystem.getDiscreteVariable(state, m_CacheIx) + ); + } + + CacheEntry& updPrevCacheEntry(State& state) const + { + return Value::updDowncast( + m_Subsystem.updDiscreteVariable(state, m_CacheIx) + ); + } + + void calcStatus(const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& cache) const; + void calcGeodesic(const GeodesicInitialConditions& g0, CacheEntry& cache) const; + + //------------------------------------------------------------------------------ + WrappingPathSubsystem m_Subsystem; + + ContactGeometry m_Geometry; + + Vec3 m_InitPointGuess; + + Real m_TouchdownAccuracy = 1e-3; + size_t m_TouchdownIter = 10; + + DiscreteVariableIndex m_CacheIx; + }; + + private: // TODO Required for accessing the cache variable? WrappingPathSubsystem m_Subsystem; // The subsystem this segment belongs to. @@ -302,6 +307,8 @@ class WrappingPath::Impl { PosInfo& updPosInfo(const State& s) const; void calcPosInfo(const State& s, PosInfo& posInfo) const; + size_t countActive(const State& s) const; + Vec3 findPrevPoint( const State& state, PathSegmentIndex idx) const; From 5aa88cfd5b673417a4a8dccc0d3e0dfe5bcfc7a9 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 23 Apr 2024 17:21:31 +0200 Subject: [PATCH 024/127] refactor: WrappingObstacle::Impl update for local Surface --- Simbody/include/simbody/internal/Wrapping.cpp | 101 +++++++++------- .../include/simbody/internal/WrappingImpl.h | 113 +++++++++--------- 2 files changed, 116 insertions(+), 98 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 2456b5f00..bd8ddaa67 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -228,6 +228,23 @@ void Surface::applyGeodesicCorrection(const State& s, const Correction& c) const calcGeodesic(g0, updCacheEntry(s)); } +size_t Surface::calcPathPoints(const State& s, std::vector& points) const +{ + realizePosition(s); + + size_t count = 0; + const CacheEntry& cache = getCacheEntry(s); + for (Vec3 p: cache.points) { + points.push_back(p); + ++count; + } + throw std::runtime_error("NOTYETIMPLEMENTED for analytic"); + return count; +} + +//------------------------------------------------------------------------------ +// PRIVATE METHODS +//------------------------------------------------------------------------------ void Surface::calcGeodesic(const GeodesicInitialConditions& g0, CacheEntry& cache) const { // Compute geodesic start boundary frame and variation. @@ -287,31 +304,25 @@ void Surface::calcStatus(const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& c } } -size_t Surface::calcPathPoints(const State& s, std::vector& points) const -{ - realizePosition(s); - - size_t count = 0; - const CacheEntry& cache = getCacheEntry(s); - for (Vec3 p: cache.points) { - points.push_back(p); - ++count; - } - throw std::runtime_error("NOTYETIMPLEMENTED for analytic"); - return count; -} - -//------------------------------------------------------------------------------ -// PRIVATE METHODS -//------------------------------------------------------------------------------ - //============================================================================== // OBSTACLE //============================================================================== -bool WrapObstacle::Impl::isActive(const State& s) const +WrapObstacle::Impl::Impl( + WrappingPath path, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess + ) + : + m_Subsystem(path.getImpl().getSubsystem()), + m_Path(path), + m_Mobod(mobod), + m_Offset(X_BS), + m_Surface(m_Subsystem, geometry, initPointGuess) { - return getPosInfo(s).status == Status::Ok; + throw std::runtime_error("NOTYETIMPLEMENTED: Must adopt to path, and give the index"); } void WrapObstacle::Impl::realizeTopology(State &s) @@ -329,47 +340,63 @@ void WrapObstacle::Impl::realizePosition(const State &s) const } } -const WrapObstacle::Impl::PosInfo& WrapObstacle::Impl::getPosInfo(const State &s) const +void WrapObstacle::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) const { - realizePosition(s); - return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); + // Get tramsform from local surface frame to ground. + Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); + + Vec3 prev_QS = X_GS.shiftBaseStationToFrame(prev_QG); + Vec3 xGuess_S = m_Surface.getInitialPointGuess(); // TODO move into function call? + + GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateZeroLengthGuess(X_GS, prev_QS, xGuess_S); + m_Surface.calcInitialGeodesic(s, g0); + + m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } void WrapObstacle::Impl::applyGeodesicCorrection(const State& s, const WrapObstacle::Impl::Correction& c) const { // Apply correction to curve. - m_Surface.getImpl().applyGeodesicCorrection(s, c); + m_Surface.applyGeodesicCorrection(s, c); // Invalidate position level cache. m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } +size_t WrapObstacle::Impl::calcPathPoints(const State& s, std::vector& points) const +{ + const Transform& X_GS = getPosInfo(s).X_GS; + size_t n = m_Surface.calcPathPoints(s, points); + for (size_t i = points.size() - n; i < points.size(); ++i) { + points.at(i) = X_GS.shiftFrameStationToBase(points.at(i)); + } + return n; +} + void WrapObstacle::Impl::calcPosInfo( const State& s, PosInfo& posInfo) const { - if(isDisabled(s)) + if(m_Surface.getStatus(s) == Status::Disabled) { return; } - // Get tramsform from local surface frame to ground. - Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); + // Compute tramsform from local surface frame to ground. + const Transform& X_GS = posInfo.X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); // Get the path points before and after this segment. - Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_PathSegmentIx); - Vec3 next_G = m_Path.getImpl().findNextPoint(s, m_PathSegmentIx); + const Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_PathSegmentIx); + const Vec3 next_G = m_Path.getImpl().findNextPoint(s, m_PathSegmentIx); // Transform the prev and next path points to the surface frame. - Vec3 prev_S = X_GS.shiftBaseStationToFrame(prev_G); - Vec3 next_S = X_GS.shiftBaseStationToFrame(next_G); + const Vec3 prev_S = X_GS.shiftBaseStationToFrame(prev_G); + const Vec3 next_S = X_GS.shiftBaseStationToFrame(next_G); // Detect liftoff, touchdown and potential invalid configurations. // TODO this doesnt follow the regular invalidation scheme... - m_Surface.getImpl().calcUpdatedStatus(s, prev_S, next_S); - // Grab the last geodesic that was computed. - const Surface::LocalGeodesic& geodesic_S = m_Surface.getImpl().getGeodesic(s); + const LocalGeodesicInfo& geodesic_S = m_Surface.calcLocalGeodesic(s, prev_S, next_S); // Store the the local geodesic in ground frame. posInfo.KP = X_GS.compose(geodesic_S.KP); @@ -395,12 +422,6 @@ void WrapObstacle::Impl::calcPosInfo( throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); } -void WrapObstacle::Impl::calcGeodesic(State& s, Vec3 x, Vec3 t, Real l) const -{ - m_Surface.getImpl().calcGeodesic(s, x, t, l); - m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); -} - //============================================================================== // PATH HELPERS //============================================================================== diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 9980feb6f..0f05bd2b9 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -36,22 +36,13 @@ class WrapObstacle::Impl Impl& operator=(const Impl& source) = default; ~Impl() = default; - /* Surface(const MobilizedBody& mobod, const Transform& X_BS, const ContactGeometry& geometry) */ - /* : impl(std::make_shared(mobod, X_BS, geometry)) {} */ - /* Transform calcSurfaceToGroundTransform(const State& state) const {return impl->calcSurfaceToGroundTransform(state);} */ - Impl( - WrappingPathSubsystem subsystem, + WrappingPath path, const MobilizedBody& mobod, const Transform& X_BS, ContactGeometry geometry, Vec3 initPointGuess - ) : - m_Subsystem(subsystem), - m_Mobod(mobod), - m_Offset(X_BS), - m_Surface(subsystem, geometry, initPointGuess) - {} + ); enum class Status { @@ -60,52 +51,6 @@ class WrapObstacle::Impl Disabled, }; - // Ground frame solution. - struct PosInfo - { - FrenetFrame KP {}; - FrenetFrame KQ {}; - - Variation dKP {}; - Variation dKQ {}; - - Real length = NaN; - - Status status = Status::Ok; - }; - - // Allocate state variables and cache entries. - void realizeTopology(State& state); - /* void realizeInstance(const State& state) const; */ - void realizePosition(const State& state) const; - /* void realizeVelocity(const State& state) const; */ - /* void realizeAcceleration(const State& state) const; */ - /* void invalidateTopology(); */ - - void invalidatePositionLevelCache(const State& state) const - { - m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); - } - - const PosInfo& getPosInfo(const State& state) const; - - bool isActive(const State& state) const; - bool isDisabled(const State& state) const; - - PosInfo& updPosInfo(const State &state) const - { - return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); - } - - size_t calcPathPoints(const State& state, std::vector& points) const; - void applyGeodesicCorrection(const State& state, const ContactGeometry::GeodesicCorrection& c) const; - void calcInitZeroLengthGeodesic(State& state, Vec3 prev_QS); - - private: - void calcPosInfo(const State& state, PosInfo& posInfo) const; - void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; - - public: // TODO public? //============================================================================== // SURFACE //============================================================================== @@ -181,6 +126,8 @@ class WrapObstacle::Impl void setInitialPointGuess(Vec3 initPointGuess) {m_InitPointGuess = initPointGuess;} Vec3 getInitialPointGuess() const {return m_InitPointGuess;} + Status getStatus(const State& s) const {return getCacheEntry(s).status;} + private: struct CacheEntry : LocalGeodesicInfo @@ -234,7 +181,56 @@ class WrapObstacle::Impl DiscreteVariableIndex m_CacheIx; }; + // Ground frame solution. + struct PosInfo + { + Transform X_GS {}; + + FrenetFrame KP {}; + FrenetFrame KQ {}; + + Variation dKP {}; + Variation dKQ {}; + + Real length = NaN; + + Status status = Status::Ok; + }; + + // Allocate state variables and cache entries. + void realizeTopology(State& state); + /* void realizeInstance(const State& state) const; */ + void realizePosition(const State& state) const; + /* void realizeVelocity(const State& state) const; */ + /* void realizeAcceleration(const State& state) const; */ + /* void invalidateTopology(); */ + + void invalidatePositionLevelCache(const State& state) const + { + m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); + } + + const PosInfo& getPosInfo(const State& s) const { + realizePosition(s); + return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); + } + + bool isActive(const State& s) const {return getPosInfo(s).status == Status::Ok;} + Status getStatus(const State& s) const {return m_Surface.getStatus(s);} + + void calcInitZeroLengthGeodesic(State& state, Vec3 prev_QS) const; + void applyGeodesicCorrection(const State& state, const ContactGeometry::GeodesicCorrection& c) const; + size_t calcPathPoints(const State& state, std::vector& points) const; + + // TODO allow for user to shoot his own geodesic. + /* void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; */ + private: + PosInfo& updPosInfo(const State &state) const + { + return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); + } + void calcPosInfo(const State& state, PosInfo& posInfo) const; // TODO Required for accessing the cache variable? WrappingPathSubsystem m_Subsystem; // The subsystem this segment belongs to. @@ -250,7 +246,6 @@ class WrapObstacle::Impl CacheEntryIndex m_PosInfoIx; }; - //============================================================================== // PATH :: IMPL //============================================================================== @@ -368,6 +363,8 @@ static double calcPathLength( const std::vector& obs, const std::vector& lines); +const WrappingPathSubsystem& getSubsystem() const {return m_Subsystem;} + WrappingPathSubsystem m_Subsystem; MobilizedBody m_OriginBody; From 111d3195dfe08b9b254a9284b2bfec13ece3f2d2 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 24 Apr 2024 07:43:39 +0200 Subject: [PATCH 025/127] add computing lengthDot --- Simbody/include/simbody/internal/Wrapping.cpp | 119 +++++++++++++----- .../include/simbody/internal/WrappingImpl.h | 69 ++++++---- 2 files changed, 129 insertions(+), 59 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index bd8ddaa67..bc88145c7 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -10,11 +10,16 @@ using namespace SimTK; using Correction = ContactGeometry::GeodesicCorrection; +using FrameVariation = ContactGeometry::GeodesicFrameVariation; using FrenetFrame = ContactGeometry::FrenetFrame; -using GeodesicInitialConditions = WrapObstacle::Impl::Surface::GeodesicInitialConditions; +using GeodesicInfo = WrapObstacle::Impl::PosInfo; using LocalGeodesicInfo = WrapObstacle::Impl::Surface::LocalGeodesicInfo; -using GeodesicInfo = WrapObstacle::Impl::GeodesicInfo; +using GeodesicInitialConditions = WrapObstacle::Impl::Surface::GeodesicInitialConditions; +using GeodesicJacobian = Vec4; +using PointVariation = ContactGeometry::GeodesicPointVariation; using Variation = ContactGeometry::GeodesicVariation; +using LineSegment = WrappingPath::LineSegment; +using Status = WrapObstacle::Impl::Status; //============================================================================== // CONSTANTS @@ -428,12 +433,6 @@ void WrapObstacle::Impl::calcPosInfo( namespace { - using GeodesicJacobian = Vec4; - using PointVariation = ContactGeometry::GeodesicPointVariation; - using FrameVariation = ContactGeometry::GeodesicFrameVariation; - using Variation = ContactGeometry::GeodesicVariation; - using Correction = ContactGeometry::GeodesicCorrection; -/* using LocalGeodesic = WrapObstacle::LocalGeodesicInfo; */ const WrapObstacle* FindPrevActiveObstacle( const State& s, @@ -513,6 +512,9 @@ void WrappingPath::Impl::realizeTopology(State &s) { PosInfo posInfo {}; m_PosInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Position, new Value(posInfo)); + + VelInfo velInfo {}; + m_VelInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Velocity, new Value(velInfo)); } void WrappingPath::Impl::realizePosition(const State &s) const @@ -522,6 +524,13 @@ void WrappingPath::Impl::realizePosition(const State &s) const m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); } +void WrappingPath::Impl::realizeVelocity(const State &s) const +{ + if (m_Subsystem.isCacheValueRealized(s, m_VelInfoIx)) {return;} + calcVelInfo(s, updVelInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_VelInfoIx); +} + const WrappingPath::Impl::PosInfo& WrappingPath::Impl::getPosInfo(const State &s) const { realizePosition(s); @@ -533,15 +542,28 @@ WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &s) cons return Value::updDowncast(m_Subsystem.updCacheEntry(s, m_PosInfoIx)); } -void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& s, std::function GetInitPointGuess) const +const WrappingPath::Impl::VelInfo& WrappingPath::Impl::getVelInfo(const State &s) const +{ + realizeVelocity(s); + return Value::downcast(m_Subsystem.getCacheEntry(s, m_VelInfoIx)); +} + +WrappingPath::Impl::VelInfo& WrappingPath::Impl::updVelInfo(const State &s) const +{ + return Value::updDowncast(m_Subsystem.updCacheEntry(s, m_VelInfoIx)); +} + +void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& s) const { - Vec3 prev = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + Vec3 prev_QG = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); size_t i = 0; for (const WrapObstacle& obstacle: m_Obstacles) { - Vec3 xGuess = GetInitPointGuess(++i); - obstacle.getImpl().calcGeodesic(s, xGuess, xGuess - prev, 0.); + if (obstacle.getImpl().getStatus(s) == Status::Disabled) { + continue; + } + obstacle.getImpl().calcInitZeroLengthGeodesic(s, prev_QG); - prev = obstacle.getImpl().getPosInfo(s).KQ.p(); + prev_QG = obstacle.getImpl().getPosInfo(s).KQ.p(); } } @@ -557,7 +579,7 @@ void WrappingPath::Impl::calcPathErrorVector( ptrdiff_t row = -1; for (const WrapObstacle& o : obs) { - if (!o.isActive(s)) { + if (!o.getImpl().isActive(s)) { continue; } @@ -581,7 +603,9 @@ void WrappingPath::Impl::calcPathErrorJacobian( Matrix& J) { constexpr size_t Nq = GeodesicDOF; - const size_t n = countActive(s, obs); + + // TODO perhaps just not make method static. + const size_t n = lines.size() - 1; 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"); @@ -589,7 +613,7 @@ void WrappingPath::Impl::calcPathErrorJacobian( size_t row = 0; size_t col = 0; for (size_t i = 0; i < n; ++i) { - if (!obs.at(i).isActive(s)) { + if (!obs.at(i).getImpl().isActive(s)) { continue; } const GeodesicInfo& g = obs.at(i).getImpl().getPosInfo(s); @@ -642,7 +666,7 @@ double WrappingPath::Impl::calcPathLength( } for (const WrapObstacle& obstacle : obs) { - if (!obstacle.isActive(s)) + if (!obstacle.getImpl().isActive(s)) { continue; } @@ -655,16 +679,14 @@ void WrappingPath::Impl::calcLineSegments( const State& s, Vec3 p_O, Vec3 p_I, - const std::vector& obs, - std::vector& lines) + std::vector& lines) const { - const size_t n = obs.size(); - lines.reserve(n + 1); + lines.resize(m_Obstacles.size() + 1); lines.clear(); Vec3 lineStart = std::move(p_O); - for (const WrapObstacle& o : obs) { - if (!o.isActive(s)) { + for (const WrapObstacle& o : m_Obstacles) { + if (!o.getImpl().isActive(s)) { continue; } @@ -720,28 +742,29 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const const size_t nActive = countActive(s); // Grab the shared data cache for computing the matrices, and lock it. - SolverDataCache& data = findDataCache(nActive); - data.lock(); + SolverDataCache& dataCache = findDataCache(nActive); + dataCache.lock(); + SolverData& data = dataCache.updData(); // Compute the straight-line segments. - calcLineSegments(s, x_O, x_I, m_Obstacles, data.updData().lineSegments); + calcLineSegments(s, x_O, x_I, data.lineSegments); // Evaluate path error, and stop when converged. - calcPathErrorVector<2>(s, m_Obstacles, posInfo.lines, axes, data.updData().pathError); - const Real maxPathError = posInfo.pathError.normInf(); + calcPathErrorVector<2>(s, m_Obstacles, data.lineSegments, axes, data.pathError); + const Real maxPathError = data.pathError.normInf(); if (maxPathError < m_PathErrorBound) { return; } // Evaluate the path error jacobian. - calcPathErrorJacobian<2>(s, m_Obstacles, posInfo.lines, axes, posInfo.pathErrorJacobian); + calcPathErrorJacobian<2>(s, m_Obstacles, data.lineSegments, axes, data.pathErrorJacobian); // Compute path corrections. - const Correction* corrIt = calcPathCorrections(data.updData()); + const Correction* corrIt = calcPathCorrections(data); // Apply path corrections. for (const WrapObstacle& obstacle : m_Obstacles) { - if (!obstacle.isActive(s)) { + if (!obstacle.getImpl().isActive(s)) { continue; } obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); @@ -749,7 +772,7 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const } // Release the lock on the shared data. - data.unlock(); + dataCache.unlock(); // Path has changed: invalidate each segment's cache. for (const WrapObstacle& obstacle : m_Obstacles) { @@ -760,6 +783,38 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const throw std::runtime_error("Failed to converge"); } +void WrappingPath::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const +{ + const PosInfo& pos = getPosInfo(s); + + Real lengthDot = 0.; + + Vec3 v_GQ = m_OriginBody.findStationVelocityInGround(s, m_OriginPoint); + const WrapObstacle* lastActive = nullptr; + for (const WrapObstacle& obstacle : m_Obstacles) { + if (!obstacle.getImpl().isActive(s)) { + continue; + } + + const GeodesicInfo& g = obstacle.getImpl().getPosInfo(s); + const UnitVec3 e_G = g.KP.R().getAxisUnitVec(TangentAxis); + + Vec3 next_v_GQ; + Vec3 v_GP; + obstacle.getImpl().calcContactPointVelocitiesInGround(s, v_GP, next_v_GQ); + + lengthDot += dot(e_G, v_GP - v_GQ); + + v_GQ = next_v_GQ; + lastActive = &obstacle; + } + + const Vec3 v_GP = m_TerminationBody.findStationVelocityInGround(s, m_TerminationPoint); + const UnitVec3 e_G = lastActive? lastActive->getImpl().getPosInfo(s).KQ.R().getAxisUnitVec(TangentAxis) : UnitVec3(pos.xO - pos.xI); + + lengthDot += dot(e_G, v_GP - v_GQ); +} + //============================================================================== // SUBSYSTEM //============================================================================== diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 0f05bd2b9..d6a5a2785 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -222,6 +222,28 @@ class WrapObstacle::Impl void applyGeodesicCorrection(const State& state, const ContactGeometry::GeodesicCorrection& c) const; size_t calcPathPoints(const State& state, std::vector& points) const; + /* const MobilizedBody& getMobilizedBody() const {return m_Mobod;} */ + void calcContactPointVelocitiesInGround(const State& s, Vec3& v_GP, Vec3& v_GQ) const + { + // TODO use builtin? + /* v_GP = m_Mobod.findStationVelocityInGround(state, P_B); */ + + // Get body kinematics in ground frame. + const Vec3 x_BG = m_Mobod.getBodyOriginLocation(s); + const Vec3& w_BG = m_Mobod.getBodyAngularVelocity(s); + const Vec3& v_BG = m_Mobod.getBodyOriginVelocity(s); + + const PosInfo& pos = getPosInfo(s); + + // Relative contact point positions to body origin, expressed in ground frame. + Vec3 r_P = pos.KP.p() - x_BG; + Vec3 r_Q = pos.KQ.p() - x_BG; + + // Compute contact point velocities in ground frame. + v_GP = v_BG + w_BG % r_P; + v_GQ = v_BG + w_BG % r_Q; + } + // TODO allow for user to shoot his own geodesic. /* void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; */ @@ -270,37 +292,38 @@ class WrappingPath::Impl { Real l = NaN; - std::vector lines; - - Vector pathError {}; - Matrix pathMatrix {}; - Matrix pathErrorJacobian {}; - Vector pathCorrections {}; - size_t loopIter = 0; - // TODO solver matrices + // TODO no matrices here? }; - std::vector& updObstacles() {return m_Obstacles;} - const std::vector& getObstacles() const {return m_Obstacles;} + struct VelInfo + { + Real lengthDot = NaN; + }; + + /* std::vector& updObstacles() {return m_Obstacles;} */ + /* const std::vector& getObstacles() const {return m_Obstacles;} */ // Allocate state variables and cache entries. void realizeTopology(State& state); void realizePosition(const State& state) const; void realizeVelocity(const State& state) const; - void realizeAcceleration(const State& state) const; void invalidateTopology() { m_Subsystem.invalidateSubsystemTopologyCache(); } const PosInfo& getPosInfo(const State& state) const; + const VelInfo& getVelInfo(const State& state) const; - void calcInitZeroLengthGeodesic(State& state, std::function GetInitPointGuess) const; + void calcInitZeroLengthGeodesic(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; size_t countActive(const State& s) const; @@ -343,27 +366,19 @@ class WrappingPath::Impl { std::array axes, Matrix& J); - static void calcLineSegments( + // Make static or not? + void calcLineSegments( const State& s, Vec3 p_O, Vec3 p_I, - const std::vector& obs, - std::vector& lines); + std::vector& lines) const; - static size_t CalcUpdatedObstaclePosInfo( - const State& s, - const Vec3& x_O, - const Vec3& x_I, + static double calcPathLength( + const State& state, const std::vector& obs, - size_t maxIter, - Real eps); - -static double calcPathLength( - const State& state, - const std::vector& obs, - const std::vector& lines); + const std::vector& lines); -const WrappingPathSubsystem& getSubsystem() const {return m_Subsystem;} + const WrappingPathSubsystem& getSubsystem() const {return m_Subsystem;} WrappingPathSubsystem m_Subsystem; From 8b53b1835086715b9c8d7b8eec65fd918ca181f7 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 24 Apr 2024 08:52:16 +0200 Subject: [PATCH 026/127] rename to CableSpan and CurveSegment --- Simbody/include/simbody/internal/Wrapping.cpp | 136 ++++++++--------- Simbody/include/simbody/internal/Wrapping.h | 66 ++++---- .../include/simbody/internal/WrappingImpl.h | 143 +++++++++--------- 3 files changed, 173 insertions(+), 172 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index bc88145c7..05b2b84fc 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -12,14 +12,14 @@ using namespace SimTK; using Correction = ContactGeometry::GeodesicCorrection; using FrameVariation = ContactGeometry::GeodesicFrameVariation; using FrenetFrame = ContactGeometry::FrenetFrame; -using GeodesicInfo = WrapObstacle::Impl::PosInfo; -using LocalGeodesicInfo = WrapObstacle::Impl::Surface::LocalGeodesicInfo; -using GeodesicInitialConditions = WrapObstacle::Impl::Surface::GeodesicInitialConditions; +using GeodesicInfo = CurveSegment::Impl::PosInfo; +using LocalGeodesicInfo = CurveSegment::Impl::LocalGeodesic::LocalGeodesicInfo; +using GeodesicInitialConditions = CurveSegment::Impl::LocalGeodesic::GeodesicInitialConditions; using GeodesicJacobian = Vec4; using PointVariation = ContactGeometry::GeodesicPointVariation; using Variation = ContactGeometry::GeodesicVariation; -using LineSegment = WrappingPath::LineSegment; -using Status = WrapObstacle::Impl::Status; +using LineSegment = CableSpan::LineSegment; +using Status = CurveSegment::Impl::Status; //============================================================================== // CONSTANTS @@ -179,7 +179,7 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateAtTouchdown(Vec3 prev //============================================================================== // SURFACE IMPL //============================================================================== -using Surface = WrapObstacle::Impl::Surface; +using Surface = CurveSegment::Impl::LocalGeodesic; //------------------------------------------------------------------------------ // REALIZE CACHE @@ -313,8 +313,8 @@ void Surface::calcStatus(const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& c // OBSTACLE //============================================================================== -WrapObstacle::Impl::Impl( - WrappingPath path, +CurveSegment::Impl::Impl( + CableSpan path, const MobilizedBody& mobod, const Transform& X_BS, ContactGeometry geometry, @@ -330,14 +330,14 @@ WrapObstacle::Impl::Impl( throw std::runtime_error("NOTYETIMPLEMENTED: Must adopt to path, and give the index"); } -void WrapObstacle::Impl::realizeTopology(State &s) +void CurveSegment::Impl::realizeTopology(State &s) { // Allocate position level cache. PosInfo posInfo {}; m_PosInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Position, new Value(posInfo)); } -void WrapObstacle::Impl::realizePosition(const State &s) const +void CurveSegment::Impl::realizePosition(const State &s) const { if (!m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { calcPosInfo(s, updPosInfo(s)); @@ -345,7 +345,7 @@ void WrapObstacle::Impl::realizePosition(const State &s) const } } -void WrapObstacle::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) const +void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) const { // Get tramsform from local surface frame to ground. Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); @@ -359,7 +359,7 @@ void WrapObstacle::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) cons m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } -void WrapObstacle::Impl::applyGeodesicCorrection(const State& s, const WrapObstacle::Impl::Correction& c) const +void CurveSegment::Impl::applyGeodesicCorrection(const State& s, const CurveSegment::Impl::Correction& c) const { // Apply correction to curve. m_Surface.applyGeodesicCorrection(s, c); @@ -368,7 +368,7 @@ void WrapObstacle::Impl::applyGeodesicCorrection(const State& s, const WrapObsta m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } -size_t WrapObstacle::Impl::calcPathPoints(const State& s, std::vector& points) const +size_t CurveSegment::Impl::calcPathPoints(const State& s, std::vector& points) const { const Transform& X_GS = getPosInfo(s).X_GS; size_t n = m_Surface.calcPathPoints(s, points); @@ -378,7 +378,7 @@ size_t WrapObstacle::Impl::calcPathPoints(const State& s, std::vector& poi return n; } -void WrapObstacle::Impl::calcPosInfo( +void CurveSegment::Impl::calcPosInfo( const State& s, PosInfo& posInfo) const { @@ -434,9 +434,9 @@ void WrapObstacle::Impl::calcPosInfo( namespace { -const WrapObstacle* FindPrevActiveObstacle( +const CurveSegment* FindPrevActiveObstacle( const State& s, - const std::vector& obs, + const std::vector& obs, size_t idx) { for (ptrdiff_t i = idx - 1; i > 0; --i) { @@ -448,9 +448,9 @@ const WrapObstacle* FindPrevActiveObstacle( return nullptr; } -const WrapObstacle* FindNextActiveObstacle( +const CurveSegment* FindNextActiveObstacle( const State& s, - const std::vector& obs, + const std::vector& obs, size_t idx) { // Find the active segment after the current. @@ -508,7 +508,7 @@ GeodesicJacobian addPathErrorJacobian( // PATH IMPL //============================================================================== -void WrappingPath::Impl::realizeTopology(State &s) +void CableSpan::Impl::realizeTopology(State &s) { PosInfo posInfo {}; m_PosInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Position, new Value(posInfo)); @@ -517,47 +517,47 @@ void WrappingPath::Impl::realizeTopology(State &s) m_VelInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Velocity, new Value(velInfo)); } -void WrappingPath::Impl::realizePosition(const State &s) const +void CableSpan::Impl::realizePosition(const State &s) const { if (m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) {return;} calcPosInfo(s, updPosInfo(s)); m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); } -void WrappingPath::Impl::realizeVelocity(const State &s) const +void CableSpan::Impl::realizeVelocity(const State &s) const { if (m_Subsystem.isCacheValueRealized(s, m_VelInfoIx)) {return;} calcVelInfo(s, updVelInfo(s)); m_Subsystem.markCacheValueRealized(s, m_VelInfoIx); } -const WrappingPath::Impl::PosInfo& WrappingPath::Impl::getPosInfo(const State &s) const +const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo(const State &s) const { realizePosition(s); return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); } -WrappingPath::Impl::PosInfo& WrappingPath::Impl::updPosInfo(const State &s) const +CableSpan::Impl::PosInfo& CableSpan::Impl::updPosInfo(const State &s) const { return Value::updDowncast(m_Subsystem.updCacheEntry(s, m_PosInfoIx)); } -const WrappingPath::Impl::VelInfo& WrappingPath::Impl::getVelInfo(const State &s) const +const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo(const State &s) const { realizeVelocity(s); return Value::downcast(m_Subsystem.getCacheEntry(s, m_VelInfoIx)); } -WrappingPath::Impl::VelInfo& WrappingPath::Impl::updVelInfo(const State &s) const +CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State &s) const { return Value::updDowncast(m_Subsystem.updCacheEntry(s, m_VelInfoIx)); } -void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& s) const +void CableSpan::Impl::calcInitZeroLengthGeodesic(State& s) const { Vec3 prev_QG = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); size_t i = 0; - for (const WrapObstacle& obstacle: m_Obstacles) { + for (const CurveSegment& obstacle: m_CurveSegments) { if (obstacle.getImpl().getStatus(s) == Status::Disabled) { continue; } @@ -568,9 +568,9 @@ void WrappingPath::Impl::calcInitZeroLengthGeodesic(State& s) const } template -void WrappingPath::Impl::calcPathErrorVector( +void CableSpan::Impl::calcPathErrorVector( const State& s, - const std::vector& obs, + const std::vector& obs, const std::vector& lines, std::array axes, Vector& pathError) @@ -578,7 +578,7 @@ void WrappingPath::Impl::calcPathErrorVector( size_t lineIx = 0; ptrdiff_t row = -1; - for (const WrapObstacle& o : obs) { + for (const CurveSegment& o : obs) { if (!o.getImpl().isActive(s)) { continue; } @@ -595,9 +595,9 @@ void WrappingPath::Impl::calcPathErrorVector( } template -void WrappingPath::Impl::calcPathErrorJacobian( +void CableSpan::Impl::calcPathErrorJacobian( const State& s, - const std::vector& obs, + const std::vector& obs, const std::vector& lines, std::array axes, Matrix& J) @@ -621,8 +621,8 @@ void WrappingPath::Impl::calcPathErrorJacobian( const LineSegment& l_P = lines.at(i); const LineSegment& l_Q = lines.at(i + 1); - const WrapObstacle* prev = FindPrevActiveObstacle(s, obs, i); - const WrapObstacle* next = FindNextActiveObstacle(s, obs, i); + const CurveSegment* prev = FindPrevActiveObstacle(s, obs, i); + const CurveSegment* next = FindNextActiveObstacle(s, obs, i); for (CoordinateAxis axis: axes) { const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); @@ -654,9 +654,9 @@ void WrappingPath::Impl::calcPathErrorJacobian( }; } -double WrappingPath::Impl::calcPathLength( +double CableSpan::Impl::calcPathLength( const State& s, - const std::vector& obs, + const std::vector& obs, const std::vector& lines) { double lTot = 0.; @@ -665,7 +665,7 @@ double WrappingPath::Impl::calcPathLength( lTot += line.l; } - for (const WrapObstacle& obstacle : obs) { + for (const CurveSegment& obstacle : obs) { if (!obstacle.getImpl().isActive(s)) { continue; @@ -675,17 +675,17 @@ double WrappingPath::Impl::calcPathLength( return lTot; } -void WrappingPath::Impl::calcLineSegments( +void CableSpan::Impl::calcLineSegments( const State& s, Vec3 p_O, Vec3 p_I, std::vector& lines) const { - lines.resize(m_Obstacles.size() + 1); + lines.resize(m_CurveSegments.size() + 1); lines.clear(); Vec3 lineStart = std::move(p_O); - for (const WrapObstacle& o : m_Obstacles) { + for (const CurveSegment& o : m_CurveSegments) { if (!o.getImpl().isActive(s)) { continue; } @@ -699,30 +699,30 @@ void WrappingPath::Impl::calcLineSegments( lines.emplace_back(lineStart, p_I); } -Vec3 WrappingPath::Impl::FindPrevPoint( +Vec3 CableSpan::Impl::FindPrevPoint( const State& s, const Vec3& originPoint, - const std::vector& obs, + const std::vector& obs, size_t idx) { - const WrapObstacle* prev = FindPrevActiveObstacle(s, obs, idx); + const CurveSegment* prev = FindPrevActiveObstacle(s, obs, idx); return prev ? prev->getImpl().getPosInfo(s).KQ.p() : originPoint; } -Vec3 WrappingPath::Impl::FindNextPoint( +Vec3 CableSpan::Impl::FindNextPoint( const State& s, const Vec3& terminationPoint, - const std::vector& obs, + const std::vector& obs, size_t idx) { - const WrapObstacle* next = FindNextActiveObstacle(s, obs, idx); + const CurveSegment* next = FindNextActiveObstacle(s, obs, idx); return next ? next->getImpl().getPosInfo(s).KP.p() : terminationPoint; } -size_t WrappingPath::Impl::countActive(const State& s) const +size_t CableSpan::Impl::countActive(const State& s) const { size_t count = 0; - for (const WrapObstacle& o : m_Obstacles) { + for (const CurveSegment& o : m_CurveSegments) { if (o.getImpl().isActive(s)) { ++count; } @@ -730,7 +730,7 @@ size_t WrappingPath::Impl::countActive(const State& s) const return count; } -void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const +void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { // Path origin and termination point. const Vec3 x_O = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); @@ -750,20 +750,20 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const calcLineSegments(s, x_O, x_I, data.lineSegments); // Evaluate path error, and stop when converged. - calcPathErrorVector<2>(s, m_Obstacles, data.lineSegments, axes, data.pathError); + calcPathErrorVector<2>(s, m_CurveSegments, data.lineSegments, axes, data.pathError); const Real maxPathError = data.pathError.normInf(); if (maxPathError < m_PathErrorBound) { return; } // Evaluate the path error jacobian. - calcPathErrorJacobian<2>(s, m_Obstacles, data.lineSegments, axes, data.pathErrorJacobian); + calcPathErrorJacobian<2>(s, m_CurveSegments, data.lineSegments, axes, data.pathErrorJacobian); // Compute path corrections. const Correction* corrIt = calcPathCorrections(data); // Apply path corrections. - for (const WrapObstacle& obstacle : m_Obstacles) { + for (const CurveSegment& obstacle : m_CurveSegments) { if (!obstacle.getImpl().isActive(s)) { continue; } @@ -775,7 +775,7 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const dataCache.unlock(); // Path has changed: invalidate each segment's cache. - for (const WrapObstacle& obstacle : m_Obstacles) { + for (const CurveSegment& obstacle : m_CurveSegments) { obstacle.getImpl().invalidatePositionLevelCache(s); } } @@ -783,15 +783,15 @@ void WrappingPath::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const throw std::runtime_error("Failed to converge"); } -void WrappingPath::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const +void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const { const PosInfo& pos = getPosInfo(s); Real lengthDot = 0.; Vec3 v_GQ = m_OriginBody.findStationVelocityInGround(s, m_OriginPoint); - const WrapObstacle* lastActive = nullptr; - for (const WrapObstacle& obstacle : m_Obstacles) { + const CurveSegment* lastActive = nullptr; + for (const CurveSegment& obstacle : m_CurveSegments) { if (!obstacle.getImpl().isActive(s)) { continue; } @@ -819,26 +819,26 @@ void WrappingPath::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const // SUBSYSTEM //============================================================================== -bool WrappingPathSubsystem::isInstanceOf(const Subsystem& s) { +bool CableSubsystem::isInstanceOf(const Subsystem& s) { return Impl::isA(s.getSubsystemGuts()); } -const WrappingPathSubsystem& WrappingPathSubsystem:: +const CableSubsystem& CableSubsystem:: downcast(const Subsystem& s) { assert(isInstanceOf(s)); - return static_cast(s); + return static_cast(s); } -WrappingPathSubsystem& WrappingPathSubsystem:: +CableSubsystem& CableSubsystem:: updDowncast(Subsystem& s) { assert(isInstanceOf(s)); - return static_cast(s); + return static_cast(s); } -const WrappingPathSubsystem::Impl& WrappingPathSubsystem:: +const CableSubsystem::Impl& CableSubsystem:: getImpl() const { return SimTK_DYNAMIC_CAST_DEBUG(getSubsystemGuts()); } -WrappingPathSubsystem::Impl& WrappingPathSubsystem:: +CableSubsystem::Impl& CableSubsystem:: updImpl() { return SimTK_DYNAMIC_CAST_DEBUG(updSubsystemGuts()); } @@ -846,20 +846,20 @@ updImpl() { // 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. -WrappingPathSubsystem::WrappingPathSubsystem() +CableSubsystem::CableSubsystem() { adoptSubsystemGuts(new Impl()); } -WrappingPathSubsystem::WrappingPathSubsystem(MultibodySystem& mbs) +CableSubsystem::CableSubsystem(MultibodySystem& mbs) { adoptSubsystemGuts(new Impl()); mbs.adoptSubsystem(*this); } // steal ownership -int WrappingPathSubsystem::getNumPaths() const +int CableSubsystem::getNumPaths() const { return getImpl().getNumPaths(); } -const WrappingPath& WrappingPathSubsystem:: +const CableSpan& CableSubsystem:: getPath(WrappingPathIndex cableIx) const { return getImpl().getCablePath(cableIx); } -WrappingPath& WrappingPathSubsystem:: +CableSpan& CableSubsystem:: updPath(WrappingPathIndex cableIx) { return updImpl().updCablePath(cableIx); } diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 91f478672..a62cec663 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -19,27 +19,26 @@ SimTK_DEFINE_UNIQUE_INDEX_TYPE(WrappingPathIndex); SimTK_DEFINE_UNIQUE_INDEX_TYPE(WrapObstacleIndex); class MultibodySystem; -class WrappingPathSubsystem; -class WrapObstacle; -class WrappingPath; +class CableSubsystem; +class CurveSegment; +class CableSpan; //============================================================================== -// OBSTACLE +// CURVE SEGMENT //============================================================================== -// Although cheap to copy, we cannot hand them out because they have a cache entry associated with them. -// The surface they hold a pointer to can be reused in the model. -class SimTK_SIMBODY_EXPORT WrapObstacle +// The total cable path/span consists of LineSegments and CurveSegments +class SimTK_SIMBODY_EXPORT CurveSegment { public: - WrapObstacle() = default; - WrapObstacle(const WrapObstacle&) = delete; - WrapObstacle& operator = (const WrapObstacle&) = delete; - WrapObstacle(WrapObstacle&&) noexcept = default; - WrapObstacle& operator = (WrapObstacle&&) noexcept = default; - ~WrapObstacle() = default; + CurveSegment() = default; + CurveSegment(const CurveSegment&) = delete; + CurveSegment& operator = (const CurveSegment&) = delete; + CurveSegment(CurveSegment&&) noexcept = default; + CurveSegment& operator = (CurveSegment&&) noexcept = default; + ~CurveSegment() = default; - WrapObstacle(const MobilizedBody& mobod, Transform X_BS, const ContactGeometry& geometry, Vec3 xHint); - bool isActive(const State& state) const; + CurveSegment(const MobilizedBody& mobod, Transform X_BS, const ContactGeometry& geometry, Vec3 xHint); + /* bool isActive(const State& state) const; */ class Impl; @@ -51,9 +50,9 @@ class SimTK_SIMBODY_EXPORT WrapObstacle }; private: - explicit WrapObstacle(std::unique_ptr impl); + explicit CurveSegment(std::unique_ptr impl); - friend WrappingPath; + friend CableSpan; const Impl& getImpl() const; Impl& updImpl(); @@ -61,9 +60,10 @@ class SimTK_SIMBODY_EXPORT WrapObstacle }; //============================================================================== -// PATH +// PATH - or SPAN //============================================================================== -class SimTK_SIMBODY_EXPORT WrappingPath +// Cable, wire, rope, cord +class SimTK_SIMBODY_EXPORT CableSpan { public: struct LineSegment @@ -73,50 +73,52 @@ class SimTK_SIMBODY_EXPORT WrappingPath }; public: - WrappingPath( - WrappingPathSubsystem& subsystem, + CableSpan( + CableSubsystem& subsystem, const MobilizedBody& originBody, const Vec3& defaultOriginPoint, const MobilizedBody& terminationBody, const Vec3& defaultTerminationPoint); - std::vector& updObstacles(); - const std::vector& getObstacles(); + std::vector& updObstacles(); + const std::vector& getObstacles(); Real getLength(const State& state) const; + Real getLengthDot(const State& state) const; class Impl; private: - explicit WrappingPath(std::unique_ptr impl); + explicit CableSpan(std::unique_ptr impl); const Impl& getImpl() const { return *impl; } Impl& updImpl() { return *impl; } std::shared_ptr impl = nullptr; - friend WrapObstacle::Impl; - friend WrappingPathSubsystem; + friend CurveSegment::Impl; + friend CableSubsystem; }; //============================================================================== // SUBSYSTEM //============================================================================== -class SimTK_SIMBODY_EXPORT WrappingPathSubsystem : public Subsystem +// Rename to CableTrackerSubSystem? WrappingPathSubsystem? +class SimTK_SIMBODY_EXPORT CableSubsystem : public Subsystem { public: - WrappingPathSubsystem(); - explicit WrappingPathSubsystem(MultibodySystem&); + CableSubsystem(); + explicit CableSubsystem(MultibodySystem&); int getNumPaths() const; - const WrappingPath& getPath(WrappingPathIndex idx) const; - WrappingPath& updPath(WrappingPathIndex idx); + const CableSpan& getPath(WrappingPathIndex idx) const; + CableSpan& updPath(WrappingPathIndex idx); size_t writePathPoints(std::vector& points) const; size_t writePathFrames(std::vector& frenetFrames) const; /* private: */ - SimTK_PIMPL_DOWNCAST(WrappingPathSubsystem, Subsystem); + SimTK_PIMPL_DOWNCAST(CableSubsystem, Subsystem); class Impl; Impl& updImpl(); const Impl& getImpl() const; diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index d6a5a2785..bf1475b1d 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -16,13 +16,13 @@ namespace SimTK { -SimTK_DEFINE_UNIQUE_INDEX_TYPE(PathSegmentIndex); +SimTK_DEFINE_UNIQUE_INDEX_TYPE(CurveSegmentIndex); SimTK_DEFINE_UNIQUE_INDEX_TYPE(PathIndex); //============================================================================== -// OBSTACLE IMPL +// ??? IMPL //============================================================================== -class WrapObstacle::Impl +class CurveSegment::Impl { using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; @@ -37,13 +37,15 @@ class WrapObstacle::Impl ~Impl() = default; Impl( - WrappingPath path, + CableSpan cable, const MobilizedBody& mobod, const Transform& X_BS, ContactGeometry geometry, Vec3 initPointGuess ); + // The status of this curve segment in relation to the surface it wraps + // over. enum class Status { Ok, @@ -52,28 +54,28 @@ class WrapObstacle::Impl }; //============================================================================== - // SURFACE + // ??? //============================================================================== // Represents the local surface wrapping problem. // Caches last computed geodesic as a warmstart. // Not exposed outside of simbody. - // Not shared amongst different paths or obstacles. - class Surface + // Not shared amongst different paths/spans or obstacles/CurveSegments. + class LocalGeodesic { using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; public: - Surface() = default; - ~Surface() = default; - Surface(Surface&&) noexcept = default; - Surface& operator=(Surface&&) noexcept = default; - Surface(const Surface&) = delete; - Surface& operator=(const Surface&) = delete; - - Surface( - WrappingPathSubsystem subsystem, + LocalGeodesic() = default; + ~LocalGeodesic() = default; + LocalGeodesic(LocalGeodesic&&) noexcept = default; + LocalGeodesic& operator=(LocalGeodesic&&) noexcept = default; + LocalGeodesic(const LocalGeodesic&) = delete; + LocalGeodesic& operator=(const LocalGeodesic&) = delete; + + LocalGeodesic( + CableSubsystem subsystem, ContactGeometry geometry, Vec3 initPointGuess ) : @@ -86,6 +88,7 @@ class WrapObstacle::Impl void realizeTopology(State& state); void realizePosition(const State& state) const; + // Some info that can be retrieved from cache. struct LocalGeodesicInfo { FrenetFrame KP {}; @@ -99,6 +102,7 @@ class WrapObstacle::Impl Status status = Status::Ok; }; + // Helper struct: Required data for shooting a new geodesic. struct GeodesicInitialConditions { private: @@ -116,13 +120,11 @@ class WrapObstacle::Impl }; const LocalGeodesicInfo& calcInitialGeodesic(State& s, const GeodesicInitialConditions& g0) const; - // TODO weird name... - const LocalGeodesicInfo& calcLocalGeodesic(const State& s, Vec3 prev_QS, Vec3 next_PS) const; + const LocalGeodesicInfo& calcLocalGeodesic(const State& s, Vec3 prev_QS, Vec3 next_PS) const; // TODO weird name void applyGeodesicCorrection(const State& s, const Correction& c) const; - - // 4. calcPathPoints size_t calcPathPoints(const State& state, std::vector& points) const; + // The user defined point that controls the initial wrapping path. void setInitialPointGuess(Vec3 initPointGuess) {m_InitPointGuess = initPointGuess;} Vec3 getInitialPointGuess() const {return m_InitPointGuess;} @@ -130,10 +132,13 @@ class WrapObstacle::Impl private: + // The cache entry: Curve in local surface coordinated. + // This is an auto update discrete cache variable, which makes it persist over integration steps. + // TODO or should it be "normal" discrete? struct CacheEntry : LocalGeodesicInfo { Vec3 trackingPointOnLine {NaN, NaN, NaN}; - std::vector points {}; + std::vector points {}; // Empty for analytic geoemetry with no allocation overhead. double sHint = NaN; }; @@ -169,7 +174,7 @@ class WrapObstacle::Impl void calcGeodesic(const GeodesicInitialConditions& g0, CacheEntry& cache) const; //------------------------------------------------------------------------------ - WrappingPathSubsystem m_Subsystem; + CableSubsystem m_Subsystem; ContactGeometry m_Geometry; @@ -181,7 +186,7 @@ class WrapObstacle::Impl DiscreteVariableIndex m_CacheIx; }; - // Ground frame solution. + // Position level cache: Curve in ground frame. struct PosInfo { Transform X_GS {}; @@ -229,7 +234,7 @@ class WrapObstacle::Impl /* v_GP = m_Mobod.findStationVelocityInGround(state, P_B); */ // Get body kinematics in ground frame. - const Vec3 x_BG = m_Mobod.getBodyOriginLocation(s); + const Vec3& x_BG = m_Mobod.getBodyOriginLocation(s); const Vec3& w_BG = m_Mobod.getBodyAngularVelocity(s); const Vec3& v_BG = m_Mobod.getBodyOriginVelocity(s); @@ -255,14 +260,14 @@ class WrapObstacle::Impl void calcPosInfo(const State& state, PosInfo& posInfo) const; // TODO Required for accessing the cache variable? - WrappingPathSubsystem m_Subsystem; // The subsystem this segment belongs to. - WrappingPath m_Path; // The path this segment belongs to. - PathSegmentIndex m_PathSegmentIx; // The index in its path. + CableSubsystem m_Subsystem; // The subsystem this segment belongs to. + CableSpan m_Path; // The path this segment belongs to. + CurveSegmentIndex m_PathSegmentIx; // The index in its path. MobilizedBody m_Mobod; Transform m_Offset; - Surface m_Surface; + LocalGeodesic m_Surface; // TOPOLOGY CACHE CacheEntryIndex m_PosInfoIx; @@ -271,10 +276,14 @@ class WrapObstacle::Impl //============================================================================== // PATH :: IMPL //============================================================================== -class WrappingPath::Impl { +// TODO +// - handout CurveSegment index +// - add other cache variables: velocity, acceleration, force +// - names: isValid, ContactStationOnBody, Entry (not info/cache) +class CableSpan::Impl { public: Impl( - WrappingPathSubsystem subsystem, + CableSubsystem subsystem, MobilizedBody originBody, Vec3 originPoint, MobilizedBody terminationBody, @@ -285,6 +294,7 @@ class WrappingPath::Impl { m_TerminationBody(terminationBody), m_TerminationPoint(terminationPoint) {} + // Position level cache entry. struct PosInfo { Vec3 xO {NaN, NaN, NaN}; @@ -297,14 +307,12 @@ class WrappingPath::Impl { // TODO no matrices here? }; + // Velocity level cache entry. struct VelInfo { Real lengthDot = NaN; }; - /* std::vector& updObstacles() {return m_Obstacles;} */ - /* const std::vector& getObstacles() const {return m_Obstacles;} */ - // Allocate state variables and cache entries. void realizeTopology(State& state); void realizePosition(const State& state) const; @@ -329,31 +337,31 @@ class WrappingPath::Impl { Vec3 findPrevPoint( const State& state, - PathSegmentIndex idx) const; + CurveSegmentIndex idx) const; Vec3 findNextPoint( const State& state, - PathSegmentIndex idx) const; + CurveSegmentIndex idx) const; static Vec3 FindPrevPoint( const State& state, const Vec3& originPoint, - const std::vector& obs, + const std::vector& obs, size_t idx); static Vec3 FindNextPoint( const State& state, const Vec3& terminationPoint, - const std::vector& obs, + const std::vector& obs, size_t idx); - WrapObstacle* findPrevActiveObstacle(const State& s, size_t obsIdx); - WrapObstacle* findNextActiveObstacle(const State& s, size_t obsIdx); + CurveSegment* findPrevActiveCurveSegment(const State& s, size_t obsIdx); + CurveSegment* findNextActiveCurveSegment(const State& s, size_t obsIdx); template static void calcPathErrorVector( const State& state, - const std::vector& obs, + const std::vector& obs, const std::vector& lines, std::array axes, Vector& pathError); @@ -361,7 +369,7 @@ class WrappingPath::Impl { template static void calcPathErrorJacobian( const State& state, - const std::vector& obs, + const std::vector& obs, const std::vector& lines, std::array axes, Matrix& J); @@ -375,12 +383,13 @@ class WrappingPath::Impl { static double calcPathLength( const State& state, - const std::vector& obs, + const std::vector& obs, const std::vector& lines); - const WrappingPathSubsystem& getSubsystem() const {return m_Subsystem;} + const CableSubsystem& getSubsystem() const {return m_Subsystem;} - WrappingPathSubsystem m_Subsystem; + // Reference back to the subsystem. + CableSubsystem m_Subsystem; MobilizedBody m_OriginBody; Vec3 m_OriginPoint; @@ -388,7 +397,7 @@ class WrappingPath::Impl { MobilizedBody m_TerminationBody; Vec3 m_TerminationPoint; - std::vector m_Obstacles {}; + std::vector m_CurveSegments {}; Real m_PathErrorBound = 0.1; Real m_ObsErrorBound = 0.1; @@ -398,33 +407,32 @@ class WrappingPath::Impl { // TOPOLOGY CACHE (set during realizeTopology()) CacheEntryIndex m_PosInfoIx; CacheEntryIndex m_VelInfoIx; - CacheEntryIndex m_VizInfoIx; - friend WrapObstacle::Impl; + friend CurveSegment::Impl; }; //============================================================================== // SUBSYSTEM :: IMPL //============================================================================== -class WrappingPathSubsystem::Impl : public Subsystem::Guts { +class CableSubsystem::Impl : public Subsystem::Guts { public: Impl() {} ~Impl() {} Impl* cloneImpl() const override { return new Impl(*this); } - int getNumPaths() const {return paths.size();} + int getNumPaths() const {return cables.size();} - const WrappingPath& getCablePath(WrappingPathIndex index) const - { return paths[index]; } + const CableSpan& getCablePath(WrappingPathIndex index) const + { return cables[index]; } - WrappingPath& updCablePath(WrappingPathIndex index) - { return paths[index]; } + CableSpan& updCablePath(WrappingPathIndex index) + { return cables[index]; } // Add a cable path to the list, bumping the reference count. - WrappingPathIndex adoptCablePath(WrappingPath& path) { - paths.push_back(path); - return WrappingPathIndex(paths.size()-1); + WrappingPathIndex adoptCablePath(CableSpan& path) { + cables.push_back(path); + return WrappingPathIndex(cables.size()-1); } // Return the MultibodySystem which owns this WrappingPathSubsystem. @@ -442,8 +450,8 @@ class WrappingPathSubsystem::Impl : public Subsystem::Guts { // Topology cache is const. Impl* wThis = const_cast(this); - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - WrappingPath& path = wThis->updCablePath(ix); + for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + CableSpan& path = wThis->updCablePath(ix); path.updImpl().realizeTopology(state); } @@ -451,35 +459,26 @@ class WrappingPathSubsystem::Impl : public Subsystem::Guts { } int realizeSubsystemPositionImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - const WrappingPath& path = getCablePath(ix); + for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + const CableSpan& path = getCablePath(ix); path.getImpl().realizePosition(state); } return 0; } int realizeSubsystemVelocityImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - const WrappingPath& path = getCablePath(ix); + for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + const CableSpan& path = getCablePath(ix); path.getImpl().realizeVelocity(state); } return 0; } - - int realizeSubsystemAccelerationImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < paths.size(); ++ix) { - const WrappingPath& path = getCablePath(ix); - path.getImpl().realizeAcceleration(state); - } - return 0; - } - SimTK_DOWNCAST(Impl, Subsystem::Guts); private: // TOPOLOGY STATE - Array_ paths; + Array_ cables; }; } // namespace SimTK From 4bac884c42f80bf2014a97b73b3acc8c6e123b98 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 24 Apr 2024 10:50:17 +0200 Subject: [PATCH 027/127] wip: refactor to use CurveSegmentIndex and more.. --- Simbody/include/simbody/internal/Wrapping.cpp | 167 +++++++++--------- Simbody/include/simbody/internal/Wrapping.h | 22 ++- .../include/simbody/internal/WrappingImpl.h | 64 ++++--- 3 files changed, 142 insertions(+), 111 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 05b2b84fc..346e56706 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -315,6 +315,7 @@ void Surface::calcStatus(const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& c CurveSegment::Impl::Impl( CableSpan path, + CurveSegmentIndex ix, const MobilizedBody& mobod, const Transform& X_BS, ContactGeometry geometry, @@ -323,12 +324,11 @@ CurveSegment::Impl::Impl( : m_Subsystem(path.getImpl().getSubsystem()), m_Path(path), + m_Index(ix), m_Mobod(mobod), m_Offset(X_BS), m_Surface(m_Subsystem, geometry, initPointGuess) -{ - throw std::runtime_error("NOTYETIMPLEMENTED: Must adopt to path, and give the index"); -} +{} void CurveSegment::Impl::realizeTopology(State &s) { @@ -391,8 +391,8 @@ void CurveSegment::Impl::calcPosInfo( const Transform& X_GS = posInfo.X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); // Get the path points before and after this segment. - const Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_PathSegmentIx); - const Vec3 next_G = m_Path.getImpl().findNextPoint(s, m_PathSegmentIx); + const Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_Index); + const Vec3 next_G = m_Path.getImpl().findNextPoint(s, m_Index); // Transform the prev and next path points to the surface frame. const Vec3 prev_S = X_GS.shiftBaseStationToFrame(prev_G); @@ -434,34 +434,6 @@ void CurveSegment::Impl::calcPosInfo( namespace { -const CurveSegment* FindPrevActiveObstacle( - const State& s, - const std::vector& obs, - size_t idx) -{ - for (ptrdiff_t i = idx - 1; i > 0; --i) { - // Find the active segment before the current. - if (obs.at(i).isActive(s)) { - return &obs.at(i); - } - } - return nullptr; -} - -const CurveSegment* FindNextActiveObstacle( - const State& s, - const std::vector& obs, - size_t idx) -{ - // Find the active segment after the current. - for (ptrdiff_t i = idx + 1; i < obs.size(); ++i) { - if (obs.at(i).isActive(s)) { - return &obs.at(i); - } - } - return nullptr; -} - static const int N_PATH_CONSTRAINTS = 4; // TODO this is awkward @@ -567,23 +539,69 @@ void CableSpan::Impl::calcInitZeroLengthGeodesic(State& s) const } } +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().isActive(s)) { + 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().isActive(s)) { + 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().getPosInfo(s).KQ.p() + : m_OriginBody.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().getPosInfo(s).KP.p() + : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase(m_TerminationPoint); +} + template void CableSpan::Impl::calcPathErrorVector( const State& s, - const std::vector& obs, const std::vector& lines, std::array axes, - Vector& pathError) + Vector& pathError) const { size_t lineIx = 0; ptrdiff_t row = -1; - for (const CurveSegment& o : obs) { - if (!o.getImpl().isActive(s)) { + for (int i = 0; i < getNumCurveSegments(); ++i) { + const CurveSegment& segment = getCurveSegment(CurveSegmentIndex(i)); + if (!segment.getImpl().isActive(s)) { continue; } - const GeodesicInfo& g = o.getImpl().getPosInfo(s); + const GeodesicInfo& g = segment.getImpl().getPosInfo(s); for (CoordinateAxis axis: axes) { pathError(++row) = calcPathError(lines.at(lineIx), g.KP.R(), axis); } @@ -597,32 +615,33 @@ void CableSpan::Impl::calcPathErrorVector( template void CableSpan::Impl::calcPathErrorJacobian( const State& s, - const std::vector& obs, const std::vector& lines, std::array axes, - Matrix& J) + Matrix& J) const { constexpr size_t Nq = GeodesicDOF; // TODO perhaps just not make method static. - const size_t n = lines.size() - 1; + const size_t n = lines.size() - 1; 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; - for (size_t i = 0; i < n; ++i) { - if (!obs.at(i).getImpl().isActive(s)) { + size_t activeIx = 0; + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().isActive(s)) { continue; } - const GeodesicInfo& g = obs.at(i).getImpl().getPosInfo(s); + const GeodesicInfo& g = segment.getImpl().getPosInfo(s); - const LineSegment& l_P = lines.at(i); - const LineSegment& l_Q = lines.at(i + 1); + const LineSegment& l_P = lines.at(activeIx); + const LineSegment& l_Q = lines.at(activeIx + 1); - const CurveSegment* prev = FindPrevActiveObstacle(s, obs, i); - const CurveSegment* next = FindNextActiveObstacle(s, obs, i); + const CurveSegmentIndex ix = segment.getImpl().getIndex(); + const CurveSegment* prev = findPrevActiveCurveSegment(s, ix); + const CurveSegment* next = findNextActiveCurveSegment(s, ix); for (CoordinateAxis axis: axes) { const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); @@ -656,8 +675,7 @@ void CableSpan::Impl::calcPathErrorJacobian( double CableSpan::Impl::calcPathLength( const State& s, - const std::vector& obs, - const std::vector& lines) + const std::vector& lines) const { double lTot = 0.; for (const LineSegment& line : lines) { @@ -665,12 +683,12 @@ double CableSpan::Impl::calcPathLength( lTot += line.l; } - for (const CurveSegment& obstacle : obs) { - if (!obstacle.getImpl().isActive(s)) + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().isActive(s)) { continue; } - lTot += obstacle.getImpl().getPosInfo(s).length; + lTot += segment.getImpl().getPosInfo(s).length; } return lTot; } @@ -685,12 +703,12 @@ void CableSpan::Impl::calcLineSegments( lines.clear(); Vec3 lineStart = std::move(p_O); - for (const CurveSegment& o : m_CurveSegments) { - if (!o.getImpl().isActive(s)) { + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().isActive(s)) { continue; } - const GeodesicInfo& g = o.getImpl().getPosInfo(s); + const GeodesicInfo& g = segment.getImpl().getPosInfo(s); const Vec3 lineEnd = g.KP.p(); lines.emplace_back(lineStart, lineEnd); @@ -699,31 +717,11 @@ void CableSpan::Impl::calcLineSegments( lines.emplace_back(lineStart, p_I); } -Vec3 CableSpan::Impl::FindPrevPoint( - const State& s, - const Vec3& originPoint, - const std::vector& obs, - size_t idx) -{ - const CurveSegment* prev = FindPrevActiveObstacle(s, obs, idx); - return prev ? prev->getImpl().getPosInfo(s).KQ.p() : originPoint; -} - -Vec3 CableSpan::Impl::FindNextPoint( - const State& s, - const Vec3& terminationPoint, - const std::vector& obs, - size_t idx) -{ - const CurveSegment* next = FindNextActiveObstacle(s, obs, idx); - return next ? next->getImpl().getPosInfo(s).KP.p() : terminationPoint; -} - size_t CableSpan::Impl::countActive(const State& s) const { size_t count = 0; - for (const CurveSegment& o : m_CurveSegments) { - if (o.getImpl().isActive(s)) { + for (const CurveSegment& segment : m_CurveSegments) { + if (segment.getImpl().isActive(s)) { ++count; } } @@ -750,14 +748,14 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const calcLineSegments(s, x_O, x_I, data.lineSegments); // Evaluate path error, and stop when converged. - calcPathErrorVector<2>(s, m_CurveSegments, data.lineSegments, axes, data.pathError); + calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); const Real maxPathError = data.pathError.normInf(); if (maxPathError < m_PathErrorBound) { return; } // Evaluate the path error jacobian. - calcPathErrorJacobian<2>(s, m_CurveSegments, data.lineSegments, axes, data.pathErrorJacobian); + calcPathErrorJacobian<2>(s, data.lineSegments, axes, data.pathErrorJacobian); // Compute path corrections. const Correction* corrIt = calcPathCorrections(data); @@ -810,10 +808,17 @@ void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const } const Vec3 v_GP = m_TerminationBody.findStationVelocityInGround(s, m_TerminationPoint); - const UnitVec3 e_G = lastActive? lastActive->getImpl().getPosInfo(s).KQ.R().getAxisUnitVec(TangentAxis) : UnitVec3(pos.xO - pos.xI); + 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& state, + Real tension, + Vector_& bodyForcesInG) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} //============================================================================== // SUBSYSTEM diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index a62cec663..45fa775a6 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -38,7 +38,6 @@ class SimTK_SIMBODY_EXPORT CurveSegment ~CurveSegment() = default; CurveSegment(const MobilizedBody& mobod, Transform X_BS, const ContactGeometry& geometry, Vec3 xHint); - /* bool isActive(const State& state) const; */ class Impl; @@ -49,6 +48,19 @@ class SimTK_SIMBODY_EXPORT CurveSegment Disabled, }; + Real getSegmentLength(const State& s); + /* { */ + /* getImpl().getSubsystem().realizePosition(s); */ + /* return getImpl().getLength(s); */ + /* } */ + + Status getStatus(const State& state) const; + void setDisabled(const State& state) const; + void setEnabled(const State& state) const; + + int calcPathPoints(const State& state, std::vector& points); + int calcPathFrenetFrames(const State& state, std::vector& frames); + private: explicit CurveSegment(std::unique_ptr impl); @@ -86,6 +98,14 @@ class SimTK_SIMBODY_EXPORT CableSpan Real getLength(const State& state) const; Real getLengthDot(const State& state) const; + void applyBodyForces( + const State& state, + Real tension, + Vector_& bodyForcesInG) const; + + int calcPathPoints(const State& state, std::vector& points); + int calcPathFrenetFrames(const State& state, std::vector& frames); + class Impl; private: explicit CableSpan(std::unique_ptr impl); diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index bf1475b1d..a0c115f25 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -38,6 +38,7 @@ class CurveSegment::Impl Impl( CableSpan cable, + CurveSegmentIndex ix, const MobilizedBody& mobod, const Transform& X_BS, ContactGeometry geometry, @@ -199,6 +200,10 @@ class CurveSegment::Impl Real length = NaN; + // TODO add force & moment here? + /* Vec3 unitForce {}; */ + /* Vec3 unitMoment {}; */ + Status status = Status::Ok; }; @@ -215,6 +220,9 @@ class CurveSegment::Impl m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); } + CurveSegmentIndex getIndex() const {return m_Index;} + void setIndex(CurveSegmentIndex ix) {m_Index = ix;} + const PosInfo& getPosInfo(const State& s) const { realizePosition(s); return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); @@ -262,7 +270,7 @@ class CurveSegment::Impl // 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_PathSegmentIx; // The index in its path. + CurveSegmentIndex m_Index; // The index in its path. MobilizedBody m_Mobod; Transform m_Offset; @@ -294,6 +302,17 @@ class CableSpan::Impl { m_TerminationBody(terminationBody), m_TerminationPoint(terminationPoint) {} + int getNumCurveSegments() const {return m_CurveSegments.size();} + + const CurveSegment& getCurveSegment(CurveSegmentIndex ix) const + { return m_CurveSegments[ix]; } + + CurveSegmentIndex adoptObstacle(CurveSegment& segment) + { + m_CurveSegments.push_back(segment); + return CurveSegmentIndex(m_CurveSegments.size()-1); + } + // Position level cache entry. struct PosInfo { @@ -303,8 +322,6 @@ class CableSpan::Impl { Real l = NaN; size_t loopIter = 0; - - // TODO no matrices here? }; // Velocity level cache entry. @@ -325,6 +342,10 @@ class CableSpan::Impl { void calcInitZeroLengthGeodesic(State& s) const; + void applyBodyForces( + const State& state, + Real tension, + Vector_& bodyForcesInG) const; private: PosInfo& updPosInfo(const State& s) const; @@ -337,42 +358,28 @@ class CableSpan::Impl { Vec3 findPrevPoint( const State& state, - CurveSegmentIndex idx) const; + CurveSegmentIndex ix) const; Vec3 findNextPoint( const State& state, - CurveSegmentIndex idx) const; - - static Vec3 FindPrevPoint( - const State& state, - const Vec3& originPoint, - const std::vector& obs, - size_t idx); - - static Vec3 FindNextPoint( - const State& state, - const Vec3& terminationPoint, - const std::vector& obs, - size_t idx); + CurveSegmentIndex ix) const; - CurveSegment* findPrevActiveCurveSegment(const State& s, size_t obsIdx); - CurveSegment* findNextActiveCurveSegment(const State& s, size_t obsIdx); + const CurveSegment* findPrevActiveCurveSegment(const State& s, CurveSegmentIndex ix) const; + const CurveSegment* findNextActiveCurveSegment(const State& s, CurveSegmentIndex ix) const; template - static void calcPathErrorVector( + void calcPathErrorVector( const State& state, - const std::vector& obs, const std::vector& lines, std::array axes, - Vector& pathError); + Vector& pathError) const; template - static void calcPathErrorJacobian( + void calcPathErrorJacobian( const State& state, - const std::vector& obs, const std::vector& lines, std::array axes, - Matrix& J); + Matrix& J) const; // Make static or not? void calcLineSegments( @@ -381,10 +388,9 @@ class CableSpan::Impl { Vec3 p_I, std::vector& lines) const; - static double calcPathLength( + double calcPathLength( const State& state, - const std::vector& obs, - const std::vector& lines); + const std::vector& lines) const; const CableSubsystem& getSubsystem() const {return m_Subsystem;} @@ -397,7 +403,7 @@ class CableSpan::Impl { MobilizedBody m_TerminationBody; Vec3 m_TerminationPoint; - std::vector m_CurveSegments {}; + Array_ m_CurveSegments {}; Real m_PathErrorBound = 0.1; Real m_ObsErrorBound = 0.1; From 179f3b63fdd47c205d25966ffbdb842949ccd975 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 24 Apr 2024 10:53:36 +0200 Subject: [PATCH 028/127] fmt --- Simbody/include/simbody/internal/Wrapping.cpp | 739 ++++++++++-------- Simbody/include/simbody/internal/Wrapping.h | 57 +- .../include/simbody/internal/WrappingImpl.h | 540 +++++++------ 3 files changed, 773 insertions(+), 563 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 346e56706..96c5022da 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1,7 +1,8 @@ +#include "Wrapping.h" + #include "SimTKcommon/internal/CoordinateAxis.h" #include "SimTKcommon/internal/ExceptionMacros.h" #include "SimTKmath.h" -#include "Wrapping.h" #include "WrappingImpl.h" #include "simmath/internal/ContactGeometry.h" #include @@ -9,141 +10,160 @@ using namespace SimTK; -using Correction = ContactGeometry::GeodesicCorrection; -using FrameVariation = ContactGeometry::GeodesicFrameVariation; -using FrenetFrame = ContactGeometry::FrenetFrame; -using GeodesicInfo = CurveSegment::Impl::PosInfo; +using Correction = ContactGeometry::GeodesicCorrection; +using FrameVariation = ContactGeometry::GeodesicFrameVariation; +using FrenetFrame = ContactGeometry::FrenetFrame; +using GeodesicInfo = CurveSegment::Impl::PosInfo; using LocalGeodesicInfo = CurveSegment::Impl::LocalGeodesic::LocalGeodesicInfo; -using GeodesicInitialConditions = CurveSegment::Impl::LocalGeodesic::GeodesicInitialConditions; +using GeodesicInitialConditions = + CurveSegment::Impl::LocalGeodesic::GeodesicInitialConditions; using GeodesicJacobian = Vec4; -using PointVariation = ContactGeometry::GeodesicPointVariation; -using Variation = ContactGeometry::GeodesicVariation; -using LineSegment = CableSpan::LineSegment; -using Status = CurveSegment::Impl::Status; +using PointVariation = ContactGeometry::GeodesicPointVariation; +using Variation = ContactGeometry::GeodesicVariation; +using LineSegment = CableSpan::LineSegment; +using Status = CurveSegment::Impl::Status; //============================================================================== // CONSTANTS //============================================================================== -namespace { - static const CoordinateAxis TangentAxis = ContactGeometry::TangentAxis; - static const CoordinateAxis NormalAxis = ContactGeometry::NormalAxis; - static const CoordinateAxis BinormalAxis = ContactGeometry::BinormalAxis; - static const int GeodesicDOF = 4; -} +namespace +{ +static const CoordinateAxis TangentAxis = ContactGeometry::TangentAxis; +static const CoordinateAxis NormalAxis = ContactGeometry::NormalAxis; +static const CoordinateAxis BinormalAxis = ContactGeometry::BinormalAxis; +static const int GeodesicDOF = 4; +} // namespace //============================================================================== // SOLVER //============================================================================== -namespace { - class SolverDataCache; - SolverDataCache& findDataCache(size_t nActive); +namespace +{ +class SolverDataCache; +SolverDataCache& findDataCache(size_t nActive); + +struct SolverData +{ + std::vector lineSegments; + + Matrix pathErrorJacobian; + Vector pathCorrection; + Vector pathError; + Matrix mat; + // TODO Cholesky decomposition... + FactorLU matInv; + Vector vec; +}; - struct SolverData +class SolverDataCache +{ +private: + SolverDataCache(size_t n) { - std::vector lineSegments; - - Matrix pathErrorJacobian; - Vector pathCorrection; - Vector pathError; - Matrix mat; - // TODO Cholesky decomposition... - FactorLU matInv; - Vector vec; - }; + static constexpr int Q = 4; + static constexpr int C = 4; + + m_Data.lineSegments.resize(n + 1); + m_Data.pathErrorJacobian = Matrix(C * n, Q * n, 0.); + m_Data.pathCorrection = Vector(Q * n, 0.); + m_Data.pathError = Vector(C * n, 0.); + m_Data.mat = Matrix(Q * n, Q * n, NaN); + m_Data.vec = Vector(Q * n, NaN); + } - class SolverDataCache +public: + void lock() { - private: - SolverDataCache(size_t n) { - static constexpr int Q = 4; - static constexpr int C = 4; - - m_Data.lineSegments.resize(n+1); - m_Data.pathErrorJacobian = Matrix(C * n, Q * n, 0.); - m_Data.pathCorrection = Vector(Q * n, 0.); - m_Data.pathError = Vector(C * n, 0.); - m_Data.mat = Matrix(Q*n, Q*n, NaN); - m_Data.vec = Vector(Q*n, NaN); - } + m_Mutex.lock(); + } - public: - void lock() { - m_Mutex.lock(); - } + void unlock() + { + m_Mutex.unlock(); + } - void unlock() { - m_Mutex.unlock(); - } + SolverData& updData() + { + return m_Data; + } - SolverData& updData() {return m_Data;} +private: + SolverData m_Data; + std::mutex m_Mutex{}; - private: - SolverData m_Data; - std::mutex m_Mutex {}; + friend SolverDataCache& findDataCache(size_t nActive); +}; - friend SolverDataCache& findDataCache(size_t nActive); - }; +SolverDataCache& findDataCache(size_t nActive) +{ + static std::vector s_GlobalCache{}; + static std::mutex s_GlobalLock{}; - SolverDataCache& findDataCache(size_t nActive) { - static std::vector s_GlobalCache {}; - static std::mutex s_GlobalLock {}; + std::lock_guard lock(s_GlobalLock); - { - std::lock_guard lock(s_GlobalLock); - - for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { - int n = i + 1; - s_GlobalCache.emplace_back(nActive); - } + for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { + int n = i + 1; + s_GlobalCache.emplace_back(nActive); } - - return s_GlobalCache.at(nActive-1); } - const Correction* calcPathCorrections(SolverData& data) { - Real w = data.pathError.normInf(); + return s_GlobalCache.at(nActive - 1); +} - data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; - for (int i = 0; i < data.mat.nrow(); ++i) { - data.mat[i][i] += w; - } - data.matInv = data.mat; - data.vec = data.pathErrorJacobian.transpose() * data.pathError; - data.matInv.solve(data.vec, data.pathCorrection); - - static_assert( - sizeof(Correction) == sizeof(double) * GeodesicDOF, - "Invalid size of corrections vector"); - SimTK_ASSERT(data.pathCorrection.size() * sizeof(double) == n * sizeof(Correction), - "Invalid size of path corrections vector"); - return reinterpret_cast(&data.pathCorrection[0]); +const Correction* calcPathCorrections(SolverData& data) +{ + Real w = data.pathError.normInf(); + + data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; + for (int i = 0; i < data.mat.nrow(); ++i) { + data.mat[i][i] += w; } + data.matInv = data.mat; + data.vec = data.pathErrorJacobian.transpose() * data.pathError; + data.matInv.solve(data.vec, data.pathCorrection); + + static_assert( + sizeof(Correction) == sizeof(double) * GeodesicDOF, + "Invalid size of corrections vector"); + SimTK_ASSERT( + data.pathCorrection.size() * sizeof(double) == n * sizeof(Correction), + "Invalid size of path corrections vector"); + return reinterpret_cast(&data.pathCorrection[0]); +} } // namespace //============================================================================== // GEODESIC INITIAL CONDITIONS //============================================================================== -GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected(const FrenetFrame& KP, const Variation& dKP, Real l, const Correction& c) +GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected( + const FrenetFrame& KP, + const Variation& dKP, + Real l, + const Correction& c) { GeodesicInitialConditions g0; Vec3 v = dKP[1] * c; - g0.x = KP.p() + v; + g0.x = KP.p() + v; - Vec3 w = dKP[0] * c; + Vec3 w = dKP[0] * c; const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); - g0.t = t + cross(w,t); + g0.t = t + cross(w, t); Real dl = c[3]; - g0.l = l + dl; + g0.l = l + dl; return g0; } -GeodesicInitialConditions GeodesicInitialConditions::CreateInSurfaceFrame(const Transform& X_GS, Vec3 x_G, Vec3 t_G, Real l) +GeodesicInitialConditions GeodesicInitialConditions::CreateInSurfaceFrame( + const Transform& X_GS, + Vec3 x_G, + Vec3 t_G, + Real l) { GeodesicInitialConditions g0; @@ -154,7 +174,10 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateInSurfaceFrame(const return g0; } -GeodesicInitialConditions GeodesicInitialConditions::CreateZeroLengthGuess(const Transform& X_GS, Vec3 prev_QS, Vec3 xGuess_S) +GeodesicInitialConditions GeodesicInitialConditions::CreateZeroLengthGuess( + const Transform& X_GS, + Vec3 prev_QS, + Vec3 xGuess_S) { GeodesicInitialConditions g0; @@ -165,7 +188,10 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateZeroLengthGuess(const return g0; } -GeodesicInitialConditions GeodesicInitialConditions::CreateAtTouchdown(Vec3 prev_QS, Vec3 next_PS, Vec3 trackingPointOnLine) +GeodesicInitialConditions GeodesicInitialConditions::CreateAtTouchdown( + Vec3 prev_QS, + Vec3 next_PS, + Vec3 trackingPointOnLine) { GeodesicInitialConditions g0; @@ -184,25 +210,31 @@ using Surface = CurveSegment::Impl::LocalGeodesic; //------------------------------------------------------------------------------ // REALIZE CACHE //------------------------------------------------------------------------------ -void Surface::realizeTopology(State &s) +void Surface::realizeTopology(State& s) { - // Allocate an auto-update discrete variable for the last computed geodesic. - CacheEntry cache {}; - m_CacheIx = m_Subsystem.allocateAutoUpdateDiscreteVariable(s, Stage::Velocity, new Value(cache), Stage::Position); + // Allocate an auto-update discrete variable for the last computed geodesic. + CacheEntry cache{}; + m_CacheIx = m_Subsystem.allocateAutoUpdateDiscreteVariable( + s, + Stage::Velocity, + new Value(cache), + Stage::Position); } -void Surface::realizePosition(const State &s) const +void Surface::realizePosition(const State& s) const { - if (!m_Subsystem.isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { - updCacheEntry(s) = getPrevCacheEntry(s); - m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); - } + if (!m_Subsystem.isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { + updCacheEntry(s) = getPrevCacheEntry(s); + m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); + } } //------------------------------------------------------------------------------ // PUBLIC METHODS //------------------------------------------------------------------------------ -const LocalGeodesicInfo& Surface::calcInitialGeodesic(State& s, const GeodesicInitialConditions& g0) const +const LocalGeodesicInfo& Surface::calcInitialGeodesic( + State& s, + const GeodesicInitialConditions& g0) const { // TODO is this correct? CacheEntry& cache = updPrevCacheEntry(s); @@ -211,7 +243,10 @@ const LocalGeodesicInfo& Surface::calcInitialGeodesic(State& s, const GeodesicIn m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); } -const LocalGeodesicInfo& Surface::calcLocalGeodesic(const State& s, Vec3 prev_QS, Vec3 next_PS) const +const LocalGeodesicInfo& Surface::calcLocalGeodesic( + const State& s, + Vec3 prev_QS, + Vec3 next_PS) const { realizePosition(s); CacheEntry& cache = updCacheEntry(s); @@ -223,89 +258,110 @@ void Surface::applyGeodesicCorrection(const State& s, const Correction& c) const { realizePosition(s); - // Get the previous geodesic. - const CacheEntry& g = getCacheEntry(s); + // Get the previous geodesic. + const CacheEntry& g = getCacheEntry(s); // Get corrected initial conditions. - const GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateCorrected(g.KP, g.dKP, g.length, c); + const GeodesicInitialConditions g0 = + GeodesicInitialConditions::CreateCorrected(g.KP, g.dKP, g.length, c); - // Shoot the new geodesic. - calcGeodesic(g0, updCacheEntry(s)); + // Shoot the new geodesic. + calcGeodesic(g0, updCacheEntry(s)); } size_t Surface::calcPathPoints(const State& s, std::vector& points) const { realizePosition(s); - size_t count = 0; + size_t count = 0; const CacheEntry& cache = getCacheEntry(s); - for (Vec3 p: cache.points) { + for (Vec3 p : cache.points) { points.push_back(p); ++count; } - throw std::runtime_error("NOTYETIMPLEMENTED for analytic"); + throw std::runtime_error("NOTYETIMPLEMENTED for analytic"); return count; } //------------------------------------------------------------------------------ // PRIVATE METHODS //------------------------------------------------------------------------------ -void Surface::calcGeodesic(const GeodesicInitialConditions& g0, CacheEntry& cache) const +void Surface::calcGeodesic( + const GeodesicInitialConditions& g0, + CacheEntry& cache) const { - // Compute geodesic start boundary frame and variation. - m_Geometry.calcNearestFrenetFrameFast(g0.x, g0.t, cache.KP); - m_Geometry.calcGeodesicStartFrameVariation(cache.KP, cache.dKP); + // Compute geodesic start boundary frame and variation. + m_Geometry.calcNearestFrenetFrameFast(g0.x, g0.t, cache.KP); + m_Geometry.calcGeodesicStartFrameVariation(cache.KP, cache.dKP); // Compute geodesic end boundary frame amd variation (shoot new geodesic). - m_Geometry.calcGeodesicEndFrameVariationImplicitly( - cache.KP.p(), - cache.KP.R().getAxisUnitVec(ContactGeometry::TangentAxis), - g0.l, - cache.sHint, - cache.KQ, - cache.dKQ, - cache.points); - - // TODO update step size. - // TODO update line tracking? - throw std::runtime_error("NOTYETIMPLEMENTED"); + m_Geometry.calcGeodesicEndFrameVariationImplicitly( + cache.KP.p(), + cache.KP.R().getAxisUnitVec(ContactGeometry::TangentAxis), + g0.l, + cache.sHint, + cache.KQ, + cache.dKQ, + cache.points); + + // TODO update step size. + // TODO update line tracking? + throw std::runtime_error("NOTYETIMPLEMENTED"); } -void Surface::calcStatus(const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& cache) const +void Surface::calcStatus( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const { LocalGeodesicInfo& g = cache; - if (g.status == Status::Disabled) {return;} + if (g.status == Status::Disabled) { + return; + } // Make sure that the previous point does not lie inside the surface. if (m_Geometry.calcSurfaceValue(prev_QS)) { // TODO use proper assert. - throw std::runtime_error("Unable to wrap over surface: Preceding point lies inside the surface"); + throw std::runtime_error("Unable to wrap over surface: Preceding point " + "lies inside the surface"); } if (m_Geometry.calcSurfaceValue(next_PS)) { // TODO use proper assert. - throw std::runtime_error("Unable to wrap over surface: Next point lies inside the surface"); + throw std::runtime_error( + "Unable to wrap over surface: Next point lies inside the surface"); } Vec3& pTrack = cache.trackingPointOnLine; // Detect touchdown. bool detectedTouchdown = g.status == Status::Liftoff; - detectedTouchdown &= m_Geometry.calcNearestPointOnLine(prev_QS, next_PS, pTrack, m_TouchdownIter, m_TouchdownAccuracy); + detectedTouchdown &= m_Geometry.calcNearestPointOnLine( + prev_QS, + next_PS, + pTrack, + m_TouchdownIter, + m_TouchdownAccuracy); if (detectedTouchdown) { g.status = Status::Ok; - GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateAtTouchdown(prev_QS, next_PS, cache.trackingPointOnLine); + GeodesicInitialConditions g0 = + GeodesicInitialConditions::CreateAtTouchdown( + prev_QS, + next_PS, + cache.trackingPointOnLine); calcGeodesic(g0, cache); } // Detect liftoff. bool detectedLiftoff = g.status == Status::Ok; detectedLiftoff &= g.length == 0.; - detectedLiftoff &= dot(prev_QS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) > 0.; - detectedLiftoff &= dot(next_PS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) > 0.; + detectedLiftoff &= + dot(prev_QS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) > 0.; + detectedLiftoff &= + dot(next_PS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) > 0.; if (detectedLiftoff) { g.status = Status::Liftoff; - pTrack = g.KP.p(); + pTrack = g.KP.p(); } } @@ -314,81 +370,87 @@ void Surface::calcStatus(const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& c //============================================================================== CurveSegment::Impl::Impl( - CableSpan path, - CurveSegmentIndex ix, - const MobilizedBody& mobod, - const Transform& X_BS, - ContactGeometry geometry, - Vec3 initPointGuess - ) - : - m_Subsystem(path.getImpl().getSubsystem()), - m_Path(path), - m_Index(ix), - m_Mobod(mobod), - m_Offset(X_BS), - m_Surface(m_Subsystem, geometry, initPointGuess) + CableSpan path, + CurveSegmentIndex ix, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess) : + m_Subsystem(path.getImpl().getSubsystem()), + m_Path(path), m_Index(ix), m_Mobod(mobod), m_Offset(X_BS), + m_Surface(m_Subsystem, geometry, initPointGuess) {} -void CurveSegment::Impl::realizeTopology(State &s) +void CurveSegment::Impl::realizeTopology(State& s) { - // Allocate position level cache. - PosInfo posInfo {}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Position, new Value(posInfo)); + // Allocate position level cache. + PosInfo posInfo{}; + m_PosInfoIx = m_Subsystem.allocateCacheEntry( + s, + Stage::Position, + new Value(posInfo)); } -void CurveSegment::Impl::realizePosition(const State &s) const +void CurveSegment::Impl::realizePosition(const State& s) const { - if (!m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { - calcPosInfo(s, updPosInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); - } + if (!m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { + calcPosInfo(s, updPosInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); + } } -void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) const +void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) + const { - // Get tramsform from local surface frame to ground. - Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); + // Get tramsform from local surface frame to ground. + Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); - Vec3 prev_QS = X_GS.shiftBaseStationToFrame(prev_QG); - Vec3 xGuess_S = m_Surface.getInitialPointGuess(); // TODO move into function call? + Vec3 prev_QS = X_GS.shiftBaseStationToFrame(prev_QG); + Vec3 xGuess_S = + m_Surface.getInitialPointGuess(); // TODO move into function call? - GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateZeroLengthGuess(X_GS, prev_QS, xGuess_S); + GeodesicInitialConditions g0 = + GeodesicInitialConditions::CreateZeroLengthGuess( + X_GS, + prev_QS, + xGuess_S); m_Surface.calcInitialGeodesic(s, g0); - m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); + m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } -void CurveSegment::Impl::applyGeodesicCorrection(const State& s, const CurveSegment::Impl::Correction& c) const +void CurveSegment::Impl::applyGeodesicCorrection( + const State& s, + const CurveSegment::Impl::Correction& c) const { // Apply correction to curve. m_Surface.applyGeodesicCorrection(s, c); - // Invalidate position level cache. - m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); + // Invalidate position level cache. + m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } -size_t CurveSegment::Impl::calcPathPoints(const State& s, std::vector& points) const +size_t CurveSegment::Impl::calcPathPoints( + const State& s, + std::vector& points) const { const Transform& X_GS = getPosInfo(s).X_GS; - size_t n = m_Surface.calcPathPoints(s, points); + size_t n = m_Surface.calcPathPoints(s, points); for (size_t i = points.size() - n; i < points.size(); ++i) { points.at(i) = X_GS.shiftFrameStationToBase(points.at(i)); } return n; } -void CurveSegment::Impl::calcPosInfo( - const State& s, - PosInfo& posInfo) const +void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { - if(m_Surface.getStatus(s) == Status::Disabled) - { + if (m_Surface.getStatus(s) == Status::Disabled) { return; } - // Compute tramsform from local surface frame to ground. - const Transform& X_GS = posInfo.X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); + // Compute tramsform from local surface frame to ground. + const Transform& X_GS = posInfo.X_GS = + m_Mobod.getBodyTransform(s).compose(m_Offset); // Get the path points before and after this segment. const Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_Index); @@ -400,8 +462,9 @@ void CurveSegment::Impl::calcPosInfo( // Detect liftoff, touchdown and potential invalid configurations. // TODO this doesnt follow the regular invalidation scheme... - // Grab the last geodesic that was computed. - const LocalGeodesicInfo& geodesic_S = m_Surface.calcLocalGeodesic(s, prev_S, next_S); + // Grab the last geodesic that was computed. + const LocalGeodesicInfo& geodesic_S = + m_Surface.calcLocalGeodesic(s, prev_S, next_S); // Store the the local geodesic in ground frame. posInfo.KP = X_GS.compose(geodesic_S.KP); @@ -437,9 +500,7 @@ namespace static const int N_PATH_CONSTRAINTS = 4; // TODO this is awkward -void addBlock( - const Vec4& values, - Matrix& block) +void addBlock(const Vec4& values, Matrix& block) { for (int i = 0; i < Vec4::size(); ++i) { block[0][i] = values[i]; @@ -448,99 +509,117 @@ void addBlock( void addDirectionJacobian( const LineSegment& e, - const UnitVec3& axis, + const UnitVec3& axis, const PointVariation& dx, Matrix& J, bool invert = false) { - Vec3 y = axis - e.d * dot(e.d,axis); + Vec3 y = axis - e.d * dot(e.d, axis); y /= e.l * (invert ? 1. : -1); addBlock(~dx * y, J); } -double calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) +double calcPathError( + const LineSegment& e, + const Rotation& R, + CoordinateAxis axis) { return dot(e.d, R.getAxisUnitVec(axis)); } GeodesicJacobian addPathErrorJacobian( const LineSegment& e, - const UnitVec3& axis, + const UnitVec3& axis, const Variation& dK, - Matrix& J, + Matrix& J, bool invertV = false) { - addDirectionJacobian(e, axis, dK[1], J, invertV); - addBlock(~dK[0] * cross(axis,e.d), J); + addDirectionJacobian(e, axis, dK[1], J, invertV); + addBlock(~dK[0] * cross(axis, e.d), J); } -} +} // namespace //============================================================================== // PATH IMPL //============================================================================== -void CableSpan::Impl::realizeTopology(State &s) +void CableSpan::Impl::realizeTopology(State& s) { - PosInfo posInfo {}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Position, new Value(posInfo)); + PosInfo posInfo{}; + m_PosInfoIx = m_Subsystem.allocateCacheEntry( + s, + Stage::Position, + new Value(posInfo)); - VelInfo velInfo {}; - m_VelInfoIx = m_Subsystem.allocateCacheEntry(s, Stage::Velocity, new Value(velInfo)); + VelInfo velInfo{}; + m_VelInfoIx = m_Subsystem.allocateCacheEntry( + s, + Stage::Velocity, + new Value(velInfo)); } -void CableSpan::Impl::realizePosition(const State &s) const +void CableSpan::Impl::realizePosition(const State& s) const { - if (m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) {return;} - calcPosInfo(s, updPosInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); + if (m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { + return; + } + calcPosInfo(s, updPosInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); } -void CableSpan::Impl::realizeVelocity(const State &s) const +void CableSpan::Impl::realizeVelocity(const State& s) const { - if (m_Subsystem.isCacheValueRealized(s, m_VelInfoIx)) {return;} - calcVelInfo(s, updVelInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_VelInfoIx); + if (m_Subsystem.isCacheValueRealized(s, m_VelInfoIx)) { + return; + } + calcVelInfo(s, updVelInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_VelInfoIx); } -const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo(const State &s) const +const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo( + const State& s) const { - realizePosition(s); + realizePosition(s); return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); } -CableSpan::Impl::PosInfo& CableSpan::Impl::updPosInfo(const State &s) const +CableSpan::Impl::PosInfo& CableSpan::Impl::updPosInfo(const State& s) const { - return Value::updDowncast(m_Subsystem.updCacheEntry(s, m_PosInfoIx)); + return Value::updDowncast( + m_Subsystem.updCacheEntry(s, m_PosInfoIx)); } -const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo(const State &s) const +const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo( + const State& s) const { - realizeVelocity(s); + realizeVelocity(s); return Value::downcast(m_Subsystem.getCacheEntry(s, m_VelInfoIx)); } -CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State &s) const +CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const { - return Value::updDowncast(m_Subsystem.updCacheEntry(s, m_VelInfoIx)); + return Value::updDowncast( + m_Subsystem.updCacheEntry(s, m_VelInfoIx)); } void CableSpan::Impl::calcInitZeroLengthGeodesic(State& s) const { - Vec3 prev_QG = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); - size_t i = 0; - for (const CurveSegment& obstacle: m_CurveSegments) { - if (obstacle.getImpl().getStatus(s) == Status::Disabled) { - continue; + Vec3 prev_QG = + m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + size_t i = 0; + for (const CurveSegment& obstacle : m_CurveSegments) { + if (obstacle.getImpl().getStatus(s) == Status::Disabled) { + continue; } - obstacle.getImpl().calcInitZeroLengthGeodesic(s, prev_QG); + obstacle.getImpl().calcInitZeroLengthGeodesic(s, prev_QG); - prev_QG = obstacle.getImpl().getPosInfo(s).KQ.p(); - } + prev_QG = obstacle.getImpl().getPosInfo(s).KQ.p(); + } } const CurveSegment* CableSpan::Impl::findPrevActiveCurveSegment( - const State& s, + const State& s, CurveSegmentIndex ix) const { for (int i = ix - 1; i > 0; --i) { @@ -553,7 +632,7 @@ const CurveSegment* CableSpan::Impl::findPrevActiveCurveSegment( } const CurveSegment* CableSpan::Impl::findNextActiveCurveSegment( - const State& s, + const State& s, CurveSegmentIndex ix) const { // Find the active segment after the current. @@ -565,35 +644,32 @@ const CurveSegment* CableSpan::Impl::findNextActiveCurveSegment( return nullptr; } -Vec3 CableSpan::Impl::findPrevPoint( - const State& s, - CurveSegmentIndex ix) const +Vec3 CableSpan::Impl::findPrevPoint(const State& s, CurveSegmentIndex ix) const { const CurveSegment* segment = findPrevActiveCurveSegment(s, ix); - return segment - ? segment->getImpl().getPosInfo(s).KQ.p() - : m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + return segment ? segment->getImpl().getPosInfo(s).KQ.p() + : m_OriginBody.getBodyTransform(s).shiftFrameStationToBase( + m_OriginPoint); } -Vec3 CableSpan::Impl::findNextPoint( - const State& s, - CurveSegmentIndex ix) const +Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) const { const CurveSegment* segment = findNextActiveCurveSegment(s, ix); return segment - ? segment->getImpl().getPosInfo(s).KP.p() - : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase(m_TerminationPoint); + ? segment->getImpl().getPosInfo(s).KP.p() + : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( + m_TerminationPoint); } -template +template void CableSpan::Impl::calcPathErrorVector( - const State& s, + const State& s, const std::vector& lines, std::array axes, Vector& pathError) const { size_t lineIx = 0; - ptrdiff_t row = -1; + ptrdiff_t row = -1; for (int i = 0; i < getNumCurveSegments(); ++i) { const CurveSegment& segment = getCurveSegment(CurveSegmentIndex(i)); @@ -602,19 +678,19 @@ void CableSpan::Impl::calcPathErrorVector( } const GeodesicInfo& g = segment.getImpl().getPosInfo(s); - for (CoordinateAxis axis: axes) { - pathError(++row) = calcPathError(lines.at(lineIx), g.KP.R(), axis); + 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); + for (CoordinateAxis axis : axes) { + pathError(++row) = calcPathError(lines.at(lineIx), g.KQ.R(), axis); } } } -template +template void CableSpan::Impl::calcPathErrorJacobian( - const State& s, + const State& s, const std::vector& lines, std::array axes, Matrix& J) const @@ -624,11 +700,15 @@ void CableSpan::Impl::calcPathErrorJacobian( // TODO perhaps just not make method static. const size_t n = lines.size() - 1; - 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"); + 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 row = 0; + size_t col = 0; size_t activeIx = 0; for (const CurveSegment& segment : m_CurveSegments) { if (!segment.getImpl().isActive(s)) { @@ -640,41 +720,55 @@ void CableSpan::Impl::calcPathErrorJacobian( const LineSegment& l_Q = lines.at(activeIx + 1); const CurveSegmentIndex ix = segment.getImpl().getIndex(); - const CurveSegment* prev = findPrevActiveCurveSegment(s, ix); - const CurveSegment* next = findNextActiveCurveSegment(s, ix); + const CurveSegment* prev = findPrevActiveCurveSegment(s, ix); + const CurveSegment* next = findNextActiveCurveSegment(s, ix); - for (CoordinateAxis axis: axes) { - const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); + for (CoordinateAxis axis : axes) { + const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); const Variation& dK_P = g.dKP; - addPathErrorJacobian(l_P, a_P, dK_P, J.block(row, col, 1, Nq)); + addPathErrorJacobian(l_P, a_P, dK_P, J.block(row, col, 1, Nq)); - if (prev) { - const Variation& prev_dK_Q = prev->getImpl().getPosInfo(s).dKQ; - addDirectionJacobian(l_P, a_P, prev_dK_Q[1], J.block(row, col-Nq, 1, Nq), true); - } - ++row; - } + if (prev) { + const Variation& prev_dK_Q = prev->getImpl().getPosInfo(s).dKQ; + addDirectionJacobian( + l_P, + a_P, + prev_dK_Q[1], + J.block(row, col - Nq, 1, Nq), + true); + } + ++row; + } - for (CoordinateAxis axis: axes) { - const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); + for (CoordinateAxis axis : axes) { + const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); const Variation& dK_Q = g.dKQ; - addPathErrorJacobian(l_Q, a_Q, dK_Q, J.block(row, col, 1, Nq), true); - - if (next) { - const Variation& next_dK_P = next->getImpl().getPosInfo(s).dKP; - addDirectionJacobian(l_Q, a_Q, next_dK_P[1], J.block(row, col+Nq, 1, Nq)); - } - ++row; - } + addPathErrorJacobian( + l_Q, + a_Q, + dK_Q, + J.block(row, col, 1, Nq), + true); + + if (next) { + const Variation& next_dK_P = next->getImpl().getPosInfo(s).dKP; + addDirectionJacobian( + l_Q, + a_Q, + next_dK_P[1], + J.block(row, col + Nq, 1, Nq)); + } + ++row; + } col += Nq; }; } double CableSpan::Impl::calcPathLength( - const State& s, + const State& s, const std::vector& lines) const { double lTot = 0.; @@ -684,17 +778,16 @@ double CableSpan::Impl::calcPathLength( } for (const CurveSegment& segment : m_CurveSegments) { - if (!segment.getImpl().isActive(s)) - { + if (!segment.getImpl().isActive(s)) { continue; - } + } lTot += segment.getImpl().getPosInfo(s).length; } return lTot; } void CableSpan::Impl::calcLineSegments( - const State& s, + const State& s, Vec3 p_O, Vec3 p_I, std::vector& lines) const @@ -709,7 +802,7 @@ void CableSpan::Impl::calcLineSegments( } const GeodesicInfo& g = segment.getImpl().getPosInfo(s); - const Vec3 lineEnd = g.KP.p(); + const Vec3 lineEnd = g.KP.p(); lines.emplace_back(lineStart, lineEnd); lineStart = g.KQ.p(); @@ -730,13 +823,17 @@ size_t CableSpan::Impl::countActive(const State& s) const void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { - // Path origin and termination point. - const Vec3 x_O = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); - const Vec3 x_I = m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase(m_TerminationPoint); + // Path origin and termination point. + const Vec3 x_O = + m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + const Vec3 x_I = + m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( + m_TerminationPoint); - const std::array axes {NormalAxis, BinormalAxis}; + const std::array axes{NormalAxis, BinormalAxis}; - for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; ++posInfo.loopIter) { + for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; + ++posInfo.loopIter) { const size_t nActive = countActive(s); // Grab the shared data cache for computing the matrices, and lock it. @@ -755,10 +852,14 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const } // Evaluate the path error jacobian. - calcPathErrorJacobian<2>(s, data.lineSegments, axes, data.pathErrorJacobian); + calcPathErrorJacobian<2>( + s, + data.lineSegments, + axes, + data.pathErrorJacobian); // Compute path corrections. - const Correction* corrIt = calcPathCorrections(data); + const Correction* corrIt = calcPathCorrections(data); // Apply path corrections. for (const CurveSegment& obstacle : m_CurveSegments) { @@ -778,7 +879,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const } } - throw std::runtime_error("Failed to converge"); + throw std::runtime_error("Failed to converge"); } void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const @@ -795,27 +896,34 @@ void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const } const GeodesicInfo& g = obstacle.getImpl().getPosInfo(s); - const UnitVec3 e_G = g.KP.R().getAxisUnitVec(TangentAxis); + const UnitVec3 e_G = g.KP.R().getAxisUnitVec(TangentAxis); Vec3 next_v_GQ; Vec3 v_GP; - obstacle.getImpl().calcContactPointVelocitiesInGround(s, v_GP, next_v_GQ); + obstacle.getImpl().calcContactPointVelocitiesInGround( + s, + v_GP, + next_v_GQ); lengthDot += dot(e_G, v_GP - v_GQ); - v_GQ = next_v_GQ; + v_GQ = next_v_GQ; lastActive = &obstacle; } - const Vec3 v_GP = m_TerminationBody.findStationVelocityInGround(s, m_TerminationPoint); - const UnitVec3 e_G = lastActive? lastActive->getImpl().getPosInfo(s).KQ.R().getAxisUnitVec(TangentAxis) : UnitVec3(pos.xI - pos.xO); + const Vec3 v_GP = + m_TerminationBody.findStationVelocityInGround(s, m_TerminationPoint); + 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& state, - Real tension, - Vector_& bodyForcesInG) const + const State& state, + Real tension, + Vector_& bodyForcesInG) const { throw std::runtime_error("NOTYETIMPLEMENTED"); } @@ -824,47 +932,56 @@ void CableSpan::Impl::applyBodyForces( // SUBSYSTEM //============================================================================== -bool CableSubsystem::isInstanceOf(const Subsystem& s) { +bool CableSubsystem::isInstanceOf(const Subsystem& s) +{ return Impl::isA(s.getSubsystemGuts()); } -const CableSubsystem& CableSubsystem:: -downcast(const Subsystem& s) { +const CableSubsystem& CableSubsystem::downcast(const Subsystem& s) +{ assert(isInstanceOf(s)); return static_cast(s); } -CableSubsystem& CableSubsystem:: -updDowncast(Subsystem& s) { +CableSubsystem& CableSubsystem::updDowncast(Subsystem& s) +{ assert(isInstanceOf(s)); return static_cast(s); } -const CableSubsystem::Impl& CableSubsystem:: -getImpl() const { +const CableSubsystem::Impl& CableSubsystem::getImpl() const +{ return SimTK_DYNAMIC_CAST_DEBUG(getSubsystemGuts()); } -CableSubsystem::Impl& CableSubsystem:: -updImpl() { +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 +// except for making std::vectors, which require a default constructor to be // available. -CableSubsystem::CableSubsystem() -{ adoptSubsystemGuts(new Impl()); } +CableSubsystem::CableSubsystem() +{ + adoptSubsystemGuts(new Impl()); +} -CableSubsystem::CableSubsystem(MultibodySystem& mbs) -{ adoptSubsystemGuts(new Impl()); - mbs.adoptSubsystem(*this); } // steal ownership +CableSubsystem::CableSubsystem(MultibodySystem& mbs) +{ + adoptSubsystemGuts(new Impl()); + mbs.adoptSubsystem(*this); +} // steal ownership int CableSubsystem::getNumPaths() const -{ return getImpl().getNumPaths(); } +{ + return getImpl().getNumPaths(); +} -const CableSpan& CableSubsystem:: -getPath(WrappingPathIndex cableIx) const -{ return getImpl().getCablePath(cableIx); } +const CableSpan& CableSubsystem::getPath(WrappingPathIndex cableIx) const +{ + return getImpl().getCablePath(cableIx); +} -CableSpan& CableSubsystem:: -updPath(WrappingPathIndex cableIx) -{ return updImpl().updCablePath(cableIx); } +CableSpan& CableSubsystem::updPath(WrappingPathIndex cableIx) +{ + return updImpl().updCablePath(cableIx); +} diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 45fa775a6..e507f863b 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -30,14 +30,18 @@ class CableSpan; class SimTK_SIMBODY_EXPORT CurveSegment { public: - CurveSegment() = default; - CurveSegment(const CurveSegment&) = delete; - CurveSegment& operator = (const CurveSegment&) = delete; - CurveSegment(CurveSegment&&) noexcept = default; - CurveSegment& operator = (CurveSegment&&) noexcept = default; - ~CurveSegment() = default; - - CurveSegment(const MobilizedBody& mobod, Transform X_BS, const ContactGeometry& geometry, Vec3 xHint); + CurveSegment() = default; + CurveSegment(const CurveSegment&) = delete; + CurveSegment& operator=(const CurveSegment&) = delete; + CurveSegment(CurveSegment&&) noexcept = default; + CurveSegment& operator=(CurveSegment&&) noexcept = default; + ~CurveSegment() = default; + + CurveSegment( + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 xHint); class Impl; @@ -59,7 +63,9 @@ class SimTK_SIMBODY_EXPORT CurveSegment void setEnabled(const State& state) const; int calcPathPoints(const State& state, std::vector& points); - int calcPathFrenetFrames(const State& state, std::vector& frames); + int calcPathFrenetFrames( + const State& state, + std::vector& frames); private: explicit CurveSegment(std::unique_ptr impl); @@ -78,11 +84,11 @@ class SimTK_SIMBODY_EXPORT CurveSegment class SimTK_SIMBODY_EXPORT CableSpan { public: - struct LineSegment - { - UnitVec3 d {NaN, NaN, NaN}; - Real l = NaN; - }; + struct LineSegment + { + UnitVec3 d{NaN, NaN, NaN}; + Real l = NaN; + }; public: CableSpan( @@ -99,19 +105,28 @@ class SimTK_SIMBODY_EXPORT CableSpan Real getLengthDot(const State& state) const; void applyBodyForces( - const State& state, - Real tension, - Vector_& bodyForcesInG) const; + const State& state, + Real tension, + Vector_& bodyForcesInG) const; int calcPathPoints(const State& state, std::vector& points); - int calcPathFrenetFrames(const State& state, std::vector& frames); + int calcPathFrenetFrames( + const State& state, + std::vector& frames); class Impl; + private: explicit CableSpan(std::unique_ptr impl); - const Impl& getImpl() const { return *impl; } - Impl& updImpl() { return *impl; } + const Impl& getImpl() const + { + return *impl; + } + Impl& updImpl() + { + return *impl; + } std::shared_ptr impl = nullptr; @@ -137,7 +152,7 @@ class SimTK_SIMBODY_EXPORT CableSubsystem : public Subsystem size_t writePathPoints(std::vector& points) const; size_t writePathFrames(std::vector& frenetFrames) const; -/* private: */ + /* private: */ SimTK_PIMPL_DOWNCAST(CableSubsystem, Subsystem); class Impl; Impl& updImpl(); diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index a0c115f25..d885ff07b 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -3,13 +3,12 @@ #include "SimTKcommon/internal/State.h" #include "SimTKmath.h" -#include "simbody/internal/Wrapping.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 @@ -25,25 +24,24 @@ SimTK_DEFINE_UNIQUE_INDEX_TYPE(PathIndex); class CurveSegment::Impl { using FrenetFrame = ContactGeometry::FrenetFrame; - using Variation = ContactGeometry::GeodesicVariation; - using Correction = ContactGeometry::GeodesicCorrection; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; - private: - Impl() = default; +private: + Impl() = default; - public: +public: Impl(const Impl& source) = default; Impl& operator=(const Impl& source) = default; - ~Impl() = default; + ~Impl() = default; Impl( - CableSpan cable, - CurveSegmentIndex ix, - const MobilizedBody& mobod, - const Transform& X_BS, - ContactGeometry geometry, - Vec3 initPointGuess - ); + CableSpan cable, + CurveSegmentIndex ix, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess); // The status of this curve segment in relation to the surface it wraps // over. @@ -64,25 +62,23 @@ class CurveSegment::Impl class LocalGeodesic { using FrenetFrame = ContactGeometry::FrenetFrame; - using Variation = ContactGeometry::GeodesicVariation; - using Correction = ContactGeometry::GeodesicCorrection; + using Variation = ContactGeometry::GeodesicVariation; + using Correction = ContactGeometry::GeodesicCorrection; - public: - LocalGeodesic() = default; - ~LocalGeodesic() = default; + public: + LocalGeodesic() = default; + ~LocalGeodesic() = default; LocalGeodesic(LocalGeodesic&&) noexcept = default; LocalGeodesic& operator=(LocalGeodesic&&) noexcept = default; LocalGeodesic(const LocalGeodesic&) = delete; LocalGeodesic& operator=(const LocalGeodesic&) = delete; LocalGeodesic( - CableSubsystem subsystem, - ContactGeometry geometry, - Vec3 initPointGuess - ) : + CableSubsystem subsystem, + ContactGeometry geometry, + Vec3 initPointGuess) : m_Subsystem(subsystem), - m_Geometry(geometry), - m_InitPointGuess(initPointGuess) + m_Geometry(geometry), m_InitPointGuess(initPointGuess) {} // Allocate state variables and cache entries. @@ -92,13 +88,13 @@ class CurveSegment::Impl // Some info that can be retrieved from cache. struct LocalGeodesicInfo { - FrenetFrame KP {}; - FrenetFrame KQ {}; + FrenetFrame KP{}; + FrenetFrame KQ{}; Real length = NaN; - Variation dKP {}; - Variation dKQ {}; + Variation dKP{}; + Variation dKQ{}; Status status = Status::Ok; }; @@ -106,73 +102,104 @@ class CurveSegment::Impl // Helper struct: Required data for shooting a new geodesic. struct GeodesicInitialConditions { - private: - GeodesicInitialConditions() = default; - - public: - static GeodesicInitialConditions CreateCorrected(const FrenetFrame& KP, const Variation& dKP, Real l, const Correction& c); - static GeodesicInitialConditions CreateInSurfaceFrame(const Transform& X_GS, Vec3 x_G, Vec3 t_G, Real l); - static GeodesicInitialConditions CreateZeroLengthGuess(const Transform& X_GS, Vec3 prev_QS, Vec3 xGuess_S); - static GeodesicInitialConditions CreateAtTouchdown(Vec3 prev_QS, Vec3 next_PS, Vec3 trackingPointOnLine); + private: + GeodesicInitialConditions() = default; - Vec3 x {NaN, NaN, NaN}; - Vec3 t {NaN, NaN, NaN}; + public: + static GeodesicInitialConditions CreateCorrected( + const FrenetFrame& KP, + const Variation& dKP, + Real l, + const Correction& c); + static GeodesicInitialConditions CreateInSurfaceFrame( + const Transform& X_GS, + Vec3 x_G, + Vec3 t_G, + Real l); + static GeodesicInitialConditions CreateZeroLengthGuess( + const Transform& X_GS, + Vec3 prev_QS, + Vec3 xGuess_S); + static GeodesicInitialConditions CreateAtTouchdown( + Vec3 prev_QS, + Vec3 next_PS, + Vec3 trackingPointOnLine); + + Vec3 x{NaN, NaN, NaN}; + Vec3 t{NaN, NaN, NaN}; Real l = NaN; }; - const LocalGeodesicInfo& calcInitialGeodesic(State& s, const GeodesicInitialConditions& g0) const; - const LocalGeodesicInfo& calcLocalGeodesic(const State& s, Vec3 prev_QS, Vec3 next_PS) const; // TODO weird name + const LocalGeodesicInfo& calcInitialGeodesic( + State& s, + const GeodesicInitialConditions& g0) const; + const LocalGeodesicInfo& calcLocalGeodesic( + const State& s, + Vec3 prev_QS, + Vec3 next_PS) const; // TODO weird name void applyGeodesicCorrection(const State& s, const Correction& c) const; - size_t calcPathPoints(const State& state, std::vector& points) const; + size_t calcPathPoints(const State& state, std::vector& points) + const; // The user defined point that controls the initial wrapping path. - void setInitialPointGuess(Vec3 initPointGuess) {m_InitPointGuess = initPointGuess;} - Vec3 getInitialPointGuess() const {return m_InitPointGuess;} - - Status getStatus(const State& s) const {return getCacheEntry(s).status;} + void setInitialPointGuess(Vec3 initPointGuess) + { + m_InitPointGuess = initPointGuess; + } + Vec3 getInitialPointGuess() const + { + return m_InitPointGuess; + } - private: + Status getStatus(const State& s) const + { + return getCacheEntry(s).status; + } + private: // The cache entry: Curve in local surface coordinated. - // This is an auto update discrete cache variable, which makes it persist over integration steps. + // This is an auto update discrete cache variable, which makes it + // persist over integration steps. // TODO or should it be "normal" discrete? struct CacheEntry : LocalGeodesicInfo { - Vec3 trackingPointOnLine {NaN, NaN, NaN}; - std::vector points {}; // Empty for analytic geoemetry with no allocation overhead. + Vec3 trackingPointOnLine{NaN, NaN, NaN}; + std::vector points{}; // Empty for analytic geoemetry with no + // allocation overhead. double sHint = NaN; }; const CacheEntry& getCacheEntry(const State& state) const { return Value::downcast( - m_Subsystem.getDiscreteVarUpdateValue(state, m_CacheIx) - ); + m_Subsystem.getDiscreteVarUpdateValue(state, m_CacheIx)); } CacheEntry& updCacheEntry(const State& state) const { return Value::updDowncast( - m_Subsystem.updDiscreteVarUpdateValue(state, m_CacheIx) - ); + m_Subsystem.updDiscreteVarUpdateValue(state, m_CacheIx)); } const CacheEntry& getPrevCacheEntry(const State& state) const { return Value::downcast( - m_Subsystem.getDiscreteVariable(state, m_CacheIx) - ); + m_Subsystem.getDiscreteVariable(state, m_CacheIx)); } CacheEntry& updPrevCacheEntry(State& state) const { return Value::updDowncast( - m_Subsystem.updDiscreteVariable(state, m_CacheIx) - ); + m_Subsystem.updDiscreteVariable(state, m_CacheIx)); } - void calcStatus(const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& cache) const; - void calcGeodesic(const GeodesicInitialConditions& g0, CacheEntry& cache) const; + void calcStatus( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const; + void calcGeodesic( + const GeodesicInitialConditions& g0, + CacheEntry& cache) const; //------------------------------------------------------------------------------ CableSubsystem m_Subsystem; @@ -182,7 +209,7 @@ class CurveSegment::Impl Vec3 m_InitPointGuess; Real m_TouchdownAccuracy = 1e-3; - size_t m_TouchdownIter = 10; + size_t m_TouchdownIter = 10; DiscreteVariableIndex m_CacheIx; }; @@ -190,13 +217,13 @@ class CurveSegment::Impl // Position level cache: Curve in ground frame. struct PosInfo { - Transform X_GS {}; + Transform X_GS{}; - FrenetFrame KP {}; - FrenetFrame KQ {}; + FrenetFrame KP{}; + FrenetFrame KQ{}; - Variation dKP {}; - Variation dKQ {}; + Variation dKP{}; + Variation dKQ{}; Real length = NaN; @@ -220,23 +247,42 @@ class CurveSegment::Impl m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); } - CurveSegmentIndex getIndex() const {return m_Index;} - void setIndex(CurveSegmentIndex ix) {m_Index = ix;} + CurveSegmentIndex getIndex() const + { + return m_Index; + } + void setIndex(CurveSegmentIndex ix) + { + m_Index = ix; + } - const PosInfo& getPosInfo(const State& s) const { + const PosInfo& getPosInfo(const State& s) const + { realizePosition(s); - return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); + return Value::downcast( + m_Subsystem.getCacheEntry(s, m_PosInfoIx)); } - bool isActive(const State& s) const {return getPosInfo(s).status == Status::Ok;} - Status getStatus(const State& s) const {return m_Surface.getStatus(s);} + bool isActive(const State& s) const + { + return getPosInfo(s).status == Status::Ok; + } + Status getStatus(const State& s) const + { + return m_Surface.getStatus(s); + } void calcInitZeroLengthGeodesic(State& state, Vec3 prev_QS) const; - void applyGeodesicCorrection(const State& state, const ContactGeometry::GeodesicCorrection& c) const; + void applyGeodesicCorrection( + const State& state, + const ContactGeometry::GeodesicCorrection& c) const; size_t calcPathPoints(const State& state, std::vector& points) const; /* const MobilizedBody& getMobilizedBody() const {return m_Mobod;} */ - void calcContactPointVelocitiesInGround(const State& s, Vec3& v_GP, Vec3& v_GQ) const + void calcContactPointVelocitiesInGround( + const State& s, + Vec3& v_GP, + Vec3& v_GQ) const { // TODO use builtin? /* v_GP = m_Mobod.findStationVelocityInGround(state, P_B); */ @@ -248,7 +294,8 @@ class CurveSegment::Impl const PosInfo& pos = getPosInfo(s); - // Relative contact point positions to body origin, expressed in ground frame. + // Relative contact point positions to body origin, expressed in ground + // frame. Vec3 r_P = pos.KP.p() - x_BG; Vec3 r_Q = pos.KQ.p() - x_BG; @@ -260,17 +307,18 @@ class CurveSegment::Impl // TODO allow for user to shoot his own geodesic. /* void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; */ - private: - PosInfo& updPosInfo(const State &state) const +private: + PosInfo& updPosInfo(const State& state) const { - return Value::updDowncast(m_Subsystem.updCacheEntry(state, m_PosInfoIx)); + return Value::updDowncast( + m_Subsystem.updCacheEntry(state, m_PosInfoIx)); } void calcPosInfo(const State& state, PosInfo& posInfo) 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. + CableSpan m_Path; // The path this segment belongs to. + CurveSegmentIndex m_Index; // The index in its path. MobilizedBody m_Mobod; Transform m_Offset; @@ -278,7 +326,7 @@ class CurveSegment::Impl LocalGeodesic m_Surface; // TOPOLOGY CACHE - CacheEntryIndex m_PosInfoIx; + CacheEntryIndex m_PosInfoIx; }; //============================================================================== @@ -288,203 +336,233 @@ class CurveSegment::Impl // - handout CurveSegment index // - add other cache variables: velocity, acceleration, force // - names: isValid, ContactStationOnBody, Entry (not info/cache) -class CableSpan::Impl { - public: - Impl( - CableSubsystem subsystem, - MobilizedBody originBody, - Vec3 originPoint, - MobilizedBody 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();} +class CableSpan::Impl +{ +public: + Impl( + CableSubsystem subsystem, + MobilizedBody originBody, + Vec3 originPoint, + MobilizedBody 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]; } + const CurveSegment& getCurveSegment(CurveSegmentIndex ix) const + { + return m_CurveSegments[ix]; + } - CurveSegmentIndex adoptObstacle(CurveSegment& segment) - { - m_CurveSegments.push_back(segment); - return CurveSegmentIndex(m_CurveSegments.size()-1); - } + CurveSegmentIndex adoptObstacle(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}; + // Position level cache entry. + struct PosInfo + { + Vec3 xO{NaN, NaN, NaN}; + Vec3 xI{NaN, NaN, NaN}; - Real l = NaN; + Real l = NaN; - size_t loopIter = 0; - }; + size_t loopIter = 0; + }; - // Velocity level cache entry. - struct VelInfo - { - Real lengthDot = NaN; - }; + // 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() - { m_Subsystem.invalidateSubsystemTopologyCache(); } + // Allocate state variables and cache entries. + void realizeTopology(State& state); + void realizePosition(const State& state) const; + void realizeVelocity(const State& state) const; + void invalidateTopology() + { + m_Subsystem.invalidateSubsystemTopologyCache(); + } - const PosInfo& getPosInfo(const State& state) const; - const VelInfo& getVelInfo(const State& state) const; + const PosInfo& getPosInfo(const State& state) const; + const VelInfo& getVelInfo(const State& state) const; - void calcInitZeroLengthGeodesic(State& s) const; + void calcInitZeroLengthGeodesic(State& s) const; - void applyBodyForces( - const State& state, - Real tension, - Vector_& bodyForcesInG) const; + void applyBodyForces( + const State& state, + Real tension, + Vector_& bodyForcesInG) const; - private: - PosInfo& updPosInfo(const State& s) const; - VelInfo& updVelInfo(const State& state) 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; + void calcPosInfo(const State& s, PosInfo& posInfo) const; + void calcVelInfo(const State& s, VelInfo& velInfo) const; - size_t countActive(const State& s) const; + size_t countActive(const State& s) const; - Vec3 findPrevPoint( - const State& state, - CurveSegmentIndex ix) const; + Vec3 findPrevPoint(const State& state, CurveSegmentIndex ix) const; - Vec3 findNextPoint( - const State& state, - CurveSegmentIndex ix) const; + Vec3 findNextPoint(const State& state, CurveSegmentIndex ix) const; - const CurveSegment* findPrevActiveCurveSegment(const State& s, CurveSegmentIndex ix) const; - const CurveSegment* findNextActiveCurveSegment(const State& s, CurveSegmentIndex ix) const; + 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 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; + template + void calcPathErrorJacobian( + const State& state, + const std::vector& lines, + std::array axes, + Matrix& J) const; - // Make static or not? - void calcLineSegments( - const State& s, - Vec3 p_O, - Vec3 p_I, - std::vector& lines) const; + // Make static or not? + void calcLineSegments( + const State& s, + Vec3 p_O, + Vec3 p_I, + std::vector& lines) const; - double calcPathLength( - const State& state, - const std::vector& lines) const; + double calcPathLength( + const State& state, + const std::vector& lines) const; - const CableSubsystem& getSubsystem() const {return m_Subsystem;} + const CableSubsystem& getSubsystem() const + { + return m_Subsystem; + } - // Reference back to the subsystem. - CableSubsystem m_Subsystem; + // Reference back to the subsystem. + CableSubsystem m_Subsystem; - MobilizedBody m_OriginBody; - Vec3 m_OriginPoint; + MobilizedBody m_OriginBody; + Vec3 m_OriginPoint; - MobilizedBody m_TerminationBody; - Vec3 m_TerminationPoint; + MobilizedBody m_TerminationBody; + Vec3 m_TerminationPoint; - Array_ m_CurveSegments {}; + Array_ m_CurveSegments{}; - Real m_PathErrorBound = 0.1; - Real m_ObsErrorBound = 0.1; - size_t m_PathMaxIter = 10; - size_t m_ObsMaxIter = 10; + Real m_PathErrorBound = 0.1; + Real m_ObsErrorBound = 0.1; + size_t m_PathMaxIter = 10; + size_t m_ObsMaxIter = 10; - // TOPOLOGY CACHE (set during realizeTopology()) - CacheEntryIndex m_PosInfoIx; - CacheEntryIndex m_VelInfoIx; + // TOPOLOGY CACHE (set during realizeTopology()) + CacheEntryIndex m_PosInfoIx; + CacheEntryIndex m_VelInfoIx; - friend CurveSegment::Impl; + friend CurveSegment::Impl; }; //============================================================================== // SUBSYSTEM :: IMPL //============================================================================== -class CableSubsystem::Impl : public Subsystem::Guts { - public: - Impl() {} - ~Impl() {} - Impl* cloneImpl() const override - { return new Impl(*this); } - - int getNumPaths() const {return cables.size();} +class CableSubsystem::Impl : public Subsystem::Guts +{ +public: + Impl() + {} + ~Impl() + {} + Impl* cloneImpl() const override + { + return new Impl(*this); + } - const CableSpan& getCablePath(WrappingPathIndex index) const - { return cables[index]; } + int getNumPaths() const + { + return cables.size(); + } - CableSpan& updCablePath(WrappingPathIndex index) - { return cables[index]; } + const CableSpan& getCablePath(WrappingPathIndex index) const + { + return cables[index]; + } - // Add a cable path to the list, bumping the reference count. - WrappingPathIndex adoptCablePath(CableSpan& path) { - cables.push_back(path); - return WrappingPathIndex(cables.size()-1); - } + CableSpan& updCablePath(WrappingPathIndex index) + { + return cables[index]; + } - // Return the MultibodySystem which owns this WrappingPathSubsystem. - const MultibodySystem& getMultibodySystem() const - { return MultibodySystem::downcast(getSystem()); } + // Add a cable path to the list, bumping the reference count. + WrappingPathIndex adoptCablePath(CableSpan& path) + { + cables.push_back(path); + return WrappingPathIndex(cables.size() - 1); + } - // Return the SimbodyMatterSubsystem from which this WrappingPathSubsystem - // gets the bodies to track. - const SimbodyMatterSubsystem& getMatterSubsystem() const - { return getMultibodySystem().getMatterSubsystem(); } + // Return the MultibodySystem which owns this WrappingPathSubsystem. + const MultibodySystem& getMultibodySystem() const + { + return MultibodySystem::downcast(getSystem()); + } - // 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); + // Return the SimbodyMatterSubsystem from which this WrappingPathSubsystem + // gets the bodies to track. + const SimbodyMatterSubsystem& getMatterSubsystem() const + { + return getMultibodySystem().getMatterSubsystem(); + } - for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { - CableSpan& path = wThis->updCablePath(ix); - path.updImpl().realizeTopology(state); - } + // 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); - return 0; + for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + CableSpan& path = wThis->updCablePath(ix); + path.updImpl().realizeTopology(state); } - int realizeSubsystemPositionImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { - const CableSpan& path = getCablePath(ix); - path.getImpl().realizePosition(state); - } - return 0; + return 0; + } + + int realizeSubsystemPositionImpl(const State& state) const override + { + for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + const CableSpan& path = getCablePath(ix); + path.getImpl().realizePosition(state); } + return 0; + } - int realizeSubsystemVelocityImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { - const CableSpan& path = getCablePath(ix); - path.getImpl().realizeVelocity(state); - } - return 0; + int realizeSubsystemVelocityImpl(const State& state) const override + { + for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + const CableSpan& path = getCablePath(ix); + path.getImpl().realizeVelocity(state); } + return 0; + } - SimTK_DOWNCAST(Impl, Subsystem::Guts); + SimTK_DOWNCAST(Impl, Subsystem::Guts); - private: - // TOPOLOGY STATE - Array_ cables; +private: + // TOPOLOGY STATE + Array_ cables; }; } // namespace SimTK From 7ad1585e0a2ff4f2cc9dca5e8a1b60169181a10f Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 24 Apr 2024 12:33:12 +0200 Subject: [PATCH 029/127] change "Path" to "Segment" --- Simbody/include/simbody/internal/Wrapping.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index e507f863b..74a3bfba2 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -62,8 +62,8 @@ class SimTK_SIMBODY_EXPORT CurveSegment void setDisabled(const State& state) const; void setEnabled(const State& state) const; - int calcPathPoints(const State& state, std::vector& points); - int calcPathFrenetFrames( + int calcSegmentPoints(const State& state, std::vector& points); + int calcSegmentFrenetFrames( const State& state, std::vector& frames); From 98a36f9abacdd04a87e142f7c7cae2b18771e8d8 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 25 Apr 2024 11:44:21 +0200 Subject: [PATCH 030/127] wip: Renaming, cleanup etc --- Simbody/include/simbody/internal/Wrapping.cpp | 186 +++++++++++------- Simbody/include/simbody/internal/Wrapping.h | 2 + .../include/simbody/internal/WrappingImpl.h | 31 ++- 3 files changed, 144 insertions(+), 75 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 96c5022da..de67c46ce 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -159,7 +159,7 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected( return g0; } -GeodesicInitialConditions GeodesicInitialConditions::CreateInSurfaceFrame( +GeodesicInitialConditions GeodesicInitialConditions::CreateFromGroundInSurfaceFrame( const Transform& X_GS, Vec3 x_G, Vec3 t_G, @@ -175,14 +175,13 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateInSurfaceFrame( } GeodesicInitialConditions GeodesicInitialConditions::CreateZeroLengthGuess( - const Transform& X_GS, Vec3 prev_QS, Vec3 xGuess_S) { GeodesicInitialConditions g0; g0.x = xGuess_S; - g0.t = g0.x - X_GS.shiftBaseStationToFrame(prev_QS); + g0.t = xGuess_S - prev_QS; g0.l = 0.; return g0; @@ -205,12 +204,12 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateAtTouchdown( //============================================================================== // SURFACE IMPL //============================================================================== -using Surface = CurveSegment::Impl::LocalGeodesic; +using LocalGeodesic = CurveSegment::Impl::LocalGeodesic; //------------------------------------------------------------------------------ // REALIZE CACHE //------------------------------------------------------------------------------ -void Surface::realizeTopology(State& s) +void LocalGeodesic::realizeTopology(State& s) { // Allocate an auto-update discrete variable for the last computed geodesic. CacheEntry cache{}; @@ -221,7 +220,7 @@ void Surface::realizeTopology(State& s) Stage::Position); } -void Surface::realizePosition(const State& s) const +void LocalGeodesic::realizePosition(const State& s) const { if (!m_Subsystem.isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { updCacheEntry(s) = getPrevCacheEntry(s); @@ -232,7 +231,7 @@ void Surface::realizePosition(const State& s) const //------------------------------------------------------------------------------ // PUBLIC METHODS //------------------------------------------------------------------------------ -const LocalGeodesicInfo& Surface::calcInitialGeodesic( +const LocalGeodesicInfo& LocalGeodesic::calcInitialGeodesic( State& s, const GeodesicInitialConditions& g0) const { @@ -243,18 +242,18 @@ const LocalGeodesicInfo& Surface::calcInitialGeodesic( m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); } -const LocalGeodesicInfo& Surface::calcLocalGeodesic( +const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( const State& s, Vec3 prev_QS, Vec3 next_PS) const { realizePosition(s); CacheEntry& cache = updCacheEntry(s); - calcStatus(prev_QS, next_PS, cache); + calcCacheEntry(prev_QS, next_PS, cache); return cache; } -void Surface::applyGeodesicCorrection(const State& s, const Correction& c) const +void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) const { realizePosition(s); @@ -269,7 +268,7 @@ void Surface::applyGeodesicCorrection(const State& s, const Correction& c) const calcGeodesic(g0, updCacheEntry(s)); } -size_t Surface::calcPathPoints(const State& s, std::vector& points) const +size_t LocalGeodesic::calcPathPoints(const State& s, std::vector& points) const { realizePosition(s); @@ -286,7 +285,7 @@ size_t Surface::calcPathPoints(const State& s, std::vector& points) const //------------------------------------------------------------------------------ // PRIVATE METHODS //------------------------------------------------------------------------------ -void Surface::calcGeodesic( +void LocalGeodesic::calcGeodesic( const GeodesicInitialConditions& g0, CacheEntry& cache) const { @@ -309,60 +308,100 @@ void Surface::calcGeodesic( throw std::runtime_error("NOTYETIMPLEMENTED"); } -void Surface::calcStatus( +void LocalGeodesic::calcLiftoffIfNeeded( const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& cache) const { + // Only attempt liftoff when currently wrapping the surface. LocalGeodesicInfo& g = cache; - - if (g.status == Status::Disabled) { + if (g.status != Status::Ok) { return; } - // Make sure that the previous point does not lie inside the surface. - if (m_Geometry.calcSurfaceValue(prev_QS)) { - // TODO use proper assert. - throw std::runtime_error("Unable to wrap over surface: Preceding point " - "lies inside the surface"); + // The curve length must have shrunk completely before lifting off. + if (g.length < 0.) { + return; } - if (m_Geometry.calcSurfaceValue(next_PS)) { - // TODO use proper assert. - throw std::runtime_error( - "Unable to wrap over surface: Next point lies inside the surface"); + + // For a zero-length curve, trigger liftoff when the prev and next points + // lie above the surface plane. + if ( + dot(prev_QS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0. + && + dot(next_PS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0.) + { + // No liftoff. + return; } - Vec3& pTrack = cache.trackingPointOnLine; + // Liftoff detected: update status. + g.status = Status::Liftoff; + // Initialize the tracking point from the last geodesic start point. + cache.trackingPointOnLine = g.KP.p(); + +} + +void LocalGeodesic::calcTouchdownIfNeeded( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const +{ + // Only attempt touchdown when liftoff. + LocalGeodesicInfo& g = cache; + if (g.status != Status::Liftoff) { + return; + } - // Detect touchdown. - bool detectedTouchdown = g.status == Status::Liftoff; - detectedTouchdown &= m_Geometry.calcNearestPointOnLine( + // Detect touchdown by computing the point on the line from x_QS to x_PS + // that is nearest to the surface. + if(!m_Geometry.calcNearestPointOnLine( prev_QS, next_PS, - pTrack, + cache.trackingPointOnLine, m_TouchdownIter, - m_TouchdownAccuracy); - if (detectedTouchdown) { - g.status = Status::Ok; - GeodesicInitialConditions g0 = - GeodesicInitialConditions::CreateAtTouchdown( + m_TouchdownAccuracy)) + { + // No touchdown detected. + return; + } + + // Touchdown detected: Remove the liftoff status flag. + g.status = Status::Ok; + // Shoot a zero length geodesic at the touchdown point. + GeodesicInitialConditions g0 = + GeodesicInitialConditions::CreateAtTouchdown( prev_QS, next_PS, cache.trackingPointOnLine); - calcGeodesic(g0, cache); - } + calcGeodesic(g0, cache); +} - // Detect liftoff. - bool detectedLiftoff = g.status == Status::Ok; - detectedLiftoff &= g.length == 0.; - detectedLiftoff &= - dot(prev_QS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) > 0.; - detectedLiftoff &= - dot(next_PS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) > 0.; - if (detectedLiftoff) { - g.status = Status::Liftoff; - pTrack = g.KP.p(); +void LocalGeodesic::assertSurfaceBounds( + const Vec3& prev_QS, + const Vec3& next_PS) const +{ + // Make sure that the previous point does not lie inside the surface. + SimTK_ASSERT(m_Geometry.calcSurfaceValue(prev_QS) < 0., + "Unable to wrap over surface: Preceding point lies inside the surface"); + SimTK_ASSERT(m_Geometry.calcSurfaceValue(next_PS) < 0., + "Unable to wrap over surface: Next point lies inside the surface"); +} + +void LocalGeodesic::calcCacheEntry( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const +{ + LocalGeodesicInfo& g = cache; + + if (g.status == Status::Disabled) { + return; } + + assertSurfaceBounds(prev_QS, next_PS); + calcTouchdownIfNeeded(prev_QS, next_PS, cache); + calcLiftoffIfNeeded(prev_QS, next_PS, cache); } //============================================================================== @@ -411,7 +450,6 @@ void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateZeroLengthGuess( - X_GS, prev_QS, xGuess_S); m_Surface.calcInitialGeodesic(s, g0); @@ -441,6 +479,36 @@ size_t CurveSegment::Impl::calcPathPoints( } return n; } +void CurveSegment::Impl::calcGeodesicInGround( + const LocalGeodesicInfo& geodesic_S, + const Transform& X_GS, + PosInfo& posInfo) const +{ + posInfo.X_GS = X_GS; + + // Store the the local geodesic in ground frame. + posInfo.KP = X_GS.compose(geodesic_S.KP); + posInfo.KQ = X_GS.compose(geodesic_S.KQ); + + posInfo.dKP[0] = X_GS.R() * geodesic_S.dKP[0]; + posInfo.dKP[1] = X_GS.R() * geodesic_S.dKP[1]; + + posInfo.dKQ[0] = X_GS.R() * geodesic_S.dKQ[0]; + posInfo.dKQ[1] = X_GS.R() * geodesic_S.dKQ[1]; + + // TODO use SpatialVec for variation. + /* for (size_t i = 0; i < GeodesicDOF; ++i) { */ + /* posInfo.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) */ + /* posInfo.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) */ + + /* posInfo.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) */ + /* posInfo.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) */ + /* } */ + + posInfo.length = geodesic_S.length; + + throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); +} void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { @@ -464,30 +532,10 @@ void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // TODO this doesnt follow the regular invalidation scheme... // Grab the last geodesic that was computed. const LocalGeodesicInfo& geodesic_S = - m_Surface.calcLocalGeodesic(s, prev_S, next_S); + m_Surface.calcLocalGeodesicInfo(s, prev_S, next_S); // Store the the local geodesic in ground frame. - posInfo.KP = X_GS.compose(geodesic_S.KP); - posInfo.KQ = X_GS.compose(geodesic_S.KQ); - - posInfo.dKP[0] = X_GS.R() * geodesic_S.dKP[0]; - posInfo.dKP[1] = X_GS.R() * geodesic_S.dKP[1]; - - posInfo.dKQ[0] = X_GS.R() * geodesic_S.dKQ[0]; - posInfo.dKQ[1] = X_GS.R() * geodesic_S.dKQ[1]; - - // TODO use SpatialVec for variation. - /* for (size_t i = 0; i < GeodesicDOF; ++i) { */ - /* posInfo.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) */ - /* posInfo.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) */ - - /* posInfo.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) */ - /* posInfo.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) */ - /* } */ - - posInfo.length = geodesic_S.length; - - throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); + calcGeodesicInGround(geodesic_S, X_GS, updPosInfo(s)); } //============================================================================== diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 74a3bfba2..8816926dc 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -62,6 +62,8 @@ class SimTK_SIMBODY_EXPORT CurveSegment void setDisabled(const State& state) const; void setEnabled(const State& state) const; + const MobilizedBody& getMobilizedBody(const State& state) const; + int calcSegmentPoints(const State& state, std::vector& points); int calcSegmentFrenetFrames( const State& state, diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index d885ff07b..3aca24f1d 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -43,8 +43,7 @@ class CurveSegment::Impl ContactGeometry geometry, Vec3 initPointGuess); - // The status of this curve segment in relation to the surface it wraps - // over. + // The status of this curve segment in relation to the surface it wraps over. enum class Status { Ok, @@ -111,13 +110,12 @@ class CurveSegment::Impl const Variation& dKP, Real l, const Correction& c); - static GeodesicInitialConditions CreateInSurfaceFrame( + static GeodesicInitialConditions CreateFromGroundInSurfaceFrame( const Transform& X_GS, Vec3 x_G, Vec3 t_G, Real l); static GeodesicInitialConditions CreateZeroLengthGuess( - const Transform& X_GS, Vec3 prev_QS, Vec3 xGuess_S); static GeodesicInitialConditions CreateAtTouchdown( @@ -133,7 +131,7 @@ class CurveSegment::Impl const LocalGeodesicInfo& calcInitialGeodesic( State& s, const GeodesicInitialConditions& g0) const; - const LocalGeodesicInfo& calcLocalGeodesic( + const LocalGeodesicInfo& calcLocalGeodesicInfo( const State& s, Vec3 prev_QS, Vec3 next_PS) const; // TODO weird name @@ -193,10 +191,25 @@ class CurveSegment::Impl m_Subsystem.updDiscreteVariable(state, m_CacheIx)); } - void calcStatus( + void calcCacheEntry( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const; + + void assertSurfaceBounds( + const Vec3& prev_QS, + const Vec3& next_PS) const; + + void calcTouchdownIfNeeded( const Vec3& prev_QS, const Vec3& next_PS, CacheEntry& cache) const; + + void calcLiftoffIfNeeded( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const; + void calcGeodesic( const GeodesicInitialConditions& g0, CacheEntry& cache) const; @@ -313,6 +326,12 @@ class CurveSegment::Impl return Value::updDowncast( m_Subsystem.updCacheEntry(state, m_PosInfoIx)); } + + void calcGeodesicInGround( + const LocalGeodesic::LocalGeodesicInfo& geodesic_S, + const Transform& X_GS, + PosInfo& posInfo) const; + void calcPosInfo(const State& state, PosInfo& posInfo) const; // TODO Required for accessing the cache variable? From 71d5a469095d6d27d692fcc921b5208c4e14d150 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 25 Apr 2024 15:51:23 +0200 Subject: [PATCH 031/127] general improvements, added computing forces: wip --- Simbody/include/simbody/internal/Wrapping.cpp | 151 ++++++++++++------ .../include/simbody/internal/WrappingImpl.h | 54 ++++--- 2 files changed, 134 insertions(+), 71 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index de67c46ce..9d6df5587 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -21,7 +21,7 @@ using GeodesicJacobian = Vec4; using PointVariation = ContactGeometry::GeodesicPointVariation; using Variation = ContactGeometry::GeodesicVariation; using LineSegment = CableSpan::LineSegment; -using Status = CurveSegment::Impl::Status; +using Status = CurveSegment::Status; //============================================================================== // CONSTANTS @@ -153,8 +153,9 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected( const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); g0.t = t + cross(w, t); - Real dl = c[3]; - g0.l = l + dl; + // Take the length correction, and add to the current length. + Real dl = c[3]; // Length increment is the last correction element. + g0.l = std::max(l + dl, 0.); // Clamp length to be nonnegative. return g0; } @@ -237,7 +238,7 @@ const LocalGeodesicInfo& LocalGeodesic::calcInitialGeodesic( { // TODO is this correct? CacheEntry& cache = updPrevCacheEntry(s); - calcGeodesic(g0, cache); + shootNewGeodesic(g0, cache); updCacheEntry(s) = cache; m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); } @@ -265,7 +266,7 @@ void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) GeodesicInitialConditions::CreateCorrected(g.KP, g.dKP, g.length, c); // Shoot the new geodesic. - calcGeodesic(g0, updCacheEntry(s)); + shootNewGeodesic(g0, updCacheEntry(s)); } size_t LocalGeodesic::calcPathPoints(const State& s, std::vector& points) const @@ -285,7 +286,7 @@ size_t LocalGeodesic::calcPathPoints(const State& s, std::vector& points) //------------------------------------------------------------------------------ // PRIVATE METHODS //------------------------------------------------------------------------------ -void LocalGeodesic::calcGeodesic( +void LocalGeodesic::shootNewGeodesic( const GeodesicInitialConditions& g0, CacheEntry& cache) const { @@ -374,7 +375,7 @@ void LocalGeodesic::calcTouchdownIfNeeded( prev_QS, next_PS, cache.trackingPointOnLine); - calcGeodesic(g0, cache); + shootNewGeodesic(g0, cache); } void LocalGeodesic::assertSurfaceBounds( @@ -479,36 +480,40 @@ size_t CurveSegment::Impl::calcPathPoints( } return n; } -void CurveSegment::Impl::calcGeodesicInGround( + +namespace +{ +void xformSurfaceGeodesicToGround( const LocalGeodesicInfo& geodesic_S, const Transform& X_GS, - PosInfo& posInfo) const + CurveSegment::Impl::PosInfo& geodesic_G) { - posInfo.X_GS = X_GS; + geodesic_G.X_GS = X_GS; // Store the the local geodesic in ground frame. - posInfo.KP = X_GS.compose(geodesic_S.KP); - posInfo.KQ = X_GS.compose(geodesic_S.KQ); + geodesic_G.KP = X_GS.compose(geodesic_S.KP); + geodesic_G.KQ = X_GS.compose(geodesic_S.KQ); - posInfo.dKP[0] = X_GS.R() * geodesic_S.dKP[0]; - posInfo.dKP[1] = X_GS.R() * geodesic_S.dKP[1]; + geodesic_G.dKP[0] = X_GS.R() * geodesic_S.dKP[0]; + geodesic_G.dKP[1] = X_GS.R() * geodesic_S.dKP[1]; - posInfo.dKQ[0] = X_GS.R() * geodesic_S.dKQ[0]; - posInfo.dKQ[1] = X_GS.R() * geodesic_S.dKQ[1]; + geodesic_G.dKQ[0] = X_GS.R() * geodesic_S.dKQ[0]; + geodesic_G.dKQ[1] = X_GS.R() * geodesic_S.dKQ[1]; // TODO use SpatialVec for variation. /* for (size_t i = 0; i < GeodesicDOF; ++i) { */ - /* posInfo.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) */ - /* posInfo.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) */ + /* geodesic_G.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) */ + /* geodesic_G.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) */ - /* posInfo.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) */ - /* posInfo.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) */ + /* geodesic_G.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) */ + /* geodesic_G.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) */ /* } */ - posInfo.length = geodesic_S.length; + geodesic_G.length = geodesic_S.length; throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); } +} void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { @@ -517,8 +522,7 @@ void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const } // Compute tramsform from local surface frame to ground. - const Transform& X_GS = posInfo.X_GS = - m_Mobod.getBodyTransform(s).compose(m_Offset); + const Transform& X_GS = calcSurfaceFrameInGround(s); // Get the path points before and after this segment. const Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_Index); @@ -535,7 +539,37 @@ void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const m_Surface.calcLocalGeodesicInfo(s, prev_S, next_S); // Store the the local geodesic in ground frame. - calcGeodesicInGround(geodesic_S, X_GS, updPosInfo(s)); + xformSurfaceGeodesicToGround(geodesic_S, X_GS, updPosInfo(s)); +} + +SpatialVec CurveSegment::Impl::calcAppliedWrenchInGround(const State& s, Real tension) const +{ + const PosInfo& posInfo = getPosInfo(s); + + const UnitVec3& t_P = posInfo.KP.R().getAxisUnitVec(TangentAxis); + const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); + const Vec3 F_G = tension * (t_Q - t_P); + + const Vec3 x_GS = posInfo.X_GS.p(); + const Vec3& r_P = posInfo.KP.p() - x_GS; + const Vec3& r_Q = posInfo.KQ.p() - x_GS; + const Vec3 M_G = tension * (r_Q % t_Q - r_P % t_P); + return {M_G, F_G}; +} + +void CurveSegment::Impl::applyBodyForce( + const State& s, + Real tension, + Vector_& bodyForcesInG) const +{ + // TODO why? + if (tension <= 0) {return;} + + if (getStatus(s) != Status::Ok) { + return; + } + + m_Mobod.applyBodyForce(s, calcAppliedWrenchInGround(s, tension), bodyForcesInG); } //============================================================================== @@ -886,36 +920,43 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Grab the shared data cache for computing the matrices, and lock it. SolverDataCache& dataCache = findDataCache(nActive); - dataCache.lock(); - SolverData& data = dataCache.updData(); - - // Compute the straight-line segments. - calcLineSegments(s, x_O, x_I, data.lineSegments); + try { + dataCache.lock(); - // Evaluate path error, and stop when converged. - calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); - const Real maxPathError = data.pathError.normInf(); - if (maxPathError < m_PathErrorBound) { - return; - } + SolverData& data = dataCache.updData(); - // Evaluate the path error jacobian. - calcPathErrorJacobian<2>( - s, - data.lineSegments, - axes, - data.pathErrorJacobian); + // Compute the straight-line segments. + calcLineSegments(s, x_O, x_I, data.lineSegments); - // Compute path corrections. - const Correction* corrIt = calcPathCorrections(data); + // Evaluate path error, and stop when converged. + calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); + const Real maxPathError = data.pathError.normInf(); + if (maxPathError < m_PathErrorBound) { + return; + } - // Apply path corrections. - for (const CurveSegment& obstacle : m_CurveSegments) { - if (!obstacle.getImpl().isActive(s)) { - continue; + // Evaluate the path error jacobian. + calcPathErrorJacobian<2>( + s, + data.lineSegments, + axes, + data.pathErrorJacobian); + + // Compute path corrections. + const Correction* corrIt = calcPathCorrections(data); + + // Apply path corrections. + for (const CurveSegment& obstacle : m_CurveSegments) { + if (!obstacle.getImpl().isActive(s)) { + continue; + } + obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); + ++corrIt; } - obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); - ++corrIt; + + } catch (const std::exception& e) { + dataCache.unlock(); + throw e; } // Release the lock on the shared data. @@ -968,12 +1009,20 @@ void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const lengthDot += dot(e_G, v_GP - v_GQ); } + void CableSpan::Impl::applyBodyForces( - const State& state, + const State& s, Real tension, Vector_& bodyForcesInG) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + // TODO why? + if (tension <= 0) {return;} + + realizePosition(s); + + for (const CurveSegment& segment: m_CurveSegments) { + segment.getImpl().applyBodyForce(s, tension, bodyForcesInG); + } } //============================================================================== diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 3aca24f1d..ca1afdcef 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -43,14 +43,6 @@ class CurveSegment::Impl ContactGeometry geometry, Vec3 initPointGuess); - // The status of this curve segment in relation to the surface it wraps over. - enum class Status - { - Ok, - Liftoff, - Disabled, - }; - //============================================================================== // ??? //============================================================================== @@ -131,19 +123,31 @@ class CurveSegment::Impl const LocalGeodesicInfo& calcInitialGeodesic( State& s, const GeodesicInitialConditions& g0) const; + + // This will reevaluate the cached geodesic and status. + // This will be called by the curve segment before updating the + // position level cache variable of the CurveSegment. + // TODO Unfortunate naming convention. const LocalGeodesicInfo& calcLocalGeodesicInfo( const State& s, Vec3 prev_QS, Vec3 next_PS) const; // TODO weird name + + // 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; - size_t calcPathPoints(const State& state, std::vector& points) - const; - // The user defined point that controls the initial wrapping path. + // Compute the path points of the current geodesic, and write them to the buffer. + // These points are in local surface coordinates. Returns the number of points written. + size_t calcPathPoints(const State& state, std::vector& points) const; + + // Set the user defined point that controls the initial wrapping path. void setInitialPointGuess(Vec3 initPointGuess) { m_InitPointGuess = initPointGuess; } + + // Get the user defined point that controls the initial wrapping path. Vec3 getInitialPointGuess() const { return m_InitPointGuess; @@ -210,7 +214,7 @@ class CurveSegment::Impl const Vec3& next_PS, CacheEntry& cache) const; - void calcGeodesic( + void shootNewGeodesic( const GeodesicInitialConditions& g0, CacheEntry& cache) const; @@ -243,8 +247,6 @@ class CurveSegment::Impl // TODO add force & moment here? /* Vec3 unitForce {}; */ /* Vec3 unitMoment {}; */ - - Status status = Status::Ok; }; // Allocate state variables and cache entries. @@ -264,6 +266,7 @@ class CurveSegment::Impl { return m_Index; } + void setIndex(CurveSegmentIndex ix) { m_Index = ix; @@ -278,17 +281,20 @@ class CurveSegment::Impl bool isActive(const State& s) const { - return getPosInfo(s).status == Status::Ok; + return m_Surface.getStatus(s) == Status::Ok; } + Status getStatus(const State& s) const { return m_Surface.getStatus(s); } void calcInitZeroLengthGeodesic(State& state, Vec3 prev_QS) const; + void applyGeodesicCorrection( const State& state, const ContactGeometry::GeodesicCorrection& c) const; + size_t calcPathPoints(const State& state, std::vector& points) const; /* const MobilizedBody& getMobilizedBody() const {return m_Mobod;} */ @@ -317,6 +323,19 @@ class CurveSegment::Impl v_GQ = v_BG + w_BG % r_Q; } + Transform calcSurfaceFrameInGround(const State& s) const + { + return m_Mobod.getBodyTransform(s).compose(m_Offset); + } + + SpatialVec calcAppliedWrenchInGround(const State& s, Real tension) const; + + // TODO Force? or wrench? + void applyBodyForce( + const State& state, + Real tension, + Vector_& bodyForcesInG) const; + // TODO allow for user to shoot his own geodesic. /* void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; */ @@ -327,11 +346,6 @@ class CurveSegment::Impl m_Subsystem.updCacheEntry(state, m_PosInfoIx)); } - void calcGeodesicInGround( - const LocalGeodesic::LocalGeodesicInfo& geodesic_S, - const Transform& X_GS, - PosInfo& posInfo) const; - void calcPosInfo(const State& state, PosInfo& posInfo) const; // TODO Required for accessing the cache variable? From 474451fdfe78f9f8425d6431dee19a3731321640 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 25 Apr 2024 18:31:14 +0200 Subject: [PATCH 032/127] add analyiticFormAvailable switch on LocalGeodesic --- .../simmath/internal/ContactGeometry.h | 68 ++++-- Simbody/include/simbody/internal/Wrapping.cpp | 198 +++++++++++------- .../include/simbody/internal/WrappingImpl.h | 34 ++- 3 files changed, 198 insertions(+), 102 deletions(-) diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h index 237cb5dff..fb53c3356 100644 --- a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h +++ b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h @@ -140,22 +140,62 @@ struct GeodesicBoundaryFrame { GeodesicVariation v_Q{}; }; -void calcNearestFrenetFrameFast(Vec3 x, Vec3 thint, FrenetFrame& X_BP) const; -void calcGeodesicStartFrameVariation(const FrenetFrame& X_BP, GeodesicVariation& dX_BP) const; -void calcGeodesicEndFrameVariationImplicitly( - Vec3 x, - UnitVec3 t, Real l, Real sHint, - FrenetFrame& X_BQ, - GeodesicVariation& dX_BQ, - std::vector& points) const; -void calcGeodesicEndFrameVariationAnalytically( Vec3 x, - UnitVec3 t, Real l, - FrenetFrame& X_BQ, - GeodesicVariation& dX_BQ) const; -void calcGeodesicPointsAnalytically(Vec3 x, UnitVec3 t, Real l, std::vector& points); bool analyticFormAvailable() const; -bool calcNearestPointOnLine(Vec3 a, Vec3 b, Vec3& point, size_t maxIter, double eps) const; +void calcGeodesicWithVariationAnalytically( + Vec3 xGuess, + Vec3 tGuess, + Real l, + FrenetFrame& X_P, + GeodesicVariation& dX_P, + FrenetFrame& X_Q, + GeodesicVariation& dX_Q) const; + +void resampleGeodesicPointsAnalytically( + const FrenetFrame& X_P, + const FrenetFrame& X_Q, + Real l, + size_t size, + std::vector& points) const; + +void resampleGeodesicFramesAnalytically( + const FrenetFrame& X_P, + const FrenetFrame& X_Q, + Real l, + size_t size, + std::vector& frames) const; + +void calcNearestFrenetFrameImplicitlyFast( + Vec3 xGuess, + Vec3 tGuess, + FrenetFrame& X_P, + size_t maxIter, + Real eps) const; + +void calcGeodesicStartFrameVariationImplicitly( + const FrenetFrame& X_P, + GeodesicVariation& dX_P) const; + +void calcGeodesicEndFrameVariationImplicitly( + const FrenetFrame& KP, + Real l, + FrenetFrame& X_Q, + GeodesicVariation& dX_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; // TODO class Cone; diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 9d6df5587..7b8cea14e 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -160,11 +160,12 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected( return g0; } -GeodesicInitialConditions GeodesicInitialConditions::CreateFromGroundInSurfaceFrame( - const Transform& X_GS, - Vec3 x_G, - Vec3 t_G, - Real l) +GeodesicInitialConditions GeodesicInitialConditions:: + CreateFromGroundInSurfaceFrame( + const Transform& X_GS, + Vec3 x_G, + Vec3 t_G, + Real l) { GeodesicInitialConditions g0; @@ -254,7 +255,8 @@ const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( return cache; } -void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) const +void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) + const { realizePosition(s); @@ -269,18 +271,24 @@ void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) shootNewGeodesic(g0, updCacheEntry(s)); } -size_t LocalGeodesic::calcPathPoints(const State& s, std::vector& points) const +void LocalGeodesic::calcPathPoints(const State& s, std::vector& points) + const { realizePosition(s); - - size_t count = 0; const CacheEntry& cache = getCacheEntry(s); - for (Vec3 p : cache.points) { - points.push_back(p); - ++count; + + if (analyticFormAvailable()) { + m_Geometry.resampleGeodesicPointsAnalytically( + cache.KP, + cache.KQ, + cache.length, + m_NumberOfAnalyticPoints, + points); + } else { + for (const FrenetFrame& frame : cache.frames) { + points.push_back(frame.p()); + } } - throw std::runtime_error("NOTYETIMPLEMENTED for analytic"); - return count; } //------------------------------------------------------------------------------ @@ -290,19 +298,38 @@ void LocalGeodesic::shootNewGeodesic( const GeodesicInitialConditions& g0, CacheEntry& cache) const { - // Compute geodesic start boundary frame and variation. - m_Geometry.calcNearestFrenetFrameFast(g0.x, g0.t, cache.KP); - m_Geometry.calcGeodesicStartFrameVariation(cache.KP, cache.dKP); - - // Compute geodesic end boundary frame amd variation (shoot new geodesic). - m_Geometry.calcGeodesicEndFrameVariationImplicitly( - cache.KP.p(), - cache.KP.R().getAxisUnitVec(ContactGeometry::TangentAxis), - g0.l, - cache.sHint, - cache.KQ, - cache.dKQ, - cache.points); + if (m_Geometry.analyticFormAvailable()) { + + m_Geometry.calcGeodesicWithVariationAnalytically( + g0.x, + g0.t, + g0.l, + cache.KP, + cache.dKP, + cache.KQ, + cache.dKQ); + } else { + // Compute geodesic start boundary frame and variation. + m_Geometry.calcNearestFrenetFrameImplicitlyFast( + g0.x, + g0.t, + cache.KP, + m_ProjectionMaxIter, + m_ProjectionRequiredAccuracy); + m_Geometry.calcGeodesicStartFrameVariationImplicitly( + cache.KP, + cache.dKP); + // Compute geodesic end boundary frame amd variation (shoot new + // geodesic). + m_Geometry.calcGeodesicEndFrameVariationImplicitly( + cache.KP, + g0.l, + cache.KQ, + cache.dKQ, + cache.sHint, + m_IntegratorAccuracy, + cache.frames); + } // TODO update step size. // TODO update line tracking? @@ -327,11 +354,8 @@ void LocalGeodesic::calcLiftoffIfNeeded( // For a zero-length curve, trigger liftoff when the prev and next points // lie above the surface plane. - if ( - dot(prev_QS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0. - && - dot(next_PS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0.) - { + if (dot(prev_QS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0. && + dot(next_PS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0.) { // No liftoff. return; } @@ -340,7 +364,6 @@ void LocalGeodesic::calcLiftoffIfNeeded( g.status = Status::Liftoff; // Initialize the tracking point from the last geodesic start point. cache.trackingPointOnLine = g.KP.p(); - } void LocalGeodesic::calcTouchdownIfNeeded( @@ -356,25 +379,31 @@ void LocalGeodesic::calcTouchdownIfNeeded( // Detect touchdown by computing the point on the line from x_QS to x_PS // that is nearest to the surface. - if(!m_Geometry.calcNearestPointOnLine( - prev_QS, - next_PS, - cache.trackingPointOnLine, - m_TouchdownIter, - m_TouchdownAccuracy)) - { - // No touchdown detected. + bool touchdownDetected; + if (m_Geometry.analyticFormAvailable()) { + touchdownDetected = m_Geometry.calcNearestPointOnLineAnalytically( + prev_QS, + next_PS, + cache.trackingPointOnLine); + } else { + touchdownDetected = m_Geometry.calcNearestPointOnLineImplicitly( + prev_QS, + next_PS, + cache.trackingPointOnLine, + m_TouchdownIter, + m_TouchdownAccuracy); + } + if (!touchdownDetected) { return; } // Touchdown detected: Remove the liftoff status flag. g.status = Status::Ok; // Shoot a zero length geodesic at the touchdown point. - GeodesicInitialConditions g0 = - GeodesicInitialConditions::CreateAtTouchdown( - prev_QS, - next_PS, - cache.trackingPointOnLine); + GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateAtTouchdown( + prev_QS, + next_PS, + cache.trackingPointOnLine); shootNewGeodesic(g0, cache); } @@ -383,9 +412,11 @@ void LocalGeodesic::assertSurfaceBounds( const Vec3& next_PS) const { // Make sure that the previous point does not lie inside the surface. - SimTK_ASSERT(m_Geometry.calcSurfaceValue(prev_QS) < 0., + SimTK_ASSERT( + m_Geometry.calcSurfaceValue(prev_QS) < 0., "Unable to wrap over surface: Preceding point lies inside the surface"); - SimTK_ASSERT(m_Geometry.calcSurfaceValue(next_PS) < 0., + SimTK_ASSERT( + m_Geometry.calcSurfaceValue(next_PS) < 0., "Unable to wrap over surface: Next point lies inside the surface"); } @@ -450,9 +481,7 @@ void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) m_Surface.getInitialPointGuess(); // TODO move into function call? GeodesicInitialConditions g0 = - GeodesicInitialConditions::CreateZeroLengthGuess( - prev_QS, - xGuess_S); + GeodesicInitialConditions::CreateZeroLengthGuess(prev_QS, xGuess_S); m_Surface.calcInitialGeodesic(s, g0); m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); @@ -469,24 +498,24 @@ void CurveSegment::Impl::applyGeodesicCorrection( m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } -size_t CurveSegment::Impl::calcPathPoints( +void CurveSegment::Impl::calcPathPoints( const State& s, std::vector& points) const { const Transform& X_GS = getPosInfo(s).X_GS; - size_t n = m_Surface.calcPathPoints(s, points); - for (size_t i = points.size() - n; i < points.size(); ++i) { + size_t initSize = points.size(); + m_Surface.calcPathPoints(s, points); + for (size_t i = points.size() - initSize; i < points.size(); ++i) { points.at(i) = X_GS.shiftFrameStationToBase(points.at(i)); } - return n; } namespace { void xformSurfaceGeodesicToGround( - const LocalGeodesicInfo& geodesic_S, - const Transform& X_GS, - CurveSegment::Impl::PosInfo& geodesic_G) + const LocalGeodesicInfo& geodesic_S, + const Transform& X_GS, + CurveSegment::Impl::PosInfo& geodesic_G) { geodesic_G.X_GS = X_GS; @@ -502,18 +531,22 @@ void xformSurfaceGeodesicToGround( // TODO use SpatialVec for variation. /* for (size_t i = 0; i < GeodesicDOF; ++i) { */ - /* geodesic_G.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) */ - /* geodesic_G.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) */ - - /* geodesic_G.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) */ - /* geodesic_G.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) */ + /* geodesic_G.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) + */ + /* geodesic_G.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) + */ + + /* geodesic_G.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) + */ + /* geodesic_G.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) + */ /* } */ geodesic_G.length = geodesic_S.length; throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); } -} +} // namespace void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { @@ -542,34 +575,41 @@ void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const xformSurfaceGeodesicToGround(geodesic_S, X_GS, updPosInfo(s)); } -SpatialVec CurveSegment::Impl::calcAppliedWrenchInGround(const State& s, Real tension) const +SpatialVec CurveSegment::Impl::calcAppliedWrenchInGround( + const State& s, + Real tension) const { const PosInfo& posInfo = getPosInfo(s); const UnitVec3& t_P = posInfo.KP.R().getAxisUnitVec(TangentAxis); const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); - const Vec3 F_G = tension * (t_Q - t_P); + const Vec3 F_G = tension * (t_Q - t_P); const Vec3 x_GS = posInfo.X_GS.p(); const Vec3& r_P = posInfo.KP.p() - x_GS; const Vec3& r_Q = posInfo.KQ.p() - x_GS; - const Vec3 M_G = tension * (r_Q % t_Q - r_P % t_P); + const Vec3 M_G = tension * (r_Q % t_Q - r_P % t_P); return {M_G, F_G}; } void CurveSegment::Impl::applyBodyForce( - const State& s, - Real tension, - Vector_& bodyForcesInG) const + const State& s, + Real tension, + Vector_& bodyForcesInG) const { // TODO why? - if (tension <= 0) {return;} + if (tension <= 0) { + return; + } if (getStatus(s) != Status::Ok) { return; } - m_Mobod.applyBodyForce(s, calcAppliedWrenchInGround(s, tension), bodyForcesInG); + m_Mobod.applyBodyForce( + s, + calcAppliedWrenchInGround(s, tension), + bodyForcesInG); } //============================================================================== @@ -937,10 +977,10 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Evaluate the path error jacobian. calcPathErrorJacobian<2>( - s, - data.lineSegments, - axes, - data.pathErrorJacobian); + s, + data.lineSegments, + axes, + data.pathErrorJacobian); // Compute path corrections. const Correction* corrIt = calcPathCorrections(data); @@ -1016,11 +1056,13 @@ void CableSpan::Impl::applyBodyForces( Vector_& bodyForcesInG) const { // TODO why? - if (tension <= 0) {return;} + if (tension <= 0) { + return; + } realizePosition(s); - for (const CurveSegment& segment: m_CurveSegments) { + for (const CurveSegment& segment : m_CurveSegments) { segment.getImpl().applyBodyForce(s, tension, bodyForcesInG); } } diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index ca1afdcef..aea20ea15 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -120,6 +120,11 @@ class CurveSegment::Impl Real l = NaN; }; + bool analyticFormAvailable() const + { + return m_Geometry.analyticFormAvailable(); + } + const LocalGeodesicInfo& calcInitialGeodesic( State& s, const GeodesicInitialConditions& g0) const; @@ -137,9 +142,11 @@ class CurveSegment::Impl // shoot a new geodesic, updating the cache variable. void applyGeodesicCorrection(const State& s, const Correction& c) const; - // Compute the path points of the current geodesic, and write them to the buffer. - // These points are in local surface coordinates. Returns the number of points written. - size_t calcPathPoints(const State& state, std::vector& points) const; + // Compute the path points of the current geodesic, and write them to + // the buffer. These points are in local surface coordinates. Returns + // the number of points written. + void calcPathPoints(const State& state, std::vector& points) + const; // Set the user defined point that controls the initial wrapping path. void setInitialPointGuess(Vec3 initPointGuess) @@ -166,8 +173,8 @@ class CurveSegment::Impl struct CacheEntry : LocalGeodesicInfo { Vec3 trackingPointOnLine{NaN, NaN, NaN}; - std::vector points{}; // Empty for analytic geoemetry with no - // allocation overhead. + std::vector frames{}; // Empty for analytic geoemetry + // with no allocation overhead. double sHint = NaN; }; @@ -200,9 +207,8 @@ class CurveSegment::Impl const Vec3& next_PS, CacheEntry& cache) const; - void assertSurfaceBounds( - const Vec3& prev_QS, - const Vec3& next_PS) const; + void assertSurfaceBounds(const Vec3& prev_QS, const Vec3& next_PS) + const; void calcTouchdownIfNeeded( const Vec3& prev_QS, @@ -225,9 +231,17 @@ class CurveSegment::Impl Vec3 m_InitPointGuess; + size_t m_ProjectionMaxIter = 10; + Real m_ProjectionRequiredAccuracy = 1e-10; + Real m_IntegratorAccuracy = 1e-6; + Real m_TouchdownAccuracy = 1e-3; size_t m_TouchdownIter = 10; + // TODO this must be a function argument such that the caller can + // decide, + size_t m_NumberOfAnalyticPoints = 10; + DiscreteVariableIndex m_CacheIx; }; @@ -295,7 +309,7 @@ class CurveSegment::Impl const State& state, const ContactGeometry::GeodesicCorrection& c) const; - size_t calcPathPoints(const State& state, std::vector& points) const; + void calcPathPoints(const State& state, std::vector& points) const; /* const MobilizedBody& getMobilizedBody() const {return m_Mobod;} */ void calcContactPointVelocitiesInGround( @@ -356,7 +370,7 @@ class CurveSegment::Impl MobilizedBody m_Mobod; Transform m_Offset; - LocalGeodesic m_Surface; + LocalGeodesic m_Surface; // TODO rename // TOPOLOGY CACHE CacheEntryIndex m_PosInfoIx; From 1757429dd15440ddc2dbb144aa94fec3bdd334eb Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 25 Apr 2024 19:18:59 +0200 Subject: [PATCH 033/127] add calcNearestFrenetFrameImplicitlyFast method --- .../simmath/internal/ContactGeometry.h | 40 ++--- SimTKmath/Geometry/src/ContactGeometry.cpp | 150 ++++++++++++++++++ SimTKmath/Geometry/src/ContactGeometryImpl.h | 69 ++++++++ 3 files changed, 234 insertions(+), 25 deletions(-) diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h index fb53c3356..b327fad95 100644 --- a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h +++ b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h @@ -118,69 +118,59 @@ class Cylinder; class Brick; class TriangleMesh; - -using FrenetFrame = Transform; - static const CoordinateAxis TangentAxis; static const CoordinateAxis NormalAxis; static const CoordinateAxis BinormalAxis; static constexpr int GEODESIC_DOF = 4; +using FrenetFrame = Transform; using GeodesicPointVariation = Mat34; using GeodesicFrameVariation = Mat34; using GeodesicVariation = std::array; using GeodesicCorrection = Vec4; -struct GeodesicBoundaryFrame { - FrenetFrame K_P{}; - FrenetFrame K_Q{}; - - GeodesicVariation v_P{}; - GeodesicVariation v_Q{}; -}; - bool analyticFormAvailable() const; void calcGeodesicWithVariationAnalytically( Vec3 xGuess, Vec3 tGuess, Real l, - FrenetFrame& X_P, - GeodesicVariation& dX_P, - FrenetFrame& X_Q, - GeodesicVariation& dX_Q) const; + FrenetFrame& K_P, + GeodesicVariation& dK_P, + FrenetFrame& K_Q, + GeodesicVariation& dK_Q) const; void resampleGeodesicPointsAnalytically( - const FrenetFrame& X_P, - const FrenetFrame& X_Q, + const FrenetFrame& K_P, + const FrenetFrame& K_Q, Real l, size_t size, std::vector& points) const; void resampleGeodesicFramesAnalytically( - const FrenetFrame& X_P, - const FrenetFrame& X_Q, + const FrenetFrame& K_P, + const FrenetFrame& K_Q, Real l, size_t size, std::vector& frames) const; -void calcNearestFrenetFrameImplicitlyFast( +size_t calcNearestFrenetFrameImplicitlyFast( Vec3 xGuess, Vec3 tGuess, - FrenetFrame& X_P, + FrenetFrame& K_P, size_t maxIter, Real eps) const; void calcGeodesicStartFrameVariationImplicitly( - const FrenetFrame& X_P, - GeodesicVariation& dX_P) const; + const FrenetFrame& K_P, + GeodesicVariation& dK_P) const; void calcGeodesicEndFrameVariationImplicitly( const FrenetFrame& KP, Real l, - FrenetFrame& X_Q, - GeodesicVariation& dX_Q, + FrenetFrame& K_Q, + GeodesicVariation& dK_Q, Real initStepSize, Real accuracy, std::vector& frames) const; diff --git a/SimTKmath/Geometry/src/ContactGeometry.cpp b/SimTKmath/Geometry/src/ContactGeometry.cpp index 22f13b40a..bf85df6df 100644 --- a/SimTKmath/Geometry/src/ContactGeometry.cpp +++ b/SimTKmath/Geometry/src/ContactGeometry.cpp @@ -181,6 +181,156 @@ 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 +//============================================================================== + +size_t ContactGeometryImpl::calcNearestFrenetFrameImplicitlyFast( + Vec3 xGuess, + Vec3 tGuess, + FrenetFrame& K_P, + size_t maxIter, + Real eps) const +{ + Vec3& x = xGuess; + size_t it = 0; + for (; it < maxIter; ++it) { + const double c = calcSurfaceValue(x); + + const double error = std::abs(c); + + if (error < eps) { + break; + } + + const Vec3 g = calcSurfaceGradient(x); + + x += -g * c / dot(g,g); + } + + Vec3 n = calcSurfaceGradient(x); + Vec3& t = tGuess; + if (!(abs(t % n) > 1e-13)) { + // TODO split NaN detection. + throw std::runtime_error("Surface projection failed: Tangent guess is parallel to surface normal, or there are NaNs..."); + } + + t = t - dot(n, t) * n / dot(n,n); + + K_P.updR().setRotationFromTwoAxes(n / n.norm(), NormalAxis, t, TangentAxis); + K_P.setP(x); + + return it; +} + +void ContactGeometryImpl::calcGeodesicStartFrameVariationImplicitly( + const FrenetFrame& K_P, + GeodesicVariation& dK_P) const +{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, + "ContactGeometryImpl", "calcGeodesicStartFrameVariationImplicitly"); } + +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"); } //------------------------------------------------------------------------------ // EVAL PARAMETRIC CURVATURE diff --git a/SimTKmath/Geometry/src/ContactGeometryImpl.h b/SimTKmath/Geometry/src/ContactGeometryImpl.h index 23bbf3576..8c01b3a98 100644 --- a/SimTKmath/Geometry/src/ContactGeometryImpl.h +++ b/SimTKmath/Geometry/src/ContactGeometryImpl.h @@ -89,6 +89,75 @@ 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"); } + // Smooth surfaces only. virtual void calcCurvature(const Vec3& point, Vec2& curvature, Rotation& orientation) const From 90cc2783e335cca3573d8b09cad9c8e1a695110f Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 26 Apr 2024 17:55:21 +0200 Subject: [PATCH 034/127] add implicit geodesic math to Wrapping.cpp --- .../simmath/internal/ContactGeometry.h | 13 + SimTKmath/Geometry/src/ContactGeometry.cpp | 36 +- SimTKmath/Geometry/src/ContactGeometryImpl.h | 7 + Simbody/include/simbody/internal/Wrapping.cpp | 482 +++++++++++++++++- 4 files changed, 526 insertions(+), 12 deletions(-) diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h index b327fad95..a1f8937e1 100644 --- a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h +++ b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h @@ -127,6 +127,16 @@ 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; @@ -187,6 +197,9 @@ bool calcNearestPointOnLineAnalytically( 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 bf85df6df..d4444e5fd 100644 --- a/SimTKmath/Geometry/src/ContactGeometry.cpp +++ b/SimTKmath/Geometry/src/ContactGeometry.cpp @@ -268,6 +268,11 @@ bool ContactGeometry::calcNearestPointOnLineAnalytically( // IMPLICIT METHODS //============================================================================== +// TODO is this right? +static const CoordinateAxis TangentAxis = CoordinateAxis::XCoordinateAxis(); +static const CoordinateAxis NormalAxis = CoordinateAxis::YCoordinateAxis(); +static const CoordinateAxis BinormalAxis = CoordinateAxis::ZCoordinateAxis(); + size_t ContactGeometryImpl::calcNearestFrenetFrameImplicitlyFast( Vec3 xGuess, Vec3 tGuess, @@ -291,16 +296,16 @@ size_t ContactGeometryImpl::calcNearestFrenetFrameImplicitlyFast( x += -g * c / dot(g,g); } - Vec3 n = calcSurfaceGradient(x); + UnitVec3 n (calcSurfaceGradient(x)); Vec3& t = tGuess; if (!(abs(t % n) > 1e-13)) { // TODO split NaN detection. throw std::runtime_error("Surface projection failed: Tangent guess is parallel to surface normal, or there are NaNs..."); } - t = t - dot(n, t) * n / dot(n,n); + t = t - dot(n, t) * n; - K_P.updR().setRotationFromTwoAxes(n / n.norm(), NormalAxis, t, TangentAxis); + K_P.updR().setRotationFromTwoAxes(n, NormalAxis, t, TangentAxis); K_P.setP(x); return it; @@ -308,9 +313,28 @@ size_t ContactGeometryImpl::calcNearestFrenetFrameImplicitlyFast( void ContactGeometryImpl::calcGeodesicStartFrameVariationImplicitly( const FrenetFrame& K_P, - GeodesicVariation& dK_P) const -{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, - "ContactGeometryImpl", "calcGeodesicStartFrameVariationImplicitly"); } + std::array& dK_P) const +{ + const UnitVec3& t = K_P.R().getAxisUnitVec(TangentAxis); + const UnitVec3& n = K_P.R().getAxisUnitVec(NormalAxis); + const UnitVec3& b = K_P.R().getAxisUnitVec(BinormalAxis); + + const Vec3& x = K_P.p(); + + dK_P.at(0)[1] = t; + dK_P.at(1)[1] = b; // * q.a; // a = 1 + dK_P.at(2)[1] = Vec3{0.}; // b * q.r; // r = 0 + dK_P.at(3)[1] = Vec3{0.}; // isEnd ? t : Vec3{0., 0., 0.}; + + const double tau_g = calcGeodesicTorsion(x, t); + const double kappa_n = calcNormalCurvature(x, t); + const double kappa_a = calcNormalCurvature(x, b); + + dK_P.at(0)[0] = tau_g * t + kappa_n * b; + dK_P.at(1)[0] = -kappa_a * t - tau_g * b; + dK_P.at(2)[0] = -n; + dK_P.at(3)[0] = Vec3{0.}; +} void ContactGeometryImpl::calcGeodesicEndFrameVariationImplicitly( const FrenetFrame& KP, diff --git a/SimTKmath/Geometry/src/ContactGeometryImpl.h b/SimTKmath/Geometry/src/ContactGeometryImpl.h index 8c01b3a98..e0f2f25c4 100644 --- a/SimTKmath/Geometry/src/ContactGeometryImpl.h +++ b/SimTKmath/Geometry/src/ContactGeometryImpl.h @@ -158,6 +158,13 @@ class SimTK_SIMMATH_EXPORT ContactGeometryImpl { { 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/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 7b8cea14e..bc4a5f1ae 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -125,10 +125,10 @@ const Correction* calcPathCorrections(SolverData& data) data.matInv.solve(data.vec, data.pathCorrection); static_assert( - sizeof(Correction) == sizeof(double) * GeodesicDOF, + sizeof(Correction) == sizeof(Real) * GeodesicDOF, "Invalid size of corrections vector"); SimTK_ASSERT( - data.pathCorrection.size() * sizeof(double) == n * sizeof(Correction), + data.pathCorrection.size() * sizeof(Real) == n * sizeof(Correction), "Invalid size of path corrections vector"); return reinterpret_cast(&data.pathCorrection[0]); } @@ -503,7 +503,7 @@ void CurveSegment::Impl::calcPathPoints( std::vector& points) const { const Transform& X_GS = getPosInfo(s).X_GS; - size_t initSize = points.size(); + size_t initSize = points.size(); m_Surface.calcPathPoints(s, points); for (size_t i = points.size() - initSize; i < points.size(); ++i) { points.at(i) = X_GS.shiftFrameStationToBase(points.at(i)); @@ -641,7 +641,7 @@ void addDirectionJacobian( addBlock(~dx * y, J); } -double calcPathError( +Real calcPathError( const LineSegment& e, const Rotation& R, CoordinateAxis axis) @@ -889,11 +889,11 @@ void CableSpan::Impl::calcPathErrorJacobian( }; } -double CableSpan::Impl::calcPathLength( +Real CableSpan::Impl::calcPathLength( const State& s, const std::vector& lines) const { - double lTot = 0.; + Real lTot = 0.; for (const LineSegment& line : lines) { // TODO spell out as length. lTot += line.l; @@ -1124,3 +1124,473 @@ CableSpan& CableSubsystem::updPath(WrappingPathIndex cableIx) { return updImpl().updCablePath(cableIx); } + +//============================================================================== +// GEODESIC +//============================================================================== + +namespace +{ + +struct ImplicitGeodesicState +{ + ImplicitGeodesicState() = default; + + ImplicitGeodesicState(Vec3 point, Vec3 tangent) : x(point), t(tangent){}; + + Vec3 x = {NaN, NaN, NaN}; + Vec3 t = {NaN, NaN, NaN}; + Real a = 1.; + Real aDot = 0.; + Real r = 0.; + Real rDot = 1.; +}; + +struct ImplicitGeodesicStateDerivative +{ + Vec3 xDot = {NAN, NAN, NAN}; + Vec3 tDot = {NAN, NAN, NAN}; + Real aDot = NAN; + Real aDDot = NAN; + Real rDot = NAN; + Real rDDot = NAN; +}; + +ImplicitGeodesicStateDerivative calcImplicitGeodesicStateDerivative( + const ImplicitGeodesicState& y, + const Vec3& acceleration, + Real gaussianCurvature) +{ + ImplicitGeodesicStateDerivative dy; + dy.xDot = y.t; + dy.tDot = acceleration; + dy.aDot = y.aDot; + dy.aDDot = -y.a * gaussianCurvature; + dy.rDot = y.rDot; + dy.rDDot = -y.r * gaussianCurvature; + return dy; +} + +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 = geometry.calcSurfaceValue(x); + + if (std::abs(c) < eps) { + break; + } + + const Vec3 g = geometry.calcSurfaceGradient(x); + x += -g * c / dot(g, g); + } + + SimTK_ASSERT( + it < maxIter, + "Surface projection failed: Reached max iterations"); + + UnitVec3 n(geometry.calcSurfaceGradient(x)); + t = t - dot(n, t) * n; + Real norm = t.norm(); + SimTK_ASSERT(!isNaN(norm), "Surface projection failed: Detected NaN"); + SimTK_ASSERT( + norm > 1e-13, + "Surface projection failed: Tangent guess is parallel to surface " + "normal"); + t = t / norm; +} + +ImplicitGeodesicState operator*( + Real dt, + const ImplicitGeodesicStateDerivative& dy) +{ + ImplicitGeodesicState y; + y.x = dt * dy.xDot; + y.t = dt * dy.tDot; + y.a = dt * dy.aDot; + y.aDot = dt * dy.aDDot; + y.r = dt * dy.rDot; + y.rDot = dt * dy.rDDot; + return y; +} + +ImplicitGeodesicState operator-( + const ImplicitGeodesicState& lhs, + const ImplicitGeodesicState& rhs) +{ + ImplicitGeodesicState y; + y.x = lhs.x - rhs.x; + y.t = lhs.t - rhs.t; + y.a = lhs.a - rhs.a; + y.aDot = lhs.aDot - rhs.aDot; + y.r = lhs.r - rhs.r; + y.rDot = lhs.rDot - rhs.rDot; + return y; +} + +ImplicitGeodesicState operator+( + const ImplicitGeodesicState& lhs, + const ImplicitGeodesicState& rhs) +{ + ImplicitGeodesicState y; + y.x = lhs.x + rhs.x; + y.t = lhs.t + rhs.t; + y.a = lhs.a + rhs.a; + y.aDot = lhs.aDot + rhs.aDot; + y.r = lhs.r + rhs.r; + y.rDot = lhs.rDot + rhs.rDot; + return y; +} + +Real calcInfNorm(const ImplicitGeodesicState& q) { + Real infNorm = 0.; + + infNorm = std::max(infNorm, q.x[0]); + infNorm = std::max(infNorm, q.x[1]); + infNorm = std::max(infNorm, q.x[2]); + + infNorm = std::max(infNorm, q.t[0]); + infNorm = std::max(infNorm, q.t[1]); + infNorm = std::max(infNorm, q.t[2]); + + infNorm = std::max(infNorm, q.a); + infNorm = std::max(infNorm, q.aDot); + + infNorm = std::max(infNorm, q.r); + infNorm = std::max(infNorm, q.rDot); + + return infNorm; +} + +class RKM +{ + using Y = ImplicitGeodesicState; + using DY = ImplicitGeodesicStateDerivative; + +public: + RKM() = default; + + // Integrate y0, populating y1, y2. + Real step(Real h, std::function& f); + + struct Sample + { + Real l; + FrenetFrame K; + }; + + Real 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; + } + + size_t getInitStepSize() const + { + return _h0; + } + +private: + static constexpr size_t ORDER = 5; + + std::array _k{}; + 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); + + // 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; +} + +Real 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 = h0; + _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); + + SimTK_ASSERT( + _h > _hMin, + "Geodesic Integrator failed: Reached very small stepsize"); + SimTK_ASSERT( + _h < _hMax, + "Geodesic Integrator failed: Reached very large stepsize"); + + if (init) { + _h0 = _h; + } + } +} + +Real calcNormalCurvature( + const ContactGeometry& geometry, + Vec3 point, + Vec3 tangent) +{ + const Vec3& p = point; + const Vec3& v = tangent; + const Vec3 g = geometry.calcSurfaceGradient(p); + const Vec3 h_v = geometry.calcSurfaceHessian(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 = geometry.calcSurfaceGradient(p); + const Vec3 h_v = geometry.calcSurfaceHessian(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 = geometry.calcSurfaceGradient(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 = geometry.calcSurfaceGradient(p); + Real gDotg = dot(g, g); + Mat33 adj = calcAdjoint(geometry.calcSurfaceHessian(p)); + + if (gDotg * gDotg < 1e-13) { + 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) = 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 = geometry.calcGeodesicTorsion(q.x, t); + const Real kappa_n = geometry.calcNormalCurvature(q.x, t); + const Real kappa_a = geometry.calcNormalCurvature(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 = ImplicitGeodesicStateDerivative; + + std::function f = [&](const Y& q) -> DY { + return calcImplicitGeodesicStateDerivative( + q, + 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) { + log.emplace_back(l, q); + }; + + Y y0(x, t); + + RKM rkm{}; + + rkm.stepTo(y0, l, ds, f, g, m, accuracy); + + SimTK_ASSERT(log.size() > 0, "Failed to integrate geodesic: Log is empty"); + calcGeodesicBoundaryState(geometry, log.front().second, false, K_P, dK_P[1], dK_P[0]); + calcGeodesicBoundaryState(geometry, log.back().second, true, K_Q, dK_Q[1], dK_Q[0]); + + ds = rkm.getInitStepSize(); +} + +} // namespace From 1a1dcf317db8d7ffdc1e19cb766536e3d2b785a5 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 29 Apr 2024 09:24:32 +0200 Subject: [PATCH 035/127] update implicit geodesic equations --- Simbody/include/simbody/internal/Wrapping.cpp | 167 ++++++++---------- 1 file changed, 76 insertions(+), 91 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index bc4a5f1ae..c945d8667 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -641,10 +641,7 @@ void addDirectionJacobian( addBlock(~dx * y, J); } -Real calcPathError( - const LineSegment& e, - const Rotation& R, - CoordinateAxis axis) +Real calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) { return dot(e.d, R.getAxisUnitVec(axis)); } @@ -1136,8 +1133,39 @@ 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.; @@ -1146,31 +1174,6 @@ struct ImplicitGeodesicState Real rDot = 1.; }; -struct ImplicitGeodesicStateDerivative -{ - Vec3 xDot = {NAN, NAN, NAN}; - Vec3 tDot = {NAN, NAN, NAN}; - Real aDot = NAN; - Real aDDot = NAN; - Real rDot = NAN; - Real rDDot = NAN; -}; - -ImplicitGeodesicStateDerivative calcImplicitGeodesicStateDerivative( - const ImplicitGeodesicState& y, - const Vec3& acceleration, - Real gaussianCurvature) -{ - ImplicitGeodesicStateDerivative dy; - dy.xDot = y.t; - dy.tDot = acceleration; - dy.aDot = y.aDot; - dy.aDDot = -y.a * gaussianCurvature; - dy.rDot = y.rDot; - dy.rDDot = -y.r * gaussianCurvature; - return dy; -} - void calcSurfaceProjectionFast( const ContactGeometry& geometry, Vec3& x, @@ -1205,72 +1208,37 @@ void calcSurfaceProjectionFast( t = t / norm; } -ImplicitGeodesicState operator*( - Real dt, - const ImplicitGeodesicStateDerivative& dy) -{ - ImplicitGeodesicState y; - y.x = dt * dy.xDot; - y.t = dt * dy.tDot; - y.a = dt * dy.aDot; - y.aDot = dt * dy.aDDot; - y.r = dt * dy.rDot; - y.rDot = dt * dy.rDDot; - return y; -} - ImplicitGeodesicState operator-( const ImplicitGeodesicState& lhs, const ImplicitGeodesicState& rhs) { - ImplicitGeodesicState y; - y.x = lhs.x - rhs.x; - y.t = lhs.t - rhs.t; - y.a = lhs.a - rhs.a; - y.aDot = lhs.aDot - rhs.aDot; - y.r = lhs.r - rhs.r; - y.rDot = lhs.rDot - rhs.rDot; - return y; + ImplicitGeodesicState out; + out.asVecMut() = lhs.asVec() - rhs.asVec(); + return out; } ImplicitGeodesicState operator+( const ImplicitGeodesicState& lhs, - const ImplicitGeodesicState& rhs) + const Vec<10, Real>& rhs) { - ImplicitGeodesicState y; - y.x = lhs.x + rhs.x; - y.t = lhs.t + rhs.t; - y.a = lhs.a + rhs.a; - y.aDot = lhs.aDot + rhs.aDot; - y.r = lhs.r + rhs.r; - y.rDot = lhs.rDot + rhs.rDot; - return y; + ImplicitGeodesicState out; + out.asVecMut() = lhs.asVec() + rhs; + return out; } -Real calcInfNorm(const ImplicitGeodesicState& q) { +Real calcInfNorm(const ImplicitGeodesicState& q) +{ Real infNorm = 0.; - - infNorm = std::max(infNorm, q.x[0]); - infNorm = std::max(infNorm, q.x[1]); - infNorm = std::max(infNorm, q.x[2]); - - infNorm = std::max(infNorm, q.t[0]); - infNorm = std::max(infNorm, q.t[1]); - infNorm = std::max(infNorm, q.t[2]); - - infNorm = std::max(infNorm, q.a); - infNorm = std::max(infNorm, q.aDot); - - infNorm = std::max(infNorm, q.r); - infNorm = std::max(infNorm, q.rDot); - + for (size_t r = 0; r < q.asVec().nrow(); ++r) { + infNorm = std::max(infNorm, q.asVec()[r]); + } return infNorm; } class RKM { using Y = ImplicitGeodesicState; - using DY = ImplicitGeodesicStateDerivative; + using DY = Vec<10, Real>; public: RKM() = default; @@ -1322,6 +1290,10 @@ 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)); @@ -1497,7 +1469,7 @@ Real calcGaussianCurvature(const ContactGeometry& geometry, Vec3 point) { const Vec3& p = point; Vec3 g = geometry.calcSurfaceGradient(p); - Real gDotg = dot(g, g); + Real gDotg = dot(g, g); Mat33 adj = calcAdjoint(geometry.calcSurfaceHessian(p)); if (gDotg * gDotg < 1e-13) { @@ -1508,12 +1480,17 @@ Real calcGaussianCurvature(const ContactGeometry& geometry, Vec3 point) return (dot(g, adj * g)) / (gDotg * gDotg); } -void calcFrenetFrame(const ContactGeometry& geometry, const ImplicitGeodesicState& q, FrenetFrame& K) +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); + calcSurfaceNormal(geometry, q.x), + NormalAxis, + q.t, + TangentAxis); } void calcGeodesicBoundaryState( @@ -1541,11 +1518,8 @@ void calcGeodesicBoundaryState( const Real kappa_a = geometry.calcNormalCurvature(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(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.}; } @@ -1565,11 +1539,10 @@ void calcGeodesicAndVariationImplicitly( std::vector>& log) { using Y = ImplicitGeodesicState; - using DY = ImplicitGeodesicStateDerivative; + using DY = Vec<10, Real>; std::function f = [&](const Y& q) -> DY { - return calcImplicitGeodesicStateDerivative( - q, + return q.calcDerivativeVector( calcAcceleration(geometry, q.x, q.t), calcGaussianCurvature(geometry, q.x)); }; @@ -1587,8 +1560,20 @@ void calcGeodesicAndVariationImplicitly( rkm.stepTo(y0, l, ds, f, g, m, accuracy); SimTK_ASSERT(log.size() > 0, "Failed to integrate geodesic: Log is empty"); - calcGeodesicBoundaryState(geometry, log.front().second, false, K_P, dK_P[1], dK_P[0]); - calcGeodesicBoundaryState(geometry, log.back().second, true, K_Q, dK_Q[1], dK_Q[0]); + calcGeodesicBoundaryState( + geometry, + log.front().second, + false, + K_P, + dK_P[1], + dK_P[0]); + calcGeodesicBoundaryState( + geometry, + log.back().second, + true, + K_Q, + dK_Q[1], + dK_Q[0]); ds = rkm.getInitStepSize(); } From 8a656549b7b678eea461caf36b637455789f1788 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 29 Apr 2024 10:42:10 +0200 Subject: [PATCH 036/127] update CurveSegment api --- Simbody/include/simbody/internal/Wrapping.cpp | 35 ++++++++++- Simbody/include/simbody/internal/Wrapping.h | 58 +++++++++--------- .../include/simbody/internal/WrappingImpl.h | 61 +++++++++++-------- 3 files changed, 96 insertions(+), 58 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index c945d8667..d181cebdc 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -7,6 +7,7 @@ #include "simmath/internal/ContactGeometry.h" #include #include +#include using namespace SimTK; @@ -437,7 +438,39 @@ void LocalGeodesic::calcCacheEntry( } //============================================================================== -// OBSTACLE +// CURVE SEGMENT +//============================================================================== + +CurveSegment::CurveSegment( + CableSpan cable, + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 xHint) + : m_Impl(std::shared_ptr(new CurveSegment::Impl(cable, mobod, X_BS, geometry, xHint))) +{ + updImpl().setIndex(cable.adoptSegment(*this)); +} + +const CableSpan& CurveSegment::getCable() const +{ + return getImpl().getCable(); +} + +Real CurveSegment::getSegmentLength(const State& s) +{ + getImpl().realizeCablePosition(s); + return getImpl().getPosInfo(s).length; +} + +CurveSegment::Status CurveSegment::getStatus(const State& s) const +{ + getImpl().realizeCablePosition(s); + return getImpl().getStatus(s); +} + +//============================================================================== +// CURVE SEGMENT IMPL //============================================================================== CurveSegment::Impl::Impl( diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 8816926dc..5afbdb76d 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -15,8 +15,8 @@ namespace SimTK { -SimTK_DEFINE_UNIQUE_INDEX_TYPE(WrappingPathIndex); -SimTK_DEFINE_UNIQUE_INDEX_TYPE(WrapObstacleIndex); +SimTK_DEFINE_UNIQUE_INDEX_TYPE(CurveSegmentIndex); +SimTK_DEFINE_UNIQUE_INDEX_TYPE(CableSpanIndex); class MultibodySystem; class CableSubsystem; @@ -31,20 +31,19 @@ class SimTK_SIMBODY_EXPORT CurveSegment { public: CurveSegment() = default; - CurveSegment(const CurveSegment&) = delete; - CurveSegment& operator=(const CurveSegment&) = delete; + CurveSegment(const CurveSegment&) = default; + CurveSegment& operator=(const CurveSegment&) = default; CurveSegment(CurveSegment&&) noexcept = default; CurveSegment& operator=(CurveSegment&&) noexcept = default; ~CurveSegment() = default; CurveSegment( + CableSpan cable, const MobilizedBody& mobod, Transform X_BS, const ContactGeometry& geometry, Vec3 xHint); - class Impl; - enum class Status { Ok, @@ -53,36 +52,33 @@ class SimTK_SIMBODY_EXPORT CurveSegment }; Real getSegmentLength(const State& s); - /* { */ - /* getImpl().getSubsystem().realizePosition(s); */ - /* return getImpl().getLength(s); */ - /* } */ Status getStatus(const State& state) const; - void setDisabled(const State& state) const; - void setEnabled(const State& state) const; - const MobilizedBody& getMobilizedBody(const State& state) const; + // TODO seems useful: + /* void setDisabled(const State& state) const; */ + /* void setEnabled(const State& state) const; */ - int calcSegmentPoints(const State& state, std::vector& points); - int calcSegmentFrenetFrames( - const State& state, - std::vector& frames); + /* const MobilizedBody& getMobilizedBody(const State& state) const; */ -private: - explicit CurveSegment(std::unique_ptr impl); + /* int calcSegmentPoints(const State& state, std::vector& points); */ + /* int calcSegmentFrenetFrames( */ + /* const State& state, */ + /* std::vector& frames); */ + class Impl; + +private: friend CableSpan; - const Impl& getImpl() const; - Impl& updImpl(); + const Impl& getImpl() const {return *m_Impl;} + Impl& updImpl() {return *m_Impl;} - std::unique_ptr impl; + std::shared_ptr m_Impl = nullptr; }; //============================================================================== -// PATH - or SPAN +// CABLE SPAN //============================================================================== -// Cable, wire, rope, cord class SimTK_SIMBODY_EXPORT CableSpan { public: @@ -100,6 +96,8 @@ class SimTK_SIMBODY_EXPORT CableSpan const MobilizedBody& terminationBody, const Vec3& defaultTerminationPoint); + CurveSegmentIndex adoptSegment(const CurveSegment& segment); + std::vector& updObstacles(); const std::vector& getObstacles(); @@ -119,18 +117,18 @@ class SimTK_SIMBODY_EXPORT CableSpan class Impl; private: - explicit CableSpan(std::unique_ptr impl); + explicit CableSpan(std::unique_ptr m_Impl); const Impl& getImpl() const { - return *impl; + return *m_Impl; } Impl& updImpl() { - return *impl; + return *m_Impl; } - std::shared_ptr impl = nullptr; + std::shared_ptr m_Impl = nullptr; friend CurveSegment::Impl; friend CableSubsystem; @@ -148,8 +146,8 @@ class SimTK_SIMBODY_EXPORT CableSubsystem : public Subsystem explicit CableSubsystem(MultibodySystem&); int getNumPaths() const; - const CableSpan& getPath(WrappingPathIndex idx) const; - CableSpan& updPath(WrappingPathIndex idx); + const CableSpan& getPath(CableSpanIndex idx) const; + CableSpan& updPath(CableSpanIndex idx); size_t writePathPoints(std::vector& points) const; size_t writePathFrames(std::vector& frenetFrames) const; diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index aea20ea15..ce88568cc 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -15,34 +15,17 @@ namespace SimTK { -SimTK_DEFINE_UNIQUE_INDEX_TYPE(CurveSegmentIndex); -SimTK_DEFINE_UNIQUE_INDEX_TYPE(PathIndex); - //============================================================================== // ??? IMPL //============================================================================== class CurveSegment::Impl { + public: + using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; -private: - Impl() = default; - -public: - Impl(const Impl& source) = default; - Impl& operator=(const Impl& source) = default; - ~Impl() = default; - - Impl( - CableSpan cable, - CurveSegmentIndex ix, - const MobilizedBody& mobod, - const Transform& X_BS, - ContactGeometry geometry, - Vec3 initPointGuess); - //============================================================================== // ??? //============================================================================== @@ -245,6 +228,22 @@ class CurveSegment::Impl DiscreteVariableIndex m_CacheIx; }; +private: + Impl() = default; + +public: + Impl(const Impl& source) = default; + Impl& operator=(const Impl& source) = default; + ~Impl() = default; + + Impl( + CableSpan path, + CurveSegmentIndex ix, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess); + // Position level cache: Curve in ground frame. struct PosInfo { @@ -267,15 +266,23 @@ class CurveSegment::Impl void realizeTopology(State& state); /* void realizeInstance(const State& state) const; */ void realizePosition(const State& state) const; + /* void realizeVelocity(const State& state) const; */ /* void realizeAcceleration(const State& state) const; */ /* void invalidateTopology(); */ + void realizeCablePosition(const State& state) const; + void invalidatePositionLevelCache(const State& state) const { m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); } + const CableSpan& getCable() const + { + return m_Path; + } + CurveSegmentIndex getIndex() const { return m_Index; @@ -542,21 +549,21 @@ class CableSubsystem::Impl : public Subsystem::Guts return cables.size(); } - const CableSpan& getCablePath(WrappingPathIndex index) const + const CableSpan& getCablePath(CableSpanIndex index) const { return cables[index]; } - CableSpan& updCablePath(WrappingPathIndex index) + CableSpan& updCablePath(CableSpanIndex index) { return cables[index]; } // Add a cable path to the list, bumping the reference count. - WrappingPathIndex adoptCablePath(CableSpan& path) + CableSpanIndex adoptCablePath(CableSpan& path) { cables.push_back(path); - return WrappingPathIndex(cables.size() - 1); + return CableSpanIndex(cables.size() - 1); } // Return the MultibodySystem which owns this WrappingPathSubsystem. @@ -579,7 +586,7 @@ class CableSubsystem::Impl : public Subsystem::Guts // Topology cache is const. Impl* wThis = const_cast(this); - for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + for (CableSpanIndex ix(0); ix < cables.size(); ++ix) { CableSpan& path = wThis->updCablePath(ix); path.updImpl().realizeTopology(state); } @@ -589,7 +596,7 @@ class CableSubsystem::Impl : public Subsystem::Guts int realizeSubsystemPositionImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + for (CableSpanIndex ix(0); ix < cables.size(); ++ix) { const CableSpan& path = getCablePath(ix); path.getImpl().realizePosition(state); } @@ -598,7 +605,7 @@ class CableSubsystem::Impl : public Subsystem::Guts int realizeSubsystemVelocityImpl(const State& state) const override { - for (WrappingPathIndex ix(0); ix < cables.size(); ++ix) { + for (CableSpanIndex ix(0); ix < cables.size(); ++ix) { const CableSpan& path = getCablePath(ix); path.getImpl().realizeVelocity(state); } @@ -609,7 +616,7 @@ class CableSubsystem::Impl : public Subsystem::Guts private: // TOPOLOGY STATE - Array_ cables; + Array_ cables; }; } // namespace SimTK From 021f9eceb8f5bf03038663f33832cefa767dfd88 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 29 Apr 2024 11:11:03 +0200 Subject: [PATCH 037/127] wip: updates Wrapping.h function bodies --- Simbody/include/simbody/internal/Wrapping.cpp | 63 ++++++++++++++++--- Simbody/include/simbody/internal/Wrapping.h | 19 +++--- .../include/simbody/internal/WrappingImpl.h | 12 ++-- 3 files changed, 72 insertions(+), 22 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index d181cebdc..ae2d95e81 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -475,13 +475,13 @@ CurveSegment::Status CurveSegment::getStatus(const State& s) const CurveSegment::Impl::Impl( CableSpan path, - CurveSegmentIndex ix, const MobilizedBody& mobod, const Transform& X_BS, ContactGeometry geometry, Vec3 initPointGuess) : m_Subsystem(path.getImpl().getSubsystem()), - m_Path(path), m_Index(ix), m_Mobod(mobod), m_Offset(X_BS), + m_Path(path), m_Index(-1), // TODO what to do with this index, and when + m_Mobod(mobod), m_Offset(X_BS), m_Surface(m_Subsystem, geometry, initPointGuess) {} @@ -693,7 +693,56 @@ GeodesicJacobian addPathErrorJacobian( } // namespace //============================================================================== -// PATH IMPL +// CABLE SPAN +//============================================================================== + +CableSpan::CableSpan( + CableSubsystem& subsystem, + const MobilizedBody& originBody, + const Vec3& defaultOriginPoint, + const MobilizedBody& terminationBody, + const Vec3& defaultTerminationPoint) : + m_Impl(std::shared_ptr(new Impl( + subsystem, + originBody, + defaultOriginPoint, + terminationBody, + defaultTerminationPoint))) {} + +CurveSegmentIndex CableSpan::adoptSegment(const CurveSegment& segment) +{ + return updImpl().adoptSegment(segment); +} + +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); +} + +//============================================================================== +// CABLE SPAN IMPL //============================================================================== void CableSpan::Impl::realizeTopology(State& s) @@ -1145,14 +1194,14 @@ int CableSubsystem::getNumPaths() const return getImpl().getNumPaths(); } -const CableSpan& CableSubsystem::getPath(WrappingPathIndex cableIx) const +const CableSpan& CableSubsystem::getPath(CableSpanIndex ix) const { - return getImpl().getCablePath(cableIx); + return getImpl().getCablePath(ix); } -CableSpan& CableSubsystem::updPath(WrappingPathIndex cableIx) +CableSpan& CableSubsystem::updPath(CableSpanIndex ix) { - return updImpl().updCablePath(cableIx); + return updImpl().updCablePath(ix); } //============================================================================== diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 5afbdb76d..b6d2505b0 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -51,6 +51,9 @@ class SimTK_SIMBODY_EXPORT CurveSegment Disabled, }; + // TODO remove? keep? make private? + const CableSpan& getCable() const; + Real getSegmentLength(const State& s); Status getStatus(const State& state) const; @@ -98,8 +101,9 @@ class SimTK_SIMBODY_EXPORT CableSpan CurveSegmentIndex adoptSegment(const CurveSegment& segment); - std::vector& updObstacles(); - const std::vector& getObstacles(); + int getNumCurveSegments() const; + + const CurveSegment& getCurveSegment(CurveSegmentIndex ix) const; Real getLength(const State& state) const; Real getLengthDot(const State& state) const; @@ -109,20 +113,19 @@ class SimTK_SIMBODY_EXPORT CableSpan Real tension, Vector_& bodyForcesInG) const; - int calcPathPoints(const State& state, std::vector& points); - int calcPathFrenetFrames( - const State& state, - std::vector& frames); + /* int calcPathPoints(const State& state, std::vector& points); */ + /* int calcPathFrenetFrames( */ + /* const State& state, */ + /* std::vector& frames); */ class Impl; private: - explicit CableSpan(std::unique_ptr m_Impl); - const Impl& getImpl() const { return *m_Impl; } + Impl& updImpl() { return *m_Impl; diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index ce88568cc..8ff89553b 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -228,17 +228,15 @@ class CurveSegment::Impl DiscreteVariableIndex m_CacheIx; }; -private: - Impl() = default; - public: - Impl(const Impl& source) = default; - Impl& operator=(const Impl& source) = default; + Impl() = delete; + Impl(const Impl& source) = delete; + Impl& operator=(const Impl& source) = delete; ~Impl() = default; + // TODO you would expect the constructor to take the index as well here? Impl( CableSpan path, - CurveSegmentIndex ix, const MobilizedBody& mobod, const Transform& X_BS, ContactGeometry geometry, @@ -414,7 +412,7 @@ class CableSpan::Impl return m_CurveSegments[ix]; } - CurveSegmentIndex adoptObstacle(CurveSegment& segment) + CurveSegmentIndex adoptSegment(const CurveSegment& segment) { m_CurveSegments.push_back(segment); return CurveSegmentIndex(m_CurveSegments.size() - 1); From ccb73f43ee082f06ff2d73d8a0667a7c8c6a96cb Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 29 Apr 2024 13:50:51 +0200 Subject: [PATCH 038/127] wip: Refactor LocalGeodesic method bodies --- Simbody/include/simbody/internal/Wrapping.cpp | 2578 +++++++++-------- .../include/simbody/internal/WrappingImpl.h | 36 +- 2 files changed, 1359 insertions(+), 1255 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index ae2d95e81..6896d1aa2 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -16,6 +16,7 @@ using FrameVariation = ContactGeometry::GeodesicFrameVariation; using FrenetFrame = ContactGeometry::FrenetFrame; using GeodesicInfo = CurveSegment::Impl::PosInfo; using LocalGeodesicInfo = CurveSegment::Impl::LocalGeodesic::LocalGeodesicInfo; +using LocalGeodesicSample = CurveSegment::Impl::LocalGeodesic::LocalGeodesicSample; using GeodesicInitialConditions = CurveSegment::Impl::LocalGeodesic::GeodesicInitialConditions; using GeodesicJacobian = Vec4; @@ -36,1100 +37,1153 @@ static const int GeodesicDOF = 4; } // namespace //============================================================================== -// SOLVER +// GEODESIC MATH TODO move to contact geometry //============================================================================== namespace { -class SolverDataCache; -SolverDataCache& findDataCache(size_t nActive); -struct SolverData +struct ImplicitGeodesicState { - std::vector lineSegments; - - Matrix pathErrorJacobian; - Vector pathCorrection; - Vector pathError; - Matrix mat; - // TODO Cholesky decomposition... - FactorLU matInv; - Vector vec; -}; + ImplicitGeodesicState() = default; -class SolverDataCache -{ -private: - SolverDataCache(size_t n) + explicit ImplicitGeodesicState( + Vec<10, Real>&& implicitGeodesicStateAsVector) { - static constexpr int Q = 4; - static constexpr int C = 4; - - m_Data.lineSegments.resize(n + 1); - m_Data.pathErrorJacobian = Matrix(C * n, Q * n, 0.); - m_Data.pathCorrection = Vector(Q * n, 0.); - m_Data.pathError = Vector(C * n, 0.); - m_Data.mat = Matrix(Q * n, Q * n, NaN); - m_Data.vec = Vector(Q * n, NaN); + asVecMut() = implicitGeodesicStateAsVector; } -public: - void lock() + ImplicitGeodesicState(Vec3 point, Vec3 tangent) : x(point), t(tangent){}; + + const Vec<10, Real>& asVec() const { - m_Mutex.lock(); + return reinterpret_cast&>(x[0]); } - void unlock() + Vec<10, Real>& asVecMut() { - m_Mutex.unlock(); + return reinterpret_cast&>(x[0]); } - SolverData& updData() + Vec<10, Real> calcDerivativeVector( + const Vec3& acceleration, + Real gaussianCurvature) const { - return m_Data; + 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()}; + ; } -private: - SolverData m_Data; - std::mutex m_Mutex{}; - - friend SolverDataCache& findDataCache(size_t nActive); + Vec3 x = {NaN, NaN, NaN}; + Vec3 t = {NaN, NaN, NaN}; + Real a = 1.; + Real aDot = 0.; + Real r = 0.; + Real rDot = 1.; }; -SolverDataCache& findDataCache(size_t nActive) +void calcSurfaceProjectionFast( + const ContactGeometry& geometry, + Vec3& x, + Vec3& t, + size_t maxIter, + Real eps) { - static std::vector s_GlobalCache{}; - static std::mutex s_GlobalLock{}; - - { - std::lock_guard lock(s_GlobalLock); + size_t it = 0; + for (; it < maxIter; ++it) { + const Real c = geometry.calcSurfaceValue(x); - for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { - int n = i + 1; - s_GlobalCache.emplace_back(nActive); + if (std::abs(c) < eps) { + break; } - } - - return s_GlobalCache.at(nActive - 1); -} - -const Correction* calcPathCorrections(SolverData& data) -{ - Real w = data.pathError.normInf(); - data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; - for (int i = 0; i < data.mat.nrow(); ++i) { - data.mat[i][i] += w; + const Vec3 g = geometry.calcSurfaceGradient(x); + x += -g * c / dot(g, g); } - data.matInv = data.mat; - data.vec = data.pathErrorJacobian.transpose() * data.pathError; - data.matInv.solve(data.vec, data.pathCorrection); - static_assert( - sizeof(Correction) == sizeof(Real) * GeodesicDOF, - "Invalid size of corrections vector"); SimTK_ASSERT( - data.pathCorrection.size() * sizeof(Real) == n * sizeof(Correction), - "Invalid size of path corrections vector"); - return reinterpret_cast(&data.pathCorrection[0]); -} -} // namespace + it < maxIter, + "Surface projection failed: Reached max iterations"); -//============================================================================== -// GEODESIC INITIAL CONDITIONS -//============================================================================== + UnitVec3 n(geometry.calcSurfaceGradient(x)); + t = t - dot(n, t) * n; + Real norm = t.norm(); + SimTK_ASSERT(!isNaN(norm), "Surface projection failed: Detected NaN"); + SimTK_ASSERT( + norm > 1e-13, + "Surface projection failed: Tangent guess is parallel to surface " + "normal"); + t = t / norm; +} -GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected( - const FrenetFrame& KP, - const Variation& dKP, - Real l, - const Correction& c) +Real calcPointOnLineNearOriginAsFactor(Vec3 a, Vec3 b) { - GeodesicInitialConditions g0; + const Vec3 e = b - a; + Real c = -dot(a,e) / dot(e,e); + return std::max(0., std::min(1., c)); +}; - Vec3 v = dKP[1] * c; - g0.x = KP.p() + v; +Real calcPointOnLineNearPointAsFactor(Vec3 a, Vec3 b, Vec3 point) +{ + return calcPointOnLineNearOriginAsFactor(a - point, b - point); +}; - Vec3 w = dKP[0] * c; - const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); - g0.t = t + cross(w, t); +bool calcNearestPointOnLineImplicitly( + const ContactGeometry& geometry, + Vec3 a, + Vec3 b, + Vec3& point, + size_t maxIter, + double eps) +{ + // Initial guess. + double alpha = calcPointOnLineNearPointAsFactor(a, b, point); + size_t iter = 0; + + for (; iter < maxIter; ++iter) { + // Touchdown point on line. + const Vec3 d = b - a; + const Vec3 pl = a + (b - a) * alpha; + + // Constraint evaluation at touchdown point. + const double c = geometry.calcSurfaceValue(pl); + + // Break on touchdown, TODO or not? + if (std::abs(c) < eps) + break; - // Take the length correction, and add to the current length. - Real dl = c[3]; // Length increment is the last correction element. - g0.l = std::max(l + dl, 0.); // Clamp length to be nonnegative. + // Gradient at point on line. + const Vec3 g = geometry.calcSurfaceGradient(pl); + const Mat33 H = geometry.calcSurfaceHessian(pl); - return g0; -} + // Add a weight to the newton step to avoid large steps. + constexpr double w = 0.5; -GeodesicInitialConditions GeodesicInitialConditions:: - CreateFromGroundInSurfaceFrame( - const Transform& X_GS, - Vec3 x_G, - Vec3 t_G, - Real l) -{ - GeodesicInitialConditions g0; + // Update alpha. + const double step = dot(g,d) / (dot(d,H * d) + w); - g0.x = X_GS.shiftBaseStationToFrame(x_G); - g0.t = X_GS.xformBaseVecToFrame(t_G); - g0.l = l; + // Stop when converged. + if (std::abs(step) < eps) + break; - return g0; -} + // Clamp the stepsize. + constexpr double maxStep = 0.25; + alpha -= std::min(std::max(-maxStep, step), maxStep); -GeodesicInitialConditions GeodesicInitialConditions::CreateZeroLengthGuess( - Vec3 prev_QS, - Vec3 xGuess_S) -{ - GeodesicInitialConditions g0; + // Stop when leaving bounds. + if (alpha < 0. || alpha > 1.) + break; + } - g0.x = xGuess_S; - g0.t = xGuess_S - prev_QS; - g0.l = 0.; + // Write the point on line nearest the surface. + alpha = std::max(std::min(1., alpha), 0.); + point = a + (b - a) * alpha; + + // Assumes a negative constraint evaluation means touchdown. + const bool contact = geometry.calcSurfaceValue(point) < eps; + + // TODO handle here? + if (iter >= maxIter) { + std::cout << "a = " << a << "\n"; + std::cout << "b = " << b << "\n"; + std::cout << "p = " << point << "\n"; + std::cout << "c = " << alpha << "\n"; + // TODO use SimTK_ASSERT + throw std::runtime_error("Failed to compute point on line nearest surface: Reached max iterations"); + } - return g0; + // Return number of iterations required. + return contact; } -GeodesicInitialConditions GeodesicInitialConditions::CreateAtTouchdown( - Vec3 prev_QS, - Vec3 next_PS, - Vec3 trackingPointOnLine) +ImplicitGeodesicState operator-( + const ImplicitGeodesicState& lhs, + const ImplicitGeodesicState& rhs) { - GeodesicInitialConditions g0; - - g0.x = trackingPointOnLine; - g0.t = next_PS - prev_QS; - g0.l = 0.; - - return g0; + ImplicitGeodesicState out; + out.asVecMut() = lhs.asVec() - rhs.asVec(); + return out; } -//============================================================================== -// SURFACE IMPL -//============================================================================== -using LocalGeodesic = CurveSegment::Impl::LocalGeodesic; - -//------------------------------------------------------------------------------ -// REALIZE CACHE -//------------------------------------------------------------------------------ -void LocalGeodesic::realizeTopology(State& s) +ImplicitGeodesicState operator+( + const ImplicitGeodesicState& lhs, + const Vec<10, Real>& rhs) { - // Allocate an auto-update discrete variable for the last computed geodesic. - CacheEntry cache{}; - m_CacheIx = m_Subsystem.allocateAutoUpdateDiscreteVariable( - s, - Stage::Velocity, - new Value(cache), - Stage::Position); + ImplicitGeodesicState out; + out.asVecMut() = lhs.asVec() + rhs; + return out; } -void LocalGeodesic::realizePosition(const State& s) const +Real calcInfNorm(const ImplicitGeodesicState& q) { - if (!m_Subsystem.isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { - updCacheEntry(s) = getPrevCacheEntry(s); - m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); + Real infNorm = 0.; + for (size_t r = 0; r < q.asVec().nrow(); ++r) { + infNorm = std::max(infNorm, q.asVec()[r]); } + return infNorm; } -//------------------------------------------------------------------------------ -// PUBLIC METHODS -//------------------------------------------------------------------------------ -const LocalGeodesicInfo& LocalGeodesic::calcInitialGeodesic( - State& s, - const GeodesicInitialConditions& g0) const +class RKM { - // TODO is this correct? - CacheEntry& cache = updPrevCacheEntry(s); - shootNewGeodesic(g0, cache); - updCacheEntry(s) = cache; - m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); -} + using Y = ImplicitGeodesicState; + using DY = Vec<10, Real>; -const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( - const State& s, - Vec3 prev_QS, - Vec3 next_PS) const -{ - realizePosition(s); - CacheEntry& cache = updCacheEntry(s); - calcCacheEntry(prev_QS, next_PS, cache); - return cache; -} +public: + RKM() = default; -void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) - const -{ - realizePosition(s); + // Integrate y0, populating y1, y2. + Real step(Real h, std::function& f); - // Get the previous geodesic. - const CacheEntry& g = getCacheEntry(s); - - // Get corrected initial conditions. - const GeodesicInitialConditions g0 = - GeodesicInitialConditions::CreateCorrected(g.KP, g.dKP, g.length, c); + struct Sample + { + Real l; + FrenetFrame K; + }; - // Shoot the new geodesic. - shootNewGeodesic(g0, updCacheEntry(s)); -} + Y stepTo( + Y y0, + Real x1, + Real h0, + std::function& f, // Dynamics + std::function& g, // Surface projection + std::function& m, // State log + Real accuracy); -void LocalGeodesic::calcPathPoints(const State& s, std::vector& points) - const -{ - realizePosition(s); - const CacheEntry& cache = getCacheEntry(s); + size_t getNumberOfFailedSteps() const + { + return _failedCount; + } - if (analyticFormAvailable()) { - m_Geometry.resampleGeodesicPointsAnalytically( - cache.KP, - cache.KQ, - cache.length, - m_NumberOfAnalyticPoints, - points); - } else { - for (const FrenetFrame& frame : cache.frames) { - points.push_back(frame.p()); - } + size_t getInitStepSize() const + { + return _h0; } -} -//------------------------------------------------------------------------------ -// PRIVATE METHODS -//------------------------------------------------------------------------------ -void LocalGeodesic::shootNewGeodesic( - const GeodesicInitialConditions& g0, - CacheEntry& cache) const -{ - if (m_Geometry.analyticFormAvailable()) { +private: + static constexpr size_t ORDER = 5; - m_Geometry.calcGeodesicWithVariationAnalytically( - g0.x, - g0.t, - g0.l, - cache.KP, - cache.dKP, - cache.KQ, - cache.dKQ); - } else { - // Compute geodesic start boundary frame and variation. - m_Geometry.calcNearestFrenetFrameImplicitlyFast( - g0.x, - g0.t, - cache.KP, - m_ProjectionMaxIter, - m_ProjectionRequiredAccuracy); - m_Geometry.calcGeodesicStartFrameVariationImplicitly( - cache.KP, - cache.dKP); - // Compute geodesic end boundary frame amd variation (shoot new - // geodesic). - m_Geometry.calcGeodesicEndFrameVariationImplicitly( - cache.KP, - g0.l, - cache.KQ, - cache.dKQ, - cache.sHint, - m_IntegratorAccuracy, - cache.frames); - } + std::array _k{}; + // [yInit, ySave, yStep] + std::array _y{}; - // TODO update step size. - // TODO update line tracking? - throw std::runtime_error("NOTYETIMPLEMENTED"); -} + Real _hMin = 1e-10; + Real _hMax = 1e-1; + Real _h = NaN; + Real _h0 = NaN; + Real _e = NaN; -void LocalGeodesic::calcLiftoffIfNeeded( - const Vec3& prev_QS, - const Vec3& next_PS, - CacheEntry& cache) const + size_t _failedCount = 0; +}; + +Real RKM::step(Real h, std::function& f) { - // Only attempt liftoff when currently wrapping the surface. - LocalGeodesicInfo& g = cache; - if (g.status != Status::Ok) { - return; - } + Y& yk = _y.at(1); - // The curve length must have shrunk completely before lifting off. - if (g.length < 0.) { - return; + for (size_t i = 0; i < 5; ++i) { + yk = _y.at(0) + (h / 3.) * _k.at(0); } - // For a zero-length curve, trigger liftoff when the prev and next points - // lie above the surface plane. - if (dot(prev_QS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0. && - dot(next_PS - g.KP.p(), g.KP.R().getAxisUnitVec(NormalAxis)) <= 0.) { - // No liftoff. - return; - } + // k1 + _k.at(0) = f(_y.at(0)); - // Liftoff detected: update status. - g.status = Status::Liftoff; - // Initialize the tracking point from the last geodesic start point. - cache.trackingPointOnLine = g.KP.p(); -} + // k2 + { + yk = _y.at(0) + (h / 3.) * _k.at(0); + _k.at(1) = f(yk); + } -void LocalGeodesic::calcTouchdownIfNeeded( - const Vec3& prev_QS, - const Vec3& next_PS, - CacheEntry& cache) const -{ - // Only attempt touchdown when liftoff. - LocalGeodesicInfo& g = cache; - if (g.status != Status::Liftoff) { - return; + // k3 + { + yk = _y.at(0) + (h / 6.) * _k.at(0) + (h / 6.) * _k.at(1); + _k.at(2) = f(yk); } - // Detect touchdown by computing the point on the line from x_QS to x_PS - // that is nearest to the surface. - bool touchdownDetected; - if (m_Geometry.analyticFormAvailable()) { - touchdownDetected = m_Geometry.calcNearestPointOnLineAnalytically( - prev_QS, - next_PS, - cache.trackingPointOnLine); - } else { - touchdownDetected = m_Geometry.calcNearestPointOnLineImplicitly( - prev_QS, - next_PS, - cache.trackingPointOnLine, - m_TouchdownIter, - m_TouchdownAccuracy); + // k4 + { + yk = _y.at(0) + (1. / 8. * h) * _k.at(0) + (3. / 8. * h) * _k.at(2); + _k.at(3) = f(yk); } - if (!touchdownDetected) { - return; + + // 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); } - // Touchdown detected: Remove the liftoff status flag. - g.status = Status::Ok; - // Shoot a zero length geodesic at the touchdown point. - GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateAtTouchdown( - prev_QS, - next_PS, - cache.trackingPointOnLine); - shootNewGeodesic(g0, cache); -} + // y1: Auxiliary --> Already updated in k5 computation. -void LocalGeodesic::assertSurfaceBounds( - const Vec3& prev_QS, - const Vec3& next_PS) const -{ - // Make sure that the previous point does not lie inside the surface. - SimTK_ASSERT( - m_Geometry.calcSurfaceValue(prev_QS) < 0., - "Unable to wrap over surface: Preceding point lies inside the surface"); - SimTK_ASSERT( - m_Geometry.calcSurfaceValue(next_PS) < 0., - "Unable to wrap over surface: Next point lies inside the surface"); + // 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; } -void LocalGeodesic::calcCacheEntry( - const Vec3& prev_QS, - const Vec3& next_PS, - CacheEntry& cache) const +RKM::Y RKM::stepTo( + Y y0, + Real x1, + Real h0, + std::function& f, + std::function& g, + std::function& m, + Real accuracy) { - LocalGeodesicInfo& g = cache; + g(y0); + m(0., y0); - if (g.status == Status::Disabled) { - return; - } + _y.at(0) = std::move(y0); + _h = h0; + _e = 0.; + _failedCount = 0; - assertSurfaceBounds(prev_QS, next_PS); - calcTouchdownIfNeeded(prev_QS, next_PS, cache); - calcLiftoffIfNeeded(prev_QS, next_PS, cache); -} + Real x = 0.; + while (x < x1 - 1e-13) { + const bool init = x == 0.; -//============================================================================== -// CURVE SEGMENT -//============================================================================== + _h = x + _h > x1 ? x1 - x : _h; -CurveSegment::CurveSegment( - CableSpan cable, - const MobilizedBody& mobod, - Transform X_BS, - const ContactGeometry& geometry, - Vec3 xHint) - : m_Impl(std::shared_ptr(new CurveSegment::Impl(cable, mobod, X_BS, geometry, xHint))) -{ - updImpl().setIndex(cable.adoptSegment(*this)); -} + // Attempt step. + Real err = step(_h, f); -const CableSpan& CurveSegment::getCable() const -{ - return getImpl().getCable(); -} + // 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)); + } -Real CurveSegment::getSegmentLength(const State& s) -{ - getImpl().realizeCablePosition(s); - return getImpl().getPosInfo(s).length; -} + // Potentially increase stepsize. + if (err < accuracy / 64.) { + _h *= 2.; + } -CurveSegment::Status CurveSegment::getStatus(const State& s) const -{ - getImpl().realizeCablePosition(s); - return getImpl().getStatus(s); -} + _e = std::max(_e, err); -//============================================================================== -// CURVE SEGMENT IMPL -//============================================================================== + SimTK_ASSERT( + _h > _hMin, + "Geodesic Integrator failed: Reached very small stepsize"); + SimTK_ASSERT( + _h < _hMax, + "Geodesic Integrator failed: Reached very large stepsize"); -CurveSegment::Impl::Impl( - CableSpan path, - const MobilizedBody& mobod, - const Transform& X_BS, - ContactGeometry geometry, - Vec3 initPointGuess) : - m_Subsystem(path.getImpl().getSubsystem()), - m_Path(path), m_Index(-1), // TODO what to do with this index, and when - m_Mobod(mobod), m_Offset(X_BS), - m_Surface(m_Subsystem, geometry, initPointGuess) -{} + if (init) { + _h0 = _h; + } + } + return _y.at(0); +} -void CurveSegment::Impl::realizeTopology(State& s) +Real calcNormalCurvature( + const ContactGeometry& geometry, + Vec3 point, + Vec3 tangent) { - // Allocate position level cache. - PosInfo posInfo{}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry( - s, - Stage::Position, - new Value(posInfo)); + const Vec3& p = point; + const Vec3& v = tangent; + const Vec3 g = geometry.calcSurfaceGradient(p); + const Vec3 h_v = geometry.calcSurfaceHessian(p) * v; + // Sign flipped compared to thesis: kn = negative, see eq 3.63 + return -dot(v, h_v) / g.norm(); } -void CurveSegment::Impl::realizePosition(const State& s) const +Real calcGeodesicTorsion( + const ContactGeometry& geometry, + Vec3 point, + Vec3 tangent) { - if (!m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { - calcPosInfo(s, updPosInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); - } + // TODO verify this! + const Vec3& p = point; + const Vec3& v = tangent; + const Vec3 g = geometry.calcSurfaceGradient(p); + const Vec3 h_v = geometry.calcSurfaceHessian(p) * v; + const Vec3 gxv = cross(g, v); + return -dot(h_v, gxv) / dot(g, g); } -void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) - const +UnitVec3 calcSurfaceNormal(const ContactGeometry& geometry, Vec3 point) { - // Get tramsform from local surface frame to ground. - Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); - - Vec3 prev_QS = X_GS.shiftBaseStationToFrame(prev_QG); - Vec3 xGuess_S = - m_Surface.getInitialPointGuess(); // TODO move into function call? - - GeodesicInitialConditions g0 = - GeodesicInitialConditions::CreateZeroLengthGuess(prev_QS, xGuess_S); - m_Surface.calcInitialGeodesic(s, g0); - - m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); + const Vec3& p = point; + const Vec3 gradient = geometry.calcSurfaceGradient(p); + return UnitVec3{gradient}; } -void CurveSegment::Impl::applyGeodesicCorrection( - const State& s, - const CurveSegment::Impl::Correction& c) const +Vec3 calcAcceleration(const ContactGeometry& geometry, Vec3 point, Vec3 tangent) { - // Apply correction to curve. - m_Surface.applyGeodesicCorrection(s, c); - - // Invalidate position level cache. - m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); + // 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); } -void CurveSegment::Impl::calcPathPoints( - const State& s, - std::vector& points) const +Mat33 calcAdjoint(const Mat33& mat) { - const Transform& X_GS = getPosInfo(s).X_GS; - size_t initSize = points.size(); - m_Surface.calcPathPoints(s, points); - for (size_t i = points.size() - initSize; i < points.size(); ++i) { - points.at(i) = X_GS.shiftFrameStationToBase(points.at(i)); + 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; } -namespace -{ -void xformSurfaceGeodesicToGround( - const LocalGeodesicInfo& geodesic_S, - const Transform& X_GS, - CurveSegment::Impl::PosInfo& geodesic_G) +Real calcGaussianCurvature(const ContactGeometry& geometry, Vec3 point) { - geodesic_G.X_GS = X_GS; + const Vec3& p = point; + Vec3 g = geometry.calcSurfaceGradient(p); + Real gDotg = dot(g, g); + Mat33 adj = calcAdjoint(geometry.calcSurfaceHessian(p)); - // Store the the local geodesic in ground frame. - geodesic_G.KP = X_GS.compose(geodesic_S.KP); - geodesic_G.KQ = X_GS.compose(geodesic_S.KQ); + if (gDotg * gDotg < 1e-13) { + throw std::runtime_error( + "Gaussian curvature inaccurate: are we normal to surface?"); + } + + return (dot(g, adj * g)) / (gDotg * gDotg); +} - geodesic_G.dKP[0] = X_GS.R() * geodesic_S.dKP[0]; - geodesic_G.dKP[1] = X_GS.R() * geodesic_S.dKP[1]; +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); +} - geodesic_G.dKQ[0] = X_GS.R() * geodesic_S.dKQ[0]; - geodesic_G.dKQ[1] = X_GS.R() * geodesic_S.dKQ[1]; +void calcGeodesicBoundaryState( + const ContactGeometry& geometry, + const ImplicitGeodesicState& q, + bool isEnd, + FrenetFrame& K, + Mat34& v, + Mat34& w) +{ + calcFrenetFrame(geometry, q, K); - // TODO use SpatialVec for variation. - /* for (size_t i = 0; i < GeodesicDOF; ++i) { */ - /* geodesic_G.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) - */ - /* geodesic_G.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) - */ + const UnitVec3& t = K.R().getAxisUnitVec(TangentAxis); + const UnitVec3& n = K.R().getAxisUnitVec(NormalAxis); + const UnitVec3& b = K.R().getAxisUnitVec(BinormalAxis); - /* geodesic_G.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) - */ - /* geodesic_G.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) - */ - /* } */ + // TODO remove fourth element? + v.col(0) = t; + v.col(1) = b * q.a; + v.col(2) = b * q.r; + v.col(3) = isEnd ? v.col(0) : Vec3{0.}; - geodesic_G.length = geodesic_S.length; + const Real tau_g = geometry.calcGeodesicTorsion(q.x, t); + const Real kappa_n = geometry.calcNormalCurvature(q.x, t); + const Real kappa_a = geometry.calcNormalCurvature(q.x, b); - throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); + 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.}; } -} // namespace -void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const +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) { - if (m_Surface.getStatus(s) == Status::Disabled) { - return; - } + using Y = ImplicitGeodesicState; + using DY = Vec<10, Real>; - // Compute tramsform from local surface frame to ground. - const Transform& X_GS = calcSurfaceFrameInGround(s); + 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.emplace_back(l, frame); + }; - // Get the path points before and after this segment. - const Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_Index); - const Vec3 next_G = m_Path.getImpl().findNextPoint(s, m_Index); + Y y0(x, t); - // Transform the prev and next path points to the surface frame. - const Vec3 prev_S = X_GS.shiftBaseStationToFrame(prev_G); - const Vec3 next_S = X_GS.shiftBaseStationToFrame(next_G); + RKM rkm{}; - // Detect liftoff, touchdown and potential invalid configurations. - // TODO this doesnt follow the regular invalidation scheme... - // Grab the last geodesic that was computed. - const LocalGeodesicInfo& geodesic_S = - m_Surface.calcLocalGeodesicInfo(s, prev_S, next_S); + Y y1 = rkm.stepTo(y0, l, ds, f, g, m, accuracy); - // Store the the local geodesic in ground frame. - xformSurfaceGeodesicToGround(geodesic_S, X_GS, updPosInfo(s)); + 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]); + + ds = rkm.getInitStepSize(); } -SpatialVec CurveSegment::Impl::calcAppliedWrenchInGround( - const State& s, - Real tension) const -{ - const PosInfo& posInfo = getPosInfo(s); +} // namespace - const UnitVec3& t_P = posInfo.KP.R().getAxisUnitVec(TangentAxis); - const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); - const Vec3 F_G = tension * (t_Q - t_P); +//============================================================================== +// SOLVER +//============================================================================== - const Vec3 x_GS = posInfo.X_GS.p(); - const Vec3& r_P = posInfo.KP.p() - x_GS; - const Vec3& r_Q = posInfo.KQ.p() - x_GS; - const Vec3 M_G = tension * (r_Q % t_Q - r_P % t_P); - return {M_G, F_G}; -} +namespace +{ +class SolverDataCache; +SolverDataCache& findDataCache(size_t nActive); -void CurveSegment::Impl::applyBodyForce( - const State& s, - Real tension, - Vector_& bodyForcesInG) const +struct SolverData { - // TODO why? - if (tension <= 0) { - return; + std::vector lineSegments; + + Matrix pathErrorJacobian; + Vector pathCorrection; + Vector pathError; + Matrix mat; + // TODO Cholesky decomposition... + FactorLU matInv; + Vector vec; +}; + +class SolverDataCache +{ +private: + SolverDataCache(size_t n) + { + static constexpr int Q = 4; + static constexpr int C = 4; + + m_Data.lineSegments.resize(n + 1); + m_Data.pathErrorJacobian = Matrix(C * n, Q * n, 0.); + m_Data.pathCorrection = Vector(Q * n, 0.); + m_Data.pathError = Vector(C * n, 0.); + m_Data.mat = Matrix(Q * n, Q * n, NaN); + m_Data.vec = Vector(Q * n, NaN); } - if (getStatus(s) != Status::Ok) { - return; +public: + void lock() + { + m_Mutex.lock(); } - m_Mobod.applyBodyForce( - s, - calcAppliedWrenchInGround(s, tension), - bodyForcesInG); -} + void unlock() + { + m_Mutex.unlock(); + } -//============================================================================== -// PATH HELPERS -//============================================================================== + SolverData& updData() + { + return m_Data; + } -namespace -{ +private: + SolverData m_Data; + std::mutex m_Mutex{}; -static const int N_PATH_CONSTRAINTS = 4; + friend SolverDataCache& findDataCache(size_t nActive); +}; -// TODO this is awkward -void addBlock(const Vec4& values, Matrix& block) +SolverDataCache& findDataCache(size_t nActive) { - for (int i = 0; i < Vec4::size(); ++i) { - block[0][i] = values[i]; + static std::vector s_GlobalCache{}; + static std::mutex s_GlobalLock{}; + + { + std::lock_guard lock(s_GlobalLock); + + for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { + int n = i + 1; + s_GlobalCache.emplace_back(nActive); + } } -} -void addDirectionJacobian( - const LineSegment& e, - const UnitVec3& axis, - const PointVariation& dx, - Matrix& J, - bool invert = false) -{ - Vec3 y = axis - e.d * dot(e.d, axis); - y /= e.l * (invert ? 1. : -1); - addBlock(~dx * y, J); + return s_GlobalCache.at(nActive - 1); } -Real calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) +const Correction* calcPathCorrections(SolverData& data) { - return dot(e.d, R.getAxisUnitVec(axis)); -} + Real w = data.pathError.normInf(); -GeodesicJacobian addPathErrorJacobian( - const LineSegment& e, - const UnitVec3& axis, - const Variation& dK, - Matrix& J, - bool invertV = false) -{ - addDirectionJacobian(e, axis, dK[1], J, invertV); - addBlock(~dK[0] * cross(axis, e.d), J); -} + data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; + for (int i = 0; i < data.mat.nrow(); ++i) { + data.mat[i][i] += w; + } + data.matInv = data.mat; + data.vec = data.pathErrorJacobian.transpose() * data.pathError; + data.matInv.solve(data.vec, data.pathCorrection); + static_assert( + sizeof(Correction) == sizeof(Real) * GeodesicDOF, + "Invalid size of corrections vector"); + SimTK_ASSERT( + data.pathCorrection.size() * sizeof(Real) == n * sizeof(Correction), + "Invalid size of path corrections vector"); + return reinterpret_cast(&data.pathCorrection[0]); +} } // namespace //============================================================================== -// CABLE SPAN +// GEODESIC INITIAL CONDITIONS //============================================================================== -CableSpan::CableSpan( - CableSubsystem& subsystem, - const MobilizedBody& originBody, - const Vec3& defaultOriginPoint, - const MobilizedBody& terminationBody, - const Vec3& defaultTerminationPoint) : - m_Impl(std::shared_ptr(new Impl( - subsystem, - originBody, - defaultOriginPoint, - terminationBody, - defaultTerminationPoint))) {} - -CurveSegmentIndex CableSpan::adoptSegment(const CurveSegment& segment) +GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected( + const FrenetFrame& KP, + const Variation& dKP, + Real l, + const Correction& c) { - return updImpl().adoptSegment(segment); -} + GeodesicInitialConditions g0; -int CableSpan::getNumCurveSegments() const -{ - return getImpl().getNumCurveSegments(); -} + Vec3 v = dKP[1] * c; + g0.x = KP.p() + v; -const CurveSegment& CableSpan::getCurveSegment(CurveSegmentIndex ix) const -{ - return getImpl().getCurveSegment(ix); + Vec3 w = dKP[0] * c; + const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); + g0.t = t + cross(w, t); + + // Take the length correction, and add to the current length. + Real dl = c[3]; // Length increment is the last correction element. + g0.l = std::max(l + dl, 0.); // Clamp length to be nonnegative. + + return g0; } -Real CableSpan::getLength(const State& s) const +GeodesicInitialConditions GeodesicInitialConditions:: + CreateFromGroundInSurfaceFrame( + const Transform& X_GS, + Vec3 x_G, + Vec3 t_G, + Real l) { - return getImpl().getPosInfo(s).l; + GeodesicInitialConditions g0; + + g0.x = X_GS.shiftBaseStationToFrame(x_G); + g0.t = X_GS.xformBaseVecToFrame(t_G); + g0.l = l; + + return g0; } -Real CableSpan::getLengthDot(const State& s) const +GeodesicInitialConditions GeodesicInitialConditions::CreateZeroLengthGuess( + Vec3 prev_QS, + Vec3 xGuess_S) { - return getImpl().getVelInfo(s).lengthDot; + GeodesicInitialConditions g0; + + g0.x = xGuess_S; + g0.t = xGuess_S - prev_QS; + g0.l = 0.; + + return g0; } -void CableSpan::applyBodyForces( - const State& s, - Real tension, - Vector_& bodyForcesInG) const + +GeodesicInitialConditions GeodesicInitialConditions::CreateAtTouchdown( + Vec3 prev_QS, + Vec3 next_PS, + Vec3 trackingPointOnLine) { - return getImpl().applyBodyForces(s, tension, bodyForcesInG); + GeodesicInitialConditions g0; + + g0.x = trackingPointOnLine; + g0.t = next_PS - prev_QS; + g0.l = 0.; + + return g0; } //============================================================================== -// CABLE SPAN IMPL +// SURFACE IMPL //============================================================================== +using LocalGeodesic = CurveSegment::Impl::LocalGeodesic; -void CableSpan::Impl::realizeTopology(State& s) +//------------------------------------------------------------------------------ +// REALIZE CACHE +//------------------------------------------------------------------------------ +void LocalGeodesic::realizeTopology(State& s) { - PosInfo posInfo{}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry( - s, - Stage::Position, - new Value(posInfo)); - - VelInfo velInfo{}; - m_VelInfoIx = m_Subsystem.allocateCacheEntry( + // Allocate an auto-update discrete variable for the last computed geodesic. + CacheEntry cache{}; + m_CacheIx = m_Subsystem.allocateAutoUpdateDiscreteVariable( s, Stage::Velocity, - new Value(velInfo)); + new Value(cache), + Stage::Position); } -void CableSpan::Impl::realizePosition(const State& s) const +void LocalGeodesic::realizePosition(const State& s) const { - if (m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { - return; + if (!m_Subsystem.isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { + updCacheEntry(s) = getPrevCacheEntry(s); + m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); } - calcPosInfo(s, updPosInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); } -void CableSpan::Impl::realizeVelocity(const State& s) const +//------------------------------------------------------------------------------ +// PUBLIC METHODS +//------------------------------------------------------------------------------ +const LocalGeodesicInfo& LocalGeodesic::calcInitialGeodesic( + State& s, + const GeodesicInitialConditions& g0) const { - if (m_Subsystem.isCacheValueRealized(s, m_VelInfoIx)) { - return; - } - calcVelInfo(s, updVelInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_VelInfoIx); + // TODO is this correct? + CacheEntry& cache = updPrevCacheEntry(s); + shootNewGeodesic(g0, cache); + updCacheEntry(s) = cache; + m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); } -const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo( - const State& s) const +const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( + const State& s, + Vec3 prev_QS, + Vec3 next_PS) const { realizePosition(s); - return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); + CacheEntry& cache = updCacheEntry(s); + calcCacheEntry(prev_QS, next_PS, cache); + return cache; } -CableSpan::Impl::PosInfo& CableSpan::Impl::updPosInfo(const State& s) const +void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) + const { - return Value::updDowncast( - m_Subsystem.updCacheEntry(s, m_PosInfoIx)); -} + realizePosition(s); -const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo( - const State& s) const -{ - realizeVelocity(s); - return Value::downcast(m_Subsystem.getCacheEntry(s, m_VelInfoIx)); -} + // Get the previous geodesic. + const CacheEntry& g = getCacheEntry(s); -CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const -{ - return Value::updDowncast( - m_Subsystem.updCacheEntry(s, m_VelInfoIx)); + // Get corrected initial conditions. + const GeodesicInitialConditions g0 = + GeodesicInitialConditions::CreateCorrected(g.K_P, g.dK_P, g.length, c); + + // Shoot the new geodesic. + shootNewGeodesic(g0, updCacheEntry(s)); } -void CableSpan::Impl::calcInitZeroLengthGeodesic(State& s) const +void LocalGeodesic::calcPathPoints(const State& s, std::vector& points) + const { - Vec3 prev_QG = - m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); - size_t i = 0; - for (const CurveSegment& obstacle : m_CurveSegments) { - if (obstacle.getImpl().getStatus(s) == Status::Disabled) { - continue; - } - obstacle.getImpl().calcInitZeroLengthGeodesic(s, prev_QG); + realizePosition(s); + const CacheEntry& cache = getCacheEntry(s); - prev_QG = obstacle.getImpl().getPosInfo(s).KQ.p(); + if (analyticFormAvailable()) { + throw std::runtime_error("NOTYETIMPLEMENTED"); + m_Geometry.resampleGeodesicPointsAnalytically( + cache.K_P, + cache.K_Q, + cache.length, + m_NumberOfAnalyticPoints, + points); + } else { + for (const LocalGeodesicSample& sample : cache.samples) { + points.push_back(sample.frame.p()); + } } } -const CurveSegment* CableSpan::Impl::findPrevActiveCurveSegment( - const State& s, - CurveSegmentIndex ix) const +void LocalGeodesic::calcLiftoffIfNeeded( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const { - for (int i = ix - 1; i > 0; --i) { - // Find the active segment before the current. - if (m_CurveSegments.at(CurveSegmentIndex(i)).getImpl().isActive(s)) { - return &m_CurveSegments.at(CurveSegmentIndex(i)); - } + // Only attempt liftoff when currently wrapping the surface. + LocalGeodesicInfo& g = cache; + if (g.status != Status::Ok) { + return; } - 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().isActive(s)) { - return &m_CurveSegments.at(CurveSegmentIndex(i)); - } + // The curve length must have shrunk completely before lifting off. + if (g.length < 0.) { + return; } - return nullptr; -} -Vec3 CableSpan::Impl::findPrevPoint(const State& s, CurveSegmentIndex ix) const -{ - const CurveSegment* segment = findPrevActiveCurveSegment(s, ix); - return segment ? segment->getImpl().getPosInfo(s).KQ.p() - : m_OriginBody.getBodyTransform(s).shiftFrameStationToBase( - m_OriginPoint); -} + // For a zero-length curve, trigger liftoff when the prev and next points + // lie above the surface plane. + if (dot(prev_QS - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= 0. && + dot(next_PS - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= 0.) { + // No liftoff. + return; + } -Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) const -{ - const CurveSegment* segment = findNextActiveCurveSegment(s, ix); - return segment - ? segment->getImpl().getPosInfo(s).KP.p() - : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( - m_TerminationPoint); + // Liftoff detected: update status. + g.status = Status::Liftoff; + // Initialize the tracking point from the last geodesic start point. + cache.trackingPointOnLine = g.K_P.p(); } -template -void CableSpan::Impl::calcPathErrorVector( - const State& s, - const std::vector& lines, - std::array axes, - Vector& pathError) const +void LocalGeodesic::calcTouchdownIfNeeded( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const { - size_t lineIx = 0; - ptrdiff_t row = -1; - - for (int i = 0; i < getNumCurveSegments(); ++i) { - const CurveSegment& segment = getCurveSegment(CurveSegmentIndex(i)); - if (!segment.getImpl().isActive(s)) { - continue; - } + // Only attempt touchdown when liftoff. + LocalGeodesicInfo& g = cache; + if (g.status != Status::Liftoff) { + return; + } - 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); - } + // Detect touchdown by computing the point on the line from x_QS to x_PS + // that is nearest to the surface. + bool touchdownDetected; + if (m_Geometry.analyticFormAvailable()) { + throw std::runtime_error("NOTYETIMPLEMENTED"); + /* touchdownDetected = m_Geometry.calcNearestPointOnLineAnalytically( */ + /* prev_QS, */ + /* next_PS, */ + /* cache.trackingPointOnLine); */ + } else { + touchdownDetected = calcNearestPointOnLineImplicitly( + m_Geometry, + prev_QS, + next_PS, + cache.trackingPointOnLine, + m_TouchdownIter, + m_TouchdownAccuracy); + } + if (!touchdownDetected) { + return; } + + // Touchdown detected: Remove the liftoff status flag. + g.status = Status::Ok; + // Shoot a zero length geodesic at the touchdown point. + GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateAtTouchdown( + prev_QS, + next_PS, + cache.trackingPointOnLine); + shootNewGeodesic(g0, cache); } -template -void CableSpan::Impl::calcPathErrorJacobian( - const State& s, - const std::vector& lines, - std::array axes, - Matrix& J) const +void LocalGeodesic::assertSurfaceBounds( + const Vec3& prev_QS, + const Vec3& next_PS) const { - constexpr size_t Nq = GeodesicDOF; - - // TODO perhaps just not make method static. - const size_t n = lines.size() - 1; - + // Make sure that the previous point does not lie inside the surface. SimTK_ASSERT( - J.rows() == n * N, - "Invalid number of rows in jacobian matrix"); + m_Geometry.calcSurfaceValue(prev_QS) < 0., + "Unable to wrap over surface: Preceding point lies inside the surface"); 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().isActive(s)) { - continue; - } - const GeodesicInfo& g = segment.getImpl().getPosInfo(s); - - const LineSegment& l_P = lines.at(activeIx); - const LineSegment& l_Q = lines.at(activeIx + 1); - - const CurveSegmentIndex ix = segment.getImpl().getIndex(); - const CurveSegment* prev = findPrevActiveCurveSegment(s, ix); - const CurveSegment* next = findNextActiveCurveSegment(s, ix); - - for (CoordinateAxis axis : axes) { - const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); - const Variation& dK_P = g.dKP; + m_Geometry.calcSurfaceValue(next_PS) < 0., + "Unable to wrap over surface: Next point lies inside the surface"); +} - addPathErrorJacobian(l_P, a_P, dK_P, J.block(row, col, 1, Nq)); +void LocalGeodesic::calcCacheEntry( + const Vec3& prev_QS, + const Vec3& next_PS, + CacheEntry& cache) const +{ + LocalGeodesicInfo& g = cache; - if (prev) { - const Variation& prev_dK_Q = prev->getImpl().getPosInfo(s).dKQ; - addDirectionJacobian( - l_P, - a_P, - prev_dK_Q[1], - J.block(row, col - Nq, 1, Nq), - true); - } - ++row; - } + if (g.status == Status::Disabled) { + return; + } - for (CoordinateAxis axis : axes) { - const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); - const Variation& dK_Q = g.dKQ; + assertSurfaceBounds(prev_QS, next_PS); + calcTouchdownIfNeeded(prev_QS, next_PS, cache); + calcLiftoffIfNeeded(prev_QS, next_PS, cache); +} - addPathErrorJacobian( - l_Q, - a_Q, - dK_Q, - J.block(row, col, 1, Nq), - true); +void LocalGeodesic::shootNewGeodesic( + const GeodesicInitialConditions& g0, + CacheEntry& cache) const +{ + calcGeodesicAndVariationImplicitly( + m_Geometry, + g0.x, + g0.t, + g0.l, + cache.sHint, + cache.K_P, + cache.dK_P, + cache.K_Q, + cache.dK_Q, + m_IntegratorAccuracy, + m_ProjectionMaxIter, + m_ProjectionAccuracy, + cache.samples); + + /* using Y = ImplicitGeodesicState; */ + /* if (m_Geometry.analyticFormAvailable()) { */ + + /* m_Geometry.calcGeodesicWithVariationAnalytically( */ + /* g0.x, */ + /* g0.t, */ + /* g0.l, */ + /* cache.KP, */ + /* cache.dKP, */ + /* cache.KQ, */ + /* cache.dKQ); */ + /* } else { */ + /* // Compute geodesic start boundary frame and variation. */ + /* m_Geometry.calcNearestFrenetFrameImplicitlyFast( */ + /* g0.x, */ + /* g0.t, */ + /* cache.KP, */ + /* m_ProjectionMaxIter, */ + /* m_ProjectionRequiredAccuracy); */ + /* m_Geometry.calcGeodesicStartFrameVariationImplicitly( */ + /* cache.KP, */ + /* cache.dKP); */ + /* // Compute geodesic end boundary frame amd variation (shoot new */ + /* // geodesic). */ + /* m_Geometry.calcGeodesicEndFrameVariationImplicitly( */ + /* cache.KP, */ + /* g0.l, */ + /* cache.KQ, */ + /* cache.dKQ, */ + /* cache.sHint, */ + /* m_IntegratorAccuracy, */ + /* cache.frames); */ + /* } */ +} - if (next) { - const Variation& next_dK_P = next->getImpl().getPosInfo(s).dKP; - addDirectionJacobian( - l_Q, - a_Q, - next_dK_P[1], - J.block(row, col + Nq, 1, Nq)); - } - ++row; - } +//============================================================================== +// CURVE SEGMENT +//============================================================================== - col += Nq; - }; +CurveSegment::CurveSegment( + CableSpan cable, + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 xHint) + : m_Impl(std::shared_ptr(new CurveSegment::Impl(cable, mobod, X_BS, geometry, xHint))) +{ + // TODO bit awkward to set the index later. + updImpl().setIndex(cable.adoptSegment(*this)); + /* CurveSegmentIndex ix = cable.adoptSegment(*this); */ + /* m_Impl = std::shared_ptr(new CurveSegment::Impl(cable, ix, mobod, X_BS, geometry, xHint)); */ } -Real CableSpan::Impl::calcPathLength( - const State& s, - const std::vector& lines) const +const CableSpan& CurveSegment::getCable() const { - Real lTot = 0.; - for (const LineSegment& line : lines) { - // TODO spell out as length. - lTot += line.l; - } + return getImpl().getCable(); +} - for (const CurveSegment& segment : m_CurveSegments) { - if (!segment.getImpl().isActive(s)) { - continue; - } - lTot += segment.getImpl().getPosInfo(s).length; - } - return lTot; +Real CurveSegment::getSegmentLength(const State& s) +{ + getImpl().realizeCablePosition(s); + return getImpl().getPosInfo(s).length; } -void CableSpan::Impl::calcLineSegments( - const State& s, - Vec3 p_O, - Vec3 p_I, - std::vector& lines) const +CurveSegment::Status CurveSegment::getStatus(const State& s) const { - lines.resize(m_CurveSegments.size() + 1); - lines.clear(); + getImpl().realizeCablePosition(s); + return getImpl().getStatus(s); +} - Vec3 lineStart = std::move(p_O); - for (const CurveSegment& segment : m_CurveSegments) { - if (!segment.getImpl().isActive(s)) { - continue; - } +//============================================================================== +// CURVE SEGMENT IMPL +//============================================================================== - const GeodesicInfo& g = segment.getImpl().getPosInfo(s); - const Vec3 lineEnd = g.KP.p(); - lines.emplace_back(lineStart, lineEnd); +CurveSegment::Impl::Impl( + CableSpan path, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess) : + m_Subsystem(path.getImpl().getSubsystem()), + m_Path(path), m_Index(-1), // TODO what to do with this index, and when + m_Mobod(mobod), m_Offset(X_BS), + m_Surface(m_Subsystem, geometry, initPointGuess) +{} - lineStart = g.KQ.p(); - } - lines.emplace_back(lineStart, p_I); +void CurveSegment::Impl::realizeTopology(State& s) +{ + // Allocate position level cache. + PosInfo posInfo{}; + m_PosInfoIx = m_Subsystem.allocateCacheEntry( + s, + Stage::Position, + new Value(posInfo)); } -size_t CableSpan::Impl::countActive(const State& s) const +void CurveSegment::Impl::realizePosition(const State& s) const { - size_t count = 0; - for (const CurveSegment& segment : m_CurveSegments) { - if (segment.getImpl().isActive(s)) { - ++count; - } + if (!m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { + calcPosInfo(s, updPosInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); } - return count; } -void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const +void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) + const { - // Path origin and termination point. - const Vec3 x_O = - m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); - const Vec3 x_I = - m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( - m_TerminationPoint); + // Get tramsform from local surface frame to ground. + Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); - const std::array axes{NormalAxis, BinormalAxis}; + Vec3 prev_QS = X_GS.shiftBaseStationToFrame(prev_QG); + Vec3 xGuess_S = + m_Surface.getInitialPointGuess(); // TODO move into function call? - for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; - ++posInfo.loopIter) { - const size_t nActive = countActive(s); + GeodesicInitialConditions g0 = + GeodesicInitialConditions::CreateZeroLengthGuess(prev_QS, xGuess_S); + m_Surface.calcInitialGeodesic(s, g0); - // Grab the shared data cache for computing the matrices, and lock it. - SolverDataCache& dataCache = findDataCache(nActive); - try { - dataCache.lock(); + m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); +} - SolverData& data = dataCache.updData(); +void CurveSegment::Impl::applyGeodesicCorrection( + const State& s, + const CurveSegment::Impl::Correction& c) const +{ + // Apply correction to curve. + m_Surface.applyGeodesicCorrection(s, c); - // Compute the straight-line segments. - calcLineSegments(s, x_O, x_I, data.lineSegments); + // Invalidate position level cache. + m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); +} - // Evaluate path error, and stop when converged. - calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); - const Real maxPathError = data.pathError.normInf(); - if (maxPathError < m_PathErrorBound) { - return; - } +void CurveSegment::Impl::calcPathPoints( + const State& s, + std::vector& points) const +{ + const Transform& X_GS = getPosInfo(s).X_GS; + size_t initSize = points.size(); + m_Surface.calcPathPoints(s, points); + for (size_t i = points.size() - initSize; i < points.size(); ++i) { + points.at(i) = X_GS.shiftFrameStationToBase(points.at(i)); + } +} - // Evaluate the path error jacobian. - calcPathErrorJacobian<2>( - s, - data.lineSegments, - axes, - data.pathErrorJacobian); +namespace +{ +void xformSurfaceGeodesicToGround( + const LocalGeodesicInfo& geodesic_S, + const Transform& X_GS, + CurveSegment::Impl::PosInfo& geodesic_G) +{ + geodesic_G.X_GS = X_GS; - // Compute path corrections. - const Correction* corrIt = calcPathCorrections(data); + // Store the the local geodesic in ground frame. + geodesic_G.KP = X_GS.compose(geodesic_S.K_P); + geodesic_G.KQ = X_GS.compose(geodesic_S.K_Q); - // Apply path corrections. - for (const CurveSegment& obstacle : m_CurveSegments) { - if (!obstacle.getImpl().isActive(s)) { - continue; - } - obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); - ++corrIt; - } + geodesic_G.dKP[0] = X_GS.R() * geodesic_S.dK_P[0]; + geodesic_G.dKP[1] = X_GS.R() * geodesic_S.dK_P[1]; - } catch (const std::exception& e) { - dataCache.unlock(); - throw e; - } + geodesic_G.dKQ[0] = X_GS.R() * geodesic_S.dK_Q[0]; + geodesic_G.dKQ[1] = X_GS.R() * geodesic_S.dK_Q[1]; - // Release the lock on the shared data. - dataCache.unlock(); + // TODO use SpatialVec for variation. + /* for (size_t i = 0; i < GeodesicDOF; ++i) { */ + /* geodesic_G.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) + */ + /* geodesic_G.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) + */ - // Path has changed: invalidate each segment's cache. - for (const CurveSegment& obstacle : m_CurveSegments) { - obstacle.getImpl().invalidatePositionLevelCache(s); - } - } + /* geodesic_G.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) + */ + /* geodesic_G.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) + */ + /* } */ - throw std::runtime_error("Failed to converge"); + geodesic_G.length = geodesic_S.length; + + throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); } +} // namespace -void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const +void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { - const PosInfo& pos = getPosInfo(s); + if (m_Surface.getStatus(s) == Status::Disabled) { + return; + } - Real lengthDot = 0.; + // Compute tramsform from local surface frame to ground. + const Transform& X_GS = calcSurfaceFrameInGround(s); - Vec3 v_GQ = m_OriginBody.findStationVelocityInGround(s, m_OriginPoint); - const CurveSegment* lastActive = nullptr; - for (const CurveSegment& obstacle : m_CurveSegments) { - if (!obstacle.getImpl().isActive(s)) { - continue; - } + // Get the path points before and after this segment. + const Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_Index); + const Vec3 next_G = m_Path.getImpl().findNextPoint(s, m_Index); - const GeodesicInfo& g = obstacle.getImpl().getPosInfo(s); - const UnitVec3 e_G = g.KP.R().getAxisUnitVec(TangentAxis); + // Transform the prev and next path points to the surface frame. + const Vec3 prev_S = X_GS.shiftBaseStationToFrame(prev_G); + const Vec3 next_S = X_GS.shiftBaseStationToFrame(next_G); - Vec3 next_v_GQ; - Vec3 v_GP; - obstacle.getImpl().calcContactPointVelocitiesInGround( - s, - v_GP, - next_v_GQ); + // Detect liftoff, touchdown and potential invalid configurations. + // TODO this doesnt follow the regular invalidation scheme... + // Grab the last geodesic that was computed. + const LocalGeodesicInfo& geodesic_S = + m_Surface.calcLocalGeodesicInfo(s, prev_S, next_S); - lengthDot += dot(e_G, v_GP - v_GQ); + // Store the the local geodesic in ground frame. + xformSurfaceGeodesicToGround(geodesic_S, X_GS, updPosInfo(s)); +} - v_GQ = next_v_GQ; - lastActive = &obstacle; - } +SpatialVec CurveSegment::Impl::calcAppliedWrenchInGround( + const State& s, + Real tension) const +{ + const PosInfo& posInfo = getPosInfo(s); - const Vec3 v_GP = - m_TerminationBody.findStationVelocityInGround(s, m_TerminationPoint); - const UnitVec3 e_G = - lastActive ? lastActive->getImpl().getPosInfo(s).KQ.R().getAxisUnitVec( - TangentAxis) - : UnitVec3(pos.xI - pos.xO); + const UnitVec3& t_P = posInfo.KP.R().getAxisUnitVec(TangentAxis); + const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); + const Vec3 F_G = tension * (t_Q - t_P); - lengthDot += dot(e_G, v_GP - v_GQ); + const Vec3 x_GS = posInfo.X_GS.p(); + const Vec3& r_P = posInfo.KP.p() - x_GS; + const Vec3& r_Q = posInfo.KQ.p() - x_GS; + const Vec3 M_G = tension * (r_Q % t_Q - r_P % t_P); + return {M_G, F_G}; } -void CableSpan::Impl::applyBodyForces( +void CurveSegment::Impl::applyBodyForce( const State& s, Real tension, Vector_& bodyForcesInG) const @@ -1139,525 +1193,571 @@ void CableSpan::Impl::applyBodyForces( return; } - realizePosition(s); + if (getStatus(s) != Status::Ok) { + return; + } + + m_Mobod.applyBodyForce( + s, + calcAppliedWrenchInGround(s, tension), + bodyForcesInG); +} + +//============================================================================== +// PATH HELPERS +//============================================================================== + +namespace +{ + +static const int N_PATH_CONSTRAINTS = 4; + +// TODO this is awkward +void addBlock(const Vec4& values, Matrix& block) +{ + for (int i = 0; i < Vec4::size(); ++i) { + block[0][i] = values[i]; + } +} + +void addDirectionJacobian( + const LineSegment& e, + const UnitVec3& axis, + const PointVariation& dx, + Matrix& J, + bool invert = false) +{ + Vec3 y = axis - e.d * dot(e.d, axis); + y /= e.l * (invert ? 1. : -1); + addBlock(~dx * y, J); +} + +Real calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) +{ + return dot(e.d, R.getAxisUnitVec(axis)); +} + +GeodesicJacobian addPathErrorJacobian( + const LineSegment& e, + const UnitVec3& axis, + const Variation& dK, + Matrix& J, + bool invertV = false) +{ + addDirectionJacobian(e, axis, dK[1], J, invertV); + addBlock(~dK[0] * cross(axis, e.d), J); +} + +} // namespace + +//============================================================================== +// CABLE SPAN +//============================================================================== + +CableSpan::CableSpan( + CableSubsystem& subsystem, + const MobilizedBody& originBody, + const Vec3& defaultOriginPoint, + const MobilizedBody& terminationBody, + const Vec3& defaultTerminationPoint) : + m_Impl(std::shared_ptr(new Impl( + subsystem, + originBody, + defaultOriginPoint, + terminationBody, + defaultTerminationPoint))) {} - for (const CurveSegment& segment : m_CurveSegments) { - segment.getImpl().applyBodyForce(s, tension, bodyForcesInG); - } +CurveSegmentIndex CableSpan::adoptSegment(const CurveSegment& segment) +{ + return updImpl().adoptSegment(segment); } -//============================================================================== -// SUBSYSTEM -//============================================================================== - -bool CableSubsystem::isInstanceOf(const Subsystem& s) +int CableSpan::getNumCurveSegments() const { - return Impl::isA(s.getSubsystemGuts()); + return getImpl().getNumCurveSegments(); } -const CableSubsystem& CableSubsystem::downcast(const Subsystem& s) +const CurveSegment& CableSpan::getCurveSegment(CurveSegmentIndex ix) const { - assert(isInstanceOf(s)); - return static_cast(s); + return getImpl().getCurveSegment(ix); } -CableSubsystem& CableSubsystem::updDowncast(Subsystem& s) + +Real CableSpan::getLength(const State& s) const { - assert(isInstanceOf(s)); - return static_cast(s); + return getImpl().getPosInfo(s).l; } -const CableSubsystem::Impl& CableSubsystem::getImpl() const +Real CableSpan::getLengthDot(const State& s) const { - return SimTK_DYNAMIC_CAST_DEBUG(getSubsystemGuts()); + return getImpl().getVelInfo(s).lengthDot; } -CableSubsystem::Impl& CableSubsystem::updImpl() +void CableSpan::applyBodyForces( + const State& s, + Real tension, + Vector_& bodyForcesInG) const { - return SimTK_DYNAMIC_CAST_DEBUG(updSubsystemGuts()); + return getImpl().applyBodyForces(s, tension, bodyForcesInG); } -// 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() +//============================================================================== +// CABLE SPAN IMPL +//============================================================================== + +void CableSpan::Impl::realizeTopology(State& s) { - adoptSubsystemGuts(new Impl()); + PosInfo posInfo{}; + m_PosInfoIx = m_Subsystem.allocateCacheEntry( + s, + Stage::Position, + new Value(posInfo)); + + VelInfo velInfo{}; + m_VelInfoIx = m_Subsystem.allocateCacheEntry( + s, + Stage::Velocity, + new Value(velInfo)); } -CableSubsystem::CableSubsystem(MultibodySystem& mbs) +void CableSpan::Impl::realizePosition(const State& s) const { - adoptSubsystemGuts(new Impl()); - mbs.adoptSubsystem(*this); -} // steal ownership + if (m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { + return; + } + calcPosInfo(s, updPosInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); +} -int CableSubsystem::getNumPaths() const +void CableSpan::Impl::realizeVelocity(const State& s) const { - return getImpl().getNumPaths(); + if (m_Subsystem.isCacheValueRealized(s, m_VelInfoIx)) { + return; + } + calcVelInfo(s, updVelInfo(s)); + m_Subsystem.markCacheValueRealized(s, m_VelInfoIx); } -const CableSpan& CableSubsystem::getPath(CableSpanIndex ix) const +const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo( + const State& s) const { - return getImpl().getCablePath(ix); + realizePosition(s); + return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); } -CableSpan& CableSubsystem::updPath(CableSpanIndex ix) +CableSpan::Impl::PosInfo& CableSpan::Impl::updPosInfo(const State& s) const { - return updImpl().updCablePath(ix); + return Value::updDowncast( + m_Subsystem.updCacheEntry(s, m_PosInfoIx)); } -//============================================================================== -// GEODESIC -//============================================================================== - -namespace +const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo( + const State& s) const { + realizeVelocity(s); + return Value::downcast(m_Subsystem.getCacheEntry(s, m_VelInfoIx)); +} -struct ImplicitGeodesicState +CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const { - ImplicitGeodesicState() = default; - - explicit ImplicitGeodesicState( - Vec<10, Real>&& implicitGeodesicStateAsVector) - { - asVecMut() = implicitGeodesicStateAsVector; - } - - ImplicitGeodesicState(Vec3 point, Vec3 tangent) : x(point), t(tangent){}; + return Value::updDowncast( + m_Subsystem.updCacheEntry(s, m_VelInfoIx)); +} - const Vec<10, Real>& asVec() const - { - return reinterpret_cast&>(x[0]); - } +void CableSpan::Impl::calcInitZeroLengthGeodesic(State& s) const +{ + Vec3 prev_QG = + m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + size_t i = 0; + for (const CurveSegment& obstacle : m_CurveSegments) { + if (obstacle.getImpl().getStatus(s) == Status::Disabled) { + continue; + } + obstacle.getImpl().calcInitZeroLengthGeodesic(s, prev_QG); - Vec<10, Real>& asVecMut() - { - return reinterpret_cast&>(x[0]); + prev_QG = obstacle.getImpl().getPosInfo(s).KQ.p(); } +} - 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()}; - ; +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().isActive(s)) { + return &m_CurveSegments.at(CurveSegmentIndex(i)); + } } + return nullptr; +} - 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) +const CurveSegment* CableSpan::Impl::findNextActiveCurveSegment( + const State& s, + CurveSegmentIndex ix) const { - size_t it = 0; - for (; it < maxIter; ++it) { - const Real c = geometry.calcSurfaceValue(x); - - if (std::abs(c) < eps) { - break; + // Find the active segment after the current. + for (int i = ix + 1; i < m_CurveSegments.size(); ++i) { + if (m_CurveSegments.at(CurveSegmentIndex(i)).getImpl().isActive(s)) { + return &m_CurveSegments.at(CurveSegmentIndex(i)); } - - const Vec3 g = geometry.calcSurfaceGradient(x); - x += -g * c / dot(g, g); } - - SimTK_ASSERT( - it < maxIter, - "Surface projection failed: Reached max iterations"); - - UnitVec3 n(geometry.calcSurfaceGradient(x)); - t = t - dot(n, t) * n; - Real norm = t.norm(); - SimTK_ASSERT(!isNaN(norm), "Surface projection failed: Detected NaN"); - SimTK_ASSERT( - norm > 1e-13, - "Surface projection failed: Tangent guess is parallel to surface " - "normal"); - t = t / norm; + return nullptr; } -ImplicitGeodesicState operator-( - const ImplicitGeodesicState& lhs, - const ImplicitGeodesicState& rhs) +Vec3 CableSpan::Impl::findPrevPoint(const State& s, CurveSegmentIndex ix) const { - ImplicitGeodesicState out; - out.asVecMut() = lhs.asVec() - rhs.asVec(); - return out; + const CurveSegment* segment = findPrevActiveCurveSegment(s, ix); + return segment ? segment->getImpl().getPosInfo(s).KQ.p() + : m_OriginBody.getBodyTransform(s).shiftFrameStationToBase( + m_OriginPoint); } -ImplicitGeodesicState operator+( - const ImplicitGeodesicState& lhs, - const Vec<10, Real>& rhs) +Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) const { - ImplicitGeodesicState out; - out.asVecMut() = lhs.asVec() + rhs; - return out; + const CurveSegment* segment = findNextActiveCurveSegment(s, ix); + return segment + ? segment->getImpl().getPosInfo(s).KP.p() + : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( + m_TerminationPoint); } -Real calcInfNorm(const ImplicitGeodesicState& q) +template +void CableSpan::Impl::calcPathErrorVector( + const State& s, + const std::vector& lines, + std::array axes, + Vector& pathError) const { - Real infNorm = 0.; - for (size_t r = 0; r < q.asVec().nrow(); ++r) { - infNorm = std::max(infNorm, q.asVec()[r]); + size_t lineIx = 0; + ptrdiff_t row = -1; + + for (int i = 0; i < getNumCurveSegments(); ++i) { + const CurveSegment& segment = getCurveSegment(CurveSegmentIndex(i)); + if (!segment.getImpl().isActive(s)) { + 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); + } } - return infNorm; } -class RKM +template +void CableSpan::Impl::calcPathErrorJacobian( + const State& s, + const std::vector& lines, + std::array axes, + Matrix& J) const { - using Y = ImplicitGeodesicState; - using DY = Vec<10, Real>; + constexpr size_t Nq = GeodesicDOF; -public: - RKM() = default; + // TODO perhaps just not make method static. + const size_t n = lines.size() - 1; - // Integrate y0, populating y1, y2. - Real step(Real h, std::function& f); + 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"); - struct Sample - { - Real l; - FrenetFrame K; - }; + size_t row = 0; + size_t col = 0; + size_t activeIx = 0; + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().isActive(s)) { + continue; + } + const GeodesicInfo& g = segment.getImpl().getPosInfo(s); - Real stepTo( - Y y0, - Real x1, - Real h0, - std::function& f, // Dynamics - std::function& g, // Surface projection - std::function& m, // State log - Real accuracy); + const LineSegment& l_P = lines.at(activeIx); + const LineSegment& l_Q = lines.at(activeIx + 1); - size_t getNumberOfFailedSteps() const - { - return _failedCount; - } + const CurveSegmentIndex ix = segment.getImpl().getIndex(); + const CurveSegment* prev = findPrevActiveCurveSegment(s, ix); + const CurveSegment* next = findNextActiveCurveSegment(s, ix); - size_t getInitStepSize() const - { - return _h0; - } + for (CoordinateAxis axis : axes) { + const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); + const Variation& dK_P = g.dKP; -private: - static constexpr size_t ORDER = 5; + addPathErrorJacobian(l_P, a_P, dK_P, J.block(row, col, 1, Nq)); - std::array _k{}; - std::array _y{}; + if (prev) { + const Variation& prev_dK_Q = prev->getImpl().getPosInfo(s).dKQ; + addDirectionJacobian( + l_P, + a_P, + prev_dK_Q[1], + J.block(row, col - Nq, 1, Nq), + true); + } + ++row; + } - Real _hMin = 1e-10; - Real _hMax = 1e-1; - Real _h = NaN; - Real _h0 = NaN; - Real _e = NaN; + for (CoordinateAxis axis : axes) { + const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); + const Variation& dK_Q = g.dKQ; - size_t _failedCount = 0; -}; + addPathErrorJacobian( + l_Q, + a_Q, + dK_Q, + J.block(row, col, 1, Nq), + true); -Real RKM::step(Real h, std::function& f) + if (next) { + const Variation& next_dK_P = next->getImpl().getPosInfo(s).dKP; + addDirectionJacobian( + l_Q, + a_Q, + next_dK_P[1], + J.block(row, col + Nq, 1, Nq)); + } + ++row; + } + + col += Nq; + }; +} + +Real CableSpan::Impl::calcPathLength( + const State& s, + const std::vector& lines) const { - Y& yk = _y.at(1); + Real lTot = 0.; + for (const LineSegment& line : lines) { + // TODO spell out as length. + lTot += line.l; + } - for (size_t i = 0; i < 5; ++i) { - yk = _y.at(0) + (h / 3.) * _k.at(0); + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().isActive(s)) { + continue; + } + lTot += segment.getImpl().getPosInfo(s).length; } + return lTot; +} - // k1 - _k.at(0) = f(_y.at(0)); +void CableSpan::Impl::calcLineSegments( + const State& s, + Vec3 p_O, + Vec3 p_I, + std::vector& lines) const +{ + lines.resize(m_CurveSegments.size() + 1); + lines.clear(); - // k2 - { - yk = _y.at(0) + (h / 3.) * _k.at(0); - _k.at(1) = f(yk); - } + Vec3 lineStart = std::move(p_O); + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().isActive(s)) { + continue; + } - // k3 - { - yk = _y.at(0) + (h / 6.) * _k.at(0) + (h / 6.) * _k.at(1); - _k.at(2) = f(yk); - } + const GeodesicInfo& g = segment.getImpl().getPosInfo(s); + const Vec3 lineEnd = g.KP.p(); + lines.emplace_back(lineStart, lineEnd); - // k4 - { - yk = _y.at(0) + (1. / 8. * h) * _k.at(0) + (3. / 8. * h) * _k.at(2); - _k.at(3) = f(yk); + lineStart = g.KQ.p(); } + lines.emplace_back(lineStart, p_I); +} - // 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); +size_t CableSpan::Impl::countActive(const State& s) const +{ + size_t count = 0; + for (const CurveSegment& segment : m_CurveSegments) { + if (segment.getImpl().isActive(s)) { + ++count; + } } + return count; +} - // y1: Auxiliary --> Already updated in k5 computation. +void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const +{ + // Path origin and termination point. + const Vec3 x_O = + m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + const Vec3 x_I = + m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( + m_TerminationPoint); - // 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); + const std::array axes{NormalAxis, BinormalAxis}; - return calcInfNorm(_y.at(1) - _y.at(2)) * 0.2; -} + for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; + ++posInfo.loopIter) { + const size_t nActive = countActive(s); -Real RKM::stepTo( - Y y0, - Real x1, - Real h0, - std::function& f, - std::function& g, - std::function& m, - Real accuracy) -{ - g(y0); - m(0., y0); + // Grab the shared data cache for computing the matrices, and lock it. + SolverDataCache& dataCache = findDataCache(nActive); + try { + dataCache.lock(); - _y.at(0) = std::move(y0); - _h = h0; - _e = 0.; - _failedCount = 0; + SolverData& data = dataCache.updData(); - Real x = 0.; - while (x < x1 - 1e-13) { - const bool init = x == 0.; + // Compute the straight-line segments. + calcLineSegments(s, x_O, x_I, data.lineSegments); - _h = x + _h > x1 ? x1 - x : _h; + // Evaluate path error, and stop when converged. + calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); + const Real maxPathError = data.pathError.normInf(); + if (maxPathError < m_PathErrorBound) { + return; + } - // Attempt step. - Real err = step(_h, f); + // Evaluate the path error jacobian. + calcPathErrorJacobian<2>( + s, + data.lineSegments, + axes, + data.pathErrorJacobian); - // 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)); - } + // Compute path corrections. + const Correction* corrIt = calcPathCorrections(data); - // Potentially increase stepsize. - if (err < accuracy / 64.) { - _h *= 2.; + // Apply path corrections. + for (const CurveSegment& obstacle : m_CurveSegments) { + if (!obstacle.getImpl().isActive(s)) { + continue; + } + obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); + ++corrIt; + } + + } catch (const std::exception& e) { + dataCache.unlock(); + throw e; } - _e = std::max(_e, err); - - SimTK_ASSERT( - _h > _hMin, - "Geodesic Integrator failed: Reached very small stepsize"); - SimTK_ASSERT( - _h < _hMax, - "Geodesic Integrator failed: Reached very large stepsize"); + // Release the lock on the shared data. + dataCache.unlock(); - if (init) { - _h0 = _h; + // Path has changed: invalidate each segment's cache. + for (const CurveSegment& obstacle : m_CurveSegments) { + obstacle.getImpl().invalidatePositionLevelCache(s); } } -} -Real calcNormalCurvature( - const ContactGeometry& geometry, - Vec3 point, - Vec3 tangent) -{ - const Vec3& p = point; - const Vec3& v = tangent; - const Vec3 g = geometry.calcSurfaceGradient(p); - const Vec3 h_v = geometry.calcSurfaceHessian(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 = geometry.calcSurfaceGradient(p); - const Vec3 h_v = geometry.calcSurfaceHessian(p) * v; - const Vec3 gxv = cross(g, v); - return -dot(h_v, gxv) / dot(g, g); + throw std::runtime_error("Failed to converge"); } -UnitVec3 calcSurfaceNormal(const ContactGeometry& geometry, Vec3 point) +void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const { - const Vec3& p = point; - const Vec3 gradient = geometry.calcSurfaceGradient(p); - return UnitVec3{gradient}; -} + const PosInfo& pos = getPosInfo(s); -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); -} + Real lengthDot = 0.; -Mat33 calcAdjoint(const Mat33& mat) -{ - Real fxx = mat(0, 0); - Real fyy = mat(1, 1); - Real fzz = mat(2, 2); + Vec3 v_GQ = m_OriginBody.findStationVelocityInGround(s, m_OriginPoint); + const CurveSegment* lastActive = nullptr; + for (const CurveSegment& obstacle : m_CurveSegments) { + if (!obstacle.getImpl().isActive(s)) { + continue; + } - Real fxy = mat(0, 1); - Real fxz = mat(0, 2); - Real fyz = mat(1, 2); + const GeodesicInfo& g = obstacle.getImpl().getPosInfo(s); + const UnitVec3 e_G = g.KP.R().getAxisUnitVec(TangentAxis); - 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; -} + Vec3 next_v_GQ; + Vec3 v_GP; + obstacle.getImpl().calcContactPointVelocitiesInGround( + s, + v_GP, + next_v_GQ); -Real calcGaussianCurvature(const ContactGeometry& geometry, Vec3 point) -{ - const Vec3& p = point; - Vec3 g = geometry.calcSurfaceGradient(p); - Real gDotg = dot(g, g); - Mat33 adj = calcAdjoint(geometry.calcSurfaceHessian(p)); + lengthDot += dot(e_G, v_GP - v_GQ); - if (gDotg * gDotg < 1e-13) { - throw std::runtime_error( - "Gaussian curvature inaccurate: are we normal to surface?"); + v_GQ = next_v_GQ; + lastActive = &obstacle; } - return (dot(g, adj * g)) / (gDotg * gDotg); -} + const Vec3 v_GP = + m_TerminationBody.findStationVelocityInGround(s, m_TerminationPoint); + const UnitVec3 e_G = + lastActive ? lastActive->getImpl().getPosInfo(s).KQ.R().getAxisUnitVec( + TangentAxis) + : UnitVec3(pos.xI - pos.xO); -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); + lengthDot += dot(e_G, v_GP - v_GQ); } -void calcGeodesicBoundaryState( - const ContactGeometry& geometry, - const ImplicitGeodesicState& q, - bool isEnd, - FrenetFrame& K, - Mat34& v, - Mat34& w) +void CableSpan::Impl::applyBodyForces( + const State& s, + Real tension, + Vector_& bodyForcesInG) const { - calcFrenetFrame(geometry, q, K); + // TODO why? + if (tension <= 0) { + return; + } - const UnitVec3& t = K.R().getAxisUnitVec(TangentAxis); - const UnitVec3& n = K.R().getAxisUnitVec(NormalAxis); - const UnitVec3& b = K.R().getAxisUnitVec(BinormalAxis); + realizePosition(s); - // TODO remove fourth element? - v.col(0) = t; - v.col(1) = b * q.a; - v.col(2) = b * q.r; - v.col(3) = isEnd ? v.col(0) : Vec3{0.}; + for (const CurveSegment& segment : m_CurveSegments) { + segment.getImpl().applyBodyForce(s, tension, bodyForcesInG); + } +} - const Real tau_g = geometry.calcGeodesicTorsion(q.x, t); - const Real kappa_n = geometry.calcNormalCurvature(q.x, t); - const Real kappa_a = geometry.calcNormalCurvature(q.x, b); +//============================================================================== +// SUBSYSTEM +//============================================================================== - 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.}; +bool CableSubsystem::isInstanceOf(const Subsystem& s) +{ + return Impl::isA(s.getSubsystemGuts()); } -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) +const CableSubsystem& CableSubsystem::downcast(const Subsystem& s) { - 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) { - log.emplace_back(l, q); - }; + assert(isInstanceOf(s)); + return static_cast(s); +} +CableSubsystem& CableSubsystem::updDowncast(Subsystem& s) +{ + assert(isInstanceOf(s)); + return static_cast(s); +} - Y y0(x, t); +const CableSubsystem::Impl& CableSubsystem::getImpl() const +{ + return SimTK_DYNAMIC_CAST_DEBUG(getSubsystemGuts()); +} +CableSubsystem::Impl& CableSubsystem::updImpl() +{ + return SimTK_DYNAMIC_CAST_DEBUG(updSubsystemGuts()); +} - RKM rkm{}; +// 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()); +} - rkm.stepTo(y0, l, ds, f, g, m, accuracy); +CableSubsystem::CableSubsystem(MultibodySystem& mbs) +{ + adoptSubsystemGuts(new Impl()); + mbs.adoptSubsystem(*this); +} // steal ownership - SimTK_ASSERT(log.size() > 0, "Failed to integrate geodesic: Log is empty"); - calcGeodesicBoundaryState( - geometry, - log.front().second, - false, - K_P, - dK_P[1], - dK_P[0]); - calcGeodesicBoundaryState( - geometry, - log.back().second, - true, - K_Q, - dK_Q[1], - dK_Q[0]); +int CableSubsystem::getNumPaths() const +{ + return getImpl().getNumPaths(); +} - ds = rkm.getInitStepSize(); +const CableSpan& CableSubsystem::getPath(CableSpanIndex ix) const +{ + return getImpl().getCablePath(ix); } -} // namespace +CableSpan& CableSubsystem::updPath(CableSpanIndex ix) +{ + return updImpl().updCablePath(ix); +} diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 8ff89553b..37a8682dd 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -20,8 +20,7 @@ namespace SimTK //============================================================================== class CurveSegment::Impl { - public: - +public: using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; @@ -62,13 +61,13 @@ class CurveSegment::Impl // Some info that can be retrieved from cache. struct LocalGeodesicInfo { - FrenetFrame KP{}; - FrenetFrame KQ{}; + FrenetFrame K_P{}; + FrenetFrame K_Q{}; Real length = NaN; - Variation dKP{}; - Variation dKQ{}; + Variation dK_P{}; + Variation dK_Q{}; Status status = Status::Ok; }; @@ -148,6 +147,12 @@ class CurveSegment::Impl return getCacheEntry(s).status; } + struct LocalGeodesicSample + { + Real length; + FrenetFrame frame; + }; + private: // The cache entry: Curve in local surface coordinated. // This is an auto update discrete cache variable, which makes it @@ -156,8 +161,7 @@ class CurveSegment::Impl struct CacheEntry : LocalGeodesicInfo { Vec3 trackingPointOnLine{NaN, NaN, NaN}; - std::vector frames{}; // Empty for analytic geoemetry - // with no allocation overhead. + std::vector samples; double sHint = NaN; }; @@ -214,9 +218,9 @@ class CurveSegment::Impl Vec3 m_InitPointGuess; - size_t m_ProjectionMaxIter = 10; - Real m_ProjectionRequiredAccuracy = 1e-10; - Real m_IntegratorAccuracy = 1e-6; + size_t m_ProjectionMaxIter = 10; + Real m_ProjectionAccuracy = 1e-10; + Real m_IntegratorAccuracy = 1e-6; Real m_TouchdownAccuracy = 1e-3; size_t m_TouchdownIter = 10; @@ -236,11 +240,11 @@ class CurveSegment::Impl // TODO you would expect the constructor to take the index as well here? Impl( - CableSpan path, - const MobilizedBody& mobod, - const Transform& X_BS, - ContactGeometry geometry, - Vec3 initPointGuess); + CableSpan path, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess); // Position level cache: Curve in ground frame. struct PosInfo From 6f816a21bef44dd8b3dde2e9fbcfa81f794d05f4 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 29 Apr 2024 13:51:34 +0200 Subject: [PATCH 039/127] fmt --- Simbody/include/simbody/internal/Wrapping.cpp | 103 ++++++++---------- 1 file changed, 48 insertions(+), 55 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 6896d1aa2..82ce042a8 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -6,8 +6,8 @@ #include "WrappingImpl.h" #include "simmath/internal/ContactGeometry.h" #include -#include #include +#include using namespace SimTK; @@ -16,7 +16,8 @@ using FrameVariation = ContactGeometry::GeodesicFrameVariation; using FrenetFrame = ContactGeometry::FrenetFrame; using GeodesicInfo = CurveSegment::Impl::PosInfo; using LocalGeodesicInfo = CurveSegment::Impl::LocalGeodesic::LocalGeodesicInfo; -using LocalGeodesicSample = CurveSegment::Impl::LocalGeodesic::LocalGeodesicSample; +using LocalGeodesicSample = + CurveSegment::Impl::LocalGeodesic::LocalGeodesicSample; using GeodesicInitialConditions = CurveSegment::Impl::LocalGeodesic::GeodesicInitialConditions; using GeodesicJacobian = Vec4; @@ -125,7 +126,7 @@ void calcSurfaceProjectionFast( Real calcPointOnLineNearOriginAsFactor(Vec3 a, Vec3 b) { const Vec3 e = b - a; - Real c = -dot(a,e) / dot(e,e); + Real c = -dot(a, e) / dot(e, e); return std::max(0., std::min(1., c)); }; @@ -135,12 +136,12 @@ Real calcPointOnLineNearPointAsFactor(Vec3 a, Vec3 b, Vec3 point) }; bool calcNearestPointOnLineImplicitly( - const ContactGeometry& geometry, - Vec3 a, - Vec3 b, - Vec3& point, - size_t maxIter, - double eps) + const ContactGeometry& geometry, + Vec3 a, + Vec3 b, + Vec3& point, + size_t maxIter, + double eps) { // Initial guess. double alpha = calcPointOnLineNearPointAsFactor(a, b, point); @@ -159,14 +160,14 @@ bool calcNearestPointOnLineImplicitly( break; // Gradient at point on line. - const Vec3 g = geometry.calcSurfaceGradient(pl); + const Vec3 g = geometry.calcSurfaceGradient(pl); const Mat33 H = geometry.calcSurfaceHessian(pl); // Add a weight to the newton step to avoid large steps. constexpr double w = 0.5; // Update alpha. - const double step = dot(g,d) / (dot(d,H * d) + w); + const double step = dot(g, d) / (dot(d, H * d) + w); // Stop when converged. if (std::abs(step) < eps) @@ -195,7 +196,8 @@ bool calcNearestPointOnLineImplicitly( std::cout << "p = " << point << "\n"; std::cout << "c = " << alpha << "\n"; // TODO use SimTK_ASSERT - throw std::runtime_error("Failed to compute point on line nearest surface: Reached max iterations"); + throw std::runtime_error("Failed to compute point on line nearest " + "surface: Reached max iterations"); } // Return number of iterations required. @@ -558,20 +560,8 @@ void calcGeodesicAndVariationImplicitly( 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]); + calcGeodesicBoundaryState(geometry, y0, false, K_P, dK_P[1], dK_P[0]); + calcGeodesicBoundaryState(geometry, y1, true, K_Q, dK_Q[1], dK_Q[0]); ds = rkm.getInitStepSize(); } @@ -943,19 +933,19 @@ void LocalGeodesic::shootNewGeodesic( CacheEntry& cache) const { calcGeodesicAndVariationImplicitly( - m_Geometry, - g0.x, - g0.t, - g0.l, - cache.sHint, - cache.K_P, - cache.dK_P, - cache.K_Q, - cache.dK_Q, - m_IntegratorAccuracy, - m_ProjectionMaxIter, - m_ProjectionAccuracy, - cache.samples); + m_Geometry, + g0.x, + g0.t, + g0.l, + cache.sHint, + cache.K_P, + cache.dK_P, + cache.K_Q, + cache.dK_Q, + m_IntegratorAccuracy, + m_ProjectionMaxIter, + m_ProjectionAccuracy, + cache.samples); /* using Y = ImplicitGeodesicState; */ /* if (m_Geometry.analyticFormAvailable()) { */ @@ -997,17 +987,19 @@ void LocalGeodesic::shootNewGeodesic( //============================================================================== CurveSegment::CurveSegment( - CableSpan cable, - const MobilizedBody& mobod, - Transform X_BS, - const ContactGeometry& geometry, - Vec3 xHint) - : m_Impl(std::shared_ptr(new CurveSegment::Impl(cable, mobod, X_BS, geometry, xHint))) + CableSpan cable, + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 xHint) : + m_Impl(std::shared_ptr( + new CurveSegment::Impl(cable, mobod, X_BS, geometry, xHint))) { // TODO bit awkward to set the index later. updImpl().setIndex(cable.adoptSegment(*this)); /* CurveSegmentIndex ix = cable.adoptSegment(*this); */ - /* m_Impl = std::shared_ptr(new CurveSegment::Impl(cable, ix, mobod, X_BS, geometry, xHint)); */ + /* m_Impl = std::shared_ptr(new + * CurveSegment::Impl(cable, ix, mobod, X_BS, geometry, xHint)); */ } const CableSpan& CurveSegment::getCable() const @@ -1255,17 +1247,18 @@ GeodesicJacobian addPathErrorJacobian( //============================================================================== CableSpan::CableSpan( - CableSubsystem& subsystem, - const MobilizedBody& originBody, - const Vec3& defaultOriginPoint, - const MobilizedBody& terminationBody, - const Vec3& defaultTerminationPoint) : + CableSubsystem& subsystem, + const MobilizedBody& originBody, + const Vec3& defaultOriginPoint, + const MobilizedBody& terminationBody, + const Vec3& defaultTerminationPoint) : m_Impl(std::shared_ptr(new Impl( subsystem, originBody, defaultOriginPoint, terminationBody, - defaultTerminationPoint))) {} + defaultTerminationPoint))) +{} CurveSegmentIndex CableSpan::adoptSegment(const CurveSegment& segment) { @@ -1292,9 +1285,9 @@ Real CableSpan::getLengthDot(const State& s) const return getImpl().getVelInfo(s).lengthDot; } void CableSpan::applyBodyForces( - const State& s, - Real tension, - Vector_& bodyForcesInG) const + const State& s, + Real tension, + Vector_& bodyForcesInG) const { return getImpl().applyBodyForces(s, tension, bodyForcesInG); } From 2297fe3b1ab0a7a530bc63d5bdb63e3a5292180c Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 29 Apr 2024 13:54:30 +0200 Subject: [PATCH 040/127] rename m_Surface to m_Geodesic --- Simbody/include/simbody/internal/Wrapping.cpp | 20 ++++++++++--------- .../include/simbody/internal/WrappingImpl.h | 6 +++--- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 82ce042a8..bb89fab65 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1030,9 +1030,11 @@ CurveSegment::Impl::Impl( ContactGeometry geometry, Vec3 initPointGuess) : m_Subsystem(path.getImpl().getSubsystem()), - m_Path(path), m_Index(-1), // TODO what to do with this index, and when - m_Mobod(mobod), m_Offset(X_BS), - m_Surface(m_Subsystem, geometry, initPointGuess) + m_Path(path), + m_Index(-1), // TODO what to do with this index, and when + m_Mobod(mobod), + m_Offset(X_BS), + m_Geodesic(m_Subsystem, geometry, initPointGuess) {} void CurveSegment::Impl::realizeTopology(State& s) @@ -1061,11 +1063,11 @@ void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) Vec3 prev_QS = X_GS.shiftBaseStationToFrame(prev_QG); Vec3 xGuess_S = - m_Surface.getInitialPointGuess(); // TODO move into function call? + m_Geodesic.getInitialPointGuess(); // TODO move into function call? GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateZeroLengthGuess(prev_QS, xGuess_S); - m_Surface.calcInitialGeodesic(s, g0); + m_Geodesic.calcInitialGeodesic(s, g0); m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); } @@ -1075,7 +1077,7 @@ void CurveSegment::Impl::applyGeodesicCorrection( const CurveSegment::Impl::Correction& c) const { // Apply correction to curve. - m_Surface.applyGeodesicCorrection(s, c); + m_Geodesic.applyGeodesicCorrection(s, c); // Invalidate position level cache. m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); @@ -1087,7 +1089,7 @@ void CurveSegment::Impl::calcPathPoints( { const Transform& X_GS = getPosInfo(s).X_GS; size_t initSize = points.size(); - m_Surface.calcPathPoints(s, points); + m_Geodesic.calcPathPoints(s, points); for (size_t i = points.size() - initSize; i < points.size(); ++i) { points.at(i) = X_GS.shiftFrameStationToBase(points.at(i)); } @@ -1133,7 +1135,7 @@ void xformSurfaceGeodesicToGround( void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { - if (m_Surface.getStatus(s) == Status::Disabled) { + if (m_Geodesic.getStatus(s) == Status::Disabled) { return; } @@ -1152,7 +1154,7 @@ void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // TODO this doesnt follow the regular invalidation scheme... // Grab the last geodesic that was computed. const LocalGeodesicInfo& geodesic_S = - m_Surface.calcLocalGeodesicInfo(s, prev_S, next_S); + m_Geodesic.calcLocalGeodesicInfo(s, prev_S, next_S); // Store the the local geodesic in ground frame. xformSurfaceGeodesicToGround(geodesic_S, X_GS, updPosInfo(s)); diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 37a8682dd..368d2971b 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -304,12 +304,12 @@ class CurveSegment::Impl bool isActive(const State& s) const { - return m_Surface.getStatus(s) == Status::Ok; + return m_Geodesic.getStatus(s) == Status::Ok; } Status getStatus(const State& s) const { - return m_Surface.getStatus(s); + return m_Geodesic.getStatus(s); } void calcInitZeroLengthGeodesic(State& state, Vec3 prev_QS) const; @@ -379,7 +379,7 @@ class CurveSegment::Impl MobilizedBody m_Mobod; Transform m_Offset; - LocalGeodesic m_Surface; // TODO rename + LocalGeodesic m_Geodesic; // TOPOLOGY CACHE CacheEntryIndex m_PosInfoIx; From 83e58bc895e641c55d5dc3b2c26e0e54b2ac4aef Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 29 Apr 2024 14:06:56 +0200 Subject: [PATCH 041/127] add missing implementations for CableSpan::Impl --- Simbody/include/simbody/internal/Wrapping.cpp | 22 ++++++++++++++++--- .../include/simbody/internal/WrappingImpl.h | 16 +++----------- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index bb89fab65..468385a22 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1055,6 +1055,17 @@ void CurveSegment::Impl::realizePosition(const State& s) const } } +void CurveSegment::Impl::realizeCablePosition(const State& s) const +{ + m_Path.getImpl().realizePosition(s); +} + +void CurveSegment::Impl::invalidatePositionLevelCache(const State& state) const +{ + m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); + m_Path.getImpl().invalidatePositionLevelCache(state); +} + void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) const { @@ -1069,7 +1080,7 @@ void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) GeodesicInitialConditions::CreateZeroLengthGuess(prev_QS, xGuess_S); m_Geodesic.calcInitialGeodesic(s, g0); - m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); + invalidatePositionLevelCache(s); } void CurveSegment::Impl::applyGeodesicCorrection( @@ -1080,7 +1091,7 @@ void CurveSegment::Impl::applyGeodesicCorrection( m_Geodesic.applyGeodesicCorrection(s, c); // Invalidate position level cache. - m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); + invalidatePositionLevelCache(s); } void CurveSegment::Impl::calcPathPoints( @@ -1331,6 +1342,11 @@ void CableSpan::Impl::realizeVelocity(const State& s) const m_Subsystem.markCacheValueRealized(s, m_VelInfoIx); } +void CableSpan::Impl::invalidatePositionLevelCache(const State& s) const +{ + m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); +} + const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo( const State& s) const { @@ -1357,7 +1373,7 @@ CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const m_Subsystem.updCacheEntry(s, m_VelInfoIx)); } -void CableSpan::Impl::calcInitZeroLengthGeodesic(State& s) const +void CableSpan::Impl::calcInitCablePath(State& s) const { Vec3 prev_QG = m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 368d2971b..7eb972084 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -258,10 +258,6 @@ class CurveSegment::Impl Variation dKQ{}; Real length = NaN; - - // TODO add force & moment here? - /* Vec3 unitForce {}; */ - /* Vec3 unitMoment {}; */ }; // Allocate state variables and cache entries. @@ -269,16 +265,9 @@ class CurveSegment::Impl /* void realizeInstance(const State& state) const; */ void realizePosition(const State& state) const; - /* void realizeVelocity(const State& state) const; */ - /* void realizeAcceleration(const State& state) const; */ - /* void invalidateTopology(); */ - void realizeCablePosition(const State& state) const; - void invalidatePositionLevelCache(const State& state) const - { - m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); - } + void invalidatePositionLevelCache(const State& state) const; const CableSpan& getCable() const { @@ -447,11 +436,12 @@ class CableSpan::Impl { m_Subsystem.invalidateSubsystemTopologyCache(); } + void invalidatePositionLevelCache(const State& state) const; const PosInfo& getPosInfo(const State& state) const; const VelInfo& getVelInfo(const State& state) const; - void calcInitZeroLengthGeodesic(State& s) const; + void calcInitCablePath(State& s) const; void applyBodyForces( const State& state, From da7476f76392ae5a23cea41f3c26f5a7ee76fc44 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 09:24:55 +0200 Subject: [PATCH 042/127] add convenience method for adding curve segments --- Simbody/include/simbody/internal/Wrapping.cpp | 11 ++++++++++- Simbody/include/simbody/internal/Wrapping.h | 9 ++++++++- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/include/simbody/internal/Wrapping.cpp index 468385a22..12e4ef9b8 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/include/simbody/internal/Wrapping.cpp @@ -1260,7 +1260,7 @@ GeodesicJacobian addPathErrorJacobian( //============================================================================== CableSpan::CableSpan( - CableSubsystem& subsystem, + CableSubsystem subsystem, const MobilizedBody& originBody, const Vec3& defaultOriginPoint, const MobilizedBody& terminationBody, @@ -1278,6 +1278,15 @@ CurveSegmentIndex CableSpan::adoptSegment(const CurveSegment& segment) return updImpl().adoptSegment(segment); } +void CableSpan::adoptWrappingObstacle( + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 contactPointHint) +{ + CurveSegment(*this, mobod, X_BS, geometry, contactPointHint); +} + int CableSpan::getNumCurveSegments() const { return getImpl().getNumCurveSegments(); diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index b6d2505b0..b7f2a417c 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -93,7 +93,7 @@ class SimTK_SIMBODY_EXPORT CableSpan public: CableSpan( - CableSubsystem& subsystem, + CableSubsystem subsystem, const MobilizedBody& originBody, const Vec3& defaultOriginPoint, const MobilizedBody& terminationBody, @@ -101,12 +101,19 @@ class SimTK_SIMBODY_EXPORT CableSpan CurveSegmentIndex adoptSegment(const CurveSegment& segment); + void adoptWrappingObstacle( + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 contactPointHint = {1., 0., 0.}); + int getNumCurveSegments() const; const CurveSegment& getCurveSegment(CurveSegmentIndex ix) const; Real getLength(const State& state) const; Real getLengthDot(const State& state) const; + Real calcCablePower(const State& state, Real tension) const {return NaN;} void applyBodyForces( const State& state, From 697ec7d3e37a6e01c2a957cf4cc7f397fd2c155d Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 09:25:21 +0200 Subject: [PATCH 043/127] wip: add wrapping example --- examples/ExampleCableSpan.cpp | 396 ++++++++++++++++++++++++++++++++++ 1 file changed, 396 insertions(+) create mode 100644 examples/ExampleCableSpan.cpp diff --git a/examples/ExampleCableSpan.cpp b/examples/ExampleCableSpan.cpp new file mode 100644 index 000000000..c050bd125 --- /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), 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); + + 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(Rad, 0, 0), // origin + body5, + Vec3(0, 0, Rad)); // 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, cable2, 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 = 2; + 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"; + } +} From 4fde8e6d961d01b545f6fda0c8224a3b01f7200c Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 09:26:05 +0200 Subject: [PATCH 044/127] update installer script --- install-simbody | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/install-simbody b/install-simbody index 07266b398..3d2e46b56 100755 --- a/install-simbody +++ b/install-simbody @@ -4,9 +4,12 @@ set -xeuo pipefail WORKSPACE=$(dirname $(dirname $(realpath "$0"))) echo $WORKSPACE -env -C $WORKSPACE cmake -B "simbody-build" -S "simbody" -DCMAKE_INSTALL_PREFIX="simbody-install" -DCMAKE_EXPORT_COMPILE_COMMANDS="ON" +# env -C $WORKSPACE cmake -B "simbody-build" -S "simbody" -DCMAKE_INSTALL_PREFIX="simbody-install" -DCMAKE_EXPORT_COMPILE_COMMANDS="ON" 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 +# env -C "$WORKSPACE/simbody-build" ctest --parallel -j$(nproc) --output-on-failure + +LD_LIBRARY_PATH="$WORKSPACE/simbody-install/lib" +env -C "$WORKSPACE/simbody-build" ./ExampleCableSpan From 7831c241c06381f5be5cf4a274f707493c0ac027 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 09:40:38 +0200 Subject: [PATCH 045/127] add todo comment --- Simbody/include/simbody/internal/WrappingImpl.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/include/simbody/internal/WrappingImpl.h index 7eb972084..fdf50d363 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/include/simbody/internal/WrappingImpl.h @@ -84,6 +84,7 @@ class CurveSegment::Impl const Variation& dKP, Real l, const Correction& c); + // TODO remove this ctor. static GeodesicInitialConditions CreateFromGroundInSurfaceFrame( const Transform& X_GS, Vec3 x_G, From 923439fdfb8ff8f5de55327ffd91aaa0f1e02d5e Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 12:52:24 +0200 Subject: [PATCH 046/127] move Wrapping code to src dir --- .../simbody/internal => src}/Wrapping.cpp | 195 +++++------------- .../simbody/internal => src}/WrappingImpl.h | 74 ++++++- 2 files changed, 128 insertions(+), 141 deletions(-) rename Simbody/{include/simbody/internal => src}/Wrapping.cpp (91%) rename Simbody/{include/simbody/internal => src}/WrappingImpl.h (89%) diff --git a/Simbody/include/simbody/internal/Wrapping.cpp b/Simbody/src/Wrapping.cpp similarity index 91% rename from Simbody/include/simbody/internal/Wrapping.cpp rename to Simbody/src/Wrapping.cpp index 12e4ef9b8..db762427b 100644 --- a/Simbody/include/simbody/internal/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1,5 +1,3 @@ -#include "Wrapping.h" - #include "SimTKcommon/internal/CoordinateAxis.h" #include "SimTKcommon/internal/ExceptionMacros.h" #include "SimTKmath.h" @@ -25,15 +23,13 @@ using PointVariation = ContactGeometry::GeodesicPointVariation; using Variation = ContactGeometry::GeodesicVariation; using LineSegment = CableSpan::LineSegment; using Status = CurveSegment::Status; +using SolverData = CableSubsystem::Impl::SolverData; //============================================================================== // CONSTANTS //============================================================================== namespace { -static const CoordinateAxis TangentAxis = ContactGeometry::TangentAxis; -static const CoordinateAxis NormalAxis = ContactGeometry::NormalAxis; -static const CoordinateAxis BinormalAxis = ContactGeometry::BinormalAxis; static const int GeodesicDOF = 4; } // namespace @@ -506,7 +502,7 @@ void calcGeodesicBoundaryState( const UnitVec3& b = K.R().getAxisUnitVec(BinormalAxis); // TODO remove fourth element? - v.col(0) = t; + 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.}; @@ -550,7 +546,7 @@ void calcGeodesicAndVariationImplicitly( std::function m = [&](Real l, const Y& q) { FrenetFrame frame; calcFrenetFrame(geometry, q, frame); - log.emplace_back(l, frame); + log.push_back(LocalGeodesicSample(l, frame)); }; Y y0(x, t); @@ -572,79 +568,7 @@ void calcGeodesicAndVariationImplicitly( // SOLVER //============================================================================== -namespace -{ -class SolverDataCache; -SolverDataCache& findDataCache(size_t nActive); - -struct SolverData -{ - std::vector lineSegments; - - Matrix pathErrorJacobian; - Vector pathCorrection; - Vector pathError; - Matrix mat; - // TODO Cholesky decomposition... - FactorLU matInv; - Vector vec; -}; - -class SolverDataCache -{ -private: - SolverDataCache(size_t n) - { - static constexpr int Q = 4; - static constexpr int C = 4; - - m_Data.lineSegments.resize(n + 1); - m_Data.pathErrorJacobian = Matrix(C * n, Q * n, 0.); - m_Data.pathCorrection = Vector(Q * n, 0.); - m_Data.pathError = Vector(C * n, 0.); - m_Data.mat = Matrix(Q * n, Q * n, NaN); - m_Data.vec = Vector(Q * n, NaN); - } - -public: - void lock() - { - m_Mutex.lock(); - } - - void unlock() - { - m_Mutex.unlock(); - } - - SolverData& updData() - { - return m_Data; - } - -private: - SolverData m_Data; - std::mutex m_Mutex{}; - - friend SolverDataCache& findDataCache(size_t nActive); -}; - -SolverDataCache& findDataCache(size_t nActive) -{ - static std::vector s_GlobalCache{}; - static std::mutex s_GlobalLock{}; - - { - std::lock_guard lock(s_GlobalLock); - - for (size_t i = s_GlobalCache.size(); i < nActive - 1; ++i) { - int n = i + 1; - s_GlobalCache.emplace_back(nActive); - } - } - - return s_GlobalCache.at(nActive - 1); -} +namespace{ const Correction* calcPathCorrections(SolverData& data) { @@ -684,7 +608,7 @@ GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected( g0.x = KP.p() + v; Vec3 w = dKP[0] * c; - const UnitVec3 t = KP.R().getAxisUnitVec(ContactGeometry::TangentAxis); + const UnitVec3 t = KP.R().getAxisUnitVec(TangentAxis); g0.t = t + cross(w, t); // Take the length correction, and add to the current length. @@ -776,6 +700,7 @@ const LocalGeodesicInfo& LocalGeodesic::calcInitialGeodesic( shootNewGeodesic(g0, cache); updCacheEntry(s) = cache; m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); + return getCacheEntry(s); } const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( @@ -813,12 +738,12 @@ void LocalGeodesic::calcPathPoints(const State& s, std::vector& points) if (analyticFormAvailable()) { throw std::runtime_error("NOTYETIMPLEMENTED"); - m_Geometry.resampleGeodesicPointsAnalytically( - cache.K_P, - cache.K_Q, - cache.length, - m_NumberOfAnalyticPoints, - points); + /* m_Geometry.resampleGeodesicPointsAnalytically( */ + /* cache.K_P, */ + /* cache.K_Q, */ + /* cache.length, */ + /* m_NumberOfAnalyticPoints, */ + /* points); */ } else { for (const LocalGeodesicSample& sample : cache.samples) { points.push_back(sample.frame.p()); @@ -870,7 +795,7 @@ void LocalGeodesic::calcTouchdownIfNeeded( // Detect touchdown by computing the point on the line from x_QS to x_PS // that is nearest to the surface. bool touchdownDetected; - if (m_Geometry.analyticFormAvailable()) { + if (analyticFormAvailable()) { throw std::runtime_error("NOTYETIMPLEMENTED"); /* touchdownDetected = m_Geometry.calcNearestPointOnLineAnalytically( */ /* prev_QS, */ @@ -948,7 +873,7 @@ void LocalGeodesic::shootNewGeodesic( cache.samples); /* using Y = ImplicitGeodesicState; */ - /* if (m_Geometry.analyticFormAvailable()) { */ + /* if (analyticFormAvailable()) { */ /* m_Geometry.calcGeodesicWithVariationAnalytically( */ /* g0.x, */ @@ -1217,24 +1142,16 @@ namespace static const int N_PATH_CONSTRAINTS = 4; -// TODO this is awkward -void addBlock(const Vec4& values, Matrix& block) -{ - for (int i = 0; i < Vec4::size(); ++i) { - block[0][i] = values[i]; - } -} - void addDirectionJacobian( const LineSegment& e, const UnitVec3& axis, const PointVariation& dx, - Matrix& J, + std::function AddBlock, bool invert = false) { Vec3 y = axis - e.d * dot(e.d, axis); y /= e.l * (invert ? 1. : -1); - addBlock(~dx * y, J); + AddBlock(~dx * y); } Real calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) @@ -1242,15 +1159,16 @@ Real calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) return dot(e.d, R.getAxisUnitVec(axis)); } -GeodesicJacobian addPathErrorJacobian( +void addPathErrorJacobian( const LineSegment& e, const UnitVec3& axis, const Variation& dK, - Matrix& J, + std::function AddBlock, bool invertV = false) { - addDirectionJacobian(e, axis, dK[1], J, invertV); - addBlock(~dK[0] * cross(axis, e.d), J); + addDirectionJacobian(e, axis, dK[1], AddBlock, invertV); + AddBlock(~dK[0] * cross(axis, e.d)); + } } // namespace @@ -1502,19 +1420,28 @@ void CableSpan::Impl::calcPathErrorJacobian( 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][blkCol+ix] = block[ix]; + } + }; + for (CoordinateAxis axis : axes) { const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); const Variation& dK_P = g.dKP; - addPathErrorJacobian(l_P, a_P, dK_P, J.block(row, col, 1, Nq)); + addPathErrorJacobian(l_P, a_P, dK_P, AddBlock); if (prev) { const Variation& prev_dK_Q = prev->getImpl().getPosInfo(s).dKQ; + blkCol = col - Nq; addDirectionJacobian( l_P, a_P, prev_dK_Q[1], - J.block(row, col - Nq, 1, Nq), + AddBlock, true); } ++row; @@ -1524,20 +1451,22 @@ void CableSpan::Impl::calcPathErrorJacobian( const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); const Variation& dK_Q = g.dKQ; + blkCol = col; addPathErrorJacobian( l_Q, a_Q, dK_Q, - J.block(row, col, 1, Nq), + 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], - J.block(row, col + Nq, 1, Nq)); + AddBlock); } ++row; } @@ -1582,7 +1511,7 @@ void CableSpan::Impl::calcLineSegments( const GeodesicInfo& g = segment.getImpl().getPosInfo(s); const Vec3 lineEnd = g.KP.p(); - lines.emplace_back(lineStart, lineEnd); + lines.push_back(LineSegment(lineStart, lineEnd)); lineStart = g.KQ.p(); } @@ -1616,49 +1545,37 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const const size_t nActive = countActive(s); // Grab the shared data cache for computing the matrices, and lock it. - SolverDataCache& dataCache = findDataCache(nActive); - try { - dataCache.lock(); + SolverData& data = m_Subsystem.getImpl().updCacheEntry(s).updOrInsert(nActive); - SolverData& data = dataCache.updData(); + // Compute the straight-line segments. + calcLineSegments(s, x_O, x_I, data.lineSegments); - // Compute the straight-line segments. - calcLineSegments(s, x_O, x_I, data.lineSegments); - - // Evaluate path error, and stop when converged. - calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); - const Real maxPathError = data.pathError.normInf(); - if (maxPathError < m_PathErrorBound) { - return; - } + // Evaluate path error, and stop when converged. + calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); + const Real maxPathError = data.pathError.normInf(); + if (maxPathError < m_PathErrorBound) { + return; + } - // Evaluate the path error jacobian. - calcPathErrorJacobian<2>( + // Evaluate the path error jacobian. + calcPathErrorJacobian<2>( s, data.lineSegments, axes, data.pathErrorJacobian); - // Compute path corrections. - const Correction* corrIt = calcPathCorrections(data); + // Compute path corrections. + const Correction* corrIt = calcPathCorrections(data); - // Apply path corrections. - for (const CurveSegment& obstacle : m_CurveSegments) { - if (!obstacle.getImpl().isActive(s)) { - continue; - } - obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); - ++corrIt; + // Apply path corrections. + for (const CurveSegment& obstacle : m_CurveSegments) { + if (!obstacle.getImpl().isActive(s)) { + continue; } - - } catch (const std::exception& e) { - dataCache.unlock(); - throw e; + obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); + ++corrIt; } - // Release the lock on the shared data. - dataCache.unlock(); - // Path has changed: invalidate each segment's cache. for (const CurveSegment& obstacle : m_CurveSegments) { obstacle.getImpl().invalidatePositionLevelCache(s); diff --git a/Simbody/include/simbody/internal/WrappingImpl.h b/Simbody/src/WrappingImpl.h similarity index 89% rename from Simbody/include/simbody/internal/WrappingImpl.h rename to Simbody/src/WrappingImpl.h index fdf50d363..7ad9d1148 100644 --- a/Simbody/include/simbody/internal/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -105,7 +105,9 @@ class CurveSegment::Impl bool analyticFormAvailable() const { - return m_Geometry.analyticFormAvailable(); + // TODO + /* return m_Geometry.analyticFormAvailable(); */ + return false; } const LocalGeodesicInfo& calcInitialGeodesic( @@ -150,6 +152,7 @@ class CurveSegment::Impl struct LocalGeodesicSample { + LocalGeodesicSample(Real l, FrenetFrame K) : length(l), frame(K) {} Real length; FrenetFrame frame; }; @@ -527,7 +530,51 @@ class CableSpan::Impl //============================================================================== class CableSubsystem::Impl : public Subsystem::Guts { -public: + public: + struct SolverData + { + SolverData(int nActive) { + static constexpr int Q = 4; + static constexpr int C = 4; + 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.); + mat = Matrix(Q * n, Q * n, NaN); + vec = Vector(Q * n, NaN); + } + + std::vector lineSegments; + + Matrix pathErrorJacobian; + Vector pathCorrection; + Vector pathError; + Matrix mat; + // TODO Cholesky decomposition... + FactorLU matInv; + Vector vec; + }; + + struct CacheEntry + { + CacheEntry() =default; + SolverData& updOrInsert(int nActive) { + if (nActive <= 0) { + 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() @@ -605,11 +652,34 @@ class CableSubsystem::Impl : public Subsystem::Guts return 0; } + void realizeTopology(State& state) + { + CacheEntry cache{}; + m_CacheIx = allocateDiscreteVariable( + state, + Stage::Velocity, + new Value(cache)); + } + + const CacheEntry& getCacheEntry(const State& state) const + { + return Value::downcast( + getDiscreteVarUpdateValue(state, m_CacheIx)); + } + + CacheEntry& updCacheEntry(const State& state) const + { + return Value::updDowncast( + updDiscreteVarUpdateValue(state, m_CacheIx)); + } + SimTK_DOWNCAST(Impl, Subsystem::Guts); private: // TOPOLOGY STATE Array_ cables; + + DiscreteVariableIndex m_CacheIx; }; } // namespace SimTK From 0a1d634f1de0b150f20c849be08d437a27560c48 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 12:52:48 +0200 Subject: [PATCH 047/127] add ctor to LineSegment --- Simbody/include/simbody/internal/Wrapping.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index b7f2a417c..0eb0de84a 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -87,8 +87,12 @@ class SimTK_SIMBODY_EXPORT CableSpan public: struct LineSegment { - UnitVec3 d{NaN, NaN, NaN}; + LineSegment() = default; + + LineSegment(Vec3 a, Vec3 b): l((b-a).norm()), d((b-a)/l) {} + Real l = NaN; + UnitVec3 d{NaN, NaN, NaN}; }; public: From c09fa3c66a5a2f4091cd2547069634c09b217bb7 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 12:53:18 +0200 Subject: [PATCH 048/127] fix compiling ExampleCableSpan --- .../simmath/internal/ContactGeometry.h | 120 +++---- SimTKmath/Geometry/src/ContactGeometry.cpp | 330 +++++++++--------- SimTKmath/Geometry/src/ContactGeometryImpl.h | 128 +++---- .../Geometry/src/ContactGeometry_Sphere.cpp | 2 + Simbody/include/SimTKsimbody.h | 1 + 5 files changed, 297 insertions(+), 284 deletions(-) diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h index a1f8937e1..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,10 +122,6 @@ class Cylinder; class Brick; class TriangleMesh; -static const CoordinateAxis TangentAxis; -static const CoordinateAxis NormalAxis; -static const CoordinateAxis BinormalAxis; - static constexpr int GEODESIC_DOF = 4; using FrenetFrame = Transform; @@ -140,62 +140,62 @@ using GeodesicFrameVariation = Mat34; 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; +/* 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; diff --git a/SimTKmath/Geometry/src/ContactGeometry.cpp b/SimTKmath/Geometry/src/ContactGeometry.cpp index d4444e5fd..86f215912 100644 --- a/SimTKmath/Geometry/src/ContactGeometry.cpp +++ b/SimTKmath/Geometry/src/ContactGeometry.cpp @@ -185,176 +185,186 @@ 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); -} +/* 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? -static const CoordinateAxis TangentAxis = CoordinateAxis::XCoordinateAxis(); -static const CoordinateAxis NormalAxis = CoordinateAxis::YCoordinateAxis(); -static const CoordinateAxis BinormalAxis = CoordinateAxis::ZCoordinateAxis(); - -size_t ContactGeometryImpl::calcNearestFrenetFrameImplicitlyFast( - Vec3 xGuess, - Vec3 tGuess, - FrenetFrame& K_P, - size_t maxIter, - Real eps) const +/* 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 { - Vec3& x = xGuess; - size_t it = 0; - for (; it < maxIter; ++it) { - const double c = calcSurfaceValue(x); - - const double error = std::abs(c); - - if (error < eps) { - break; - } - - const Vec3 g = calcSurfaceGradient(x); - - x += -g * c / dot(g,g); - } - - UnitVec3 n (calcSurfaceGradient(x)); - Vec3& t = tGuess; - if (!(abs(t % n) > 1e-13)) { - // TODO split NaN detection. - throw std::runtime_error("Surface projection failed: Tangent guess is parallel to surface normal, or there are NaNs..."); - } - - t = t - dot(n, t) * n; - - K_P.updR().setRotationFromTwoAxes(n, NormalAxis, t, TangentAxis); - K_P.setP(x); - - return it; + return getImpl().calcNormalCurvature(x, t); } -void ContactGeometryImpl::calcGeodesicStartFrameVariationImplicitly( - const FrenetFrame& K_P, - std::array& dK_P) const +Real ContactGeometry::calcGeodesicTorsion(Vec3 x, UnitVec3 t) const { - const UnitVec3& t = K_P.R().getAxisUnitVec(TangentAxis); - const UnitVec3& n = K_P.R().getAxisUnitVec(NormalAxis); - const UnitVec3& b = K_P.R().getAxisUnitVec(BinormalAxis); - - const Vec3& x = K_P.p(); - - dK_P.at(0)[1] = t; - dK_P.at(1)[1] = b; // * q.a; // a = 1 - dK_P.at(2)[1] = Vec3{0.}; // b * q.r; // r = 0 - dK_P.at(3)[1] = Vec3{0.}; // isEnd ? t : Vec3{0., 0., 0.}; - - const double tau_g = calcGeodesicTorsion(x, t); - const double kappa_n = calcNormalCurvature(x, t); - const double kappa_a = calcNormalCurvature(x, b); - - dK_P.at(0)[0] = tau_g * t + kappa_n * b; - dK_P.at(1)[0] = -kappa_a * t - tau_g * b; - dK_P.at(2)[0] = -n; - dK_P.at(3)[0] = Vec3{0.}; -} - -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"); } + return getImpl().calcGeodesicTorsion(x, t); +} //------------------------------------------------------------------------------ // EVAL PARAMETRIC CURVATURE diff --git a/SimTKmath/Geometry/src/ContactGeometryImpl.h b/SimTKmath/Geometry/src/ContactGeometryImpl.h index e0f2f25c4..5142b63a9 100644 --- a/SimTKmath/Geometry/src/ContactGeometryImpl.h +++ b/SimTKmath/Geometry/src/ContactGeometryImpl.h @@ -93,70 +93,70 @@ class SimTK_SIMMATH_EXPORT ContactGeometryImpl { 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"); } + /* 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, 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" From 9614eaad543b0746212e6d9ba082f3a49076f606 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 14:04:20 +0200 Subject: [PATCH 049/127] fix segfault: distribute cable subsystem by pointer --- Simbody/include/simbody/internal/Wrapping.h | 2 +- Simbody/src/Wrapping.cpp | 67 ++++++++++++--------- Simbody/src/WrappingImpl.h | 41 ++++++++----- 3 files changed, 64 insertions(+), 46 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 0eb0de84a..8a69666bc 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -97,7 +97,7 @@ class SimTK_SIMBODY_EXPORT CableSpan public: CableSpan( - CableSubsystem subsystem, + CableSubsystem& subsystem, const MobilizedBody& originBody, const Vec3& defaultOriginPoint, const MobilizedBody& terminationBody, diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index db762427b..be0c51820 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -673,7 +673,7 @@ void LocalGeodesic::realizeTopology(State& s) { // Allocate an auto-update discrete variable for the last computed geodesic. CacheEntry cache{}; - m_CacheIx = m_Subsystem.allocateAutoUpdateDiscreteVariable( + m_CacheIx = updSubsystem().allocateAutoUpdateDiscreteVariable( s, Stage::Velocity, new Value(cache), @@ -682,9 +682,9 @@ void LocalGeodesic::realizeTopology(State& s) void LocalGeodesic::realizePosition(const State& s) const { - if (!m_Subsystem.isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { + if (!getSubsystem().isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { updCacheEntry(s) = getPrevCacheEntry(s); - m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); + getSubsystem().markDiscreteVarUpdateValueRealized(s, m_CacheIx); } } @@ -699,7 +699,7 @@ const LocalGeodesicInfo& LocalGeodesic::calcInitialGeodesic( CacheEntry& cache = updPrevCacheEntry(s); shootNewGeodesic(g0, cache); updCacheEntry(s) = cache; - m_Subsystem.markDiscreteVarUpdateValueRealized(s, m_CacheIx); + getSubsystem().markDiscreteVarUpdateValueRealized(s, m_CacheIx); return getCacheEntry(s); } @@ -954,19 +954,18 @@ CurveSegment::Impl::Impl( const Transform& X_BS, ContactGeometry geometry, Vec3 initPointGuess) : - m_Subsystem(path.getImpl().getSubsystem()), - m_Path(path), - m_Index(-1), // TODO what to do with this index, and when - m_Mobod(mobod), - m_Offset(X_BS), - m_Geodesic(m_Subsystem, geometry, initPointGuess) + m_Subsystem(&path.updImpl().updSubsystem()), + m_Path(path), m_Index(-1), // TODO what to do with this index, and when + m_Mobod(mobod), m_Offset(X_BS), + m_Geodesic(path.updImpl().updSubsystem(), geometry, initPointGuess) {} void CurveSegment::Impl::realizeTopology(State& s) { - // Allocate position level cache. + m_Geodesic.realizeTopology(s); + PosInfo posInfo{}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry( + m_PosInfoIx = updSubsystem().allocateCacheEntry( s, Stage::Position, new Value(posInfo)); @@ -974,9 +973,9 @@ void CurveSegment::Impl::realizeTopology(State& s) void CurveSegment::Impl::realizePosition(const State& s) const { - if (!m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { + if (!getSubsystem().isCacheValueRealized(s, m_PosInfoIx)) { calcPosInfo(s, updPosInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); + getSubsystem().markCacheValueRealized(s, m_PosInfoIx); } } @@ -987,7 +986,7 @@ void CurveSegment::Impl::realizeCablePosition(const State& s) const void CurveSegment::Impl::invalidatePositionLevelCache(const State& state) const { - m_Subsystem.markCacheValueNotRealized(state, m_PosInfoIx); + getSubsystem().markCacheValueNotRealized(state, m_PosInfoIx); m_Path.getImpl().invalidatePositionLevelCache(state); } @@ -1168,7 +1167,6 @@ void addPathErrorJacobian( { addDirectionJacobian(e, axis, dK[1], AddBlock, invertV); AddBlock(~dK[0] * cross(axis, e.d)); - } } // namespace @@ -1178,7 +1176,7 @@ void addPathErrorJacobian( //============================================================================== CableSpan::CableSpan( - CableSubsystem subsystem, + CableSubsystem& subsystem, const MobilizedBody& originBody, const Vec3& defaultOriginPoint, const MobilizedBody& terminationBody, @@ -1189,7 +1187,9 @@ CableSpan::CableSpan( defaultOriginPoint, terminationBody, defaultTerminationPoint))) -{} +{ + subsystem.updImpl().adoptCablePath(*this); +} CurveSegmentIndex CableSpan::adoptSegment(const CurveSegment& segment) { @@ -1238,14 +1238,18 @@ void CableSpan::applyBodyForces( void CableSpan::Impl::realizeTopology(State& s) { + for (CurveSegment segment : m_CurveSegments) { + segment.updImpl().realizeTopology(s); + } + PosInfo posInfo{}; - m_PosInfoIx = m_Subsystem.allocateCacheEntry( + m_PosInfoIx = updSubsystem().allocateCacheEntry( s, Stage::Position, new Value(posInfo)); VelInfo velInfo{}; - m_VelInfoIx = m_Subsystem.allocateCacheEntry( + m_VelInfoIx = updSubsystem().allocateCacheEntry( s, Stage::Velocity, new Value(velInfo)); @@ -1253,51 +1257,53 @@ void CableSpan::Impl::realizeTopology(State& s) void CableSpan::Impl::realizePosition(const State& s) const { - if (m_Subsystem.isCacheValueRealized(s, m_PosInfoIx)) { + if (getSubsystem().isCacheValueRealized(s, m_PosInfoIx)) { return; } calcPosInfo(s, updPosInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_PosInfoIx); + getSubsystem().markCacheValueRealized(s, m_PosInfoIx); } void CableSpan::Impl::realizeVelocity(const State& s) const { - if (m_Subsystem.isCacheValueRealized(s, m_VelInfoIx)) { + if (getSubsystem().isCacheValueRealized(s, m_VelInfoIx)) { return; } calcVelInfo(s, updVelInfo(s)); - m_Subsystem.markCacheValueRealized(s, m_VelInfoIx); + getSubsystem().markCacheValueRealized(s, m_VelInfoIx); } void CableSpan::Impl::invalidatePositionLevelCache(const State& s) const { - m_Subsystem.markCacheValueNotRealized(s, m_PosInfoIx); + getSubsystem().markCacheValueNotRealized(s, m_PosInfoIx); } const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo( const State& s) const { realizePosition(s); - return Value::downcast(m_Subsystem.getCacheEntry(s, m_PosInfoIx)); + return Value::downcast( + getSubsystem().getCacheEntry(s, m_PosInfoIx)); } CableSpan::Impl::PosInfo& CableSpan::Impl::updPosInfo(const State& s) const { return Value::updDowncast( - m_Subsystem.updCacheEntry(s, m_PosInfoIx)); + getSubsystem().updCacheEntry(s, m_PosInfoIx)); } const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo( const State& s) const { realizeVelocity(s); - return Value::downcast(m_Subsystem.getCacheEntry(s, m_VelInfoIx)); + return Value::downcast( + getSubsystem().getCacheEntry(s, m_VelInfoIx)); } CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const { return Value::updDowncast( - m_Subsystem.updCacheEntry(s, m_VelInfoIx)); + getSubsystem().updCacheEntry(s, m_VelInfoIx)); } void CableSpan::Impl::calcInitCablePath(State& s) const @@ -1545,7 +1551,8 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const const size_t nActive = countActive(s); // Grab the shared data cache for computing the matrices, and lock it. - SolverData& data = m_Subsystem.getImpl().updCacheEntry(s).updOrInsert(nActive); + SolverData& data = + getSubsystem().getImpl().updCacheEntry(s).updOrInsert(nActive); // Compute the straight-line segments. calcLineSegments(s, x_O, x_I, data.lineSegments); diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 7ad9d1148..705e62937 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -47,10 +47,10 @@ class CurveSegment::Impl LocalGeodesic& operator=(const LocalGeodesic&) = delete; LocalGeodesic( - CableSubsystem subsystem, + CableSubsystem& subsystem, ContactGeometry geometry, Vec3 initPointGuess) : - m_Subsystem(subsystem), + m_Subsystem(&subsystem), m_Geometry(geometry), m_InitPointGuess(initPointGuess) {} @@ -157,6 +157,9 @@ class CurveSegment::Impl FrenetFrame frame; }; + CableSubsystem& updSubsystem() {return *m_Subsystem;} + const CableSubsystem& getSubsystem() const {return *m_Subsystem;} + private: // The cache entry: Curve in local surface coordinated. // This is an auto update discrete cache variable, which makes it @@ -172,25 +175,25 @@ class CurveSegment::Impl const CacheEntry& getCacheEntry(const State& state) const { return Value::downcast( - m_Subsystem.getDiscreteVarUpdateValue(state, m_CacheIx)); + m_Subsystem->getDiscreteVarUpdateValue(state, m_CacheIx)); } CacheEntry& updCacheEntry(const State& state) const { return Value::updDowncast( - m_Subsystem.updDiscreteVarUpdateValue(state, m_CacheIx)); + m_Subsystem->updDiscreteVarUpdateValue(state, m_CacheIx)); } const CacheEntry& getPrevCacheEntry(const State& state) const { return Value::downcast( - m_Subsystem.getDiscreteVariable(state, m_CacheIx)); + m_Subsystem->getDiscreteVariable(state, m_CacheIx)); } CacheEntry& updPrevCacheEntry(State& state) const { return Value::updDowncast( - m_Subsystem.updDiscreteVariable(state, m_CacheIx)); + m_Subsystem->updDiscreteVariable(state, m_CacheIx)); } void calcCacheEntry( @@ -216,7 +219,7 @@ class CurveSegment::Impl CacheEntry& cache) const; //------------------------------------------------------------------------------ - CableSubsystem m_Subsystem; + CableSubsystem* m_Subsystem; // TODO just a pointer? ContactGeometry m_Geometry; @@ -292,7 +295,7 @@ class CurveSegment::Impl { realizePosition(s); return Value::downcast( - m_Subsystem.getCacheEntry(s, m_PosInfoIx)); + m_Subsystem->getCacheEntry(s, m_PosInfoIx)); } bool isActive(const State& s) const @@ -355,17 +358,20 @@ class CurveSegment::Impl // TODO allow for user to shoot his own geodesic. /* void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; */ + CableSubsystem& updSubsystem() {return *m_Subsystem;} + const CableSubsystem& getSubsystem() const {return *m_Subsystem;} + private: PosInfo& updPosInfo(const State& state) const { return Value::updDowncast( - m_Subsystem.updCacheEntry(state, m_PosInfoIx)); + m_Subsystem->updCacheEntry(state, m_PosInfoIx)); } void calcPosInfo(const State& state, PosInfo& posInfo) const; // TODO Required for accessing the cache variable? - CableSubsystem m_Subsystem; // The subsystem this segment belongs to. + 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. @@ -389,12 +395,12 @@ class CableSpan::Impl { public: Impl( - CableSubsystem subsystem, + CableSubsystem& subsystem, MobilizedBody originBody, Vec3 originPoint, MobilizedBody terminationBody, Vec3 terminationPoint) : - m_Subsystem(subsystem), + m_Subsystem(&subsystem), m_OriginBody(originBody), m_OriginPoint(originPoint), m_TerminationBody(terminationBody), m_TerminationPoint(terminationPoint) {} @@ -438,7 +444,7 @@ class CableSpan::Impl void realizeVelocity(const State& state) const; void invalidateTopology() { - m_Subsystem.invalidateSubsystemTopologyCache(); + m_Subsystem->invalidateSubsystemTopologyCache(); } void invalidatePositionLevelCache(const State& state) const; @@ -499,11 +505,16 @@ class CableSpan::Impl const CableSubsystem& getSubsystem() const { - return m_Subsystem; + return *m_Subsystem; + } + + CableSubsystem& updSubsystem() + { + return *m_Subsystem; } // Reference back to the subsystem. - CableSubsystem m_Subsystem; + CableSubsystem* m_Subsystem; // TODO just a pointer? MobilizedBody m_OriginBody; Vec3 m_OriginPoint; From 762b7a30267ac195ff8ae32a4ec8397dcaaced9d Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 14:07:28 +0200 Subject: [PATCH 050/127] prefer getter over member access --- Simbody/src/WrappingImpl.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 705e62937..0a8401ad0 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -175,25 +175,25 @@ class CurveSegment::Impl const CacheEntry& getCacheEntry(const State& state) const { return Value::downcast( - m_Subsystem->getDiscreteVarUpdateValue(state, m_CacheIx)); + getSubsystem().getDiscreteVarUpdateValue(state, m_CacheIx)); } CacheEntry& updCacheEntry(const State& state) const { return Value::updDowncast( - m_Subsystem->updDiscreteVarUpdateValue(state, m_CacheIx)); + getSubsystem().updDiscreteVarUpdateValue(state, m_CacheIx)); } const CacheEntry& getPrevCacheEntry(const State& state) const { return Value::downcast( - m_Subsystem->getDiscreteVariable(state, m_CacheIx)); + getSubsystem().getDiscreteVariable(state, m_CacheIx)); } CacheEntry& updPrevCacheEntry(State& state) const { return Value::updDowncast( - m_Subsystem->updDiscreteVariable(state, m_CacheIx)); + getSubsystem().updDiscreteVariable(state, m_CacheIx)); } void calcCacheEntry( @@ -295,7 +295,7 @@ class CurveSegment::Impl { realizePosition(s); return Value::downcast( - m_Subsystem->getCacheEntry(s, m_PosInfoIx)); + getSubsystem().getCacheEntry(s, m_PosInfoIx)); } bool isActive(const State& s) const @@ -365,7 +365,7 @@ class CurveSegment::Impl PosInfo& updPosInfo(const State& state) const { return Value::updDowncast( - m_Subsystem->updCacheEntry(state, m_PosInfoIx)); + getSubsystem().updCacheEntry(state, m_PosInfoIx)); } void calcPosInfo(const State& state, PosInfo& posInfo) const; @@ -444,7 +444,7 @@ class CableSpan::Impl void realizeVelocity(const State& state) const; void invalidateTopology() { - m_Subsystem->invalidateSubsystemTopologyCache(); + getSubsystem().invalidateSubsystemTopologyCache(); } void invalidatePositionLevelCache(const State& state) const; From eb98aada363b036a36feff28bff791d2fa7208c5 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 16:51:27 +0200 Subject: [PATCH 051/127] fix cache variable latest stages --- Simbody/src/Wrapping.cpp | 12 ++++++------ Simbody/src/WrappingImpl.h | 7 +++++-- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index be0c51820..68082f6a8 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -672,11 +672,11 @@ using LocalGeodesic = CurveSegment::Impl::LocalGeodesic; void LocalGeodesic::realizeTopology(State& s) { // Allocate an auto-update discrete variable for the last computed geodesic. - CacheEntry cache{}; + Value* cache = new Value(); m_CacheIx = updSubsystem().allocateAutoUpdateDiscreteVariable( s, - Stage::Velocity, - new Value(cache), + Stage::Report, + cache, Stage::Position); } @@ -700,7 +700,7 @@ const LocalGeodesicInfo& LocalGeodesic::calcInitialGeodesic( shootNewGeodesic(g0, cache); updCacheEntry(s) = cache; getSubsystem().markDiscreteVarUpdateValueRealized(s, m_CacheIx); - return getCacheEntry(s); + return cache; } const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( @@ -717,8 +717,6 @@ const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) const { - realizePosition(s); - // Get the previous geodesic. const CacheEntry& g = getCacheEntry(s); @@ -1246,12 +1244,14 @@ void CableSpan::Impl::realizeTopology(State& s) m_PosInfoIx = updSubsystem().allocateCacheEntry( s, Stage::Position, + Stage::Infinity, new Value(posInfo)); VelInfo velInfo{}; m_VelInfoIx = updSubsystem().allocateCacheEntry( s, Stage::Velocity, + Stage::Infinity, new Value(velInfo)); } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 0a8401ad0..3877985fa 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -172,10 +172,12 @@ class CurveSegment::Impl double sHint = NaN; }; - const CacheEntry& getCacheEntry(const State& state) const + const CacheEntry& getCacheEntry(const State& s) const { + std::cout << "LocalGeodesic::getCacheEntry\n"; + realizePosition(s); return Value::downcast( - getSubsystem().getDiscreteVarUpdateValue(state, m_CacheIx)); + getSubsystem().getDiscreteVarUpdateValue(s, m_CacheIx)); } CacheEntry& updCacheEntry(const State& state) const @@ -637,6 +639,7 @@ class CableSubsystem::Impl : public Subsystem::Guts // 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); From 4956a98166d7a2844404260486a58d6055e19a7b Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 18:40:08 +0200 Subject: [PATCH 052/127] do not use Geodesic math ftom ContactGeometry --- Simbody/src/Wrapping.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 68082f6a8..88e095c0a 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -23,14 +23,14 @@ using PointVariation = ContactGeometry::GeodesicPointVariation; using Variation = ContactGeometry::GeodesicVariation; using LineSegment = CableSpan::LineSegment; using Status = CurveSegment::Status; -using SolverData = CableSubsystem::Impl::SolverData; +using SolverData = CableSubsystem::Impl::SolverData; //============================================================================== // CONSTANTS //============================================================================== namespace { -static const int GeodesicDOF = 4; +static const int GeodesicDOF = 4; } // namespace //============================================================================== @@ -507,9 +507,9 @@ void calcGeodesicBoundaryState( v.col(2) = b * q.r; v.col(3) = isEnd ? v.col(0) : Vec3{0.}; - const Real tau_g = geometry.calcGeodesicTorsion(q.x, t); - const Real kappa_n = geometry.calcNormalCurvature(q.x, t); - const Real kappa_a = geometry.calcNormalCurvature(q.x, b); + 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; @@ -568,7 +568,8 @@ void calcGeodesicAndVariationImplicitly( // SOLVER //============================================================================== -namespace{ +namespace +{ const Correction* calcPathCorrections(SolverData& data) { From b3071a05b428f0b4216165260945bfa8c56ceda0 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 18:40:57 +0200 Subject: [PATCH 053/127] set initial values of LocalGeodesic --- Simbody/src/Wrapping.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 88e095c0a..aee25489b 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -674,6 +674,10 @@ void LocalGeodesic::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 = Status::Liftoff; + cache->upd().trackingPointOnLine = getInitialPointGuess(); + m_CacheIx = updSubsystem().allocateAutoUpdateDiscreteVariable( s, Stage::Report, From d5cf4afd9d582417d65c3837826edeac79f82bd4 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 18:44:59 +0200 Subject: [PATCH 054/127] wip cleanup --- Simbody/src/Wrapping.cpp | 57 +++++++++++++++++++------------------- Simbody/src/WrappingImpl.h | 27 ++++++++---------- 2 files changed, 39 insertions(+), 45 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index aee25489b..c9c1c4090 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -736,7 +736,6 @@ void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) void LocalGeodesic::calcPathPoints(const State& s, std::vector& points) const { - realizePosition(s); const CacheEntry& cache = getCacheEntry(s); if (analyticFormAvailable()) { @@ -1431,11 +1430,10 @@ void CableSpan::Impl::calcPathErrorJacobian( const CurveSegment* prev = findPrevActiveCurveSegment(s, ix); const CurveSegment* next = findNextActiveCurveSegment(s, ix); - int blkCol = col; - std::function AddBlock = [&](const Vec4& block) - { + int blkCol = col; + std::function AddBlock = [&](const Vec4& block) { for (int ix = 0; ix < 4; ++ix) { - J[row][blkCol+ix] = block[ix]; + J[row][blkCol + ix] = block[ix]; } }; @@ -1447,13 +1445,8 @@ void CableSpan::Impl::calcPathErrorJacobian( 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); + blkCol = col - Nq; + addDirectionJacobian(l_P, a_P, prev_dK_Q[1], AddBlock, true); } ++row; } @@ -1463,21 +1456,12 @@ void CableSpan::Impl::calcPathErrorJacobian( const Variation& dK_Q = g.dKQ; blkCol = col; - addPathErrorJacobian( - l_Q, - a_Q, - dK_Q, - AddBlock, - true); + 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); + blkCol = col + Nq; + addDirectionJacobian(l_Q, a_Q, next_dK_P[1], AddBlock); } ++row; } @@ -1549,15 +1533,23 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( m_TerminationPoint); + posInfo.xO = x_O; + posInfo.xI = x_I; + const std::array axes{NormalAxis, BinormalAxis}; for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; ++posInfo.loopIter) { const size_t nActive = countActive(s); + if (nActive == 0) { + posInfo.l = (x_I - x_O).norm(); + return; + } + // Grab the shared data cache for computing the matrices, and lock it. SolverData& data = - getSubsystem().getImpl().updCacheEntry(s).updOrInsert(nActive); + getSubsystem().getImpl().updCachedScratchboard(s).updOrInsert(nActive); // Compute the straight-line segments. calcLineSegments(s, x_O, x_I, data.lineSegments); @@ -1566,15 +1558,22 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); const Real maxPathError = data.pathError.normInf(); if (maxPathError < m_PathErrorBound) { + posInfo.l = 0.; + for (const LineSegment& line: data.lineSegments) { + posInfo.l += line.l; + } + for (const CurveSegment& curve: m_CurveSegments) { + posInfo.l += curve.getImpl().getPosInfo(s).length; + } return; } // Evaluate the path error jacobian. calcPathErrorJacobian<2>( - s, - data.lineSegments, - axes, - data.pathErrorJacobian); + s, + data.lineSegments, + axes, + data.pathErrorJacobian); // Compute path corrections. const Correction* corrIt = calcPathCorrections(data); diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 3877985fa..c97f09f15 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -69,7 +69,7 @@ class CurveSegment::Impl Variation dK_P{}; Variation dK_Q{}; - Status status = Status::Ok; + Status status = Status::Liftoff; }; // Helper struct: Required data for shooting a new geodesic. @@ -111,7 +111,7 @@ class CurveSegment::Impl } const LocalGeodesicInfo& calcInitialGeodesic( - State& s, + const State& s, const GeodesicInitialConditions& g0) const; // This will reevaluate the cached geodesic and status. @@ -170,11 +170,11 @@ class CurveSegment::Impl Vec3 trackingPointOnLine{NaN, NaN, NaN}; std::vector samples; double sHint = NaN; + int count = 0; }; const CacheEntry& getCacheEntry(const State& s) const { - std::cout << "LocalGeodesic::getCacheEntry\n"; realizePosition(s); return Value::downcast( getSubsystem().getDiscreteVarUpdateValue(s, m_CacheIx)); @@ -302,6 +302,7 @@ class CurveSegment::Impl bool isActive(const State& s) const { + realizePosition(s); return m_Geodesic.getStatus(s) == Status::Ok; } @@ -310,7 +311,7 @@ class CurveSegment::Impl return m_Geodesic.getStatus(s); } - void calcInitZeroLengthGeodesic(State& state, Vec3 prev_QS) const; + void calcInitZeroLengthGeodesic(const State& state, Vec3 prev_QS) const; void applyGeodesicCorrection( const State& state, @@ -669,22 +670,16 @@ class CableSubsystem::Impl : public Subsystem::Guts void realizeTopology(State& state) { CacheEntry cache{}; - m_CacheIx = allocateDiscreteVariable( + m_CacheIx = allocateCacheEntry( state, - Stage::Velocity, + Stage::Instance, + Stage::Infinity, new Value(cache)); } - const CacheEntry& getCacheEntry(const State& state) const - { - return Value::downcast( - getDiscreteVarUpdateValue(state, m_CacheIx)); - } - - CacheEntry& updCacheEntry(const State& state) const + CacheEntry& updCachedScratchboard(const State& state) const { - return Value::updDowncast( - updDiscreteVarUpdateValue(state, m_CacheIx)); + return Value::updDowncast(updCacheEntry(state, m_CacheIx)); } SimTK_DOWNCAST(Impl, Subsystem::Guts); @@ -693,7 +688,7 @@ class CableSubsystem::Impl : public Subsystem::Guts // TOPOLOGY STATE Array_ cables; - DiscreteVariableIndex m_CacheIx; + CacheEntryIndex m_CacheIx; }; } // namespace SimTK From 7ae7501cc5082ae04c92a6955599e0dcb25c2c94 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 30 Apr 2024 18:58:05 +0200 Subject: [PATCH 055/127] cleanup: remove path initializers --- Simbody/src/Wrapping.cpp | 66 +++----------------------------------- Simbody/src/WrappingImpl.h | 8 ----- 2 files changed, 4 insertions(+), 70 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index c9c1c4090..dd9c5e1ad 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -696,17 +696,6 @@ void LocalGeodesic::realizePosition(const State& s) const //------------------------------------------------------------------------------ // PUBLIC METHODS //------------------------------------------------------------------------------ -const LocalGeodesicInfo& LocalGeodesic::calcInitialGeodesic( - State& s, - const GeodesicInitialConditions& g0) const -{ - // TODO is this correct? - CacheEntry& cache = updPrevCacheEntry(s); - shootNewGeodesic(g0, cache); - updCacheEntry(s) = cache; - getSubsystem().markDiscreteVarUpdateValueRealized(s, m_CacheIx); - return cache; -} const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( const State& s, @@ -992,23 +981,6 @@ void CurveSegment::Impl::invalidatePositionLevelCache(const State& state) const m_Path.getImpl().invalidatePositionLevelCache(state); } -void CurveSegment::Impl::calcInitZeroLengthGeodesic(State& s, Vec3 prev_QG) - const -{ - // Get tramsform from local surface frame to ground. - Transform X_GS = m_Mobod.getBodyTransform(s).compose(m_Offset); - - Vec3 prev_QS = X_GS.shiftBaseStationToFrame(prev_QG); - Vec3 xGuess_S = - m_Geodesic.getInitialPointGuess(); // TODO move into function call? - - GeodesicInitialConditions g0 = - GeodesicInitialConditions::CreateZeroLengthGuess(prev_QS, xGuess_S); - m_Geodesic.calcInitialGeodesic(s, g0); - - invalidatePositionLevelCache(s); -} - void CurveSegment::Impl::applyGeodesicCorrection( const State& s, const CurveSegment::Impl::Correction& c) const @@ -1051,22 +1023,7 @@ void xformSurfaceGeodesicToGround( geodesic_G.dKQ[0] = X_GS.R() * geodesic_S.dK_Q[0]; geodesic_G.dKQ[1] = X_GS.R() * geodesic_S.dK_Q[1]; - // TODO use SpatialVec for variation. - /* for (size_t i = 0; i < GeodesicDOF; ++i) { */ - /* geodesic_G.dKP[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][0]) - */ - /* geodesic_G.dKP[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKP[i][1]) - */ - - /* geodesic_G.dKQ[i][0] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][0]) - */ - /* geodesic_G.dKQ[i][1] = X_GS.xformFrameVecToBase(geodesic_S.dKQ[i][1]) - */ - /* } */ - geodesic_G.length = geodesic_S.length; - - throw std::runtime_error("NOTYETIMPLEMENTED: Check transformation order"); } } // namespace @@ -1199,10 +1156,10 @@ CurveSegmentIndex CableSpan::adoptSegment(const CurveSegment& segment) } void CableSpan::adoptWrappingObstacle( - const MobilizedBody& mobod, - Transform X_BS, - const ContactGeometry& geometry, - Vec3 contactPointHint) + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 contactPointHint) { CurveSegment(*this, mobod, X_BS, geometry, contactPointHint); } @@ -1310,21 +1267,6 @@ CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const getSubsystem().updCacheEntry(s, m_VelInfoIx)); } -void CableSpan::Impl::calcInitCablePath(State& s) const -{ - Vec3 prev_QG = - m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); - size_t i = 0; - for (const CurveSegment& obstacle : m_CurveSegments) { - if (obstacle.getImpl().getStatus(s) == Status::Disabled) { - continue; - } - obstacle.getImpl().calcInitZeroLengthGeodesic(s, prev_QG); - - prev_QG = obstacle.getImpl().getPosInfo(s).KQ.p(); - } -} - const CurveSegment* CableSpan::Impl::findPrevActiveCurveSegment( const State& s, CurveSegmentIndex ix) const diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index c97f09f15..4d7372cee 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -110,10 +110,6 @@ class CurveSegment::Impl return false; } - const LocalGeodesicInfo& calcInitialGeodesic( - const State& s, - const GeodesicInitialConditions& g0) const; - // This will reevaluate the cached geodesic and status. // This will be called by the curve segment before updating the // position level cache variable of the CurveSegment. @@ -311,8 +307,6 @@ class CurveSegment::Impl return m_Geodesic.getStatus(s); } - void calcInitZeroLengthGeodesic(const State& state, Vec3 prev_QS) const; - void applyGeodesicCorrection( const State& state, const ContactGeometry::GeodesicCorrection& c) const; @@ -454,8 +448,6 @@ class CableSpan::Impl const PosInfo& getPosInfo(const State& state) const; const VelInfo& getVelInfo(const State& state) const; - void calcInitCablePath(State& s) const; - void applyBodyForces( const State& state, Real tension, From 028d2ca4637c1ca1a42a1124bd4d373d7924da0c Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 09:06:53 +0200 Subject: [PATCH 056/127] clamp initial integrator stepsize --- Simbody/src/Wrapping.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index dd9c5e1ad..5a0ef7df9 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -372,12 +372,12 @@ RKM::Y RKM::stepTo( _e = std::max(_e, err); - SimTK_ASSERT( - _h > _hMin, - "Geodesic Integrator failed: Reached very small stepsize"); - SimTK_ASSERT( - _h < _hMax, - "Geodesic Integrator failed: Reached very large stepsize"); + if(_h < _hMin) { + throw std::runtime_error("Geodesic Integrator failed: Reached very small stepsize"); + } + if(_h > _hMax) { + throw std::runtime_error("Geodesic Integrator failed: Reached very large stepsize"); + } if (init) { _h0 = _h; From 64f6092827d027fe13fdc04d55a3d44c632e8284 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 09:07:42 +0200 Subject: [PATCH 057/127] fix liftoff detection --- Simbody/src/Wrapping.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 5a0ef7df9..fc7cae76f 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -760,7 +760,7 @@ void LocalGeodesic::calcLiftoffIfNeeded( // For a zero-length curve, trigger liftoff when the prev and next points // lie above the surface plane. - if (dot(prev_QS - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= 0. && + if (dot(prev_QS - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= 0. || dot(next_PS - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= 0.) { // No liftoff. return; From 637b49a81af00cd766d00f0ab1d0342581889de6 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 09:08:33 +0200 Subject: [PATCH 058/127] set length after shooting new geodesic --- Simbody/src/Wrapping.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index fc7cae76f..976478b95 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -862,6 +862,7 @@ void LocalGeodesic::shootNewGeodesic( m_ProjectionMaxIter, m_ProjectionAccuracy, cache.samples); + cache.length = g0.l; /* using Y = ImplicitGeodesicState; */ /* if (analyticFormAvailable()) { */ From b3124ccebfd862173bf5ccfe4116631a0b235511 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 09:09:48 +0200 Subject: [PATCH 059/127] fix invalidating and updating lDot --- Simbody/src/Wrapping.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 976478b95..6e4e3c2a0 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1209,25 +1209,33 @@ void CableSpan::Impl::realizeTopology(State& s) 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 { + std::cout << "Cablespan: realizePosition\n"; if (getSubsystem().isCacheValueRealized(s, m_PosInfoIx)) { return; } calcPosInfo(s, updPosInfo(s)); getSubsystem().markCacheValueRealized(s, m_PosInfoIx); + std::cout << "done Path calcPosInfo\n"; + std::cout << " l = " << getPosInfo(s).l << "\n"; } void CableSpan::Impl::realizeVelocity(const State& s) const { + realizePosition(s); if (getSubsystem().isCacheValueRealized(s, m_VelInfoIx)) { return; } @@ -1238,6 +1246,7 @@ void CableSpan::Impl::realizeVelocity(const State& s) const void CableSpan::Impl::invalidatePositionLevelCache(const State& s) const { getSubsystem().markCacheValueNotRealized(s, m_PosInfoIx); + getSubsystem().markCacheValueNotRealized(s, m_VelInfoIx); } const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo( @@ -1258,8 +1267,7 @@ const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo( const State& s) const { realizeVelocity(s); - return Value::downcast( - getSubsystem().getCacheEntry(s, m_VelInfoIx)); + return Value::downcast(getSubsystem().getCacheEntry(s, m_VelInfoIx)); } CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const @@ -1543,7 +1551,7 @@ void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const { const PosInfo& pos = getPosInfo(s); - Real lengthDot = 0.; + Real& lengthDot = (velInfo.lengthDot = 0.); Vec3 v_GQ = m_OriginBody.findStationVelocityInGround(s, m_OriginPoint); const CurveSegment* lastActive = nullptr; From 719fc42c5293722e098647eb5a689aee999e6466 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 09:59:28 +0200 Subject: [PATCH 060/127] fix geodesic integration: flipped implicit fun sign --- Simbody/src/Wrapping.cpp | 83 ++++++++++++++++++++++++---------------- 1 file changed, 51 insertions(+), 32 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 6e4e3c2a0..530084298 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -40,6 +40,30 @@ static const int GeodesicDOF = 4; 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; @@ -94,13 +118,13 @@ void calcSurfaceProjectionFast( { size_t it = 0; for (; it < maxIter; ++it) { - const Real c = geometry.calcSurfaceValue(x); + const Real c = calcSurfaceConstraintValue(geometry, x); if (std::abs(c) < eps) { break; } - const Vec3 g = geometry.calcSurfaceGradient(x); + const Vec3 g = calcSurfaceConstraintGradient(geometry, x); x += -g * c / dot(g, g); } @@ -108,7 +132,7 @@ void calcSurfaceProjectionFast( it < maxIter, "Surface projection failed: Reached max iterations"); - UnitVec3 n(geometry.calcSurfaceGradient(x)); + UnitVec3 n(calcSurfaceConstraintGradient(geometry, x)); t = t - dot(n, t) * n; Real norm = t.norm(); SimTK_ASSERT(!isNaN(norm), "Surface projection failed: Detected NaN"); @@ -149,15 +173,15 @@ bool calcNearestPointOnLineImplicitly( const Vec3 pl = a + (b - a) * alpha; // Constraint evaluation at touchdown point. - const double c = geometry.calcSurfaceValue(pl); + const double c = calcSurfaceConstraintValue(geometry,pl); // Break on touchdown, TODO or not? if (std::abs(c) < eps) break; // Gradient at point on line. - const Vec3 g = geometry.calcSurfaceGradient(pl); - const Mat33 H = geometry.calcSurfaceHessian(pl); + const Vec3 g = calcSurfaceConstraintGradient(geometry, pl); + const Mat33 H = calcSurfaceConstraintHessian(geometry, pl); // Add a weight to the newton step to avoid large steps. constexpr double w = 0.5; @@ -183,7 +207,7 @@ bool calcNearestPointOnLineImplicitly( point = a + (b - a) * alpha; // Assumes a negative constraint evaluation means touchdown. - const bool contact = geometry.calcSurfaceValue(point) < eps; + const bool contact = calcSurfaceConstraintValue(geometry,point) < eps; // TODO handle here? if (iter >= maxIter) { @@ -221,8 +245,9 @@ ImplicitGeodesicState operator+( Real calcInfNorm(const ImplicitGeodesicState& q) { Real infNorm = 0.; - for (size_t r = 0; r < q.asVec().nrow(); ++r) { - infNorm = std::max(infNorm, q.asVec()[r]); + 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; } @@ -247,7 +272,7 @@ class RKM Y stepTo( Y y0, Real x1, - Real h0, + Real& h0, std::function& f, // Dynamics std::function& g, // Surface projection std::function& m, // State log @@ -258,11 +283,6 @@ class RKM return _failedCount; } - size_t getInitStepSize() const - { - return _h0; - } - private: static constexpr size_t ORDER = 5; @@ -327,7 +347,7 @@ Real RKM::step(Real h, std::function& f) RKM::Y RKM::stepTo( Y y0, Real x1, - Real h0, + Real& h0, std::function& f, std::function& g, std::function& m, @@ -337,7 +357,7 @@ RKM::Y RKM::stepTo( m(0., y0); _y.at(0) = std::move(y0); - _h = h0; + _h = std::min(h0 > _hMin ? h0 : _hMin, _hMax); _e = 0.; _failedCount = 0; @@ -372,17 +392,18 @@ RKM::Y RKM::stepTo( _e = std::max(_e, err); - if(_h < _hMin) { - throw std::runtime_error("Geodesic Integrator failed: Reached very small stepsize"); - } - if(_h > _hMax) { - throw std::runtime_error("Geodesic Integrator failed: Reached very large stepsize"); + if (_h < _hMin) { + throw std::runtime_error( + "Geodesic Integrator failed: Reached very small stepsize"); } if (init) { - _h0 = _h; + h0 = _h; } } + if (std::abs(x - x1) > 1e-13) { + throw std::runtime_error("failed to integrate"); + } return _y.at(0); } @@ -393,8 +414,8 @@ Real calcNormalCurvature( { const Vec3& p = point; const Vec3& v = tangent; - const Vec3 g = geometry.calcSurfaceGradient(p); - const Vec3 h_v = geometry.calcSurfaceHessian(p) * v; + 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(); } @@ -407,8 +428,8 @@ Real calcGeodesicTorsion( // TODO verify this! const Vec3& p = point; const Vec3& v = tangent; - const Vec3 g = geometry.calcSurfaceGradient(p); - const Vec3 h_v = geometry.calcSurfaceHessian(p) * v; + 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); } @@ -416,7 +437,7 @@ Real calcGeodesicTorsion( UnitVec3 calcSurfaceNormal(const ContactGeometry& geometry, Vec3 point) { const Vec3& p = point; - const Vec3 gradient = geometry.calcSurfaceGradient(p); + const Vec3 gradient = calcSurfaceConstraintGradient(geometry, p); return UnitVec3{gradient}; } @@ -462,9 +483,9 @@ Mat33 calcAdjoint(const Mat33& mat) Real calcGaussianCurvature(const ContactGeometry& geometry, Vec3 point) { const Vec3& p = point; - Vec3 g = geometry.calcSurfaceGradient(p); + Vec3 g = calcSurfaceConstraintGradient(geometry, p); Real gDotg = dot(g, g); - Mat33 adj = calcAdjoint(geometry.calcSurfaceHessian(p)); + Mat33 adj = calcAdjoint(calcSurfaceConstraintHessian(geometry, p)); if (gDotg * gDotg < 1e-13) { throw std::runtime_error( @@ -558,8 +579,6 @@ void calcGeodesicAndVariationImplicitly( 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]); - - ds = rkm.getInitStepSize(); } } // namespace From 844dcd1bc302a04a6976c47d35141438bfdd7c33 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 15:04:14 +0200 Subject: [PATCH 061/127] add decorativeGeometry --- Simbody/src/Wrapping.cpp | 63 +++++++++++++++++++++++++++++++++++++- Simbody/src/WrappingImpl.h | 46 ++++++++++++++++++++++++++-- 2 files changed, 106 insertions(+), 3 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 530084298..defff2aab 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -968,7 +968,9 @@ CurveSegment::Impl::Impl( m_Subsystem(&path.updImpl().updSubsystem()), m_Path(path), m_Index(-1), // TODO what to do with this index, and when m_Mobod(mobod), m_Offset(X_BS), - m_Geodesic(path.updImpl().updSubsystem(), geometry, initPointGuess) + m_Geodesic(path.updImpl().updSubsystem(), geometry, initPointGuess), + m_Decoration(geometry.createDecorativeGeometry() + .setColor(Orange).setOpacity(.75).setResolution(3)) {} void CurveSegment::Impl::realizeTopology(State& s) @@ -1622,6 +1624,65 @@ void CableSpan::Impl::applyBodyForces( } } +int CableSpan::Impl::calcDecorativeGeometryAndAppend( + const State& s, + Stage stage, + Array_& decorations) const +{ + const Vec3 color = Green; // Red Purple + + const PosInfo& ppe = getPosInfo(s); + + // Draw point at origin and termination. + decorations.push_back(DecorativePoint(ppe.xO).setColor(Green)); + decorations.push_back(DecorativePoint(ppe.xI).setColor(Red)); + + /* decorations.push_back(DecorativeLine(ppe.xO, ppe.xI) */ + /* .setColor(Purple) */ + /* .setLineThickness(3)); */ + + Vec3 lastCurvePoint = ppe.xO; + for (const CurveSegment& curveSegment : m_CurveSegments) { + const CurveSegment::Impl& curve = curveSegment.getImpl(); + + const Transform X_GS = curve.calcSurfaceFrameInGround(s); + DecorativeGeometry geo = curve.getDecoration(); + const Transform& X_SD = geo.getTransform(); // TODO getTransform? + + // Inactive surfaces are dimmed. + if (!curve.isActive(s)) { + decorations.push_back(geo.setTransform(X_GS * X_SD) + .setColor(Real(0.75) * geo.getColor())); + continue; + } + + decorations.push_back(geo.setTransform(X_GS * X_SD)); + + Vec3 prevPoint = findPrevPoint(s, curve.getIndex()); + Vec3 x_P = curve.getPosInfo(s).KP.p(); + + /* decorations.push_back(DecorativeLine(prevPoint, x_P) */ + /* .setColor(Purple) */ + /* .setLineThickness(3)); */ + + lastCurvePoint = curve.getPosInfo(s).KQ.p(); + + Transform K_P = curve.getPosInfo(s).KP; + Transform K_Q = curve.getPosInfo(s).KQ; + decorations.push_back(DecorativeLine(K_P.p(), K_P.p() + K_P.R().getAxisUnitVec(TangentAxis)) + .setColor(Orange) + .setLineThickness(3)); + decorations.push_back(DecorativeLine(K_Q.p(), K_Q.p() + K_Q.R().getAxisUnitVec(TangentAxis)) + .setColor(Blue) + .setLineThickness(3)); + } + + /* decorations.push_back(DecorativeLine(lastCurvePoint, ppe.xI) */ + /* .setColor(Purple) */ + /* .setLineThickness(3)); */ + return 0; +} + //============================================================================== // SUBSYSTEM //============================================================================== diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 4d7372cee..09739176c 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -355,8 +355,16 @@ class CurveSegment::Impl // TODO allow for user to shoot his own geodesic. /* void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; */ - CableSubsystem& updSubsystem() {return *m_Subsystem;} - const CableSubsystem& getSubsystem() const {return *m_Subsystem;} + CableSubsystem& updSubsystem() + { + return *m_Subsystem; + } + const CableSubsystem& getSubsystem() const + { + return *m_Subsystem; + } + + const DecorativeGeometry& getDecoration() const {return m_Decoration;} private: PosInfo& updPosInfo(const State& state) const @@ -377,6 +385,9 @@ class CurveSegment::Impl LocalGeodesic m_Geodesic; + // Decoration TODO should this be here? + DecorativeGeometry m_Decoration; + // TOPOLOGY CACHE CacheEntryIndex m_PosInfoIx; }; @@ -453,6 +464,20 @@ class CableSpan::Impl Real tension, Vector_& bodyForcesInG) const; + int calcDecorativeGeometryAndAppend( + const State& state, + Stage stage, + Array_& decorations) const; + + void calcPathPoints(const State& state, std::vector& points) const + { + points.push_back(getPosInfo(state).xO); + for (const CurveSegment& curve: m_CurveSegments) { + curve.getImpl().calcPathPoints(state, points); + } + points.push_back(getPosInfo(state).xI); + } + private: PosInfo& updPosInfo(const State& s) const; VelInfo& updVelInfo(const State& state) const; @@ -674,6 +699,23 @@ class CableSubsystem::Impl : public Subsystem::Guts return Value::updDowncast(updCacheEntry(state, m_CacheIx)); } + 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; + } + SimTK_DOWNCAST(Impl, Subsystem::Guts); private: From b26dc19a6ba42ec26edea3bf2b1bc7a02d237b6a Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 17:40:12 +0200 Subject: [PATCH 062/127] fmt --- Simbody/include/simbody/internal/Wrapping.h | 31 ++++++++++++++++++++- Simbody/src/Wrapping.cpp | 26 +++++++---------- 2 files changed, 40 insertions(+), 17 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 8a69666bc..d5205b420 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -54,10 +54,39 @@ class SimTK_SIMBODY_EXPORT CurveSegment // TODO remove? keep? make private? const CableSpan& getCable() const; - Real getSegmentLength(const State& s); + Real getSegmentLength(const State& s) const; + const Transform& getFrenetFrameStart(const State& s) const { + throw std::runtime_error("NOTYETIMPLEMENTED"); + } + const Transform& getFrenetFrameEnd(const State& s) const { + throw std::runtime_error("NOTYETIMPLEMENTED"); + } Status getStatus(const State& state) const; + const ContactGeometry& getContactGeometry() const; + const Transform& getSurfaceToGroundTransform(const State& s) const; + + Real calcNormalCurvature(Vec3 point, Vec3 tangent) const { + throw std::runtime_error("NOTYETIMPLEMENTED"); + return NaN; + } + + Real calcGeodesicTorsion(Vec3 point, Vec3 tangent) const { + throw std::runtime_error("NOTYETIMPLEMENTED"); + return NaN; + } + + UnitVec3 calcSurfaceNormal(Vec3 point) const { + throw std::runtime_error("NOTYETIMPLEMENTED"); + return {NaN, NaN, NaN}; + } + + Real calcSurfaceValue(Vec3 point) const { + throw std::runtime_error("NOTYETIMPLEMENTED"); + return NaN; + } + // TODO seems useful: /* void setDisabled(const State& state) const; */ /* void setEnabled(const State& state) const; */ diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index defff2aab..754868188 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -41,25 +41,19 @@ namespace { // TODO Sign of implicit function was flipped. -Real calcSurfaceConstraintValue( - const ContactGeometry& geometry, - Vec3 point) +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) +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) +Mat33 calcSurfaceConstraintHessian(const ContactGeometry& geometry, Vec3 point) { return -geometry.calcSurfaceHessian(point); } @@ -173,7 +167,7 @@ bool calcNearestPointOnLineImplicitly( const Vec3 pl = a + (b - a) * alpha; // Constraint evaluation at touchdown point. - const double c = calcSurfaceConstraintValue(geometry,pl); + const double c = calcSurfaceConstraintValue(geometry, pl); // Break on touchdown, TODO or not? if (std::abs(c) < eps) @@ -207,7 +201,7 @@ bool calcNearestPointOnLineImplicitly( point = a + (b - a) * alpha; // Assumes a negative constraint evaluation means touchdown. - const bool contact = calcSurfaceConstraintValue(geometry,point) < eps; + const bool contact = calcSurfaceConstraintValue(geometry, point) < eps; // TODO handle here? if (iter >= maxIter) { @@ -244,7 +238,7 @@ ImplicitGeodesicState operator+( Real calcInfNorm(const ImplicitGeodesicState& q) { - Real infNorm = 0.; + 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])); @@ -414,8 +408,8 @@ Real calcNormalCurvature( { const Vec3& p = point; const Vec3& v = tangent; - const Vec3 g = calcSurfaceConstraintGradient(geometry,p); - const Vec3 h_v = calcSurfaceConstraintHessian(geometry,p) * v; + 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(); } @@ -428,8 +422,8 @@ Real calcGeodesicTorsion( // 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 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); } From 9b63f6d95bc69316d270eb1f6fed627126b03abb Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 17:41:02 +0200 Subject: [PATCH 063/127] use throw instead of simtk_assert --- Simbody/src/Wrapping.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 754868188..1b5478d1d 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -122,18 +122,20 @@ void calcSurfaceProjectionFast( x += -g * c / dot(g, g); } - SimTK_ASSERT( - it < maxIter, + if(it >= maxIter) {throw std::runtime_error( "Surface projection failed: Reached max iterations"); + } + + if(std::abs(geometry.calcSurfaceValue(x)) > eps) {throw std::runtime_error( + "Surface projection failed: no longer on surface"); + } UnitVec3 n(calcSurfaceConstraintGradient(geometry, x)); t = t - dot(n, t) * n; Real norm = t.norm(); - SimTK_ASSERT(!isNaN(norm), "Surface projection failed: Detected NaN"); - SimTK_ASSERT( - norm > 1e-13, - "Surface projection failed: Tangent guess is parallel to surface " - "normal"); + if(isNaN(norm)) throw std::runtime_error("Surface projection failed: Detected NaN"); + if(norm < 1e-13) throw std::runtime_error( "Surface projection failed: Tangent guess is parallel to surface normal"); + t = t / norm; } From f6dbf01e9914a67b045c60e6e4abe3e9d2daa6ba Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 17:41:24 +0200 Subject: [PATCH 064/127] project initial state --- Simbody/src/Wrapping.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 1b5478d1d..15ecb6fc9 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -567,6 +567,7 @@ void calcGeodesicAndVariationImplicitly( }; Y y0(x, t); + g(y0); RKM rkm{}; From 9492dff5a9238ae5ea6ab5867996ea6260032e48 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 17:41:50 +0200 Subject: [PATCH 065/127] fix path correctin computaiton --- Simbody/src/Wrapping.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 15ecb6fc9..ef019f82c 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -593,10 +593,10 @@ const Correction* calcPathCorrections(SolverData& data) data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; for (int i = 0; i < data.mat.nrow(); ++i) { - data.mat[i][i] += w; + data.mat[i][i] += w + 1.; } data.matInv = data.mat; - data.vec = data.pathErrorJacobian.transpose() * data.pathError; + data.vec = data.pathErrorJacobian.transpose() * (data.pathError * (-1.)); data.matInv.solve(data.vec, data.pathCorrection); static_assert( From c8e794390f7a99e86fa1c2507b53c9d60b45ba59 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 17:42:20 +0200 Subject: [PATCH 066/127] fmt --- Simbody/src/Wrapping.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index ef019f82c..b39ab2689 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -689,9 +689,9 @@ using LocalGeodesic = CurveSegment::Impl::LocalGeodesic; void LocalGeodesic::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 = Status::Liftoff; + Value* cache = new Value(); + cache->upd().length = 0.; + cache->upd().status = Status::Liftoff; cache->upd().trackingPointOnLine = getInitialPointGuess(); m_CacheIx = updSubsystem().allocateAutoUpdateDiscreteVariable( From 4ce3ed97017b0181f4fecd86b88e52b51224e141 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 18:22:23 +0200 Subject: [PATCH 067/127] use throw instead of simtk_assert --- Simbody/src/Wrapping.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index b39ab2689..c977d18b2 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -720,6 +720,9 @@ const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( { realizePosition(s); CacheEntry& cache = updCacheEntry(s); + if (isNaN(cache.length)) { + throw std::runtime_error("Found cached length = NaN"); + } calcCacheEntry(prev_QS, next_PS, cache); return cache; } @@ -835,13 +838,20 @@ void LocalGeodesic::assertSurfaceBounds( const Vec3& prev_QS, const Vec3& next_PS) const { + /* std::cout << "LocalGeodesic::assertSurfaceBounds\n"; */ // Make sure that the previous point does not lie inside the surface. - SimTK_ASSERT( - m_Geometry.calcSurfaceValue(prev_QS) < 0., - "Unable to wrap over surface: Preceding point lies inside the surface"); - SimTK_ASSERT( - m_Geometry.calcSurfaceValue(next_PS) < 0., - "Unable to wrap over surface: Next point lies inside the surface"); + if (calcSurfaceConstraintValue(m_Geometry, prev_QS) < 0.) { + std::cout << "prev_QS = " << prev_QS << "\n"; + std::cout << "prev_PS = " << next_PS << "\n"; + throw std::runtime_error("Unable to wrap over surface: Preceding point " + "lies inside the surface"); + } + if (calcSurfaceConstraintValue(m_Geometry, next_PS) < 0.) { + std::cout << "prev_QS = " << prev_QS << "\n"; + std::cout << "prev_PS = " << next_PS << "\n"; + throw std::runtime_error( + "Unable to wrap over surface: Next point lies inside the surface"); + } } void LocalGeodesic::calcCacheEntry( From a5d87a6f4bf44ad563f84d834d2d82455c07cc1e Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 18:24:38 +0200 Subject: [PATCH 068/127] skip calculating the pathpoints if not active --- Simbody/src/Wrapping.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index c977d18b2..202e55054 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -950,7 +950,7 @@ const CableSpan& CurveSegment::getCable() const return getImpl().getCable(); } -Real CurveSegment::getSegmentLength(const State& s) +Real CurveSegment::getSegmentLength(const State& s) const { getImpl().realizeCablePosition(s); return getImpl().getPosInfo(s).length; @@ -1007,25 +1007,30 @@ void CurveSegment::Impl::realizeCablePosition(const State& s) const void CurveSegment::Impl::invalidatePositionLevelCache(const State& state) const { getSubsystem().markCacheValueNotRealized(state, m_PosInfoIx); - m_Path.getImpl().invalidatePositionLevelCache(state); + /* m_Path.getImpl().invalidatePositionLevelCache(state); */ } void CurveSegment::Impl::applyGeodesicCorrection( const State& s, const CurveSegment::Impl::Correction& c) const { + /* std::cout << "apply correction = " << c << "\n"; */ // Apply correction to curve. m_Geodesic.applyGeodesicCorrection(s, c); // Invalidate position level cache. - invalidatePositionLevelCache(s); + /* invalidatePositionLevelCache(s); */ } void CurveSegment::Impl::calcPathPoints( const State& s, std::vector& points) const { - const Transform& X_GS = getPosInfo(s).X_GS; + if (!isActive(s)) { + return; + } + const PosInfo& ppe = getPosInfo(s); + const Transform& X_GS = ppe.X_GS; size_t initSize = points.size(); m_Geodesic.calcPathPoints(s, points); for (size_t i = points.size() - initSize; i < points.size(); ++i) { From 47a080018fcd0050b5542c8e6b8a74a1750041d8 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 18:31:26 +0200 Subject: [PATCH 069/127] fix wrench computation --- Simbody/src/Wrapping.cpp | 8 +++++--- Simbody/src/WrappingImpl.h | 6 ++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 202e55054..07d6a57b2 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1098,10 +1098,12 @@ SpatialVec CurveSegment::Impl::calcAppliedWrenchInGround( const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); const Vec3 F_G = tension * (t_Q - t_P); - const Vec3 x_GS = posInfo.X_GS.p(); - const Vec3& r_P = posInfo.KP.p() - x_GS; - const Vec3& r_Q = posInfo.KQ.p() - x_GS; + const Transform& X_GB = m_Mobod.getBodyTransform(s); + const Vec3 x_GB = X_GB.p(); + const Vec3 r_P = posInfo.KP.p() - x_GB; + const Vec3 r_Q = posInfo.KQ.p() - x_GB; const Vec3 M_G = tension * (r_Q % t_Q - r_P % t_P); + return {M_G, F_G}; } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 09739176c..8ce76d983 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -544,10 +544,8 @@ class CableSpan::Impl Array_ m_CurveSegments{}; - Real m_PathErrorBound = 0.1; - Real m_ObsErrorBound = 0.1; - size_t m_PathMaxIter = 10; - size_t m_ObsMaxIter = 10; + Real m_PathErrorBound = 1e-4; + size_t m_PathMaxIter = 50; // TOPOLOGY CACHE (set during realizeTopology()) CacheEntryIndex m_PosInfoIx; From 699076bfcf9eea4f0c93814c4f150307df046323 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 1 May 2024 18:37:11 +0200 Subject: [PATCH 070/127] remove realizing position and velocity cache from Cablesubsystem --- Simbody/src/WrappingImpl.h | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 8ce76d983..f6c485212 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -664,24 +664,6 @@ class CableSubsystem::Impl : public Subsystem::Guts return 0; } - int realizeSubsystemPositionImpl(const State& state) const override - { - for (CableSpanIndex ix(0); ix < cables.size(); ++ix) { - const CableSpan& path = getCablePath(ix); - path.getImpl().realizePosition(state); - } - return 0; - } - - int realizeSubsystemVelocityImpl(const State& state) const override - { - for (CableSpanIndex ix(0); ix < cables.size(); ++ix) { - const CableSpan& path = getCablePath(ix); - path.getImpl().realizeVelocity(state); - } - return 0; - } - void realizeTopology(State& state) { CacheEntry cache{}; From 138f8141b43725248aa8af7c3791f42f261abf50 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 07:19:54 +0200 Subject: [PATCH 071/127] Refactor: Remove LocalGeodesic layer from CurveSegment --- Simbody/src/Wrapping.cpp | 566 ++++++++-------------------------- Simbody/src/WrappingImpl.h | 546 +++++++++++++++++--------------- examples/ExampleCableSpan.cpp | 54 ++-- 3 files changed, 450 insertions(+), 716 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 07d6a57b2..82910e9c3 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -9,21 +9,18 @@ using namespace SimTK; -using Correction = ContactGeometry::GeodesicCorrection; -using FrameVariation = ContactGeometry::GeodesicFrameVariation; -using FrenetFrame = ContactGeometry::FrenetFrame; -using GeodesicInfo = CurveSegment::Impl::PosInfo; -using LocalGeodesicInfo = CurveSegment::Impl::LocalGeodesic::LocalGeodesicInfo; -using LocalGeodesicSample = - CurveSegment::Impl::LocalGeodesic::LocalGeodesicSample; -using GeodesicInitialConditions = - CurveSegment::Impl::LocalGeodesic::GeodesicInitialConditions; -using GeodesicJacobian = Vec4; -using PointVariation = ContactGeometry::GeodesicPointVariation; -using Variation = ContactGeometry::GeodesicVariation; -using LineSegment = CableSpan::LineSegment; -using Status = CurveSegment::Status; -using SolverData = CableSubsystem::Impl::SolverData; +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::Status; +using Variation = ContactGeometry::GeodesicVariation; //============================================================================== // CONSTANTS @@ -122,19 +119,24 @@ void calcSurfaceProjectionFast( x += -g * c / dot(g, g); } - if(it >= maxIter) {throw std::runtime_error( - "Surface projection failed: Reached max iterations"); + if (it >= maxIter) { + throw std::runtime_error( + "Surface projection failed: Reached max iterations"); } - if(std::abs(geometry.calcSurfaceValue(x)) > eps) {throw std::runtime_error( - "Surface projection failed: no longer on surface"); + if (std::abs(geometry.calcSurfaceValue(x)) > eps) { + throw std::runtime_error( + "Surface projection failed: no longer on surface"); } UnitVec3 n(calcSurfaceConstraintGradient(geometry, x)); t = t - dot(n, t) * n; Real norm = t.norm(); - if(isNaN(norm)) throw std::runtime_error("Surface projection failed: Detected NaN"); - if(norm < 1e-13) throw std::runtime_error( "Surface projection failed: Tangent guess is parallel to surface normal"); + if (isNaN(norm)) + throw std::runtime_error("Surface projection failed: Detected NaN"); + if (norm < 1e-13) + throw std::runtime_error("Surface projection failed: Tangent guess is " + "parallel to surface normal"); t = t / norm; } @@ -607,164 +609,17 @@ const Correction* calcPathCorrections(SolverData& data) "Invalid size of path corrections vector"); return reinterpret_cast(&data.pathCorrection[0]); } -} // namespace - -//============================================================================== -// GEODESIC INITIAL CONDITIONS -//============================================================================== - -GeodesicInitialConditions GeodesicInitialConditions::CreateCorrected( - const FrenetFrame& KP, - const Variation& dKP, - Real l, - const Correction& c) -{ - GeodesicInitialConditions g0; - - Vec3 v = dKP[1] * c; - g0.x = KP.p() + v; - - Vec3 w = dKP[0] * c; - const UnitVec3 t = KP.R().getAxisUnitVec(TangentAxis); - g0.t = t + cross(w, t); - - // Take the length correction, and add to the current length. - Real dl = c[3]; // Length increment is the last correction element. - g0.l = std::max(l + dl, 0.); // Clamp length to be nonnegative. - - return g0; -} - -GeodesicInitialConditions GeodesicInitialConditions:: - CreateFromGroundInSurfaceFrame( - const Transform& X_GS, - Vec3 x_G, - Vec3 t_G, - Real l) -{ - GeodesicInitialConditions g0; - - g0.x = X_GS.shiftBaseStationToFrame(x_G); - g0.t = X_GS.xformBaseVecToFrame(t_G); - g0.l = l; - - return g0; -} - -GeodesicInitialConditions GeodesicInitialConditions::CreateZeroLengthGuess( - Vec3 prev_QS, - Vec3 xGuess_S) -{ - GeodesicInitialConditions g0; - g0.x = xGuess_S; - g0.t = xGuess_S - prev_QS; - g0.l = 0.; - - return g0; -} - -GeodesicInitialConditions GeodesicInitialConditions::CreateAtTouchdown( - Vec3 prev_QS, - Vec3 next_PS, - Vec3 trackingPointOnLine) -{ - GeodesicInitialConditions g0; - - g0.x = trackingPointOnLine; - g0.t = next_PS - prev_QS; - g0.l = 0.; - - return g0; -} +} // namespace //============================================================================== // SURFACE IMPL //============================================================================== -using LocalGeodesic = CurveSegment::Impl::LocalGeodesic; - -//------------------------------------------------------------------------------ -// REALIZE CACHE -//------------------------------------------------------------------------------ -void LocalGeodesic::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 = Status::Liftoff; - cache->upd().trackingPointOnLine = getInitialPointGuess(); - - m_CacheIx = updSubsystem().allocateAutoUpdateDiscreteVariable( - s, - Stage::Report, - cache, - Stage::Position); -} - -void LocalGeodesic::realizePosition(const State& s) const -{ - if (!getSubsystem().isDiscreteVarUpdateValueRealized(s, m_CacheIx)) { - updCacheEntry(s) = getPrevCacheEntry(s); - getSubsystem().markDiscreteVarUpdateValueRealized(s, m_CacheIx); - } -} - -//------------------------------------------------------------------------------ -// PUBLIC METHODS -//------------------------------------------------------------------------------ - -const LocalGeodesicInfo& LocalGeodesic::calcLocalGeodesicInfo( - const State& s, - Vec3 prev_QS, - Vec3 next_PS) const -{ - realizePosition(s); - CacheEntry& cache = updCacheEntry(s); - if (isNaN(cache.length)) { - throw std::runtime_error("Found cached length = NaN"); - } - calcCacheEntry(prev_QS, next_PS, cache); - return cache; -} -void LocalGeodesic::applyGeodesicCorrection(const State& s, const Correction& c) - const -{ - // Get the previous geodesic. - const CacheEntry& g = getCacheEntry(s); - - // Get corrected initial conditions. - const GeodesicInitialConditions g0 = - GeodesicInitialConditions::CreateCorrected(g.K_P, g.dK_P, g.length, c); - - // Shoot the new geodesic. - shootNewGeodesic(g0, updCacheEntry(s)); -} - -void LocalGeodesic::calcPathPoints(const State& s, std::vector& points) - const -{ - const CacheEntry& cache = getCacheEntry(s); - - if (analyticFormAvailable()) { - throw std::runtime_error("NOTYETIMPLEMENTED"); - /* m_Geometry.resampleGeodesicPointsAnalytically( */ - /* cache.K_P, */ - /* cache.K_Q, */ - /* cache.length, */ - /* m_NumberOfAnalyticPoints, */ - /* points); */ - } else { - for (const LocalGeodesicSample& sample : cache.samples) { - points.push_back(sample.frame.p()); - } - } -} - -void LocalGeodesic::calcLiftoffIfNeeded( +void CurveSegment::Impl::calcLiftoffIfNeeded( const Vec3& prev_QS, const Vec3& next_PS, - CacheEntry& cache) const + CurveSegment::Impl::InstanceEntry& cache) const { // Only attempt liftoff when currently wrapping the surface. LocalGeodesicInfo& g = cache; @@ -791,10 +646,10 @@ void LocalGeodesic::calcLiftoffIfNeeded( cache.trackingPointOnLine = g.K_P.p(); } -void LocalGeodesic::calcTouchdownIfNeeded( +void CurveSegment::Impl::calcTouchdownIfNeeded( const Vec3& prev_QS, const Vec3& next_PS, - CacheEntry& cache) const + CurveSegment::Impl::InstanceEntry& cache) const { // Only attempt touchdown when liftoff. LocalGeodesicInfo& g = cache; @@ -804,22 +659,13 @@ void LocalGeodesic::calcTouchdownIfNeeded( // Detect touchdown by computing the point on the line from x_QS to x_PS // that is nearest to the surface. - bool touchdownDetected; - if (analyticFormAvailable()) { - throw std::runtime_error("NOTYETIMPLEMENTED"); - /* touchdownDetected = m_Geometry.calcNearestPointOnLineAnalytically( */ - /* prev_QS, */ - /* next_PS, */ - /* cache.trackingPointOnLine); */ - } else { - touchdownDetected = calcNearestPointOnLineImplicitly( - m_Geometry, - prev_QS, - next_PS, - cache.trackingPointOnLine, - m_TouchdownIter, - m_TouchdownAccuracy); - } + const bool touchdownDetected = calcNearestPointOnLineImplicitly( + m_Geometry, + prev_QS, + next_PS, + cache.trackingPointOnLine, + m_TouchdownIter, + m_TouchdownAccuracy); if (!touchdownDetected) { return; } @@ -827,14 +673,15 @@ void LocalGeodesic::calcTouchdownIfNeeded( // Touchdown detected: Remove the liftoff status flag. g.status = Status::Ok; // Shoot a zero length geodesic at the touchdown point. - GeodesicInitialConditions g0 = GeodesicInitialConditions::CreateAtTouchdown( - prev_QS, - next_PS, - cache.trackingPointOnLine); - shootNewGeodesic(g0, cache); + shootNewGeodesic( + cache.trackingPointOnLine, + next_PS - prev_QS, + 0., + cache.sHint, + cache); } -void LocalGeodesic::assertSurfaceBounds( +void CurveSegment::Impl::assertSurfaceBounds( const Vec3& prev_QS, const Vec3& next_PS) const { @@ -854,31 +701,18 @@ void LocalGeodesic::assertSurfaceBounds( } } -void LocalGeodesic::calcCacheEntry( - const Vec3& prev_QS, - const Vec3& next_PS, - CacheEntry& cache) const -{ - LocalGeodesicInfo& g = cache; - - if (g.status == Status::Disabled) { - return; - } - - assertSurfaceBounds(prev_QS, next_PS); - calcTouchdownIfNeeded(prev_QS, next_PS, cache); - calcLiftoffIfNeeded(prev_QS, next_PS, cache); -} - -void LocalGeodesic::shootNewGeodesic( - const GeodesicInitialConditions& g0, - CacheEntry& cache) const +void CurveSegment::Impl::shootNewGeodesic( + Vec3 x, + Vec3 t, + Real l, + Real dsHint, + InstanceEntry& cache) const { calcGeodesicAndVariationImplicitly( m_Geometry, - g0.x, - g0.t, - g0.l, + x, + t, + l, cache.sHint, cache.K_P, cache.dK_P, @@ -888,41 +722,7 @@ void LocalGeodesic::shootNewGeodesic( m_ProjectionMaxIter, m_ProjectionAccuracy, cache.samples); - cache.length = g0.l; - - /* using Y = ImplicitGeodesicState; */ - /* if (analyticFormAvailable()) { */ - - /* m_Geometry.calcGeodesicWithVariationAnalytically( */ - /* g0.x, */ - /* g0.t, */ - /* g0.l, */ - /* cache.KP, */ - /* cache.dKP, */ - /* cache.KQ, */ - /* cache.dKQ); */ - /* } else { */ - /* // Compute geodesic start boundary frame and variation. */ - /* m_Geometry.calcNearestFrenetFrameImplicitlyFast( */ - /* g0.x, */ - /* g0.t, */ - /* cache.KP, */ - /* m_ProjectionMaxIter, */ - /* m_ProjectionRequiredAccuracy); */ - /* m_Geometry.calcGeodesicStartFrameVariationImplicitly( */ - /* cache.KP, */ - /* cache.dKP); */ - /* // Compute geodesic end boundary frame amd variation (shoot new */ - /* // geodesic). */ - /* m_Geometry.calcGeodesicEndFrameVariationImplicitly( */ - /* cache.KP, */ - /* g0.l, */ - /* cache.KQ, */ - /* cache.dKQ, */ - /* cache.sHint, */ - /* m_IntegratorAccuracy, */ - /* cache.frames); */ - /* } */ + cache.length = l; } //============================================================================== @@ -953,13 +753,13 @@ const CableSpan& CurveSegment::getCable() const Real CurveSegment::getSegmentLength(const State& s) const { getImpl().realizeCablePosition(s); - return getImpl().getPosInfo(s).length; + return getImpl().getInstanceEntry(s).length; } CurveSegment::Status CurveSegment::getStatus(const State& s) const { getImpl().realizeCablePosition(s); - return getImpl().getStatus(s); + return getImpl().getInstanceEntry(s).status; } //============================================================================== @@ -974,159 +774,19 @@ CurveSegment::Impl::Impl( Vec3 initPointGuess) : m_Subsystem(&path.updImpl().updSubsystem()), m_Path(path), m_Index(-1), // TODO what to do with this index, and when - m_Mobod(mobod), m_Offset(X_BS), - m_Geodesic(path.updImpl().updSubsystem(), geometry, initPointGuess), + m_Mobod(mobod), m_Offset(X_BS), m_Geometry(geometry), + m_InitPointGuess(initPointGuess), m_Decoration(geometry.createDecorativeGeometry() - .setColor(Orange).setOpacity(.75).setResolution(3)) + .setColor(Orange) + .setOpacity(.75) + .setResolution(3)) {} -void CurveSegment::Impl::realizeTopology(State& s) -{ - m_Geodesic.realizeTopology(s); - - PosInfo posInfo{}; - m_PosInfoIx = updSubsystem().allocateCacheEntry( - s, - Stage::Position, - new Value(posInfo)); -} - -void CurveSegment::Impl::realizePosition(const State& s) const -{ - if (!getSubsystem().isCacheValueRealized(s, m_PosInfoIx)) { - calcPosInfo(s, updPosInfo(s)); - getSubsystem().markCacheValueRealized(s, m_PosInfoIx); - } -} - void CurveSegment::Impl::realizeCablePosition(const State& s) const { m_Path.getImpl().realizePosition(s); } -void CurveSegment::Impl::invalidatePositionLevelCache(const State& state) const -{ - getSubsystem().markCacheValueNotRealized(state, m_PosInfoIx); - /* m_Path.getImpl().invalidatePositionLevelCache(state); */ -} - -void CurveSegment::Impl::applyGeodesicCorrection( - const State& s, - const CurveSegment::Impl::Correction& c) const -{ - /* std::cout << "apply correction = " << c << "\n"; */ - // Apply correction to curve. - m_Geodesic.applyGeodesicCorrection(s, c); - - // Invalidate position level cache. - /* invalidatePositionLevelCache(s); */ -} - -void CurveSegment::Impl::calcPathPoints( - const State& s, - std::vector& points) const -{ - if (!isActive(s)) { - return; - } - const PosInfo& ppe = getPosInfo(s); - const Transform& X_GS = ppe.X_GS; - size_t initSize = points.size(); - m_Geodesic.calcPathPoints(s, points); - for (size_t i = points.size() - initSize; i < points.size(); ++i) { - points.at(i) = X_GS.shiftFrameStationToBase(points.at(i)); - } -} - -namespace -{ -void xformSurfaceGeodesicToGround( - const LocalGeodesicInfo& geodesic_S, - const Transform& X_GS, - CurveSegment::Impl::PosInfo& geodesic_G) -{ - geodesic_G.X_GS = X_GS; - - // Store the the local geodesic in ground frame. - geodesic_G.KP = X_GS.compose(geodesic_S.K_P); - geodesic_G.KQ = X_GS.compose(geodesic_S.K_Q); - - geodesic_G.dKP[0] = X_GS.R() * geodesic_S.dK_P[0]; - geodesic_G.dKP[1] = X_GS.R() * geodesic_S.dK_P[1]; - - geodesic_G.dKQ[0] = X_GS.R() * geodesic_S.dK_Q[0]; - geodesic_G.dKQ[1] = X_GS.R() * geodesic_S.dK_Q[1]; - - geodesic_G.length = geodesic_S.length; -} -} // namespace - -void CurveSegment::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const -{ - if (m_Geodesic.getStatus(s) == Status::Disabled) { - return; - } - - // Compute tramsform from local surface frame to ground. - const Transform& X_GS = calcSurfaceFrameInGround(s); - - // Get the path points before and after this segment. - const Vec3 prev_G = m_Path.getImpl().findPrevPoint(s, m_Index); - const Vec3 next_G = m_Path.getImpl().findNextPoint(s, m_Index); - - // Transform the prev and next path points to the surface frame. - const Vec3 prev_S = X_GS.shiftBaseStationToFrame(prev_G); - const Vec3 next_S = X_GS.shiftBaseStationToFrame(next_G); - - // Detect liftoff, touchdown and potential invalid configurations. - // TODO this doesnt follow the regular invalidation scheme... - // Grab the last geodesic that was computed. - const LocalGeodesicInfo& geodesic_S = - m_Geodesic.calcLocalGeodesicInfo(s, prev_S, next_S); - - // Store the the local geodesic in ground frame. - xformSurfaceGeodesicToGround(geodesic_S, X_GS, updPosInfo(s)); -} - -SpatialVec CurveSegment::Impl::calcAppliedWrenchInGround( - const State& s, - Real tension) const -{ - const PosInfo& posInfo = getPosInfo(s); - - const UnitVec3& t_P = posInfo.KP.R().getAxisUnitVec(TangentAxis); - const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); - const Vec3 F_G = tension * (t_Q - t_P); - - const Transform& X_GB = m_Mobod.getBodyTransform(s); - const Vec3 x_GB = X_GB.p(); - const Vec3 r_P = posInfo.KP.p() - x_GB; - const Vec3 r_Q = posInfo.KQ.p() - x_GB; - const Vec3 M_G = tension * (r_Q % t_Q - r_P % t_P); - - return {M_G, F_G}; -} - -void CurveSegment::Impl::applyBodyForce( - const State& s, - Real tension, - Vector_& bodyForcesInG) const -{ - // TODO why? - if (tension <= 0) { - return; - } - - if (getStatus(s) != Status::Ok) { - return; - } - - m_Mobod.applyBodyForce( - s, - calcAppliedWrenchInGround(s, tension), - bodyForcesInG); -} - //============================================================================== // PATH HELPERS //============================================================================== @@ -1258,14 +918,11 @@ void CableSpan::Impl::realizeTopology(State& s) void CableSpan::Impl::realizePosition(const State& s) const { - std::cout << "Cablespan: realizePosition\n"; if (getSubsystem().isCacheValueRealized(s, m_PosInfoIx)) { return; } calcPosInfo(s, updPosInfo(s)); getSubsystem().markCacheValueRealized(s, m_PosInfoIx); - std::cout << "done Path calcPosInfo\n"; - std::cout << " l = " << getPosInfo(s).l << "\n"; } void CableSpan::Impl::realizeVelocity(const State& s) const @@ -1278,12 +935,6 @@ void CableSpan::Impl::realizeVelocity(const State& s) const getSubsystem().markCacheValueRealized(s, m_VelInfoIx); } -void CableSpan::Impl::invalidatePositionLevelCache(const State& s) const -{ - getSubsystem().markCacheValueNotRealized(s, m_PosInfoIx); - getSubsystem().markCacheValueNotRealized(s, m_VelInfoIx); -} - const CableSpan::Impl::PosInfo& CableSpan::Impl::getPosInfo( const State& s) const { @@ -1302,7 +953,8 @@ const CableSpan::Impl::VelInfo& CableSpan::Impl::getVelInfo( const State& s) const { realizeVelocity(s); - return Value::downcast(getSubsystem().getCacheEntry(s, m_VelInfoIx)); + return Value::downcast( + getSubsystem().getCacheEntry(s, m_VelInfoIx)); } CableSpan::Impl::VelInfo& CableSpan::Impl::updVelInfo(const State& s) const @@ -1317,7 +969,10 @@ const CurveSegment* CableSpan::Impl::findPrevActiveCurveSegment( { for (int i = ix - 1; i > 0; --i) { // Find the active segment before the current. - if (m_CurveSegments.at(CurveSegmentIndex(i)).getImpl().isActive(s)) { + if (m_CurveSegments.at(CurveSegmentIndex(i)) + .getImpl() + .getInstanceEntry(s) + .isActive()) { return &m_CurveSegments.at(CurveSegmentIndex(i)); } } @@ -1330,26 +985,33 @@ const CurveSegment* CableSpan::Impl::findNextActiveCurveSegment( { // Find the active segment after the current. for (int i = ix + 1; i < m_CurveSegments.size(); ++i) { - if (m_CurveSegments.at(CurveSegmentIndex(i)).getImpl().isActive(s)) { + 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 +Vec3 CableSpan::Impl::findPrevPoint(const State& s, const CurveSegment& curve) + const { - const CurveSegment* segment = findPrevActiveCurveSegment(s, ix); - return segment ? segment->getImpl().getPosInfo(s).KQ.p() + const CurveSegment* segment = + findPrevActiveCurveSegment(s, curve.getImpl().getIndex()); + return segment ? segment->getImpl().calcFinalContactPoint(s) : m_OriginBody.getBodyTransform(s).shiftFrameStationToBase( m_OriginPoint); } -Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) const +Vec3 CableSpan::Impl::findNextPoint(const State& s, const CurveSegment& curve) + const { - const CurveSegment* segment = findNextActiveCurveSegment(s, ix); + const CurveSegment* segment = + findNextActiveCurveSegment(s, curve.getImpl().getIndex()); return segment - ? segment->getImpl().getPosInfo(s).KP.p() + ? segment->getImpl().calcInitialContactPoint(s) : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( m_TerminationPoint); } @@ -1364,9 +1026,8 @@ void CableSpan::Impl::calcPathErrorVector( size_t lineIx = 0; ptrdiff_t row = -1; - for (int i = 0; i < getNumCurveSegments(); ++i) { - const CurveSegment& segment = getCurveSegment(CurveSegmentIndex(i)); - if (!segment.getImpl().isActive(s)) { + for (const CurveSegment& segment : m_CurveSegments) { + if (!segment.getImpl().getInstanceEntry(s).isActive()) { continue; } @@ -1404,7 +1065,7 @@ void CableSpan::Impl::calcPathErrorJacobian( size_t col = 0; size_t activeIx = 0; for (const CurveSegment& segment : m_CurveSegments) { - if (!segment.getImpl().isActive(s)) { + if (!segment.getImpl().getInstanceEntry(s).isActive()) { continue; } const GeodesicInfo& g = segment.getImpl().getPosInfo(s); @@ -1467,10 +1128,10 @@ Real CableSpan::Impl::calcPathLength( } for (const CurveSegment& segment : m_CurveSegments) { - if (!segment.getImpl().isActive(s)) { + if (!segment.getImpl().getInstanceEntry(s).isActive()) { continue; } - lTot += segment.getImpl().getPosInfo(s).length; + lTot += segment.getImpl().getInstanceEntry(s).length; } return lTot; } @@ -1486,7 +1147,7 @@ void CableSpan::Impl::calcLineSegments( Vec3 lineStart = std::move(p_O); for (const CurveSegment& segment : m_CurveSegments) { - if (!segment.getImpl().isActive(s)) { + if (!segment.getImpl().getInstanceEntry(s).isActive()) { continue; } @@ -1503,7 +1164,7 @@ size_t CableSpan::Impl::countActive(const State& s) const { size_t count = 0; for (const CurveSegment& segment : m_CurveSegments) { - if (segment.getImpl().isActive(s)) { + if (segment.getImpl().getInstanceEntry(s).isActive()) { ++count; } } @@ -1526,6 +1187,14 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; ++posInfo.loopIter) { + + 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) { @@ -1535,7 +1204,8 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Grab the shared data cache for computing the matrices, and lock it. SolverData& data = - getSubsystem().getImpl().updCachedScratchboard(s).updOrInsert(nActive); + getSubsystem().getImpl().updCachedScratchboard(s).updOrInsert( + nActive); // Compute the straight-line segments. calcLineSegments(s, x_O, x_I, data.lineSegments); @@ -1545,11 +1215,11 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const const Real maxPathError = data.pathError.normInf(); if (maxPathError < m_PathErrorBound) { posInfo.l = 0.; - for (const LineSegment& line: data.lineSegments) { + for (const LineSegment& line : data.lineSegments) { posInfo.l += line.l; } - for (const CurveSegment& curve: m_CurveSegments) { - posInfo.l += curve.getImpl().getPosInfo(s).length; + for (const CurveSegment& curve : m_CurveSegments) { + posInfo.l += curve.getImpl().getInstanceEntry(s).length; } return; } @@ -1566,7 +1236,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Apply path corrections. for (const CurveSegment& obstacle : m_CurveSegments) { - if (!obstacle.getImpl().isActive(s)) { + if (!obstacle.getImpl().getInstanceEntry(s).isActive()) { continue; } obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); @@ -1591,7 +1261,7 @@ void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const Vec3 v_GQ = m_OriginBody.findStationVelocityInGround(s, m_OriginPoint); const CurveSegment* lastActive = nullptr; for (const CurveSegment& obstacle : m_CurveSegments) { - if (!obstacle.getImpl().isActive(s)) { + if (!obstacle.getImpl().getInstanceEntry(s).isActive()) { continue; } @@ -1655,7 +1325,6 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( /* .setColor(Purple) */ /* .setLineThickness(3)); */ - Vec3 lastCurvePoint = ppe.xO; for (const CurveSegment& curveSegment : m_CurveSegments) { const CurveSegment::Impl& curve = curveSegment.getImpl(); @@ -1664,7 +1333,7 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( const Transform& X_SD = geo.getTransform(); // TODO getTransform? // Inactive surfaces are dimmed. - if (!curve.isActive(s)) { + if (!curve.getInstanceEntry(s).isActive()) { decorations.push_back(geo.setTransform(X_GS * X_SD) .setColor(Real(0.75) * geo.getColor())); continue; @@ -1672,23 +1341,36 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( decorations.push_back(geo.setTransform(X_GS * X_SD)); - Vec3 prevPoint = findPrevPoint(s, curve.getIndex()); - Vec3 x_P = curve.getPosInfo(s).KP.p(); + { + const Vec3 prevPoint = findPrevPoint(s, curveSegment); + const Vec3 x_P = curve.getPosInfo(s).KP.p(); - /* decorations.push_back(DecorativeLine(prevPoint, x_P) */ - /* .setColor(Purple) */ - /* .setLineThickness(3)); */ - - lastCurvePoint = curve.getPosInfo(s).KQ.p(); + decorations.push_back(DecorativeLine(prevPoint, x_P) + .setColor(Purple) + .setLineThickness(3)); + } Transform K_P = curve.getPosInfo(s).KP; Transform K_Q = curve.getPosInfo(s).KQ; - decorations.push_back(DecorativeLine(K_P.p(), K_P.p() + K_P.R().getAxisUnitVec(TangentAxis)) + decorations.push_back(DecorativeLine( + K_P.p(), + K_P.p() + K_P.R().getAxisUnitVec(NormalAxis)) .setColor(Orange) .setLineThickness(3)); - decorations.push_back(DecorativeLine(K_Q.p(), K_Q.p() + K_Q.R().getAxisUnitVec(TangentAxis)) + decorations.push_back(DecorativeLine( + K_Q.p(), + K_Q.p() + K_Q.R().getAxisUnitVec(NormalAxis)) .setColor(Blue) .setLineThickness(3)); + + { + const Vec3 nextPoint = findNextPoint(s, curveSegment); + const Vec3 x_Q = curve.getPosInfo(s).KQ.p(); + + decorations.push_back(DecorativeLine(nextPoint, x_Q) + .setColor(Green) + .setLineThickness(3)); + } } /* decorations.push_back(DecorativeLine(lastCurvePoint, ppe.xI) */ diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index f6c485212..a46c28a2c 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -25,231 +25,163 @@ class CurveSegment::Impl using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; - //============================================================================== - // ??? - //============================================================================== - // Represents the local surface wrapping problem. - // Caches last computed geodesic as a warmstart. - // Not exposed outside of simbody. - // Not shared amongst different paths/spans or obstacles/CurveSegments. - class LocalGeodesic - { - using FrenetFrame = ContactGeometry::FrenetFrame; - using Variation = ContactGeometry::GeodesicVariation; - using Correction = ContactGeometry::GeodesicCorrection; - - public: - LocalGeodesic() = default; - ~LocalGeodesic() = default; - LocalGeodesic(LocalGeodesic&&) noexcept = default; - LocalGeodesic& operator=(LocalGeodesic&&) noexcept = default; - LocalGeodesic(const LocalGeodesic&) = delete; - LocalGeodesic& operator=(const LocalGeodesic&) = delete; - - LocalGeodesic( - CableSubsystem& subsystem, - ContactGeometry geometry, - Vec3 initPointGuess) : - m_Subsystem(&subsystem), - m_Geometry(geometry), m_InitPointGuess(initPointGuess) - {} - - // Allocate state variables and cache entries. - void realizeTopology(State& state); - void realizePosition(const State& state) const; - - // Some info that can be retrieved from cache. - struct LocalGeodesicInfo - { - FrenetFrame K_P{}; - FrenetFrame K_Q{}; +public: + Impl() = delete; + Impl(const Impl& source) = delete; + Impl& operator=(const Impl& source) = delete; + ~Impl() = default; - Real length = NaN; + // TODO you would expect the constructor to take the index as well here? + Impl( + CableSpan path, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess); - Variation dK_P{}; - Variation dK_Q{}; + struct LocalGeodesicSample + { + LocalGeodesicSample(Real l, FrenetFrame K) : length(l), frame(K) + {} - Status status = Status::Liftoff; - }; + Real length; + FrenetFrame frame; + }; - // Helper struct: Required data for shooting a new geodesic. - struct GeodesicInitialConditions - { - private: - GeodesicInitialConditions() = default; - - public: - static GeodesicInitialConditions CreateCorrected( - const FrenetFrame& KP, - const Variation& dKP, - Real l, - const Correction& c); - // TODO remove this ctor. - static GeodesicInitialConditions CreateFromGroundInSurfaceFrame( - const Transform& X_GS, - Vec3 x_G, - Vec3 t_G, - Real l); - static GeodesicInitialConditions CreateZeroLengthGuess( - Vec3 prev_QS, - Vec3 xGuess_S); - static GeodesicInitialConditions CreateAtTouchdown( - Vec3 prev_QS, - Vec3 next_PS, - Vec3 trackingPointOnLine); - - Vec3 x{NaN, NaN, NaN}; - Vec3 t{NaN, NaN, NaN}; - Real l = NaN; - }; - - bool analyticFormAvailable() const + struct InstanceEntry + { + bool isActive() const { - // TODO - /* return m_Geometry.analyticFormAvailable(); */ - return false; + return status == Status::Ok; } - // This will reevaluate the cached geodesic and status. - // This will be called by the curve segment before updating the - // position level cache variable of the CurveSegment. - // TODO Unfortunate naming convention. - const LocalGeodesicInfo& calcLocalGeodesicInfo( - const State& s, - Vec3 prev_QS, - Vec3 next_PS) const; // TODO weird name - - // 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; - - // Compute the path points of the current geodesic, and write them to - // the buffer. These points are in local surface coordinates. Returns - // the number of points written. - void calcPathPoints(const State& state, std::vector& points) - const; - - // Set the user defined point that controls the initial wrapping path. - void setInitialPointGuess(Vec3 initPointGuess) - { - m_InitPointGuess = initPointGuess; - } + FrenetFrame K_P{}; + FrenetFrame K_Q{}; - // Get the user defined point that controls the initial wrapping path. - Vec3 getInitialPointGuess() const - { - return m_InitPointGuess; - } + Real length = NaN; - Status getStatus(const State& s) const - { - return getCacheEntry(s).status; - } + Variation dK_P{}; + Variation dK_Q{}; - struct LocalGeodesicSample - { - LocalGeodesicSample(Real l, FrenetFrame K) : length(l), frame(K) {} - Real length; - FrenetFrame frame; - }; - - CableSubsystem& updSubsystem() {return *m_Subsystem;} - const CableSubsystem& getSubsystem() const {return *m_Subsystem;} - - private: - // The cache entry: Curve in local surface coordinated. - // This is an auto update discrete cache variable, which makes it - // persist over integration steps. - // TODO or should it be "normal" discrete? - struct CacheEntry : LocalGeodesicInfo - { - Vec3 trackingPointOnLine{NaN, NaN, NaN}; - std::vector samples; - double sHint = NaN; - int count = 0; - }; + std::vector samples; + double sHint = NaN; - const CacheEntry& getCacheEntry(const State& s) const - { - realizePosition(s); - return Value::downcast( - getSubsystem().getDiscreteVarUpdateValue(s, m_CacheIx)); - } + Vec3 trackingPointOnLine{NaN, NaN, NaN}; + Status status = Status::Liftoff; + }; - CacheEntry& updCacheEntry(const State& state) const - { - return Value::updDowncast( - getSubsystem().updDiscreteVarUpdateValue(state, m_CacheIx)); - } + // 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 + { + // Get the previous geodesic. + const InstanceEntry& cache = getInstanceEntry(s); + const Variation& dK_P = getInstanceEntry(s).dK_P; + const FrenetFrame& K_P = getInstanceEntry(s).K_P; - const CacheEntry& getPrevCacheEntry(const State& state) const - { - return Value::downcast( - getSubsystem().getDiscreteVariable(state, m_CacheIx)); - } + // Get corrected initial conditions. + const Vec3 v = dK_P[1] * c; + const Vec3 point = K_P.p() + v; - CacheEntry& updPrevCacheEntry(State& state) const - { - return Value::updDowncast( - getSubsystem().updDiscreteVariable(state, m_CacheIx)); - } + const Vec3 w = dK_P[0] * c; + const UnitVec3 t = K_P.R().getAxisUnitVec(TangentAxis); + const Vec3 tangent = t + cross(w, t); - void calcCacheEntry( - const Vec3& prev_QS, - const Vec3& next_PS, - CacheEntry& cache) const; + // Take the length correction, and add to the current length. + const Real dl = + c[3]; // Length increment is the last correction element. + const Real length = + std::max(cache.length + dl, 0.); // Clamp length to be nonnegative. - void assertSurfaceBounds(const Vec3& prev_QS, const Vec3& next_PS) - const; + // Shoot the new geodesic. + shootNewGeodesic( + point, + tangent, + length, + cache.sHint, + updInstanceEntry(s)); - void calcTouchdownIfNeeded( - const Vec3& prev_QS, - const Vec3& next_PS, - CacheEntry& cache) const; + getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); - void calcLiftoffIfNeeded( - const Vec3& prev_QS, - const Vec3& next_PS, - CacheEntry& cache) const; + invalidatePositionLevelCache(s); + } - void shootNewGeodesic( - const GeodesicInitialConditions& g0, - CacheEntry& cache) const; + // Compute the path points of the current geodesic, and write them to + // the buffer. These points are in local surface coordinates. Returns + // the number of points written. + void calcPathPoints(const State& s, std::vector& points) const + { + const Transform& X_GS = getPosInfo(s).X_GS; + const InstanceEntry& geodesic_S = getInstanceEntry(s); + if (!geodesic_S.isActive()) { + return; + } + for (const LocalGeodesicSample& sample : geodesic_S.samples) { + points.push_back(X_GS.shiftFrameStationToBase(sample.frame.p())); + } + } - //------------------------------------------------------------------------------ - CableSubsystem* m_Subsystem; // TODO just a pointer? + // Set the user defined point that controls the initial wrapping path. + // Point is in surface coordinates. + void setInitialPointGuess(Vec3 initPointGuess) + { + m_InitPointGuess = initPointGuess; + } - ContactGeometry m_Geometry; + // Get the user defined point that controls the initial wrapping path. + Vec3 getInitialPointGuess() const + { + return m_InitPointGuess; + } - Vec3 m_InitPointGuess; + 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)); + } - size_t m_ProjectionMaxIter = 10; - Real m_ProjectionAccuracy = 1e-10; - Real m_IntegratorAccuracy = 1e-6; + InstanceEntry& updInstanceEntry(const State& state) const + { + return Value::updDowncast( + getSubsystem().updDiscreteVarUpdateValue(state, m_InstanceIx)); + } - Real m_TouchdownAccuracy = 1e-3; - size_t m_TouchdownIter = 10; + const InstanceEntry& getPrevInstanceEntry(const State& state) const + { + return Value::downcast( + getSubsystem().getDiscreteVariable(state, m_InstanceIx)); + } - // TODO this must be a function argument such that the caller can - // decide, - size_t m_NumberOfAnalyticPoints = 10; + InstanceEntry& updPrevInstanceEntry(State& state) const + { + return Value::updDowncast( + getSubsystem().updDiscreteVariable(state, m_InstanceIx)); + } - DiscreteVariableIndex m_CacheIx; - }; + void assertSurfaceBounds(const Vec3& prevPointS, const Vec3& nextPointS) + const; -public: - Impl() = delete; - Impl(const Impl& source) = delete; - Impl& operator=(const Impl& source) = delete; - ~Impl() = default; + void calcTouchdownIfNeeded( + const Vec3& prev_QS, + const Vec3& next_PS, + InstanceEntry& cache) const; - // TODO you would expect the constructor to take the index as well here? - Impl( - CableSpan path, - const MobilizedBody& mobod, - const Transform& X_BS, - ContactGeometry geometry, - Vec3 initPointGuess); + void calcLiftoffIfNeeded( + const Vec3& prev_QS, + const Vec3& next_PS, + InstanceEntry& cache) const; + + void shootNewGeodesic( + Vec3 x, + Vec3 t, + Real l, + Real dsHint, + InstanceEntry& cache) const; // Position level cache: Curve in ground frame. struct PosInfo @@ -262,17 +194,91 @@ class CurveSegment::Impl Variation dKP{}; Variation dKQ{}; - Real length = NaN; + SpatialVec unitForce; }; // Allocate state variables and cache entries. - void realizeTopology(State& state); - /* void realizeInstance(const State& state) const; */ - void realizePosition(const State& state) const; + 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 = Status::Liftoff; + cache->upd().trackingPointOnLine = getInitialPointGuess(); + + m_InstanceIx = updSubsystem().allocateAutoUpdateDiscreteVariable( + s, + Stage::Report, + cache, + Stage::Position); + + PosInfo posInfo{}; + m_PosInfoIx = updSubsystem().allocateCacheEntry( + s, + Stage::Position, + Stage::Infinity, + new Value(posInfo)); + } - void realizeCablePosition(const State& state) const; + void realizePosition(const State& s, Vec3 prevPointG, Vec3 nextPointG) const + { + if (getSubsystem().isCacheValueRealized(s, m_PosInfoIx)) { + throw std::runtime_error( + "expected not realized when calling realizePosition"); + } - void invalidatePositionLevelCache(const State& state) const; + if (getInstanceEntry(s).status == Status::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 prevPointS = X_GS.shiftBaseStationToFrame(prevPointG); + const Vec3 nextPointS = 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(prevPointS, nextPointS); + + calcTouchdownIfNeeded(prevPointS, nextPointS, updInstanceEntry(s)); + calcLiftoffIfNeeded(prevPointS, nextPointS, updInstanceEntry(s)); + + getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); + } + + // Transform geodesic in local surface coordinates to ground. + { + const InstanceEntry& ie = getInstanceEntry(s); + PosInfo& ppe = updPosInfo(s); + // Store the the local geodesic in ground frame. + ppe.X_GS = X_GS; + + // Store the the local geodesic in ground frame. + ppe.KP = X_GS.compose(ie.K_P); + ppe.KQ = X_GS.compose(ie.K_Q); + + ppe.dKP[0] = X_GS.R() * ie.dK_P[0]; + ppe.dKP[1] = X_GS.R() * ie.dK_P[1]; + + ppe.dKQ[0] = X_GS.R() * ie.dK_Q[0]; + ppe.dKQ[1] = X_GS.R() * ie.dK_Q[1]; + } + + getSubsystem().markCacheValueRealized(s, m_PosInfoIx); + } + + void realizeCablePosition(const State& s) const; + + void invalidatePositionLevelCache(const State& state) const + { + getSubsystem().markCacheValueNotRealized(state, m_PosInfoIx); + } const CableSpan& getCable() const { @@ -291,28 +297,10 @@ class CurveSegment::Impl const PosInfo& getPosInfo(const State& s) const { - realizePosition(s); return Value::downcast( getSubsystem().getCacheEntry(s, m_PosInfoIx)); } - bool isActive(const State& s) const - { - realizePosition(s); - return m_Geodesic.getStatus(s) == Status::Ok; - } - - Status getStatus(const State& s) const - { - return m_Geodesic.getStatus(s); - } - - void applyGeodesicCorrection( - const State& state, - const ContactGeometry::GeodesicCorrection& c) const; - - void calcPathPoints(const State& state, std::vector& points) const; - /* const MobilizedBody& getMobilizedBody() const {return m_Mobod;} */ void calcContactPointVelocitiesInGround( const State& s, @@ -344,13 +332,34 @@ class CurveSegment::Impl return m_Mobod.getBodyTransform(s).compose(m_Offset); } - SpatialVec calcAppliedWrenchInGround(const State& s, Real tension) const; + SpatialVec calcUnitForceInGround(const State& s) const + { + const PosInfo& posInfo = getPosInfo(s); + + const UnitVec3& t_P = posInfo.KP.R().getAxisUnitVec(TangentAxis); + const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); + const Vec3 F_G = t_Q - t_P; + + const Transform& X_GB = m_Mobod.getBodyTransform(s); + const Vec3 x_GB = X_GB.p(); + const Vec3 r_P = posInfo.KP.p() - x_GB; + const Vec3 r_Q = posInfo.KQ.p() - x_GB; + const Vec3 M_G = r_Q % t_Q - r_P % t_P; + + return {M_G, F_G}; + } - // TODO Force? or wrench? void applyBodyForce( - const State& state, + const State& s, Real tension, - Vector_& bodyForcesInG) const; + Vector_& bodyForcesInG) const + { + if (!getInstanceEntry(s).isActive()) { + return; + } + + m_Mobod.applyBodyForce(s, calcUnitForceInGround(s), bodyForcesInG); + } // TODO allow for user to shoot his own geodesic. /* void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; */ @@ -364,7 +373,32 @@ class CurveSegment::Impl return *m_Subsystem; } - const DecorativeGeometry& getDecoration() const {return m_Decoration;} + const DecorativeGeometry& getDecoration() const + { + return m_Decoration; + } + + Vec3 calcInitialContactPoint(const State& s) const + { + const InstanceEntry& ic = getInstanceEntry(s); + if (!ic.isActive()) { + 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()) { + 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); + } private: PosInfo& updPosInfo(const State& state) const @@ -373,23 +407,35 @@ class CurveSegment::Impl getSubsystem().updCacheEntry(state, m_PosInfoIx)); } - void calcPosInfo(const State& state, PosInfo& posInfo) 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. + CableSpan m_Path; // The path this segment belongs to. + CurveSegmentIndex m_Index; // The index in its path. MobilizedBody m_Mobod; Transform m_Offset; - LocalGeodesic m_Geodesic; - // Decoration TODO should this be here? - DecorativeGeometry m_Decoration; + ContactGeometry m_Geometry; + DecorativeGeometry m_Decoration; // TOPOLOGY CACHE CacheEntryIndex m_PosInfoIx; + DiscreteVariableIndex m_InstanceIx; + + Vec3 m_InitPointGuess{NaN, NaN, NaN}; + + size_t m_ProjectionMaxIter = 10; + Real m_ProjectionAccuracy = 1e-10; + + Real m_IntegratorAccuracy = 1e-6; + + Real m_TouchdownAccuracy = 1e-4; + size_t m_TouchdownIter = 10; + + // TODO this must be a function argument such that the caller can + // decide, + size_t m_NumberOfAnalyticPoints = 10; }; //============================================================================== @@ -472,7 +518,7 @@ class CableSpan::Impl void calcPathPoints(const State& state, std::vector& points) const { points.push_back(getPosInfo(state).xO); - for (const CurveSegment& curve: m_CurveSegments) { + for (const CurveSegment& curve : m_CurveSegments) { curve.getImpl().calcPathPoints(state, points); } points.push_back(getPosInfo(state).xI); @@ -487,9 +533,9 @@ class CableSpan::Impl size_t countActive(const State& s) const; - Vec3 findPrevPoint(const State& state, CurveSegmentIndex ix) const; + Vec3 findPrevPoint(const State& state, const CurveSegment& curve) const; - Vec3 findNextPoint(const State& state, CurveSegmentIndex ix) const; + Vec3 findNextPoint(const State& state, const CurveSegment& curve) const; const CurveSegment* findPrevActiveCurveSegment( const State& s, @@ -559,13 +605,14 @@ class CableSpan::Impl //============================================================================== class CableSubsystem::Impl : public Subsystem::Guts { - public: +public: struct SolverData { - SolverData(int nActive) { + SolverData(int nActive) + { static constexpr int Q = 4; static constexpr int C = 4; - const int n = nActive; + const int n = nActive; lineSegments.resize(n + 1); pathErrorJacobian = Matrix(C * n, Q * n, 0.); @@ -588,10 +635,12 @@ class CableSubsystem::Impl : public Subsystem::Guts struct CacheEntry { - CacheEntry() =default; - SolverData& updOrInsert(int nActive) { + CacheEntry() = default; + SolverData& updOrInsert(int nActive) + { if (nActive <= 0) { - throw std::runtime_error("Cannot produce solver data of zero dimension"); + throw std::runtime_error( + "Cannot produce solver data of zero dimension"); } for (int i = m_Data.size(); i < nActive; ++i) { @@ -668,10 +717,10 @@ class CableSubsystem::Impl : public Subsystem::Guts { CacheEntry cache{}; m_CacheIx = allocateCacheEntry( - state, - Stage::Instance, - Stage::Infinity, - new Value(cache)); + state, + Stage::Instance, + Stage::Infinity, + new Value(cache)); } CacheEntry& updCachedScratchboard(const State& state) const @@ -688,7 +737,10 @@ class CableSubsystem::Impl : public Subsystem::Guts return 0; for (const CableSpan& cable : cables) { - int returnValue = cable.getImpl().calcDecorativeGeometryAndAppend(state, stage, decorations); + int returnValue = cable.getImpl().calcDecorativeGeometryAndAppend( + state, + stage, + decorations); if (returnValue != 0) { return returnValue; } diff --git a/examples/ExampleCableSpan.cpp b/examples/ExampleCableSpan.cpp index c050bd125..5d6f2b560 100644 --- a/examples/ExampleCableSpan.cpp +++ b/examples/ExampleCableSpan.cpp @@ -188,10 +188,10 @@ class ShowStuff : public PeriodicEventReporter ShowStuff( const MultibodySystem& mbs, const MyCableSpring& cable1, - const MyCableSpring& cable2, + /* const MyCableSpring& cable2, */ Real interval) : PeriodicEventReporter(interval), - mbs(mbs), cable1(cable1), cable2(cable2) + mbs(mbs), cable1(cable1) {} static void showHeading(std::ostream& o) @@ -214,7 +214,7 @@ class ShowStuff : public PeriodicEventReporter void handleEvent(const State& s) const override { const CableSpan& path1 = cable1.getCable(); - const CableSpan& path2 = cable2.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(), @@ -224,25 +224,25 @@ class ShowStuff : public PeriodicEventReporter 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)); + /* 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; + MyCableSpring cable1; }; int main() @@ -330,13 +330,13 @@ int main() 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); + /* 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)); @@ -345,7 +345,7 @@ int main() viz.setShowFrameNumber(true); system.addEventReporter(new Visualizer::Reporter(viz, 0.1 * 1. / 30)); system.addEventReporter( - new ShowStuff(system, cable1, cable2, 0.1 * 0.1)); + new ShowStuff(system, cable1, 0.1 * 0.1)); // Initialize the system and s. system.realizeTopology(); @@ -359,7 +359,7 @@ int main() 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 << "path2 init length=" << path2.getLength(s) << endl; */ cout << "Hit ENTER ..."; getchar(); @@ -375,7 +375,7 @@ int main() ts.initialize(s); ShowStuff::showHeading(cout); - const Real finalTime = 2; + const Real finalTime = 1.; const double startTime = realTime(); ts.stepTo(finalTime); cout << "DONE with " << finalTime << "s simulated in " From 0c4311958535b489cf68dab6b0dad5dd31054044 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 07:32:11 +0200 Subject: [PATCH 072/127] clear samples before intergration --- Simbody/src/Wrapping.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 82910e9c3..29702e344 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -708,6 +708,7 @@ void CurveSegment::Impl::shootNewGeodesic( Real dsHint, InstanceEntry& cache) const { + cache.samples.clear(); calcGeodesicAndVariationImplicitly( m_Geometry, x, From 257d55773001804189ab3c368944e0a4fdeaa822 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 07:32:36 +0200 Subject: [PATCH 073/127] add decorative geometry method to curve segment --- Simbody/src/Wrapping.cpp | 2 ++ Simbody/src/WrappingImpl.h | 16 ++++++++++++++++ 2 files changed, 18 insertions(+) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 29702e344..a55d3727e 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1372,6 +1372,8 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( .setColor(Green) .setLineThickness(3)); } + + curveSegment.getImpl().calcDecorativeGeometryAndAppend(s, decorations); } /* decorations.push_back(DecorativeLine(lastCurvePoint, ppe.xI) */ diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index a46c28a2c..47577047c 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -400,6 +400,22 @@ class CurveSegment::Impl return calcSurfaceFrameInGround(s).shiftFrameStationToBase(x_QS); } + void calcDecorativeGeometryAndAppend( + const State& s, + Array_& decorations) const + { + const InstanceEntry& cache = getInstanceEntry(s); + const Transform& X_GS = calcSurfaceFrameInGround(s); + Vec3 a = X_GS.shiftFrameStationToBase(cache.K_P.p()); + for (size_t i = 1; i < cache.samples.size(); ++i) { + const Vec3 b = X_GS.shiftFrameStationToBase(cache.samples.at(i).frame.p()); + decorations.push_back(DecorativeLine(a, b) + .setColor(Purple) + .setLineThickness(3)); + a = b; + } + } + private: PosInfo& updPosInfo(const State& state) const { From 4e0b5b711233f5b4c2008e1ebeb4869f8af281a0 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 07:42:34 +0200 Subject: [PATCH 074/127] add simple cable span example --- examples/ExampleCableSpanSimple.cpp | 309 ++++++++++++++++++++++++++++ 1 file changed, 309 insertions(+) create mode 100644 examples/ExampleCableSpanSimple.cpp diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp new file mode 100644 index 000000000..601ba1cbc --- /dev/null +++ b/examples/ExampleCableSpanSimple.cpp @@ -0,0 +1,309 @@ +#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), 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); + + Body::Rigid ballBody(MassProperties(1.0, Vec3(0), Inertia(1))); + const Real Rad = 1.; + + MobilizedBody Ground = matter.Ground(); + + 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(Rotation(angle0, axis0), Vec3{0.}), + ballBody, + Transform(Rotation(angle1, axis1), Vec3{0.})); + + CableSpan path1( + cables, + Ground, + Vec3(2., 0.1, 0.), // origin + Ground, + Vec3(-2, 0.0, 0.1)); // termination + + // obs4 + path1.adoptWrappingObstacle( + ball, + Transform(), + ContactGeometry::Sphere(Rad), + {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(); + + system.realize(s, Stage::Position); + viz.report(s); + const Real l = path1.getLength(s); + cout << "path1 init length=" << l << endl; + + } catch (const std::exception& e) { + cout << "EXCEPTION: " << e.what() << "\n"; + } +} From 7ccb63a32145545b5979a971bf3b693b6553888b Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 07:50:26 +0200 Subject: [PATCH 075/127] tweak example settings --- examples/ExampleCableSpanSimple.cpp | 46 +++++++++++++++++++++-------- 1 file changed, 33 insertions(+), 13 deletions(-) diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp index 601ba1cbc..a32f4a898 100644 --- a/examples/ExampleCableSpanSimple.cpp +++ b/examples/ExampleCableSpanSimple.cpp @@ -264,22 +264,25 @@ int main() MobilizedBody Ground = matter.Ground(); - UnitVec3 axis0(Vec3{1., 1., 1.}); - Real angle0 = 0.5; - UnitVec3 axis1(Vec3{1., -2., 3.}); - Real angle1 = 0.22; + Vec3 offset = {0., 0., 0.}; + Vec3 arm = {0.5, 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(Rotation(angle0, axis0), Vec3{0.}), + Transform(Vec3{offset + arm}), ballBody, - Transform(Rotation(angle1, axis1), Vec3{0.})); + Transform(Vec3{offset - arm})); CableSpan path1( cables, Ground, - Vec3(2., 0.1, 0.), // origin + Vec3(-2., 0.1, 0.) + offset, // origin Ground, - Vec3(-2, 0.0, 0.1)); // termination + Vec3(2., 0.0, 0.1) + offset); // termination // obs4 path1.adoptWrappingObstacle( @@ -298,11 +301,28 @@ int main() system.realizeTopology(); State s = system.getDefaultState(); - system.realize(s, Stage::Position); - viz.report(s); - const Real l = path1.getLength(s); - cout << "path1 init length=" << l << endl; - + Real v = 0.; + Random::Gaussian random; + while(true) { + system.realize(s, Stage::Position); + viz.report(s); + const Real l = path1.getLength(s); + cout << "path1 init length=" << l << endl; + + v += random.getValue() * 1e-2; + v = std::max(-1e-1, v); + v = std::min(1e-1, v); + + for (int i = 0; i < s.getNQ(); ++i) + s.updQ()[i] += random.getValue() * 5e-1 + v; + for (int i = 0; i < s.getNU(); ++i) + s.updU()[i] += random.getValue() * 1e-3; + /* cout << "Hit ENTER ..., or q\n"; */ + /* const char ch = getchar(); */ + /* if (ch == 'Q' || ch == 'q') */ + /* break; */ + sleepInSec(0.1); + } } catch (const std::exception& e) { cout << "EXCEPTION: " << e.what() << "\n"; } From bf2cffb7ebe07bf60af1139013981fcdce0bb55e Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 09:46:17 +0200 Subject: [PATCH 076/127] fix calculating pathErrorJacobian indexing --- Simbody/src/Wrapping.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index a55d3727e..55dff9bdd 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -595,7 +595,7 @@ const Correction* calcPathCorrections(SolverData& data) data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; for (int i = 0; i < data.mat.nrow(); ++i) { - data.mat[i][i] += w + 1.; + data.mat[i][i] += w + 1e-3; } data.matInv = data.mat; data.vec = data.pathErrorJacobian.transpose() * (data.pathError * (-1.)); @@ -968,7 +968,7 @@ const CurveSegment* CableSpan::Impl::findPrevActiveCurveSegment( const State& s, CurveSegmentIndex ix) const { - for (int i = ix - 1; i > 0; --i) { + for (int i = ix - 1; i >= 0; --i) { // Find the active segment before the current. if (m_CurveSegments.at(CurveSegmentIndex(i)) .getImpl() @@ -1072,7 +1072,7 @@ void CableSpan::Impl::calcPathErrorJacobian( const GeodesicInfo& g = segment.getImpl().getPosInfo(s); const LineSegment& l_P = lines.at(activeIx); - const LineSegment& l_Q = lines.at(activeIx + 1); + const LineSegment& l_Q = lines.at(++activeIx); const CurveSegmentIndex ix = segment.getImpl().getIndex(); const CurveSegment* prev = findPrevActiveCurveSegment(s, ix); @@ -1081,7 +1081,7 @@ void CableSpan::Impl::calcPathErrorJacobian( int blkCol = col; std::function AddBlock = [&](const Vec4& block) { for (int ix = 0; ix < 4; ++ix) { - J[row][blkCol + ix] = block[ix]; + J.row(row).col(blkCol + ix) = block[ix]; } }; @@ -1089,6 +1089,7 @@ void CableSpan::Impl::calcPathErrorJacobian( const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); const Variation& dK_P = g.dKP; + blkCol = col; addPathErrorJacobian(l_P, a_P, dK_P, AddBlock); if (prev) { From 49b0e75458aa540a8199bd815d15fec009ca0cd5 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 10:23:19 +0200 Subject: [PATCH 077/127] HOTFIX: prevent exception on non-convergence --- Simbody/src/Wrapping.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 55dff9bdd..06e117fdc 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1215,7 +1215,8 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Evaluate path error, and stop when converged. calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); const Real maxPathError = data.pathError.normInf(); - if (maxPathError < m_PathErrorBound) { + std::cout << " max = " << maxPathError << "\n"; + if (maxPathError < m_PathErrorBound || (posInfo.loopIter + 1) >= m_PathMaxIter) { posInfo.l = 0.; for (const LineSegment& line : data.lineSegments) { posInfo.l += line.l; From 41ed0b2a78761e664fc1c30533c4bcb572cd6b62 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 10:23:58 +0200 Subject: [PATCH 078/127] add decoration of frenet frame --- Simbody/src/Wrapping.cpp | 37 +++++++++++++++++++++++-------------- 1 file changed, 23 insertions(+), 14 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 06e117fdc..bbb671d7a 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1349,30 +1349,39 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( const Vec3 x_P = curve.getPosInfo(s).KP.p(); decorations.push_back(DecorativeLine(prevPoint, x_P) - .setColor(Purple) - .setLineThickness(3)); + .setColor(Orange) + .setLineThickness(2)); } Transform K_P = curve.getPosInfo(s).KP; Transform K_Q = curve.getPosInfo(s).KQ; - decorations.push_back(DecorativeLine( - K_P.p(), - K_P.p() + K_P.R().getAxisUnitVec(NormalAxis)) - .setColor(Orange) - .setLineThickness(3)); - decorations.push_back(DecorativeLine( - K_Q.p(), - K_Q.p() + K_Q.R().getAxisUnitVec(NormalAxis)) - .setColor(Blue) - .setLineThickness(3)); + const std::array axes = { + TangentAxis, + NormalAxis, + BinormalAxis}; + const std::array colors = { + Red, Green, Blue}; + + for (size_t i = 0; i < 3; ++i) { + decorations.push_back(DecorativeLine( + K_P.p(), + K_P.p() + 0.5 * K_P.R().getAxisUnitVec(axes.at(i))) + .setColor(colors.at(i)) + .setLineThickness(4)); + decorations.push_back(DecorativeLine( + K_Q.p(), + K_Q.p() + 0.5 * K_Q.R().getAxisUnitVec(axes.at(i))) + .setColor(colors.at(i)) + .setLineThickness(4)); + } { const Vec3 nextPoint = findNextPoint(s, curveSegment); const Vec3 x_Q = curve.getPosInfo(s).KQ.p(); decorations.push_back(DecorativeLine(nextPoint, x_Q) - .setColor(Green) - .setLineThickness(3)); + .setColor(Gray) + .setLineThickness(2)); } curveSegment.getImpl().calcDecorativeGeometryAndAppend(s, decorations); From 5f8e2f5a33dcb3d50747b94b49d13a048cbe8e23 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 10:24:30 +0200 Subject: [PATCH 079/127] tweak example settings and script --- examples/ExampleCableSpan.cpp | 4 +- examples/ExampleCableSpanSimple.cpp | 70 ++++++++++++++++++++--------- install-simbody | 12 ++++- 3 files changed, 63 insertions(+), 23 deletions(-) diff --git a/examples/ExampleCableSpan.cpp b/examples/ExampleCableSpan.cpp index 5d6f2b560..f01a80bb3 100644 --- a/examples/ExampleCableSpan.cpp +++ b/examples/ExampleCableSpan.cpp @@ -308,9 +308,9 @@ int main() CableSpan path1( cables, body1, - Vec3(Rad, 0, 0), // origin + Vec3(-2*Rad, 0, 0), // origin body5, - Vec3(0, 0, Rad)); // termination + Vec3(2*Rad, 0, 0)); // termination /* CableObstacle::ViaPoint p1(path1, body2, Rad * UnitVec3(1, 1, 0)); */ diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp index a32f4a898..3241e4fa7 100644 --- a/examples/ExampleCableSpanSimple.cpp +++ b/examples/ExampleCableSpanSimple.cpp @@ -259,13 +259,20 @@ int main() Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0)); // Force::GlobalDamper(forces, matter, 5); - Body::Rigid ballBody(MassProperties(1.0, Vec3(0), Inertia(1))); - const Real Rad = 1.; - 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.1; + Vec3 offset = {0., 0., 0.}; - Vec3 arm = {0.5, 0., 0.}; + Vec3 arm = {0.25, 0., 0.}; /* UnitVec3 axis0(Vec3{1., 1., 1.}); */ /* Real angle0 = 0.5; */ @@ -277,13 +284,6 @@ int main() ballBody, Transform(Vec3{offset - arm})); - CableSpan path1( - cables, - Ground, - Vec3(-2., 0.1, 0.) + offset, // origin - Ground, - Vec3(2., 0.0, 0.1) + offset); // termination - // obs4 path1.adoptWrappingObstacle( ball, @@ -291,6 +291,24 @@ int main() ContactGeometry::Sphere(Rad), {0., 1., 0.}); + Body::Rigid ball2Body(MassProperties(1.0, Vec3(0), Inertia(1))); + const Real Rad2 = 1.1; + Vec3 offset2 = {4., 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::Sphere(Rad2), + {0., 1., 0.}); + MyCableSpring cable1(forces, path1, 100., 3.5, 0.1); Visualizer viz(system); @@ -302,26 +320,38 @@ int main() State s = system.getDefaultState(); Real v = 0.; - Random::Gaussian random; + bool continuous = false; + Random::Gaussian random; while(true) { system.realize(s, Stage::Position); viz.report(s); const Real l = path1.getLength(s); cout << "path1 init length=" << l << endl; + { + /* Random::Gaussian random; */ v += random.getValue() * 1e-2; v = std::max(-1e-1, v); v = std::min(1e-1, v); - - for (int i = 0; i < s.getNQ(); ++i) - s.updQ()[i] += random.getValue() * 5e-1 + v; - for (int i = 0; i < s.getNU(); ++i) + } + + for (int i = 0; i < s.getNQ(); ++i) { + /* Random::Gaussian random; */ + s.updQ()[i] += random.getValue() * 1e-1 + v; + } + for (int i = 0; i < s.getNU(); ++i) { + /* Random::Gaussian random; */ s.updU()[i] += random.getValue() * 1e-3; - /* cout << "Hit ENTER ..., or q\n"; */ - /* const char ch = getchar(); */ - /* if (ch == 'Q' || ch == 'q') */ - /* break; */ + } + 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 index 3d2e46b56..3106f97c0 100755 --- a/install-simbody +++ b/install-simbody @@ -4,7 +4,13 @@ set -xeuo pipefail WORKSPACE=$(dirname $(dirname $(realpath "$0"))) echo $WORKSPACE -# env -C $WORKSPACE cmake -B "simbody-build" -S "simbody" -DCMAKE_INSTALL_PREFIX="simbody-install" -DCMAKE_EXPORT_COMPILE_COMMANDS="ON" +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" @@ -12,4 +18,8 @@ ln -sf "$WORKSPACE/simbody-build/compile_commands.json" "$WORKSPACE/simbody/comp # 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 From 3a6c71ffa0d62f18f5957cae0c7da876a6114216 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 19:01:43 +0200 Subject: [PATCH 080/127] Fix nasty bugs in computing pathErrorJacobian --- Simbody/src/Wrapping.cpp | 33 +++++++++++++++------------------ 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index bbb671d7a..fe70bc0a2 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -685,17 +685,12 @@ void CurveSegment::Impl::assertSurfaceBounds( const Vec3& prev_QS, const Vec3& next_PS) const { - /* std::cout << "LocalGeodesic::assertSurfaceBounds\n"; */ // Make sure that the previous point does not lie inside the surface. if (calcSurfaceConstraintValue(m_Geometry, prev_QS) < 0.) { - std::cout << "prev_QS = " << prev_QS << "\n"; - std::cout << "prev_PS = " << next_PS << "\n"; throw std::runtime_error("Unable to wrap over surface: Preceding point " "lies inside the surface"); } if (calcSurfaceConstraintValue(m_Geometry, next_PS) < 0.) { - std::cout << "prev_QS = " << prev_QS << "\n"; - std::cout << "prev_PS = " << next_PS << "\n"; throw std::runtime_error( "Unable to wrap over surface: Next point lies inside the surface"); } @@ -801,12 +796,12 @@ void addDirectionJacobian( const LineSegment& e, const UnitVec3& axis, const PointVariation& dx, - std::function AddBlock, + std::function& AddBlock, bool invert = false) { Vec3 y = axis - e.d * dot(e.d, axis); - y /= e.l * (invert ? 1. : -1); - AddBlock(~dx * y); + y /= e.l * (invert ? -1. : 1); + AddBlock((~dx) * y); } Real calcPathError(const LineSegment& e, const Rotation& R, CoordinateAxis axis) @@ -818,11 +813,11 @@ void addPathErrorJacobian( const LineSegment& e, const UnitVec3& axis, const Variation& dK, - std::function AddBlock, + std::function& AddBlock, bool invertV = false) { addDirectionJacobian(e, axis, dK[1], AddBlock, invertV); - AddBlock(~dK[0] * cross(axis, e.d)); + AddBlock((~dK[0]) * cross(axis, e.d)); } } // namespace @@ -1026,6 +1021,7 @@ void CableSpan::Impl::calcPathErrorVector( { size_t lineIx = 0; ptrdiff_t row = -1; + pathError *= 0; for (const CurveSegment& segment : m_CurveSegments) { if (!segment.getImpl().getInstanceEntry(s).isActive()) { @@ -1054,6 +1050,7 @@ void CableSpan::Impl::calcPathErrorJacobian( // TODO perhaps just not make method static. const size_t n = lines.size() - 1; + J *= 0.; SimTK_ASSERT( J.rows() == n * N, @@ -1081,16 +1078,16 @@ void CableSpan::Impl::calcPathErrorJacobian( int blkCol = col; std::function AddBlock = [&](const Vec4& block) { for (int ix = 0; ix < 4; ++ix) { - J.row(row).col(blkCol + ix) = block[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); - const Variation& dK_P = g.dKP; + const UnitVec3 a_P = g.KP.R().getAxisUnitVec(axis); - blkCol = col; - addPathErrorJacobian(l_P, a_P, dK_P, AddBlock); + blkCol = col; + addPathErrorJacobian(l_P, a_P, dK_P, AddBlock, false); if (prev) { const Variation& prev_dK_Q = prev->getImpl().getPosInfo(s).dKQ; @@ -1100,9 +1097,9 @@ void CableSpan::Impl::calcPathErrorJacobian( ++row; } + const Variation& dK_Q = g.dKQ; for (CoordinateAxis axis : axes) { - const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); - const Variation& dK_Q = g.dKQ; + const UnitVec3 a_Q = g.KQ.R().getAxisUnitVec(axis); blkCol = col; addPathErrorJacobian(l_Q, a_Q, dK_Q, AddBlock, true); @@ -1110,7 +1107,7 @@ void CableSpan::Impl::calcPathErrorJacobian( 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); + addDirectionJacobian(l_Q, a_Q, next_dK_P[1], AddBlock, false); } ++row; } From b3b2758a46b4661748c84006a8bc6977dea26879 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 19:02:39 +0200 Subject: [PATCH 081/127] add runtime-unittest for boundary frame jacobian --- Simbody/include/simbody/internal/Wrapping.h | 45 +++++-- Simbody/src/Wrapping.cpp | 129 +++++++++++++++++--- Simbody/src/WrappingImpl.h | 102 ++++++++++++++-- examples/ExampleCableSpanSimple.cpp | 107 ++++++++++++---- 4 files changed, 328 insertions(+), 55 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index d5205b420..ab167be72 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -55,10 +55,12 @@ class SimTK_SIMBODY_EXPORT CurveSegment const CableSpan& getCable() const; Real getSegmentLength(const State& s) const; - const Transform& getFrenetFrameStart(const State& s) const { + const Transform& getFrenetFrameStart(const State& s) const + { throw std::runtime_error("NOTYETIMPLEMENTED"); } - const Transform& getFrenetFrameEnd(const State& s) const { + const Transform& getFrenetFrameEnd(const State& s) const + { throw std::runtime_error("NOTYETIMPLEMENTED"); } @@ -67,22 +69,26 @@ class SimTK_SIMBODY_EXPORT CurveSegment const ContactGeometry& getContactGeometry() const; const Transform& getSurfaceToGroundTransform(const State& s) const; - Real calcNormalCurvature(Vec3 point, Vec3 tangent) const { + Real calcNormalCurvature(Vec3 point, Vec3 tangent) const + { throw std::runtime_error("NOTYETIMPLEMENTED"); return NaN; } - Real calcGeodesicTorsion(Vec3 point, Vec3 tangent) const { + Real calcGeodesicTorsion(Vec3 point, Vec3 tangent) const + { throw std::runtime_error("NOTYETIMPLEMENTED"); return NaN; } - UnitVec3 calcSurfaceNormal(Vec3 point) const { + UnitVec3 calcSurfaceNormal(Vec3 point) const + { throw std::runtime_error("NOTYETIMPLEMENTED"); return {NaN, NaN, NaN}; } - Real calcSurfaceValue(Vec3 point) const { + Real calcSurfaceValue(Vec3 point) const + { throw std::runtime_error("NOTYETIMPLEMENTED"); return NaN; } @@ -102,8 +108,14 @@ class SimTK_SIMBODY_EXPORT CurveSegment private: friend CableSpan; - const Impl& getImpl() const {return *m_Impl;} - Impl& updImpl() {return *m_Impl;} + const Impl& getImpl() const + { + return *m_Impl; + } + Impl& updImpl() + { + return *m_Impl; + } std::shared_ptr m_Impl = nullptr; }; @@ -118,7 +130,8 @@ class SimTK_SIMBODY_EXPORT CableSpan { LineSegment() = default; - LineSegment(Vec3 a, Vec3 b): l((b-a).norm()), d((b-a)/l) {} + LineSegment(Vec3 a, Vec3 b) : l((b - a).norm()), d((b - a) / l) + {} Real l = NaN; UnitVec3 d{NaN, NaN, NaN}; @@ -146,13 +159,25 @@ class SimTK_SIMBODY_EXPORT CableSpan Real getLength(const State& state) const; Real getLengthDot(const State& state) const; - Real calcCablePower(const State& state, Real tension) const {return NaN;} + Real calcCablePower(const State& state, Real tension) const + { + return NaN; + } void applyBodyForces( const State& state, Real tension, Vector_& bodyForcesInG) const; + void calcPathErrorJacobian( + const State& state, + Vector& pathError, + Matrix& pathErrorJacobian) const; + + void applyCorrection(const State& state, const Vector& correction) const; + + size_t countActive(const State& s) const; + /* int calcPathPoints(const State& state, std::vector& points); */ /* int calcPathFrenetFrames( */ /* const State& state, */ diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index fe70bc0a2..985a2e8b2 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1159,6 +1159,11 @@ void CableSpan::Impl::calcLineSegments( lines.emplace_back(lineStart, p_I); } +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; @@ -1170,6 +1175,99 @@ size_t CableSpan::Impl::countActive(const State& s) const 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()) { + 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().invalidatePositionLevelCache(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, and lock it. + SolverData& data = + getSubsystem().getImpl().updCachedScratchboard(s).updOrInsert(nActive); + + // Compute the straight-line segments. + const Vec3 x_O = + m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + const Vec3 x_I = + m_TerminationBody.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::calcPosInfo(const State& s, PosInfo& posInfo) const { // Path origin and termination point. @@ -1235,17 +1333,17 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const const Correction* corrIt = calcPathCorrections(data); // Apply path corrections. - for (const CurveSegment& obstacle : m_CurveSegments) { - if (!obstacle.getImpl().getInstanceEntry(s).isActive()) { + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.getImpl().getInstanceEntry(s).isActive()) { continue; } - obstacle.getImpl().applyGeodesicCorrection(s, *corrIt); + curve.getImpl().applyGeodesicCorrection(s, *corrIt); ++corrIt; } // Path has changed: invalidate each segment's cache. - for (const CurveSegment& obstacle : m_CurveSegments) { - obstacle.getImpl().invalidatePositionLevelCache(s); + for (const CurveSegment& curve : m_CurveSegments) { + curve.getImpl().invalidatePositionLevelCache(s); } } @@ -1350,24 +1448,25 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( .setLineThickness(2)); } - Transform K_P = curve.getPosInfo(s).KP; - Transform K_Q = curve.getPosInfo(s).KQ; + Transform K_P = curve.getPosInfo(s).KP; + Transform K_Q = curve.getPosInfo(s).KQ; const std::array axes = { TangentAxis, NormalAxis, BinormalAxis}; - const std::array colors = { - Red, Green, Blue}; + const std::array colors = {Red, Green, Blue}; for (size_t i = 0; i < 3; ++i) { - decorations.push_back(DecorativeLine( - K_P.p(), - K_P.p() + 0.5 * K_P.R().getAxisUnitVec(axes.at(i))) + decorations.push_back( + DecorativeLine( + K_P.p(), + K_P.p() + 0.5 * K_P.R().getAxisUnitVec(axes.at(i))) .setColor(colors.at(i)) .setLineThickness(4)); - decorations.push_back(DecorativeLine( - K_Q.p(), - K_Q.p() + 0.5 * K_Q.R().getAxisUnitVec(axes.at(i))) + decorations.push_back( + DecorativeLine( + K_Q.p(), + K_Q.p() + 0.5 * K_Q.R().getAxisUnitVec(axes.at(i))) .setColor(colors.at(i)) .setLineThickness(4)); } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 47577047c..190202929 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -76,8 +76,14 @@ class CurveSegment::Impl { // Get the previous geodesic. const InstanceEntry& cache = getInstanceEntry(s); - const Variation& dK_P = getInstanceEntry(s).dK_P; - const FrenetFrame& K_P = getInstanceEntry(s).K_P; + const Variation& dK_P = cache.dK_P; + const FrenetFrame& K_P = cache.K_P; + + // TODO Frames and Variations stored below should be part of a unti test... + const FrenetFrame K0_P = cache.K_P; + const FrenetFrame K0_Q = cache.K_Q; + const Variation dK0_P = cache.dK_P; + const Variation dK0_Q = cache.dK_Q; // Get corrected initial conditions. const Vec3 v = dK_P[1] * c; @@ -103,6 +109,77 @@ class CurveSegment::Impl getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); + // TODO Below code should be moved to a unit test... + const bool DO_UNIT_TEST_HERE = true; + if (DO_UNIT_TEST_HERE) + { + const Real delta = c.norm(); + const Real eps = delta / 10.; + 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 expected_diff = cross(w, a0); + const Vec3 got_diff = a1 - a0; + const bool isOk = (expected_diff - got_diff).norm() < eps; + if (!isOk) { + std::cout << " a0 = " << R0.transpose() * a0 << "\n"; + std::cout << " a1 = " << R0.transpose() * a1 << "\n"; + std::cout << " expected diff = " + << R0.transpose() * expected_diff / delta << "\n"; + std::cout << " got dt_Q = " + << R0.transpose() * got_diff / delta << "\n"; + std::cout << " err = " + << (expected_diff - got_diff).norm() / delta << "\n"; + } + return isOk; + }; + + auto AssertFrame = [&](const Transform& K0, + const Transform& K1, + const Variation& dK0) -> bool { + const Vec3 dx_got = K1.p() - K0.p(); + const Vec3 dx_expected = dK0[1] * c; + bool isOk = (dx_expected - dx_got).norm() < eps; + if (!isOk) { + std::cout << "Apply variation c = " << c << "\n"; + std::cout << " x0 = " << K0.p() << "\n"; + std::cout << " x1 = " << K1.p() << "\n"; + std::cout << " expected dx_Q = " + << K0.R().transpose() * dx_expected / delta << "\n"; + std::cout << " got dx_Q = " + << K0.R().transpose() * dx_got / delta << "\n"; + std::cout << " err = " + << (dx_expected - dx_got).norm() / delta << "\n"; + std::cout << "WARNING: Large deviation in final position\n"; + } + const Vec3 w = dK0[0] * c; + std::array axes = { + TangentAxis, + NormalAxis, + BinormalAxis}; + for (size_t i = 0; i < 3; ++i) { + isOk &= AssertAxis(K0.R(), K1.R(), w, axes.at(i)); + if (!isOk) { + std::cout << "FAILED axis " << i << "\n"; + } + } + + return isOk; + }; + + if (delta > 1e-10) { + if (!AssertFrame(K0_P, getInstanceEntry(s).K_P, dK0_P)) { + throw std::runtime_error("Start frame variation check failed"); + } + if (!AssertFrame(K0_Q, getInstanceEntry(s).K_Q, dK0_Q)) { + throw std::runtime_error("End frame variation check failed"); + } + } + } + invalidatePositionLevelCache(s); } @@ -405,13 +482,13 @@ class CurveSegment::Impl Array_& decorations) const { const InstanceEntry& cache = getInstanceEntry(s); - const Transform& X_GS = calcSurfaceFrameInGround(s); + const Transform& X_GS = calcSurfaceFrameInGround(s); Vec3 a = X_GS.shiftFrameStationToBase(cache.K_P.p()); for (size_t i = 1; i < cache.samples.size(); ++i) { - const Vec3 b = X_GS.shiftFrameStationToBase(cache.samples.at(i).frame.p()); - decorations.push_back(DecorativeLine(a, b) - .setColor(Purple) - .setLineThickness(3)); + const Vec3 b = + X_GS.shiftFrameStationToBase(cache.samples.at(i).frame.p()); + decorations.push_back( + DecorativeLine(a, b).setColor(Purple).setLineThickness(3)); a = b; } } @@ -540,6 +617,15 @@ class CableSpan::Impl points.push_back(getPosInfo(state).xI); } + 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; @@ -547,8 +633,6 @@ class CableSpan::Impl void calcPosInfo(const State& s, PosInfo& posInfo) const; void calcVelInfo(const State& s, VelInfo& velInfo) const; - size_t countActive(const State& s) const; - Vec3 findPrevPoint(const State& state, const CurveSegment& curve) const; Vec3 findNextPoint(const State& state, const CurveSegment& curve) const; diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp index 3241e4fa7..5d2b3daf2 100644 --- a/examples/ExampleCableSpanSimple.cpp +++ b/examples/ExampleCableSpanSimple.cpp @@ -7,6 +7,56 @@ 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 << " d = " << d << "\n"; + os << " q1 = " << q1 << "\n"; + os << " J = " << J0 << "\n"; + os << " y0 = " << y0 << "\n"; + os << " y1 = " << y1 << "\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 @@ -269,10 +319,10 @@ int main() Vec3(10., 0.0, 0.1)); Body::Rigid ballBody(MassProperties(1.0, Vec3(0), Inertia(1))); - const Real Rad = 1.1; + const Real Rad = 2.0; Vec3 offset = {0., 0., 0.}; - Vec3 arm = {0.25, 0., 0.}; + Vec3 arm = {0.25, 0., 0.}; /* UnitVec3 axis0(Vec3{1., 1., 1.}); */ /* Real angle0 = 0.5; */ @@ -293,21 +343,21 @@ int main() Body::Rigid ball2Body(MassProperties(1.0, Vec3(0), Inertia(1))); const Real Rad2 = 1.1; - Vec3 offset2 = {4., 0., 0.}; - Vec3 arm2 = {0.25, 0., 0.}; + Vec3 offset2 = {5., 0., 0.}; + Vec3 arm2 = {0.25, 0., 0.}; MobilizedBody::Ball ball2( Ground, Transform(Vec3{offset2 + arm2}), ball2Body, - Transform(Vec3{- arm2})); + Transform(Vec3{-arm2})); // obs4 - path1.adoptWrappingObstacle( - ball2, - Transform(), - ContactGeometry::Sphere(Rad2), - {0., 1., 0.}); + /* path1.adoptWrappingObstacle( */ + /* ball2, */ + /* Transform(), */ + /* ContactGeometry::Sphere(Rad2), */ + /* {0., 1., 0.}); */ MyCableSpring cable1(forces, path1, 100., 3.5, 0.1); @@ -319,32 +369,47 @@ int main() system.realizeTopology(); State s = system.getDefaultState(); - Real v = 0.; + Real v = 0.; bool continuous = false; - Random::Gaussian random; - while(true) { + Random::Gaussian random; + while (true) { system.realize(s, Stage::Position); - viz.report(s); + /* 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"; + + return 0; + { - /* Random::Gaussian random; */ - v += random.getValue() * 1e-2; - v = std::max(-1e-1, v); - v = std::min(1e-1, v); + /* Random::Gaussian random; */ + v += random.getValue() * 1e-2; + v = std::max(-1e-1, v); + v = std::min(1e-1, v); } for (int i = 0; i < s.getNQ(); ++i) { - /* Random::Gaussian random; */ + /* Random::Gaussian random; */ s.updQ()[i] += random.getValue() * 1e-1 + v; } for (int i = 0; i < s.getNU(); ++i) { - /* Random::Gaussian random; */ + /* Random::Gaussian random; */ s.updU()[i] += random.getValue() * 1e-3; } + if (continuous) { - sleepInSec(0.1); + sleepInSec(0.1); } else { cout << "Hit ENTER ..., or q\n"; const char ch = getchar(); From c10f9fed2a123c1c26e041d898a5ef080f35774c Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 2 May 2024 19:08:15 +0200 Subject: [PATCH 082/127] tweak cable test example --- examples/ExampleCableSpanSimple.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp index 5d2b3daf2..c13cec4d6 100644 --- a/examples/ExampleCableSpanSimple.cpp +++ b/examples/ExampleCableSpanSimple.cpp @@ -353,11 +353,11 @@ int main() Transform(Vec3{-arm2})); // obs4 - /* path1.adoptWrappingObstacle( */ - /* ball2, */ - /* Transform(), */ - /* ContactGeometry::Sphere(Rad2), */ - /* {0., 1., 0.}); */ + path1.adoptWrappingObstacle( + ball2, + Transform(), + ContactGeometry::Sphere(Rad2), + {0., 1., 0.}); MyCableSpring cable1(forces, path1, 100., 3.5, 0.1); @@ -374,7 +374,7 @@ int main() Random::Gaussian random; while (true) { system.realize(s, Stage::Position); - /* viz.report(s); */ + viz.report(s); const Real l = path1.getLength(s); cout << "path1 init length=" << l << endl; @@ -390,8 +390,6 @@ int main() testJacobian(f, qDim, 1e-4, 1e-3, oss); std::cout << oss.str() << "\n"; - return 0; - { /* Random::Gaussian random; */ v += random.getValue() * 1e-2; From 5304974edd67e4af2eca7d0c4ef78ec4dde58bc1 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 09:24:00 +0200 Subject: [PATCH 083/127] fmt --- Simbody/src/Wrapping.cpp | 4 ++-- Simbody/src/WrappingImpl.h | 40 ++++++++++++++++++-------------------- 2 files changed, 21 insertions(+), 23 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 985a2e8b2..a3a1fab8c 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -770,8 +770,8 @@ CurveSegment::Impl::Impl( Vec3 initPointGuess) : m_Subsystem(&path.updImpl().updSubsystem()), m_Path(path), m_Index(-1), // TODO what to do with this index, and when - m_Mobod(mobod), m_Offset(X_BS), m_Geometry(geometry), - m_InitPointGuess(initPointGuess), + m_Mobod(mobod), m_X_BS(X_BS), m_Geometry(geometry), + m_ContactPointHintInSurface(initPointGuess), m_Decoration(geometry.createDecorativeGeometry() .setColor(Orange) .setOpacity(.75) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 190202929..60b4b2bda 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -94,8 +94,7 @@ class CurveSegment::Impl 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 correction element. + 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. @@ -179,6 +178,7 @@ class CurveSegment::Impl } } } + // End of unit test code block... invalidatePositionLevelCache(s); } @@ -200,15 +200,15 @@ class CurveSegment::Impl // Set the user defined point that controls the initial wrapping path. // Point is in surface coordinates. - void setInitialPointGuess(Vec3 initPointGuess) + void setContactPointHint(Vec3 contactPointHintInSurface) { - m_InitPointGuess = initPointGuess; + m_ContactPointHintInSurface = contactPointHintInSurface; } // Get the user defined point that controls the initial wrapping path. - Vec3 getInitialPointGuess() const + Vec3 getContactPointHint() const { - return m_InitPointGuess; + return m_ContactPointHintInSurface; } const InstanceEntry& getInstanceEntry(const State& s) const @@ -283,7 +283,7 @@ class CurveSegment::Impl cache->upd().length = 0.; cache->upd().status = Status::Liftoff; - cache->upd().trackingPointOnLine = getInitialPointGuess(); + cache->upd().trackingPointOnLine = getContactPointHint(); m_InstanceIx = updSubsystem().allocateAutoUpdateDiscreteVariable( s, @@ -292,7 +292,7 @@ class CurveSegment::Impl Stage::Position); PosInfo posInfo{}; - m_PosInfoIx = updSubsystem().allocateCacheEntry( + m_PosIx = updSubsystem().allocateCacheEntry( s, Stage::Position, Stage::Infinity, @@ -301,7 +301,7 @@ class CurveSegment::Impl void realizePosition(const State& s, Vec3 prevPointG, Vec3 nextPointG) const { - if (getSubsystem().isCacheValueRealized(s, m_PosInfoIx)) { + if (getSubsystem().isCacheValueRealized(s, m_PosIx)) { throw std::runtime_error( "expected not realized when calling realizePosition"); } @@ -347,14 +347,14 @@ class CurveSegment::Impl ppe.dKQ[1] = X_GS.R() * ie.dK_Q[1]; } - getSubsystem().markCacheValueRealized(s, m_PosInfoIx); + getSubsystem().markCacheValueRealized(s, m_PosIx); } void realizeCablePosition(const State& s) const; void invalidatePositionLevelCache(const State& state) const { - getSubsystem().markCacheValueNotRealized(state, m_PosInfoIx); + getSubsystem().markCacheValueNotRealized(state, m_PosIx); } const CableSpan& getCable() const @@ -375,7 +375,7 @@ class CurveSegment::Impl const PosInfo& getPosInfo(const State& s) const { return Value::downcast( - getSubsystem().getCacheEntry(s, m_PosInfoIx)); + getSubsystem().getCacheEntry(s, m_PosIx)); } /* const MobilizedBody& getMobilizedBody() const {return m_Mobod;} */ @@ -406,7 +406,7 @@ class CurveSegment::Impl Transform calcSurfaceFrameInGround(const State& s) const { - return m_Mobod.getBodyTransform(s).compose(m_Offset); + return m_Mobod.getBodyTransform(s).compose(m_X_BS); } SpatialVec calcUnitForceInGround(const State& s) const @@ -497,7 +497,7 @@ class CurveSegment::Impl PosInfo& updPosInfo(const State& state) const { return Value::updDowncast( - getSubsystem().updCacheEntry(state, m_PosInfoIx)); + getSubsystem().updCacheEntry(state, m_PosIx)); } // TODO Required for accessing the cache variable? @@ -505,18 +505,20 @@ class CurveSegment::Impl CableSpan m_Path; // The path this segment belongs to. CurveSegmentIndex m_Index; // The index in its path. + // MobilizedBody that surface is attached to. MobilizedBody m_Mobod; - Transform m_Offset; + // Surface to body transform. + Transform m_X_BS; // Decoration TODO should this be here? ContactGeometry m_Geometry; DecorativeGeometry m_Decoration; // TOPOLOGY CACHE - CacheEntryIndex m_PosInfoIx; + CacheEntryIndex m_PosIx; DiscreteVariableIndex m_InstanceIx; - Vec3 m_InitPointGuess{NaN, NaN, NaN}; + Vec3 m_ContactPointHintInSurface{NaN, NaN, NaN}; size_t m_ProjectionMaxIter = 10; Real m_ProjectionAccuracy = 1e-10; @@ -525,10 +527,6 @@ class CurveSegment::Impl Real m_TouchdownAccuracy = 1e-4; size_t m_TouchdownIter = 10; - - // TODO this must be a function argument such that the caller can - // decide, - size_t m_NumberOfAnalyticPoints = 10; }; //============================================================================== From fd9b96c303902d7389798bc13345eae278515c40 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 09:57:03 +0200 Subject: [PATCH 084/127] fmt --- Simbody/src/Wrapping.cpp | 32 +++++++++--------- Simbody/src/WrappingImpl.h | 68 +++++++++++++++++++++----------------- 2 files changed, 54 insertions(+), 46 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index a3a1fab8c..976ae92bd 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -617,8 +617,8 @@ const Correction* calcPathCorrections(SolverData& data) //============================================================================== void CurveSegment::Impl::calcLiftoffIfNeeded( - const Vec3& prev_QS, - const Vec3& next_PS, + const Vec3& prevPoint_S, + const Vec3& nextPoint_S, CurveSegment::Impl::InstanceEntry& cache) const { // Only attempt liftoff when currently wrapping the surface. @@ -634,8 +634,8 @@ void CurveSegment::Impl::calcLiftoffIfNeeded( // For a zero-length curve, trigger liftoff when the prev and next points // lie above the surface plane. - if (dot(prev_QS - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= 0. || - dot(next_PS - g.K_P.p(), g.K_P.R().getAxisUnitVec(NormalAxis)) <= 0.) { + 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 liftoff. return; } @@ -647,8 +647,8 @@ void CurveSegment::Impl::calcLiftoffIfNeeded( } void CurveSegment::Impl::calcTouchdownIfNeeded( - const Vec3& prev_QS, - const Vec3& next_PS, + const Vec3& prevPoint_S, + const Vec3& nextPoint_S, CurveSegment::Impl::InstanceEntry& cache) const { // Only attempt touchdown when liftoff. @@ -661,8 +661,8 @@ void CurveSegment::Impl::calcTouchdownIfNeeded( // that is nearest to the surface. const bool touchdownDetected = calcNearestPointOnLineImplicitly( m_Geometry, - prev_QS, - next_PS, + prevPoint_S, + nextPoint_S, cache.trackingPointOnLine, m_TouchdownIter, m_TouchdownAccuracy); @@ -675,22 +675,22 @@ void CurveSegment::Impl::calcTouchdownIfNeeded( // Shoot a zero length geodesic at the touchdown point. shootNewGeodesic( cache.trackingPointOnLine, - next_PS - prev_QS, + nextPoint_S - prevPoint_S, 0., cache.sHint, cache); } void CurveSegment::Impl::assertSurfaceBounds( - const Vec3& prev_QS, - const Vec3& next_PS) const + 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, prev_QS) < 0.) { + if (calcSurfaceConstraintValue(m_Geometry, prevPoint_S) < 0.) { throw std::runtime_error("Unable to wrap over surface: Preceding point " "lies inside the surface"); } - if (calcSurfaceConstraintValue(m_Geometry, next_PS) < 0.) { + if (calcSurfaceConstraintValue(m_Geometry, nextPoint_S) < 0.) { throw std::runtime_error( "Unable to wrap over surface: Next point lies inside the surface"); } @@ -700,7 +700,7 @@ void CurveSegment::Impl::shootNewGeodesic( Vec3 x, Vec3 t, Real l, - Real dsHint, + Real sHint, InstanceEntry& cache) const { cache.samples.clear(); @@ -709,7 +709,7 @@ void CurveSegment::Impl::shootNewGeodesic( x, t, l, - cache.sHint, + sHint, cache.K_P, cache.dK_P, cache.K_Q, @@ -771,7 +771,7 @@ CurveSegment::Impl::Impl( m_Subsystem(&path.updImpl().updSubsystem()), m_Path(path), m_Index(-1), // TODO what to do with this index, and when m_Mobod(mobod), m_X_BS(X_BS), m_Geometry(geometry), - m_ContactPointHintInSurface(initPointGuess), + m_ContactPointHint_S(initPointGuess), m_Decoration(geometry.createDecorativeGeometry() .setColor(Orange) .setOpacity(.75) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 60b4b2bda..977dfae0c 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -200,15 +200,15 @@ class CurveSegment::Impl // Set the user defined point that controls the initial wrapping path. // Point is in surface coordinates. - void setContactPointHint(Vec3 contactPointHintInSurface) + void setContactPointHint(Vec3 contactPointHint_S) { - m_ContactPointHintInSurface = contactPointHintInSurface; + m_ContactPointHint_S = contactPointHint_S; } // Get the user defined point that controls the initial wrapping path. Vec3 getContactPointHint() const { - return m_ContactPointHintInSurface; + return m_ContactPointHint_S; } const InstanceEntry& getInstanceEntry(const State& s) const @@ -240,26 +240,6 @@ class CurveSegment::Impl getSubsystem().updDiscreteVariable(state, m_InstanceIx)); } - void assertSurfaceBounds(const Vec3& prevPointS, const Vec3& nextPointS) - const; - - void calcTouchdownIfNeeded( - const Vec3& prev_QS, - const Vec3& next_PS, - InstanceEntry& cache) const; - - void calcLiftoffIfNeeded( - const Vec3& prev_QS, - const Vec3& next_PS, - InstanceEntry& cache) const; - - void shootNewGeodesic( - Vec3 x, - Vec3 t, - Real l, - Real dsHint, - InstanceEntry& cache) const; - // Position level cache: Curve in ground frame. struct PosInfo { @@ -315,16 +295,16 @@ class CurveSegment::Impl { // Transform the prev and next path points to the surface frame. - const Vec3 prevPointS = X_GS.shiftBaseStationToFrame(prevPointG); - const Vec3 nextPointS = X_GS.shiftBaseStationToFrame(nextPointG); + 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(prevPointS, nextPointS); + assertSurfaceBounds(prevPoint_S, nextPoint_S); - calcTouchdownIfNeeded(prevPointS, nextPointS, updInstanceEntry(s)); - calcLiftoffIfNeeded(prevPointS, nextPointS, updInstanceEntry(s)); + calcTouchdownIfNeeded(prevPoint_S, nextPoint_S, updInstanceEntry(s)); + calcLiftoffIfNeeded(prevPoint_S, nextPoint_S, updInstanceEntry(s)); getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); } @@ -500,6 +480,34 @@ class CurveSegment::Impl 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; + + // 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. @@ -518,12 +526,12 @@ class CurveSegment::Impl CacheEntryIndex m_PosIx; DiscreteVariableIndex m_InstanceIx; - Vec3 m_ContactPointHintInSurface{NaN, NaN, NaN}; + Vec3 m_ContactPointHint_S{NaN, NaN, NaN}; size_t m_ProjectionMaxIter = 10; Real m_ProjectionAccuracy = 1e-10; - Real m_IntegratorAccuracy = 1e-6; + Real m_IntegratorAccuracy = 1e-8; Real m_TouchdownAccuracy = 1e-4; size_t m_TouchdownIter = 10; From 1bb1548815be391622981144dbbfee4b99d5d15d Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 10:03:51 +0200 Subject: [PATCH 085/127] cleanup --- Simbody/src/WrappingImpl.h | 38 +++++++++++++++++------------ examples/ExampleCableSpanSimple.cpp | 4 --- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 977dfae0c..6328e9010 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -20,25 +20,16 @@ namespace SimTK //============================================================================== class CurveSegment::Impl { + +//------------------------------------------------------------------------------ +// Some public types and aliases +//------------------------------------------------------------------------------ public: using FrenetFrame = ContactGeometry::FrenetFrame; using Variation = ContactGeometry::GeodesicVariation; using Correction = ContactGeometry::GeodesicCorrection; -public: - Impl() = delete; - Impl(const Impl& source) = delete; - Impl& operator=(const Impl& source) = delete; - ~Impl() = default; - - // TODO you would expect the constructor to take the index as well here? - Impl( - CableSpan path, - const MobilizedBody& mobod, - const Transform& X_BS, - ContactGeometry geometry, - Vec3 initPointGuess); - + // Instance level cache entry. struct LocalGeodesicSample { LocalGeodesicSample(Real l, FrenetFrame K) : length(l), frame(K) @@ -48,6 +39,7 @@ class CurveSegment::Impl FrenetFrame frame; }; + // Instance level cache entry. struct InstanceEntry { bool isActive() const @@ -69,6 +61,21 @@ class CurveSegment::Impl Vec3 trackingPointOnLine{NaN, NaN, NaN}; Status status = Status::Liftoff; }; +//------------------------------------------------------------------------------ + +public: + Impl() = delete; + Impl(const Impl& source) = delete; + Impl& operator=(const Impl& source) = delete; + ~Impl() = default; + + // TODO you would expect the constructor to take the index as well here? + Impl( + CableSpan path, + const MobilizedBody& mobod, + const Transform& X_BS, + ContactGeometry geometry, + Vec3 initPointGuess); // Apply the correction to the initial condition of the geodesic, and // shoot a new geodesic, updating the cache variable. @@ -79,11 +86,12 @@ class CurveSegment::Impl const Variation& dK_P = cache.dK_P; const FrenetFrame& K_P = cache.K_P; - // TODO Frames and Variations stored below should be part of a unti test... + // TODO This is part of the unit test block below... const FrenetFrame K0_P = cache.K_P; const FrenetFrame K0_Q = cache.K_Q; const Variation dK0_P = cache.dK_P; const Variation dK0_Q = cache.dK_Q; + // TODO end of part belongning to unit test below.. // Get corrected initial conditions. const Vec3 v = dK_P[1] * c; diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp index c13cec4d6..6290eef75 100644 --- a/examples/ExampleCableSpanSimple.cpp +++ b/examples/ExampleCableSpanSimple.cpp @@ -44,11 +44,7 @@ int testJacobian( } else { os << "PASSED Perturbation test " << i << "\n"; } - os << " d = " << d << "\n"; os << " q1 = " << q1 << "\n"; - os << " J = " << J0 << "\n"; - os << " y0 = " << y0 << "\n"; - os << " y1 = " << y1 << "\n"; os << " dy = " << dy << "\n"; os << " dyExp = " << dyExpected << "\n"; os << " e = " << e << ".norm() = " << e.norm() << "\n"; From 123e913fead77e3d837baa5b7fee7271fd31b77f Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 12:46:18 +0200 Subject: [PATCH 086/127] sketch of CurveSegment API --- Simbody/include/simbody/internal/Wrapping.h | 158 +++++--- Simbody/src/WrappingImpl.h | 384 ++++++++++---------- 2 files changed, 310 insertions(+), 232 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index ab167be72..22fd5fdbf 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -8,7 +8,6 @@ #include "simbody/internal/SimbodyMatterSubsystem.h" #include "simbody/internal/common.h" #include "simmath/internal/ContactGeometry.h" - #include #include @@ -26,11 +25,13 @@ class CableSpan; //============================================================================== // CURVE SEGMENT //============================================================================== -// The total cable path/span consists of LineSegments and CurveSegments -class SimTK_SIMBODY_EXPORT CurveSegment +// A curved segment on a surface that is part of a `CableSpan`. +class SimTK_SIMBODY_EXPORT CurveSegment final { -public: +private: CurveSegment() = default; + +public: CurveSegment(const CurveSegment&) = default; CurveSegment& operator=(const CurveSegment&) = default; CurveSegment(CurveSegment&&) noexcept = default; @@ -51,59 +52,98 @@ class SimTK_SIMBODY_EXPORT CurveSegment Disabled, }; - // TODO remove? keep? make private? +//------------------------------------------------------------------------------ +// Parameter interface +//------------------------------------------------------------------------------ const CableSpan& getCable() const; - Real getSegmentLength(const State& s) const; - const Transform& getFrenetFrameStart(const State& s) const + const ContactGeometry& getContactGeometry() const { throw std::runtime_error("NOTYETIMPLEMENTED"); } - const Transform& getFrenetFrameEnd(const State& s) const + + const Mobod& getMobilizedBody() const + { + throw std::runtime_error("NOTYETIMPLEMENTED"); + } + const Transform& getContactGeometryOffsetFrame(const State& state) const { throw std::runtime_error("NOTYETIMPLEMENTED"); } - Status getStatus(const State& state) const; +//------------------------------------------------------------------------------ +// State dependent getters. +//------------------------------------------------------------------------------ + Real getSegmentLength(const State& state) const; - const ContactGeometry& getContactGeometry() const; - const Transform& getSurfaceToGroundTransform(const State& s) const; + const Transform& getFrenetFrameStart(const State& state) const + { + throw std::runtime_error("NOTYETIMPLEMENTED"); + } - Real calcNormalCurvature(Vec3 point, Vec3 tangent) const + const Transform& getFrenetFrameEnd(const State& state) const { throw std::runtime_error("NOTYETIMPLEMENTED"); - return NaN; } - Real calcGeodesicTorsion(Vec3 point, Vec3 tangent) const + Status getStatus(const State& state) const { throw std::runtime_error("NOTYETIMPLEMENTED"); - return NaN; } - UnitVec3 calcSurfaceNormal(Vec3 point) const + int getNumberOfIntegratorStepsTaken(const State& state) { throw std::runtime_error("NOTYETIMPLEMENTED"); - return {NaN, NaN, NaN}; } - Real calcSurfaceValue(Vec3 point) const + Real getInitialIntegratorStepSize(const State& state) { throw std::runtime_error("NOTYETIMPLEMENTED"); - return NaN; } - // TODO seems useful: + // TODO useful? /* void setDisabled(const State& state) const; */ /* void setEnabled(const State& state) const; */ - /* const MobilizedBody& getMobilizedBody(const State& state) const; */ +//------------------------------------------------------------------------------ +// State dependent computations. +//------------------------------------------------------------------------------ + void calcUnitForce(const State& state, SpatialVec& unitForce_G) const + { + throw std::runtime_error("NOTYETIMPLEMENTED"); + } - /* int calcSegmentPoints(const State& state, std::vector& points); */ - /* int calcSegmentFrenetFrames( */ - /* const State& state, */ - /* std::vector& frames); */ + // Compute the curve points in ground frame. + // + // Use `nPoints` to resample over the curve length at equal intervals using Hermite interpolation. + // If `nPoints=0` no interpolation is applied, and the points from the numerical integration are used directly. + // + // Special cases that will override the `nPoints` argument: + // - If the curve length is zero, a single point is written. + // - If the curve is not active (disabled or lifted), no points are written. + // + // Note: + // If `nPoints=1`, and the curve length is not zero, an exception is thrown. + // + // Returns the number of points written. + int calcPoints(const State& state, std::vector& points_G, int nPoints = 0) + { + throw std::runtime_error("NOTYETIMPLEMENTED"); + } + // Same as `calcPoints` but will write the frenet frames along the curve. + int calcFrenetFrames(const State& state, std::vector& frames_G, int nPoints = 0) + { + throw std::runtime_error("NOTYETIMPLEMENTED"); + } + + bool isActive(const State& state) const + { + getStatus(state) == Status::Ok; + } + +//------------------------------------------------------------------------------ + // TODO public? private? class Impl; private: @@ -123,10 +163,22 @@ class SimTK_SIMBODY_EXPORT CurveSegment //============================================================================== // CABLE SPAN //============================================================================== -class SimTK_SIMBODY_EXPORT CableSpan +// 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: - struct LineSegment + +//------------------------------------------------------------------------------ + + // Helper struct representing the cable segments that do not lie on a surface. + struct LineSegment final { LineSegment() = default; @@ -137,7 +189,17 @@ class SimTK_SIMBODY_EXPORT CableSpan UnitVec3 d{NaN, NaN, NaN}; }; -public: +//------------------------------------------------------------------------------ + 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, const MobilizedBody& originBody, @@ -145,8 +207,6 @@ class SimTK_SIMBODY_EXPORT CableSpan const MobilizedBody& terminationBody, const Vec3& defaultTerminationPoint); - CurveSegmentIndex adoptSegment(const CurveSegment& segment); - void adoptWrappingObstacle( const MobilizedBody& mobod, Transform X_BS, @@ -157,11 +217,21 @@ class SimTK_SIMBODY_EXPORT CableSpan 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 +//------------------------------------------------------------------------------ Real calcCablePower(const State& state, Real tension) const { - return NaN; + throw std::runtime_error("NOTYETIMPLEMENTED"); } void applyBodyForces( @@ -169,21 +239,26 @@ class SimTK_SIMBODY_EXPORT CableSpan Real tension, Vector_& bodyForcesInG) const; + // Calls `CurveSegment::calcPoints` on each active curve segment. + int calcPoints(const State& state, std::vector& points_G, int nPointsPerCurveSegment = 0) + { + throw std::runtime_error("NOTYETIMPLEMENTED"); + } + +//------------------------------------------------------------------------------ +// 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; - size_t countActive(const State& s) const; - - /* int calcPathPoints(const State& state, std::vector& points); */ - /* int calcPathFrenetFrames( */ - /* const State& state, */ - /* std::vector& frames); */ - - class Impl; +//------------------------------------------------------------------------------ + class Impl; // TODO private? public? private: const Impl& getImpl() const @@ -206,7 +281,7 @@ class SimTK_SIMBODY_EXPORT CableSpan // SUBSYSTEM //============================================================================== -// Rename to CableTrackerSubSystem? WrappingPathSubsystem? +// TODO Alternative names: CableTrackerSubSystem, WrappingPathSubsystem. class SimTK_SIMBODY_EXPORT CableSubsystem : public Subsystem { public: @@ -217,9 +292,6 @@ class SimTK_SIMBODY_EXPORT CableSubsystem : public Subsystem const CableSpan& getPath(CableSpanIndex idx) const; CableSpan& updPath(CableSpanIndex idx); - size_t writePathPoints(std::vector& points) const; - size_t writePathFrames(std::vector& frenetFrames) const; - /* private: */ SimTK_PIMPL_DOWNCAST(CableSubsystem, Subsystem); class Impl; diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 6328e9010..646f2778c 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -61,13 +61,30 @@ class CurveSegment::Impl Vec3 trackingPointOnLine{NaN, NaN, NaN}; Status status = Status::Liftoff; }; + + // Position level cache: Curve in ground frame. + struct PosInfo + { + Transform X_GS{}; + + FrenetFrame KP{}; + FrenetFrame KQ{}; + + Variation dKP{}; + Variation dKQ{}; + + SpatialVec unitForce; + }; //------------------------------------------------------------------------------ public: - Impl() = delete; - Impl(const Impl& source) = delete; - Impl& operator=(const Impl& source) = delete; - ~Impl() = default; + 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( @@ -77,6 +94,160 @@ class CurveSegment::Impl 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 = Status::Liftoff; + 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)) { + throw std::runtime_error( + "expected not realized when calling realizePosition"); + } + + if (getInstanceEntry(s).status == Status::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); + + 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]; + } + + // Compute the unit force and moment in ground frame. + { + const UnitVec3& t_P = pos.KP.R().getAxisUnitVec(TangentAxis); + const UnitVec3& t_Q = pos.KQ.R().getAxisUnitVec(TangentAxis); + const Vec3 F_G = t_Q - t_P; + + const Vec3 x_GB = m_Mobod.getBodyOriginLocation(s); + const Vec3 r_P = pos.KP.p() - x_GB; + const Vec3 r_Q = pos.KQ.p() - x_GB; + const Vec3 M_G = r_Q % t_Q - r_P % t_P; + pos.F_G{M_G, F_G}; + } + + getSubsystem().markCacheValueRealized(s, m_PosIx); + } + + void realizeCablePosition(const State& s) const; + + void invalidatePositionLevelCache(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; + } + +//------------------------------------------------------------------------------ +// 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)); + } + // 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 @@ -206,191 +377,7 @@ class CurveSegment::Impl } } - // 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 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)); - } - - 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)); - } - - // Position level cache: Curve in ground frame. - struct PosInfo - { - Transform X_GS{}; - - FrenetFrame KP{}; - FrenetFrame KQ{}; - - Variation dKP{}; - Variation dKQ{}; - - SpatialVec unitForce; - }; - - // 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 = Status::Liftoff; - 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)) { - throw std::runtime_error( - "expected not realized when calling realizePosition"); - } - - if (getInstanceEntry(s).status == Status::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); - - calcTouchdownIfNeeded(prevPoint_S, nextPoint_S, updInstanceEntry(s)); - calcLiftoffIfNeeded(prevPoint_S, nextPoint_S, updInstanceEntry(s)); - - getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); - } - - // Transform geodesic in local surface coordinates to ground. - { - const InstanceEntry& ie = getInstanceEntry(s); - PosInfo& ppe = updPosInfo(s); - // Store the the local geodesic in ground frame. - ppe.X_GS = X_GS; - - // Store the the local geodesic in ground frame. - ppe.KP = X_GS.compose(ie.K_P); - ppe.KQ = X_GS.compose(ie.K_Q); - - ppe.dKP[0] = X_GS.R() * ie.dK_P[0]; - ppe.dKP[1] = X_GS.R() * ie.dK_P[1]; - - ppe.dKQ[0] = X_GS.R() * ie.dK_Q[0]; - ppe.dKQ[1] = X_GS.R() * ie.dK_Q[1]; - } - - getSubsystem().markCacheValueRealized(s, m_PosIx); - } - - void realizeCablePosition(const State& s) const; - - void invalidatePositionLevelCache(const State& state) const - { - getSubsystem().markCacheValueNotRealized(state, m_PosIx); - } - - const CableSpan& getCable() const - { - return m_Path; - } - - CurveSegmentIndex getIndex() const - { - return m_Index; - } - - void setIndex(CurveSegmentIndex ix) - { - m_Index = ix; - } - - const PosInfo& getPosInfo(const State& s) const - { - return Value::downcast( - getSubsystem().getCacheEntry(s, m_PosIx)); - } - - /* const MobilizedBody& getMobilizedBody() const {return m_Mobod;} */ - void calcContactPointVelocitiesInGround( - const State& s, - Vec3& v_GP, - Vec3& v_GQ) const - { - // TODO use builtin? - /* v_GP = m_Mobod.findStationVelocityInGround(state, P_B); */ - - // Get body kinematics in ground frame. - const Vec3& x_BG = m_Mobod.getBodyOriginLocation(s); - const Vec3& w_BG = m_Mobod.getBodyAngularVelocity(s); - const Vec3& v_BG = m_Mobod.getBodyOriginVelocity(s); - - const PosInfo& pos = getPosInfo(s); - - // Relative contact point positions to body origin, expressed in ground - // frame. - Vec3 r_P = pos.KP.p() - x_BG; - Vec3 r_Q = pos.KQ.p() - x_BG; - - // Compute contact point velocities in ground frame. - v_GP = v_BG + w_BG % r_P; - v_GQ = v_BG + w_BG % r_Q; - } + const MobilizedBody& getMobilizedBody() const {return m_Mobod;} Transform calcSurfaceFrameInGround(const State& s) const { @@ -482,6 +469,25 @@ class CurveSegment::Impl } 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( From 7c0b9ef1de9f896d79132b89c99881d558c4d616 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 13:36:14 +0200 Subject: [PATCH 087/127] rename Status::Liftoff to Status::Lifted --- Simbody/include/simbody/internal/Wrapping.h | 2 +- Simbody/src/Wrapping.cpp | 4 ++-- Simbody/src/WrappingImpl.h | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 22fd5fdbf..2dc2cda52 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -48,7 +48,7 @@ class SimTK_SIMBODY_EXPORT CurveSegment final enum class Status { Ok, - Liftoff, + Lifted, Disabled, }; diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 976ae92bd..9d4972ec4 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -641,7 +641,7 @@ void CurveSegment::Impl::calcLiftoffIfNeeded( } // Liftoff detected: update status. - g.status = Status::Liftoff; + g.status = Status::Lifted; // Initialize the tracking point from the last geodesic start point. cache.trackingPointOnLine = g.K_P.p(); } @@ -653,7 +653,7 @@ void CurveSegment::Impl::calcTouchdownIfNeeded( { // Only attempt touchdown when liftoff. LocalGeodesicInfo& g = cache; - if (g.status != Status::Liftoff) { + if (g.status != Status::Lifted) { return; } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 646f2778c..6dab7c03e 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -59,7 +59,7 @@ class CurveSegment::Impl double sHint = NaN; Vec3 trackingPointOnLine{NaN, NaN, NaN}; - Status status = Status::Liftoff; + Status status = Status::Lifted; }; // Position level cache: Curve in ground frame. @@ -105,7 +105,7 @@ class CurveSegment::Impl Value* cache = new Value(); cache->upd().length = 0.; - cache->upd().status = Status::Liftoff; + cache->upd().status = Status::Lifted; cache->upd().trackingPointOnLine = getContactPointHint(); m_InstanceIx = updSubsystem().allocateAutoUpdateDiscreteVariable( From e413e172a0cfa1717a70641b0a9a7e64273201cd Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 13:38:21 +0200 Subject: [PATCH 088/127] cleanup --- Simbody/src/Wrapping.cpp | 58 ++++++++++------ Simbody/src/WrappingImpl.h | 131 ++++++++++++++----------------------- 2 files changed, 86 insertions(+), 103 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 9d4972ec4..1921ade88 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1213,7 +1213,7 @@ void CableSpan::Impl::applyCorrection(const State& s, const Vector& c) const void CableSpan::Impl::invalidatePositionLevelCache(const State& s) const { for (const CurveSegment& curve : m_CurveSegments) { - curve.getImpl().invalidatePositionLevelCache(s); + curve.getImpl().invalidatePosEntry(s); } getSubsystem().markCacheValueNotRealized(s, m_PosInfoIx); } @@ -1343,7 +1343,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Path has changed: invalidate each segment's cache. for (const CurveSegment& curve : m_CurveSegments) { - curve.getImpl().invalidatePositionLevelCache(s); + curve.getImpl().invalidatePosEntry(s); } } @@ -1352,35 +1352,50 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const { - const PosInfo& pos = getPosInfo(s); + 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 = m_OriginBody.findStationVelocityInGround(s, m_OriginPoint); const CurveSegment* lastActive = nullptr; - for (const CurveSegment& obstacle : m_CurveSegments) { - if (!obstacle.getImpl().getInstanceEntry(s).isActive()) { + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.getImpl().getInstanceEntry(s).isActive()) { continue; } - const GeodesicInfo& g = obstacle.getImpl().getPosInfo(s); + 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); - Vec3 next_v_GQ; - Vec3 v_GP; - obstacle.getImpl().calcContactPointVelocitiesInGround( - s, - v_GP, - next_v_GQ); + const Vec3 v_GP = CalcPointVelocityInGround(mobod, g.KP.p()); lengthDot += dot(e_G, v_GP - v_GQ); - v_GQ = next_v_GQ; - lastActive = &obstacle; + v_GQ = CalcPointVelocityInGround(mobod, g.KQ.p()); + + lastActive = &curve; } const Vec3 v_GP = m_TerminationBody.findStationVelocityInGround(s, m_TerminationPoint); + + const PosInfo& pos = getPosInfo(s); const UnitVec3 e_G = lastActive ? lastActive->getImpl().getPosInfo(s).KQ.R().getAxisUnitVec( TangentAxis) @@ -1394,15 +1409,18 @@ void CableSpan::Impl::applyBodyForces( Real tension, Vector_& bodyForcesInG) const { - // TODO why? - if (tension <= 0) { - return; + if (tension < 0.) { + throw std::runtime_error("Cable tension can not go below zero."); } - realizePosition(s); + SpatialVec unitForce_G; + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.isActive(s)) { + return; + } - for (const CurveSegment& segment : m_CurveSegments) { - segment.getImpl().applyBodyForce(s, tension, bodyForcesInG); + curve.calcUnitForce(s, unitForce_G); + curve.getMobilizedBody().applyBodyForce(s, unitForce_G * tension, bodyForcesInG); } } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 6dab7c03e..193b5661b 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -174,25 +174,12 @@ class CurveSegment::Impl pos.dKQ[1] = X_GS.R() * ie.dK_Q[1]; } - // Compute the unit force and moment in ground frame. - { - const UnitVec3& t_P = pos.KP.R().getAxisUnitVec(TangentAxis); - const UnitVec3& t_Q = pos.KQ.R().getAxisUnitVec(TangentAxis); - const Vec3 F_G = t_Q - t_P; - - const Vec3 x_GB = m_Mobod.getBodyOriginLocation(s); - const Vec3 r_P = pos.KP.p() - x_GB; - const Vec3 r_Q = pos.KQ.p() - x_GB; - const Vec3 M_G = r_Q % t_Q - r_P % t_P; - pos.F_G{M_G, F_G}; - } - getSubsystem().markCacheValueRealized(s, m_PosIx); } void realizeCablePosition(const State& s) const; - void invalidatePositionLevelCache(const State& state) const + void invalidatePosEntry(const State& state) const { getSubsystem().markCacheValueNotRealized(state, m_PosIx); } @@ -228,6 +215,13 @@ class CurveSegment::Impl return m_ContactPointHint_S; } + const MobilizedBody& getMobilizedBody() const {return m_Mobod;} + + CableSubsystem& updSubsystem() { return *m_Subsystem; } + const CableSubsystem& getSubsystem() const { return *m_Subsystem; } + + const DecorativeGeometry& getDecoration() const { return m_Decoration; } + //------------------------------------------------------------------------------ // Cache entry access //------------------------------------------------------------------------------ @@ -248,6 +242,43 @@ class CurveSegment::Impl getSubsystem().getCacheEntry(s, m_PosIx)); } + Transform calcSurfaceFrameInGround(const State& s) const + { + return m_Mobod.getBodyTransform(s).compose(m_X_BS); + } + + // Compute the path points of the current geodesic, and write them to + // the buffer. These points are in local surface coordinates. Returns + // the number of points written. + void calcPathPoints(const State& s, std::vector& points) const + { + const Transform& X_GS = getPosInfo(s).X_GS; + const InstanceEntry& geodesic_S = getInstanceEntry(s); + if (!geodesic_S.isActive()) { + return; + } + for (const LocalGeodesicSample& sample : geodesic_S.samples) { + points.push_back(X_GS.shiftFrameStationToBase(sample.frame.p())); + } + } + + void calcUnitForce(const State& s, SpatialVec& unitForce_G) const + { + const PosInfo& posInfo = getPosInfo(s); + const Vec3& x_BG = m_Mobod.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; + } + // 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 @@ -359,75 +390,7 @@ class CurveSegment::Impl } // End of unit test code block... - invalidatePositionLevelCache(s); - } - - // Compute the path points of the current geodesic, and write them to - // the buffer. These points are in local surface coordinates. Returns - // the number of points written. - void calcPathPoints(const State& s, std::vector& points) const - { - const Transform& X_GS = getPosInfo(s).X_GS; - const InstanceEntry& geodesic_S = getInstanceEntry(s); - if (!geodesic_S.isActive()) { - return; - } - for (const LocalGeodesicSample& sample : geodesic_S.samples) { - points.push_back(X_GS.shiftFrameStationToBase(sample.frame.p())); - } - } - - const MobilizedBody& getMobilizedBody() const {return m_Mobod;} - - Transform calcSurfaceFrameInGround(const State& s) const - { - return m_Mobod.getBodyTransform(s).compose(m_X_BS); - } - - SpatialVec calcUnitForceInGround(const State& s) const - { - const PosInfo& posInfo = getPosInfo(s); - - const UnitVec3& t_P = posInfo.KP.R().getAxisUnitVec(TangentAxis); - const UnitVec3& t_Q = posInfo.KQ.R().getAxisUnitVec(TangentAxis); - const Vec3 F_G = t_Q - t_P; - - const Transform& X_GB = m_Mobod.getBodyTransform(s); - const Vec3 x_GB = X_GB.p(); - const Vec3 r_P = posInfo.KP.p() - x_GB; - const Vec3 r_Q = posInfo.KQ.p() - x_GB; - const Vec3 M_G = r_Q % t_Q - r_P % t_P; - - return {M_G, F_G}; - } - - void applyBodyForce( - const State& s, - Real tension, - Vector_& bodyForcesInG) const - { - if (!getInstanceEntry(s).isActive()) { - return; - } - - m_Mobod.applyBodyForce(s, calcUnitForceInGround(s), bodyForcesInG); - } - - // TODO allow for user to shoot his own geodesic. - /* void calcGeodesic(State& state, Vec3 x, Vec3 t, Real l) const; */ - - CableSubsystem& updSubsystem() - { - return *m_Subsystem; - } - const CableSubsystem& getSubsystem() const - { - return *m_Subsystem; - } - - const DecorativeGeometry& getDecoration() const - { - return m_Decoration; + invalidatePosEntry(s); } Vec3 calcInitialContactPoint(const State& s) const @@ -457,6 +420,8 @@ class CurveSegment::Impl Array_& decorations) const { const InstanceEntry& cache = getInstanceEntry(s); + if (!cache.isActive()) {return;} + const Transform& X_GS = calcSurfaceFrameInGround(s); Vec3 a = X_GS.shiftFrameStationToBase(cache.K_P.p()); for (size_t i = 1; i < cache.samples.size(); ++i) { From d53ac6c5f7c6b45712aa6dd4ebdd413f9ec38856 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 14:31:22 +0200 Subject: [PATCH 089/127] move function bodies to source file --- Simbody/include/simbody/internal/Wrapping.h | 69 ++---- Simbody/src/Wrapping.cpp | 249 ++++++++++++-------- 2 files changed, 167 insertions(+), 151 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 2dc2cda52..150d9c7fa 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -57,49 +57,26 @@ class SimTK_SIMBODY_EXPORT CurveSegment final //------------------------------------------------------------------------------ const CableSpan& getCable() const; - const ContactGeometry& getContactGeometry() const - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + const ContactGeometry& getContactGeometry() const; - const Mobod& getMobilizedBody() const - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } - const Transform& getContactGeometryOffsetFrame(const State& state) const - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + const Mobod& getMobilizedBody() const; + + const Transform& getContactGeometryOffsetFrame(const State& state) const; //------------------------------------------------------------------------------ // State dependent getters. //------------------------------------------------------------------------------ Real getSegmentLength(const State& state) const; - const Transform& getFrenetFrameStart(const State& state) const - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + const Transform& getFrenetFrameStart(const State& state) const; - const Transform& getFrenetFrameEnd(const State& state) const - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + const Transform& getFrenetFrameEnd(const State& state) const; - Status getStatus(const State& state) const - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + Status getStatus(const State& state) const; - int getNumberOfIntegratorStepsTaken(const State& state) - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + int getNumberOfIntegratorStepsTaken(const State& state); - Real getInitialIntegratorStepSize(const State& state) - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + Real getInitialIntegratorStepSize(const State& state); // TODO useful? /* void setDisabled(const State& state) const; */ @@ -108,10 +85,7 @@ class SimTK_SIMBODY_EXPORT CurveSegment final //------------------------------------------------------------------------------ // State dependent computations. //------------------------------------------------------------------------------ - void calcUnitForce(const State& state, SpatialVec& unitForce_G) const - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + void calcUnitForce(const State& state, SpatialVec& unitForce_G) const; // Compute the curve points in ground frame. // @@ -126,20 +100,14 @@ class SimTK_SIMBODY_EXPORT CurveSegment final // If `nPoints=1`, and the curve length is not zero, an exception is thrown. // // Returns the number of points written. - int calcPoints(const State& state, std::vector& points_G, int nPoints = 0) - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + int calcPoints(const State& state, std::vector& points_G, int nPoints = 0) const; // Same as `calcPoints` but will write the frenet frames along the curve. - int calcFrenetFrames(const State& state, std::vector& frames_G, int nPoints = 0) - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + int calcFrenetFrames(const State& state, std::vector& frames_G, int nPoints = 0) const; bool isActive(const State& state) const { - getStatus(state) == Status::Ok; + return getStatus(state) == Status::Ok; } //------------------------------------------------------------------------------ @@ -229,10 +197,7 @@ class SimTK_SIMBODY_EXPORT CableSpan final //------------------------------------------------------------------------------ // State dependent calculations //------------------------------------------------------------------------------ - Real calcCablePower(const State& state, Real tension) const - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + Real calcCablePower(const State& state, Real tension) const; void applyBodyForces( const State& state, @@ -240,10 +205,7 @@ class SimTK_SIMBODY_EXPORT CableSpan final Vector_& bodyForcesInG) const; // Calls `CurveSegment::calcPoints` on each active curve segment. - int calcPoints(const State& state, std::vector& points_G, int nPointsPerCurveSegment = 0) - { - throw std::runtime_error("NOTYETIMPLEMENTED"); - } + int calcPoints(const State& state, std::vector& points_G, int nPointsPerCurveSegment = 0) const; //------------------------------------------------------------------------------ // TODO TEMPORARY FOR UNIT TESTING @@ -273,6 +235,7 @@ class SimTK_SIMBODY_EXPORT CableSpan final std::shared_ptr m_Impl = nullptr; + friend CurveSegment; friend CurveSegment::Impl; friend CableSubsystem; }; diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 1921ade88..32022cd49 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -22,6 +22,157 @@ using SolverData = CableSubsystem::Impl::SolverData; using Status = CurveSegment::Status; using Variation = ContactGeometry::GeodesicVariation; +//============================================================================== +// CURVE SEGMENT +//============================================================================== + +CurveSegment::CurveSegment( + CableSpan cable, + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 xHint) : + m_Impl(std::shared_ptr( + new CurveSegment::Impl(cable, mobod, 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 +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +const Mobod& CurveSegment::getMobilizedBody() const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +const Transform& CurveSegment::getContactGeometryOffsetFrame(const State& state) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +CurveSegment::Status CurveSegment::getStatus(const State& s) const +{ + getImpl().realizeCablePosition(s); + return getImpl().getInstanceEntry(s).status; +} + +const Transform& CurveSegment::getFrenetFrameStart(const State& state) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +const Transform& CurveSegment::getFrenetFrameEnd(const State& state) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +int CurveSegment::getNumberOfIntegratorStepsTaken(const State& state) +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +Real CurveSegment::getInitialIntegratorStepSize(const State& state) +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} +void CurveSegment::calcUnitForce(const State& state, SpatialVec& unitForce_G) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +int CurveSegment::calcPoints(const State& state, std::vector& points_G, int nPoints) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +// Same as `calcPoints` but will write the frenet frames along the curve. +int CurveSegment::calcFrenetFrames(const State& state, std::vector& frames_G, int nPoints) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +//============================================================================== +// CABLE SPAN +//============================================================================== + +CableSpan::CableSpan( + CableSubsystem& subsystem, + const MobilizedBody& originBody, + const Vec3& defaultOriginPoint, + const MobilizedBody& terminationBody, + const Vec3& defaultTerminationPoint) : + m_Impl(std::shared_ptr(new Impl( + subsystem, + originBody, + defaultOriginPoint, + terminationBody, + defaultTerminationPoint))) +{ + subsystem.updImpl().adoptCablePath(*this); +} + +void CableSpan::adoptWrappingObstacle( + const MobilizedBody& mobod, + Transform X_BS, + const ContactGeometry& geometry, + Vec3 contactPointHint) +{ + CurveSegment(*this, mobod, 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::vector& points_G, int nPointsPerCurveSegment) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + +Real CableSpan::calcCablePower(const State& state, Real tension) const +{ + throw std::runtime_error("NOTYETIMPLEMENTED"); +} + //============================================================================== // CONSTANTS //============================================================================== @@ -721,43 +872,6 @@ void CurveSegment::Impl::shootNewGeodesic( cache.length = l; } -//============================================================================== -// CURVE SEGMENT -//============================================================================== - -CurveSegment::CurveSegment( - CableSpan cable, - const MobilizedBody& mobod, - Transform X_BS, - const ContactGeometry& geometry, - Vec3 xHint) : - m_Impl(std::shared_ptr( - new CurveSegment::Impl(cable, mobod, X_BS, geometry, xHint))) -{ - // TODO bit awkward to set the index later. - updImpl().setIndex(cable.adoptSegment(*this)); - /* CurveSegmentIndex ix = cable.adoptSegment(*this); */ - /* m_Impl = std::shared_ptr(new - * CurveSegment::Impl(cable, ix, mobod, X_BS, geometry, xHint)); */ -} - -const CableSpan& CurveSegment::getCable() const -{ - return getImpl().getCable(); -} - -Real CurveSegment::getSegmentLength(const State& s) const -{ - getImpl().realizeCablePosition(s); - return getImpl().getInstanceEntry(s).length; -} - -CurveSegment::Status CurveSegment::getStatus(const State& s) const -{ - getImpl().realizeCablePosition(s); - return getImpl().getInstanceEntry(s).status; -} - //============================================================================== // CURVE SEGMENT IMPL //============================================================================== @@ -822,67 +936,6 @@ void addPathErrorJacobian( } // namespace -//============================================================================== -// CABLE SPAN -//============================================================================== - -CableSpan::CableSpan( - CableSubsystem& subsystem, - const MobilizedBody& originBody, - const Vec3& defaultOriginPoint, - const MobilizedBody& terminationBody, - const Vec3& defaultTerminationPoint) : - m_Impl(std::shared_ptr(new Impl( - subsystem, - originBody, - defaultOriginPoint, - terminationBody, - defaultTerminationPoint))) -{ - subsystem.updImpl().adoptCablePath(*this); -} - -CurveSegmentIndex CableSpan::adoptSegment(const CurveSegment& segment) -{ - return updImpl().adoptSegment(segment); -} - -void CableSpan::adoptWrappingObstacle( - const MobilizedBody& mobod, - Transform X_BS, - const ContactGeometry& geometry, - Vec3 contactPointHint) -{ - CurveSegment(*this, mobod, 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); -} - //============================================================================== // CABLE SPAN IMPL //============================================================================== From 5d113af1fff9e9c80a166d6cbbe712499dc9f74f Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 18:14:17 +0200 Subject: [PATCH 090/127] add resampling of geodesic --- Simbody/src/Wrapping.cpp | 132 ++++++++++++++++++++++++++++++++++++- Simbody/src/WrappingImpl.h | 15 +---- 2 files changed, 132 insertions(+), 15 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 32022cd49..f57f9c5f6 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -734,7 +734,137 @@ void calcGeodesicAndVariationImplicitly( } // namespace //============================================================================== -// SOLVER +// 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::vector& interpolatedSamples) + { + // Some sanity checks. + if (geodesic.empty()) { + throw std::runtime_error("Resampling of geodesic failed: Provided geodesic is empty."); + } + if (geodesic.front().length != 0.) { + throw std::runtime_error("Resampling of geodesic failed: First frame must be at length = zero"); + } + if (geodesic.front().length < 0.) { + throw std::runtime_error("Resampling of geodesic failed: Last frame must be at length > zero"); + } + if (nSamples == 1 && geodesic.size() != 1) { + throw std::runtime_error("Resampling of geodesic failed: Requested number of samples must be unequal to 1"); + } + + // Capture the start of the geodesic. + interpolatedSamples.push_back( + 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()) { + 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) { + 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. + interpolatedSamples.push_back(point_G); + + break; + } + } + + // Capture the last point of the geodesic. + interpolatedSamples.push_back(X_GS.shiftFrameStationToBase(geodesic.back().frame.p())); + + return nSamples; + } +} + +int CurveSegment::Impl::calcPathPoints(const State& s, std::vector& points, int nSamples) const +{ + const Transform& X_GS = getPosInfo(s).X_GS; + const InstanceEntry& geodesic_S = getInstanceEntry(s); + 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) { + points.push_back(X_GS.shiftFrameStationToBase(sample.frame.p())); + } + } + + // Resample the points from the integrator by interpolating at equal intervals. + return calcResampledGeodesicPoints(geodesic_S.samples, X_GS, nSamples, points); +} + +//============================================================================== +// ??? //============================================================================== namespace diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 193b5661b..573a91632 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -247,20 +247,7 @@ class CurveSegment::Impl return m_Mobod.getBodyTransform(s).compose(m_X_BS); } - // Compute the path points of the current geodesic, and write them to - // the buffer. These points are in local surface coordinates. Returns - // the number of points written. - void calcPathPoints(const State& s, std::vector& points) const - { - const Transform& X_GS = getPosInfo(s).X_GS; - const InstanceEntry& geodesic_S = getInstanceEntry(s); - if (!geodesic_S.isActive()) { - return; - } - for (const LocalGeodesicSample& sample : geodesic_S.samples) { - points.push_back(X_GS.shiftFrameStationToBase(sample.frame.p())); - } - } + int calcPathPoints(const State& s, std::vector& points, int nSamples=0) const; void calcUnitForce(const State& s, SpatialVec& unitForce_G) const { From 16f0a24b5f4d983f4f424749687834645f5116c4 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 3 May 2024 19:09:09 +0200 Subject: [PATCH 091/127] cleanup of Wrapping header --- Simbody/include/simbody/internal/Wrapping.h | 6 +-- Simbody/src/Wrapping.cpp | 43 ++++++++++++--------- Simbody/src/WrappingImpl.h | 31 +++++++++++---- 3 files changed, 50 insertions(+), 30 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 150d9c7fa..9f6676ddf 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -61,7 +61,8 @@ class SimTK_SIMBODY_EXPORT CurveSegment final const Mobod& getMobilizedBody() const; - const Transform& getContactGeometryOffsetFrame(const State& state) const; + const Transform& getXformSurfaceToBody() const; + void setXformSurfaceToBody(Transform X_BS); //------------------------------------------------------------------------------ // State dependent getters. @@ -102,9 +103,6 @@ class SimTK_SIMBODY_EXPORT CurveSegment final // Returns the number of points written. int calcPoints(const State& state, std::vector& points_G, int nPoints = 0) const; - // Same as `calcPoints` but will write the frenet frames along the curve. - int calcFrenetFrames(const State& state, std::vector& frames_G, int nPoints = 0) const; - bool isActive(const State& state) const { return getStatus(state) == Status::Ok; diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index f57f9c5f6..32338a8d0 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -52,17 +52,22 @@ Real CurveSegment::getSegmentLength(const State& s) const const ContactGeometry& CurveSegment::getContactGeometry() const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + return getImpl().getContactGeometry(); } const Mobod& CurveSegment::getMobilizedBody() const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + return getImpl().getMobilizedBody(); } -const Transform& CurveSegment::getContactGeometryOffsetFrame(const State& state) const +void CurveSegment::setXformSurfaceToBody(Transform X_BS) { - throw std::runtime_error("NOTYETIMPLEMENTED"); + updImpl().setXformSurfaceToBody(std::move(X_BS)); +} + +const Transform& CurveSegment::getXformSurfaceToBody() const +{ + return getImpl().getXformSurfaceToBody(); } CurveSegment::Status CurveSegment::getStatus(const State& s) const @@ -73,37 +78,38 @@ CurveSegment::Status CurveSegment::getStatus(const State& s) const const Transform& CurveSegment::getFrenetFrameStart(const State& state) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + getImpl().realizeCablePosition(state); + return getImpl().getPosInfo(state).KP; } const Transform& CurveSegment::getFrenetFrameEnd(const State& state) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + getImpl().realizeCablePosition(state); + return getImpl().getPosInfo(state).KQ; } int CurveSegment::getNumberOfIntegratorStepsTaken(const State& state) { - throw std::runtime_error("NOTYETIMPLEMENTED"); + getImpl().realizeCablePosition(state); + return getImpl().getInstanceEntry(state).samples.size(); } Real CurveSegment::getInitialIntegratorStepSize(const State& state) { + getImpl().realizeCablePosition(state); throw std::runtime_error("NOTYETIMPLEMENTED"); } + void CurveSegment::calcUnitForce(const State& state, SpatialVec& unitForce_G) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + getImpl().realizeCablePosition(state); + getImpl().calcUnitForce(state, unitForce_G); } int CurveSegment::calcPoints(const State& state, std::vector& points_G, int nPoints) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); -} - -// Same as `calcPoints` but will write the frenet frames along the curve. -int CurveSegment::calcFrenetFrames(const State& state, std::vector& frames_G, int nPoints) const -{ - throw std::runtime_error("NOTYETIMPLEMENTED"); + getImpl().realizeCablePosition(state); + return getImpl().calcPathPoints(state, points_G, nPoints); } //============================================================================== @@ -165,7 +171,7 @@ void CableSpan::applyBodyForces( int CableSpan::calcPoints(const State& state, std::vector& points_G, int nPointsPerCurveSegment) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + return getImpl().calcPathPoints(state, points_G, nPointsPerCurveSegment); } Real CableSpan::calcCablePower(const State& state, Real tension) const @@ -1493,8 +1499,8 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Evaluate path error, and stop when converged. calcPathErrorVector<2>(s, data.lineSegments, axes, data.pathError); const Real maxPathError = data.pathError.normInf(); - std::cout << " max = " << maxPathError << "\n"; - if (maxPathError < m_PathErrorBound || (posInfo.loopIter + 1) >= m_PathMaxIter) { + if (maxPathError < m_PathErrorBound || + (posInfo.loopIter + 1) >= m_PathMaxIter) { posInfo.l = 0.; for (const LineSegment& line : data.lineSegments) { posInfo.l += line.l; @@ -1592,6 +1598,7 @@ void CableSpan::Impl::applyBodyForces( Real tension, Vector_& bodyForcesInG) const { + realizePosition(s); if (tension < 0.) { throw std::runtime_error("Cable tension can not go below zero."); } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 573a91632..d3d3861c6 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -215,12 +215,17 @@ class CurveSegment::Impl return m_ContactPointHint_S; } + const ContactGeometry& getContactGeometry() const {return m_Geometry;} + + const DecorativeGeometry& getDecoration() const { return m_Decoration; } + const MobilizedBody& getMobilizedBody() const {return m_Mobod;} - CableSubsystem& updSubsystem() { return *m_Subsystem; } - const CableSubsystem& getSubsystem() const { return *m_Subsystem; } + const Transform& getXformSurfaceToBody() const {return m_X_BS;} + void setXformSurfaceToBody(Transform X_BS) {m_X_BS = std::move(X_BS);} - const DecorativeGeometry& getDecoration() const { return m_Decoration; } + const CableSubsystem& getSubsystem() const { return *m_Subsystem; } + CableSubsystem& updSubsystem() { return *m_Subsystem; } //------------------------------------------------------------------------------ // Cache entry access @@ -247,7 +252,7 @@ class CurveSegment::Impl return m_Mobod.getBodyTransform(s).compose(m_X_BS); } - int calcPathPoints(const State& s, std::vector& points, int nSamples=0) const; + int calcPathPoints(const State& s, std::vector& points, int nSamples) const; void calcUnitForce(const State& s, SpatialVec& unitForce_G) const { @@ -580,13 +585,23 @@ class CableSpan::Impl Stage stage, Array_& decorations) const; - void calcPathPoints(const State& state, std::vector& points) const + int calcPathPoints(const State& state, std::vector& points_G, int nPointsPerCurveSegment) const { - points.push_back(getPosInfo(state).xO); + // Write the initial point. + const PosInfo& pos = getPosInfo(state); + points_G.push_back(pos.xO); + + // Write points along each of the curves. + int count = 0; // Count number of points written. for (const CurveSegment& curve : m_CurveSegments) { - curve.getImpl().calcPathPoints(state, points); + count += curve.getImpl().calcPathPoints(state, points_G, nPointsPerCurveSegment); } - points.push_back(getPosInfo(state).xI); + + // Write the termination point. + points_G.push_back(pos.xI); + + // Return number of points written. + return count + 2; } void calcPathErrorJacobianUtility( From a0d0504c6c680752a3dd3b79ec1e7234c182be4d Mon Sep 17 00:00:00 2001 From: pepbos Date: Sun, 5 May 2024 16:40:10 +0200 Subject: [PATCH 092/127] add doc comment --- Simbody/include/simbody/internal/Wrapping.h | 23 ++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 9f6676ddf..93a172641 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -38,13 +38,34 @@ class SimTK_SIMBODY_EXPORT CurveSegment final 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, const MobilizedBody& mobod, Transform X_BS, const ContactGeometry& geometry, - Vec3 xHint); + Vec3 initialContactPointHint); + /** A helper class, representing the wrapping status of this segment in + relation to the contact geometry. + + Status::Ok indicates that the cable is in contact with the surface. + Status::Lifted indicates that the cable is not in contact with the surface. + Status::Disabled indicates that the surface obstacle is "disabled", + preventing any interaction with the + cable. */ enum class Status { Ok, From e68e14b1474bc5f1bd5f1c3f57bf460ad78724c8 Mon Sep 17 00:00:00 2001 From: pepbos Date: Sun, 5 May 2024 17:11:28 +0200 Subject: [PATCH 093/127] update CurveSegment doc comments --- Simbody/include/simbody/internal/Wrapping.h | 65 +++++++++++++-------- 1 file changed, 40 insertions(+), 25 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 93a172641..7768f913d 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -23,9 +23,7 @@ class CurveSegment; class CableSpan; //============================================================================== -// CURVE SEGMENT -//============================================================================== -// A curved segment on a surface that is part of a `CableSpan`. +// A curved cable segment on a surface that is part of a `CableSpan`. class SimTK_SIMBODY_EXPORT CurveSegment final { private: @@ -73,55 +71,72 @@ class SimTK_SIMBODY_EXPORT CurveSegment final Disabled, }; -//------------------------------------------------------------------------------ -// Parameter interface -//------------------------------------------------------------------------------ + /** 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); -//------------------------------------------------------------------------------ -// State dependent getters. -//------------------------------------------------------------------------------ + /** 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. */ Status 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; */ -//------------------------------------------------------------------------------ -// State dependent computations. -//------------------------------------------------------------------------------ + /** 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 the curve points in ground frame. - // - // Use `nPoints` to resample over the curve length at equal intervals using Hermite interpolation. - // If `nPoints=0` no interpolation is applied, and the points from the numerical integration are used directly. - // - // Special cases that will override the `nPoints` argument: - // - If the curve length is zero, a single point is written. - // - If the curve is not active (disabled or lifted), no points are written. - // - // Note: - // If `nPoints=1`, and the curve length is not zero, an exception is thrown. - // - // Returns the number of points written. + /** 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::vector& points_G, int nPoints = 0) const; bool isActive(const State& state) const From 1e01a35e9cc0aef5918222250919254a7c482f94 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 09:47:54 +0200 Subject: [PATCH 094/127] implement function for getting the integrator step size --- Simbody/src/Wrapping.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 32338a8d0..34da62d70 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -97,7 +97,7 @@ int CurveSegment::getNumberOfIntegratorStepsTaken(const State& state) Real CurveSegment::getInitialIntegratorStepSize(const State& state) { getImpl().realizeCablePosition(state); - throw std::runtime_error("NOTYETIMPLEMENTED"); + return getImpl().getInstanceEntry(state).sHint; } void CurveSegment::calcUnitForce(const State& state, SpatialVec& unitForce_G) const From 5056f23fd94abe41aaca4b84b24316781455bd0d Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 09:49:23 +0200 Subject: [PATCH 095/127] fix calcCablePower --- Simbody/src/Wrapping.cpp | 26 ++++++++---- Simbody/src/WrappingImpl.h | 83 ++++++++++++++++++++++++++++++++++++-- 2 files changed, 98 insertions(+), 11 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 34da62d70..1fb5e9087 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -176,7 +176,7 @@ int CableSpan::calcPoints(const State& state, std::vector& points_G, int n Real CableSpan::calcCablePower(const State& state, Real tension) const { - throw std::runtime_error("NOTYETIMPLEMENTED"); + return getImpl().calcCablePower(state, tension); } //============================================================================== @@ -1180,21 +1180,21 @@ const CurveSegment* CableSpan::Impl::findNextActiveCurveSegment( return nullptr; } -Vec3 CableSpan::Impl::findPrevPoint(const State& s, const CurveSegment& curve) +Vec3 CableSpan::Impl::findPrevPoint(const State& s, CurveSegmentIndex ix) const { const CurveSegment* segment = - findPrevActiveCurveSegment(s, curve.getImpl().getIndex()); + findPrevActiveCurveSegment(s, ix); return segment ? segment->getImpl().calcFinalContactPoint(s) : m_OriginBody.getBodyTransform(s).shiftFrameStationToBase( m_OriginPoint); } -Vec3 CableSpan::Impl::findNextPoint(const State& s, const CurveSegment& curve) +Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) const { const CurveSegment* segment = - findNextActiveCurveSegment(s, curve.getImpl().getIndex()); + findNextActiveCurveSegment(s, ix); return segment ? segment->getImpl().calcInitialContactPoint(s) : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( @@ -1598,12 +1598,19 @@ void CableSpan::Impl::applyBodyForces( Real tension, Vector_& bodyForcesInG) const { - realizePosition(s); if (tension < 0.) { - throw std::runtime_error("Cable tension can not go below zero."); + // TODO throw? or skip? + throw std::runtime_error("Cable tension should be nonnegative."); } + realizePosition(s); 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; @@ -1612,6 +1619,11 @@ void CableSpan::Impl::applyBodyForces( 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( diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index d3d3861c6..5ce7f0449 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -56,7 +56,7 @@ class CurveSegment::Impl Variation dK_Q{}; std::vector samples; - double sHint = NaN; + Real sHint = NaN; Vec3 trackingPointOnLine{NaN, NaN, NaN}; Status status = Status::Lifted; @@ -604,6 +604,36 @@ class CableSpan::Impl return count + 2; } + Real calcCablePower( + const State& state, Real tension) const + { + SpatialVec unitForce; + + Real unitPower = 0.; + + { + calcUnitForceAtOrigin(state, unitForce); + SpatialVec v = m_OriginBody.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 = m_TerminationBody.getBodyVelocity(state); + unitPower += ~unitForce * v; + } + + return unitPower * tension; + } + void calcPathErrorJacobianUtility( const State& state, Vector& pathError, @@ -620,9 +650,17 @@ class CableSpan::Impl void calcPosInfo(const State& s, PosInfo& posInfo) const; void calcVelInfo(const State& s, VelInfo& velInfo) const; - Vec3 findPrevPoint(const State& state, const CurveSegment& curve) 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, const CurveSegment& curve) const; + 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, @@ -652,10 +690,47 @@ class CableSpan::Impl Vec3 p_I, std::vector& lines) const; - double calcPathLength( + Real calcPathLength( const State& state, const std::vector& lines) const; + const Mobod& getOriginBody() const {return m_OriginBody;} + const Mobod& getTerminationBody() const {return m_TerminationBody;} + + 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; + + // Tangent direction at origin contact point in ground. + // TODO fix finding first active segment. + const UnitVec3& t = UnitVec3(findNextPoint(s, CurveSegmentIndex(-1)) - 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; + + // Tangent directions at termination contact point in ground. + // TODO fix finding last active segment. + const UnitVec3& t = UnitVec3( + posInfo.xI - + findNextPoint(s, CurveSegmentIndex(getNumCurveSegments()))); + + unitForce_G[0] = r % t; + unitForce_G[1] = Vec3(t); + } + const CableSubsystem& getSubsystem() const { return *m_Subsystem; From df7b425b511b0f3b6a3765a9c9bd62c552bc8345 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 10:07:07 +0200 Subject: [PATCH 096/127] cleanup & fmt --- Simbody/include/simbody/internal/Wrapping.h | 15 +- Simbody/src/Wrapping.cpp | 10 + Simbody/src/WrappingImpl.h | 257 ++++++++++++-------- 3 files changed, 184 insertions(+), 98 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 7768f913d..c2503b1b2 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -231,13 +231,26 @@ class SimTK_SIMBODY_EXPORT CableSpan final //------------------------------------------------------------------------------ // State dependent calculations //------------------------------------------------------------------------------ - Real calcCablePower(const State& state, Real tension) const; + + /** 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::vector& points_G, int nPointsPerCurveSegment = 0) const; diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 1fb5e9087..27d7fb3fd 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -174,6 +174,16 @@ int CableSpan::calcPoints(const State& state, std::vector& points_G, int n return getImpl().calcPathPoints(state, points_G, 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); diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 5ce7f0449..1661df073 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -78,9 +78,9 @@ class CurveSegment::Impl //------------------------------------------------------------------------------ public: - Impl() = delete; - Impl(const Impl&) = delete; - Impl& operator=(const Impl&) = delete; + Impl() = delete; + Impl(const Impl&) = delete; + Impl& operator=(const Impl&) = delete; ~Impl() = default; Impl(Impl&&) noexcept = default; @@ -125,6 +125,7 @@ class CurveSegment::Impl 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"); } @@ -146,7 +147,10 @@ class CurveSegment::Impl // Grab the last geodesic that was computed. assertSurfaceBounds(prevPoint_S, nextPoint_S); - calcTouchdownIfNeeded(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); @@ -156,7 +160,7 @@ class CurveSegment::Impl const InstanceEntry& ie = getInstanceEntry(s); // Start updating the position level cache. - PosInfo& pos = updPosInfo(s); + PosInfo& pos = updPosInfo(s); // Transform geodesic in local surface coordinates to ground. { @@ -215,17 +219,38 @@ class CurveSegment::Impl return m_ContactPointHint_S; } - const ContactGeometry& getContactGeometry() const {return m_Geometry;} + const ContactGeometry& getContactGeometry() const + { + return m_Geometry; + } - const DecorativeGeometry& getDecoration() const { return m_Decoration; } + const DecorativeGeometry& getDecoration() const + { + return m_Decoration; + } - const MobilizedBody& getMobilizedBody() const {return m_Mobod;} + const MobilizedBody& getMobilizedBody() const + { + return m_Mobod; + } - const Transform& getXformSurfaceToBody() const {return m_X_BS;} - void setXformSurfaceToBody(Transform X_BS) {m_X_BS = std::move(X_BS);} + 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; } + const CableSubsystem& getSubsystem() const + { + return *m_Subsystem; + } + CableSubsystem& updSubsystem() + { + return *m_Subsystem; + } //------------------------------------------------------------------------------ // Cache entry access @@ -252,12 +277,13 @@ class CurveSegment::Impl return m_Mobod.getBodyTransform(s).compose(m_X_BS); } - int calcPathPoints(const State& s, std::vector& points, int nSamples) const; + int calcPathPoints(const State& s, std::vector& points, int nSamples) + const; void calcUnitForce(const State& s, SpatialVec& unitForce_G) const { const PosInfo& posInfo = getPosInfo(s); - const Vec3& x_BG = m_Mobod.getBodyOriginLocation(s); + const Vec3& x_BG = m_Mobod.getBodyOriginLocation(s); // Contact point moment arms in ground. const Vec3 r_P = posInfo.KP.p() - x_BG; @@ -311,49 +337,51 @@ class CurveSegment::Impl getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); // TODO Below code should be moved to a unit test... - const bool DO_UNIT_TEST_HERE = true; - if (DO_UNIT_TEST_HERE) - { + const bool DO_UNIT_TEST_HERE = false; + if (DO_UNIT_TEST_HERE) { const Real delta = c.norm(); const Real eps = delta / 10.; auto AssertAxis = [&](const Rotation& R0, - const Rotation& R1, - const Vec3& w, - CoordinateAxis axis) -> bool { + const Rotation& R1, + const Vec3& w, + CoordinateAxis axis) -> bool { const UnitVec3 a0 = R0.getAxisUnitVec(axis); const UnitVec3 a1 = R1.getAxisUnitVec(axis); const Vec3 expected_diff = cross(w, a0); const Vec3 got_diff = a1 - a0; - const bool isOk = (expected_diff - got_diff).norm() < eps; + const bool isOk = (expected_diff - got_diff).norm() < eps; if (!isOk) { std::cout << " a0 = " << R0.transpose() * a0 << "\n"; std::cout << " a1 = " << R0.transpose() * a1 << "\n"; std::cout << " expected diff = " - << R0.transpose() * expected_diff / delta << "\n"; + << R0.transpose() * expected_diff / delta << "\n"; std::cout << " got dt_Q = " - << R0.transpose() * got_diff / delta << "\n"; + << R0.transpose() * got_diff / delta << "\n"; std::cout << " err = " - << (expected_diff - got_diff).norm() / delta << "\n"; + << (expected_diff - got_diff).norm() / delta + << "\n"; } return isOk; }; auto AssertFrame = [&](const Transform& K0, - const Transform& K1, - const Variation& dK0) -> bool { + const Transform& K1, + const Variation& dK0) -> bool { const Vec3 dx_got = K1.p() - K0.p(); const Vec3 dx_expected = dK0[1] * c; bool isOk = (dx_expected - dx_got).norm() < eps; if (!isOk) { - std::cout << "Apply variation c = " << c << "\n"; + std::cout << "Apply variation c = " << c + << ".norm() = " << delta << "\n"; std::cout << " x0 = " << K0.p() << "\n"; std::cout << " x1 = " << K1.p() << "\n"; std::cout << " expected dx_Q = " - << K0.R().transpose() * dx_expected / delta << "\n"; + << K0.R().transpose() * dx_expected / delta + << "\n"; std::cout << " got dx_Q = " - << K0.R().transpose() * dx_got / delta << "\n"; + << K0.R().transpose() * dx_got / delta << "\n"; std::cout << " err = " - << (dx_expected - dx_got).norm() / delta << "\n"; + << (dx_expected - dx_got).norm() / delta << "\n"; std::cout << "WARNING: Large deviation in final position\n"; } const Vec3 w = dK0[0] * c; @@ -373,10 +401,14 @@ class CurveSegment::Impl if (delta > 1e-10) { if (!AssertFrame(K0_P, getInstanceEntry(s).K_P, dK0_P)) { - throw std::runtime_error("Start frame variation check failed"); + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Start frame variation check failed"); } if (!AssertFrame(K0_Q, getInstanceEntry(s).K_Q, dK0_Q)) { - throw std::runtime_error("End frame variation check failed"); + // TODO use SimTK_ASSERT + throw std::runtime_error( + "End frame variation check failed"); } } } @@ -389,6 +421,7 @@ class CurveSegment::Impl { const InstanceEntry& ic = getInstanceEntry(s); if (!ic.isActive()) { + // TODO use SimTK_ASSERT throw std::runtime_error( "Invalid contact point: Curve is not active"); } @@ -400,6 +433,7 @@ class CurveSegment::Impl { const InstanceEntry& ic = getInstanceEntry(s); if (!ic.isActive()) { + // TODO use SimTK_ASSERT throw std::runtime_error( "Invalid contact point: Curve is not active"); } @@ -412,10 +446,12 @@ class CurveSegment::Impl Array_& decorations) const { const InstanceEntry& cache = getInstanceEntry(s); - if (!cache.isActive()) {return;} + if (!cache.isActive()) { + return; + } - const Transform& X_GS = calcSurfaceFrameInGround(s); - Vec3 a = X_GS.shiftFrameStationToBase(cache.K_P.p()); + const Transform& X_GS = calcSurfaceFrameInGround(s); + Vec3 a = X_GS.shiftFrameStationToBase(cache.K_P.p()); for (size_t i = 1; i < cache.samples.size(); ++i) { const Vec3 b = X_GS.shiftFrameStationToBase(cache.samples.at(i).frame.p()); @@ -426,7 +462,6 @@ class CurveSegment::Impl } private: - InstanceEntry& updInstanceEntry(const State& state) const { return Value::updDowncast( @@ -499,11 +534,14 @@ class CurveSegment::Impl Vec3 m_ContactPointHint_S{NaN, NaN, NaN}; + // TODO expose getters and setters. size_t m_ProjectionMaxIter = 10; 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; size_t m_TouchdownIter = 10; }; @@ -585,7 +623,10 @@ class CableSpan::Impl Stage stage, Array_& decorations) const; - int calcPathPoints(const State& state, std::vector& points_G, int nPointsPerCurveSegment) const + int calcPathPoints( + const State& state, + std::vector& points_G, + int nPointsPerCurveSegment) const { // Write the initial point. const PosInfo& pos = getPosInfo(state); @@ -594,7 +635,10 @@ class CableSpan::Impl // 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, points_G, nPointsPerCurveSegment); + count += curve.getImpl().calcPathPoints( + state, + points_G, + nPointsPerCurveSegment); } // Write the termination point. @@ -604,9 +648,48 @@ class CableSpan::Impl return count + 2; } - Real calcCablePower( - const State& state, Real tension) const + 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; + + // Tangent direction at origin contact point in ground. + // TODO fix finding first active segment. + const UnitVec3& t = + UnitVec3(findNextPoint(s, CurveSegmentIndex(-1)) - 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; + + // Tangent directions at termination contact point in ground. + // TODO fix finding last active segment. + const UnitVec3& t = UnitVec3( + posInfo.xI - + findNextPoint(s, CurveSegmentIndex(getNumCurveSegments()))); + + 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.; @@ -617,8 +700,10 @@ class CableSpan::Impl unitPower += ~unitForce * v; } - for (const CurveSegment& curve: m_CurveSegments) { - if (!curve.isActive(state)) {continue;} + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.isActive(state)) { + continue; + } curve.calcUnitForce(state, unitForce); SpatialVec v = curve.getMobilizedBody().getBodyVelocity(state); @@ -694,41 +779,13 @@ class CableSpan::Impl const State& state, const std::vector& lines) const; - const Mobod& getOriginBody() const {return m_OriginBody;} - const Mobod& getTerminationBody() const {return m_TerminationBody;} - - void calcUnitForceAtOrigin(const State& s, SpatialVec& unitForce_G) const + const Mobod& getOriginBody() 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; - - // Tangent direction at origin contact point in ground. - // TODO fix finding first active segment. - const UnitVec3& t = UnitVec3(findNextPoint(s, CurveSegmentIndex(-1)) - posInfo.xO); - - unitForce_G[0] = - r % t; - unitForce_G[1] = - Vec3(t); + return m_OriginBody; } - - void calcUnitForceAtTermination(const State& s, SpatialVec& unitForce_G) const + const Mobod& getTerminationBody() 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; - - // Tangent directions at termination contact point in ground. - // TODO fix finding last active segment. - const UnitVec3& t = UnitVec3( - posInfo.xI - - findNextPoint(s, CurveSegmentIndex(getNumCurveSegments()))); - - unitForce_G[0] = r % t; - unitForce_G[1] = Vec3(t); + return m_TerminationBody; } const CableSubsystem& getSubsystem() const @@ -752,6 +809,7 @@ class CableSpan::Impl Array_ m_CurveSegments{}; + // TODO expose getters and setters. Real m_PathErrorBound = 1e-4; size_t m_PathMaxIter = 50; @@ -768,6 +826,9 @@ class CableSpan::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) @@ -790,7 +851,7 @@ class CableSubsystem::Impl : public Subsystem::Guts Vector pathCorrection; Vector pathError; Matrix mat; - // TODO Cholesky decomposition... + // TODO Cholesky decomposition would be more efficient. FactorLU matInv; Vector vec; }; @@ -801,6 +862,7 @@ class CableSubsystem::Impl : public Subsystem::Guts SolverData& updOrInsert(int nActive) { if (nActive <= 0) { + // TODO use SimTK_ASSERT throw std::runtime_error( "Cannot produce solver data of zero dimension"); } @@ -819,10 +881,6 @@ class CableSubsystem::Impl : public Subsystem::Guts {} ~Impl() {} - Impl* cloneImpl() const override - { - return new Impl(*this); - } int getNumPaths() const { @@ -859,22 +917,6 @@ class CableSubsystem::Impl : public Subsystem::Guts return getMultibodySystem().getMatterSubsystem(); } - // 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; - } - void realizeTopology(State& state) { CacheEntry cache{}; @@ -890,6 +932,14 @@ class CableSubsystem::Impl : public Subsystem::Guts 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, @@ -910,9 +960,22 @@ class CableSubsystem::Impl : public Subsystem::Guts return 0; } - SimTK_DOWNCAST(Impl, Subsystem::Guts); + // 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; + } -private: // TOPOLOGY STATE Array_ cables; From 01db1e51a2eb2e754c67550b79c96191fd0de180 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 10:14:29 +0200 Subject: [PATCH 097/127] replace double by Real --- Simbody/src/Wrapping.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 27d7fb3fd..e7dc20ca3 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -326,10 +326,10 @@ bool calcNearestPointOnLineImplicitly( Vec3 b, Vec3& point, size_t maxIter, - double eps) + Real eps) { // Initial guess. - double alpha = calcPointOnLineNearPointAsFactor(a, b, point); + Real alpha = calcPointOnLineNearPointAsFactor(a, b, point); size_t iter = 0; for (; iter < maxIter; ++iter) { @@ -338,7 +338,7 @@ bool calcNearestPointOnLineImplicitly( const Vec3 pl = a + (b - a) * alpha; // Constraint evaluation at touchdown point. - const double c = calcSurfaceConstraintValue(geometry, pl); + const Real c = calcSurfaceConstraintValue(geometry, pl); // Break on touchdown, TODO or not? if (std::abs(c) < eps) @@ -349,17 +349,17 @@ bool calcNearestPointOnLineImplicitly( const Mat33 H = calcSurfaceConstraintHessian(geometry, pl); // Add a weight to the newton step to avoid large steps. - constexpr double w = 0.5; + constexpr Real w = 0.5; // Update alpha. - const double step = dot(g, d) / (dot(d, H * d) + w); + const Real step = dot(g, d) / (dot(d, H * d) + w); // Stop when converged. if (std::abs(step) < eps) break; // Clamp the stepsize. - constexpr double maxStep = 0.25; + constexpr Real maxStep = 0.25; alpha -= std::min(std::max(-maxStep, step), maxStep); // Stop when leaving bounds. From 56810aa92ea107e76ffdac0869024f3225908737 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 10:31:24 +0200 Subject: [PATCH 098/127] fix touchdown initialization --- Simbody/src/Wrapping.cpp | 28 +++++++++------------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index e7dc20ca3..2a13bbf56 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -308,18 +308,6 @@ void calcSurfaceProjectionFast( t = t / norm; } -Real calcPointOnLineNearOriginAsFactor(Vec3 a, Vec3 b) -{ - const Vec3 e = b - a; - Real c = -dot(a, e) / dot(e, e); - return std::max(0., std::min(1., c)); -}; - -Real calcPointOnLineNearPointAsFactor(Vec3 a, Vec3 b, Vec3 point) -{ - return calcPointOnLineNearOriginAsFactor(a - point, b - point); -}; - bool calcNearestPointOnLineImplicitly( const ContactGeometry& geometry, Vec3 a, @@ -329,13 +317,15 @@ bool calcNearestPointOnLineImplicitly( Real eps) { // Initial guess. - Real alpha = calcPointOnLineNearPointAsFactor(a, b, point); + 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 d = b - a; - const Vec3 pl = a + (b - a) * alpha; + const Vec3 pl = a + d * alpha; // Constraint evaluation at touchdown point. const Real c = calcSurfaceConstraintValue(geometry, pl); @@ -368,8 +358,8 @@ bool calcNearestPointOnLineImplicitly( } // Write the point on line nearest the surface. - alpha = std::max(std::min(1., alpha), 0.); - point = a + (b - a) * alpha; + 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; @@ -948,7 +938,7 @@ void CurveSegment::Impl::calcTouchdownIfNeeded( const Vec3& nextPoint_S, CurveSegment::Impl::InstanceEntry& cache) const { - // Only attempt touchdown when liftoff. + // Only attempt touchdown when lifted. LocalGeodesicInfo& g = cache; if (g.status != Status::Lifted) { return; @@ -967,7 +957,7 @@ void CurveSegment::Impl::calcTouchdownIfNeeded( return; } - // Touchdown detected: Remove the liftoff status flag. + // Touchdown detected: Remove the Lifted status flag. g.status = Status::Ok; // Shoot a zero length geodesic at the touchdown point. shootNewGeodesic( From d1fd9346385cd735f91c181d18858616cdf9aefa Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 10:35:46 +0200 Subject: [PATCH 099/127] fmt --- Simbody/src/Wrapping.cpp | 324 +++++++++++++++++++++++---------------- 1 file changed, 194 insertions(+), 130 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 2a13bbf56..d4f322fb6 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -100,13 +100,17 @@ Real CurveSegment::getInitialIntegratorStepSize(const State& state) return getImpl().getInstanceEntry(state).sHint; } -void CurveSegment::calcUnitForce(const State& state, SpatialVec& unitForce_G) const +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::vector& points_G, int nPoints) const +int CurveSegment::calcPoints( + const State& state, + std::vector& points_G, + int nPoints) const { getImpl().realizeCablePosition(state); return getImpl().calcPathPoints(state, points_G, nPoints); @@ -169,17 +173,24 @@ void CableSpan::applyBodyForces( return getImpl().applyBodyForces(s, tension, bodyForcesInG); } -int CableSpan::calcPoints(const State& state, std::vector& points_G, int nPointsPerCurveSegment) const +int CableSpan::calcPoints( + const State& state, + std::vector& points_G, + int nPointsPerCurveSegment) const { return getImpl().calcPathPoints(state, points_G, nPointsPerCurveSegment); } -void CableSpan::calcUnitForceAtOrigin(const State& state, SpatialVec& unitForce_G) const +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 +void CableSpan::calcUnitForceAtTermination( + const State& state, + SpatialVec& unitForce_G) const { getImpl().calcUnitForceAtTermination(state, unitForce_G); } @@ -287,11 +298,13 @@ void calcSurfaceProjectionFast( } 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"); } @@ -300,8 +313,10 @@ void calcSurfaceProjectionFast( 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) + // TODO use SimTK_ASSERT throw std::runtime_error("Surface projection failed: Tangent guess is " "parallel to surface normal"); @@ -318,10 +333,10 @@ bool calcNearestPointOnLineImplicitly( { // Initial guess. const Vec3 d = b - a; - Real alpha = -dot(d, a - point) / dot(d,d); - alpha = std::max(0., std::min(alpha, 1.)); + Real alpha = -dot(d, a - point) / dot(d, d); + alpha = std::max(0., std::min(alpha, 1.)); - size_t iter = 0; + size_t iter = 0; for (; iter < maxIter; ++iter) { // Touchdown point on line. @@ -366,10 +381,6 @@ bool calcNearestPointOnLineImplicitly( // TODO handle here? if (iter >= maxIter) { - std::cout << "a = " << a << "\n"; - std::cout << "b = " << b << "\n"; - std::cout << "p = " << point << "\n"; - std::cout << "c = " << alpha << "\n"; // TODO use SimTK_ASSERT throw std::runtime_error("Failed to compute point on line nearest " "surface: Reached max iterations"); @@ -548,6 +559,7 @@ RKM::Y RKM::stepTo( _e = std::max(_e, err); if (_h < _hMin) { + // TODO use SimTK_ASSERT throw std::runtime_error( "Geodesic Integrator failed: Reached very small stepsize"); } @@ -557,6 +569,7 @@ RKM::Y RKM::stepTo( } } if (std::abs(x - x1) > 1e-13) { + // TODO use SimTK_ASSERT throw std::runtime_error("failed to integrate"); } return _y.at(0); @@ -643,6 +656,7 @@ Real calcGaussianCurvature(const ContactGeometry& geometry, Vec3 point) 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?"); } @@ -745,112 +759,150 @@ void calcGeodesicAndVariationImplicitly( 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( + 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::vector& interpolatedSamples) +{ + // Some sanity checks. + if (geodesic.empty()) { + // TODO use SimTK_ASSERT + throw std::runtime_error( + "Resampling of geodesic failed: Provided geodesic is empty."); } - - 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); + 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"); } - size_t calcResampledGeodesicPoints(const std::vector& geodesic, const Transform& X_GS, int nSamples, std::vector& interpolatedSamples) - { - // Some sanity checks. - if (geodesic.empty()) { - throw std::runtime_error("Resampling of geodesic failed: Provided geodesic is empty."); - } - if (geodesic.front().length != 0.) { - throw std::runtime_error("Resampling of geodesic failed: First frame must be at length = zero"); - } - if (geodesic.front().length < 0.) { - throw std::runtime_error("Resampling of geodesic failed: Last frame must be at length > zero"); - } - if (nSamples == 1 && geodesic.size() != 1) { - throw std::runtime_error("Resampling of geodesic failed: Requested number of samples must be unequal to 1"); - } + // Capture the start of the geodesic. + interpolatedSamples.push_back( + X_GS.shiftFrameStationToBase(geodesic.front().frame.p())); - // Capture the start of the geodesic. - interpolatedSamples.push_back( - 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; + } - // 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"); + } - // 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()) { - 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) { - 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. - interpolatedSamples.push_back(point_G); - - break; + // 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."); } - } - // Capture the last point of the geodesic. - interpolatedSamples.push_back(X_GS.shiftFrameStationToBase(geodesic.back().frame.p())); + // 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. + interpolatedSamples.push_back(point_G); - return nSamples; + break; + } } + + // Capture the last point of the geodesic. + interpolatedSamples.push_back( + X_GS.shiftFrameStationToBase(geodesic.back().frame.p())); + + return nSamples; } +} // namespace -int CurveSegment::Impl::calcPathPoints(const State& s, std::vector& points, int nSamples) const +int CurveSegment::Impl::calcPathPoints( + const State& s, + std::vector& points, + int nSamples) const { const Transform& X_GS = getPosInfo(s).X_GS; const InstanceEntry& geodesic_S = getInstanceEntry(s); @@ -858,15 +910,21 @@ int CurveSegment::Impl::calcPathPoints(const State& s, std::vector& points return 0; } - // Do not do any resampling if nSamples==0, simply write the points from the integrator to the output buffer. + // 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) { points.push_back(X_GS.shiftFrameStationToBase(sample.frame.p())); } } - // Resample the points from the integrator by interpolating at equal intervals. - return calcResampledGeodesicPoints(geodesic_S.samples, X_GS, nSamples, points); + // Resample the points from the integrator by interpolating at equal + // intervals. + return calcResampledGeodesicPoints( + geodesic_S.samples, + X_GS, + nSamples, + points); } //============================================================================== @@ -921,9 +979,11 @@ void CurveSegment::Impl::calcLiftoffIfNeeded( // 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 liftoff. + 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; } @@ -974,10 +1034,12 @@ void CurveSegment::Impl::assertSurfaceBounds( { // 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"); } @@ -1180,21 +1242,17 @@ const CurveSegment* CableSpan::Impl::findNextActiveCurveSegment( return nullptr; } -Vec3 CableSpan::Impl::findPrevPoint(const State& s, CurveSegmentIndex ix) - const +Vec3 CableSpan::Impl::findPrevPoint(const State& s, CurveSegmentIndex ix) const { - const CurveSegment* segment = - findPrevActiveCurveSegment(s, ix); + const CurveSegment* segment = findPrevActiveCurveSegment(s, ix); return segment ? segment->getImpl().calcFinalContactPoint(s) : m_OriginBody.getBodyTransform(s).shiftFrameStationToBase( m_OriginPoint); } -Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) - const +Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) const { - const CurveSegment* segment = - findNextActiveCurveSegment(s, ix); + const CurveSegment* segment = findNextActiveCurveSegment(s, ix); return segment ? segment->getImpl().calcInitialContactPoint(s) : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( @@ -1210,7 +1268,7 @@ void CableSpan::Impl::calcPathErrorVector( { size_t lineIx = 0; ptrdiff_t row = -1; - pathError *= 0; + pathError *= 0; for (const CurveSegment& segment : m_CurveSegments) { if (!segment.getImpl().getInstanceEntry(s).isActive()) { @@ -1239,7 +1297,7 @@ void CableSpan::Impl::calcPathErrorJacobian( // TODO perhaps just not make method static. const size_t n = lines.size() - 1; - J *= 0.; + J *= 0.; SimTK_ASSERT( J.rows() == n * N, @@ -1383,6 +1441,7 @@ 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"); } @@ -1536,15 +1595,14 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const } } + // TODO use SimTK_ASSERT throw std::runtime_error("Failed to converge"); } void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const { - auto CalcPointVelocityInGround = [&]( - const MobilizedBody& mobod, - const Vec3& point_G) -> Vec3 - { + 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). @@ -1570,13 +1628,13 @@ void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const 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 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()); + v_GQ = CalcPointVelocityInGround(mobod, g.KQ.p()); lastActive = &curve; } @@ -1617,12 +1675,18 @@ void CableSpan::Impl::applyBodyForces( } curve.calcUnitForce(s, unitForce_G); - curve.getMobilizedBody().applyBodyForce(s, unitForce_G * tension, bodyForcesInG); + curve.getMobilizedBody().applyBodyForce( + s, + unitForce_G * tension, + bodyForcesInG); } { calcUnitForceAtTermination(s, unitForce_G); - getTerminationBody().applyBodyForce(s, unitForce_G * tension, bodyForcesInG); + getTerminationBody().applyBodyForce( + s, + unitForce_G * tension, + bodyForcesInG); } } From 4e17edc00c0c084c934bf9fc590ac646f7922fb3 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 11:25:35 +0200 Subject: [PATCH 100/127] cleanup --- Simbody/src/Wrapping.cpp | 64 ++++++++++++++++++----------- Simbody/src/WrappingImpl.h | 2 +- examples/ExampleCableSpanSimple.cpp | 16 +++++--- 3 files changed, 51 insertions(+), 31 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index d4f322fb6..51b593b23 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1363,7 +1363,7 @@ void CableSpan::Impl::calcPathErrorJacobian( }; } -Real CableSpan::Impl::calcPathLength( +Real CableSpan::Impl::calcCableLength( const State& s, const std::vector& lines) const { @@ -1528,11 +1528,14 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const posInfo.xO = x_O; posInfo.xI = x_I; + // Axes considered when computing the path error. const std::array axes{NormalAxis, BinormalAxis}; - for (posInfo.loopIter = 0; posInfo.loopIter < m_PathMaxIter; - ++posInfo.loopIter) { - + posInfo.loopIter = 0; + 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, @@ -1540,48 +1543,55 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const 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(); - return; + break; } - // Grab the shared data cache for computing the matrices, and lock it. + // 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. + // Compute the straight-line segments of this cable span. Note that + // there is one more straight line segment, than there are active curve + // segments. calcLineSegments(s, x_O, x_I, data.lineSegments); - // Evaluate path error, and stop when converged. + // Evaluate 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(); - if (maxPathError < m_PathErrorBound || - (posInfo.loopIter + 1) >= m_PathMaxIter) { - posInfo.l = 0.; - for (const LineSegment& line : data.lineSegments) { - posInfo.l += line.l; - } - for (const CurveSegment& curve : m_CurveSegments) { - posInfo.l += curve.getImpl().getInstanceEntry(s).length; - } - return; + + // Stop iterating if max path error is small, or max iterations is reached. + if (maxPathError < m_PathErrorBound || posInfo.loopIter >= m_PathMaxIter) { + posInfo.l = calcCableLength(s, data.lineSegments); + break; } - // Evaluate the path error jacobian. + // Evaluate the path error jacobian to the natural geodesic corrections + // of each curve segment. calcPathErrorJacobian<2>( s, data.lineSegments, axes, data.pathErrorJacobian); - // Compute path corrections. + // Compute the geodesic corrections for each curve segment. const Correction* corrIt = calcPathCorrections(data); - // Apply path corrections. + // Apply corrections to the curve segments. for (const CurveSegment& curve : m_CurveSegments) { + // The corrections were computed for the active segments: Skip any + // non-active here. if (!curve.getImpl().getInstanceEntry(s).isActive()) { continue; } @@ -1589,14 +1599,20 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const ++corrIt; } - // Path has changed: invalidate each segment's cache. + // Applying the corrections changes 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 use SimTK_ASSERT - throw std::runtime_error("Failed to converge"); + // 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 diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 1661df073..d98543a81 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -775,7 +775,7 @@ class CableSpan::Impl Vec3 p_I, std::vector& lines) const; - Real calcPathLength( + Real calcCableLength( const State& state, const std::vector& lines) const; diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp index 6290eef75..f031bd6e9 100644 --- a/examples/ExampleCableSpanSimple.cpp +++ b/examples/ExampleCableSpanSimple.cpp @@ -315,9 +315,9 @@ int main() Vec3(10., 0.0, 0.1)); Body::Rigid ballBody(MassProperties(1.0, Vec3(0), Inertia(1))); - const Real Rad = 2.0; + const Real Rad = 1.0; - Vec3 offset = {0., 0., 0.}; + Vec3 offset = {-1., 0., 0.}; Vec3 arm = {0.25, 0., 0.}; /* UnitVec3 axis0(Vec3{1., 1., 1.}); */ @@ -338,7 +338,7 @@ int main() {0., 1., 0.}); Body::Rigid ball2Body(MassProperties(1.0, Vec3(0), Inertia(1))); - const Real Rad2 = 1.1; + const Real Rad2 = 1.5; Vec3 offset2 = {5., 0., 0.}; Vec3 arm2 = {0.25, 0., 0.}; @@ -352,7 +352,8 @@ int main() path1.adoptWrappingObstacle( ball2, Transform(), - ContactGeometry::Sphere(Rad2), + ContactGeometry::Ellipsoid({Rad2, Rad2 * 1.2, Rad2 * 0.9}), + /* ContactGeometry::Sphere(Rad2), */ {0., 1., 0.}); MyCableSpring cable1(forces, path1, 100., 3.5, 0.1); @@ -368,6 +369,7 @@ int main() Real v = 0.; bool continuous = false; Random::Gaussian random; + Real phi = 0.; while (true) { system.realize(s, Stage::Position); viz.report(s); @@ -393,13 +395,15 @@ int main() v = std::min(1e-1, v); } + phi += 0.01; for (int i = 0; i < s.getNQ(); ++i) { /* Random::Gaussian random; */ - s.updQ()[i] += random.getValue() * 1e-1 + v; + s.updQ()[i] = sin(phi * static_cast(i) + random.getValue()*1e-3); + std::cout << " q = " << s.getQ()[i] << "\n"; } for (int i = 0; i < s.getNU(); ++i) { /* Random::Gaussian random; */ - s.updU()[i] += random.getValue() * 1e-3; + s.updU()[i] = sin(phi + random.getValue()*1e-3); } if (continuous) { From b816517dbc5696daf18910eb0c3a8ae3cbdc6cf7 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 13:05:13 +0200 Subject: [PATCH 101/127] show straight line decoration when no obstacles are active --- Simbody/src/Wrapping.cpp | 8 +++++--- examples/ExampleCableSpanSimple.cpp | 5 ----- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 51b593b23..752f6ab7c 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1719,9 +1719,11 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( decorations.push_back(DecorativePoint(ppe.xO).setColor(Green)); decorations.push_back(DecorativePoint(ppe.xI).setColor(Red)); - /* decorations.push_back(DecorativeLine(ppe.xO, ppe.xI) */ - /* .setColor(Purple) */ - /* .setLineThickness(3)); */ + if (countActive(s) == 0) { + decorations.push_back(DecorativeLine(ppe.xO, ppe.xI) + .setColor(Purple) + .setLineThickness(3)); + } for (const CurveSegment& curveSegment : m_CurveSegments) { const CurveSegment::Impl& curve = curveSegment.getImpl(); diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp index f031bd6e9..f3897b87b 100644 --- a/examples/ExampleCableSpanSimple.cpp +++ b/examples/ExampleCableSpanSimple.cpp @@ -399,11 +399,6 @@ int main() for (int i = 0; i < s.getNQ(); ++i) { /* Random::Gaussian random; */ s.updQ()[i] = sin(phi * static_cast(i) + random.getValue()*1e-3); - std::cout << " q = " << s.getQ()[i] << "\n"; - } - for (int i = 0; i < s.getNU(); ++i) { - /* Random::Gaussian random; */ - s.updU()[i] = sin(phi + random.getValue()*1e-3); } if (continuous) { From 33b08255f3ee61275406dbdce7c3e10bb8836037 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 13:06:51 +0200 Subject: [PATCH 102/127] add some TODO's --- Simbody/src/Wrapping.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 752f6ab7c..74d7a21a1 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1711,6 +1711,7 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( Stage stage, Array_& decorations) const { + // TODO clean this up. (also, should decorations be done here?) const Vec3 color = Green; // Red Purple const PosInfo& ppe = getPosInfo(s); @@ -1750,6 +1751,7 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( .setLineThickness(2)); } + // TODO this is for debugging: Draw the Frenet frames Transform K_P = curve.getPosInfo(s).KP; Transform K_Q = curve.getPosInfo(s).KQ; const std::array axes = { From d02d7c9147b17dc7367370122258b0207cc9c764 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 17:54:32 +0200 Subject: [PATCH 103/127] add asserting the solver error --- Simbody/src/Wrapping.cpp | 7 ++++++- Simbody/src/WrappingImpl.h | 6 ++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 74d7a21a1..80af21186 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -940,12 +940,17 @@ const Correction* calcPathCorrections(SolverData& data) data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; for (int i = 0; i < data.mat.nrow(); ++i) { - data.mat[i][i] += w + 1e-3; + data.mat[i][i] += w + 1e-6; } data.matInv = data.mat; data.vec = data.pathErrorJacobian.transpose() * (data.pathError * (-1.)); data.matInv.solve(data.vec, data.pathCorrection); + data.err = data.mat * data.pathCorrection - data.vec; + if (data.err.normInf() > 1e-10) { + throw std::runtime_error("Failed to invert matrix"); + } + static_assert( sizeof(Correction) == sizeof(Real) * GeodesicDOF, "Invalid size of corrections vector"); diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index d98543a81..af7f0f421 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -810,8 +810,8 @@ class CableSpan::Impl Array_ m_CurveSegments{}; // TODO expose getters and setters. - Real m_PathErrorBound = 1e-4; - size_t m_PathMaxIter = 50; + Real m_PathErrorBound = 1e-6; + size_t m_PathMaxIter = 2; // TODO set to something reasonable. // TOPOLOGY CACHE (set during realizeTopology()) CacheEntryIndex m_PosInfoIx; @@ -843,6 +843,7 @@ class CableSubsystem::Impl : public Subsystem::Guts pathError = Vector(C * n, 0.); mat = Matrix(Q * n, Q * n, NaN); vec = Vector(Q * n, NaN); + err = Vector(Q * n, NaN); } std::vector lineSegments; @@ -854,6 +855,7 @@ class CableSubsystem::Impl : public Subsystem::Guts // TODO Cholesky decomposition would be more efficient. FactorLU matInv; Vector vec; + Vector err; }; struct CacheEntry From 772d2e6b879defd2d8dbf699f070541951607bfc Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 17:56:19 +0200 Subject: [PATCH 104/127] refactor runtime tests of geodesic correction --- Simbody/src/Wrapping.cpp | 25 +++- Simbody/src/WrappingImpl.h | 245 ++++++++++++++++++++++++------------- 2 files changed, 184 insertions(+), 86 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 80af21186..fae70a759 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1072,6 +1072,16 @@ void CurveSegment::Impl::shootNewGeodesic( m_ProjectionMaxIter, m_ProjectionAccuracy, cache.samples); + + cache.curvatures_P = { + calcNormalCurvature(m_Geometry, cache.K_P.p(), cache.K_P.R().getAxisUnitVec(TangentAxis)), + calcNormalCurvature(m_Geometry, cache.K_P.p(), cache.K_P.R().getAxisUnitVec(BinormalAxis)), + }; + cache.curvatures_Q = { + calcNormalCurvature(m_Geometry, cache.K_Q.p(), cache.K_Q.R().getAxisUnitVec(TangentAxis)), + calcNormalCurvature(m_Geometry, cache.K_Q.p(), cache.K_Q.R().getAxisUnitVec(BinormalAxis)), + }; + cache.length = l; } @@ -1593,6 +1603,19 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Compute the geodesic corrections for each curve segment. const Correction* corrIt = calcPathCorrections(data); + Real scale = 1.; + size_t offset = 0; + for (const CurveSegment& curve : m_CurveSegments) { + if (!curve.getImpl().getInstanceEntry(s).isActive()) { + continue; + } + Real cScale = curve.getImpl().calcCorrectionScale(s, *(corrIt + offset)); + scale = cScale < scale ? cScale : scale; + } + if (scale != 1.) { + std::cout << "Applied scale = " << scale << "\n"; + } + // Apply corrections to the curve segments. for (const CurveSegment& curve : m_CurveSegments) { // The corrections were computed for the active segments: Skip any @@ -1600,7 +1623,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const if (!curve.getImpl().getInstanceEntry(s).isActive()) { continue; } - curve.getImpl().applyGeodesicCorrection(s, *corrIt); + curve.getImpl().applyGeodesicCorrection(s, (*corrIt) * scale); ++corrIt; } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index af7f0f421..ef63854b8 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -60,6 +60,17 @@ class CurveSegment::Impl Vec3 trackingPointOnLine{NaN, NaN, NaN}; Status status = Status::Lifted; + + // TODO experimental, might remove later. + FrenetFrame prev_K_P; + FrenetFrame prev_K_Q; + Variation prev_dK_P; + Variation prev_dK_Q; + Correction prev_correction; + Status prev_status = Status::Disabled; + + std::array curvatures_P {NaN, NaN}; + std::array curvatures_Q {NaN, NaN}; }; // Position level cache: Curve in ground frame. @@ -297,22 +308,164 @@ class CurveSegment::Impl unitForce_G[1] = t_Q - t_P; } + Real calcCorrectionScale(const State& s, const Correction& c) const + { + const InstanceEntry& cache = getInstanceEntry(s); + Real scale = 1.; + constexpr Real smallAngle = 5. / 180. * M_PI; + + auto UpdateScale = [&](Real curvature, Real tryStep) { + const Real maxStep = std::abs(smallAngle / curvature); + if (std::abs(tryStep) > maxStep) { + const Real s = std::abs(maxStep / tryStep); + scale = s < scale ? s : scale; + } + }; + + UpdateScale(cache.curvatures_P.at(0), c[0]); + UpdateScale(cache.curvatures_P.at(1), c[1]); + + for (int r = 0; r < 2; ++r) { + Real tryStep = 0; + for (int i = 0; i < 4; ++i) { + tryStep += std::abs(cache.dK_Q[1].row(r)[i] * c[i]); + } + UpdateScale(cache.curvatures_Q.at(r), tryStep); + } + + return scale; + } + + // TODO This is experimental, might remove later. + // 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 bool isOk = std::abs( (exp_diff - got_diff).norm() / exp_diff.norm() - 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"; + } + return isOk; + }; + + auto AssertFrame = [&]( + const Transform& K0, + const Transform& K1, + const Variation& dK, + const std::array& curvatures) -> 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 << " maxStep = " << + std::abs(smallAngle / curvatures.at(0)) + << ", " << + std::abs(smallAngle / curvatures.at(1)) + << "\n"; + oss << " gotStep = " << + (K0.R().transpose() * got_diff)[0] + << ", " << + (K0.R().transpose() * got_diff)[2] + << "\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; + + return isOk; + + 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 (delta > 1e-10) { + const bool assertStart = !AssertFrame(cache.prev_K_P, cache.K_P, cache.prev_dK_P, cache.curvatures_P); + const bool assertEnd = !AssertFrame(cache.prev_K_Q, cache.K_Q, cache.prev_dK_Q, cache.curvatures_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 { + assertLastCorrection(s); + + // TODO This is experimental, might remove later. + { + 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; + } + // Get the previous geodesic. const InstanceEntry& cache = getInstanceEntry(s); const Variation& dK_P = cache.dK_P; const FrenetFrame& K_P = cache.K_P; - // TODO This is part of the unit test block below... - const FrenetFrame K0_P = cache.K_P; - const FrenetFrame K0_Q = cache.K_Q; - const Variation dK0_P = cache.dK_P; - const Variation dK0_Q = cache.dK_Q; - // TODO end of part belongning to unit test below.. - // Get corrected initial conditions. const Vec3 v = dK_P[1] * c; const Vec3 point = K_P.p() + v; @@ -336,84 +489,6 @@ class CurveSegment::Impl getSubsystem().markDiscreteVarUpdateValueRealized(s, m_InstanceIx); - // TODO Below code should be moved to a unit test... - const bool DO_UNIT_TEST_HERE = false; - if (DO_UNIT_TEST_HERE) { - const Real delta = c.norm(); - const Real eps = delta / 10.; - 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 expected_diff = cross(w, a0); - const Vec3 got_diff = a1 - a0; - const bool isOk = (expected_diff - got_diff).norm() < eps; - if (!isOk) { - std::cout << " a0 = " << R0.transpose() * a0 << "\n"; - std::cout << " a1 = " << R0.transpose() * a1 << "\n"; - std::cout << " expected diff = " - << R0.transpose() * expected_diff / delta << "\n"; - std::cout << " got dt_Q = " - << R0.transpose() * got_diff / delta << "\n"; - std::cout << " err = " - << (expected_diff - got_diff).norm() / delta - << "\n"; - } - return isOk; - }; - - auto AssertFrame = [&](const Transform& K0, - const Transform& K1, - const Variation& dK0) -> bool { - const Vec3 dx_got = K1.p() - K0.p(); - const Vec3 dx_expected = dK0[1] * c; - bool isOk = (dx_expected - dx_got).norm() < eps; - if (!isOk) { - std::cout << "Apply variation c = " << c - << ".norm() = " << delta << "\n"; - std::cout << " x0 = " << K0.p() << "\n"; - std::cout << " x1 = " << K1.p() << "\n"; - std::cout << " expected dx_Q = " - << K0.R().transpose() * dx_expected / delta - << "\n"; - std::cout << " got dx_Q = " - << K0.R().transpose() * dx_got / delta << "\n"; - std::cout << " err = " - << (dx_expected - dx_got).norm() / delta << "\n"; - std::cout << "WARNING: Large deviation in final position\n"; - } - const Vec3 w = dK0[0] * c; - std::array axes = { - TangentAxis, - NormalAxis, - BinormalAxis}; - for (size_t i = 0; i < 3; ++i) { - isOk &= AssertAxis(K0.R(), K1.R(), w, axes.at(i)); - if (!isOk) { - std::cout << "FAILED axis " << i << "\n"; - } - } - - return isOk; - }; - - if (delta > 1e-10) { - if (!AssertFrame(K0_P, getInstanceEntry(s).K_P, dK0_P)) { - // TODO use SimTK_ASSERT - throw std::runtime_error( - "Start frame variation check failed"); - } - if (!AssertFrame(K0_Q, getInstanceEntry(s).K_Q, dK0_Q)) { - // TODO use SimTK_ASSERT - throw std::runtime_error( - "End frame variation check failed"); - } - } - } - // End of unit test code block... - invalidatePosEntry(s); } From 16f45175b1e03e94b08bae3031331c9d6c09ac54 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 17:57:03 +0200 Subject: [PATCH 105/127] import cmath --- Simbody/src/WrappingImpl.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index ef63854b8..b15839db4 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -9,6 +9,7 @@ #include "simbody/internal/Wrapping.h" #include "simbody/internal/common.h" #include "simmath/internal/ContactGeometry.h" +#include #include #include From e206b52781f747d8c7c2d2b8b6935dc55593518c Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 17:57:45 +0200 Subject: [PATCH 106/127] always realize position when computing power --- Simbody/src/Wrapping.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index fae70a759..34206df8c 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1700,12 +1700,12 @@ void CableSpan::Impl::applyBodyForces( Real tension, Vector_& bodyForcesInG) const { + realizePosition(s); if (tension < 0.) { // TODO throw? or skip? throw std::runtime_error("Cable tension should be nonnegative."); } - realizePosition(s); SpatialVec unitForce_G; { From 9a2d115502631f03beec35bd31cab11f25b9d725 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 17:59:24 +0200 Subject: [PATCH 107/127] set default max iteration params to high values --- Simbody/src/WrappingImpl.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index b15839db4..55ca0a327 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -611,15 +611,15 @@ class CurveSegment::Impl Vec3 m_ContactPointHint_S{NaN, NaN, NaN}; // TODO expose getters and setters. - size_t m_ProjectionMaxIter = 10; + 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; - size_t m_TouchdownIter = 10; + Real m_TouchdownAccuracy = 1e-4; // TODO set to reasonable value + size_t m_TouchdownIter = 50; // TODO set to reasonable value }; //============================================================================== From e8119719d8e38e178c61b69d5283d3ab5c042e38 Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 17:59:59 +0200 Subject: [PATCH 108/127] tweak example values --- examples/ExampleCableSpanSimple.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/ExampleCableSpanSimple.cpp b/examples/ExampleCableSpanSimple.cpp index f3897b87b..9bd957ecf 100644 --- a/examples/ExampleCableSpanSimple.cpp +++ b/examples/ExampleCableSpanSimple.cpp @@ -352,7 +352,7 @@ int main() path1.adoptWrappingObstacle( ball2, Transform(), - ContactGeometry::Ellipsoid({Rad2, Rad2 * 1.2, Rad2 * 0.9}), + ContactGeometry::Ellipsoid({Rad2, Rad2 * 2., Rad2 * 0.9}), /* ContactGeometry::Sphere(Rad2), */ {0., 1., 0.}); @@ -390,7 +390,7 @@ int main() { /* Random::Gaussian random; */ - v += random.getValue() * 1e-2; + v += random.getValue() * 1e-3; v = std::max(-1e-1, v); v = std::min(1e-1, v); } From 2d13c3aa4d723c43266cdeef48db8768c19563ea Mon Sep 17 00:00:00 2001 From: pepbos Date: Mon, 6 May 2024 18:09:29 +0200 Subject: [PATCH 109/127] add doc comments --- Simbody/src/Wrapping.cpp | 2 ++ Simbody/src/WrappingImpl.h | 15 ++++++++------- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 34206df8c..14e3126a5 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1603,6 +1603,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Compute the geodesic corrections for each curve segment. const Correction* corrIt = calcPathCorrections(data); + // TODO verify effectiveness of scaling. Real scale = 1.; size_t offset = 0; for (const CurveSegment& curve : m_CurveSegments) { @@ -1613,6 +1614,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const scale = cScale < scale ? cScale : scale; } if (scale != 1.) { + // TODO remove this debug printing. std::cout << "Applied scale = " << scale << "\n"; } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 55ca0a327..09d9d50d6 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -337,8 +337,7 @@ class CurveSegment::Impl return scale; } - // TODO This is experimental, might remove later. - // TODO Below code should be moved to a unit test... + // TODO Below code should be moved to a unit test. void assertLastCorrection(const State& s) const { std::ostream& oss = std::cout; @@ -366,7 +365,8 @@ class CurveSegment::Impl const Vec3 exp_diff = cross(w, a0); const Vec3 got_diff = a1 - a0; - const bool isOk = std::abs( (exp_diff - got_diff).norm() / exp_diff.norm() - 1.) < eps; + 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"; @@ -377,6 +377,7 @@ class CurveSegment::Impl oss << " err = " << (exp_diff - got_diff).norm() / delta << "\n"; + oss << " factor = " << factor << "\n"; } return isOk; }; @@ -420,8 +421,6 @@ class CurveSegment::Impl const Vec3 w = dK[0] * c; - return isOk; - std::array axes = { TangentAxis, NormalAxis, @@ -450,10 +449,12 @@ class CurveSegment::Impl // shoot a new geodesic, updating the cache variable. void applyGeodesicCorrection(const State& s, const Correction& c) const { - assertLastCorrection(s); - // TODO This is experimental, might remove later. + // 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; From ab2320a868a67c95952c0376a5089bd23282bea1 Mon Sep 17 00:00:00 2001 From: pepbos Date: Wed, 8 May 2024 11:42:28 +0200 Subject: [PATCH 110/127] fix computing the max correction stepsize --- Simbody/src/Wrapping.cpp | 42 +++++++++++++++++++++++++------- Simbody/src/WrappingImpl.h | 50 +++++--------------------------------- 2 files changed, 39 insertions(+), 53 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 14e3126a5..9e3a5fcbc 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1073,16 +1073,40 @@ void CurveSegment::Impl::shootNewGeodesic( m_ProjectionAccuracy, cache.samples); - cache.curvatures_P = { - calcNormalCurvature(m_Geometry, cache.K_P.p(), cache.K_P.R().getAxisUnitVec(TangentAxis)), - calcNormalCurvature(m_Geometry, cache.K_P.p(), cache.K_P.R().getAxisUnitVec(BinormalAxis)), - }; - cache.curvatures_Q = { - calcNormalCurvature(m_Geometry, cache.K_Q.p(), cache.K_Q.R().getAxisUnitVec(TangentAxis)), - calcNormalCurvature(m_Geometry, cache.K_Q.p(), cache.K_Q.R().getAxisUnitVec(BinormalAxis)), + cache.length = l; +} + +Real CurveSegment::Impl::calcMaxCorrectionStepSize(const State& s, const Correction& c) const +{ + Real maxStepSize = 1.; + + const Real angle = m_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; + } }; - cache.length = l; + 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); + + return maxStepSize; } //============================================================================== @@ -1610,7 +1634,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const if (!curve.getImpl().getInstanceEntry(s).isActive()) { continue; } - Real cScale = curve.getImpl().calcCorrectionScale(s, *(corrIt + offset)); + Real cScale = curve.getImpl().calcMaxCorrectionStepSize(s, *(corrIt + offset)); scale = cScale < scale ? cScale : scale; } if (scale != 1.) { diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 09d9d50d6..5c2da71dd 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -69,9 +69,6 @@ class CurveSegment::Impl Variation prev_dK_Q; Correction prev_correction; Status prev_status = Status::Disabled; - - std::array curvatures_P {NaN, NaN}; - std::array curvatures_Q {NaN, NaN}; }; // Position level cache: Curve in ground frame. @@ -309,33 +306,7 @@ class CurveSegment::Impl unitForce_G[1] = t_Q - t_P; } - Real calcCorrectionScale(const State& s, const Correction& c) const - { - const InstanceEntry& cache = getInstanceEntry(s); - Real scale = 1.; - constexpr Real smallAngle = 5. / 180. * M_PI; - - auto UpdateScale = [&](Real curvature, Real tryStep) { - const Real maxStep = std::abs(smallAngle / curvature); - if (std::abs(tryStep) > maxStep) { - const Real s = std::abs(maxStep / tryStep); - scale = s < scale ? s : scale; - } - }; - - UpdateScale(cache.curvatures_P.at(0), c[0]); - UpdateScale(cache.curvatures_P.at(1), c[1]); - - for (int r = 0; r < 2; ++r) { - Real tryStep = 0; - for (int i = 0; i < 4; ++i) { - tryStep += std::abs(cache.dK_Q[1].row(r)[i] * c[i]); - } - UpdateScale(cache.curvatures_Q.at(r), tryStep); - } - - return scale; - } + Real calcMaxCorrectionStepSize(const State& s, const Correction& c) const; // TODO Below code should be moved to a unit test. void assertLastCorrection(const State& s) const @@ -385,8 +356,7 @@ class CurveSegment::Impl auto AssertFrame = [&]( const Transform& K0, const Transform& K1, - const Variation& dK, - const std::array& curvatures) -> bool { + const Variation& dK) -> bool { const Vec3 got_diff = K1.p() - K0.p(); const Vec3 exp_diff = dK[1] * c; @@ -398,16 +368,6 @@ class CurveSegment::Impl << ".norm() = " << delta << "\n"; oss << " x0 = " << K0.p() << "\n"; oss << " x1 = " << K1.p() << "\n"; - oss << " maxStep = " << - std::abs(smallAngle / curvatures.at(0)) - << ", " << - std::abs(smallAngle / curvatures.at(1)) - << "\n"; - oss << " gotStep = " << - (K0.R().transpose() * got_diff)[0] - << ", " << - (K0.R().transpose() * got_diff)[2] - << "\n"; oss << " expected dx_Q = " << K0.R().transpose() * exp_diff / delta << "\n"; @@ -436,8 +396,8 @@ class CurveSegment::Impl }; if (delta > 1e-10) { - const bool assertStart = !AssertFrame(cache.prev_K_P, cache.K_P, cache.prev_dK_P, cache.curvatures_P); - const bool assertEnd = !AssertFrame(cache.prev_K_Q, cache.K_Q, cache.prev_dK_Q, cache.curvatures_Q); + 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"); @@ -621,6 +581,8 @@ class CurveSegment::Impl // 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 + + Real m_MaxCorrectionStepDeg = 10.; }; //============================================================================== From 308eb2cebbd186fa975b26748e8a53c619cd1456 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 9 May 2024 13:53:57 +0200 Subject: [PATCH 111/127] add evaluation of pathErrorJacobian at runtime --- Simbody/src/Wrapping.cpp | 246 +++++++++++++++++++++++++++---------- Simbody/src/WrappingImpl.h | 22 +++- 2 files changed, 195 insertions(+), 73 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 9e3a5fcbc..3c8124685 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -934,7 +934,7 @@ int CurveSegment::Impl::calcPathPoints( namespace { -const Correction* calcPathCorrections(SolverData& data) +void calcPathCorrections(SolverData& data) { Real w = data.pathError.normInf(); @@ -950,7 +950,38 @@ const Correction* calcPathCorrections(SolverData& data) if (data.err.normInf() > 1e-10) { throw std::runtime_error("Failed to invert matrix"); } +} + +Real calcMaxRelativeDeviationFromLinear(SolverData& data) +{ + data.err = + data.pathErrorJacobian * data.pathCorrection + data.prevPathError; + if (data.err.nrow() != data.pathError.nrow()) { + throw std::runtime_error("Incompatible vector sizes"); + } + const Real err = (data.err - data.pathError).norm(); + const Real maxRelErr = err / data.pathCorrectionNorm; + return maxRelErr; +} +const Vec4* getPathError(SolverData& data) +{ + SimTK_ASSERT( + data.pathError.size() * sizeof(Real) == n * sizeof(Vec4), + "Invalid size of path error vector"); + return reinterpret_cast(&data.pathError[0]); +} + +const Vec4* getPredictedPathError(SolverData& data) +{ + SimTK_ASSERT( + data.err.size() * sizeof(Real) == n * sizeof(Vec4), + "Invalid size of path error vector"); + return reinterpret_cast(&data.err[0]); +} + +const Correction* getPathCorrections(SolverData& data) +{ static_assert( sizeof(Correction) == sizeof(Real) * GeodesicDOF, "Invalid size of corrections vector"); @@ -1076,37 +1107,36 @@ void CurveSegment::Impl::shootNewGeodesic( cache.length = l; } -Real CurveSegment::Impl::calcMaxCorrectionStepSize(const State& s, const Correction& c) const +void CurveSegment::Impl::calcMaxCorrectionStepSize( + const State& s, + const Correction& c, + Real maxCorrectionStepDeg, + Real& maxStepSize) const { - Real maxStepSize = 1.; - - const Real angle = m_MaxCorrectionStepDeg / 180. * M_PI; + 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; - } - }; + 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); - - return maxStepSize; } //============================================================================== @@ -1421,7 +1451,7 @@ Real CableSpan::Impl::calcCableLength( return lTot; } -void CableSpan::Impl::calcLineSegments( +Real CableSpan::Impl::calcLineSegments( const State& s, Vec3 p_O, Vec3 p_I, @@ -1430,19 +1460,25 @@ void CableSpan::Impl::calcLineSegments( lines.resize(m_CurveSegments.size() + 1); lines.clear(); - Vec3 lineStart = std::move(p_O); - for (const CurveSegment& segment : m_CurveSegments) { - if (!segment.getImpl().getInstanceEntry(s).isActive()) { + 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; } - const GeodesicInfo& g = segment.getImpl().getPosInfo(s); + 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 @@ -1555,6 +1591,20 @@ void CableSpan::Impl::calcPathErrorJacobianUtility( 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. @@ -1570,7 +1620,8 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Axes considered when computing the path error. const std::array axes{NormalAxis, BinormalAxis}; - posInfo.loopIter = 0; + 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 @@ -1592,27 +1643,76 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const 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. + // 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. Note that - // there is one more straight line segment, than there are active curve - // segments. - calcLineSegments(s, x_O, x_I, data.lineSegments); + // 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; + } - // Evaluate path error as the misalignment of the straight line - // segments with the curve segment's tangent vectors at the contact - // points. + // 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(); - // Stop iterating if max path error is small, or max iterations is reached. - if (maxPathError < m_PathErrorBound || posInfo.loopIter >= m_PathMaxIter) { - posInfo.l = calcCableLength(s, data.lineSegments); + // 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; } @@ -1624,43 +1724,53 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const axes, data.pathErrorJacobian); - // Compute the geodesic corrections for each curve segment. - const Correction* corrIt = calcPathCorrections(data); + // Compute the geodesic corrections for each curve segment: This gives + // us a correction vector in a direction that lowers the path error. + calcPathCorrections(data); - // TODO verify effectiveness of scaling. - Real scale = 1.; - size_t offset = 0; - for (const CurveSegment& curve : m_CurveSegments) { - if (!curve.getImpl().getInstanceEntry(s).isActive()) { - continue; + // 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) { + std::cout << "WARNING: Local minimum found\n"; + break; } - Real cScale = curve.getImpl().calcMaxCorrectionStepSize(s, *(corrIt + offset)); - scale = cScale < scale ? cScale : scale; - } - if (scale != 1.) { - // TODO remove this debug printing. - std::cout << "Applied scale = " << scale << "\n"; } // Apply corrections to the curve segments. - for (const CurveSegment& curve : m_CurveSegments) { - // The corrections were computed for the active segments: Skip any - // non-active here. - if (!curve.getImpl().getInstanceEntry(s).isActive()) { - continue; - } - curve.getImpl().applyGeodesicCorrection(s, (*corrIt) * scale); + corrIt = getPathCorrections(data); + callForEachActiveCurveSegment(s, [&](const CurveSegment::Impl& curve) { + curve.applyGeodesicCorrection(s, *corrIt); ++corrIt; - } + }); - // Applying the corrections changes the path: invalidate each segment's - // cache. + // 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. + // Also invalidate non-active segments: They might touchdown + // again. curve.getImpl().invalidatePosEntry(s); } - ++posInfo.loopIter; + ++posInfo.loopIter; } // TODO throw? diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 5c2da71dd..f214e7d14 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -306,7 +306,7 @@ class CurveSegment::Impl unitForce_G[1] = t_Q - t_P; } - Real calcMaxCorrectionStepSize(const State& s, const Correction& c) const; + 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 @@ -421,6 +421,7 @@ class CurveSegment::Impl 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. @@ -454,6 +455,10 @@ class CurveSegment::Impl invalidatePosEntry(s); } + Status getPrevStatus(const State& s) const { + return getInstanceEntry(s).prev_status; + } + Vec3 calcInitialContactPoint(const State& s) const { const InstanceEntry& ic = getInstanceEntry(s); @@ -581,8 +586,6 @@ class CurveSegment::Impl // 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 - - Real m_MaxCorrectionStepDeg = 10.; }; //============================================================================== @@ -808,7 +811,7 @@ class CableSpan::Impl Matrix& J) const; // Make static or not? - void calcLineSegments( + Real calcLineSegments( const State& s, Vec3 p_O, Vec3 p_I, @@ -837,6 +840,8 @@ class CableSpan::Impl return *m_Subsystem; } + void callForEachActiveCurveSegment(const State& s, std::function f) const; + // Reference back to the subsystem. CableSubsystem* m_Subsystem; // TODO just a pointer? @@ -850,7 +855,11 @@ class CableSpan::Impl // TODO expose getters and setters. Real m_PathErrorBound = 1e-6; - size_t m_PathMaxIter = 2; // TODO set to something reasonable. + size_t m_PathMaxIter = 50; // TODO set to something reasonable. + + // For each curve segment the max allowed radial curvature. + Real m_MaxCorrectionStepDeg = 5.; // TODO describe + Real m_PathPredErrBound = 0.05; // TODO describe. // TOPOLOGY CACHE (set during realizeTopology()) CacheEntryIndex m_PosInfoIx; @@ -880,6 +889,7 @@ class CableSubsystem::Impl : public Subsystem::Guts pathErrorJacobian = Matrix(C * n, Q * n, 0.); pathCorrection = Vector(Q * n, 0.); pathError = Vector(C * n, 0.); + prevPathError = Vector(C * n, NaN); mat = Matrix(Q * n, Q * n, NaN); vec = Vector(Q * n, NaN); err = Vector(Q * n, NaN); @@ -890,11 +900,13 @@ class CableSubsystem::Impl : public Subsystem::Guts Matrix pathErrorJacobian; Vector pathCorrection; Vector pathError; + Vector prevPathError; Matrix mat; // TODO Cholesky decomposition would be more efficient. FactorLU matInv; Vector vec; Vector err; + Real pathCorrectionNorm = NaN; }; struct CacheEntry From c2fa935955c50f3e3048d69e1c0f4ee22e7dc12c Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 9 May 2024 13:55:33 +0200 Subject: [PATCH 112/127] correct comment, data was not locked --- Simbody/src/Wrapping.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 3c8124685..1d4ed6aac 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1565,7 +1565,7 @@ void CableSpan::Impl::calcPathErrorJacobianUtility( return; } - // Grab the shared data cache for computing the matrices, and lock it. + // Grab the shared data cache for computing the matrices. SolverData& data = getSubsystem().getImpl().updCachedScratchboard(s).updOrInsert(nActive); From cd27c9cc4611b1eea2b68f30d668c56060cde313 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 9 May 2024 14:04:33 +0200 Subject: [PATCH 113/127] allow for 20 deg radial curvature --- Simbody/src/WrappingImpl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index f214e7d14..ff614ada8 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -858,7 +858,7 @@ class CableSpan::Impl size_t m_PathMaxIter = 50; // TODO set to something reasonable. // For each curve segment the max allowed radial curvature. - Real m_MaxCorrectionStepDeg = 5.; // TODO describe + Real m_MaxCorrectionStepDeg = 20.; // TODO describe Real m_PathPredErrBound = 0.05; // TODO describe. // TOPOLOGY CACHE (set during realizeTopology()) From eabac1eca4b5fee774ff1dd5b27045e196476a97 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 9 May 2024 14:18:58 +0200 Subject: [PATCH 114/127] cleanup: rename variables --- Simbody/src/Wrapping.cpp | 36 ++++++++++++------------------------ Simbody/src/WrappingImpl.h | 6 ++++-- 2 files changed, 16 insertions(+), 26 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 1d4ed6aac..67868f15a 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -946,38 +946,25 @@ void calcPathCorrections(SolverData& data) data.vec = data.pathErrorJacobian.transpose() * (data.pathError * (-1.)); data.matInv.solve(data.vec, data.pathCorrection); - data.err = data.mat * data.pathCorrection - data.vec; - if (data.err.normInf() > 1e-10) { - throw std::runtime_error("Failed to invert matrix"); + data.solverError = data.mat * data.pathCorrection - data.vec; + if (data.solverError.normInf() > 1e-10) { + // TODO use warning channel or exception? + std::cout << "WARNING: Failed to invert matrix: This is bad.\n"; } } Real calcMaxRelativeDeviationFromLinear(SolverData& data) { - data.err = + data.localLinearityError = data.pathErrorJacobian * data.pathCorrection + data.prevPathError; - if (data.err.nrow() != data.pathError.nrow()) { + if (data.localLinearityError.nrow() != data.pathError.nrow()) { throw std::runtime_error("Incompatible vector sizes"); } - const Real err = (data.err - data.pathError).norm(); - const Real maxRelErr = err / data.pathCorrectionNorm; - return maxRelErr; -} - -const Vec4* getPathError(SolverData& data) -{ - SimTK_ASSERT( - data.pathError.size() * sizeof(Real) == n * sizeof(Vec4), - "Invalid size of path error vector"); - return reinterpret_cast(&data.pathError[0]); -} - -const Vec4* getPredictedPathError(SolverData& data) -{ - SimTK_ASSERT( - data.err.size() * sizeof(Real) == n * sizeof(Vec4), - "Invalid size of path error vector"); - return reinterpret_cast(&data.err[0]); + const Real deviationFromLinear = + (data.localLinearityError - data.pathError).norm(); + const Real maxRelativeDeviation = + deviationFromLinear / data.pathCorrectionNorm; + return maxRelativeDeviation; } const Correction* getPathCorrections(SolverData& data) @@ -1750,6 +1737,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const 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; } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index ff614ada8..e2a0fefec 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -892,7 +892,8 @@ class CableSubsystem::Impl : public Subsystem::Guts prevPathError = Vector(C * n, NaN); mat = Matrix(Q * n, Q * n, NaN); vec = Vector(Q * n, NaN); - err = Vector(Q * n, NaN); + solverError = Vector(Q * n, NaN); + localLinearityError = Vector(Q * n, NaN); } std::vector lineSegments; @@ -905,7 +906,8 @@ class CableSubsystem::Impl : public Subsystem::Guts // TODO Cholesky decomposition would be more efficient. FactorLU matInv; Vector vec; - Vector err; + Vector solverError; + Vector localLinearityError; Real pathCorrectionNorm = NaN; }; From 2f4b5fca2fd596eea6456aeb7944994c19b1b498 Mon Sep 17 00:00:00 2001 From: pepbos Date: Thu, 9 May 2024 14:20:22 +0200 Subject: [PATCH 115/127] cleanup: Remove helper function --- Simbody/src/Wrapping.cpp | 19 ------------------- Simbody/src/WrappingImpl.h | 4 ---- 2 files changed, 23 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 67868f15a..0fe3abe31 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1419,25 +1419,6 @@ void CableSpan::Impl::calcPathErrorJacobian( }; } -Real CableSpan::Impl::calcCableLength( - const State& s, - const std::vector& lines) const -{ - Real lTot = 0.; - for (const LineSegment& line : lines) { - // TODO spell out as length. - lTot += line.l; - } - - for (const CurveSegment& segment : m_CurveSegments) { - if (!segment.getImpl().getInstanceEntry(s).isActive()) { - continue; - } - lTot += segment.getImpl().getInstanceEntry(s).length; - } - return lTot; -} - Real CableSpan::Impl::calcLineSegments( const State& s, Vec3 p_O, diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index e2a0fefec..6561b6e80 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -817,10 +817,6 @@ class CableSpan::Impl Vec3 p_I, std::vector& lines) const; - Real calcCableLength( - const State& state, - const std::vector& lines) const; - const Mobod& getOriginBody() const { return m_OriginBody; From 0319a7949cdde7a94cc461621cbc55151716f988 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 10 May 2024 10:56:46 +0200 Subject: [PATCH 116/127] rename Status to WrappingStatus --- Simbody/include/simbody/internal/Wrapping.h | 18 ++++++------------ Simbody/src/Wrapping.cpp | 12 ++++++------ Simbody/src/WrappingImpl.h | 10 +++++----- 3 files changed, 17 insertions(+), 23 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index c2503b1b2..f2a609714 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -57,17 +57,11 @@ class SimTK_SIMBODY_EXPORT CurveSegment final Vec3 initialContactPointHint); /** A helper class, representing the wrapping status of this segment in - relation to the contact geometry. - - Status::Ok indicates that the cable is in contact with the surface. - Status::Lifted indicates that the cable is not in contact with the surface. - Status::Disabled indicates that the surface obstacle is "disabled", - preventing any interaction with the - cable. */ - enum class Status + relation to the contact geometry.*/ + enum class WrappingStatus { - Ok, - Lifted, + InContactWithSurface, + LiftedFromSurface, Disabled, }; @@ -104,7 +98,7 @@ class SimTK_SIMBODY_EXPORT CurveSegment final /** Get the wrapping status of this segment. The system must be realiezd to Stage::Position. */ - Status getStatus(const State& state) const; + 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. */ @@ -141,7 +135,7 @@ class SimTK_SIMBODY_EXPORT CurveSegment final bool isActive(const State& state) const { - return getStatus(state) == Status::Ok; + return getStatus(state) == WrappingStatus::InContactWithSurface; } //------------------------------------------------------------------------------ diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 0fe3abe31..81da6de2a 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -19,7 +19,7 @@ using LocalGeodesicInfo = CurveSegment::Impl::InstanceEntry; using LocalGeodesicSample = CurveSegment::Impl::LocalGeodesicSample; using PointVariation = ContactGeometry::GeodesicPointVariation; using SolverData = CableSubsystem::Impl::SolverData; -using Status = CurveSegment::Status; +using Status = CurveSegment::WrappingStatus; using Variation = ContactGeometry::GeodesicVariation; //============================================================================== @@ -70,7 +70,7 @@ const Transform& CurveSegment::getXformSurfaceToBody() const return getImpl().getXformSurfaceToBody(); } -CurveSegment::Status CurveSegment::getStatus(const State& s) const +CurveSegment::WrappingStatus CurveSegment::getStatus(const State& s) const { getImpl().realizeCablePosition(s); return getImpl().getInstanceEntry(s).status; @@ -991,7 +991,7 @@ void CurveSegment::Impl::calcLiftoffIfNeeded( { // Only attempt liftoff when currently wrapping the surface. LocalGeodesicInfo& g = cache; - if (g.status != Status::Ok) { + if (g.status != WrappingStatus::InContactWithSurface) { return; } @@ -1011,7 +1011,7 @@ void CurveSegment::Impl::calcLiftoffIfNeeded( } // Liftoff detected: update status. - g.status = Status::Lifted; + g.status = WrappingStatus::LiftedFromSurface; // Initialize the tracking point from the last geodesic start point. cache.trackingPointOnLine = g.K_P.p(); } @@ -1023,7 +1023,7 @@ void CurveSegment::Impl::calcTouchdownIfNeeded( { // Only attempt touchdown when lifted. LocalGeodesicInfo& g = cache; - if (g.status != Status::Lifted) { + if (g.status != WrappingStatus::LiftedFromSurface) { return; } @@ -1041,7 +1041,7 @@ void CurveSegment::Impl::calcTouchdownIfNeeded( } // Touchdown detected: Remove the Lifted status flag. - g.status = Status::Ok; + g.status = WrappingStatus::InContactWithSurface; // Shoot a zero length geodesic at the touchdown point. shootNewGeodesic( cache.trackingPointOnLine, diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 6561b6e80..99181dc2b 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -45,7 +45,7 @@ class CurveSegment::Impl { bool isActive() const { - return status == Status::Ok; + return status == WrappingStatus::InContactWithSurface; } FrenetFrame K_P{}; @@ -60,7 +60,7 @@ class CurveSegment::Impl Real sHint = NaN; Vec3 trackingPointOnLine{NaN, NaN, NaN}; - Status status = Status::Lifted; + WrappingStatus status = WrappingStatus::InContactWithSurface; // TODO experimental, might remove later. FrenetFrame prev_K_P; @@ -68,7 +68,7 @@ class CurveSegment::Impl Variation prev_dK_P; Variation prev_dK_Q; Correction prev_correction; - Status prev_status = Status::Disabled; + WrappingStatus prev_status = WrappingStatus::Disabled; }; // Position level cache: Curve in ground frame. @@ -114,7 +114,7 @@ class CurveSegment::Impl Value* cache = new Value(); cache->upd().length = 0.; - cache->upd().status = Status::Lifted; + cache->upd().status = WrappingStatus::LiftedFromSurface; cache->upd().trackingPointOnLine = getContactPointHint(); m_InstanceIx = updSubsystem().allocateAutoUpdateDiscreteVariable( @@ -139,7 +139,7 @@ class CurveSegment::Impl "expected not realized when calling realizePosition"); } - if (getInstanceEntry(s).status == Status::Disabled) { + if (getInstanceEntry(s).status == WrappingStatus::Disabled) { return; } From 4cd9b209c7a8278ee3f425f030b6a2da53ea7148 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 10 May 2024 10:57:22 +0200 Subject: [PATCH 117/127] fix parallel projection error --- Simbody/src/Wrapping.cpp | 33 ++++++++++++++++++++++----------- 1 file changed, 22 insertions(+), 11 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 81da6de2a..fc49b6908 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -310,17 +310,28 @@ void calcSurfaceProjectionFast( } UnitVec3 n(calcSurfaceConstraintGradient(geometry, x)); - 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) - // TODO use SimTK_ASSERT - throw std::runtime_error("Surface projection failed: Tangent guess is " - "parallel to surface normal"); - - t = t / norm; + 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( From f8db437bc29999ac872c99b583087d904d2d54f4 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 10 May 2024 10:57:47 +0200 Subject: [PATCH 118/127] update initial step size of geodesic integrator --- Simbody/src/Wrapping.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index fc49b6908..49f256547 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -1087,12 +1087,14 @@ void CurveSegment::Impl::shootNewGeodesic( InstanceEntry& cache) const { cache.samples.clear(); + cache.sHint = sHint; + calcGeodesicAndVariationImplicitly( m_Geometry, x, t, l, - sHint, + cache.sHint, cache.K_P, cache.dK_P, cache.K_Q, From 25f3915d0cdf8684a241fc9e1f297b14fa13fe53 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 10 May 2024 10:59:09 +0200 Subject: [PATCH 119/127] remove unused field from cache --- Simbody/src/WrappingImpl.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 99181dc2b..ee31ee035 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -81,8 +81,6 @@ class CurveSegment::Impl Variation dKP{}; Variation dKQ{}; - - SpatialVec unitForce; }; //------------------------------------------------------------------------------ From 7a127a2daef941a372bb297e9011ec6905b20898 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 10 May 2024 11:00:05 +0200 Subject: [PATCH 120/127] add path initializer --- Simbody/src/WrappingImpl.h | 34 +++++++++++++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index ee31ee035..23f4a660f 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -154,6 +154,11 @@ class CurveSegment::Impl // Grab the last geodesic that was computed. assertSurfaceBounds(prevPoint_S, nextPoint_S); + calcInitialPathIfNeeded( + prevPoint_S, + nextPoint_S, + updInstanceEntry(s)); + calcTouchdownIfNeeded( prevPoint_S, nextPoint_S, @@ -304,7 +309,11 @@ class CurveSegment::Impl unitForce_G[1] = t_Q - t_P; } - void calcMaxCorrectionStepSize(const State& s, const Correction& c, Real maxCorrectionStepDeg, Real& maxStepSize) const; + 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 @@ -534,6 +543,29 @@ class CurveSegment::Impl 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, From a92d2d0f0ce0bdc92e66ef3734d715d3cf32cd97 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 10 May 2024 11:00:46 +0200 Subject: [PATCH 121/127] fmt --- Simbody/src/WrappingImpl.h | 66 ++++++++++++++++++-------------------- 1 file changed, 32 insertions(+), 34 deletions(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 23f4a660f..936af2c04 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -323,8 +323,8 @@ class CurveSegment::Impl const InstanceEntry& cache = getInstanceEntry(s); const Correction& c = cache.prev_correction; - const Real delta = c.norm(); - const Real eps = 0.05; + const Real delta = c.norm(); + const Real eps = 0.05; const Real smallAngle = 20. / 180. * Pi; @@ -332,19 +332,18 @@ class CurveSegment::Impl 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); + 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 Vec3 got_diff = a1 - a0; const Real factor = got_diff.norm() / exp_diff.norm(); - bool isOk = std::abs(factor - 1.) < eps; + bool isOk = std::abs(factor - 1.) < eps; if (!isOk) { oss << " a0 = " << R0.transpose() * a0 << "\n"; oss << " a1 = " << R0.transpose() * a1 << "\n"; @@ -353,31 +352,27 @@ class CurveSegment::Impl oss << " got dt_Q = " << R0.transpose() * got_diff / delta << "\n"; oss << " err = " - << (exp_diff - got_diff).norm() / delta - << "\n"; + << (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 { - + 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; + bool isOk = std::abs(factor - 1.) < eps; if (!isOk) { - oss << "Apply variation c = " << c - << ".norm() = " << delta << "\n"; + 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"; + << K0.R().transpose() * exp_diff / delta << "\n"; oss << " got dx_Q = " << K0.R().transpose() * got_diff / delta << "\n"; oss << " err = " @@ -386,7 +381,7 @@ class CurveSegment::Impl oss << "FAILED position correction\n"; } - const Vec3 w = dK[0] * c; + const Vec3 w = dK[0] * c; std::array axes = { TangentAxis, @@ -402,9 +397,11 @@ class CurveSegment::Impl return isOk; }; - if (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 (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"); @@ -422,13 +419,13 @@ class CurveSegment::Impl // 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; + 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; + cache.prev_status = cache.status; } // Get the previous geodesic. @@ -462,7 +459,8 @@ class CurveSegment::Impl invalidatePosEntry(s); } - Status getPrevStatus(const State& s) const { + WrappingStatus getPrevStatus(const State& s) const + { return getInstanceEntry(s).prev_status; } From 9c106dda551449557612cb37b6efc07ddc9b5d27 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 10 May 2024 11:01:33 +0200 Subject: [PATCH 122/127] fix finding initial tangent directions --- Simbody/src/WrappingImpl.h | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 936af2c04..518b8bf09 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -726,10 +726,15 @@ class CableSpan::Impl // 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. - // TODO fix finding first active segment. - const UnitVec3& t = - UnitVec3(findNextPoint(s, CurveSegmentIndex(-1)) - posInfo.xO); + const UnitVec3& t = UnitVec3(nextPointG - posInfo.xO); unitForce_G[0] = -r % t; unitForce_G[1] = -Vec3(t); @@ -744,11 +749,16 @@ class CableSpan::Impl // 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. - // TODO fix finding last active segment. - const UnitVec3& t = UnitVec3( - posInfo.xI - - findNextPoint(s, CurveSegmentIndex(getNumCurveSegments()))); + const UnitVec3& t = UnitVec3(posInfo.xI - prevPointG); unitForce_G[0] = r % t; unitForce_G[1] = Vec3(t); From abd0617881987ebb5af660b3ad49550dc0aef742 Mon Sep 17 00:00:00 2001 From: pepbos Date: Fri, 10 May 2024 11:03:18 +0200 Subject: [PATCH 123/127] add missing decleration --- Simbody/src/WrappingImpl.h | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 518b8bf09..73a8956ca 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -874,7 +874,9 @@ class CableSpan::Impl return *m_Subsystem; } - void callForEachActiveCurveSegment(const State& s, std::function f) const; + void callForEachActiveCurveSegment( + const State& s, + std::function f) const; // Reference back to the subsystem. CableSubsystem* m_Subsystem; // TODO just a pointer? @@ -920,14 +922,14 @@ class CableSubsystem::Impl : public Subsystem::Guts 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); - mat = Matrix(Q * n, Q * n, NaN); - vec = Vector(Q * n, NaN); - solverError = Vector(Q * n, NaN); - localLinearityError = Vector(Q * n, NaN); + pathErrorJacobian = Matrix(C * n, Q * n, 0.); + pathCorrection = Vector(Q * n, 0.); + pathError = Vector(C * n, 0.); + prevPathError = Vector(C * n, NaN); + mat = Matrix(Q * n, Q * n, NaN); + vec = Vector(Q * n, NaN); + solverError = Vector(Q * n, NaN); + localLinearityError = Vector(Q * n, NaN); } std::vector lineSegments; From 77c40e1353e3788e74a79e4ec6c18e59613c7a01 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 21 May 2024 11:14:28 +0200 Subject: [PATCH 124/127] fix using QTZ solver for computing path corrections --- Simbody/src/Wrapping.cpp | 46 +++++++++++++++++--------------------- Simbody/src/WrappingImpl.h | 21 ++++++++--------- 2 files changed, 30 insertions(+), 37 deletions(-) diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 49f256547..578d763ec 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -945,34 +945,29 @@ int CurveSegment::Impl::calcPathPoints( namespace { -void calcPathCorrections(SolverData& data) -{ - Real w = data.pathError.normInf(); - - data.mat = data.pathErrorJacobian.transpose() * data.pathErrorJacobian; - for (int i = 0; i < data.mat.nrow(); ++i) { - data.mat[i][i] += w + 1e-6; - } - data.matInv = data.mat; - data.vec = data.pathErrorJacobian.transpose() * (data.pathError * (-1.)); - data.matInv.solve(data.vec, data.pathCorrection); - - data.solverError = data.mat * data.pathCorrection - data.vec; - if (data.solverError.normInf() > 1e-10) { - // TODO use warning channel or exception? - std::cout << "WARNING: Failed to invert matrix: This is bad.\n"; +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.localLinearityError = - data.pathErrorJacobian * data.pathCorrection + data.prevPathError; - if (data.localLinearityError.nrow() != data.pathError.nrow()) { + 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.localLinearityError - data.pathError).norm(); + (data.predictedPathError - data.pathError).norm(); const Real maxRelativeDeviation = deviationFromLinear / data.pathCorrectionNorm; return maxRelativeDeviation; @@ -982,10 +977,11 @@ const Correction* getPathCorrections(SolverData& data) { static_assert( sizeof(Correction) == sizeof(Real) * GeodesicDOF, - "Invalid size of corrections vector"); - SimTK_ASSERT( - data.pathCorrection.size() * sizeof(Real) == n * sizeof(Correction), - "Invalid size of path corrections vector"); + "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]); } @@ -1707,7 +1703,7 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const // Compute the geodesic corrections for each curve segment: This gives // us a correction vector in a direction that lowers the path error. - calcPathCorrections(data); + calcPathCorrections(data, maxPathError); // Compute the step size that we take along the correction vector. Real stepSize = 1.; diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 73a8956ca..702bbd0b7 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -915,10 +915,11 @@ class CableSubsystem::Impl : public Subsystem::Guts After computing the position level cache, this data is discarded. */ struct SolverData { - SolverData(int nActive) + SolverData(int nActive) : nCurves(nActive) { static constexpr int Q = 4; - static constexpr int C = 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); @@ -926,10 +927,7 @@ class CableSubsystem::Impl : public Subsystem::Guts pathCorrection = Vector(Q * n, 0.); pathError = Vector(C * n, 0.); prevPathError = Vector(C * n, NaN); - mat = Matrix(Q * n, Q * n, NaN); - vec = Vector(Q * n, NaN); - solverError = Vector(Q * n, NaN); - localLinearityError = Vector(Q * n, NaN); + predictedPathError = Vector(C * n, NaN); } std::vector lineSegments; @@ -937,14 +935,13 @@ class CableSubsystem::Impl : public Subsystem::Guts Matrix pathErrorJacobian; Vector pathCorrection; Vector pathError; + // TODO remove prev, and cycle pathError Vector prevPathError; - Matrix mat; - // TODO Cholesky decomposition would be more efficient. - FactorLU matInv; - Vector vec; - Vector solverError; - Vector localLinearityError; + // TODO Best solver? + FactorQTZ matInv; // TODO rename to inv + Vector predictedPathError; Real pathCorrectionNorm = NaN; + int nCurves = -1; }; struct CacheEntry From 0bb1788b7ac31ec34b0cba850cc0a7709c213f86 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 21 May 2024 11:17:11 +0200 Subject: [PATCH 125/127] take MobilizedBody by index --- Simbody/include/simbody/internal/Wrapping.h | 8 +-- Simbody/src/Wrapping.cpp | 54 ++++++++++++++------- Simbody/src/WrappingImpl.h | 35 +++++-------- 3 files changed, 54 insertions(+), 43 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index f2a609714..2be9b3d97 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -51,7 +51,7 @@ class SimTK_SIMBODY_EXPORT CurveSegment final belong to a valid cable path.*/ CurveSegment( CableSpan cable, - const MobilizedBody& mobod, + MobilizedBodyIndex body, Transform X_BS, const ContactGeometry& geometry, Vec3 initialContactPointHint); @@ -198,13 +198,13 @@ class SimTK_SIMBODY_EXPORT CableSpan final //------------------------------------------------------------------------------ CableSpan( CableSubsystem& subsystem, - const MobilizedBody& originBody, + MobilizedBodyIndex originBody, const Vec3& defaultOriginPoint, - const MobilizedBody& terminationBody, + MobilizedBodyIndex terminationBody, const Vec3& defaultTerminationPoint); void adoptWrappingObstacle( - const MobilizedBody& mobod, + MobilizedBodyIndex mobod, Transform X_BS, const ContactGeometry& geometry, Vec3 contactPointHint = {1., 0., 0.}); diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index 578d763ec..a78c10a61 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -28,12 +28,12 @@ using Variation = ContactGeometry::GeodesicVariation; CurveSegment::CurveSegment( CableSpan cable, - const MobilizedBody& mobod, + MobilizedBodyIndex body, Transform X_BS, const ContactGeometry& geometry, Vec3 xHint) : m_Impl(std::shared_ptr( - new CurveSegment::Impl(cable, mobod, X_BS, geometry, xHint))) + new CurveSegment::Impl(cable, body, X_BS, geometry, xHint))) { // TODO bit awkward to set the index later. updImpl().setIndex(cable.updImpl().adoptSegment(*this)); @@ -122,9 +122,9 @@ int CurveSegment::calcPoints( CableSpan::CableSpan( CableSubsystem& subsystem, - const MobilizedBody& originBody, + MobilizedBodyIndex originBody, const Vec3& defaultOriginPoint, - const MobilizedBody& terminationBody, + MobilizedBodyIndex terminationBody, const Vec3& defaultTerminationPoint) : m_Impl(std::shared_ptr(new Impl( subsystem, @@ -137,12 +137,12 @@ CableSpan::CableSpan( } void CableSpan::adoptWrappingObstacle( - const MobilizedBody& mobod, + MobilizedBodyIndex body, Transform X_BS, const ContactGeometry& geometry, Vec3 contactPointHint) { - CurveSegment(*this, mobod, X_BS, geometry, contactPointHint); + CurveSegment(*this, body, X_BS, geometry, contactPointHint); } int CableSpan::getNumCurveSegments() const @@ -1141,19 +1141,27 @@ void CurveSegment::Impl::calcMaxCorrectionStepSize( CurveSegment::Impl::Impl( CableSpan path, - const MobilizedBody& mobod, + 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_Mobod(mobod), m_X_BS(X_BS), m_Geometry(geometry), + 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 { @@ -1203,6 +1211,18 @@ void addPathErrorJacobian( // 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) { @@ -1311,7 +1331,7 @@ Vec3 CableSpan::Impl::findPrevPoint(const State& s, CurveSegmentIndex ix) const { const CurveSegment* segment = findPrevActiveCurveSegment(s, ix); return segment ? segment->getImpl().calcFinalContactPoint(s) - : m_OriginBody.getBodyTransform(s).shiftFrameStationToBase( + : getOriginBody().getBodyTransform(s).shiftFrameStationToBase( m_OriginPoint); } @@ -1320,7 +1340,7 @@ Vec3 CableSpan::Impl::findNextPoint(const State& s, CurveSegmentIndex ix) const const CurveSegment* segment = findNextActiveCurveSegment(s, ix); return segment ? segment->getImpl().calcInitialContactPoint(s) - : m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( + : getTerminationBody().getBodyTransform(s).shiftFrameStationToBase( m_TerminationPoint); } @@ -1548,9 +1568,9 @@ void CableSpan::Impl::calcPathErrorJacobianUtility( // Compute the straight-line segments. const Vec3 x_O = - m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + getOriginBody().getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); const Vec3 x_I = - m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( + getTerminationBody().getBodyTransform(s).shiftFrameStationToBase( m_TerminationPoint); calcLineSegments(s, x_O, x_I, data.lineSegments); @@ -1586,9 +1606,9 @@ void CableSpan::Impl::calcPosInfo(const State& s, PosInfo& posInfo) const { // Path origin and termination point. const Vec3 x_O = - m_OriginBody.getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); + getOriginBody().getBodyTransform(s).shiftFrameStationToBase(m_OriginPoint); const Vec3 x_I = - m_TerminationBody.getBodyTransform(s).shiftFrameStationToBase( + getTerminationBody().getBodyTransform(s).shiftFrameStationToBase( m_TerminationPoint); posInfo.xO = x_O; @@ -1776,7 +1796,7 @@ void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const Real& lengthDot = (velInfo.lengthDot = 0.); - Vec3 v_GQ = m_OriginBody.findStationVelocityInGround(s, m_OriginPoint); + Vec3 v_GQ = getOriginBody().findStationVelocityInGround(s, m_OriginPoint); const CurveSegment* lastActive = nullptr; for (const CurveSegment& curve : m_CurveSegments) { if (!curve.getImpl().getInstanceEntry(s).isActive()) { @@ -1798,7 +1818,7 @@ void CableSpan::Impl::calcVelInfo(const State& s, VelInfo& velInfo) const } const Vec3 v_GP = - m_TerminationBody.findStationVelocityInGround(s, m_TerminationPoint); + getTerminationBody().findStationVelocityInGround(s, m_TerminationPoint); const PosInfo& pos = getPosInfo(s); const UnitVec3 e_G = diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 702bbd0b7..23c301d5f 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -241,10 +241,7 @@ class CurveSegment::Impl return m_Decoration; } - const MobilizedBody& getMobilizedBody() const - { - return m_Mobod; - } + const MobilizedBody& getMobilizedBody() const; const Transform& getXformSurfaceToBody() const { @@ -286,7 +283,7 @@ class CurveSegment::Impl Transform calcSurfaceFrameInGround(const State& s) const { - return m_Mobod.getBodyTransform(s).compose(m_X_BS); + return getMobilizedBody().getBodyTransform(s).compose(m_X_BS); } int calcPathPoints(const State& s, std::vector& points, int nSamples) @@ -295,7 +292,7 @@ class CurveSegment::Impl void calcUnitForce(const State& s, SpatialVec& unitForce_G) const { const PosInfo& posInfo = getPosInfo(s); - const Vec3& x_BG = m_Mobod.getBodyOriginLocation(s); + const Vec3& x_BG = getMobilizedBody().getBodyOriginLocation(s); // Contact point moment arms in ground. const Vec3 r_P = posInfo.KP.p() - x_BG; @@ -590,7 +587,7 @@ class CurveSegment::Impl CurveSegmentIndex m_Index; // The index in its path. // MobilizedBody that surface is attached to. - MobilizedBody m_Mobod; + MobilizedBodyIndex m_Body; // Surface to body transform. Transform m_X_BS; @@ -628,9 +625,9 @@ class CableSpan::Impl public: Impl( CableSubsystem& subsystem, - MobilizedBody originBody, + MobilizedBodyIndex originBody, Vec3 originPoint, - MobilizedBody terminationBody, + MobilizedBodyIndex terminationBody, Vec3 terminationPoint) : m_Subsystem(&subsystem), m_OriginBody(originBody), m_OriginPoint(originPoint), @@ -776,7 +773,7 @@ class CableSpan::Impl { calcUnitForceAtOrigin(state, unitForce); - SpatialVec v = m_OriginBody.getBodyVelocity(state); + SpatialVec v = getOriginBody().getBodyVelocity(state); unitPower += ~unitForce * v; } @@ -792,7 +789,7 @@ class CableSpan::Impl { calcUnitForceAtTermination(state, unitForce); - SpatialVec v = m_TerminationBody.getBodyVelocity(state); + SpatialVec v = getTerminationBody().getBodyVelocity(state); unitPower += ~unitForce * v; } @@ -855,14 +852,8 @@ class CableSpan::Impl Vec3 p_I, std::vector& lines) const; - const Mobod& getOriginBody() const - { - return m_OriginBody; - } - const Mobod& getTerminationBody() const - { - return m_TerminationBody; - } + const Mobod& getOriginBody() const; + const Mobod& getTerminationBody() const; const CableSubsystem& getSubsystem() const { @@ -881,10 +872,10 @@ class CableSpan::Impl // Reference back to the subsystem. CableSubsystem* m_Subsystem; // TODO just a pointer? - MobilizedBody m_OriginBody; + MobilizedBodyIndex m_OriginBody; Vec3 m_OriginPoint; - MobilizedBody m_TerminationBody; + MobilizedBodyIndex m_TerminationBody; Vec3 m_TerminationPoint; Array_ m_CurveSegments{}; @@ -985,9 +976,9 @@ class CableSubsystem::Impl : public Subsystem::Guts return cables[index]; } - // Add a cable path to the list, bumping the reference count. CableSpanIndex adoptCablePath(CableSpan& path) { + invalidateSubsystemTopologyCache(); cables.push_back(path); return CableSpanIndex(cables.size() - 1); } From 63fcf83693d78771d9a9e37bdd723756343ad7a7 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 21 May 2024 13:07:04 +0200 Subject: [PATCH 126/127] refactor calcDecorations to use calcPathPoints --- Simbody/include/simbody/internal/Wrapping.h | 10 +- Simbody/src/Wrapping.cpp | 119 +++++--------------- Simbody/src/WrappingImpl.h | 86 +++++++++++--- 3 files changed, 105 insertions(+), 110 deletions(-) diff --git a/Simbody/include/simbody/internal/Wrapping.h b/Simbody/include/simbody/internal/Wrapping.h index 2be9b3d97..262fdd148 100644 --- a/Simbody/include/simbody/internal/Wrapping.h +++ b/Simbody/include/simbody/internal/Wrapping.h @@ -131,7 +131,13 @@ class SimTK_SIMBODY_EXPORT CurveSegment final 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::vector& points_G, int nPoints = 0) const; + 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 { @@ -246,7 +252,7 @@ class SimTK_SIMBODY_EXPORT CableSpan final Real calcCablePower(const State& state, Real tension) const; // Calls `CurveSegment::calcPoints` on each active curve segment. - int calcPoints(const State& state, std::vector& points_G, int nPointsPerCurveSegment = 0) const; + int calcPoints(const State& state, std::function& sink, int nPointsPerCurveSegment = 0) const; //------------------------------------------------------------------------------ // TODO TEMPORARY FOR UNIT TESTING diff --git a/Simbody/src/Wrapping.cpp b/Simbody/src/Wrapping.cpp index a78c10a61..d0ccda156 100644 --- a/Simbody/src/Wrapping.cpp +++ b/Simbody/src/Wrapping.cpp @@ -107,13 +107,10 @@ void CurveSegment::calcUnitForce(const State& state, SpatialVec& unitForce_G) getImpl().calcUnitForce(state, unitForce_G); } -int CurveSegment::calcPoints( - const State& state, - std::vector& points_G, - int nPoints) const +int CurveSegment::calcPoints(const State& state, std::function& sink, int nPoints) const { getImpl().realizeCablePosition(state); - return getImpl().calcPathPoints(state, points_G, nPoints); + return getImpl().calcPathPoints(state, sink, nPoints); } //============================================================================== @@ -173,12 +170,10 @@ void CableSpan::applyBodyForces( return getImpl().applyBodyForces(s, tension, bodyForcesInG); } -int CableSpan::calcPoints( - const State& state, - std::vector& points_G, +int CableSpan::calcPoints(const State& state, std::function& sink, int nPointsPerCurveSegment) const { - return getImpl().calcPathPoints(state, points_G, nPointsPerCurveSegment); + return getImpl().calcPathPoints(state, sink, nPointsPerCurveSegment); } void CableSpan::calcUnitForceAtOrigin( @@ -812,7 +807,7 @@ size_t calcResampledGeodesicPoints( const std::vector& geodesic, const Transform& X_GS, int nSamples, - std::vector& interpolatedSamples) + std::function& sink) { // Some sanity checks. if (geodesic.empty()) { @@ -837,7 +832,7 @@ size_t calcResampledGeodesicPoints( } // Capture the start of the geodesic. - interpolatedSamples.push_back( + sink( X_GS.shiftFrameStationToBase(geodesic.front().frame.p())); // If there is but one sample in the geodesic, write that sample and exit. @@ -896,27 +891,23 @@ size_t calcResampledGeodesicPoints( // Transform to ground frame. const Vec3 point_G = X_GS.shiftFrameStationToBase(point_S); // Write interpolated point to the output buffer. - interpolatedSamples.push_back(point_G); + sink(point_G); break; } } // Capture the last point of the geodesic. - interpolatedSamples.push_back( - X_GS.shiftFrameStationToBase(geodesic.back().frame.p())); + sink(X_GS.shiftFrameStationToBase(geodesic.back().frame.p())); return nSamples; } } // namespace -int CurveSegment::Impl::calcPathPoints( - const State& s, - std::vector& points, - int nSamples) const +int CurveSegment::Impl::calcPathPoints(const State& state, std::function& sink, int nSamples) const { - const Transform& X_GS = getPosInfo(s).X_GS; - const InstanceEntry& geodesic_S = getInstanceEntry(s); + const Transform& X_GS = getPosInfo(state).X_GS; + const InstanceEntry& geodesic_S = getInstanceEntry(state); if (!geodesic_S.isActive()) { return 0; } @@ -925,8 +916,9 @@ int CurveSegment::Impl::calcPathPoints( // integrator to the output buffer. if (nSamples == 0) { for (const LocalGeodesicSample& sample : geodesic_S.samples) { - points.push_back(X_GS.shiftFrameStationToBase(sample.frame.p())); + sink(X_GS.shiftFrameStationToBase(sample.frame.p())); } + return geodesic_S.samples.size(); } // Resample the points from the integrator by interpolating at equal @@ -935,7 +927,7 @@ int CurveSegment::Impl::calcPathPoints( geodesic_S.samples, X_GS, nSamples, - points); + sink); } //============================================================================== @@ -1873,85 +1865,32 @@ int CableSpan::Impl::calcDecorativeGeometryAndAppend( Stage stage, Array_& decorations) const { - // TODO clean this up. (also, should decorations be done here?) - const Vec3 color = Green; // Red Purple + static constexpr int LINE_THICKNESS = 3; const PosInfo& ppe = getPosInfo(s); - - // Draw point at origin and termination. - decorations.push_back(DecorativePoint(ppe.xO).setColor(Green)); - decorations.push_back(DecorativePoint(ppe.xI).setColor(Red)); - - if (countActive(s) == 0) { - decorations.push_back(DecorativeLine(ppe.xO, ppe.xI) - .setColor(Purple) - .setLineThickness(3)); - } + Vec3 prevPoint = ppe.xO; for (const CurveSegment& curveSegment : m_CurveSegments) { const CurveSegment::Impl& curve = curveSegment.getImpl(); - const Transform X_GS = curve.calcSurfaceFrameInGround(s); - DecorativeGeometry geo = curve.getDecoration(); - const Transform& X_SD = geo.getTransform(); // TODO getTransform? + if (curve.getInstanceEntry(s).isActive()) { + const CurveSegment::Impl::PosInfo cppe = curve.getPosInfo(s); - // Inactive surfaces are dimmed. - if (!curve.getInstanceEntry(s).isActive()) { - decorations.push_back(geo.setTransform(X_GS * X_SD) - .setColor(Real(0.75) * geo.getColor())); - continue; + const Vec3 nextPoint = cppe.KP.p(); + decorations.push_back(DecorativeLine(prevPoint, nextPoint) + .setColor(Purple) + .setLineThickness(LINE_THICKNESS)); + prevPoint = cppe.KQ.p(); } - decorations.push_back(geo.setTransform(X_GS * X_SD)); - - { - const Vec3 prevPoint = findPrevPoint(s, curveSegment); - const Vec3 x_P = curve.getPosInfo(s).KP.p(); - - decorations.push_back(DecorativeLine(prevPoint, x_P) - .setColor(Orange) - .setLineThickness(2)); - } - - // TODO this is for debugging: Draw the Frenet frames - Transform K_P = curve.getPosInfo(s).KP; - Transform K_Q = curve.getPosInfo(s).KQ; - const std::array axes = { - TangentAxis, - NormalAxis, - BinormalAxis}; - const std::array colors = {Red, Green, Blue}; - - for (size_t i = 0; i < 3; ++i) { - decorations.push_back( - DecorativeLine( - K_P.p(), - K_P.p() + 0.5 * K_P.R().getAxisUnitVec(axes.at(i))) - .setColor(colors.at(i)) - .setLineThickness(4)); - decorations.push_back( - DecorativeLine( - K_Q.p(), - K_Q.p() + 0.5 * K_Q.R().getAxisUnitVec(axes.at(i))) - .setColor(colors.at(i)) - .setLineThickness(4)); - } - - { - const Vec3 nextPoint = findNextPoint(s, curveSegment); - const Vec3 x_Q = curve.getPosInfo(s).KQ.p(); - - decorations.push_back(DecorativeLine(nextPoint, x_Q) - .setColor(Gray) - .setLineThickness(2)); - } - - curveSegment.getImpl().calcDecorativeGeometryAndAppend(s, decorations); + curve.calcDecorativeGeometryAndAppend(s, decorations); } - /* decorations.push_back(DecorativeLine(lastCurvePoint, ppe.xI) */ - /* .setColor(Purple) */ - /* .setLineThickness(3)); */ + // TODO choose colors. + decorations.push_back(DecorativeLine(prevPoint, ppe.xI) + .setColor(Purple) + .setLineThickness(3)); + return 0; } diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index 23c301d5f..bc0175361 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -286,8 +286,7 @@ class CurveSegment::Impl return getMobilizedBody().getBodyTransform(s).compose(m_X_BS); } - int calcPathPoints(const State& s, std::vector& points, int nSamples) - const; + int calcPathPoints(const State& state, std::function& sink, int nSamples = 0) const; void calcUnitForce(const State& s, SpatialVec& unitForce_G) const { @@ -489,19 +488,73 @@ class CurveSegment::Impl const State& s, Array_& decorations) const { + static constexpr int LINE_THICKNESS = 3; + static constexpr Real INACTIVE_OPACITY = 0.25; + const InstanceEntry& cache = getInstanceEntry(s); - if (!cache.isActive()) { + 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; } - const Transform& X_GS = calcSurfaceFrameInGround(s); - Vec3 a = X_GS.shiftFrameStationToBase(cache.K_P.p()); - for (size_t i = 1; i < cache.samples.size(); ++i) { - const Vec3 b = - X_GS.shiftFrameStationToBase(cache.samples.at(i).frame.p()); - decorations.push_back( - DecorativeLine(a, b).setColor(Purple).setLineThickness(3)); - a = b; + // 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); } } @@ -690,26 +743,23 @@ class CableSpan::Impl Stage stage, Array_& decorations) const; - int calcPathPoints( - const State& state, - std::vector& points_G, - int nPointsPerCurveSegment) const + int calcPathPoints(const State& state, std::function& sink, int nPointsPerCurveSegment) const { // Write the initial point. const PosInfo& pos = getPosInfo(state); - points_G.push_back(pos.xO); + 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, - points_G, + sink, nPointsPerCurveSegment); } // Write the termination point. - points_G.push_back(pos.xI); + sink(pos.xI); // Return number of points written. return count + 2; From 1fb0c857e3594af292ae10b8ffc3819ba6c48e30 Mon Sep 17 00:00:00 2001 From: pepbos Date: Tue, 21 May 2024 13:07:59 +0200 Subject: [PATCH 127/127] fix compilation error (take body by index) --- Simbody/src/WrappingImpl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Simbody/src/WrappingImpl.h b/Simbody/src/WrappingImpl.h index bc0175361..24e2bf01f 100644 --- a/Simbody/src/WrappingImpl.h +++ b/Simbody/src/WrappingImpl.h @@ -96,7 +96,7 @@ class CurveSegment::Impl // TODO you would expect the constructor to take the index as well here? Impl( CableSpan path, - const MobilizedBody& mobod, + MobilizedBodyIndex body, const Transform& X_BS, ContactGeometry geometry, Vec3 initPointGuess);