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/VirtualRobot.h>
28 
30 #include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.h>
32 
33 namespace armarx
34 {
35  class ControlTarget1DoFActuatorVelocity;
36  class SensorValue1DoFActuatorTorque;
37  class SensorValue1DoFGravityTorque;
38 
39 
40  TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityController);
41 
42  TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerControlData);
43 
45  {
46  public:
47  float xVel = 0;
48  float yVel = 0;
49  float zVel = 0;
50  float rollVel = 0;
51  float pitchVel = 0;
52  float yawVel = 0;
53  std::vector<float> nullspaceJointVelocities;
54  float avoidJointLimitsKp = 0;
55  std::vector<float> torqueKp;
56  std::vector<float> torqueKd;
57 
58  VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
59  };
60 
61  class SimplePID
62  {
63  public:
64  float Kp = 0, Kd = 0;
65  float lastError = 0;
66  float update(float dt, float error);
67  };
68 
69  /**
70  * @brief The NJointCartesianVelocityController class
71  * @ingroup Library-RobotUnit-NJointControllers
72  */
74  public NJointControllerWithTripleBuffer<NJointCartesianVelocityControllerControlData>,
75  public NJointCartesianVelocityControllerInterface
76  {
77  public:
78  using ConfigPtrT = NJointCartesianVelocityControllerConfigPtr;
80  const NJointCartesianVelocityControllerConfigPtr& config,
81  const VirtualRobot::RobotPtr&);
82 
83  // NJointControllerInterface interface
84  std::string getClassName(const Ice::Current&) const override;
86  getFunctionDescriptions(const Ice::Current&) const override;
87  void callDescribedFunction(const std::string& name,
88  const StringVariantBaseMap& valueMap,
89  const Ice::Current&) override;
90 
91  // NJointController interface
92  void rtRun(const IceUtil::Time& sensorValuesTimestamp,
93  const IceUtil::Time& timeSinceLastIteration) override;
96  const std::map<std::string, ConstControlDevicePtr>&,
97  const std::map<std::string, ConstSensorDevicePtr>&);
98 
99  static NJointCartesianVelocityControllerConfigPtr
102  NJointCartesianVelocityControllerConfigPtr config,
103  const VirtualRobot::RobotPtr& r);
104 
105  // for TCPControlUnit
106  void setVelocities(float xVel,
107  float yVel,
108  float zVel,
109  float rollVel,
110  float pitchVel,
111  float yawVel,
113  void setAvoidJointLimitsKp(float kp);
114  std::string getNodeSetName() const;
115  static ::armarx::WidgetDescription::WidgetSeq createSliders();
116  WidgetDescription::HBoxLayoutPtr
117  createJointSlidersLayout(float min, float max, float defaultValue) const;
118 
119  static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode);
121  IceModeFromString(const std::string mode);
124 
125  protected:
126  void rtPreActivateController() override;
127  void rtPostDeactivateController() override;
128 
129  private:
130  std::vector<ControlTarget1DoFActuatorVelocity*> targets;
131  //std::vector<const SensorValue1DoFActuatorPosition*> sensors;
132  std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
133  std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
134  std::vector<SimplePID> torquePIDs;
135 
136  CartesianVelocityControllerPtr tcpController;
137 
138  std::string nodeSetName;
139 
140 
141  // NJointCartesianVelocityControllerInterface interface
142  public:
143  void setControllerTarget(float x,
144  float y,
145  float z,
146  float roll,
147  float pitch,
148  float yaw,
149  float avoidJointLimitsKp,
151  const Ice::Current&) override;
152  void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
153  void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities,
154  const Ice::Current&) override;
155  };
156 
157 } // namespace armarx
armarx::SimplePID::update
float update(float dt, float error)
Definition: NJointCartesianVelocityController.cpp:451
armarx::NJointCartesianVelocityControllerControlData::rollVel
float rollVel
Definition: NJointCartesianVelocityController.h:50
NJointControllerWithTripleBuffer.h
armarx::NJointCartesianVelocityControllerControlData::mode
VirtualRobot::IKSolver::CartesianSelection mode
Definition: NJointCartesianVelocityController.h:58
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
armarx::TYPEDEF_PTRS_HANDLE
TYPEDEF_PTRS_HANDLE(NJointCartesianNaturalPositionController)
armarx::NJointControllerWithTripleBuffer
Definition: NJointControllerWithTripleBuffer.h:10
armarx::NJointCartesianVelocityController
The NJointCartesianVelocityController class.
Definition: NJointCartesianVelocityController.h:73
armarx::NJointCartesianVelocityControllerControlData::torqueKp
std::vector< float > torqueKp
Definition: NJointCartesianVelocityController.h:55
armarx::WidgetDescription::StringWidgetDictionary
::std::map<::std::string, ::armarx::WidgetDescription::WidgetPtr > StringWidgetDictionary
Definition: NJointControllerBase.h:70
armarx::NJointCartesianVelocityController::getFunctionDescriptions
WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current &) const override
Definition: NJointCartesianVelocityController.cpp:383
armarx::NJointCartesianVelocityController::setNullspaceJointVelocities
void setNullspaceJointVelocities(const StringFloatDictionary &nullspaceJointVelocities, const Ice::Current &) override
Definition: NJointCartesianVelocityController.cpp:499
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:297
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::NJointCartesianVelocityController::setTorqueKp
void setTorqueKp(const StringFloatDictionary &torqueKp, const Ice::Current &) override
Definition: NJointCartesianVelocityController.cpp:486
armarx::NJointCartesianVelocityControllerControlData::xVel
float xVel
Definition: NJointCartesianVelocityController.h:47
armarx::NJointCartesianVelocityController::callDescribedFunction
void callDescribedFunction(const std::string &name, const StringVariantBaseMap &valueMap, const Ice::Current &) override
Definition: NJointCartesianVelocityController.cpp:397
armarx::SimplePID::Kd
float Kd
Definition: NJointCartesianVelocityController.h:64
armarx::NJointCartesianVelocityController::createSliders
::armarx::WidgetDescription::WidgetSeq createSliders()
Definition: NJointCartesianVelocityController.cpp:171
armarx::CartesianVelocityControllerPtr
std::shared_ptr< CartesianVelocityController > CartesianVelocityControllerPtr
Definition: CartesianVelocityController.h:34
armarx::NJointCartesianVelocityControllerControlData
Definition: NJointCartesianVelocityController.h:44
armarx::NJointCartesianVelocityController::getNodeSetName
std::string getNodeSetName() const
Definition: NJointCartesianVelocityController.cpp:372
armarx::NJointCartesianVelocityController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointCartesianVelocityController.cpp:108
armarx::NJointCartesianVelocityControllerControlData::zVel
float zVel
Definition: NJointCartesianVelocityController.h:49
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::SimplePID::Kp
float Kp
Definition: NJointCartesianVelocityController.h:64
armarx::NJointCartesianVelocityControllerControlData::pitchVel
float pitchVel
Definition: NJointCartesianVelocityController.h:51
armarx::NJointCartesianVelocityController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: NJointCartesianVelocityController.cpp:46
armarx::NJointCartesianVelocityController::NJointCartesianVelocityController
NJointCartesianVelocityController(RobotUnit *prov, const NJointCartesianVelocityControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointCartesianVelocityController.cpp:51
armarx::NJointCartesianVelocityController::ModeFromIce
static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode)
Definition: NJointCartesianVelocityController.cpp:323
armarx::NJointCartesianVelocityController::setAvoidJointLimitsKp
void setAvoidJointLimitsKp(float kp)
Definition: NJointCartesianVelocityController.cpp:364
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:343
armarx::NJointCartesianVelocityController::rtPostDeactivateController
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Definition: NJointCartesianVelocityController.cpp:378
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:274
armarx::SimplePID::lastError
float lastError
Definition: NJointCartesianVelocityController.h:65
armarx::NJointCartesianVelocityControllerControlData::avoidJointLimitsKp
float avoidJointLimitsKp
Definition: NJointCartesianVelocityController.h:54
armarx::NJointCartesianVelocityController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: NJointCartesianVelocityController.cpp:103
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:225
IceUtil::Handle< class RobotUnit >
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:327
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:53
armarx::NJointCartesianVelocityController::ConfigPtrT
NJointCartesianVelocityControllerConfigPtr ConfigPtrT
Definition: NJointCartesianVelocityController.h:78
armarx::NJointCartesianVelocityController::ModeFromString
static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode)
Definition: NJointCartesianVelocityController.cpp:284
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:462
armarx::NJointCartesianVelocityControllerControlData::yawVel
float yawVel
Definition: NJointCartesianVelocityController.h:52
armarx::NJointCartesianVelocityController::IceModeFromString
static NJointCartesianVelocityControllerMode::CartesianSelection IceModeFromString(const std::string mode)
Definition: NJointCartesianVelocityController.cpp:304
armarx::NJointCartesianVelocityController::createJointSlidersLayout
WidgetDescription::HBoxLayoutPtr createJointSlidersLayout(float min, float max, float defaultValue) const
Definition: NJointCartesianVelocityController.cpp:199
armarx::SimplePID
Definition: NJointCartesianVelocityController.h:61
armarx::NJointCartesianVelocityControllerControlData::torqueKd
std::vector< float > torqueKd
Definition: NJointCartesianVelocityController.h:56
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::NJointCartesianVelocityControllerControlData::yVel
float yVel
Definition: NJointCartesianVelocityController.h:48
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:45
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19