Implemented "push" actuator which can be used as a simple thruster fo…

…r underwater vehicles or as a general 1D force generating actuator.

Library/include/actuators/Actuator.h

@@ -34,7 +34,7 @@ namespace sf
     struct Renderable;
 
     //! An enum designating a type of the actuator.
-    enum class ActuatorType {MOTOR, SERVO, PROPELLER, THRUSTER, VBS, LIGHT, RUDDER, SUCTION_CUP};
+    enum class ActuatorType {MOTOR, SERVO, PROPELLER, THRUSTER, VBS, LIGHT, RUDDER, SUCTION_CUP, PUSH};
 
     //! An abstract class representing any actuator.
     class Actuator

Library/include/actuators/Push.h

@@ -0,0 +1,77 @@
+/*    
+    This file is a part of Stonefish.
+
+    Stonefish is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    Stonefish is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+*/
+
+//
+//  Push.h
+//  Stonefish
+//
+//  Created by Patryk Cieslak on 04/07/2023.
+//  Copyright (c) 2023 Patryk Cieslak. All rights reserved.
+//
+
+#ifndef __Stonefish_Push__
+#define __Stonefish_Push__
+
+#include "actuators/LinkActuator.h"
+
+namespace sf
+{
+    //! A class representing a thruster.
+    class Push : public LinkActuator
+    {
+    public:
+        //! A constructor.
+        /*!
+         \param uniqueName a name for the push
+         \param inverted a flag indicating if the direction of the generated force should be reversed
+         \param onlyWorksSubmerged a flag indicating if the actuator is simulating an underwater thruster
+        */
+        Push(std::string uniqueName, bool inverted = false, bool onlyWorksSubmerged = false);
+
+        //! A method used to update the internal state of the push actuator.
+        /*!
+         \param dt a time step of the simulation [s]
+         */
+        void Update(Scalar dt);
+
+        //! A method implementing the rendering of the push actuator.
+        std::vector<Renderable> Render();
+
+        //! A method used to set the force limits.
+        void setForceLimits(double lower, double upper);
+
+        //! A method setting the new value of the desired force.
+        /*!
+         \param f the desired force to be generated
+         */
+        void setForce(Scalar f);
+
+        //! A method returning the current setpoint.
+        Scalar getForce() const;
+
+        //! A method returning the type of the actuator.
+        ActuatorType getType() const;
+
+    private:
+        Scalar setpoint;
+        bool underwater;
+        bool inv;
+        std::pair<double, double> limits;
+    };
+}
+
+#endif

Library/src/actuators/Actuator.cpp

@@ -40,7 +40,7 @@ Actuator::Actuator(std::string uniqueName)
 
 Actuator::~Actuator()
 {
-    if(SimulationApp::getApp() != NULL)
+    if(SimulationApp::getApp() != nullptr)
         SimulationApp::getApp()->getSimulationManager()->getNameManager()->RemoveName(name);
 }
 

Library/src/actuators/Propeller.cpp

