From 12d04a98d32c7004371293d34de1d607e2c5a6f7 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Fri, 19 May 2017 15:27:40 +0200
Subject: [PATCH] added joint acceleration to KinematicUnitObserver
---
.../units/KinematicUnitObserver.cpp | 33 ++++++++++++++-----
1 file changed, 25 insertions(+), 8 deletions(-)
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index b0cdbf2ff..10e5e4954 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -98,7 +98,8 @@ void KinematicUnitObserver::onConnectObserver()
// register all channels
offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + "kinematic chain");
+ offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointaccelerations", "Joint accelerations of the " + robotNodeSetName + " kinematic chain");
offerChannel("jointtorques", "Joint torques of the" + robotNodeSetName + " kinematic chain");
offerChannel("jointcurrents", "Joint currents of the " + robotNodeSetName + " kinematic chain");
offerChannel("jointmotortemperatures", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
@@ -111,11 +112,12 @@ void KinematicUnitObserver::onConnectObserver()
std::string jointName = (*it)->getName();
ARMARX_VERBOSE << "* " << jointName << std::endl;
offerDataFieldWithDefault("jointcontrolmodes", jointName, ControlModeToString(eUnknown), "Controlmode of the " + jointName + " joint");
- offerDataField("jointangles", jointName, VariantType::Float, "Joint angle of the " + jointName + " joint in radians");
- offerDataField("jointvelocities", jointName, VariantType::Float, "Joint velocity of the " + jointName + " joint");
- offerDataField("jointtorques", jointName, VariantType::Float, "Joint torque of the " + jointName + " joint");
- offerDataField("jointcurrents", jointName, VariantType::Float, "Joint current of the " + jointName + " joint");
- offerDataField("jointmotortemperatures", jointName, VariantType::Float, "Joint motor temperature of the " + jointName + " joint");
+ offerDataField("jointangles", jointName, VariantType::Float, "Joint angle of the " + jointName + " joint in radians");
+ offerDataField("jointvelocities", jointName, VariantType::Float, "Joint velocity of the " + jointName + " joint");
+ offerDataField("jointaccelerations", jointName, VariantType::Float, "Joint acceleration of the " + jointName + " joint");
+ offerDataField("jointtorques", jointName, VariantType::Float, "Joint torque of the " + jointName + " joint");
+ offerDataField("jointcurrents", jointName, VariantType::Float, "Joint current of the " + jointName + " joint");
+ offerDataField("jointmotortemperatures", jointName, VariantType::Float, "Joint motor temperature of the " + jointName + " joint");
}
updateChannel("jointcontrolmodes");
@@ -214,7 +216,22 @@ void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques,
void KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
+ try
+ {
+ if (jointAccelerations.size() == 0)
+ {
+ return;
+ }
+
+ nameValueMapToDataFields("jointaccelerations", jointAccelerations, timestamp, aValueChanged);
+
+ updateChannel("jointaccelerations");
+ }
+ catch (...)
+ {
+ handleExceptions();
+ }
}
void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
@@ -279,14 +296,14 @@ void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelN
boost::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
if (timestamp < 0)
{
- for (const auto& it : nameValueMap)
+ for (const auto & it : nameValueMap)
{
map[it.first] = new Variant(it.second);
}
}
else
{
- for (const auto& it : nameValueMap)
+ for (const auto & it : nameValueMap)
{
map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
}
--
GitLab