SuctionGripper.h

Go to the documentation of this file.

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material unless having a written signed agreement with Algoryx Simulation AB, or having been
advised so by Algoryx Simulation AB for a time limited evaluation, or having purchased a
valid commercial license from Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
 
#pragma once
 
#include <agxModel/export.h>
 
#include <agx/Real.h>
#include <agx/Frame.h>
#include <agx/Vec2.h>
#include <agx/Constraint.h>
#include <agx/Referenced.h>
#include <agxUtil/agxUtil.h>
#include <agxUtil/TrimeshDeformer.h>
#include <agxStream/Serializable.h>
#include <agxSDK/Assembly.h>
#include <agxCollide/Line.h>
#include <agxCollide/Sphere.h>
#include <agxCollide/Mesh.h>
#include <agx/LockJoint.h>
 
namespace agxSDK
{
  class Simulation;
}
 
 
namespace agxModel
{
 
 
  AGX_DECLARE_POINTER_TYPES(SuctionCupSensorFilter);
  class SuctionCupSensorFilter : public agxSDK::ExecuteFilter
  {
  public:
 
    SuctionCupSensorFilter()
    {}
 
    void addGeometry(agxCollide::Geometry* geometry)
    {
      m_collisionGeometries.push_back(geometry);
    }
 
    const agxCollide::GeometryRefVector& getGeometries() const
    {
      return m_collisionGeometries;
    }
 
    virtual bool match(const agxCollide::Geometry* g0, const agxCollide::Geometry* g1) const override
    {
      const agxCollide::Geometry* geos[] = { g0, g1 };
 
      for (size_t j = 0; j < 2; ++j)
      {
        for (size_t i = 0; i < m_collisionGeometries.size(); ++i)
        {
          if (geos[j] == m_collisionGeometries[i])
            return true;
        }
      }
      return false;
    }
 
    using ExecuteFilter::match;
 
  protected:
    virtual ~SuctionCupSensorFilter()
    {
      m_collisionGeometries.clear();
    };
 
 
  protected:
    agxCollide::GeometryRefVector m_sensors;
    agxCollide::GeometryRefVector m_collisionGeometries;
  };
 
 
 
  AGX_DECLARE_POINTER_TYPES(SuctionCup);
  AGX_DECLARE_VECTOR_TYPES(SuctionCup);
 
  AGX_DECLARE_POINTER_TYPES(VacuumSystem);
  AGX_DECLARE_VECTOR_TYPES(VacuumSystem);
 
  AGX_DECLARE_POINTER_TYPES(SuctionCupInteraction);
  AGX_DECLARE_VECTOR_TYPES(SuctionCupInteraction);
  class AGXMODEL_EXPORT SuctionCupInteraction : public agx::Referenced
  {
    public:
      SuctionCupInteraction();
 
 
      size_t getSealCount() const;
 
      bool getCentered() const;
 
      agx::Real getCenterDistance() const;
 
      agx::Vec3 getCenterNormalWorld() const;
 
      agx::Real getCalculatedScalarForce() const;
 
      agx::Vec3 getCalculatedForce() const;
 
      agx::Vec3 getFoundSurfacePositionWorld() const;
 
      agx::RigidBody* getRigidBody();
 
      agx::Vec3 getCurrentFrictionForce() const;
 
      agx::Vec3 getCurrentContactNormalForce() const;
 
      agx::Real getCurrentRelativeVelocity() const;
 
 
    protected:
      virtual ~SuctionCupInteraction();
 
      friend class SuctionCup;
      friend class SuctionGripper;
 
      /*
      Update the latest position found for where to apply the cvacuum force
      */
      void setFoundSurfacePositionWorld(agx::Vec3 worldPosition);
 
      void setRigidBody(agx::RigidBody* body);
 
      void updateEstimatedForcePosition(const SuctionCup* cup, agx::Real depth);
 
      void setCenterDistance(agx::Real distance);
 