@@ -146,7 +146,7 @@ void Propeller::Update(Scalar dt)
 std::vector<Renderable> Propeller::Render()
 {
     Transform propTrans = Transform::getIdentity();
-    if(attach != NULL)
+    if(attach != nullptr)
         propTrans = attach->getOTransform() * o2a;
     else
         LinkActuator::Render();

Library/src/actuators/Push.cpp

@@ -0,0 +1,110 @@
+/*    
+    This file is a part of Stonefish.
+
+    Stonefish is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    Stonefish is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+*/
+
+//
+//  Push.cpp
+//  Stonefish
+//
+//  Created by Patryk Cieslak on 04/07/2023.
+//  Copyright (c) 2023 Patryk Cieslak. All rights reserved.
+//
+
+#include "actuators/Push.h"
+
+#include "core/SimulationApp.h"
+#include "core/SimulationManager.h"
+
+namespace sf
+{
+
+Push::Push(std::string uniqueName, bool inverted, bool onlyWorksSubmerged) : LinkActuator(uniqueName)
+{
+    setpoint = Scalar(0);
+    inv = inverted;
+    underwater = onlyWorksSubmerged;
+    setForceLimits(1, -1); // No limits
+}
+
+ActuatorType Push::getType() const
+{
+    return ActuatorType::PUSH;
+}
+
+void Push::setForceLimits(double lower, double upper)
+{
+    limits.first = lower;
+    limits.second = upper;
+}
+
+void Push::setForce(Scalar f)
+{
+    if(limits.second > limits.first) // Limitted
+        setpoint = f < limits.first ? limits.first : (f > limits.second ? limits.second : f);
+    else
+        setpoint = f;
+}
+
+Scalar Push::getForce() const
+{
+    return setpoint;
+}
+
+void Push::Update(Scalar dt)
+{
+    if(attach != nullptr)
+    {
+        //Get transforms
+        Transform solidTrans = attach->getCGTransform();
+        Transform pushTrans = attach->getOTransform() * o2a;
+
+        Scalar force(0);
+        if(underwater)
+        {
+            Ocean* ocn = SimulationApp::getApp()->getSimulationManager()->getOcean();
+            if(ocn != nullptr && ocn->IsInsideFluid(pushTrans.getOrigin()))
+                force = inv ? -setpoint : setpoint;
+        }
+        else
+            force = inv ? -setpoint : setpoint;
+
+        Vector3 forceV(force, 0, 0);
+        attach->ApplyCentralForce(pushTrans.getBasis() * forceV);
+        attach->ApplyTorque((pushTrans.getOrigin() - solidTrans.getOrigin()).cross(pushTrans.getBasis() * forceV));    
+    }
+}
+
+std::vector<Renderable> Push::Render()
+{
+    Transform pushTrans = Transform::getIdentity();
+    if(attach != nullptr)
+        pushTrans = attach->getOTransform() * o2a;
+    else
+        LinkActuator::Render();
+
+    //Add renderable
+    std::vector<Renderable> items(0);
+    Renderable item;
+    item.model = glMatrixFromTransform(pushTrans);  
+    item.type = RenderableType::ACTUATOR_LINES;
+    item.points.push_back(glm::vec3(0,0,0));
+    item.points.push_back(glm::vec3(0.1f*(inv ? -setpoint : setpoint),0,0));
+    items.push_back(item);
+
+    return items;
+}
+
+}

Library/src/actuators/Servo.cpp

@@ -197,7 +197,10 @@ void Servo::Update(Scalar dt)
         }
         else
         {
-            vSetpoint2 = vSetpoint;  //Kv * (vSetpoint - getVelocity()) + vSetpoint; (not natural to use)
+            //vSetpoint2 = vSetpoint;
+            Scalar err = vSetpoint - getVelocity();  
+            vSetpoint2 = Kv * err + vSetpoint;
+            cInfo("Velocity setpoint: %1.6lf Error: %1.6lf", vSetpoint, err);
         }
 
         switch(j->getType())

Library/src/actuators/Thruster.cpp

@@ -113,7 +113,7 @@ bool Thruster::isPropellerRight() const
 
 void Thruster::Update(Scalar dt)
 {
-    if(attach != NULL)
+    if(attach != nullptr)
     {
         //Update thruster velocity
         Scalar error = setpoint * omegaLim - omega;
@@ -173,7 +173,7 @@ void Thruster::Update(Scalar dt)
 std::vector<Renderable> Thruster::Render()
 {
     Transform thrustTrans = Transform::getIdentity();
-    if(attach != NULL)
+    if(attach != nullptr)
         thrustTrans = attach->getOTransform() * o2a;
     else
         LinkActuator::Render();

Library/src/core/ScenarioParser.cpp

@@ -66,6 +66,7 @@
 #include "sensors/vision/SSS.h"
 #include "sensors/vision/MSIS.h"
 #include "sensors/Contact.h"
+#include "actuators/Push.h"
 #include "actuators/Light.h"
 #include "actuators/Servo.h"
 #include "actuators/Propeller.h"
@@ -2204,6 +2205,7 @@ bool ScenarioParser::ParseActuator(XMLElement* element, Robot* robot)
             break;
 
         //Link actuators
+        case ActuatorType::PUSH:
         case ActuatorType::THRUSTER:
         case ActuatorType::PROPELLER:
         case ActuatorType::RUDDER:
@@ -2436,6 +2438,27 @@ Actuator* ScenarioParser::ParseActuator(XMLElement* element, const std::string&
         srv->setDesiredPosition(initialPos);
         return srv;
     }
+    else if(typeStr == "simple_thruster" || typeStr == "push")
+    {
+        Push* push; 
+        if((item = element->FirstChildElement("specs")) != nullptr)
+        {
+            bool inverted = false;
+            item->QueryAttribute("inverted", &inverted);
+
+            push = new Push(actuatorName, inverted, typeStr == "simple_thruster");
+
+            double lower, upper;
+            if(item->QueryAttribute("lower_limit", &lower) == XML_SUCCESS 
+                && item->QueryAttribute("upper_limit", &upper) == XML_SUCCESS)
+            {
+                push->setForceLimits(lower, upper);
+            }
+        }
+        else
+            push = new Push(actuatorName, false, typeStr == "simple_thruster");
+        return push;
+    }
     else if(typeStr == "thruster" || typeStr == "propeller")
     {
         const char* propFile = nullptr;

Library/src/entities/SolidEntity.cpp

@@ -1130,7 +1130,9 @@ void SolidEntity::AddToSimulation(SimulationManager *sm, const Transform& origin
         if(linSleep <= Scalar(0) || angSleep <= Scalar(0))        
             rigidBody->setActivationState(DISABLE_DEACTIVATION);
         else
+        {
             rigidBody->setSleepingThresholds(linSleep, angSleep);
+        }
 
         // Add to world
         Transform Tcg = origin * T_CG2O.inverse();