NJointCartesianVelocityController.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package ArmarX
19  * @author Mirko Waechter( mirko.waechter at kit dot edu)
20  * @date 2017
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
26 
27 #include <VirtualRobot/IK/DifferentialIK.h>
28 #include <VirtualRobot/VirtualRobot.h>
29 
33 #include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.h>
35 
36 namespace armarx
37 {
38  TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityController);
39 
40  TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerControlData);
41 
43  {
44  public:
45  float xVel = 0;
46  float yVel = 0;
47  float zVel = 0;
48  float rollVel = 0;
49  float pitchVel = 0;
50  float yawVel = 0;
51  std::vector<float> nullspaceJointVelocities;
52  float avoidJointLimitsKp = 0;
53  std::vector<float> torqueKp;
54  std::vector<float> torqueKd;
55 
56  VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
57  };
58 
59  class SimplePID
60  {
61  public:
62  float Kp = 0, Kd = 0;
63  float lastError = 0;
64  float update(float dt, float error);
65  };
66 
67  /**
68  * @brief The NJointCartesianVelocityController class
69  * @ingroup Library-RobotUnit-NJointControllers
70  */
72  public NJointControllerWithTripleBuffer<NJointCartesianVelocityControllerControlData>,
73  public NJointCartesianVelocityControllerInterface
74  {
75  public:
76  using ConfigPtrT = NJointCartesianVelocityControllerConfigPtr;
78  const NJointCartesianVelocityControllerConfigPtr& config,
79  const VirtualRobot::RobotPtr&);
80 
81  // NJointControllerInterface interface
82  std::string getClassName(const Ice::Current&) const override;
84  getFunctionDescriptions(const Ice::Current&) const override;
85  void callDescribedFunction(const std::string& name,
86  const StringVariantBaseMap& valueMap,
87  const Ice::Current&) override;
88 
89  // NJointController interface
90  void rtRun(const IceUtil::Time& sensorValuesTimestamp,
91  const IceUtil::Time& timeSinceLastIteration) override;
94  const std::map<std::string, ConstControlDevicePtr>&,
95  const std::map<std::string, ConstSensorDevicePtr>&);
96 
97  static NJointCartesianVelocityControllerConfigPtr
99  NJointCartesianVelocityController(RobotUnitPtr prov,
100  NJointCartesianVelocityControllerConfigPtr config,
101  const VirtualRobot::RobotPtr& r);
102 
103  // for TCPControlUnit
104  void setVelocities(float xVel,
105  float yVel,
106  float zVel,
107  float rollVel,
108  float pitchVel,
109  float yawVel,
111  void setAvoidJointLimitsKp(float kp);
112  std::string getNodeSetName() const;
113  static ::armarx::WidgetDescription::WidgetSeq createSliders();
114  WidgetDescription::HBoxLayoutPtr
115  createJointSlidersLayout(float min, float max, float defaultValue) const;
116 
117  static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode);
119  IceModeFromString(const std::string mode);
122 
123  protected:
124  void rtPreActivateController() override;
125  void rtPostDeactivateController() override;
126 
127  private:
128  std::vector<ControlTarget1DoFActuatorVelocity*> targets;
129  //std::vector<const SensorValue1DoFActuatorPosition*> sensors;
130  std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
131  std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
132  std::vector<SimplePID> torquePIDs;
133 
134  CartesianVelocityControllerPtr tcpController;
135 
136  std::string nodeSetName;
137 
138 
139  // NJointCartesianVelocityControllerInterface interface
140  public:
141  void setControllerTarget(float x,
142  float y,
143  float z,
144  float roll,
145  float pitch,
146  float yaw,
147  float avoidJointLimitsKp,
149  const Ice::Current&) override;
150  void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
151  void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities,
152  const Ice::Current&) override;
153  };
154 
155 } // namespace armarx
armarx::SimplePID::update
float update(float dt, float error)
Definition: NJointCartesianVelocityController.cpp:447
armarx::NJointCartesianVelocityControllerControlData::rollVel
float rollVel
Definition: NJointCartesianVelocityController.h:48
NJointControllerWithTripleBuffer.h
armarx::NJointCartesianVelocityControllerControlData::mode
VirtualRobot::IKSolver::CartesianSelection mode
Definition: NJointCartesianVelocityController.h:56
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::TYPEDEF_PTRS_HANDLE
TYPEDEF_PTRS_HANDLE(NJointCartesianNaturalPositionController)
armarx::NJointControllerWithTripleBuffer
Definition: NJointControllerWithTripleBuffer.h:10
armarx::NJointCartesianVelocityController
The NJointCartesianVelocityController class.
Definition: NJointCartesianVelocityController.h:71
armarx::NJointCartesianVelocityControllerControlData::torqueKp
std::vector< float > torqueKp
Definition: NJointCartesianVelocityController.h:53
armarx::WidgetDescription::StringWidgetDictionary
::std::map<::std::string, ::armarx::WidgetDescription::WidgetPtr > StringWidgetDictionary
Definition: NJointControllerBase.h:69
armarx::NJointCartesianVelocityController::getFunctionDescriptions
WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current &) const override
Definition: NJointCartesianVelocityController.cpp:379
armarx::NJointCartesianVelocityController::setNullspaceJointVelocities
void setNullspaceJointVelocities(const StringFloatDictionary &nullspaceJointVelocities, const Ice::Current &) override
Definition: NJointCartesianVelocityController.cpp:495
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::NJointCartesianVelocityController::setTorqueKp
void setTorqueKp(const StringFloatDictionary &torqueKp, const Ice::Current &) override
Definition: NJointCartesianVelocityController.cpp:482
armarx::NJointCartesianVelocityControllerControlData::xVel
float xVel
Definition: NJointCartesianVelocityController.h:45
armarx::NJointCartesianVelocityController::callDescribedFunction
void callDescribedFunction(const std::string &name, const StringVariantBaseMap &valueMap, const Ice::Current &) override
Definition: NJointCartesianVelocityController.cpp:393
armarx::SimplePID::Kd
float Kd
Definition: NJointCartesianVelocityController.h:62
armarx::NJointCartesianVelocityController::createSliders
::armarx::WidgetDescription::WidgetSeq createSliders()
Definition: NJointCartesianVelocityController.cpp:167
armarx::CartesianVelocityControllerPtr
std::shared_ptr< CartesianVelocityController > CartesianVelocityControllerPtr
Definition: CartesianVelocityController.h:34
armarx::NJointCartesianVelocityControllerControlData
Definition: NJointCartesianVelocityController.h:42
armarx::NJointCartesianVelocityController::getNodeSetName
std::string getNodeSetName() const
Definition: NJointCartesianVelocityController.cpp:368
armarx::NJointCartesianVelocityController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointCartesianVelocityController.cpp:104
armarx::NJointCartesianVelocityControllerControlData::zVel
float zVel
Definition: NJointCartesianVelocityController.h:47
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::SimplePID::Kp
float Kp
Definition: NJointCartesianVelocityController.h:62
armarx::NJointCartesianVelocityControllerControlData::pitchVel
float pitchVel
Definition: NJointCartesianVelocityController.h:49
armarx::NJointCartesianVelocityController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: NJointCartesianVelocityController.cpp:42
armarx::NJointCartesianVelocityController::NJointCartesianVelocityController
NJointCartesianVelocityController(RobotUnit *prov, const NJointCartesianVelocityControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointCartesianVelocityController.cpp:47
armarx::NJointCartesianVelocityController::ModeFromIce
static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode)
Definition: NJointCartesianVelocityController.cpp:319
armarx::NJointCartesianVelocityController::setAvoidJointLimitsKp
void setAvoidJointLimitsKp(float kp)
Definition: NJointCartesianVelocityController.cpp:360
armarx::NJointTaskSpaceDMPControllerMode::CartesianSelection
CartesianSelection
Definition: ControllerInterface.ice:34
armarx::NJointCartesianVelocityController::setVelocities
void setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode)
Definition: NJointCartesianVelocityController.cpp:339
ControlTarget1DoFActuator.h
armarx::NJointCartesianVelocityController::rtPostDeactivateController
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Definition: NJointCartesianVelocityController.cpp:374
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
CartesianVelocityController.h
armarx::NJointCartesianVelocityController::GenerateConfigFromVariants
static NJointCartesianVelocityControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap &values)
Definition: NJointCartesianVelocityController.cpp:270
armarx::SimplePID::lastError
float lastError
Definition: NJointCartesianVelocityController.h:63
armarx::NJointCartesianVelocityControllerControlData::avoidJointLimitsKp
float avoidJointLimitsKp
Definition: NJointCartesianVelocityController.h:52
armarx::NJointCartesianVelocityController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: NJointCartesianVelocityController.cpp:99
armarx::NJointCartesianVelocityController::GenerateConfigDescription
static WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr &, const std::map< std::string, ConstControlDevicePtr > &, const std::map< std::string, ConstSensorDevicePtr > &)
Definition: NJointCartesianVelocityController.cpp:221
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:294
armarx::RobotUnit
The RobotUnit class manages a robot and its controllers.
Definition: RobotUnit.h:180
armarx::NJointCartesianVelocityControllerControlData::nullspaceJointVelocities
std::vector< float > nullspaceJointVelocities
Definition: NJointCartesianVelocityController.h:51
armarx::NJointCartesianVelocityController::ConfigPtrT
NJointCartesianVelocityControllerConfigPtr ConfigPtrT
Definition: NJointCartesianVelocityController.h:76
armarx::NJointCartesianVelocityController::ModeFromString
static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode)
Definition: NJointCartesianVelocityController.cpp:280
armarx::NJointCartesianVelocityController::setControllerTarget
void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode, const Ice::Current &) override
Definition: NJointCartesianVelocityController.cpp:458
armarx::NJointCartesianVelocityControllerControlData::yawVel
float yawVel
Definition: NJointCartesianVelocityController.h:50
armarx::NJointCartesianVelocityController::IceModeFromString
static NJointCartesianVelocityControllerMode::CartesianSelection IceModeFromString(const std::string mode)
Definition: NJointCartesianVelocityController.cpp:300
armarx::NJointCartesianVelocityController::createJointSlidersLayout
WidgetDescription::HBoxLayoutPtr createJointSlidersLayout(float min, float max, float defaultValue) const
Definition: NJointCartesianVelocityController.cpp:195
armarx::SimplePID
Definition: NJointCartesianVelocityController.h:59
armarx::NJointCartesianVelocityControllerControlData::torqueKd
std::vector< float > torqueKd
Definition: NJointCartesianVelocityController.h:54
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::NJointCartesianVelocityControllerControlData::yVel
float yVel
Definition: NJointCartesianVelocityController.h:46
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:42
SensorValue1DoFActuator.h
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18