      void setCenterNormalWorld(agx::Vec3 normal);
 
      void updateInteractedSeals(size_t sealIndex, agx::Real depth);
 
      void setCalculatedForce(agx::Vec3 force);
      void setCalculatedScalarForce(agx::Real scalarForce);
 
      agx::Real getSealDepth(size_t sealIndex) const;
 
      bool hasSealInteraction(size_t sealIndex) const;
 
      virtual bool updateVacuumForce(const SuctionCup* cup, const VacuumSystem* vacuumSystem, agx::Real fraction);
 
      void reset();
 
      void addTemporaryGeometryContact(agxCollide::GeometryContact* gc);
 
      void renderInteraction(const SuctionCup* cup);
 
      bool m_centered;
      agx::Real m_centerDistance;
      agx::Vec3 m_centerNormalWorld;
      agx::Vec3 m_foundSurfacePosition;
      agx::Real m_calculatedScalarForce;
      agx::Vec3 m_calculatedForce;
      agx::Vec3 m_calculatedForcePosition;
      agx::RigidBodyObserver m_body;
      agx::HashVector<size_t, agx::Real> m_interactedSealDepth;
      agxCollide::GeometryContactPtrVector m_tempContacts;
  };
 
 
  AGX_DECLARE_POINTER_TYPES(SuctionGripper);
  AGX_DECLARE_VECTOR_TYPES(SuctionGripper);
 
  class AGXMODEL_EXPORT VacuumSystem : public virtual agx::Referenced, public virtual agxStream::Serializable
  {
    public:
      VacuumSystem(agx::Real desiredVacuum);
 
      agx::Real calculateMaximumSuctionForce(SuctionCup* cup) const;
 
      virtual agx::Real calculateAvailableVacuumForce(SuctionCup* cup) const;
 
      virtual void step(agx::Real dt, const SuctionGripper* gripper);
 
      void setEnablePump(const bool& enable);
 
      bool getEnablePump() const;
 
      agx::Real getCurrentVacuum() const;
 
      void setDesiredVacuum(agx::Real vacuumLevel);
 
      agx::Real getDesiredVacuum() const;
 
      void setOutsidePressure(agx::Real pressure);
 
      agx::Real getOutsidePressure() const;
 
      agx::Real calculateGasSpeedAtLip(const SuctionCup* cup) const;
 
      void setOutsideGasDensity(agx::Real density);
 
      agx::Real getOutsideGasDensity() const;
 
      AGXSTREAM_DECLARE_SERIALIZABLE(agxModel::VacuumSystem);
 
    protected:
      virtual ~VacuumSystem();
      VacuumSystem();
      void setCurrentVacuum(agx::Real vacuum);
      agx::Real m_desiredVacuum;
      agx::Real m_currentVacuum;
      bool m_pumpEnabled;
      agx::Real m_timeOfPumpStateToggled;
      agx::Real m_currentTime;
      agx::Real m_outsidePressure;
      agx::Real m_outsideGasDensity;
  };
 
 
  typedef agx::HashSet< SuctionCupRef > SuctionCupRefSet;
  class AGXMODEL_EXPORT SuctionCup : public virtual agx::Referenced, public virtual agxStream::Serializable
  {
  public:
    SuctionCup(agx::RigidBody* cupBody,
      agx::Real lipRadius = agx::Real(0.025),
      agx::Real lipHeight = agx::Real(0.01),
      size_t sealResolution = size_t(0),
      agx::Vec3 localLineSensorVector = agx::Vec3(0,0,-0.1),
      agx::Real sealSensorReach = agx::Real(0.01));
 
 
    agx::Vec3 getNormal() const;
 
    agx::Real getArea() const;
 
    const agx::RigidBody* getRigidBody() const;
 
    agx::RigidBody* getRigidBody();
 
    agx::Real getLipRadius() const;
 
    void setMountingRadius(agx::Real radius);
 
    agx::Real getMountingRadius() const;
 
    bool isValid() const;
 
