Merge branch 'main' into feature/fsi

src/chrono/functions/ChFunctionSequence.cpp

@@ -100,6 +100,8 @@ void ChFseqNode::ArchiveIn(ChArchiveIn& archive_in) {
 // Register into the object factory, to enable run-time dynamic creation and persistence
 CH_FACTORY_REGISTER(ChFunctionSequence)
 
+ChFunctionSequence::ChFunctionSequence() : m_start(0), m_clamped(false) {}
+
 ChFunctionSequence::ChFunctionSequence(const ChFunctionSequence& other) {
     m_start = other.m_start;
     m_functions = other.m_functions;
@@ -253,14 +255,12 @@ double ChFunctionSequence::GetVal(double x) const {
     double res = 0;
     double localtime = 0;
 
-    if (x < 0) {  // clamp to zero before start
-        res = 0;
-    } else if (x >= m_functions.back().t_end) { // clamp to last node value after end
+    if (m_clamped && x >= m_functions.back().t_end) {  // clamp to last node value after end
         auto last_node = &m_functions.back();
         localtime = m_functions.back().duration;
         res = last_node->fx->GetVal(localtime) + last_node->Iy + last_node->Iydt * localtime +
               last_node->Iydtdt * localtime * localtime;
-    } else {
+    } else {  // find current node value
         for (auto iter = m_functions.begin(); iter != m_functions.end(); ++iter) {
             if ((x >= iter->t_start) && (x < iter->t_end)) {
                 localtime = x - iter->t_start;
@@ -315,6 +315,7 @@ void ChFunctionSequence::ArchiveOut(ChArchiveOut& archive_out) {
     // serialize all member data:
     archive_out << CHNVP(m_start);
     archive_out << CHNVP(m_functions);
+    archive_out << CHNVP(m_clamped);
 }
 
 void ChFunctionSequence::ArchiveIn(ChArchiveIn& archive_in) {
@@ -325,6 +326,7 @@ void ChFunctionSequence::ArchiveIn(ChArchiveIn& archive_in) {
     // stream in all member data:
     archive_in >> CHNVP(m_start);
     archive_in >> CHNVP(m_functions);
+    archive_in >> CHNVP(m_clamped);
 }
 
 }  // end namespace chrono

src/chrono/functions/ChFunctionSequence.h

@@ -64,10 +64,12 @@ CH_CLASS_VERSION(ChFseqNode, 0)
 ///   `y = sequence_of_functions(f1(y), f2(y), f3(y))`
 /// All other function types can be inserted into this.
 class ChApi ChFunctionSequence : public ChFunction {
-public:
-    ChFunctionSequence() : m_start(0) {}
+  public:
+    ChFunctionSequence();
+
     ChFunctionSequence(const ChFunctionSequence& other);
-    ~ChFunctionSequence() {}
+
+    virtual ~ChFunctionSequence() {}
 
     /// "Virtual" copy constructor (covariant return type).
     virtual ChFunctionSequence* Clone() const override { return new ChFunctionSequence(*this); }
@@ -136,15 +138,22 @@ class ChApi ChFunctionSequence : public ChFunction {
 
     virtual double GetWeight(double x) const override;
 
+    /// Clamp end return value of sequence to last node value.
+    void SetClamped(bool clamped) { m_clamped = clamped; }
+
+    /// Get if sequence end return value is clamped.
+    bool IsClamped() const { return m_clamped; }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 
     /// Method to allow de-serialization of transient data from archives.
     virtual void ArchiveIn(ChArchiveIn& archive_in) override;
 
-protected:
+  protected:
     std::list<ChFseqNode> m_functions;  ///< the list of sub functions
     double m_start;                     ///< start time for sequence
+    bool m_clamped;                     ///< trigger end value clamp
 };
 
 /// @} chrono_functions

src/chrono/physics/ChLinkMate.h

@@ -245,6 +245,8 @@ class ChApi ChLinkMatePlanar : public ChLinkMateGeneric {
     /// Get the requested distance between the two planes, in normal direction.
     double GetDistance() const { return m_distance; }
 
+    using ChLinkMateGeneric::Initialize;
+
     /// Initialize the link by providing a point and a normal direction on each plane, each expressed in body or abs
     /// reference. Normals can be either aligned or opposed depending on the SetFlipped() method.
     virtual void Initialize(std::shared_ptr<ChBodyFrame> body1,  ///< first body to link
@@ -329,12 +331,12 @@ class ChApi ChLinkMateRevolute : public ChLinkMateGeneric {
     /// "Virtual" copy constructor (covariant return type).
     virtual ChLinkMateRevolute* Clone() const override { return new ChLinkMateRevolute(*this); }
 
-    using ChLinkMateGeneric::Initialize;
-
     /// Tell if the two axes must be opposed (flipped=true) or must have the same verse (flipped=false)
     void SetFlipped(bool doflip);
     bool IsFlipped() const { return m_flipped; }
 
+    using ChLinkMateGeneric::Initialize;
+
     /// Specialized initialization for revolute mate, given the two bodies to be connected, two points, two directions
     /// (each expressed in body or abs. coordinates). These two directions are the Z axes of slave frame F1 and master
     /// frame F2
@@ -381,12 +383,12 @@ class ChApi ChLinkMatePrismatic : public ChLinkMateGeneric {
     /// "Virtual" copy constructor (covariant return type).
     virtual ChLinkMatePrismatic* Clone() const override { return new ChLinkMatePrismatic(*this); }
 
-    using ChLinkMateGeneric::Initialize;
-
     /// Tell if the two axes must be opposed (flipped=true) or must have the same verse (flipped=false)
     void SetFlipped(bool doflip);
     bool IsFlipped() const { return m_flipped; }
 
+    using ChLinkMateGeneric::Initialize;
+
     /// Specialized initialization for prismatic mate, given the two bodies to be connected, two points, two directions.
     /// These two directions are the X axes of secondary frame F1 and principal frame F2.
     /// All quantities can be expressed in body or in absolute coordinates.
@@ -511,6 +513,8 @@ class ChApi ChLinkMateParallel : public ChLinkMateGeneric {
     void SetFlipped(bool doflip);
     bool IsFlipped() const { return m_flipped; }
 
+    using ChLinkMateGeneric::Initialize;
+
     /// Specialized initialization for parallel mate, given the two bodies to be connected, two points and two
     /// directions (each expressed in body or abs. coordinates).
     virtual void Initialize(std::shared_ptr<ChBodyFrame> body1,  ///< first body to link
@@ -549,6 +553,8 @@ class ChApi ChLinkMateOrthogonal : public ChLinkMateGeneric {
     /// "Virtual" copy constructor (covariant return type).
     virtual ChLinkMateOrthogonal* Clone() const override { return new ChLinkMateOrthogonal(*this); }
 
+    using ChLinkMateGeneric::Initialize;
+
     /// Specialized initialization for orthogonal mate, given the two bodies to be connected, two points and two
     /// directions (each expressed in body or abs. coordinates).
     virtual void Initialize(std::shared_ptr<ChBodyFrame> body1,  ///< first body to link

src/chrono/timestepper/ChTimestepperHHT.h

@@ -15,6 +15,8 @@
 #ifndef CHTIMESTEPPER_HHT_H
 #define CHTIMESTEPPER_HHT_H
 
+#include <array>
+
 #include "chrono/timestepper/ChTimestepper.h"
 
 namespace chrono {

src/chrono_models/robot/industrial/IndustrialKinematics.h

@@ -16,8 +16,8 @@
 //
 // =============================================================================
 
-#ifndef CH_INDUSTRIAL_ROBOT_KINEMATICS_H
-#define CH_INDUSTRIAL_ROBOT_KINEMATICS_H
+#ifndef INDUSTRIAL_ROBOT_KINEMATICS_H
+#define INDUSTRIAL_ROBOT_KINEMATICS_H
 
 #include "chrono_models/ChApiModels.h"
 #include "chrono/core/ChCoordsys.h"
@@ -50,4 +50,4 @@ class CH_MODELS_API IndustrialKinematics {
 }  // end namespace industrial
 }  // end namespace chrono
 
-#endif  // end CH_INDUSTRIAL_ROBOT_KINEMATICS_H
+#endif  // end INDUSTRIAL_ROBOT_KINEMATICS_H

src/chrono_models/robot/industrial/IndustrialKinematics6dofSpherical.h

@@ -16,8 +16,8 @@
 //
 // =============================================================================
 
-#ifndef CH_INDUSTRIAL_ROBOT_KINEMATICS_6DOF_SPHERICAL_H
-#define CH_INDUSTRIAL_ROBOT_KINEMATICS_6DOF_SPHERICAL_H
+#ifndef INDUSTRIAL_ROBOT_KINEMATICS_6DOF_SPHERICAL_H
+#define INDUSTRIAL_ROBOT_KINEMATICS_6DOF_SPHERICAL_H
 
 #include "IndustrialKinematics.h"
 
@@ -85,4 +85,4 @@ class CH_MODELS_API IndustrialKinematics6dofSpherical : public IndustrialKinemat
 }  // end namespace industrial
 }  // end namespace chrono
 
-#endif  // end CH_INDUSTRIAL_ROBOT_KINEMATICS_6DOF_SPHERICAL_H
+#endif  // end INDUSTRIAL_ROBOT_KINEMATICS_6DOF_SPHERICAL_H

src/chrono_models/robot/industrial/IndustrialKinematicsNdofNumerical.h

@@ -16,8 +16,8 @@
 //
 // =============================================================================
 
-#ifndef CH_INDUSTRIAL_ROBOT_KINEMATICS_NDOF_NUMERICAL_H
-#define CH_INDUSTRIAL_ROBOT_KINEMATICS_NDOF_NUMERICAL_H
+#ifndef INDUSTRIAL_ROBOT_KINEMATICS_NDOF_NUMERICAL_H
+#define INDUSTRIAL_ROBOT_KINEMATICS_NDOF_NUMERICAL_H
 
 #include "IndustrialKinematics.h"
 
@@ -92,4 +92,4 @@ class CH_MODELS_API IndustrialKinematicsNdofNumerical : public IndustrialKinemat
 }  // end namespace industrial
 }  // end namespace chrono
 
-#endif  // end CH_INDUSTRIAL_ROBOT_KINEMATICS_NDOF_NUMERICAL_H
+#endif  // end INDUSTRIAL_ROBOT_KINEMATICS_NDOF_NUMERICAL_H

src/chrono_models/robot/industrial/IndustrialKinematicsSCARA.h

@@ -16,8 +16,8 @@
 //
 // =============================================================================
 
-#ifndef CH_INDUSTRIAL_ROBOT_KINEMATICS_SCARA_H
-#define CH_INDUSTRIAL_ROBOT_KINEMATICS_SCARA_H
+#ifndef INDUSTRIAL_ROBOT_KINEMATICS_SCARA_H
+#define INDUSTRIAL_ROBOT_KINEMATICS_SCARA_H
 
 #include "IndustrialKinematics.h"
 
@@ -72,4 +72,4 @@ class CH_MODELS_API IndustrialKinematicsSCARA : public IndustrialKinematics {
 }  // end namespace industrial
 }  // end namespace chrono
 
-#endif  // end CH_INDUSTRIAL_ROBOT_KINEMATICS_SCARA_H
+#endif  // end INDUSTRIAL_ROBOT_KINEMATICS_SCARA_H

src/chrono_models/robot/industrial/IndustrialRobot.cpp

@@ -31,9 +31,10 @@ void IndustrialRobot::SetSetpoints(double setpoint, double t) {
         m_motfunlist[i]->SetSetpoint(setpoint, t);
 }
 
-void IndustrialRobot::DisableMotors(bool disable) {
+void IndustrialRobot::SetMotorsDisabled(bool disable) {
+    m_motors_disabled = disable;
     for (const auto& motor : m_motorlist) {
-        motor->SetDisabled(disable);
+        motor->SetDisabled(m_motors_disabled);
     }
 }
 
@@ -43,28 +44,51 @@ void IndustrialRobot::SetBaseFrame(const ChFramed& base_frame) {
     m_sys->Update();
 }
 
+AssemblyAnalysis::ExitFlag IndustrialRobot::SetPoseTCP(const ChFramed& target_frame, unsigned int max_iters) {
+    bool original_motors_disabled = m_motors_disabled;
+
+    // Temporary disable motors to guide end-effector
+    if (!original_motors_disabled)
+        SetMotorsDisabled(true);
+
+    // Add temporary guide link
+    auto link_teach = chrono_types::make_shared<ChLinkMateFix>();
+    link_teach->Initialize(m_end_effector, m_base, false, m_marker_TCP->GetAbsFrame(), target_frame);
+    m_sys->Add(link_teach);
+    auto exit_flag = m_sys->DoAssembly(AssemblyAnalysis::Level::POSITION, max_iters);
+
+    // Remove temporary link
+    m_sys->Remove(link_teach);
+
+    // Update motor setpoints and restore original activation state
+    SetSetpoints(GetMotorsPos(), m_sys->GetChTime());
+    SetMotorsDisabled(original_motors_disabled);
+
+    return exit_flag;
+}
+
 void IndustrialRobot::SetColor(const ChColor& col) {
     for (unsigned int i = 0; i < m_bodylist.size(); ++i)
         if (m_bodylist[i]->GetVisualModel())
             for (unsigned int j = 0; j < m_bodylist[i]->GetVisualModel()->GetNumShapes(); ++j)
                 m_bodylist[i]->GetVisualShape(j)->SetColor(col);
 }
 
-void IndustrialRobot::AttachBody(std::shared_ptr<ChBody> slave,
-                                 std::shared_ptr<ChBody> master,
+void IndustrialRobot::AttachBody(std::shared_ptr<ChBody> body_attach,
+                                 std::shared_ptr<ChBody> robot_body,
                                  const ChFrame<>& frame) {
     m_body_attached = true;
-    m_link_attach->Initialize(slave, m_bodylist.back(), frame);
-    slave->SetFixed(false);
+    m_link_attach->Initialize(body_attach, robot_body, frame);
+    body_attach->SetFixed(false);
     m_sys->AddLink(m_link_attach);
     m_sys->Update();
 }
 
-void IndustrialRobot::DetachBody(std::shared_ptr<ChBody> slave, bool setfix) {
+void IndustrialRobot::DetachBody(std::shared_ptr<ChBody> body_attach, bool setfix) {
     if (m_body_attached) {
         m_body_attached = false;
         m_sys->RemoveLink(m_link_attach);
-        slave->SetFixed(setfix);
+        body_attach->SetFixed(setfix);
         m_sys->Update();
     }
 }

src/chrono_models/robot/industrial/IndustrialRobot.h

@@ -16,8 +16,8 @@
 //
 // =============================================================================
 
-#ifndef CH_INDUSTRIAL_ROBOT_H
-#define CH_INDUSTRIAL_ROBOT_H
+#ifndef INDUSTRIAL_ROBOT_H
+#define INDUSTRIAL_ROBOT_H
 
 #include "chrono_models/ChApiModels.h"
 #include "chrono/physics/ChSystem.h"
@@ -43,16 +43,25 @@ class CH_MODELS_API IndustrialRobot {
     ChSystem* GetSystem() { return m_sys; }
 
     /// Get the list of bodies in robot model.
-    std::vector<std::shared_ptr<ChBody>> GetBodylist() { return m_bodylist; }
+    std::vector<std::shared_ptr<ChBody>> GetBodies() { return m_bodylist; }
 
     /// Get the list of joint markers in robot model.
-    std::vector<std::shared_ptr<ChMarker>> GetMarkerlist() { return m_markerlist; }
+    std::vector<std::shared_ptr<ChMarker>> GetMarkers() { return m_markerlist; }
 
     /// Get the list of motor functions in robot model.
-    std::vector<std::shared_ptr<ChFunctionSetpoint>> GetMotfunlist() { return m_motfunlist; }
+    std::vector<std::shared_ptr<ChFunctionSetpoint>> GetMotfuns() { return m_motfunlist; }
 
-    /// Get the lsit of motors in robot model.
-    std::vector<std::shared_ptr<ChLinkMotor>> GetMotorlist() { return m_motorlist; }
+    /// Get the list of motors in robot model.
+    std::vector<std::shared_ptr<ChLinkMotor>> GetMotors() { return m_motorlist; }
+
+    /// Get robot base body.
+    std::shared_ptr<ChBody> GetBase() { return m_base; };
+
+    /// Get robot end-effector body.
+    std::shared_ptr<ChBody> GetEndEffector() { return m_end_effector; };
+
+    /// Get robot Tool-Center-Point marker.
+    std::shared_ptr<ChMarker> GetMarkerTCP() const { return m_marker_TCP; }
 
     /// Get motors rotations/positions, depending on specific robot architecture.
     /// Optionally wrap angles in interval [-PI..+PI].
@@ -75,26 +84,33 @@ class CH_MODELS_API IndustrialRobot {
     virtual double GetMass() const;
 
     /// Disable robot motors (true), or reactivate them (false).
-    void DisableMotors(bool disable);
+    void SetMotorsDisabled(bool disable);
+
+    /// Get if motors are currently disabled.
+    bool GetMotorsDisabled() const { return m_motors_disabled; }
 
     /// Set robot base coordinates and consequently update other internal frames.
     virtual void SetBaseFrame(const ChFramed& base_frame);
 
-    /// Set individual motors setpoints at given time.
+    /// Set individual motors setpoints, at given time.
     void SetSetpoints(const ChVectorDynamic<>& setpoints, double t);
 
     /// Set motors setpoints all equal to given value, at given time.
     void SetSetpoints(double setpoint, double t);
 
+    /// Move robot TCP to imposed absolute frame.
+    /// Useful to get single-shot numerical Inverse Kinematics. Not suitable for trajectory following.
+    AssemblyAnalysis::ExitFlag SetPoseTCP(const ChFramed& target_frame, unsigned int max_iters = 10);
+
     /// Set robot color (if visual shapes are added).
     void SetColor(const ChColor& col);
 
     /// Kinematically link external body to a specific robot body, in given frame.
     /// Useful for grip and relocation of some object by robot end-effector.
-    void AttachBody(std::shared_ptr<ChBody> slave, std::shared_ptr<ChBody> master, const ChFrame<>& frame);
+    void AttachBody(std::shared_ptr<ChBody> body_attach, std::shared_ptr<ChBody> robot_body, const ChFrame<>& frame);
 
     /// Unlink external body from robot (if previously attached) and potentially set it to 'fixed' thereafter.
-    void DetachBody(std::shared_ptr<ChBody> slave, bool setfix = false);
+    void DetachBody(std::shared_ptr<ChBody> body_attach, bool setfix = false);
 
     /// Update internal encoder to keep track of distance travelled by motors.
     /// NB: call this function at runtime and get reading with GetEncoderReading() function.
@@ -135,11 +151,14 @@ class CH_MODELS_API IndustrialRobot {
     ChSystem* m_sys = nullptr;  ///< containing sys
     ChFramed m_base_frame;      ///< coordinates of robot base
 
+    std::shared_ptr<ChBody> m_base, m_end_effector;
     std::vector<std::shared_ptr<ChBody>> m_bodylist;                ///< list of robot bodies
     std::vector<std::shared_ptr<ChMarker>> m_markerlist;            ///< list of robot joint markers
     std::vector<std::shared_ptr<ChFunctionSetpoint>> m_motfunlist;  ///< list of robot motors functions
     std::vector<std::shared_ptr<ChLinkMotor>> m_motorlist;          ///< list of robot motors
     std::vector<ChFramed> m_joint_frames;                           ///< list of starting joint frames
+    std::shared_ptr<ChMarker> m_marker_TCP;                         ///< Tool-Center-Point marker
+    bool m_motors_disabled = false;                                 ///< activation state of motors
 
     std::shared_ptr<ChLinkMateFix> m_link_attach = nullptr;  ///< internal link to grip external body
     bool m_body_attached = false;                            ///< flag for external body attached/not attached
@@ -151,4 +170,4 @@ class CH_MODELS_API IndustrialRobot {
 }  // end namespace industrial
 }  // end namespace chrono
 
-#endif  // end CH_INDUSTRIAL_ROBOT_H
+#endif  // end INDUSTRIAL_ROBOT_H