diff --git a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
index 5699ab54b7ebadcbeeabc82a4cf9c97b5ac591be..2d846aa86220f7b6ca80fdf577947ad98e52af2c 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
@@ -33,7 +33,7 @@
<Joint type="revolute">
<!--DH theta="90" d="0" a="0" alpha="90" unitsangle="degree" unitslength="mm"/-->
<axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-45" hi="42"/>
+ <Limits unit="degree" lo="-45" hi="38"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
<MaxTorque value="3000"/>
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml b/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
index 4e462ca1eb3ed06f68f9828e9272de46950e7baf..77c6a952c08ae513210efe3a714046f30ab50a86 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
@@ -72,7 +72,7 @@
<Joint type="revolute">
<!--DH a="20" d="-310" theta="-90" alpha="90" units="degree"/-->
<axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-70" hi="109"/>
+ <Limits unit="degree" lo="-70" hi="70"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
<MaxTorque value="3000"/>
@@ -99,7 +99,7 @@
<Joint type="revolute">
<!--DH a="0" d="-7.5" theta="90" alpha="-90" units="degree"/-->
<axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-120" hi="40"/>
+ <Limits unit="degree" lo="-120" hi="19"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
<MaxTorque value="3000"/>
@@ -127,7 +127,7 @@
<!--DH a="0" d="-240" theta="90" alpha="-90" units="degree"/-->
<axis x="0" y="0" z="-1"/>
<!--Limits unit="degree" lo="-57.29" hi="174.48"/-->
- <Limits unit="degree" lo="-90" hi="200"/>
+ <Limits unit="degree" lo="-90" hi="158"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
<MaxTorque value="3000"/>
@@ -153,7 +153,7 @@
<Joint type="revolute">
<!--DH a="0" d="0" theta="-90" alpha="-90" units="degree"/-->
<axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-30" hi="30"/>
+ <Limits unit="degree" lo="-29" hi="30"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
<MaxTorque value="3000"/>
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 2fa220da656ab2d002693284a68f4e4ab660f74e..653bf523d4624394dee8434a86efc07d4f69f824 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -31,6 +31,7 @@
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
@@ -75,12 +76,48 @@ void ForceTorqueObserver::onInitObserver()
offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
+ usingProxy("RobotStateComponent");
+ usingTopic("DebugDrawerUpdates");
}
void ForceTorqueObserver::onConnectObserver()
{
+ assignProxy(robot, "RobotStateComponent");
+ debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
+ visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 50, false, "visualizerFunction");
+ localRobot = RemoteRobot::createLocalClone(robot);
+ visualizerTask->start();
+}
+void ForceTorqueObserver::visualizerFunction()
+{
+ RemoteRobot::synchronizeLocalClone(localRobot, robot);
+ auto channels = getAvailableChannels(false);
+ for(auto& channel : channels)
+ {
+ if(localRobot->hasRobotNode(channel.first))
+ {
+ DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::Current()));
+ FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
+ auto force = value->toGlobal(localRobot);
+// ARMARX_INFO << deactivateSpam(5,channel.first) << "new force" << channel.first << " : " << force->toEigen();
+
+ float forceMag = force->toEigen().norm()/100;
+ forceMag = std::min(1.0f, forceMag);
+ debugDrawer->setArrowVisu("Forces",
+ "Force" + channel.first,
+ new Vector3(localRobot->getRobotNode(channel.first)->getGlobalPose()),
+ force,
+ DrawColor{forceMag, 1.0f-forceMag, 0.0f, 0.5f},
+ 50,
+ 5);
+ }
+ else
+ {
+// ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
+ }
+ }
}
@@ -106,7 +143,7 @@ void ForceTorqueObserver::offerValue(std::string nodeName, const std::string& ty
}
else
{
- setDataField(id->channelName, id->datafieldName, Variant(value));
+ setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
}
@@ -232,3 +269,9 @@ DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::stri
DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
return id;
}
+
+void ForceTorqueObserver::onExitObserver()
+{
+ if(visualizerTask)
+ visualizerTask->stop();
+}
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 8aa5dbd05cf2ebcd1b8ebe227c6fe70823811215..ca2f27cc632d4be8d85676e4aba158accc60b929 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -26,6 +26,7 @@
#include <RobotAPI/interface/units/ForceTorqueUnit.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
namespace armarx
{
@@ -68,6 +69,9 @@ namespace armarx
}
void onInitObserver();
void onConnectObserver();
+ void onExitObserver();
+
+ void visualizerFunction();
virtual void reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&);
@@ -84,7 +88,10 @@ namespace armarx
private:
armarx::Mutex dataMutex;
std::string topicName;
-
+ RobotStateComponentInterfacePrx robot;
+ VirtualRobot::RobotPtr localRobot;
+ DebugDrawerInterfacePrx debugDrawer;
+ PeriodicTask<ForceTorqueObserver>::pointer_type visualizerTask;
void offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
@@ -97,6 +104,7 @@ namespace armarx
DataFieldIdentifierPtr getForceDatafieldId(const std::string& nodeName, const std::string& frame);
DataFieldIdentifierPtr getTorqueDatafieldId(const std::string& nodeName, const std::string& frame);
+
};
}
diff --git a/source/RobotAPI/components/units/HandUnit.cpp b/source/RobotAPI/components/units/HandUnit.cpp
index b30f96136afd126e2f7b90e7b5ef4b0c4aae6b31..03d8b9bb97b2bf517d119f3642f7efaa77f2b62d 100644
--- a/source/RobotAPI/components/units/HandUnit.cpp
+++ b/source/RobotAPI/components/units/HandUnit.cpp
@@ -191,6 +191,16 @@ std::string armarx::HandUnit::getHandName(const Ice::Current&)
return eef->getName();
}
+void HandUnit::setJointForces(const NameValueMap& targetJointForces, const Ice::Current&)
+{
+
+}
+
+void HandUnit::sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&)
+{
+
+}
+
void armarx::HandUnit::setJointAngles(const armarx::NameValueMap& targetJointAngles, const Ice::Current& c)
{
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 1fd2c51bfa6d13018024dae430a9346d0e58bb12..77d86ff97a8c38a64dbea8e185cfa0df37209d91 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -166,8 +166,8 @@ namespace armarx
// HandUnitInterface interface
public:
std::string getHandName(const Ice::Current&);
-
-
+ void setJointForces(const NameValueMap& targetJointForces, const Ice::Current&);
+ void sendJointCommands(const NameCommandMap& targetJointCommands , const Ice::Current&);
};
}
diff --git a/source/RobotAPI/components/units/HapticObserver.cpp b/source/RobotAPI/components/units/HapticObserver.cpp
index 569894080e054db4adcfd7e6c8639341dc21996a..ed10cc0355424c71e974b67d624b754d92991d7d 100644
--- a/source/RobotAPI/components/units/HapticObserver.cpp
+++ b/source/RobotAPI/components/units/HapticObserver.cpp
@@ -61,6 +61,7 @@ void HapticObserver::reportSensorValues(const std::string& device, const std::st
float max = eigenMatrix.maxCoeff();
float mean = eigenMatrix.mean();
std::string channelName = name;
+ Eigen::MatrixXf M = matrix->toEigen();
if (!existsChannel(channelName))
{
@@ -72,6 +73,17 @@ void HapticObserver::reportSensorValues(const std::string& device, const std::st
offerDataFieldWithDefault(channelName, "mean", Variant(mean), "Mean value");
offerDataFieldWithDefault(channelName, "timestamp", timestampPtr, "Timestamp");
offerDataFieldWithDefault(channelName, "rate", Variant(0.0f), "Sample rate");
+
+ for (int i = 0; i < M.rows(); i++)
+ {
+ for (int j = 0; j < M.cols(); j++)
+ {
+ std::stringstream s;
+ s << "entry_" << i << "," << j;
+ offerDataFieldWithDefault(channelName, s.str(), Variant(M(i, j)), "Individual matrix entry");
+ }
+ }
+
ARMARX_INFO << "Offering new channel: " << channelName;
}
else
@@ -82,6 +94,17 @@ void HapticObserver::reportSensorValues(const std::string& device, const std::st
setDataField(channelName, "max", Variant(max));
setDataField(channelName, "mean", Variant(mean));
setDataField(channelName, "timestamp", timestampPtr);
+
+ for (int i = 0; i < M.rows(); i++)
+ {
+ for (int j = 0; j < M.cols(); j++)
+ {
+ std::stringstream s;
+ s << "entry_" << i << "," << j;
+ setDataField(channelName, s.str(), Variant(M(i, j)));
+ }
+ }
+
}
/*if(statistics.count(device) > 0)
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 8b2e980f6d6facd4e5949dbb221f83b60d387a56..c30bfdb063a7a01368047e2e17c68421148a1123 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -79,6 +79,7 @@ namespace armarx
if (drawer)
{
drawer->removeSphereDebugLayerVisu("HeadViewTarget");
+ drawer->removeSphereDebugLayerVisu("HeadViewTargetSolution");
}
@@ -204,6 +205,7 @@ namespace armarx
if (drawer)
{
drawer->removeSphereDebugLayerVisu("HeadViewTarget");
+ drawer->removeSphereDebugLayerVisu("HeadViewTargetSolution");
}
// why do we stop the kin unit?
@@ -286,7 +288,7 @@ namespace armarx
if (drawer && localRobot->hasRobotNode("Cameras"))
{
Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose());
- drawer->setSphereDebugLayerVisu("HeadViewTarget",
+ drawer->setSphereDebugLayerVisu("HeadViewTargetSolution",
startPos,
DrawColor {0, 1, 1, 0.2},
17);
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 19673d07830dfa863605cb1648c0f2b4902d30a0..dc4c2776f599c187c33bb8404f536662b3ad3bb9 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -260,12 +260,13 @@ void KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatus
void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap)
{
NameValueMap::const_iterator iter = nameValueMap.begin();
-
+ StringVariantBaseMap map;
while (iter != nameValueMap.end())
{
- setDataFieldFlatCopy(channelName, iter->first, new Variant(iter->second));
+ map[iter->first] = new Variant(iter->second);
iter++;
}
+ setDataFieldsFlatCopy(channelName, map);
}
PropertyDefinitionsPtr KinematicUnitObserver::createPropertyDefinitions()
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
index ca3390b1aeb5fe7f7133f25abda89b8beab1d6e5..759b4cb477dbeeb79773b0cea1ed8ec93dec9e06 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
@@ -234,7 +234,7 @@ namespace armarx
{
logstream << "Timestamp";
- for (auto & channel : selectedChannels)
+ for (auto& channel : selectedChannels)
{
logstream << "," << channel.toStdString();
}
@@ -679,7 +679,7 @@ namespace armarx
}
// QDateTime time(QDateTime::currentDateTime());
- VariantBaseList variants = proxyMap[observerName]->getDataFields(it->second);
+ TimedVariantBaseList variants = proxyMap[observerName]->getDataFields(it->second);
// ARMARX_IMPORTANT << "data from observer: " << observerName;
for (unsigned int i = 0; i < variants.size(); ++i)
@@ -698,7 +698,7 @@ namespace armarx
// ARMARX_IMPORTANT << id;
auto dict = JSONObject::ConvertToBasicVariantMap(json, var);
- for (const auto & e : dict)
+ for (const auto& e : dict)
{
std::string key = id + "." + e.first;
// ARMARX_INFO << key << ": " << *VariantPtr::dynamicCast(e.second);
@@ -742,7 +742,7 @@ namespace armarx
{
logstream << (time - logStartTime).toMilliSecondsDouble();
- for (const auto & elem : dataMaptoAppend)
+ for (const auto& elem : dataMaptoAppend)
{
logstream << "," /*<< elem.first << ","*/ << elem.second->getOutputValueOnly();
}
diff --git a/source/RobotAPI/interface/speech/SpeechInterface.ice b/source/RobotAPI/interface/speech/SpeechInterface.ice
index d9df885c189f5fae6c2ae7538248d3e5ece2694c..cbdba5932b0eefdbd184479b02640a191cf6a8e4 100644
--- a/source/RobotAPI/interface/speech/SpeechInterface.ice
+++ b/source/RobotAPI/interface/speech/SpeechInterface.ice
@@ -23,6 +23,8 @@
#ifndef _ARMARX_ROBOTAPI_SPEECH_INTERFACE_SLICE_
#define _ARMARX_ROBOTAPI_SPEECH_INTERFACE_SLICE_
+#include <Ice/BuiltinSequences.ice>
+
module armarx
{
@@ -74,6 +76,12 @@ module armarx
* \param text Text.
*/
void reportText(string text);
+ /*!
+ * \brief Callback method that is called when a piece of text with params has been published.
+ * \param text Text.
+ * \param string vector params.
+ */
+ //void reportTextWithParams(string text,Ice::StringSeq params);
};
enum FeedbackType
diff --git a/source/RobotAPI/interface/units/HandUnitInterface.ice b/source/RobotAPI/interface/units/HandUnitInterface.ice
index f3df38d6289ac39d3d8a93f5f63da3f0a8be4ab9..1935af853964dfeda0dc046692e9750e7eda71a7 100644
--- a/source/RobotAPI/interface/units/HandUnitInterface.ice
+++ b/source/RobotAPI/interface/units/HandUnitInterface.ice
@@ -34,6 +34,8 @@
module armarx
{
+ dictionary<string, string> NameCommandMap;
+
/**
* Implements an interface to a HandUnit.
*/
@@ -72,6 +74,10 @@ module armarx
* @param targetJointAngles Map of joint names and corresponding joint angle values.
*/
void setJointAngles(NameValueMap targetJointAngles);
+
+ void setJointForces(NameValueMap targetJointForces);
+
+ void sendJointCommands(NameCommandMap targetJointCommands);
};
/**
* Implements an interface to a HandUnitListener.