NJointTaskSpaceImpedanceController.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package TaskSpaceActiveImpedanceControl::ArmarXObjects::NJointTaskSpaceImpedanceController
17  * @author zhou ( you dot zhou at kit dot edu )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
25 #include <SimoxUtility/math/convert/mat4f_to_pos.h>
26 #include <SimoxUtility/math/convert/mat4f_to_quat.h>
27 #include <VirtualRobot/IK/DifferentialIK.h>
28 #include <VirtualRobot/Robot.h>
29 #include <VirtualRobot/RobotNodeSet.h>
30 
32 
33 using namespace armarx;
34 
36  registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
37 
39  const RobotUnitPtr& robotUnit,
40  const NJointControllerConfigPtr& config,
42 {
43 
44  NJointTaskSpaceImpedanceControlConfigPtr cfg =
45  NJointTaskSpaceImpedanceControlConfigPtr::dynamicCast(config);
46 
48  ARMARX_CHECK_EXPRESSION(robotUnit);
49  ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
51  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
52 
53  jointNames.clear();
54  ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
55  for (size_t i = 0; i < rns->getSize(); ++i)
56  {
57  std::string jointName = rns->getNode(i)->getName();
58  jointNames.push_back(jointName);
59  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
60  ARMARX_CHECK_EXPRESSION(ct) << jointName;
61  const SensorValueBase* sv = useSensorValue(jointName);
62  ARMARX_CHECK_EXPRESSION(sv) << jointName;
63  auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
64  ARMARX_CHECK_EXPRESSION(casted_ct) << jointName;
65  targets.push_back(casted_ct);
66 
67  const SensorValue1DoFActuatorTorque* torqueSensor =
68  sv->asA<SensorValue1DoFActuatorTorque>();
69  const SensorValue1DoFActuatorVelocity* velocitySensor =
70  sv->asA<SensorValue1DoFActuatorVelocity>();
71  const SensorValue1DoFActuatorPosition* positionSensor =
72  sv->asA<SensorValue1DoFActuatorPosition>();
73  if (!torqueSensor)
74  {
75  ARMARX_WARNING << "No Torque sensor available for " << jointName;
76  }
77 
78 
79  torqueSensors.push_back(torqueSensor);
80  velocitySensors.push_back(velocitySensor);
81  positionSensors.push_back(positionSensor);
82  };
83 
84  controller.initialize(rns);
85 
87  initData.desiredOrientation = cfg->desiredOrientation;
88  initData.desiredPosition = cfg->desiredPosition;
89 
90  initData.Kpos = cfg->Kpos;
91  initData.Dpos = cfg->Dpos;
92  initData.Kori = cfg->Kori;
93  initData.Dori = cfg->Dori;
94 
95  initData.Knull = cfg->Knull;
96  initData.Dnull = cfg->Dnull;
97  initData.desiredJointPosition = cfg->desiredJointPositions;
98 
99  initData.torqueLimit = cfg->torqueLimit;
100  reinitTripleBuffer(initData);
101 }
102 
103 void
105  const IceUtil::Time& /*timeSinceLastIteration*/)
106 {
108  const auto& jointDesiredTorques =
109  controller.run(rtGetControlStruct(), torqueSensors, velocitySensors, positionSensors);
110  ARMARX_CHECK_EQUAL(targets.size(), static_cast<size_t>(jointDesiredTorques.size()));
111 
112  for (size_t i = 0; i < targets.size(); ++i)
113  {
114  targets.at(i)->torque = jointDesiredTorques(i);
115  if (!targets.at(i)->isValid())
116  {
117  targets.at(i)->torque = 0;
118  }
119  }
120 
121  debugDataInfo.getWriteBuffer().desiredForce_x = controller.dbg.tcpDesiredForce(0);
122  debugDataInfo.getWriteBuffer().desiredForce_y = controller.dbg.tcpDesiredForce(1);
123  debugDataInfo.getWriteBuffer().desiredForce_z = controller.dbg.tcpDesiredForce(2);
124  debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = controller.dbg.tcpDesiredTorque(0);
125  debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = controller.dbg.tcpDesiredTorque(1);
126  debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = controller.dbg.tcpDesiredTorque(2);
127  debugDataInfo.getWriteBuffer().quatError = controller.dbg.errorAngle;
128  debugDataInfo.getWriteBuffer().posiError = controller.dbg.posiError;
129  debugDataInfo.commitWrite();
130 }
131 
132 void
135  const DebugObserverInterfacePrx& debugObs)
136 {
137  StringVariantBaseMap datafields;
138  auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
139  for (auto& pair : values)
140  {
141  datafields[pair.first] = new Variant(pair.second);
142  }
143 
144  datafields["desiredForce_x"] =
145  new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
146  datafields["desiredForce_y"] =
147  new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
148  datafields["desiredForce_z"] =
149  new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
150 
151  datafields["tcpDesiredTorque_x"] =
152  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
153  datafields["tcpDesiredTorque_y"] =
154  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
155  datafields["tcpDesiredTorque_z"] =
156  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
157 
158  datafields["quatError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().quatError);
159  datafields["posiError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().posiError);
160 
161  debugObs->setDebugChannel("DSControllerOutput", datafields);
162 }
163 
164 void
165 NJointTaskSpaceImpedanceController::setPosition(const Eigen::Vector3f& target, const Ice::Current&)
166 {
170 }
171 
172 void
174  const Ice::Current&)
175 {
179 }
180 
181 void
183  const Eigen::Quaternionf& ori,
184  const Ice::Current&)
185 {
190 }
191 
192 void
194 {
196  getWriterControlStruct().desiredPosition = simox::math::mat4f_to_pos(mat);
197  getWriterControlStruct().desiredOrientation = simox::math::mat4f_to_quat(mat);
199 }
200 
201 void
203  const Ice::FloatSeq& vals,
204  const Ice::Current&)
205 {
207  if (name == "Kpos")
208  {
209  ARMARX_CHECK_EQUAL(vals.size(), 3);
210 
211  Eigen::Vector3f vec;
212  for (size_t i = 0; i < 3; ++i)
213  {
214  vec(i) = vals.at(i);
215  }
217  }
218  else if (name == "Kori")
219  {
220  ARMARX_CHECK_EQUAL(vals.size(), 3);
221 
222  Eigen::Vector3f vec;
223  for (size_t i = 0; i < 3; ++i)
224  {
225  vec(i) = vals.at(i);
226  }
228  }
229  else if (name == "Dpos")
230  {
231  ARMARX_CHECK_EQUAL(vals.size(), 3);
232 
233  Eigen::Vector3f vec;
234  for (size_t i = 0; i < 3; ++i)
235  {
236  vec(i) = vals.at(i);
237  }
239  }
240  else if (name == "Dori")
241  {
242  ARMARX_CHECK_EQUAL(vals.size(), 3);
243 
244  Eigen::Vector3f vec;
245  for (size_t i = 0; i < 3; ++i)
246  {
247  vec(i) = vals.at(i);
248  }
250  }
251  else if (name == "Knull")
252  {
253  ARMARX_CHECK_EQUAL(vals.size(), 8);
254 
255  Eigen::VectorXf vec;
256  vec.setZero(8);
257  for (size_t i = 0; i < 8; ++i)
258  {
259  vec(i) = vals.at(i);
260  }
262  }
263  else if (name == "Dnull")
264  {
265  ARMARX_CHECK_EQUAL(vals.size(), 8);
266 
267  Eigen::VectorXf vec;
268  vec.setZero(8);
269  for (size_t i = 0; i < 8; ++i)
270  {
271  vec(i) = vals.at(i);
272  }
274  }
276 }
277 
278 void
280  const Eigen::VectorXf& knull,
281  const Eigen::VectorXf& dnull,
282  const Ice::Current&)
283 {
285  ARMARX_CHECK_EQUAL(static_cast<std::size_t>(joint.size()), targets.size());
286  ARMARX_CHECK_EQUAL(static_cast<std::size_t>(knull.size()), targets.size());
287  ARMARX_CHECK_EQUAL(static_cast<std::size_t>(dnull.size()), targets.size());
288  getWriterControlStruct().Knull = knull;
289  getWriterControlStruct().Dnull = dnull;
292 }
293 
294 void
296  const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg,
297  const Ice::Current&)
298 {
300  ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.Knull.size()), targets.size());
301  ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.Dnull.size()), targets.size());
302  ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.desiredJointPositions.size()), targets.size());
303 
304  getWriterControlStruct().Kpos = cfg.Kpos;
305  getWriterControlStruct().Dpos = cfg.Dpos;
306  getWriterControlStruct().Kori = cfg.Kori;
307  getWriterControlStruct().Dori = cfg.Dori;
308 
309  getWriterControlStruct().Knull = cfg.Knull;
310  getWriterControlStruct().Dnull = cfg.Dnull;
311  getWriterControlStruct().desiredJointPosition = cfg.desiredJointPositions;
312 
313  getWriterControlStruct().torqueLimit = cfg.torqueLimit;
315 }
316 
317 void
319 {
320 }
armarx::NJointControllerWithTripleBuffer< CartesianImpedanceController::Config >::reinitTripleBuffer
void reinitTripleBuffer(const CartesianImpedanceController::Config &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::NJointTaskSpaceImpedanceController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointTaskSpaceImpedanceController.cpp:104
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::CartesianImpedanceController::Config::Kori
Eigen::Vector3f Kori
Definition: CartesianImpedanceController.h:22
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
armarx::NJointControllerRegistration
Definition: NJointControllerRegistry.h:74
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< CartesianImpedanceController::Config >::rtGetControlStruct
const CartesianImpedanceController::Config & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
armarx::CartesianImpedanceController::Config::Knull
Eigen::VectorXf Knull
Definition: CartesianImpedanceController.h:24
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::CartesianImpedanceController::Config::desiredJointPosition
Eigen::VectorXf desiredJointPosition
Definition: CartesianImpedanceController.h:26
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::CartesianImpedanceController::Config::Dnull
Eigen::VectorXf Dnull
Definition: CartesianImpedanceController.h:25
armarx::NJointTaskSpaceImpedanceController::setPositionOrientation
void setPositionOrientation(const Eigen::Vector3f &pos, const Eigen::Quaternionf &ori, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceController.cpp:182
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:668
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::CartesianImpedanceController::Config::torqueLimit
float torqueLimit
Definition: CartesianImpedanceController.h:27
armarx::NJointControllerWithTripleBuffer< CartesianImpedanceController::Config >::getWriterControlStruct
CartesianImpedanceController::Config & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::CartesianImpedanceController::Config::Dpos
Eigen::Vector3f Dpos
Definition: CartesianImpedanceController.h:21
armarx::NJointControllerWithTripleBuffer< CartesianImpedanceController::Config >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::NJointTaskSpaceImpedanceController::setNullspaceConfig
void setNullspaceConfig(const Eigen::VectorXf &joint, const Eigen::VectorXf &knull, const Eigen::VectorXf &dnull, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceController.cpp:279
armarx::NJointTaskSpaceImpedanceController::setPosition
void setPosition(const Eigen::Vector3f &, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceController.cpp:165
armarx::CartesianImpedanceController::Config::desiredOrientation
Eigen::Quaternionf desiredOrientation
Definition: CartesianImpedanceController.h:19
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::CartesianImpedanceController::Config::Dori
Eigen::Vector3f Dori
Definition: CartesianImpedanceController.h:23
controller
Definition: AddOperation.h:39
armarx::NJointControllerWithTripleBuffer< CartesianImpedanceController::Config >::rtUpdateControlStruct
bool rtUpdateControlStruct()
Definition: NJointControllerWithTripleBuffer.h:38
armarx::NJointControllerWithTripleBuffer< CartesianImpedanceController::Config >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::CartesianImpedanceController::Config::desiredPosition
Eigen::Vector3f desiredPosition
Definition: CartesianImpedanceController.h:18
armarx::NJointControllerWithTripleBuffer< CartesianImpedanceController::Config >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::NJointTaskSpaceImpedanceController::setConfig
void setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig &cfg, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceController.cpp:295
armarx::CartesianImpedanceController::Config
Definition: CartesianImpedanceController.h:16
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
registrationControllerDSRTController
NJointControllerRegistration< NJointTaskSpaceImpedanceController > registrationControllerDSRTController("NJointTaskSpaceImpedanceController")
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::NJointTaskSpaceImpedanceController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: NJointTaskSpaceImpedanceController.cpp:133
armarx::Quaternion< float, 0 >
IceUtil::Handle< class RobotUnit >
armarx::NJointTaskSpaceImpedanceController::setOrientation
void setOrientation(const Eigen::Quaternionf &, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceController.cpp:173
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::NJointTaskSpaceImpedanceController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: NJointTaskSpaceImpedanceController.cpp:318
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:116
armarx::NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController
NJointTaskSpaceImpedanceController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointTaskSpaceImpedanceController.cpp:38
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
NJointTaskSpaceImpedanceController.h
armarx::NJointTaskSpaceImpedanceController::setPose
void setPose(const Eigen::Matrix4f &mat, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceController.cpp:193
armarx::CartesianImpedanceController::Config::Kpos
Eigen::Vector3f Kpos
Definition: CartesianImpedanceController.h:20
NJointControllerRegistry.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition: NJointController.cpp:383
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::NJointTaskSpaceImpedanceController::setImpedanceParameters
void setImpedanceParameters(const std::string &, const Ice::FloatSeq &, const Ice::Current &) override
Definition: NJointTaskSpaceImpedanceController.cpp:202