CableSpan.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_CABLE_SPAN_H_
#define SimTK_SIMBODY_CABLE_SPAN_H_
 
/*-----------------------------------------------------------------------------
                               Simbody(tm)
-------------------------------------------------------------------------------
 Copyright (c) 2024 Authors.
 Authors: Pepijn van den Bos
 Contributors: Nicholas Bianco
 
 Licensed under the Apache License, Version 2.0 (the "License"); you may
 not use this file except in compliance with the License. You may obtain a
 copy of the License at http://www.apache.org/licenses/LICENSE-2.0.
 
 Unless required by applicable law or agreed to in writing, software
 distributed under the License is distributed on an "AS IS" BASIS,
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.
 ----------------------------------------------------------------------------*/
 
#include "simbody/internal/MobilizedBody.h"
#include "simbody/internal/common.h"
 
namespace SimTK
{
 
SimTK_DEFINE_UNIQUE_INDEX_TYPE(CableSpanObstacleIndex);
 
SimTK_DEFINE_UNIQUE_INDEX_TYPE(CableSpanViaPointIndex);
 
SimTK_DEFINE_UNIQUE_INDEX_TYPE(CableSpanIndex);
 
enum class CableSpanAlgorithm
{
    Scholz2015,
    MinimumLength,
};
 
class MultibodySystem;
class CableSubsystem;
class CableSubsystemTestHelper;
 
//==============================================================================
class SimTK_SIMBODY_EXPORT CableSpan final {
public:
    CableSpan();
    ~CableSpan() noexcept = default;
    CableSpan(const CableSpan&) = default;
    CableSpan& operator=(const CableSpan& source) = default;
    CableSpan(CableSpan&&) noexcept               = default;
    CableSpan& operator=(CableSpan&&) noexcept    = default;
 
 
    CableSpan(
        CableSubsystem& subsystem,
        MobilizedBodyIndex originBody,
        const Vec3& originStation,
        MobilizedBodyIndex terminationBody,
        const Vec3& terminationStation);
 
    CableSpanObstacleIndex addObstacle(
        MobilizedBodyIndex obstacleBody,
        const Transform& X_BS,
        std::shared_ptr<const ContactGeometry> obstacleGeometry);
 
    CableSpanObstacleIndex addObstacle(
        MobilizedBodyIndex obstacleBody,
        const Transform& X_BS,
        std::shared_ptr<const ContactGeometry> obstacleGeometry,
        const Vec3& contactPointHint_S);
 
    int getNumObstacles() const;
 
    CableSpanViaPointIndex addViaPoint(
        MobilizedBodyIndex viaPointBody,
        const Vec3& station_B);
 
    int getNumViaPoints() const;
 
    CableSpanIndex getIndex() const;
 
 
 
    MobilizedBodyIndex getOriginBodyIndex() const;
 
    void setOriginBodyIndex(MobilizedBodyIndex originBody);
 
    MobilizedBodyIndex getTerminationBodyIndex() const;
 
    void setTerminationBodyIndex(MobilizedBodyIndex terminationBody);
 
    Vec3 getOriginStation() const;
 
    void setOriginStation(const Vec3& originStation);
 
    Vec3 getTerminationStation() const;
 
    void setTerminationStation(const Vec3& terminationStation);
 
 
 
    UnitVec3 calcOriginTangentDirection(const State& state) const;
 
    void calcOriginUnitForce(
        const State& state,
        SpatialVec& unitForce_G) const;
 
    UnitVec3 calcTerminationTangentDirection(const State& state) const;
 
    void calcTerminationUnitForce(
        const State& state,
        SpatialVec& unitForce_G) const;
 
 
 
    const MobilizedBodyIndex& getObstacleMobilizedBodyIndex(
        CableSpanObstacleIndex ix) const;
 
    void setObstacleMobilizedBodyIndex(
        CableSpanObstacleIndex ix,
        MobilizedBodyIndex body);
 
    const Transform& getObstacleTransformSurfaceToBody(
        CableSpanObstacleIndex ix) const;
 
    void setObstacleTransformSurfaceToBody(
        CableSpanObstacleIndex ix,
        const Transform& X_BS);
 
    const ContactGeometry& getObstacleContactGeometry(
        CableSpanObstacleIndex ix) const;
 
    void setObstacleContactGeometry(
        CableSpanObstacleIndex ix,
        std::shared_ptr<const ContactGeometry> obstacleGeometry);
 
    Vec3 getObstacleContactPointHint(CableSpanObstacleIndex ix) const;
 
    void setObstacleContactPointHint(
        CableSpanObstacleIndex ix,
        Vec3 contactPointHint_S);
 
 
 
    const MobilizedBodyIndex& getViaPointMobilizedBodyIndex(
        CableSpanViaPointIndex ix) const;
 