    agx::Real getCurrentLeakageArea() const;
 
    size_t getSealResolution() const;
 
    agx::Real getSensorDistance() const;
 
    SuctionCupSensorFilter* getSensorFilter();
 
    agx::Real getAvailableVacuumForce() const;
 
    const agxCollide::GeometryRefSetVector& getSealSensors() const;
 
    agx::Vector < agxModel::SuctionCupInteraction*> getActiveInteractionsVector() const;
 
    virtual void updateLeakageArea();
 
    void applyVacuumForces(const VacuumSystem* vacuumSystem);
 
 
    void updateVacuumForces(const VacuumSystem* vacuumSystem);
 
 
    AGXSTREAM_DECLARE_SERIALIZABLE(agxModel::SuctionCup);
 
  protected:
    SuctionCup();
 
    virtual ~SuctionCup();
 
    // Contact events.
    friend class agxUtil::GeneralContactEventListener< SuctionCup >;
 
    friend class SuctionGripper;
 
    void invalidateInteractions();
 
    agx::RigidBody* createDefaultCupBody();
 
    const agx::HashVector<agx::RigidBody*, SuctionCupInteractionRef>& getActiveInteractions() const;
 
    const SuctionCupInteractionRefVector& getDisabledInteractions() const;
 
    virtual agxModel::SuctionCupInteraction* createInteraction();
 
    void addNotification(agxSDK::Simulation* simulation);
    void removeNotification(agxSDK::Simulation* simulation);
 
 
    friend class SuctionGripper;
 
    agxSDK::ContactEventListener::KeepContactPolicy impact(
      const agx::TimeStamp&, agxCollide::GeometryContact* gc);
 
    agxSDK::ContactEventListener::KeepContactPolicy contact(
      const agx::TimeStamp&, agxCollide::GeometryContact* gc);
 
    void separation(
      const agx::TimeStamp&, agxCollide::GeometryPair&);
 
    agxSDK::ContactEventListener::KeepContactPolicy handleContacts(
      const agx::TimeStamp&, agxCollide::GeometryContact* gc);
 
    agx::Vec3 calculateLineP2() const;
    void createLineSensor();
    agxCollide::Geometry* createSealSensor(agx::RigidBody* body, agx::Vec3 bodyLocalPosition);
    void createCupSealSensors();
 
    agx::Vec3 findSurfaceNormal(agxCollide::Shape* collidedShape,
                                agx::Vec3 pointAtSurface,
                                bool& success);
 
    SuctionCupInteraction* getActiveInteraction(agx::RigidBody* rigidBody);
 
    void collectCollisionGeometries();
 
  protected:
    agx::RigidBodyRef m_body;
    agx::Real m_lipRadius;
    agx::Real m_lipHeight;
    agx::Real m_mountingRadius;
    agx::Real m_currentLeakageArea;
    size_t m_sealResolution;
    agx::Vec3 m_localLineSensorVector;
    agx::Real m_sealSensorReach;
    agx::ref_ptr<agxCollide::Line> m_line;
    agx::AffineMatrix4x4 m_miniSphereTransform;
    agxCollide::SphereRef m_miniSphere;
    agxCollide::GeometryRef m_lineSensor;
    SuctionCupSensorFilterRef m_collisionFilter;
    agx::Real m_calculatedAvailableVacuumForce;
    agxCollide::GeometryRefSetVector m_sealSensors;
    agx::RealVector m_sealDepthMeasurements;
    agx::ref_ptr< agxUtil::GeneralContactEventListener< SuctionCup > > m_contactCallback;
    // Implementation dependent temporary variables, only valid per time step.
    agx::HashVector<agx::RigidBody*, SuctionCupInteractionRef> m_activeInteractions;
    SuctionCupInteractionRefVector m_interactionsPool;
 
  };
 
  class AGXMODEL_EXPORT SuctionGripper : public agxSDK::Assembly
  {
    public:
      SuctionGripper( agx::RigidBody* holderBody );
 
