From cefef332734760dd7e354de01bac95c9875c18eb Mon Sep 17 00:00:00 2001
From: Markus Swarowsky <markus.swarowsky@student.kit.edu>
Date: Wed, 1 Mar 2017 15:26:49 +0100
Subject: [PATCH] added the PlatformWheelVelocityTarget; changed the
JointTargetBase to a general TargetBase; added wheel speed reporting to the
PlatformDataUnit
---
.../RobotRTControllers/CMakeLists.txt | 4 +-
.../DataUnits/PlatformDataUnit.h | 50 ++++++++++++++++
.../RobotRTControllers/LVL0Controller.h | 4 +-
.../RobotRTControllers/LVL1Controller.h | 4 +-
.../RobotRTControllers/RobotUnit.cpp | 2 +-
.../libraries/RobotRTControllers/RobotUnit.h | 2 +-
.../RobotRTControllers/SyntaxCheck.cpp | 2 +-
.../Targets/JointPositionTarget.h | 4 +-
.../Targets/JointTorqueTarget.h | 4 +-
.../Targets/JointVelocityTarget.h | 5 +-
.../Targets/PlatformWheelVelocityTarget.h | 57 +++++++++++++++++++
.../{JointTargetBase.h => TargetBase.h} | 6 +-
12 files changed, 125 insertions(+), 19 deletions(-)
create mode 100644 source/RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h
rename source/RobotAPI/libraries/RobotRTControllers/Targets/{JointTargetBase.h => TargetBase.h} (93%)
diff --git a/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt
index 1ca0215a8..289708c82 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt
@@ -21,11 +21,11 @@ set(LIB_FILES
set(LIB_HEADERS
Constants.h
ControlModes.h
-
- Targets/JointTargetBase.h
+ Targets/TargetBase.h
Targets/JointPositionTarget.h
Targets/JointVelocityTarget.h
Targets/JointTorqueTarget.h
+ Targets/PlatformWheelVelocityTarget.h
DataUnits/ForceTorqueDataUnit.h
DataUnits/HapticDataUnit.h
diff --git a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h
index 4d4b70ee0..cf1930643 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h
@@ -34,6 +34,10 @@ namespace armarx
virtual const float* getVelocityX() const = 0;
virtual const float* getVelocityY() const = 0;
virtual const float* getVelocityRotation() const = 0;
+ virtual const float* getFrontRightWheelVelocity() const = 0;
+ virtual const float* getFrontLeftWheelVelocity() const = 0;
+ virtual const float* getRearRightWheelVelocity() const = 0;
+ virtual const float* getRearLeftWheelVelocity() const = 0;
};
class PlatformDataUnitPtrProvider : public virtual PlatformDataUnitInterface
@@ -64,7 +68,48 @@ namespace armarx
{
return velocityRotation;
}
+
+ const float* getFrontRightWheelVelocity() const override
+ {
+ return frontRightVelocity;
+ }
+
+ const float* getFrontLeftWheelVelocity() const override
+ {
+ return frontLeftVelocity;
+ }
+
+ const float* getRearRightWheelVelocity() const override
+ {
+ return rearRightVelocity;
+ }
+
+ const float* getRearLeftWheelVelocity() const override
+ {
+ return rearLeftVelocity;
+ }
+
protected:
+ void setFrontRightVelocityPtr(float* velocity)
+ {
+ frontRightVelocity = velocity;
+ }
+
+ void setFrontLeftVelocityPtr(float* velocity)
+ {
+ frontRightVelocity = velocity;
+ }
+
+ void setRearRightVelocityPtr(float* velocity)
+ {
+ frontRightVelocity = velocity;
+ }
+
+ void setRearLeftVelocityPtr(float* velocity)
+ {
+ frontRightVelocity = velocity;
+ }
+
float* positionX;
float* positionY;
float* rotation;
@@ -72,6 +117,11 @@ namespace armarx
float* velocityX;
float* velocityY;
float* velocityRotation;
+
+ float* frontRightVelocity;
+ float* frontLeftVelocity;
+ float* rearRightVelocity;
+ float* rearLeftVelocity;
};
}
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h
index 403565e80..707f92008 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h
@@ -23,7 +23,7 @@
#ifndef _ARMARX_LIB_RobotAPI_LVL0Controller_H
#define _ARMARX_LIB_RobotAPI_LVL0Controller_H
-#include "Targets/JointTargetBase.h"
+#include "Targets/TargetBase.h"
#include <memory>
@@ -36,7 +36,7 @@ namespace armarx
{
public:
virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual JointTargetBase* getTarget() const = 0;
+ virtual TargetBase* getTarget() const = 0;
virtual void resetTarget()
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
index aa19370a1..7c525d152 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
@@ -35,7 +35,7 @@
#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.h>
-#include "Targets/JointTargetBase.h"
+#include "Targets/TargetBase.h"
//units
#include "DataUnits/ForceTorqueDataUnit.h"
@@ -55,7 +55,7 @@ namespace armarx
virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
- virtual JointTargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) = 0;
+ virtual TargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) = 0;
virtual std::string getName() const = 0;
};
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
index 4c0c61880..c10b03c2f 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
@@ -361,7 +361,7 @@ armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::
return LVL1ControllerInterfacePrx::uncheckedCast(prx);
}
-armarx::JointTargetBase* armarx::RobotUnit::getJointTarget(const std::string& jointName, const std::string& controlMode)
+armarx::TargetBase* armarx::RobotUnit::getJointTarget(const std::string& jointName, const std::string& controlMode)
{
GuardType guard {dataMutex};
if (!hasLVL0Controller(jointName, controlMode))
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
index bf0b29c1c..29b64a714 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
@@ -231,7 +231,7 @@ namespace armarx
virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
//targets (nonrt but results are used in rt)
- virtual JointTargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) override;
+ virtual TargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) override;
protected:
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp b/source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp
index 8e6bcbcfe..537a95b05 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp
+++ b/source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp
@@ -31,7 +31,7 @@
#include "DataUnits/PlatformDataUnit.h"
#include "Targets/JointPositionTarget.h"
-#include "Targets/JointTargetBase.h"
+#include "Targets/TargetBase.h"
#include "Targets/JointTorqueTarget.h"
#include "Targets/JointVelocityTarget.h"
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h
index 6e7205de9..2fe706d36 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h
@@ -23,7 +23,7 @@
#ifndef _ARMARX_LIB_RobotAPI_JointPositionTarget_H
#define _ARMARX_LIB_RobotAPI_JointPositionTarget_H
-#include "JointTargetBase.h"
+#include "TargetBase.h"
namespace armarx
{
@@ -38,7 +38,7 @@ namespace armarx
*
* Detailed description of class JointPositionTarget.
*/
- class JointPositionTarget: public JointTargetBase
+ class JointPositionTarget: public TargetBase
{
public:
float position = ControllerConstants::ValueNotSetNaN;
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h
index 9421f2218..0c70ed9a0 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h
@@ -23,7 +23,7 @@
#ifndef _ARMARX_LIB_RobotAPI_JointTorqueTarget_H
#define _ARMARX_LIB_RobotAPI_JointTorqueTarget_H
-#include "JointTargetBase.h"
+#include "TargetBase.h"
namespace armarx
{
@@ -38,7 +38,7 @@ namespace armarx
*
* Detailed description of class JointTorqueTarget.
*/
- class JointTorqueTarget: public JointTargetBase
+ class JointTorqueTarget: public TargetBase
{
public:
float torque = ControllerConstants::ValueNotSetNaN;
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h
index a4b307de9..1c6296d95 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h
@@ -23,7 +23,7 @@
#ifndef _ARMARX_LIB_RobotAPI_JointVelocityTarget_H
#define _ARMARX_LIB_RobotAPI_JointVelocityTarget_H
-#include "JointTargetBase.h"
+#include "TargetBase.h"
namespace armarx
{
@@ -38,7 +38,7 @@ namespace armarx
*
* Detailed description of class JointVelocityTarget.
*/
- class JointVelocityTarget: public JointTargetBase
+ class JointVelocityTarget: public TargetBase
{
public:
float velocity = ControllerConstants::ValueNotSetNaN;
@@ -55,7 +55,6 @@ namespace armarx
return std::isfinite(velocity);
}
};
-
}
#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h
new file mode 100644
index 000000000..c2bafb0ad
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h
@@ -0,0 +1,57 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 <http://www.gnu.org/licenses/>.
+ *
+ * @author Markus Swarowsky (markus dot swarowsky at student dot kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#ifndef _ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
+#define _ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
+
+
+#include "TargetBase.h"
+
+
+namespace armarx
+{
+
+ class PlatformWheelVelocityTarget : public TargetBase
+ {
+ public:
+ float velocity = ControllerConstants::ValueNotSetNaN;
+ virtual std::string getControlMode() const override
+ {
+ return ControlModes::VelocityMode;
+ }
+ virtual void reset() override
+ {
+ velocity = ControllerConstants::ValueNotSetNaN;
+ }
+ virtual bool isValid() const override
+ {
+ return std::isfinite(velocity);
+ }
+
+ private:
+ };
+
+}
+
+#endif //_ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTargetBase.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/TargetBase.h
similarity index 93%
rename from source/RobotAPI/libraries/RobotRTControllers/Targets/JointTargetBase.h
rename to source/RobotAPI/libraries/RobotRTControllers/Targets/TargetBase.h
index 3f9736b89..d303e24eb 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTargetBase.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/TargetBase.h
@@ -34,13 +34,13 @@ namespace armarx
* @ingroup RobotAPI
* A description of the library RobotRTControllers.
*
- * @class JointTargetBase
+ * @class TargetBase
* @ingroup Library-RobotRTControllers
* @brief Brief description of class JointTargetBase.
*
- * Detailed description of class JointTargetBase.
+ * Detailed description of class TargetBase.
*/
- class JointTargetBase
+ class TargetBase
{
public:
virtual std::string getControlMode() const = 0;
--
GitLab