    void setViaPointMobilizedBodyIndex(
        CableSpanViaPointIndex ix,
        MobilizedBodyIndex body);
 
    Vec3 getViaPointStation(CableSpanViaPointIndex ix) const;
 
    void setViaPointStation(
        CableSpanViaPointIndex ix,
        const Vec3& station_B);
 
 
 
    Real getCurveSegmentAccuracy() const;
 
    void setCurveSegmentAccuracy(Real accuracy);
 
    int getSolverMaxIterations() const;
 
    void setSolverMaxIterations(int maxIterations);
 
    int getNumSolverIterations(const State& state) const;
 
    Real getSmoothnessTolerance() const;
 
    void setSmoothnessTolerance(Real tolerance);
 
    Real getSmoothness(const State& state) const;
 
    void setAlgorithm(CableSpanAlgorithm algorithm);
 
    bool getUseWarmStart() const;
 
    void setUseWarmStart(bool useWarmStart);
 
 
 
    Real calcLength(const State& state) const;
 
    Real calcLengthDot(const State& state) const;
 
    void applyBodyForces(
        const State& state,
        Real tension,
        Vector_<SpatialVec>& bodyForcesInG) const;
 
    Real calcCablePower(const State& state, Real tension) const;
 
    void calcDecorativePathPoints(
        const State& state,
        const std::function<void(Vec3 point_G)>& sink) const;
 
    void calcResampledDecorativePathPoints(
        const State& state,
        int numCurveSegmentSamples,
        const std::function<void(Vec3 point_G)>& sink) const;
 
 
 
    bool isInContactWithObstacle(const State& state, CableSpanObstacleIndex ix)
        const;
 
    Transform calcCurveSegmentInitialFrenetFrame(
        const State& state,
        CableSpanObstacleIndex ix) const;
 
    Transform calcCurveSegmentFinalFrenetFrame(
        const State& state,
        CableSpanObstacleIndex ix) const;
 
    Real calcCurveSegmentArcLength(
        const State& state,
        CableSpanObstacleIndex ix) const;
 
    void calcCurveSegmentResampledPoints(
        const State& state,
        CableSpanObstacleIndex ix,
        int numSamples,
        const std::function<void(Vec3 point_G)>& sink) const;
 
    void calcCurveSegmentUnitForce(
        const State& state,
        CableSpanObstacleIndex ix,
        SpatialVec& unitForce_G) const;
 
 
 
    Vec3 calcViaPointLocation(
        const State& state,
        CableSpanViaPointIndex ix) const;
 
    void calcViaPointUnitForce(
        const State& state,
        CableSpanViaPointIndex ix,
        SpatialVec& unitForce_G) const;
 
    UnitVec3 calcViaPointIncomingTangentDirection(
        const State& state,
        CableSpanViaPointIndex ix) const;
 
    UnitVec3 calcViaPointOutgoingTangentDirection(
        const State& state,
        CableSpanViaPointIndex ix) const;
 
 
    class Impl;
 
private:
    const Impl& getImpl() const
    {
        return *m_impl;
    }
 
    Impl& updImpl()
    {
        return *m_impl;
    }
 
    std::shared_ptr<Impl> m_impl;
 
    friend CableSubsystem;
 
    // Befriend the helper class for testing the implementation.
    friend CableSubsystemTestHelper;
};
 
//==============================================================================
class SimTK_SIMBODY_EXPORT CableSubsystem : public Subsystem {
public:
    explicit CableSubsystem(MultibodySystem&);
 
    const MultibodySystem& getMultibodySystem() const;
 
    int getNumCables() const;
 
    const CableSpan& getCable(CableSpanIndex ix) const;
 
    CableSpan& updCable(CableSpanIndex ix);
 
    SimTK_PIMPL_DOWNCAST(CableSubsystem, Subsystem);
 
    class Impl;
    Impl& updImpl();
    const Impl& getImpl() const;
 
    // Befriend the helper class for testing the implementation.
    friend CableSubsystemTestHelper;
};
 
//==============================================================================
class SimTK_SIMBODY_EXPORT CableSubsystemTestHelper {
public:
    CableSubsystemTestHelper();
    ~CableSubsystemTestHelper() noexcept;
    CableSubsystemTestHelper(const CableSubsystemTestHelper&);
    CableSubsystemTestHelper& operator=(const CableSubsystemTestHelper&);
    CableSubsystemTestHelper(CableSubsystemTestHelper&&) noexcept;
    CableSubsystemTestHelper& operator=(CableSubsystemTestHelper&&) noexcept;
 
    void testCurrentPath(
        const State& state,
        const CableSubsystem& subsystem,
        std::ostream& testReport) const;
 
    class Impl;
 
private:
    std::unique_ptr<Impl> m_impl;
};
} // namespace SimTK
 
#endif