diff --git a/scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg b/scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg
index 01fcafba4fec1e48dbcca6eb8abde6ec4582352c..6c8b174ef8ed1ea4d7a1aee4bf64f939d87bee4b 100644
--- a/scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg
+++ b/scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg
@@ -112,6 +112,6 @@ ArmarX.RobotControlStateOfferer.proxyName = "StatechartProfilesTestGroupRemoteSt
# - Case sensitivity: no
# - Required: no
# ArmarX.RobotControlStateOfferer.ObjectName = ""
-
+ArmarX.RobotControlStateOfferer.XMLStatechartProfile = "Armar3Simulation"
diff --git a/scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg b/scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg
index 01fcafba4fec1e48dbcca6eb8abde6ec4582352c..0de35bad4f0b589d4eee3c528a38b196d3119eeb 100644
--- a/scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg
+++ b/scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg
@@ -113,5 +113,5 @@ ArmarX.RobotControlStateOfferer.proxyName = "StatechartProfilesTestGroupRemoteSt
# - Required: no
# ArmarX.RobotControlStateOfferer.ObjectName = ""
-
+ArmarX.XMLStateComponent.XMLStatechartProfile = "Armar3a"
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 9966218f2fb5ee6c7d0adecbff44a09255a7a207..1fff83b1986ec58b475429d4e48a4f5fdd3c0ecf 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -72,6 +72,14 @@ namespace armarx
this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
_synchronized->setName(getProperty<std::string>("AgentName").getValue());
+ robotModelScaling = getProperty<float>("RobotModelScaling").getValue();
+ ARMARX_INFO << "scale factor: " << robotModelScaling;
+ if (robotModelScaling != 1.0f)
+ {
+ ARMARX_INFO << "Scaling robot model with scale factor " << robotModelScaling;
+ _synchronized = _synchronized->clone(_synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
+ }
+
if (this->_synchronized)
{
ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
@@ -105,6 +113,7 @@ namespace armarx
}
usingTopic(robotNodeSetName + "State");
+
/*VirtualRobot::RobotNodeSetPtr pns = this->_synchronized->getRobotNodeSet("Platform");
if (pns)
{
@@ -319,6 +328,11 @@ namespace armarx
return relativeRobotFile;
}
+ float RobotStateComponent::getScaling(const Ice::Current&) const
+ {
+ return robotModelScaling;
+ }
+
std::vector<string> RobotStateComponent::getArmarXPackages(const Current&) const
{
std::vector<string> result;
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index aa626e121f01edcfb7d78d9c13a62b6c720127fe..90984bb8fce80b6d702eaf21da7a482b83a98bfd 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -52,6 +52,7 @@ namespace armarx
defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history");
+ defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
}
};
@@ -132,6 +133,8 @@ namespace armarx
void setRobotStateObserver(RobotStateObserverPtr observer);
bool interpolate(long time, RobotStateConfig& config) const;
+
+ float getScaling(const Ice::Current&) const;
protected:
/**
* Load and create a VirtualRobot::Robot instance from the RobotFileName
@@ -171,7 +174,7 @@ namespace armarx
std::string robotNodeSetName;
-
+ float robotModelScaling;
};
}
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 756b00b155b3bcd8c0e551d14a9d00eef6879c83..edf26cd26e856103945f568019cf8798335cd5d4 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -76,17 +76,17 @@ void ForceTorqueObserver::onInitObserver()
offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
- usingProxy("RobotStateComponent");
+ usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
offeringTopic("DebugDrawerUpdates");
}
void ForceTorqueObserver::onConnectObserver()
{
-
+ robot = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
if (getProperty<bool>("VisualizeForce").getValue())
{
- visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
+ visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 1000 / getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
visualizerTask->start();
}
@@ -125,7 +125,7 @@ void ForceTorqueObserver::visualizerFunction()
float forceFactor = getProperty<float>("ForceVisualizerFactor").getValue();
RemoteRobot::synchronizeLocalClone(localRobot, robot);
auto channels = getAvailableChannels(false);
- for (auto& channel : channels)
+ for (auto & channel : channels)
{
if (localRobot->hasRobotNode(channel.first))
{
@@ -164,38 +164,61 @@ PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
}
-void ForceTorqueObserver::offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id)
+void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id)
{
FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
- if (!existsDataField(id->channelName, id->datafieldName))
+
+ try
{
- if (!existsChannel(id->channelName))
- {
- offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
- }
- offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+ setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
}
- else
+ catch (...)
{
- setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
+ ARMARX_INFO << "Creating force/torque fields";
+ if (!existsDataField(id->channelName, id->datafieldName))
+ {
+ if (!existsChannel(id->channelName))
+ {
+ offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
+ }
+ offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+ }
}
// pod = plain old data
std::string pod_channelName = id->channelName + POD_SUFFIX;
- if (!existsChannel(pod_channelName))
+
+
+ try
+ {
+ StringVariantBaseMap values;
+ values[id->datafieldName + "_x" ] = new Variant(vec->x);
+ values[id->datafieldName + "_y" ] = new Variant(vec->y);
+ values[id->datafieldName + "_z" ] = new Variant(vec->z);
+ values[id->datafieldName + "_frame"] = new Variant(vec->frame);
+ setDataFieldsFlatCopy(pod_channelName, values);
+ }
+ catch (...)
{
- offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
+ ARMARX_INFO << "Creating force/torque pod fields";
+ if (!existsChannel(pod_channelName))
+ {
+ offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
+
+ }
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
}
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
+
+
}
void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&)
@@ -223,7 +246,7 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
updateChannel(id->channelName);
updateChannel(id->channelName + POD_SUFFIX);
}
- catch (std::exception& e)
+ catch (std::exception&)
{
ARMARX_ERROR << "Reporting force torque for " << sensorNodeName << " failed! ";
handleExceptions();
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 8cf255be21512233a4d8caf8f3c836c5a4a4fd40..e291a52e49d65f872febe613e777f97fc3b8c771 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -43,9 +43,11 @@ namespace armarx
{
defineRequiredProperty<std::string>("ForceTorqueTopicName", "Name of the ForceTorqueUnit Topic");
defineOptionalProperty<bool>("VisualizeForce", true, "Visualize the force with an arrow in the debug drawer");
- defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 20, "Frequency with which the force is visualized");
- defineOptionalProperty<float>("ForceVisualizerFactor", 0.01, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
+ defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 30, "Frequency with which the force is visualized");
+ defineOptionalProperty<float>("ForceVisualizerFactor", 0.01f, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
defineOptionalProperty<float>("MaxForceArrowLength", 150, "Length of the force visu arrow in mm");
+ defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
+
}
};
@@ -97,7 +99,7 @@ namespace armarx
DebugDrawerInterfacePrx debugDrawer;
PeriodicTask<ForceTorqueObserver>::pointer_type visualizerTask;
- void offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
+ void offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id);
// ForceTorqueUnitObserverInterface interface
public:
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
index 01596e7e599a2fe8cf22a57e095e5fae4ed1336a..f70637e0f1b7fba0562011159b5bfd6e0fd45eb2 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
@@ -49,10 +49,11 @@ KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
connect(this->ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfig()));
ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
proxyFinder = new IceProxyFinder<KinematicUnitInterfacePrx>(this);
- proxyFinder->setSearchMask("*");
+ proxyFinder->setSearchMask("*Unit");
ui->gridLayout->addWidget(proxyFinder, 0, 1, 1, 2);
connect(proxyFinder->getUi()->cbProxyName, SIGNAL(currentIndexChanged(int)), this, SLOT(selectionChanged(int)));
+ connect(proxyFinder->getUi()->cbProxyName, SIGNAL(editTextChanged(QString)), this, SLOT(proxyNameChanged(QString)));
}
KinematicUnitConfigDialog::~KinematicUnitConfigDialog()
@@ -88,20 +89,13 @@ void KinematicUnitConfigDialog::verifyConfig()
}
}
-void KinematicUnitConfigDialog::selectionChanged(int nr)
+void KinematicUnitConfigDialog::updateSubconfig(std::string kinematicUnitName)
{
- ARMARX_LOG << "Selected entry:" << nr;
- ui->labelTopic->setText("");
- ui->labelRobotModel->setText("");
- ui->labelRNS->setText("");
- if (nr < 0)
- {
- return;
- }
- std::string kinematicUnitName = proxyFinder->getUi()->cbProxyName->currentText().toStdString();
+
try
{
ARMARX_INFO << "Connecting to KinematicUnitProxy " << kinematicUnitName;
+
KinematicUnitInterfacePrx kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
std::string topicName = kinematicUnitInterfacePrx->getReportTopicName();
std::string robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
@@ -114,7 +108,30 @@ void KinematicUnitConfigDialog::selectionChanged(int nr)
{
ARMARX_INFO << "Could not connect to KinematicUnitProxy " << kinematicUnitName;
}
+}
+
+void KinematicUnitConfigDialog::selectionChanged(int nr)
+{
+ ARMARX_LOG << "Selected entry:" << nr;
+ ui->labelTopic->setText("");
+ ui->labelRobotModel->setText("");
+ ui->labelRNS->setText("");
+ if (nr < 0)
+ {
+ return;
+ }
+ std::string kinematicUnitName = proxyFinder->getUi()->cbProxyName->currentText().toStdString();
+
+ updateSubconfig(kinematicUnitName);
+
+}
+void KinematicUnitConfigDialog::proxyNameChanged(QString kinematicUnitName)
+{
+ ui->labelTopic->setText("");
+ ui->labelRobotModel->setText("");
+ ui->labelRNS->setText("");
+ updateSubconfig(kinematicUnitName.toStdString());
}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
index 9e0ab1ea9821fe878dd71014dbc54abef8a6d026..21e73d58f30a570633355075a159d19852203ca7 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
@@ -65,6 +65,10 @@ namespace armarx
void verifyConfig();
void selectionChanged(int nr);
+ void proxyNameChanged(QString);
+ protected slots:
+ void updateSubconfig(std::string kinematicUnitName);
+
private:
IceProxyFinderBase* proxyFinder;
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index bf49e7c30e9c5771b48f8eba2526742e3a7c68cf..211e62273f9bd6ab6ba908b23167db89a205b106 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -189,6 +189,14 @@ module armarx
["cpp:const"]
idempotent string getRobotName() throws NotInitializedException;
+
+ /**
+ * @return The scaling of the robot model represented by this component.
+ *
+ */
+ ["cpp:const"]
+ idempotent float getScaling();
+
/**
* @return The name of the robot node set that is represented by this component.
*
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 7c293b74840d6f55ec21f0cf1fabe253468441f9..f640b6f7d5ba4c2d724271d1d967fe966f102e70 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -327,10 +327,10 @@ namespace armarx
VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename)
{
- return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename);
+ return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling());
}
- RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename)
+ RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename, float scaling)
{
boost::recursive_mutex::scoped_lock cloneLock(m);
ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
@@ -362,6 +362,12 @@ namespace armarx
ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
return result;
}
+
+ if (scaling != 1.0f)
+ {
+ ARMARX_INFO_S << "Scaling robot to " << scaling;
+ result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
+ }
}
synchronizeLocalClone(result, sharedRobotPrx);
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index c87eaf1602e23b3597aacf5f37f59af32b76c6f5..1a7b24532f5baca5567912a50b836e40a56f5747 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -184,7 +184,7 @@ namespace armarx
*/
static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string());
- static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string());
+ static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string(), float scaling = 1.0f);
/*!
Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.cpp b/source/RobotAPI/statecharts/operations/RobotControl.cpp
index 47c178faafd6d44ddb8c597dc59cad5fdb4085c2..d99d262ac3203a9a9037f747b80cc057b02f12e8 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.cpp
+++ b/source/RobotAPI/statecharts/operations/RobotControl.cpp
@@ -80,7 +80,7 @@ namespace armarx
robotFunctionalState = stateList.begin()->second;
callRemoteState(stateId, StringVariantContainerBaseMap());
- const std::string proxyName = getProperty<std::string>("proxyName").getValue();
+ const std::string proxyName = getProperty<std::string>("XMLStatechartProfile").getValue() + getProperty<std::string>("proxyName").getValue();
const std::string stateName = getProperty<std::string>("stateName").getValue();
ARMARX_IMPORTANT << VAROUT(proxyName) << VAROUT(stateName);
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.h b/source/RobotAPI/statecharts/operations/RobotControl.h
index c0d2cb409b03da05d24c8ea87e18ee8d7ee16949..fa875b00e5da55fc8a6e58a4232bf481f4af342d 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.h
+++ b/source/RobotAPI/statecharts/operations/RobotControl.h
@@ -38,6 +38,8 @@ namespace armarx
RobotControlContextProperties(std::string prefix):
StatechartContextPropertyDefinitions(prefix)
{
+
+ defineOptionalProperty<std::string>("XMLStatechartProfile", "", "Name of the statechart profile to be used. This is used as prefix to the proxyName. So GraspGroupRemoteStateOfferer will be Armar3aGraspGroupRemoteStateOfferer");
defineOptionalProperty<std::string>("proxyName", "", "name of the proxy to load");
defineOptionalProperty<std::string>("stateName", "", "name of the state to load");
}