      bool setHolderRigidBody(agx::RigidBody* holderBody);
 
      agx::RigidBody* getHolderRigidBody();
 
      const agx::RigidBody* getHolderRigidBody() const;
 
      const SuctionCupRefVector& getSuctionCups() const;
 
      SuctionCupRefVector& getSuctionCups();
 
      void addSuctionCup(SuctionCup* cup);
 
      bool isValid() const;
 
      agx::Real getCurrentLeakage() const;
 
      const VacuumSystem* getVacuumSystem() const;
 
      VacuumSystem* getVacuumSystem();
 
      void setVacuumSystem(VacuumSystem* vacuumSystem);
 
      static agx::LockJoint* positionAndLockSuctionCup( agxModel::SuctionGripper* gripper,
                                                        agxModel::SuctionCup* cup,
                                                        agx::Vec3 holderLocalTranslate,
                                                        agx::Vec3 cupLocalTranslate);
 
      AGXSTREAM_DECLARE_SERIALIZABLE(SuctionGripper);
 
    protected:
      SuctionGripper();
 
      virtual void updateValid();
 
      virtual ~SuctionGripper();
 
      // Adding to/removing from simulation.
      virtual void addNotification( agxSDK::Simulation* simulation ) override;
 
      using agxSDK::Assembly::addNotification;
 
      virtual void removeNotification( agxSDK::Simulation* simulation )override;
 
      using agxSDK::Assembly::removeNotification;
 
 
      // Step events.
      friend class agxUtil::GeneralStepListener< SuctionGripper >;
 
      virtual void preCollide(const agx::TimeStamp&);
      virtual void pre(const agx::TimeStamp&);
      virtual void post(const agx::TimeStamp&);
      virtual void last(const agx::TimeStamp&);
 
 
 
    protected:
      // More persistent variables, will be serialized.
      agx::RigidBodyRef m_holderBody;
      bool m_valid;
      VacuumSystemRef m_vacuumSystem;
      agx::ref_ptr< agxUtil::GeneralStepListener< SuctionGripper > > m_stepCallback;
      agxModel::SuctionCupRefVector m_suctionCups;
 
  };
 
  AGX_DECLARE_POINTER_TYPES(SingleCupSuctionGripper);
  AGX_DECLARE_VECTOR_TYPES(SingleCupSuctionGripper);
  class AGXMODEL_EXPORT SingleCupSuctionGripper : public SuctionGripper
  {
  public:
    SingleCupSuctionGripper(size_t sealResolution,
      agx::Real gripperRadius = agx::Real(0.025),
      agx::Real gripperHeight = agx::Real(0.01),
      agx::Real cupHeight = agx::Real(0.05),
      agx::Real lipHeight = agx::Real(0.01),
      agxCollide::Trimesh* deformableMesh = nullptr,
      agx::Material* cupMaterial = nullptr,
      agx::RigidBody* holderBody = nullptr,
      agx::RigidBody* cupBody = nullptr);
 
    agx::LockJoint* getLockJoint() const;
 
    agxCollide::Geometry* getTrimeshGeometry() const;
 
    agxCollide::Trimesh* getDeformableTrimesh() const;
 
    agxUtil::TrimeshDeformer* getTrimeshDeformer() const;
 
    AGXSTREAM_DECLARE_SERIALIZABLE(SingleCupSuctionGripper);
 
  protected:
    agx::RigidBody* createDefaultHolderBody(agx::Real radius, agx::Real height) const;
    void setupDeformableTrimesh(SuctionCup* cup, agx::Frame* holderFrame);
 
    SingleCupSuctionGripper();
    virtual ~SingleCupSuctionGripper();
 
    agx::ref_ptr<agx::LockJoint> m_lock;
    agxCollide::TrimeshRef m_trimesh;
    agxCollide::GeometryRef m_trimeshGeometry;
    agxUtil::TrimeshDeformerRef m_trimeshDeformer;
 
  };
}