diff --git a/interface/slice/hardware/HardwareInterface.ice b/interface/slice/hardware/HardwareInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..063eb09cb12b2842f284d1067264d50ca2edc2da
--- /dev/null
+++ b/interface/slice/hardware/HardwareInterface.ice
@@ -0,0 +1,97 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX::Core
+ * @author Peter Kaiser (peter dot kaiser at kit dot edu)
+ * @author Matthias Hadlich (matthias dot hadlich at student dot kit dot edu)
+ * @copyright 2014 Peter Kaiser
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_HARDWAREINTERFACE_SLICE_
+#define _ARMARX_CORE_HARDWAREINTERFACE_SLICE_
+
+#include <core/UserException.ice>
+#include <core/BasicTypes.ice>
+#include <units/KinematicUnitInterface.ice>
+
+module armarx
+{
+ exception UnknownBusException extends UserException
+ {
+ string busName;
+ };
+
+ exception UnknownDeviceException extends UserException
+ {
+ string deviceName;
+ };
+
+ exception UnknownCommandException extends UserException
+ {
+ string commandName;
+ };
+
+
+ struct DeviceStatus
+ {
+ OperationStatus operation;
+ ErrorStatus error;
+
+ bool enabled;
+ bool emergencyStop;
+ };
+
+ sequence<string> BusNames;
+ sequence<string> DeviceNames;
+
+ struct DeviceInformation
+ {
+ int id;
+ string type;
+ string node;
+ int heartBeatErrorCounter;
+
+ DeviceStatus status;
+ };
+
+ struct BusInformation
+ {
+ int baudrate;
+ int amountOfErrors;
+ };
+
+ enum BasicOperation { eStart, eStop, eReset };
+
+
+ interface HardwareInterface
+ {
+ BusNames getConfiguredBusses();
+ BusInformation getBusInformation(string bus) throws UnknownBusException;
+ DeviceNames getDevicesOnBus(string bus) throws UnknownBusException;
+ DeviceInformation getDeviceInformation(string device) throws UnknownDeviceException;
+ int performBasicOperation(BasicOperation operation);
+ int sendCommand(string command) throws UnknownCommandException;
+ };
+
+ interface HardwareListener extends KinematicUnitListener
+ {
+
+ };
+};
+
+#endif
diff --git a/interface/slice/observers/KinematicUnitObserverInterface.ice b/interface/slice/observers/KinematicUnitObserverInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..5e87abadd2bb2b97ee979d98bdbd0ba6b1a28e87
--- /dev/null
+++ b/interface/slice/observers/KinematicUnitObserverInterface.ice
@@ -0,0 +1,37 @@
+ /*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core::Observers
+* @author Kai Welke <welke@kit.edu>
+* @copyright 2011 Humanoids Group, HIS, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_KINEMATIC_UNIT_OBSERVER_SLICE_
+#define _ARMARX_CORE_KINEMATIC_UNIT_OBSERVER_SLICE_
+
+#include <observers/ObserverInterface.ice>
+#include <units/KinematicUnitInterface.ice>
+
+module armarx
+{
+ interface KinematicUnitObserverInterface extends ObserverInterface, KinematicUnitListener
+ {
+ };
+};
+
+#endif
diff --git a/interface/slice/observers/PlatformUnitObserverInterface.ice b/interface/slice/observers/PlatformUnitObserverInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..c2d0f73872f38f99ce527793ddae9468676589dd
--- /dev/null
+++ b/interface/slice/observers/PlatformUnitObserverInterface.ice
@@ -0,0 +1,37 @@
+ /*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core::Observers
+* @author Manfred Kroehnert (manfred dot kroehnert at kit dot edu)
+* @copyright 2013 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_PLATFORM_UNIT_OBSERVER_SLICE_
+#define _ARMARX_CORE_PLATFORM_UNIT_OBSERVER_SLICE_
+
+#include <observers/ObserverInterface.ice>
+#include <units/PlatformUnitInterface.ice>
+
+module armarx
+{
+ interface PlatformUnitObserverInterface extends ObserverInterface, PlatformUnitListener
+ {
+ };
+};
+
+#endif
diff --git a/interface/slice/robotstate/LinkedPoseBase.ice b/interface/slice/robotstate/LinkedPoseBase.ice
new file mode 100644
index 0000000000000000000000000000000000000000..c1415a6b3721b9a74e4d060a2b6665e82338458d
--- /dev/null
+++ b/interface/slice/robotstate/LinkedPoseBase.ice
@@ -0,0 +1,45 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Manfred Kroehnert (Manfred dot Kroehnert at kit dot edu)
+* @copyright 2013 H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_ROBOTSTATE_LINKEDPOSE_SLICE_
+#define _ARMARX_CORE_ROBOTSTATE_LINKEDPOSE_SLICE_
+
+#include <robotstate/PoseBase.ice>
+#include <robotstate/RobotState.ice>
+
+module armarx
+{
+ ["cpp:virtual"]
+ class LinkedPoseBase extends FramedPoseBase
+ {
+ SharedRobotInterface* referenceRobot;
+ };
+
+ ["cpp:virtual"]
+ class LinkedVector3Base extends FramedVector3Base
+ {
+ SharedRobotInterface* referenceRobot;
+ };
+};
+
+#endif
diff --git a/interface/slice/robotstate/PoseBase.ice b/interface/slice/robotstate/PoseBase.ice
new file mode 100644
index 0000000000000000000000000000000000000000..a8a57a485abbec42eef55c6f8d29f2099888bb39
--- /dev/null
+++ b/interface/slice/robotstate/PoseBase.ice
@@ -0,0 +1,87 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Stefan Ulbrich (ulbrich at kit dot edu)
+* @copyright 2011 Humanoids Group, HIS, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_ROBOTSTATE_ARMAR_POSE_SLICE_
+#define _ARMARX_CORE_ROBOTSTATE_ARMAR_POSE_SLICE_
+
+#include <observers/VariantBase.ice>
+
+
+
+module armarx
+{
+
+ class Vector3Base extends VariantDataClass
+ {
+ float x = zeroInt;
+ float y = zeroInt;
+ float z = zeroInt;
+ };
+
+ class QuaternionBase extends VariantDataClass
+ {
+ float qw = zeroInt;
+ float qx = zeroInt;
+ float qy = zeroInt;
+ float qz = zeroInt;
+ };
+
+ ["cpp:virtual"]
+ class PoseBase extends VariantDataClass
+ {
+ Vector3Base position;
+ QuaternionBase orientation;
+ };
+
+ ["cpp:virtual"]
+ class FramedVector3Base extends Vector3Base
+ {
+ string frame;
+ };
+
+
+ ["cpp:virtual"]
+ class FramedPoseBase extends PoseBase
+ {
+ string frame;
+ };
+
+ ["cpp:virtual"]
+ class FramedPositionBase extends Vector3Base
+ {
+ string frame;
+ };
+
+ ["cpp:virtual"]
+ class FramedOrientationBase extends QuaternionBase
+ {
+ string frame;
+ };
+
+ dictionary<string, FramedVector3Base> FramedVector3Map;
+ dictionary<string, FramedPositionBase> FramedPositionMap;
+
+
+};
+
+#endif
diff --git a/interface/slice/robotstate/RobotState.ice b/interface/slice/robotstate/RobotState.ice
new file mode 100644
index 0000000000000000000000000000000000000000..a881463fb14bf45f3b0f17c2988ec87da75cb244
--- /dev/null
+++ b/interface/slice/robotstate/RobotState.ice
@@ -0,0 +1,130 @@
+#ifndef ARMARX_ROBOTSTATE_ROBOTSTATE_SLICE
+#define ARMARX_ROBOTSTATE_ROBOTSTATE_SLICE
+
+#include <units/KinematicUnitInterface.ice>
+#include <units/PlatformUnitInterface.ice>
+#include <robotstate/PoseBase.ice>
+
+module armarx
+{
+ /**
+ * Available VirtualRobot joint types.
+ */
+ enum JointType
+ {
+ ePrismatic,
+ eFixed,
+ eRevolute
+ };
+
+ /**
+ * Distributed reference counting interface for the SharedRobotInterace
+ * instances.
+ */
+ interface SharedObjectInterface
+ {
+ void ref();
+ void unref();
+ };
+
+ sequence<string> NameList;
+
+ /**
+ * The data stored in a VirtualRobot::RobotNodeSet but accessable over Ice.
+ */
+ class RobotNodeSetInfo
+ {
+ NameList names;
+ string name;
+ string tcpName;
+ string rootName;
+ };
+
+ /**
+ * The SharedRobotNodeInterface provides access to a limited amount of
+ * VirtualRobot::RobotNode methods over the Ice network.
+ */
+ interface SharedRobotNodeInterface extends SharedObjectInterface
+ {
+
+ ["cpp:const"] idempotent
+ float getJointValue();
+ ["cpp:const"] idempotent
+ string getName();
+
+ ["cpp:const"] idempotent
+ PoseBase getLocalTransformation();
+
+ ["cpp:const"] idempotent
+ PoseBase getGlobalPose();
+ ["cpp:const"] idempotent
+ PoseBase getGlobalPoseJoint();
+
+ ["cpp:const"] idempotent
+ JointType getType();
+ ["cpp:const"] idempotent
+ Vector3Base getJointTranslationDirection();
+ ["cpp:const"] idempotent
+ Vector3Base getJointRotationAxis();
+
+ ["cpp:const"] idempotent
+ bool hasChild(string name, bool recursive);
+ ["cpp:const"] idempotent
+ string getParent();
+ ["cpp:const"] idempotent
+ NameList getChildren();
+ ["cpp:const"] idempotent
+ NameList getAllParents(string name);
+
+ ["cpp:const"] idempotent
+ float getJointValueOffest();
+ ["cpp:const"] idempotent
+ float getJointLimitHigh();
+ ["cpp:const"] idempotent
+ float getJointLimitLow();
+
+ };
+
+
+ /**
+ * The SharedRobotInterface provides access to a limited amount of
+ * VirtualRobot::Robot methods over the Ice network.
+ */
+ interface SharedRobotInterface extends SharedObjectInterface
+ {
+ SharedRobotNodeInterface* getRobotNode(string name);
+ SharedRobotNodeInterface* getRootNode();
+ bool hasRobotNode(string name);
+ NameList getRobotNodes();
+
+ RobotNodeSetInfo getRobotNodeSet(string name);
+ NameList getRobotNodeSets();
+ bool hasRobotNodeSet(string name);
+
+ string getName();
+ string getType();
+
+ PoseBase getGlobalPose();
+ NameValueMap getConfig();
+ };
+
+
+ /**
+ * The interface used by the RobotStateComponent which allows creating
+ * snapshots of a robot configuration or retrieving a proxy to a constantly
+ * updating synchronized robot.
+ */
+ interface RobotStateComponentInterface extends KinematicUnitListener, PlatformUnitListener
+ {
+ /**
+ * @return proxy to the shared robot which constantly updates all joint values
+ */
+ SharedRobotInterface* getSynchronizedRobot();
+ /**
+ * @return proxy to a copy of the shared robot with non updating joint values
+ */
+ SharedRobotInterface* getRobotSnapshot(string time);
+ };
+};
+
+#endif
diff --git a/interface/slice/robotstate/RobotStateObserverInterface.ice b/interface/slice/robotstate/RobotStateObserverInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..58b9554b834327e04d984ed768f9f7dcded74cf5
--- /dev/null
+++ b/interface/slice/robotstate/RobotStateObserverInterface.ice
@@ -0,0 +1,38 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Stefan Ulbrich (stefan dot ulbrich at kit dot edu)
+* @author Kai Welke (welke at kit dot edu)
+* @copyright 2011 Humanoids Group, HIS, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_ROBOTSTATE_OBSERVER_SLICE_
+#define _ARMARX_CORE_ROBOTSTATE_OBSERVER_SLICE_
+
+#include <observers/ObserverInterface.ice>
+#include <units/KinematicUnitInterface.ice>
+
+module armarx
+{
+ interface RobotStateObserverInterface extends ObserverInterface, KinematicUnitListener
+ {
+ };
+};
+
+#endif
diff --git a/interface/slice/units/HandUnitInterface.ice b/interface/slice/units/HandUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..acd22363daf3b15cdf94421aec36986822891496
--- /dev/null
+++ b/interface/slice/units/HandUnitInterface.ice
@@ -0,0 +1,54 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX::Core
+ * @author Manfred Kroehnert (manfred dot kroehnert at kit dot edu)
+ * @copyright 2013 Manfred Kroehnert
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_UNITS_HANDUNIT_SLICE_
+#define _ARMARX_CORE_UNITS_HANDUNIT_SLICE_
+
+#include <units/UnitInterface.ice>
+#include <units/KinematicUnitInterface.ice>
+#include <core/UserException.ice>
+#include <core/BasicTypes.ice>
+#include <observers/VariantContainers.ice>
+
+module armarx
+{
+
+ interface HandUnitInterface extends SensorActorUnitInterface
+ {
+ void open();
+ void close();
+ void preshape(string preshapeName);
+ SingleTypeVariantListBase getPreshapeNames();
+ NameValueMap getPreshapeJointValues(string preshapeName);
+ };
+
+ interface HandUnitListener
+ {
+ void reportHandClosed();
+ void reportHandOpened();
+ void reportHandPreshaped();
+ };
+
+};
+
+#endif
diff --git a/interface/slice/units/KinematicUnitInterface.ice b/interface/slice/units/KinematicUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..71cb7e6609f54e590ab126a56e4d371008341eed
--- /dev/null
+++ b/interface/slice/units/KinematicUnitInterface.ice
@@ -0,0 +1,163 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX::Core
+ * @author Christian Boege (boege at kit dot edu)
+ * @copyright 2011 Christian Boege
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_KINEMATICUNIT_SLICE_
+#define _ARMARX_CORE_KINEMATICUNIT_SLICE_
+
+#include <core/UserException.ice>
+#include <core/BasicTypes.ice>
+#include <units/UnitInterface.ice>
+
+module armarx
+{
+ struct RangeViolation
+ {
+ float rangeFrom;
+ float rangeTo;
+ float actualValue;
+ };
+
+ sequence<RangeViolation> RangeViolationSequence;
+
+ exception OutOfRangeException extends UserException
+ {
+ RangeViolationSequence violation;
+ };
+
+ enum ControlMode
+ {
+ eDisabled,
+ eUnknown,
+ ePositionControl,
+ eVelocityControl,
+ eTorqueControl,
+ ePositionVelocityControl
+ };
+
+ enum OperationStatus
+ {
+ eOffline,
+ eOnline,
+ eInitialized
+ };
+
+ enum ErrorStatus
+ {
+ eOk,
+ eWarning,
+ eError
+ };
+
+ struct JointStatus
+ {
+ OperationStatus operation;
+ ErrorStatus error;
+
+ bool enabled;
+ bool emergencyStop;
+ };
+
+ exception ControlModeNotSupportedException extends UserException
+ {
+ ControlMode mode;
+ };
+
+ exception KinematicUnitUnavailable extends ResourceUnavailableException
+ {
+ StringStringDictionary nodeOwners;
+ };
+
+ exception KinematicUnitNotOwnedException extends ResourceNotOwnedException
+ {
+ StringSequence nodes;
+ };
+
+ dictionary<string, float> NameValueMap;
+ dictionary<string, ControlMode> NameControlModeMap;
+ dictionary<string, JointStatus> NameStatusMap;
+
+ interface KinematicUnitInterface extends SensorActorUnitInterface
+ {
+ /*
+ * Requesting and releasing joints guarantees that a joint is controlled by exactly
+ * one component at any time
+ *
+ * The request only affects the active access, sensor values can always be read.
+ *
+ * TODO: The actual access has to be checked using a token.
+ */
+ void requestJoints(StringSequence joints) throws KinematicUnitUnavailable;
+ void releaseJoints(StringSequence joints) throws KinematicUnitNotOwnedException;
+
+ void switchControlMode(NameControlModeMap targetJointModes) throws ControlModeNotSupportedException;
+
+ /*
+ * Depending on the chosen control mode, one or more of the following functions need to be called
+ * to set all necessary parameters.
+ *
+ * The motion starts after all values have been set.
+ */
+ void setJointAngles(NameValueMap targetJointAngles) throws OutOfRangeException;
+ void setJointVelocities(NameValueMap targetJointVelocities) throws OutOfRangeException;
+ void setJointTorques(NameValueMap targetJointTorques) throws OutOfRangeException;
+ void setJointAccelerations(NameValueMap targetJointAccelerations) throws OutOfRangeException;
+ void setJointDecelerations(NameValueMap targetJointDecelerations) throws OutOfRangeException;
+
+ /*
+ * NYI
+ */
+ //void set Trajectory(...);
+ };
+
+ interface KinematicUnitListener
+ {
+ void reportControlModeChanged(NameControlModeMap actualJointModes, bool aValueChanged);
+
+ void reportJointAngles(NameValueMap actualJointAngles, bool aValueChanged);
+ void reportJointVelocities(NameValueMap actualJointVelocities, bool aValueChanged);
+ void reportJointTorques(NameValueMap actualJointTorques, bool aValueChanged);
+ void reportJointCurrents(NameValueMap actualJointCurrents, bool aValueChanged);
+ void reportJointMotorTemperatures(NameValueMap actualJointMotorTemperatures, bool aValueChanged);
+ void reportJointStatuses(NameStatusMap actualJointStatuses, bool aValueChanged);
+ };
+
+ interface KinematicUnitHandInterface extends KinematicUnitInterface
+ {
+ void open();
+ void close();
+ void resetTactileSensors(bool r);
+
+ void getObjectPoseInHandFrame();
+ void getHandPoseInObjectFrame();
+ };
+
+ interface KinematicUnitHandListener
+ {
+ void reportTactileSensorData(NameValueMap actualTactileSensorData);
+ void reportTactileSensorRawData(NameValueMap actualTactileSensorRawData);
+ void reportTactileSensorContactData(NameValueMap actualTactileSensorContactData);
+ };
+
+};
+
+#endif
diff --git a/interface/slice/units/PlatformUnitInterface.ice b/interface/slice/units/PlatformUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..9c207acc415e3f1b74d165b6df921e73f7f3caf5
--- /dev/null
+++ b/interface/slice/units/PlatformUnitInterface.ice
@@ -0,0 +1,50 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX::Core
+ * @author Manfred Kroehnert (manfred dot kroehnert at kit dot edu)
+ * @copyright 2013 Manfred Kroehnert
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_UNITS_PLATFORMUNIT_SLICE_
+#define _ARMARX_CORE_UNITS_PLATFORMUNIT_SLICE_
+
+#include <units/UnitInterface.ice>
+#include <core/UserException.ice>
+#include <core/BasicTypes.ice>
+
+module armarx
+{
+ interface PlatformUnitInterface extends SensorActorUnitInterface
+ {
+ void moveTo(float targetPlatformPositionX, float targetPlatformPositionY, float targetPlatformRotation, float positionalAccuracy, float orientationalAccuracy);
+ void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation);
+ void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy);
+ void setMaxVelocities(float positionalVelocity, float orientationalVelocity);
+ };
+
+ interface PlatformUnitListener
+ {
+ void reportPlatformPose(float currentPlatformPositionX, float currentPlatformPositionY, float currentPlatformRotation);
+ void reportNewTargetPose(float newPlatformPositionX, float newPlatformPositionY, float newPlatformRotation);
+ void reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation);
+ };
+
+};
+
+#endif
diff --git a/interface/slice/units/TCPMoverUnitInterface.ice b/interface/slice/units/TCPMoverUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..dbe8e4d230d48ed453745e426750a2c3552de24b
--- /dev/null
+++ b/interface/slice/units/TCPMoverUnitInterface.ice
@@ -0,0 +1,112 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX::Core
+ * @author Christian Boege (boege at kit dot edu)
+ * @copyright 2011 Christian Boege
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_TCPMOVERUNIT_SLICE_
+#define _ARMARX_CORE_TCPMOVERUNIT_SLICE_
+
+#include <core/UserException.ice>
+#include <units/KinematicUnitInterface.ice>
+
+module armarx
+{
+ enum HandSelection{
+ eNone,
+ eLeftHand,
+ eRightHand
+ };
+
+ interface TCPMoverUnitInterface
+ {
+// void moveTCPRelative(bool leftHand, float x, float y, float z, float speedFactor);
+ /**
+ * Resets the joint angles of the arm to the home position.
+ * NO MOVEMENT! Just sets the angles, so not suitable for a real robot.
+ * @param selectedHand Select which hand to move to home position.
+ */
+ void resetArmToHomePosition(HandSelection selectedHand);
+ /**
+ * Request control over the robot.
+ */
+ void request();
+
+ /**
+ * Release control over the robot.
+ */
+ void release();
+
+ /**
+ * Set the velocities of one hand.
+ * The velocity values should be currently between 0-30.
+ * @param selectedHand Select which hand to move.
+ * @param x The velocity on the x-axis.
+ * @param y The velocity on the y-axis.
+ * @param z The velocity on the z-axis.
+ * @param speedFactor A factor by which the x,y,z values should be multiplied.
+ */
+ void setCartesianTCPVelocity(HandSelection selectedHand, float x, float y, float z, float speedFactor);
+ //! angles in radiant
+
+ /**
+ * Sets the orientation of one hand.<br/>
+ * All angles are in radiant.
+ * @param selectedHand Select which hand to move.
+ * @param alpha
+ * @param beta
+ * @param gamma
+ *
+ */
+ void setTCPOrientation(HandSelection selectedHand, float alpha, float beta, float gamma);
+
+ /**
+ * Let the robot look to a specific point in space. Coordinates in platform coordinates.<br/>
+ * Not yet implemented.
+ */
+ void lookWithHeadTo(float x, float y, float z);
+
+ /**
+ * Not yet implemented.
+ * @param x
+ * @param y
+ * @param theta
+ */
+ void setPlatformVelocities(float x, float y, float theta);
+
+ /**
+ * @brief setHandConfiguration
+ * @param configValue eOPEN = 1,
+ ePRESHAPE = 2,
+ eGRASP = 4,
+ eTHREE_FINGER_GRASP = 5,
+ eMOVE_THUMB = 7,
+ ePRESHAPE_FINGERS = 8,
+ eCLOSE_FINGERS = 9
+ */
+ void setHandConfiguration(HandSelection selectedHand, int configValue);
+
+ void stopArm(HandSelection selectedHand);
+ void stopHead();
+ void stopPlatform();
+ };
+};
+
+#endif
diff --git a/interface/slice/units/UnitInterface.ice b/interface/slice/units/UnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..f46941fa09fd0da76636a7682a9c6857e145ff84
--- /dev/null
+++ b/interface/slice/units/UnitInterface.ice
@@ -0,0 +1,67 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Kai Welke (welke at kit dot edu)
+* @copyright 2011 Humanoids Group, HIS, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_UNITS_UNITINTERFACE_SLICE_
+#define _ARMARX_CORE_UNITS_UNITINTERFACE_SLICE_
+
+#include <core/UserException.ice>
+
+module armarx
+{
+ exception ResourceUnavailableException extends UserException
+ {
+ };
+
+ exception ResourceNotOwnedException extends UserException
+ {
+ };
+
+ enum UnitExecutionState
+ {
+ eUnitConstructed,
+ eUnitInitialized,
+ eUnitStarted,
+ eUnitStopped
+ };
+
+ interface UnitExecutionManagementInterface
+ {
+ void init();
+ void start();
+ void stop();
+
+ UnitExecutionState getExecutionState();
+ };
+
+ interface UnitResourceManagementInterface
+ {
+ void request() throws ResourceUnavailableException;
+ void release() throws ResourceNotOwnedException;
+ };
+
+ interface SensorActorUnitInterface extends UnitExecutionManagementInterface, UnitResourceManagementInterface
+ {
+ };
+};
+
+#endif
diff --git a/scenarios/RemoteRobotTest/RemoteRobotTestProject/CMakeLists.txt b/scenarios/RemoteRobotTest/RemoteRobotTestProject/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7a759e12aa690ee77337ce5ff94c615314fbee26
--- /dev/null
+++ b/scenarios/RemoteRobotTest/RemoteRobotTestProject/CMakeLists.txt
@@ -0,0 +1,62 @@
+# RemoteRobotTestProject project
+
+cmake_minimum_required(VERSION 2.8)
+if (COMMAND cmake_policy)
+ if (POLICY CMP0011)
+ cmake_policy(SET CMP0011 NEW)
+ endif()
+endif()
+
+find_package("ArmarXCore" REQUIRED
+ PATHS "$ENV{HOME}/armarx/Core/build/cmake"
+ "$ENV{HOME}/Projects/armarx-8.04/Core/build/cmake"
+ "$ENV{HOME}/armarx-install/share/cmake/ArmarXCore"
+ "/org/share/archive/SFB588_RefDist/armarx/share/cmake/ArmarXCore"
+)
+
+
+include(${ArmarXCore_CMAKE_DIR}/ArmarXProject.cmake)
+
+armarx_project("RemoteRobotTestProject")
+
+#add_subdirectory(interface)
+
+
+#########################################################################
+#
+# start of RemoteRobotTestProject content
+#
+#########################################################################
+
+armarx_component_set_name(RemoteRobotTestProject)
+
+set(COMPONENT_BUILD TRUE)
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_component_if(COMPONENT_BUILD "component disabled")
+armarx_component_if(Eigen3_FOUND "Eigen3 not available")
+armarx_component_if(Simox_FOUND "VirtualRobot not available")
+
+
+if (ARMARX_BUILD)
+
+ include_directories(${ArmarXCore_INCLUDE_DIRS} )
+ include_directories(${Eigen3_INCLUDE_DIR})
+ include_directories(${Simox_INCLUDE_DIR})
+
+ set(COMPONENT_LIBS RemoteRobotTestProjectInterfaces ArmarXInterfaces ArmarXCoreRemoteRobot ArmarXCore ${Simox_LIBRARIES} ${Coin3D_LIBRARIES})
+
+ set(SOURCES RemoteRobotTestProject.cpp
+ RemoteRobotTestProject.h
+ RemoteRobotTestProjectApp.h)
+
+ armarx_add_component_executable("${SOURCES}")
+
+ armarx_add_component_test(RemoteRobotTestProjectTest test/RemoteRobotTestProjectTest.cpp)
+endif()
+
+#########################################################################
+
+install_project()
diff --git a/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProject.cpp b/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProject.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ccebd56e79d35980bf79ba9993d0520714fcc540
--- /dev/null
+++ b/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProject.cpp
@@ -0,0 +1,109 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RemoteRobotTestProject::
+* @author ( at kit dot edu)
+* @date
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#include "RemoteRobotTestProject.h"
+
+#include <Core/robotstate/RobotStateObjectFactories.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/LinkedCoordinate.h>
+
+namespace armarx
+{
+ void RemoteRobotTestProject::onInitComponent()
+ {
+ std::cout << "onInitComponent" << std::endl;
+
+
+ usingProxy("RobotStateComponent");
+
+
+
+
+ // onInitComponent has to do the following tasks
+ // 1) Make sure the component is in a consistent state (initialize all members).
+ // After onInitComponent, the Ice object adapter is activated and the component
+ // is accessible via Ice. Therefore, the component needs to be in a sane state.
+ // 2) Make Ice dependencies explicit
+ // All proxies and topics used by this component need to be made explicit. Further
+ // all topics that are provided by the component need to be made explicit.
+ // Use the following calls:
+ // usingProxy("proxyName"): this component used the proxy "proxyName"
+ // usingTopic("topciName"): this component subscribes to the topic "topicName"
+ // offeringTopic("topciName"): this component provides the topic "topicName"
+ // The initialization procedure assures that all required proxies and topics are
+ // available before the component is started.
+
+ }
+ void test2(VirtualRobot::RobotPtr robot){
+ std::cout << "!!!!!!!!!!!Robot: " << robot->getName() << std::endl;
+ }
+
+ void test(VirtualRobot::RobotWeakPtr robot){
+ //std::cout << "!!!!!!Robot: " << robot->getName() << std::endl;
+ test2(robot.lock());
+ }
+
+
+ /* Comment for gdb:
+ * start with: startApplication -> debugApplication (w/o `&' at the end)
+ * gdb $> b armarx::RemoteRobotTestProject::onStartComponent()
+ * gdb $> b RobotNodeSet.cpp:46
+ * gdb $> b RemoteRobot.cpp:48
+ */
+ void RemoteRobotTestProject::onStartComponent()
+ {
+ std::cout << "onStartComponent" << std::endl;
+ RobotStateObjectFactories::addFactories(getIceManager()->ic());
+
+ this->server = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
+ this->robot.reset(new RemoteRobot(server->getSynchronizedRobot()));
+
+ // method removed
+ //test(boost::dynamic_pointer_cast<RemoteRobot>(robot)->getRobotPtr());
+
+ VirtualRobot::RobotNodeSetPtr set = this->robot->getRobotNodeSet("LeftArm");
+
+ VirtualRobot::RobotNodePtr robotNodePtr = this->robot->getRobotNode("Wrist 2 L");
+
+ VirtualRobot::LinkedCoordinate c(this->robot);
+ c.set(robotNodePtr);
+ c.changeFrame("Shoulder 1 L");
+
+ std::cout << "Left tcp in right tcp frame: " << c.getPosition() << std::endl;
+
+
+ ARMARX_LOG << eINFO << "Starting RemoteRobotTestProject" << flush;
+
+ // onStartComponent is called once after the initialization procedure finished and the Ice object adapter is activated.
+ // common tasks are:
+ // 1) retrieve used topic:
+ // MyTopicPrx topicProxy = getTopic<MyTopicPrx>("topicName");
+ // 2) retrieve used proxy:
+ // MyPrx proxy = getProxy<MyPrx>("proxyName");
+
+ }
+}
diff --git a/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProject.h b/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProject.h
new file mode 100644
index 0000000000000000000000000000000000000000..05fe0c172c0ad0f8c78894e86f4d212f6c4a5edd
--- /dev/null
+++ b/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProject.h
@@ -0,0 +1,53 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RemoteRobotTestProject::
+* @author ( at kit dot edu)
+* @date
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_COMPONENT_RemoteRobotTestProject_RemoteRobotTestProject_H
+#define ARMARX_COMPONENT_RemoteRobotTestProject_RemoteRobotTestProject_H
+
+#include <Core/core/Component.h>
+#include <Core/core/ImportExportComponent.h>
+//#include <interface/cpp/RemoteRobotTestProject/RemoteRobotTestProject.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <Core/robotstate/remote/RemoteRobot.h>
+
+namespace armarx
+{
+ class ARMARXCOMPONENT_IMPORT_EXPORT RemoteRobotTestProject :
+ virtual public Component
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() { return "RemoteRobotTestProject"; };
+ virtual void onInitComponent();
+ virtual void onStartComponent();
+
+ private:
+ std::string robotNodeSetName;
+ RobotStateComponentInterfacePrx server;
+ VirtualRobot::RobotPtr robot ;
+ };
+
+}
+
+#endif
diff --git a/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProjectApp.h b/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProjectApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..bbcb9cb5cd8ab5ae1fbbd34af6096cc48cbd4f00
--- /dev/null
+++ b/scenarios/RemoteRobotTest/RemoteRobotTestProject/RemoteRobotTestProjectApp.h
@@ -0,0 +1,39 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RemoteRobotTestProject::
+* @author ( at kit dot edu)
+* @date
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+
+#include "RemoteRobotTestProject.h"
+
+#include <Core/core/Application.h>
+
+namespace armarx
+{
+ class RemoteRobotTestProjectApp :
+ virtual public armarx::Application
+ {
+ void setup()
+ {
+ addComponent<RemoteRobotTestProject>();
+ }
+ };
+};
diff --git a/scenarios/RemoteRobotTest/RemoteRobotTestProject/build/.gitkeep b/scenarios/RemoteRobotTest/RemoteRobotTestProject/build/.gitkeep
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/scenarios/RemoteRobotTest/RemoteRobotTestProject/test/RemoteRobotTestProjectTest.cpp b/scenarios/RemoteRobotTest/RemoteRobotTestProject/test/RemoteRobotTestProjectTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f1f617d0a94254c4ce6ca6fb2596c2742e74df1d
--- /dev/null
+++ b/scenarios/RemoteRobotTest/RemoteRobotTestProject/test/RemoteRobotTestProjectTest.cpp
@@ -0,0 +1,37 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RemoteRobotTestProject::
+* @author (at kit dot edu)
+* @date
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#define BOOST_TEST_MODULE ArmarX::Component::RemoteRobotTestProject
+#define ARMARX_BOOST_TEST
+#include <Core/Test.h>
+
+#include <iostream>
+#include "../RemoteRobotTestProject.h"
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::RemoteRobotTestProject instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
+
diff --git a/scenarios/RemoteRobotTest/configs/ArmarXGui.cfg b/scenarios/RemoteRobotTest/configs/ArmarXGui.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..fbb6a74a2efd9036a234cda00d1221884bc485c1
--- /dev/null
+++ b/scenarios/RemoteRobotTest/configs/ArmarXGui.cfg
@@ -0,0 +1,3 @@
+#ArmarX.ArmarXGui.LoadPlugins=${ArmarXHome_DIR}/Gui/build/lib/libKinematicUnitGuiPlugin.so
+ArmarX.ArmarXGui.LoadPlugins=${ArmarXHome_DIR}/Gui/build/lib/libObserverPropertiesGuiPlugin.so
+ArmarX.ArmarXGui.Show3DViewer=no
diff --git a/scenarios/RemoteRobotTest/configs/ConditionHandler.cfg b/scenarios/RemoteRobotTest/configs/ConditionHandler.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0c18d43f83d31fe758f58d0eb9ded3f2bb5cc52d
--- /dev/null
+++ b/scenarios/RemoteRobotTest/configs/ConditionHandler.cfg
@@ -0,0 +1 @@
+ArmarX.ConditionHandler.Observers=HeadKinematicUnitObserver,SystemObserver
diff --git a/scenarios/RemoteRobotTest/configs/DummyObjectRecognitionConfig.txt b/scenarios/RemoteRobotTest/configs/DummyObjectRecognitionConfig.txt
new file mode 100644
index 0000000000000000000000000000000000000000..927e3cd49603929597972049b59d80e85a4e0ebc
--- /dev/null
+++ b/scenarios/RemoteRobotTest/configs/DummyObjectRecognitionConfig.txt
@@ -0,0 +1,11 @@
+VisionX.DummyObjectRecognition.Object.Name = Amicelli
+VisionX.DummyObjectRecognition.Object.Pose.x = 1.0
+VisionX.DummyObjectRecognition.Object.Pose.y = 2.0
+VisionX.DummyObjectRecognition.Object.Pose.z = 3.0
+VisionX.DummyObjectRecognition.Object.Pose.qw = 4.0
+VisionX.DummyObjectRecognition.Object.Pose.qx = 5.0
+VisionX.DummyObjectRecognition.Object.Pose.qy = 6.0
+VisionX.DummyObjectRecognition.Object.Pose.qz = 7.0
+VisionX.DummyObjectRecognition.Object.RecognitionCertainty = 0.9
+VisionX.DummyObjectRecognition.LocalizationDelay = 100
+
diff --git a/scenarios/RemoteRobotTest/configs/HeadUnit.cfg b/scenarios/RemoteRobotTest/configs/HeadUnit.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4d5461f65d9b3b5c262485c291c219e0684c0987
--- /dev/null
+++ b/scenarios/RemoteRobotTest/configs/HeadUnit.cfg
@@ -0,0 +1,5 @@
+# test config file for KinematicUnit configured to be a Head
+
+ArmarX.KinematicUnitSimulation.RobotFileName=${ArmarXHome_DIR}/Armar3/data/robotmodel/ArmarIII-Head.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
+ArmarX.KinematicUnitSimulation.RobotNodeSetName=Head # node set name
+ArmarX.KinematicUnitSimulation.AdapterName=HeadKinematicUnit # name of the Ice adapter
diff --git a/scenarios/RemoteRobotTest/configs/HeadUnitObserver.cfg b/scenarios/RemoteRobotTest/configs/HeadUnitObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..39353728c0e190eb24f1e6733daef3c35002c45e
--- /dev/null
+++ b/scenarios/RemoteRobotTest/configs/HeadUnitObserver.cfg
@@ -0,0 +1,5 @@
+# test config file for KinematicUnit configured to be a Head
+
+ArmarX.KinematicUnitObserver.RobotFileName=${ArmarXHome_DIR}/Armar3/data/robotmodel/ArmarIII-Head.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
+ArmarX.KinematicUnitObserver.RobotNodeSetName=Head # node set name
+ArmarX.KinematicUnitObserver.AdapterName=HeadKinematicUnitObserver # name of the Ice adapter
diff --git a/scenarios/RemoteRobotTest/configs/ObjectMemoryConfig.txt b/scenarios/RemoteRobotTest/configs/ObjectMemoryConfig.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9f7a783128a6cf6aeaee595c74bffc3603044041
--- /dev/null
+++ b/scenarios/RemoteRobotTest/configs/ObjectMemoryConfig.txt
@@ -0,0 +1,3 @@
+VisionX.ObjectMemory.ObjectDatabaseFile = /home/staff/schieben/armarx/data/Objects/ObjectDB.txt
+VisionX.ObjectMemory.UpdateCycleTimeInMs = 10
+
diff --git a/scenarios/RemoteRobotTest/configs/ObjectMemoryObserverConfig.txt b/scenarios/RemoteRobotTest/configs/ObjectMemoryObserverConfig.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6f35289d21f6825a967d80a2a258d7b04c4fa08c
--- /dev/null
+++ b/scenarios/RemoteRobotTest/configs/ObjectMemoryObserverConfig.txt
@@ -0,0 +1 @@
+VisionX.ObjectMemoryObserver.ObjectDatabaseFile = /home/staff/schieben/armarx/data/Objects/ObjectDB.txt
diff --git a/scenarios/RemoteRobotTest/configs/RobotStateComponent.cfg b/scenarios/RemoteRobotTest/configs/RobotStateComponent.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..46260981c2de37a0dc21938875fdcf57797d5778
--- /dev/null
+++ b/scenarios/RemoteRobotTest/configs/RobotStateComponent.cfg
@@ -0,0 +1,5 @@
+# test config file for KinematicUnit configured to be a Head
+
+ArmarX.RobotStateComponent.RobotFileName=${ArmarXHome_DIR}/Armar3/data/robotmodel/ArmarIII-LeftArm.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
+ArmarX.RobotStateComponent.RobotNodeSetName=LeftArm # node set name
+ArmarX.RobotStateComponent.AdapterName=RobotStateComponent # name of the Ice adapter
diff --git a/scenarios/TCPMoverScenario/configs/ArmarXGui.cfg b/scenarios/TCPMoverScenario/configs/ArmarXGui.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..510feb9a8f61aa560bfa61d31a2e34f9f8e72303
--- /dev/null
+++ b/scenarios/TCPMoverScenario/configs/ArmarXGui.cfg
@@ -0,0 +1,12 @@
+ArmarX.LoadPlugins=${ArmarXHome_DIR}/Gui/build/lib/libMdiGuiPlugin.so,${ArmarXHome_DIR}//Gui/build/lib/libKinematicUnitGuiPlugin.so
+ArmarX.Show3DViewer=yes
+ArmarX.GlobalMinimumLoggingLevel=INFO
+ArmarX.DisableLogging=False
+
+
+ArmarX.KinematicUnitGuiPlugin.MinimumLoggingLevel=Info
+ArmarX.KinematicUnitGuiPlugin.RobotFileName=${ArmarXHome_DIR}/Armar4/data/robotmodel/ArmarIV.xml
+ArmarX.KinematicUnitGuiPlugin.RobotNodeSetName=Robot
+ArmarX.KinematicUnitGuiPlugin.KinematicUnitName=RobotKinematicUnit
+#ArmarX.MdiPlugin.Profile=TCPMover
+Armarx.ArmarXTCPMover.TCPMoverUnitName=ArmarXStatechart
diff --git a/scenarios/TCPMoverScenario/configs/ConditionHandler.cfg b/scenarios/TCPMoverScenario/configs/ConditionHandler.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..6d2073641857dc0c384872b03dcfd6fdd49b1675
--- /dev/null
+++ b/scenarios/TCPMoverScenario/configs/ConditionHandler.cfg
@@ -0,0 +1 @@
+ArmarX.ConditionHandler.Observers=RobotKinematicUnitObserver,SystemObserver
diff --git a/scenarios/TCPMoverScenario/configs/Gui.armarxgui b/scenarios/TCPMoverScenario/configs/Gui.armarxgui
new file mode 100644
index 0000000000000000000000000000000000000000..32ff7093b9f934abbb176cf72a7e69c27abd58bf
--- /dev/null
+++ b/scenarios/TCPMoverScenario/configs/Gui.armarxgui
@@ -0,0 +1,25 @@
+[General]
+loadedLibs=Gui/build/lib/libConditionViewerGuiPlugin.so, Gui/build/lib/libHandUnitGuiPlugin.so, Gui/build/lib/libHardwareGuiPlugin.so, Gui/build/lib/libKinematicUnitGuiPlugin.so, Gui/build/lib/libLoggingGuiPlugin.so, Gui/build/lib/libObjectExaminerGuiPlugin.so, Gui/build/lib/libObserverPropertiesGuiPlugin.so, Gui/build/lib/libPlatformUnitGuiPlugin.so, Gui/build/lib/libSensorActorWidgetsGuiPlugin.so, Gui/build/lib/libStateChartGuiPlugin.so, Gui/build/lib/libSystemStateMonitorGuiPlugin.so
+MainWindowGeometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\x18\0\0\x6\xde\0\0\x4s\0\0\0\x1\0\0\0\x34\0\0\x6\xdd\0\0\x4r\0\0\0\0\0\0)
+DockWidgetsState="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x1\0\0\0\x1\0\0\x6\xdd\0\0\x4\x10\xfc\x2\0\0\0\x1\xfc\0\0\0\x19\0\0\x4\x10\0\0\x1^\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfc\0\0\0\0\0\0\x2\x98\0\0\x2W\0\xff\xff\xff\xfa\0\0\0\x1\x1\0\0\0\x2\xfb\0\0\0,\0\x44\0o\0\x63\0k\0S\0y\0s\0t\0\x65\0m\0S\0t\0\x61\0t\0\x65\0M\0o\0n\0i\0t\0o\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\xd3\0\xff\xff\xff\xfb\0\0\0(\0\x44\0o\0\x63\0k\0K\0i\0n\0\x65\0m\0\x61\0t\0i\0\x63\0U\0n\0i\0t\0G\0U\0I\x1\0\0\0\0\xff\xff\xff\xff\0\0\x2W\0\xff\xff\xff\xfb\0\0\0\x1a\0\x44\0o\0\x63\0k\0\x33\0\x44\0 \0V\0i\0\x65\0w\0\x65\0r\x1\0\0\x2\x9e\0\0\x4?\0\0\0\xc7\0\xff\xff\xff\0\0\0\0\0\0\x4\x10\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\0)"
+WidgetCustomNames=3D Viewer, KinematicUnitGUI, SystemStateMonitor
+
+[3D%20Viewer]
+WidgetBaseName=3D Viewer
+widgetWidth=1087
+widgetHeight=1020
+
+[KinematicUnitGUI]
+WidgetBaseName=KinematicUnitGUI
+widgetWidth=664
+widgetHeight=990
+kinematicUnitName=KinematicUnitSimulation
+robotNodeSetName=Robot
+kinematicUnitFile=Armar4/data/robotmodel/ArmarIV.xml
+
+[SystemStateMonitor]
+WidgetBaseName=SystemStateMonitor
+widgetWidth=796
+widgetHeight=564
+ManagerRepository=@Invalid()
+MonitoredManagers=ArmarXGuiManager, ConditionHandlerManager, KinematicUnitObserverManager, KinematicUnitSimulationManager, SystemObserverManager
diff --git a/scenarios/TCPMoverScenario/configs/HeadUnit.cfg b/scenarios/TCPMoverScenario/configs/HeadUnit.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..fdc0834859cb4c1bab784ecd3d10eef2cf80d89e
--- /dev/null
+++ b/scenarios/TCPMoverScenario/configs/HeadUnit.cfg
@@ -0,0 +1,6 @@
+# test config file for KinematicUnit configured to be a Head
+
+ArmarX.KinematicUnitSimulation.RobotFileName=${ArmarXHome_DIR}/Armar4/data/robotmodel/ArmarIV.xml
+
+ArmarX.KinematicUnitSimulation.RobotNodeSetName=Robot # node set name vorher Head
+ArmarX.KinematicUnitSimulation.AdapterName=RobotKinematicUnit # name of the Ice adapter vorher HeadKinematicUnit
diff --git a/scenarios/TCPMoverScenario/configs/RobotUnit.cfg b/scenarios/TCPMoverScenario/configs/RobotUnit.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..570dc025f13903ade94afc6aa47addc3742e65e6
--- /dev/null
+++ b/scenarios/TCPMoverScenario/configs/RobotUnit.cfg
@@ -0,0 +1,8 @@
+# A test config file for KinematicUnit configured to be a Head
+
+ArmarX.KinematicUnitSimulation.RobotFileName=${ArmarXHome_DIR}/Armar4/data/robotmodel/ArmarIV.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
+
+ArmarX.KinematicUnitSimulation.RobotNodeSetName=Robot # node set name vorher Head
+ArmarX.KinematicUnitSimulation.AdapterName=RobotKinematicUnit # name of the Ice adapter vorher HeadKinematicUnit
+ArmarX.ExecutionTimeMs=1000
+
diff --git a/scenarios/TCPMoverScenario/configs/RobotUnitObserver.cfg b/scenarios/TCPMoverScenario/configs/RobotUnitObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..9b84969c59bb31c3c3026f641a655e42e7a52896
--- /dev/null
+++ b/scenarios/TCPMoverScenario/configs/RobotUnitObserver.cfg
@@ -0,0 +1,5 @@
+# test config file for KinematicUnit configured to be a Head
+
+ArmarX.KinematicUnitObserver.RobotFileName=${ArmarXHome_DIR}/Armar4/data/robotmodel/ArmarIV.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
+ArmarX.KinematicUnitObserver.RobotNodeSetName=Robot # node set name
+ArmarX.KinematicUnitObserver.AdapterName=RobotKinematicUnitObserver # name of the Ice adapter
diff --git a/source/RobotAPI/CMakeLists.txt b/source/RobotAPI/CMakeLists.txt
index 602631618f7001892a3d75cae0bf1cf744fd6373..928669e4b4bbbbf4dbbf12d6bbd79138aa6bfaa5 100644
--- a/source/RobotAPI/CMakeLists.txt
+++ b/source/RobotAPI/CMakeLists.txt
@@ -6,3 +6,6 @@ add_subdirectory(armarx-objects)
add_subdirectory(statecharts)
add_subdirectory(drivers)
+add_subdirectory(robotstate)
+add_subdirectory(robotstate/remote)
+add_subdirectory(operations)
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 712ae22258ea34af1d8e82845c7d763fb61de97e..343addf16d384b97802185c482262eaea61242d9 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -9,3 +9,14 @@ add_subdirectory(MovePlatformToLandmark)
add_subdirectory(WeissHapticSensorsUnit)
add_subdirectory(WeissHapticSensor)
add_subdirectory(HapticObserver)
+
+
+add_subdirectory(RobotControl)
+add_subdirectory(RobotControlUI)
+add_subdirectory(TCPMoverUnit)
+add_subdirectory(KinematicUnitObserver)
+add_subdirectory(KinematicUnitSimulation)
+add_subdirectory(PlatformUnitSimulation)
+add_subdirectory(PlatformUnitObserver)
+add_subdirectory(RobotStateComponent)
+add_subdirectory(RobotStateObserver)
diff --git a/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt b/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
index 449734e08065f0bade441eb2d435caf1eb2de911..fe1e3025a21324c21da24594ed522b7379c21022 100644
--- a/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
+++ b/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
@@ -8,7 +8,7 @@ find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot)
+set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers RobotAPIRemoteRobot)
set(SOURCES main.cpp ForceTorqueObserverApp.h)
diff --git a/source/RobotAPI/applications/HeadIKUnit/CMakeLists.txt b/source/RobotAPI/applications/HeadIKUnit/CMakeLists.txt
index 8b57c0a7eaec0b6c99a026a638e95f845ad82dd6..666c2f9c63467498eed3602cd781a4192f12a2b3 100644
--- a/source/RobotAPI/applications/HeadIKUnit/CMakeLists.txt
+++ b/source/RobotAPI/applications/HeadIKUnit/CMakeLists.txt
@@ -13,7 +13,8 @@ if (Eigen3_FOUND AND Simox_FOUND)
${Simox_INCLUDE_DIRS})
endif()
-set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot ${Simox_LIBRARIES})
+set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers RobotAPIRemoteRobot
+ ${Simox_LIBRARIES})
set(SOURCES main.cpp HeadIKUnitApp.h)
diff --git a/source/RobotAPI/applications/KinematicUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/KinematicUnitObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..38b60d712b06b977365a860105baa8d734625eeb
--- /dev/null
+++ b/source/RobotAPI/applications/KinematicUnitObserver/CMakeLists.txt
@@ -0,0 +1,14 @@
+
+armarx_component_set_name(KinematicUnitObserver)
+
+find_package(Simox 2.3.0 QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore #ArmarXCoreUnits
+ RobotAPIUnits
+ ArmarXCoreObservers)
+
+set(SOURCES main.cpp KinematicUnitObserverApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/KinematicUnitObserver/KinematicUnitObserverApp.h b/source/RobotAPI/applications/KinematicUnitObserver/KinematicUnitObserverApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..608359985fa485ad7b703b2d0ec941e492e2ed83
--- /dev/null
+++ b/source/RobotAPI/applications/KinematicUnitObserver/KinematicUnitObserverApp.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Christian Boege (boege dot at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/KinematicUnitObserver.h>
+
+namespace armarx
+{
+ /**
+ * Application for testing the armarx::KinematicUnitObserver
+ */
+ class KinematicUnitObserverApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<KinematicUnitObserver>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/KinematicUnitObserver/config/testProperty b/source/RobotAPI/applications/KinematicUnitObserver/config/testProperty
new file mode 100644
index 0000000000000000000000000000000000000000..fd3d73770dab1e8e52b2daef51b188df7b83d1da
--- /dev/null
+++ b/source/RobotAPI/applications/KinematicUnitObserver/config/testProperty
@@ -0,0 +1,7 @@
+# test config file for KinematicUnit configured to be a Head
+
+ArmarX.KinematicUnitObserver.RobotFileName=/org/share/home/staff/welke/projects/armarx/data/ArmarIII/ArmarIII-Head.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
+ArmarX.KinematicUnitObserver.RobotNodeSetName=Head # node set name
+ArmarX.KinematicUnitObserver.AdapterName=KinematicUnitObserver # name of the ICE adapter
+
+
diff --git a/source/RobotAPI/applications/KinematicUnitObserver/main.cpp b/source/RobotAPI/applications/KinematicUnitObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6caa89a542892ca94ff669dc94fc9034bb8c0dbf
--- /dev/null
+++ b/source/RobotAPI/applications/KinematicUnitObserver/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::KinematicUnitObserver
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "KinematicUnitObserverApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::KinematicUnitObserverApp>();
+ app->setName("KinematicUnitObserver");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/KinematicUnitSimulation/CMakeLists.txt b/source/RobotAPI/applications/KinematicUnitSimulation/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..89f9c9c8fb1d687abf6f19244d9e8c163433780a
--- /dev/null
+++ b/source/RobotAPI/applications/KinematicUnitSimulation/CMakeLists.txt
@@ -0,0 +1,20 @@
+
+armarx_component_set_name(KinematicUnitSimulation)
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+find_package(Simox 2.3.0 QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+
+include_directories(${Eigen3_INCLUDE_DIR})
+include_directories(${Simox_INCLUDE_DIRS})
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore #ArmarXCoreUnits
+ RobotAPIUnits
+ ${Simox_LIBRARIES})
+
+set(SOURCES main.cpp KinematicUnitSimulationApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/KinematicUnitSimulation/KinematicUnitSimulationApp.h b/source/RobotAPI/applications/KinematicUnitSimulation/KinematicUnitSimulationApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..f9d41bb16c93c29a3f8f76a6b1b9066dfa6785da
--- /dev/null
+++ b/source/RobotAPI/applications/KinematicUnitSimulation/KinematicUnitSimulationApp.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Christian Boege (boege at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/KinematicUnitSimulation.h>
+
+namespace armarx
+{
+ /**
+ * Application for testing the armarx::KinematicUnitSimulation
+ */
+ class KinematicUnitSimulationApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<KinematicUnitSimulation>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/KinematicUnitSimulation/config/testProperty b/source/RobotAPI/applications/KinematicUnitSimulation/config/testProperty
new file mode 100644
index 0000000000000000000000000000000000000000..f5a03878511ff258740126bb8b4b916e603cd261
--- /dev/null
+++ b/source/RobotAPI/applications/KinematicUnitSimulation/config/testProperty
@@ -0,0 +1,5 @@
+# test config file for KinematicUnit configured to be a Head
+
+ArmarX.KinematicUnitSimulation.RobotFileName=${ArmarXHome_DIR}/Armar3/data/robotmodel/ArmarIII-Head.xml
+ArmarX.KinematicUnitSimulation.RobotNodeSetName=Head # node set name
+ArmarX.KinematicUnitSimulation.AdapterName=HeadKinematicUnitInstructionChannel # name of the Ice adapter
diff --git a/source/RobotAPI/applications/KinematicUnitSimulation/main.cpp b/source/RobotAPI/applications/KinematicUnitSimulation/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..14648392913909c1fe51dc553e4e05938278251c
--- /dev/null
+++ b/source/RobotAPI/applications/KinematicUnitSimulation/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::KinematicUnitSimulation
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "KinematicUnitSimulationApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::KinematicUnitSimulationApp>();
+ app->setName("KinematicUnitSimulation");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/MotionControl/CMakeLists.txt b/source/RobotAPI/applications/MotionControl/CMakeLists.txt
index 65c7ee082dbf861f5c0a489e1999ead63da481cd..ae286cd4687e124e389649e5d2133718fb6f9207 100644
--- a/source/RobotAPI/applications/MotionControl/CMakeLists.txt
+++ b/source/RobotAPI/applications/MotionControl/CMakeLists.txt
@@ -8,7 +8,7 @@ find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-set(COMPONENT_LIBS MotionControl RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations)
+set(COMPONENT_LIBS MotionControl RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers RobotAPIRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations)
set(SOURCES main.cpp MotionControlApp.h)
diff --git a/source/RobotAPI/applications/MotionControlTest/CMakeLists.txt b/source/RobotAPI/applications/MotionControlTest/CMakeLists.txt
index 1d9334a529d21a2724aa4fbf24895c81d5efe76d..e608b5cd8192928f3f03f087ad66a842cdb06821 100644
--- a/source/RobotAPI/applications/MotionControlTest/CMakeLists.txt
+++ b/source/RobotAPI/applications/MotionControlTest/CMakeLists.txt
@@ -13,7 +13,7 @@ if (Eigen3_FOUND AND Simox_FOUND)
${Simox_INCLUDE_DIRS})
endif()
-set(COMPONENT_LIBS MotionControl RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations ${Simox_LIBRARIES})
+set(COMPONENT_LIBS MotionControl RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers RobotAPIRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations ${Simox_LIBRARIES})
set(SOURCES main.cpp MotionControlTestApp.h)
diff --git a/source/RobotAPI/applications/MovePlatform/CMakeLists.txt b/source/RobotAPI/applications/MovePlatform/CMakeLists.txt
index 1f767fbf93c4ecabd098a863fdd4582b62356c1e..0af49ec7c21cc1eedd06f970e509de59b31013fb 100644
--- a/source/RobotAPI/applications/MovePlatform/CMakeLists.txt
+++ b/source/RobotAPI/applications/MovePlatform/CMakeLists.txt
@@ -18,7 +18,7 @@ set(COMPONENT_LIBS
ArmarXCoreStatechart
ArmarXCoreObservers
ArmarXCoreOperations
- ArmarXCoreRemoteRobot
+ RobotAPIRemoteRobot
#MemoryXInterfaces
#MemoryXCore
#MemoryXMemoryTypes
diff --git a/source/RobotAPI/applications/MovePlatformToLandmark/CMakeLists.txt b/source/RobotAPI/applications/MovePlatformToLandmark/CMakeLists.txt
index 187ae9481a1e679f0b52ec30e92fd949fec3ccab..fa87a997919866e0012a3481f11ab32bc19afb79 100644
--- a/source/RobotAPI/applications/MovePlatformToLandmark/CMakeLists.txt
+++ b/source/RobotAPI/applications/MovePlatformToLandmark/CMakeLists.txt
@@ -19,7 +19,7 @@ set(COMPONENT_LIBS
ArmarXCoreStatechart
ArmarXCoreObservers
ArmarXCoreOperations
- ArmarXCoreRemoteRobot
+ RobotAPIRemoteRobot
#MemoryXInterfaces
#MemoryXCore
#MemoryXMemoryTypes
diff --git a/source/RobotAPI/applications/PlatformUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/PlatformUnitObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..13dbfdf627791c08a9c98c07ec3d853bb7b533d9
--- /dev/null
+++ b/source/RobotAPI/applications/PlatformUnitObserver/CMakeLists.txt
@@ -0,0 +1,14 @@
+
+armarx_component_set_name(PlatformUnitObserver)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox VirtualRobot not found")
+
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers #ArmarXCoreUnits
+ RobotAPIUnits
+ ${Simox_LIBRARIES})
+
+set(SOURCES main.cpp PlatformUnitObserverApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/PlatformUnitObserver/PlatformUnitObserverApp.h b/source/RobotAPI/applications/PlatformUnitObserver/PlatformUnitObserverApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..786ea4c924fb71172e7b86cc50402c9a20f23c7d
--- /dev/null
+++ b/source/RobotAPI/applications/PlatformUnitObserver/PlatformUnitObserverApp.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Manfred Kroehnert (manfred dot kroehnert at kit dot edu)
+ * @date 2013
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/PlatformUnitObserver.h>
+
+namespace armarx
+{
+ /**
+ * Application for executing armarx::PlatformUnitObserver
+ */
+ class PlatformUnitObserverApp :
+ public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<PlatformUnitObserver>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/PlatformUnitObserver/main.cpp b/source/RobotAPI/applications/PlatformUnitObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d8c784b40dc36d5d5efc51150230d70d807170ba
--- /dev/null
+++ b/source/RobotAPI/applications/PlatformUnitObserver/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::PlatformUnitObserver
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "PlatformUnitObserverApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::PlatformUnitObserverApp>();
+ app->setName("PlatformUnitObserver");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/PlatformUnitSimulation/CMakeLists.txt b/source/RobotAPI/applications/PlatformUnitSimulation/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bb60f53c08754be1ca3ac91d8aee93db959ba1ea
--- /dev/null
+++ b/source/RobotAPI/applications/PlatformUnitSimulation/CMakeLists.txt
@@ -0,0 +1,13 @@
+
+armarx_component_set_name(PlatformUnitSimulation)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore #ArmarXCoreUnits
+ RobotAPIUnits)
+
+set(SOURCES main.cpp PlatformUnitSimulationApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/PlatformUnitSimulation/PlatformUnitSimulationApp.h b/source/RobotAPI/applications/PlatformUnitSimulation/PlatformUnitSimulationApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..35c7d14d438da8e10af571c14f0fedded868b9dc
--- /dev/null
+++ b/source/RobotAPI/applications/PlatformUnitSimulation/PlatformUnitSimulationApp.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author tobias haass
+ * @date 2013
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/PlatformUnitSimulation.h>
+
+namespace armarx
+{
+ /**
+ * Application for testing the armarx::PlatformUnitSimulation
+ */
+ class PlatformUnitSimulationApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<PlatformUnitSimulation>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/PlatformUnitSimulation/main.cpp b/source/RobotAPI/applications/PlatformUnitSimulation/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0f0d8d799942effde9f5208bcd0d1ca89cbd1ee
--- /dev/null
+++ b/source/RobotAPI/applications/PlatformUnitSimulation/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::PlatformUnitSimulation
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "PlatformUnitSimulationApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::PlatformUnitSimulationApp>();
+ app->setName("PlatformUnitSimulation");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/RobotControl/CMakeLists.txt b/source/RobotAPI/applications/RobotControl/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1041e0aaebf39c9ba8ec224ea9c18539190d22f7
--- /dev/null
+++ b/source/RobotAPI/applications/RobotControl/CMakeLists.txt
@@ -0,0 +1,12 @@
+
+armarx_component_set_name(RobotControl)
+
+#find_package(Simox QUIET)
+#armarx_build_if(Simox_FOUND 2.3.0 "Simox-VirtualRobot not available")
+
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers RobotAPIInterfaces RobotAPIRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations RobotAPIUnits RobotAPIRemoteRobot)
+
+set(SOURCES main.cpp RobotControlApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/RobotControl/RobotControlApp.h b/source/RobotAPI/applications/RobotControl/RobotControlApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..4fda633b799936d686aca189fd185f622e9fce4a
--- /dev/null
+++ b/source/RobotAPI/applications/RobotControl/RobotControlApp.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Kai Welke (weöle dot at kit dot edu)
+ * @date 2012
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/operations/RobotControl.h>
+
+namespace armarx
+{
+ /**
+ * Application for testing armarx::RobotControl
+ */
+ class RobotControlApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<RobotControl>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/RobotControl/main.cpp b/source/RobotAPI/applications/RobotControl/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b42109ee66362d378a75014a5d4da3802577101c
--- /dev/null
+++ b/source/RobotAPI/applications/RobotControl/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::RobotControl
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "RobotControlApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::RobotControlApp>();
+ app->setName("RobotControl");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..943dba547a816236b6c9f69f7941bb4a3fc6758f
--- /dev/null
+++ b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
@@ -0,0 +1,18 @@
+
+armarx_component_set_name(RobotControlUI)
+
+# Removed dependency on Simox 2014-08-04
+#find_package(Simox 2.3.0 QUIET)
+#armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore #ArmarXCoreUnits
+ ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreOperations)
+
+set(SOURCES main.cpp
+ RobotControlUI.cpp
+ RobotControlUI.h
+ RobotControlUIApp.h
+ )
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7862de8c1e1f915800f9c81443e5e6ae1aa66f5a
--- /dev/null
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
@@ -0,0 +1,73 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public LIcense as
+ * published by the Free Software Foundation; either version 2 of
+ * the LIcense, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public LIcense for more details.
+ *
+ * You should have received a copy of the GNU General Public LIcense
+ * along with this program. If not, see <http://www.gnu.org/lIcenses/>.
+ *
+ * @package ArmarX::Applications
+ * @author Jan Issac (jan dot issac at gmx dot net)
+ * @date 2011
+ * @copyright http://www.gnu.org/lIcenses/gpl-2.0.txt
+ * GNU General Public LIcense
+ */
+
+#include "RobotControlUI.h"
+#include <Core/core/application/Application.h>
+
+#include <iostream>
+#include <Core/observers/ObserverObjectFactories.h>
+
+using namespace armarx;
+using namespace std;
+
+void RobotControlUI::onInitComponent()
+{
+ usingProxy("RobotControl");
+ stateId = -1;
+ controlTask = new RunningTask<RobotControlUI>(this, &RobotControlUI::run, "RobotControlUI");
+}
+
+void RobotControlUI::onConnectComponent()
+{
+ robotProxy = getProxy<RobotControlInterfacePrx>("RobotControl");
+ controlTask->start();
+}
+
+void RobotControlUI::onExitComponent()
+{
+ controlTask->stop();
+}
+
+void RobotControlUI::run()
+{
+ string eventstring;
+ cout << "Please insert the event string: " << std::flush;
+// cin >> eventstring;
+ eventstring = "EvLoadScenario";
+ if(eventstring == "q")
+ {
+// shutdown();
+ }
+ else{
+ cout << "Please insert the state id of the state that should process the event: " << std::flush;
+ int id;
+// cin >> id;
+ id = 11;
+ cout << "sending to id:" << id << endl;
+ EventPtr evt = new Event("EVENTTOALL",eventstring);
+ StateUtilFunctions::addToDictionary(evt, "proxyName", "RemoteStateOfferer");
+ StateUtilFunctions::addToDictionary(evt, "stateName", "MoveArm");
+ //robotProxy->issueEvent(id, evt);
+ }
+// cin >> eventstring;
+}
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h
new file mode 100644
index 0000000000000000000000000000000000000000..9e54c2978a45bd10707b3e05d6c14765e3067886
--- /dev/null
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h
@@ -0,0 +1,52 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public LIcense as
+ * published by the Free Software Foundation; either version 2 of
+ * the LIcense, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public LIcense for more details.
+ *
+ * You should have received a copy of the GNU General Public LIcense
+ * along with this program. If not, see <http://www.gnu.org/lIcenses/>.
+ *
+ * @package ArmarX::Applications
+ * @author Jan Issac (jan dot issac at gmx dot net)
+ * @date 2011
+ * @copyright http://www.gnu.org/lIcenses/gpl-2.0.txt
+ * GNU General Public LIcense
+ */
+
+#ifndef _ARMARX_SIMPLECOMPONENTTEST_H
+#define _ARMARX_SIMPLECOMPONENTTEST_H
+
+// ArmarXCore
+#include <Core/core/Component.h>
+#include <Core/core/services/tasks/RunningTask.h>
+#include <Core/core/IceManager.h>
+#include <RobotAPI/operations/RobotControl.h>
+#include <string>
+
+namespace armarx
+{
+ class RobotControlUI :
+ virtual public Component
+ {
+ public:
+ int stateId;
+ RobotControlInterfacePrx robotProxy;
+ virtual std::string getDefaultName() const { return "RobotControlUI"; }
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onExitComponent();
+ void run();
+ private:
+ RunningTask<RobotControlUI>::pointer_type controlTask;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUIApp.h b/source/RobotAPI/applications/RobotControlUI/RobotControlUIApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..923620f9e4ab35948a4d0ebc5f064cfda444c7cf
--- /dev/null
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUIApp.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Kai Welke (weöle dot at kit dot edu)
+ * @date 2012
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include "RobotControlUI.h"
+
+namespace armarx
+{
+ /**
+ * Application for testing armarx::RobotControlUI
+ */
+ class RobotControlUIApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<RobotControlUI>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/RobotControlUI/main.cpp b/source/RobotAPI/applications/RobotControlUI/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..69a727b32d7c36e304ed6ed72e9ba04b5cb07671
--- /dev/null
+++ b/source/RobotAPI/applications/RobotControlUI/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::RobotControlUI
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "RobotControlUIApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::RobotControlUIApp>();
+ app->setName("RobotControlUI");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/RobotStateComponent/._CMakeLists.txt b/source/RobotAPI/applications/RobotStateComponent/._CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d447e07e4bda6d9ea8f234bffb1da91c3490cf81
Binary files /dev/null and b/source/RobotAPI/applications/RobotStateComponent/._CMakeLists.txt differ
diff --git a/source/RobotAPI/applications/RobotStateComponent/CMakeLists.txt b/source/RobotAPI/applications/RobotStateComponent/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a1c1856f83776f3aceba1abd1caf46716cb1b669
--- /dev/null
+++ b/source/RobotAPI/applications/RobotStateComponent/CMakeLists.txt
@@ -0,0 +1,19 @@
+
+armarx_component_set_name(RobotStateComponent)
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "component disabled")
+
+find_package(Simox 2.3.0 QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+
+include_directories(${Eigen3_INCLUDE_DIR})
+include_directories(${Simox_INCLUDE_DIRS})
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore RobotAPIUnits RobotAPIInterfaces #ArmarXCoreUnits
+ RobotAPIRobotStateComponent)
+
+set(SOURCES main.cpp RobotStateComponentApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h b/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..30361b7b20ef4588704b563ead1867dc4e82240e
--- /dev/null
+++ b/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
@@ -0,0 +1,45 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotStateComponent::
+* @author ( at kit dot edu)
+* @date
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/robotstate/RobotStateComponent.h>
+
+namespace armarx
+{
+ /**
+ * Application for testing armarx::RobotStateComponent
+ */
+ class RobotStateComponentApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<RobotStateComponent>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/RobotStateComponent/main.cpp b/source/RobotAPI/applications/RobotStateComponent/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b602b6260fc3ddd8116c45c2e168bc13b5151a3
--- /dev/null
+++ b/source/RobotAPI/applications/RobotStateComponent/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::RobotStateComponent
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "RobotStateComponentApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::RobotStateComponentApp>();
+ app->setName("RobotStateComponent");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/RobotStateObserver/CMakeLists.txt b/source/RobotAPI/applications/RobotStateObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b59006f2cc85180f9944068d1186cff65a435843
--- /dev/null
+++ b/source/RobotAPI/applications/RobotStateObserver/CMakeLists.txt
@@ -0,0 +1,17 @@
+
+armarx_component_set_name(RobotStateObserver)
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "component disabled")
+find_package(Simox 2.3.0 QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+include_directories(${Eigen3_INCLUDE_DIR})
+include_directories(${Simox_INCLUDE_DIRS})
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore RobotAPIInterfaces RobotAPIUnits #ArmarXCoreUnits
+ ArmarXCoreObservers RobotAPIRemoteRobot)
+
+set(SOURCES main.cpp RobotStateObserverApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h b/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..c9b85a6868a5040af556d41cd71811e9744bc69f
--- /dev/null
+++ b/source/RobotAPI/applications/RobotStateObserver/RobotStateObserverApp.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Kai Welke (welke dot at kit dot edu)
+ * @date 2012
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/robotstate/remote/RobotStateObserver.h>
+
+namespace armarx
+{
+ /**
+ * Application for testing armarx::RobotStateObserver
+ */
+ class RobotStateObserverApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<RobotStateObserver>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/RobotStateObserver/config/testProperty b/source/RobotAPI/applications/RobotStateObserver/config/testProperty
new file mode 100644
index 0000000000000000000000000000000000000000..fd3d73770dab1e8e52b2daef51b188df7b83d1da
--- /dev/null
+++ b/source/RobotAPI/applications/RobotStateObserver/config/testProperty
@@ -0,0 +1,7 @@
+# test config file for KinematicUnit configured to be a Head
+
+ArmarX.KinematicUnitObserver.RobotFileName=/org/share/home/staff/welke/projects/armarx/data/ArmarIII/ArmarIII-Head.xml # (absolut path required) model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
+ArmarX.KinematicUnitObserver.RobotNodeSetName=Head # node set name
+ArmarX.KinematicUnitObserver.AdapterName=KinematicUnitObserver # name of the ICE adapter
+
+
diff --git a/source/RobotAPI/applications/RobotStateObserver/main.cpp b/source/RobotAPI/applications/RobotStateObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..30ecac9a8f70d004c44097c49ab5e7c5d640658d
--- /dev/null
+++ b/source/RobotAPI/applications/RobotStateObserver/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::RobotStateObserver
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "RobotStateObserverApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::RobotStateObserverApp>();
+ app->setName("RobotStateObserver");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt b/source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt
index c914803ce89b716f6ce7e82e844f599a6b205023..eabfd7b4aee6ecf6030530433236f0d481de9648 100644
--- a/source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt
+++ b/source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt
@@ -13,7 +13,8 @@ if (Eigen3_FOUND AND Simox_FOUND)
${Simox_INCLUDE_DIRS})
endif()
-set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot ${Simox_LIBRARIES})
+set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers RobotAPIRemoteRobot
+ ${Simox_LIBRARIES})
set(SOURCES main.cpp TCPControlUnitApp.h)
diff --git a/source/RobotAPI/applications/TCPMoverUnit/CMakeLists.txt b/source/RobotAPI/applications/TCPMoverUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c17f356fee3d4ecbc879653effd1ae93294bafa9
--- /dev/null
+++ b/source/RobotAPI/applications/TCPMoverUnit/CMakeLists.txt
@@ -0,0 +1,16 @@
+
+armarx_component_set_name(TCPMoverUnit)
+
+find_package(Simox 2.3.0 QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+include_directories(${Eigen3_INCLUDE_DIR})
+include_directories(${Simox_INCLUDE_DIRS})
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore #ArmarXCoreUnits
+ RobotAPIInterfaces RobotAPIUnits
+ ${Simox_LIBRARIES})
+
+set(SOURCES main.cpp TCPMoverUnitApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/TCPMoverUnit/TCPMoverUnitApp.h b/source/RobotAPI/applications/TCPMoverUnit/TCPMoverUnitApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..f03d33496fc7d3ea6b55dc1da651951e29cb10d2
--- /dev/null
+++ b/source/RobotAPI/applications/TCPMoverUnit/TCPMoverUnitApp.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::applications
+ * @author Christian Boege (boege at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/TCPMoverUnit.h>
+
+namespace armarx
+{
+ /**
+ * Application for testing armarx::TCPMoverUnit
+ */
+ class TCPMoverUnitApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<TCPMoverUnit>(properties) );
+ }
+ };
+}
diff --git a/source/RobotAPI/applications/TCPMoverUnit/main.cpp b/source/RobotAPI/applications/TCPMoverUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f941f038d5f42b4915eb785f669c9cc790319bc9
--- /dev/null
+++ b/source/RobotAPI/applications/TCPMoverUnit/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core::application::TCPMoverUnit
+ * @author Manfred Kroehnert
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "TCPMoverUnitApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::TCPMoverUnitApp>();
+ app->setName("TCPMoverUnit");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/core/CMakeLists.txt b/source/RobotAPI/core/CMakeLists.txt
index 6f5e670c2bdfe09f914c6e25232b4fb4bd756e16..58a91cb8bfe810ac80cd9c76d21930d371f791b7 100644
--- a/source/RobotAPI/core/CMakeLists.txt
+++ b/source/RobotAPI/core/CMakeLists.txt
@@ -17,7 +17,7 @@ set(LIB_NAME RobotAPICore)
set(LIB_VERSION 0.1.0)
set(LIB_SOVERSION 0)
-set(LIBS RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreRobotStateComponent ${Simox_LIBRARIES})
+set(LIBS RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreStatechart RobotAPIRobotStateComponent ${Simox_LIBRARIES})
set(LIB_FILES
RobotStatechartContext.cpp
diff --git a/source/RobotAPI/core/RobotStatechartContext.cpp b/source/RobotAPI/core/RobotStatechartContext.cpp
index 07046eed4fe71c42dc2fb370d1fe21160d5fb0ee..ca0e20b251d6fbec28687dd816a0339a244bd685 100644
--- a/source/RobotAPI/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/core/RobotStatechartContext.cpp
@@ -24,8 +24,8 @@
#include <Core/core/Component.h>
#include <Core/core/system/ImportExportComponent.h>
#include <Core/statechart/Statechart.h>
-#include <Core/robotstate/remote/RemoteRobot.h>
-#include <Core/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/robotstate/remote/RemoteRobot.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
//#include <VirtualRobot/VirtualRobot.h>
diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index a8ce3d992ddc73a91567be210f6fbfa13598d54c..09b35842c788ddcb20bbf77ae0e539c3aea5409d 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/source/RobotAPI/core/RobotStatechartContext.h
@@ -28,13 +28,13 @@
#include <Core/core/Component.h>
#include <Core/core/system/ImportExportComponent.h>
#include <Core/statechart/StatechartContext.h>
-#include <Core/robotstate/remote/RemoteRobot.h>
-#include <Core/interface/units/KinematicUnitInterface.h>
-#include <Core/interface/units/HandUnitInterface.h>
+#include <RobotAPI/robotstate/remote/RemoteRobot.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
#include <RobotAPI/interface/units/TCPControlUnit.h>
-#include <Core/units/KinematicUnitObserver.h>
+#include <RobotAPI/units/KinematicUnitObserver.h>
//#include <VirtualRobot/VirtualRobot.h>
#include <IceUtil/Time.h>
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index b0a3bf1ede7957bb68b01eb1250a3b222c5a6fe2..1b2631c9fd049b345840b104fc7c215dc6705992 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -1,7 +1,7 @@
#include "MotionControl.h"
// Core
-#include <Core/robotstate/remote/RemoteRobot.h>
+#include <RobotAPI/robotstate/remote/RemoteRobot.h>
#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/ConditionCheck.h>
#include <Core/core/system/ArmarXDataPath.h>
diff --git a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
index 2730d8056bca6899edc9dd9bc8b15a6b6ab97871..fb240c1643e68c5ef50d068e3ff170f39741d56d 100644
--- a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
+++ b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
@@ -23,7 +23,7 @@
#include "ZeroForceControl.h"
-#include <Core/robotstate/remote/RobotStateObjectFactories.h>
+#include <RobotAPI/robotstate/remote/RobotStateObjectFactories.h>
#include <Core/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/core/RobotStatechartContext.h>
diff --git a/source/RobotAPI/operations/CMakeLists.txt b/source/RobotAPI/operations/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..63e05442cd7f5aa0f754651dfba18d36e824d6a9
--- /dev/null
+++ b/source/RobotAPI/operations/CMakeLists.txt
@@ -0,0 +1,21 @@
+
+armarx_set_target("Core Library: ArmarXCoreOperations")
+find_package(Eigen3 QUIET)
+find_package(Simox 2.3.0 QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+include_directories(${Eigen3_INCLUDE_DIR})
+include_directories(${Simox_INCLUDE_DIRS})
+
+set(LIB_NAME ArmarXCoreOperations)
+set(LIB_VERSION 0.1.0)
+set(LIB_SOVERSION 0)
+
+set(LIBS RobotAPIRemoteRobot ArmarXInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers )
+
+set(LIB_FILES RobotControl.cpp)
+set(LIB_HEADERS RobotControl.h)
+
+armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/operations/RobotControl.cpp b/source/RobotAPI/operations/RobotControl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..21023484ac54b8aed0b4e0d23bee5f23cfa95d79
--- /dev/null
+++ b/source/RobotAPI/operations/RobotControl.cpp
@@ -0,0 +1,148 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date 2012
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#include "RobotControl.h"
+
+#include <RobotAPI/units/KinematicUnitObserver.h>
+
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/statechart/DynamicRemoteState.h>
+#include <RobotAPI/robotstate/remote/RobotStateObjectFactories.h>
+
+
+namespace armarx
+{
+ void RobotControl::onInitRemoteStateOfferer()
+ {
+ addState<Statechart_Robot>("RobotControl");
+ }
+
+ void RobotControl::onConnectRemoteStateOfferer()
+ {
+ startRobotStatechart();
+ }
+
+ void RobotControl::onExitRemoteStateOfferer()
+ {
+ removeInstance(stateId);
+ robotFunctionalState = NULL;
+ }
+
+ // this function creates an instance of the robot-statechart,
+ // so there is always one running-instance active that can
+ // be controlled by the GUI
+ void RobotControl::startRobotStatechart()
+ {
+
+
+ task = new RunningTask< RobotControl >(this, &RobotControl::createStaticInstance);
+ task->start();
+ }
+
+
+ void RobotControl::hardReset(const Ice::Current &)
+ {
+ removeInstance(robotFunctionalState->getLocalUniqueId());
+ startRobotStatechart();
+ }
+
+ void RobotControl::createStaticInstance()
+ {
+ getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted, 5000);
+ stateId = createRemoteStateInstance("RobotControl", NULL, "RobotStatechart");
+ std::map<int, StateBasePtr> stateList = getChildStatesByName(stateId,"Functional");
+ ARMARX_INFO << "Functional State Id:" << stateList.begin()->first << flush;
+ assert(stateList.size() > 0);
+ robotFunctionalState = stateList.begin()->second;
+ callRemoteState(stateId, StringVariantContainerBaseMap());
+
+ }
+
+
+
+ void Statechart_Robot::defineState()
+ {
+ }
+ void Statechart_Robot::defineSubstates()
+ {
+ //add substates
+ StateBasePtr stateFunctional = setInitState(addState<StateRobotControl>("Functional"));
+ StateBasePtr stateStopped = addState<State>("Stopped");
+ StateBasePtr stateFailure = addState<State>("Failure");
+
+ // add transitions
+ addTransition<EvFailure>(stateFunctional, stateFailure);
+ addTransition<EvStopRobot>(stateFunctional, stateStopped);
+ addTransition<EvStartRobot>(stateStopped, stateFunctional);
+ addTransition<EvStartRobot>(stateFailure, stateFunctional);
+ }
+
+
+
+
+ void StateRobotControl::defineSubstates()
+ {
+ // add substates
+ StateBasePtr stateIdling = addState<State>("Idling");
+ StateBasePtr stateRobotPreInitialized = addState<RobotPreInitialized>("RobotPreInitialized");
+ StateBasePtr stateRobotInitialized = addState<State>("RobotInitialized");
+ StateBasePtr stateScenarioRunning = addDynamicRemoteState();
+ StateBasePtr stateFatalError = addState<FailureState>("FatalError");
+
+ setInitState(stateRobotInitialized);
+ // add transitions
+ addTransition<EvInit>(stateIdling, stateRobotPreInitialized);
+ addTransition<EvInitialized>(stateRobotPreInitialized, stateRobotInitialized);
+ addTransition<EvLoadScenario>(stateRobotInitialized, stateScenarioRunning,
+ PM::createMapping()->mapFromEvent("*","*"));
+ addTransition<EvInit>(stateScenarioRunning, stateRobotPreInitialized);
+ addTransition<EvSuccess>(stateScenarioRunning, stateRobotPreInitialized);
+
+ // failure transitions
+ addTransition<EvLoadingFailed>(stateScenarioRunning, stateFatalError);
+ addTransition<EvConnectionLost>(stateScenarioRunning, stateFatalError);
+ addTransition<EvInitFailed>(stateRobotPreInitialized, stateFatalError);
+ addTransitionFromAllStates<EvFailure>(stateFatalError);
+ }
+
+ void StateRobotControl::onEnter()
+ {
+ // install global running conditions for the robot (e.g. temperature < xx°C)
+// KinematicUnitObserverInterfacePrx prx = getContext()->getProxy<KinematicUnitObserverInterfacePrx>("KinematicUnitObserver");
+// installCondition<EvFailure>(Expression::create()
+// ->add(channels::KinematicUnitObserver::jointTemperatures.getDatafield("KinematicUnitObserver", "NECK_JOINT0"), checks::KinematicUnitObserver::larger->getCheckStr(), 50.f));
+ }
+
+
+
+
+ RobotPreInitialized::RobotPreInitialized()
+ {
+ }
+
+ void RobotPreInitialized::onEnter()
+ {
+ // issue init on remote components and install condition "RobotInitialized"
+ }
+
+}
diff --git a/source/RobotAPI/operations/RobotControl.h b/source/RobotAPI/operations/RobotControl.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd7c8e2b1fec11fcd8c71fc9fe420a061d4ada3a
--- /dev/null
+++ b/source/RobotAPI/operations/RobotControl.h
@@ -0,0 +1,146 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package Robot::
+* @author Mirko Waechter( mirko.waechter at kit dot edu)
+* @date 2012
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_COMPONENT_ROBOTCONTROL_H
+#define _ARMARX_COMPONENT_ROBOTCONTROL_H
+
+#include <Core/statechart/Statechart.h>
+#include <Core/core/system/ImportExportComponent.h>
+#include <Core/interface/operations/RobotControlIceBase.h>
+
+namespace armarx
+{
+ /**
+ * \class RobotControl
+ * \brief RobotControl is used for dynamically loading and starting robot programs
+ * \ingroup Statechart
+ *
+ * The toplevel state Statechart_Robot describes the basic operational levels
+ * of the robot.
+ *
+ * The behavior of the functional state is described in StateRobotControl
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT RobotControl :
+ virtual public RemoteStateOfferer<StatechartContext>,
+ virtual public RobotControlIceBase
+ {
+ public:
+ /**
+ * Refernce to the currently active Functionsl state.
+ */
+ StateBasePtr robotFunctionalState;
+ // inherited from RemoteStateOfferer
+ std::string getStateOffererName() const
+ {
+ return "RobotControl";
+ }
+
+ void onInitRemoteStateOfferer();
+ void onConnectRemoteStateOfferer();
+ void onExitRemoteStateOfferer();
+ void startRobotStatechart();
+
+ void hardReset(const Ice::Current & = ::Ice::Current());
+
+
+
+ private:
+ void createStaticInstance();
+ RunningTask< RobotControl >::pointer_type task;
+ int stateId;
+ };
+
+ DEFINEEVENT(EvRobotFailure)
+ DEFINEEVENT(EvStopRobot)
+ DEFINEEVENT(EvStartRobot)
+ /**
+ * Statechart which describes the most basic states of a robot:
+
+ \dot
+ digraph States {
+ functional [ label="Functional (Initial State)" ];
+ stopped [ label="Stopped" ];
+ failure [ label="Failure" ];
+ functional -> failure [ label="EvFailure" ];
+ functional -> stopped [ label="EvStopRobot" ];
+ stopped -> functional [ label="EvStartRobot" ];
+ failure -> functional [ label="EvStartRobot" ];
+ }
+ \enddot
+ */
+ struct Statechart_Robot :
+ StateTemplate<Statechart_Robot>
+ {
+ void defineState();
+ void defineSubstates();
+ };
+
+
+ DEFINEEVENT(EvInit)
+ DEFINEEVENT(EvInitialized)
+ DEFINEEVENT(EvLoadScenario)
+ DEFINEEVENT(EvStartScenario)
+ DEFINEEVENT(EvInitFailed)
+ /**
+ * Statechart which describes the operational states of a robot program.
+ * The state "Robot Program Running" is a DynamicRemoteState and needs to
+ * be passed the input parameters \p proxyName and \p stateName via Event
+ * parameters.
+
+ \dot
+ digraph States {
+ idle [ label="Idling" ];
+ preinit [ label="RobotPreInitialized" ];
+ init [ label="RobotInitialized (Initial State)" ];
+ running [ label="Robot Program Running" ];
+ error [ label="FatalError" ];
+ idle -> preinit [ label="EvInit" ];
+ preinit -> init [ label="EvInitialized" ];
+ init -> running [ label="EvLoadScenario" ];
+ running -> preinit [ label="EvInit" ];
+ running -> preinit [ label="EvSuccess" ];
+ running -> error [ label="EvLoadingFailed" ];
+ preinit -> error [ label="EvInitFailed" ];
+ ALL -> error [ label="EvFailure" ];
+ }
+ \enddot
+ */
+ struct StateRobotControl :
+ StateTemplate<StateRobotControl>
+ {
+ void onEnter();
+ void defineSubstates();
+ };
+
+ /**
+ * Robot is in the state preinitialized.
+ */
+ struct RobotPreInitialized :
+ StateTemplate<RobotPreInitialized>
+ {
+ RobotPreInitialized();
+ void onEnter();
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/robotstate/CMakeLists.txt b/source/RobotAPI/robotstate/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..fa5318d9c58517cda33f3a78a3e51f9c099ebc84
--- /dev/null
+++ b/source/RobotAPI/robotstate/CMakeLists.txt
@@ -0,0 +1,29 @@
+
+armarx_set_target("Core Library: ArmarXCoreRobotStateComponent")
+find_package(Eigen3 QUIET)
+find_package(Simox 2.3.0 QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+include_directories(${Eigen3_INCLUDE_DIR})
+include_directories(${Simox_INCLUDE_DIRS})
+
+set(LIB_NAME RobotAPIRobotStateComponent)
+set(LIB_VERSION 0.1.0)
+set(LIB_SOVERSION 0)
+
+set(LIBS ArmarXInterfaces RobotAPIRemoteRobot
+ ArmarXCore ${Simox_LIBRARIES})
+
+set(LIB_FILES
+ RobotStateComponent.cpp
+ SharedRobotServants.cpp
+ )
+
+set(LIB_HEADERS
+ RobotStateComponent.h
+ SharedRobotServants.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/robotstate/RobotStateComponent.cpp b/source/RobotAPI/robotstate/RobotStateComponent.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5726bd529dcf9ee102f20978b1406655f5be23e9
--- /dev/null
+++ b/source/RobotAPI/robotstate/RobotStateComponent.cpp
@@ -0,0 +1,190 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotStateComponent::
+ * @author ( at kit dot edu)
+ * @date
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "RobotStateComponent.h"
+#include <boost/foreach.hpp>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <boost/format.hpp>
+#include <boost/foreach.hpp>
+#include <Core/core/system/ArmarXDataPath.h>
+
+using namespace std;
+using namespace VirtualRobot;
+using namespace Eigen;
+using namespace Ice;
+using namespace boost;
+
+std::ofstream out_file;
+
+namespace armarx
+{
+ RobotStateComponent::~RobotStateComponent()
+ {
+ if (_synchronizedPrx)
+ {
+ _synchronizedPrx->unref();
+ }
+ ARMARX_VERBOSE << "Destructor";
+ }
+
+
+ void RobotStateComponent::onInitComponent()
+ {
+ std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
+ if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile))
+ {
+ throw UserException("Could not find robot file " + robotFile);
+ }
+
+ this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+
+ if (this->_synchronized)
+ {
+ ARMARX_VERBOSE << "Loaded robot from file " << robotFile;
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Failed loading robot from file " << robotFile;
+ }
+
+ string robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+
+
+ if(robotNodeSetName == "")
+ {
+ throw UserException("RobotNodeSet not defined");
+ }
+
+ VirtualRobot::RobotNodeSetPtr rns = this->_synchronized->getRobotNodeSet(robotNodeSetName);
+ if (!rns)
+ {
+ throw UserException("RobotNodeSet not defined");
+ }
+
+ vector<RobotNodePtr> nodes = rns->getAllRobotNodes();
+
+ ARMARX_VERBOSE << "Using RobotNodeSet " << robotNodeSetName << " with " << nodes.size() << " robot nodes.";
+ BOOST_FOREACH(RobotNodePtr node, nodes)
+ {
+ ARMARX_VERBOSE << "Node: " << node->getName() << endl;
+ }
+ usingTopic(robotNodeSetName + "State");
+
+ VirtualRobot::RobotNodeSetPtr pns = this->_synchronized->getRobotNodeSet("Platform");
+ if (pns)
+ {
+ usingTopic("PlatformState");
+ vector<RobotNodePtr> nodes = pns->getAllRobotNodes();
+
+ ARMARX_VERBOSE << "Using RobotNodeSet Platform with " << nodes.size() << " robot nodes.";
+ BOOST_FOREACH(RobotNodePtr node, nodes)
+ {
+ ARMARX_VERBOSE << "Node: " << node->getName() << endl;
+ }
+ }
+ }
+
+
+ void RobotStateComponent::onConnectComponent()
+ {
+ ARMARX_INFO << "Started RobotStateComponent" << flush;
+ }
+
+
+ SharedRobotInterfacePrx RobotStateComponent::getSynchronizedRobot(const Current & current)
+ {
+ if (!this->_synchronizedPrx)
+ {
+ SharedRobotInterfacePtr shared = new SharedRobotServant(this->_synchronized);
+ shared->ref();
+ this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(current.adapter->addWithUUID(shared));
+ }
+ return this->_synchronizedPrx;
+ }
+
+
+ SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshot(const string & time,const Current & current)
+ {
+ SharedRobotInterfacePtr p = new SharedRobotServant(this->_synchronized->clone(time));
+ return SharedRobotInterfacePrx::uncheckedCast(current.adapter->addWithUUID(p));
+ }
+
+ void RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Current& c)
+ {
+// IceUtil::Time start = IceUtil::Time::now();
+ if(aValueChanged)
+ {
+ ReadLockPtr lock = _synchronized->getReadLock();
+
+ std::vector<float> jv;
+ std::vector< RobotNodePtr > nodes;
+ BOOST_FOREACH(NameValueMap::value_type namedAngle, jointAngles )
+ {
+ RobotNodePtr node = this->_synchronized->getRobotNode(namedAngle.first);
+ nodes.push_back(node);
+ jv.push_back(namedAngle.second);
+ }
+ _synchronized->setJointValues(nodes, jv);
+ }
+// ARMARX_VERBOSE_S << "duration: " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
+ }
+
+
+ void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged,const Current& c){}
+ void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged,const Current& c){}
+ void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged,const Current& c){}
+ void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged,const Current& c){}
+ void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Current& c){}
+ void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Current& c){}
+
+ void RobotStateComponent::reportPlatformPose(float currentPlatformPositionX, float currentPlatformPositionY, float currentPlatformRotation, const Ice::Current& c)
+ {
+ if (!this->_synchronized->hasRobotNodeSet("Platform") && !this->_synchronized->hasRobotNode("Platform"))
+ {
+ return;
+ }
+ {
+ WriteLockPtr lock = _synchronized->getWriteLock();
+ _synchronized->setJointValue("X_Platform", currentPlatformPositionX);
+ _synchronized->setJointValue("Y_Platform", currentPlatformPositionY);
+ _synchronized->setJointValue("Yaw_Platform", currentPlatformRotation);
+ _synchronized->applyJointValues();
+ }
+ }
+
+ void RobotStateComponent::reportNewTargetPose(float newPlatformPositionX, float newPlatformPositionY, float newPlatformRotation, const Ice::Current& c)
+ {
+ }
+
+ void RobotStateComponent::reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation, const Ice::Current &c)
+ {
+
+ }
+
+ PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new RobotStatePropertyDefinitions(
+ getConfigIdentifier()));
+ }
+}
diff --git a/source/RobotAPI/robotstate/RobotStateComponent.h b/source/RobotAPI/robotstate/RobotStateComponent.h
new file mode 100644
index 0000000000000000000000000000000000000000..11c4128aa387f64d63029daa97e621fc06cfc3ed
--- /dev/null
+++ b/source/RobotAPI/robotstate/RobotStateComponent.h
@@ -0,0 +1,126 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotStateComponent::
+ * @author ( stefan dot ulbrich at kit dot edu)
+ * @date
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotStateComponent_RobotStateComponent_H
+#define _ARMARX_COMPONENT_RobotStateComponent_RobotStateComponent_H
+
+#include "SharedRobotServants.h"
+
+#include <Core/core/Component.h>
+#include <Core/core/application/properties/Properties.h>
+#include <Core/core/system/ImportExportComponent.h>
+#include <RobotAPI/interface/robotstate/RobotState.h>
+
+#include "remote/RobotStateObjectFactories.h"
+
+namespace armarx
+{
+ /**
+ * RobotStatePropertyDefinition Property Definitions
+ */
+ class RobotStatePropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ RobotStatePropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("RobotNodeSetName","Set of nodes that is controlled by the KinematicUnit");
+ defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
+ }
+ };
+
+
+ /**
+ * The RobotStateComponent creates an internal robot representation from
+ * a VirtualRobot XML file.
+ * Upon request, an Ice proxy to a shared instance of this internal robot
+ * can be acquired through a call to RobotStateComponent::getSynchronizedRobot().
+ * Additionally it is possible to retrieve a proxy for robot snapshot with
+ * RobotStateComponent::getRobotSnapshot().
+ * While the synchronized robot will constantly update its internal state
+ * the robot snapshot is a clone of the original robot an won't update its
+ * configuration over time.
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT RobotStateComponent :
+ virtual public Component,
+ virtual public RobotStateComponentInterface
+ {
+ public:
+ // inherited from KinematicUnitInterface
+ void reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+
+ // inherited from PlatformUnitInterface
+ void reportPlatformPose(float currentPlatformPositionX, float currentPlatformPositionY, float currentPlatformRotation, const Ice::Current& c = ::Ice::Current());
+ void reportNewTargetPose(float newPlatformPositionX, float newPlatformPositionY, float newPlatformRotation, const Ice::Current& c = ::Ice::Current());
+ void reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * \return SharedRobotInterface proxy to the internal synchronized robot (gets created upon first call)
+ */
+ virtual SharedRobotInterfacePrx getSynchronizedRobot(const Ice::Current&);
+
+ /**
+ * \return clone of the internal synchronized robot
+ */
+ virtual SharedRobotInterfacePrx getRobotSnapshot(const std::string & time, const Ice::Current&);
+
+
+ /**
+ * Create an instance of RobotStatePropertyDefinitions.
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ virtual std::string getDefaultName() const
+ {
+ return "RobotStateComponent";
+ }
+ protected:
+ /**
+ * Load and create a VirtualRobot::Robot instance from the RobotFileName
+ * property. Additionally listen on the KinematicUnit topic for the
+ * RobotNodeSet specified in the RobotNodeSetName property.
+ */
+ virtual void onInitComponent();
+ /**
+ * Setup RobotStateObjectFactories needed for creating RemoteRobot instances.
+ */
+ virtual void onConnectComponent();
+ /**
+ * Calls unref() on RobotStateComponent::_synchronizedPrx.
+ */
+ virtual ~RobotStateComponent();
+ private:
+ SharedRobotInterfacePrx _synchronizedPrx;
+ VirtualRobot::RobotPtr _synchronized;
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/robotstate/SharedRobotServants.cpp b/source/RobotAPI/robotstate/SharedRobotServants.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..28c84a870741e8f20b39c634fe8ad1e7fc1a0ce9
--- /dev/null
+++ b/source/RobotAPI/robotstate/SharedRobotServants.cpp
@@ -0,0 +1,347 @@
+#include "SharedRobotServants.h"
+
+#include <Core/core/logging/Logging.h>
+
+#include <boost/foreach.hpp>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
+#include <VirtualRobot/RobotConfig.h>
+
+using namespace std;
+using namespace VirtualRobot;
+using namespace Eigen;
+using namespace Ice;
+using namespace boost;
+
+
+#undef VERBOSE
+
+
+namespace armarx
+{
+
+typedef std::map<std::string, SharedRobotNodeInterfacePrx> NodeCache;
+
+///////////////////////////////
+// SharedObjectBase
+///////////////////////////////
+
+SharedObjectBase::SharedObjectBase()
+{
+ this-> _referenceCount=0;
+ #ifdef VERBOSE
+ ARMARX_LOG_S << "construct " << this << flush;
+ #endif
+}
+
+
+void SharedObjectBase::ref(const Current &c)
+{
+ lock_guard<mutex> lock(this->_counterMutex);
+
+ _referenceCount++;
+ #ifdef VERBOSE
+ ARMARX_LOG_S << "ref: " << _referenceCount << " " << this << flush;
+ #endif
+}
+
+void SharedObjectBase::unref(const Current &c)
+{
+ lock_guard<mutex> lock(this->_counterMutex);
+
+#ifdef VERBOSE
+ ARMARX_LOG_S << "unref: " << _referenceCount << " " << this << flush;
+#endif
+ _referenceCount --;
+ if (_referenceCount==0)
+ this->destroy(c);
+}
+
+void SharedObjectBase::destroy(const Current &c)
+{
+ try {
+ c.adapter->remove(c.id);
+ #ifdef VERBOSE
+ ARMARX_ERROR_S << "[SharedObject] destroy " << " " << this << flush;
+ #endif
+ } catch (const NotRegisteredException&){
+ throw ObjectNotExistException(__FILE__, __LINE__);
+ };
+}
+
+///////////////////////////////
+// SharedRobotNodeServant
+///////////////////////////////
+
+SharedRobotNodeServant::SharedRobotNodeServant(RobotNodePtr node) :
+ _node(node)
+{
+#ifdef VERBOSE
+ ARMARX_LOG_S << "[SharedRobotNodeServant] construct " << " " << this << flush;
+#endif
+}
+
+
+SharedRobotNodeServant::~SharedRobotNodeServant()
+{
+#ifdef VERBOSE
+ ARMARX_LOG_S << "[SharedRobotNodeServant] destruct " << " " << this << flush;
+#endif
+}
+
+float SharedRobotNodeServant::getJointValue(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getJointValue();
+}
+
+string SharedRobotNodeServant::getName(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getName();
+}
+
+PoseBasePtr SharedRobotNodeServant::getLocalTransformation(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return new Pose(_node->getLocalTransformation());
+}
+/*
+PoseBasePtr SharedRobotNodeServant::getPostJointTransformation(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return new Pose(_node->getPostJointTransformation());
+}
+*/
+PoseBasePtr SharedRobotNodeServant::getGlobalPose(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return new Pose(_node->getGlobalPose());
+}
+
+PoseBasePtr SharedRobotNodeServant::getGlobalPoseJoint(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return new Pose(_node->getGlobalPoseJoint());
+}
+
+JointType SharedRobotNodeServant::getType(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ if (_node->isRotationalJoint()) return eRevolute;
+ else if (_node->isTranslationalJoint()) return ePrismatic;
+ else return eFixed;
+}
+
+Vector3BasePtr SharedRobotNodeServant::getJointTranslationDirection(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ shared_ptr<RobotNodePrismatic> prismatic = dynamic_pointer_cast<RobotNodePrismatic>(_node);
+ if (prismatic)
+ return new Vector3(prismatic->getJointTranslationDirection());
+ else
+ return new Vector3;
+}
+
+Vector3BasePtr SharedRobotNodeServant::getJointRotationAxis(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ shared_ptr<RobotNodeRevolute> revolute = dynamic_pointer_cast<RobotNodeRevolute>(_node);
+ if (revolute)
+ return new Vector3(revolute->getJointRotationAxis());
+ else
+ return new Vector3;
+}
+
+
+bool SharedRobotNodeServant::hasChild(const string &name, bool recursive, const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ //return _node->hasChild(name,recursive);
+ return false;
+}
+
+string SharedRobotNodeServant::getParent(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ SceneObjectPtr parent = _node->getParent();
+ if (!parent) throw UserException("This RobotNode has no parent.");
+ return parent->getName();
+}
+
+NameList SharedRobotNodeServant::getChildren(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ vector<SceneObjectPtr> children = _node->getChildren();
+ NameList names;
+ BOOST_FOREACH(SceneObjectPtr node, children){
+ names.push_back(node->getName());
+ }
+ return names;
+}
+
+NameList SharedRobotNodeServant::getAllParents(const std::string &name, const Ice::Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ vector<RobotNodePtr> parents = _node->getAllParents(_node->getRobot()->getRobotNodeSet(name));
+ NameList names;
+ BOOST_FOREACH(RobotNodePtr node, parents){
+ names.push_back(node->getName());
+ }
+ return names;
+}
+
+float SharedRobotNodeServant::getJointValueOffest(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getJointValueOffset();
+}
+
+float SharedRobotNodeServant::getJointLimitHigh(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getJointLimitHigh();
+}
+
+float SharedRobotNodeServant::getJointLimitLow(const Current ¤t) const
+{
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getJointLimitLow();
+}
+
+
+///////////////////////////////
+// SharedRobotServant
+///////////////////////////////
+
+SharedRobotServant::SharedRobotServant(RobotPtr robot)
+ : _robot(robot)
+{
+#ifdef VERBOSE
+ ARMARX_WARNING_S << "construct " << this << flush;
+#endif
+}
+
+SharedRobotServant::~SharedRobotServant()
+{
+#ifdef VERBOSE
+ ARMARX_WARNING_S << "destruct " << this << flush;
+#endif
+ BOOST_FOREACH(NodeCache::value_type value, this->_cachedNodes)
+ value.second->unref();
+}
+
+SharedRobotNodeInterfacePrx SharedRobotServant::getRobotNode(const string &name, const Current ¤t)
+{
+// ARMARX_LOG_S << "Looking for node: " << name << flush;
+ assert(_robot);
+
+ if (this->_cachedNodes.find(name) == this->_cachedNodes.end()){
+ RobotNodePtr robotNode = _robot->getRobotNode(name);
+ if (!robotNode){
+ ARMARX_WARNING_S << "RobotNode \"" + name + "\" not defined.";
+ throw UserException("RobotNode \"" + name + "\" not defined.");
+ }
+
+ SharedRobotNodeInterfacePtr servant = new SharedRobotNodeServant(
+ _robot->getRobotNode(name));
+ //servant->ref();
+ this->_cachedNodes[name] = SharedRobotNodeInterfacePrx::uncheckedCast(current.adapter->addWithUUID(servant));
+ this->_cachedNodes[name]->ref();
+ }
+ return this->_cachedNodes[name];
+}
+
+SharedRobotNodeInterfacePrx SharedRobotServant::getRootNode(const Current ¤t)
+{
+ assert(_robot);
+ string name = _robot->getRootNode()/*,current*/->getName();
+ return this->getRobotNode(name, current);
+}
+
+
+bool SharedRobotServant::hasRobotNode(const string & name, const Current ¤t)
+{
+ return _robot->hasRobotNode(name);
+}
+
+NameList SharedRobotServant::getRobotNodes(const Current ¤t)
+{
+ vector<RobotNodePtr> robotNodes = _robot->getRobotNodes();
+ NameList names;
+ BOOST_FOREACH(RobotNodePtr node, robotNodes){
+ names.push_back(node->getName());
+ }
+ return names;
+}
+
+RobotNodeSetInfoPtr SharedRobotServant::getRobotNodeSet(const string & name, const Current ¤t)
+{
+ RobotNodeSetPtr robotNodeSet = _robot->getRobotNodeSet(name);
+ if (!robotNodeSet) throw UserException("RobotNodeSet \""+name+"\" not defined");
+ vector<RobotNodePtr> robotNodes = robotNodeSet->getAllRobotNodes();
+ NameList names;
+ BOOST_FOREACH(RobotNodePtr node, robotNodes){
+ names.push_back(node->getName());
+ }
+
+ RobotNodeSetInfoPtr info = new RobotNodeSetInfo;
+ info->names = names;
+ info->name = name;
+ info->tcpName = robotNodeSet->getTCP()->getName();
+ info->rootName = robotNodeSet->getKinematicRoot()->getName();
+
+ return info;
+
+}
+
+NameList SharedRobotServant::getRobotNodeSets(const Current ¤t)
+{
+ vector<RobotNodeSetPtr> robotNodeSets = _robot->getRobotNodeSets();
+ NameList names;
+ BOOST_FOREACH(RobotNodeSetPtr set, robotNodeSets){
+ names.push_back(set->getName());
+ }
+
+ return names;
+}
+
+bool SharedRobotServant::hasRobotNodeSet(const string & name, const Current ¤t)
+{
+ return _robot->hasRobotNodeSet(name);
+}
+
+string SharedRobotServant::getName(const Current ¤t)
+{
+ return _robot->getName();
+}
+
+string SharedRobotServant::getType(const Current ¤t)
+{
+ return _robot->getType();
+}
+
+PoseBasePtr SharedRobotServant::getGlobalPose(const Current ¤t)
+{
+ ReadLockPtr lock = _robot->getReadLock();
+ return new Pose(_robot->getGlobalPose());
+}
+
+NameValueMap SharedRobotServant::getConfig(const Current ¤t)
+{
+ if(!_robot)
+ {
+ ARMARX_WARNING_S << "no robot set";
+ return NameValueMap();
+ }
+
+ std::map < std::string, float > values = _robot->getConfig()->getRobotNodeJointValueMap();
+ NameValueMap result(values.begin(), values.end());
+ return result;
+}
+
+}
diff --git a/source/RobotAPI/robotstate/SharedRobotServants.h b/source/RobotAPI/robotstate/SharedRobotServants.h
new file mode 100644
index 0000000000000000000000000000000000000000..89a063b4649f3f48caaede8487210235810c7b4b
--- /dev/null
+++ b/source/RobotAPI/robotstate/SharedRobotServants.h
@@ -0,0 +1,103 @@
+#ifndef __SHARED_ROBOT_SERVANTS_H__
+#define __SHARED_ROBOT_SERVANTS_H__
+
+#include <Core/core/Component.h>
+#include <Core/core/system/ImportExportComponent.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <RobotAPI/interface/robotstate/RobotState.h>
+
+#include <boost/thread.hpp>
+
+namespace armarx {
+ // Zugriff muss mutex geschuetzt werden!
+
+
+ class SharedObjectBase :
+ virtual public SharedObjectInterface
+ {
+ public:
+ virtual void ref(const Ice::Current &c);
+ virtual void unref(const Ice::Current &c);
+ virtual void destroy(const Ice::Current &c);
+ SharedObjectBase();
+ private:
+ unsigned int _referenceCount;
+ boost::mutex _counterMutex;
+ };
+
+ class SharedRobotNodeServant :
+ virtual public SharedRobotNodeInterface,
+ public SharedObjectBase
+ {
+ public:
+ SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current ¤t*/);
+ ~SharedRobotNodeServant();
+
+ virtual float getJointValue(const Ice::Current ¤t) const;
+ virtual std::string getName(const Ice::Current ¤t) const;
+
+ virtual PoseBasePtr getLocalTransformation(const Ice::Current ¤t) const;
+ //virtual PoseBasePtr getPostJointTransformation(const Ice::Current ¤t) const;
+ virtual PoseBasePtr getGlobalPose(const Ice::Current ¤t) const;
+ virtual PoseBasePtr getGlobalPoseJoint(const Ice::Current ¤t) const;
+
+ virtual JointType getType(const Ice::Current ¤t) const;
+ virtual Vector3BasePtr getJointTranslationDirection(const Ice::Current ¤t) const;
+ virtual Vector3BasePtr getJointRotationAxis(const Ice::Current ¤t) const;
+
+ virtual bool hasChild(const std::string &name, bool recursive, const Ice::Current ¤t) const;
+ virtual std::string getParent(const Ice::Current ¤t) const;
+ virtual NameList getChildren(const Ice::Current ¤t) const;
+ virtual NameList getAllParents(const std::string &name, const Ice::Current ¤t) const;
+
+ virtual float getJointValueOffest(const Ice::Current ¤t) const;
+ virtual float getJointLimitHigh(const Ice::Current ¤t) const;
+ virtual float getJointLimitLow(const Ice::Current ¤t) const;
+
+
+
+ protected:
+ VirtualRobot::RobotNodePtr _node;
+ };
+
+
+ class SharedRobotServant :
+ public virtual SharedRobotInterface,
+ public SharedObjectBase
+ {
+ public:
+ SharedRobotServant(VirtualRobot::RobotPtr robot);
+ ~SharedRobotServant();
+
+ virtual SharedRobotNodeInterfacePrx getRobotNode(const std::string & name, const Ice::Current ¤t);
+ virtual SharedRobotNodeInterfacePrx getRootNode(const Ice::Current ¤t);
+ virtual bool hasRobotNode(const std::string & name, const Ice::Current ¤t);
+
+ virtual NameList getRobotNodes(const Ice::Current ¤t);
+ virtual RobotNodeSetInfoPtr getRobotNodeSet(const std::string & name, const Ice::Current ¤t);
+ virtual NameList getRobotNodeSets(const Ice::Current ¤t);
+ virtual bool hasRobotNodeSet(const std::string & name, const Ice::Current ¤t);
+
+ virtual std::string getName(const Ice::Current ¤t);
+ virtual std::string getType(const Ice::Current ¤t);
+
+ virtual PoseBasePtr getGlobalPose(const Ice::Current ¤t);
+ NameValueMap getConfig(const Ice::Current ¤t);
+
+ VirtualRobot::RobotPtr getRobot()
+ {
+ return this->_robot;
+ }
+
+ protected:
+ VirtualRobot::RobotPtr _robot;
+ std::map<std::string, SharedRobotNodeInterfacePrx> _cachedNodes;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/robotstate/remote/ArmarPose.cpp b/source/RobotAPI/robotstate/remote/ArmarPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..311615f9fd09cfb71ea45fb6c345ad1f9d514d83
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/ArmarPose.cpp
@@ -0,0 +1,594 @@
+#include "ArmarPose.h"
+
+// boost includes
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/Robot.h>
+
+using namespace Eigen;
+using namespace std;
+
+namespace armarx
+{
+
+ Vector3::Vector3()
+ {
+ x = 0;
+ y = 0;
+ z = 0;
+ }
+
+ Vector3::Vector3(const Vector3f & v)
+ {
+ x = v(0);
+ y = v(1);
+ z = v(2);
+ }
+
+ Vector3::Vector3(const Matrix4f & m)
+ {
+ x = m(0,3);
+ y = m(1,3);
+ z = m(2,3);
+ }
+
+ Vector3f Vector3::toEigen() const
+ {
+ Vector3f v;
+ v << this->x,this->y,this->z;
+ return v;
+ }
+
+ int Vector3::readFromXML(const string &xmlData, const Ice::Current &c)
+ {
+ boost::property_tree::ptree pt = Variant::GetPropertyTree(xmlData);
+
+ x = (pt.get<float>("x"));
+ y = (pt.get<float>("y"));
+ z = (pt.get<float>("z"));
+ return 1;
+ }
+
+ string Vector3::writeAsXML(const Ice::Current &)
+ {
+ using namespace boost::property_tree;
+ ptree pt;
+ pt.add("x", x);
+ pt.add("y", y);
+ pt.add("z", z);
+
+ std::stringstream stream;
+ xml_parser::write_xml(stream, pt);
+ return stream.str();
+ }
+
+ void Vector3::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ obj->setFloat("x", x);
+ obj->setFloat("y", y);
+ obj->setFloat("z", z);
+ }
+
+ void Vector3::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ x = obj->getFloat("x");
+ y = obj->getFloat("y");
+ z = obj->getFloat("z");
+ }
+
+
+ Quaternion::Quaternion()
+ {
+ }
+
+ Quaternion::Quaternion(const Matrix4f &m4)
+ {
+ Matrix3f m3=m4.block<3,3>(0,0);
+ this->init(m3);
+ }
+
+ Quaternion::Quaternion(const Matrix3f &m3)
+ {
+ this->init(m3);
+ }
+
+ Quaternion::Quaternion(const Eigen::Quaternionf &q)
+ {
+ this->init(q);
+ }
+
+ Matrix3f Quaternion::toEigen() const
+ {
+ Matrix3f rot;
+ rot = Quaternionf(
+ this->qw,
+ this->qx,
+ this->qy,
+ this->qz);
+ return rot;
+ }
+
+ void Quaternion::init(const Matrix3f &m3)
+ {
+ Quaternionf q;
+ q = m3;
+ init(q);
+ }
+
+ void Quaternion::init(const Eigen::Quaternionf &q)
+ {
+ this->qw=q.w();
+ this->qx=q.x();
+ this->qy=q.y();
+ this->qz=q.z();
+ }
+
+ Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f &m)
+ {
+ return Quaternion::slerp(alpha,this->toEigen(),m);
+ }
+
+ Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f &m1, const Eigen::Matrix3f &m2)
+ {
+ Matrix3f result;
+ Quaternionf q1,q2;
+ q1 = m1;
+ q2=m2;
+ result= q1.slerp(alpha,q2);
+ return result;
+ }
+
+ int Quaternion::readFromXML(const string &xmlData, const Ice::Current &c)
+ {
+ boost::property_tree::ptree pt = Variant::GetPropertyTree(xmlData);
+
+ using namespace Eigen;
+
+ Quaternionf q;
+ if(pt.get_optional<float>("angle"))
+ {// AxisAngle-Notation
+ float angle = pt.get<float>("angle");
+
+ Vector3f axis;
+ axis << pt.get<float>("x") , pt.get<float>("y"), pt.get<float>("z");
+
+ AngleAxisf aa(angle, axis);
+ q = aa;
+ qw = q.w();
+ qx = q.x();
+ qy = q.y();
+ qz = q.z();
+ }
+ else
+ {
+ qx = (pt.get<float>("qx"));
+ qy = (pt.get<float>("qy"));
+ qz = (pt.get<float>("qz"));
+ qw = (pt.get<float>("qw"));
+ }
+
+ return 1;
+ }
+
+ string Quaternion::writeAsXML(const Ice::Current &)
+ {
+ using namespace boost::property_tree;
+ ptree pt;
+ pt.add("qx", qx);
+ pt.add("qy", qy);
+ pt.add("qz", qz);
+ pt.add("qw", qw);
+
+ std::stringstream stream;
+ xml_parser::write_xml(stream, pt);
+ return stream.str();
+ }
+
+ void Quaternion::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ obj->setFloat("qx", qx);
+ obj->setFloat("qy", qy);
+ obj->setFloat("qz", qz);
+ obj->setFloat("qw", qw);
+ }
+
+ void Quaternion::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ qx = obj->getFloat("qx");
+ qy = obj->getFloat("qy");
+ qz = obj->getFloat("qz");
+ qw = obj->getFloat("qw");
+ }
+
+ Pose::Pose(const Eigen::Matrix3f &m, const Eigen::Vector3f &v)
+ {
+ this->orientation = new Quaternion(m);
+ this->position = new Vector3(v);
+
+ //The initializiation has to take place ALSO after the demarshalling
+ this->c_position = Vector3Ptr::dynamicCast(this->position);
+ this->c_orientation = QuaternionPtr::dynamicCast(this->orientation);
+ }
+
+ Pose::Pose()
+ {
+ this->orientation = new Quaternion();
+ this->position = new Vector3();
+ this->c_position = Vector3Ptr::dynamicCast(this->position);
+ this->c_orientation = QuaternionPtr::dynamicCast(this->orientation);
+ }
+
+ Pose::Pose(const Matrix4f &m)
+ {
+ this->orientation = new Quaternion(m);
+ this->position = new Vector3(m);
+ this->c_position = Vector3Ptr::dynamicCast(this->position);
+ this->c_orientation = QuaternionPtr::dynamicCast(this->orientation);
+ }
+
+ Matrix4f Pose::toEigen() const
+ {
+ Matrix4f m = Matrix4f::Identity();
+ // WARNING dynamicCast is slow! To speed this up (if necessary)
+ // create Member variables in the demarshalization.
+ m.block<3,3>(0,0) = QuaternionPtr::dynamicCast(this->orientation)->toEigen();
+ m.block<3,1>(0,3) = Vector3Ptr::dynamicCast(this->position)->toEigen();
+ return m;
+ }
+
+ int Pose::readFromXML(const string &xmlData, const Ice::Current &c)
+ {
+ position->readFromXML(xmlData);
+ orientation->readFromXML(xmlData);
+
+ return 1;
+ }
+
+ string Pose::writeAsXML(const Ice::Current &)
+ {
+ using namespace boost::property_tree;
+ std::stringstream stream;
+ stream <<
+ position->writeAsXML() <<
+ orientation->writeAsXML();
+ return stream.str();
+ }
+
+ void Pose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ {
+ position->serialize(serializer, c);
+ orientation->serialize(serializer, c);
+ }
+
+ void Pose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ position->deserialize(obj);
+ orientation->deserialize(obj);
+ }
+
+ FramedVector3::FramedVector3()
+ {
+ }
+
+ FramedVector3::FramedVector3(const FramedVector3 &source) :
+ IceUtil::Shared(source),
+ Vector3Base(source),
+ FramedVector3Base(source),
+ Vector3(source)
+ {
+ }
+
+ FramedVector3::FramedVector3(const Eigen::Vector3f &vec, const string &frame) :
+ Vector3(vec)
+ {
+ this->frame = frame;
+ }
+
+ string FramedVector3::getFrame() const
+ {
+ return frame;
+ }
+
+ FramedVector3Ptr FramedVector3::ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedVector3 &framedVec, const string &newFrame)
+ {
+ if(framedVec.getFrame() == newFrame)
+ return FramedVector3Ptr::dynamicCast(framedVec.clone());
+
+ Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(framedVec.frame, newFrame, robot);
+
+ Eigen::Vector3f vecOldFrame = framedVec.Vector3::toEigen();
+
+ Eigen::Vector3f newVec = rotToNewFrame.block(0,0,3,3).inverse() * vecOldFrame;
+
+ FramedVector3Ptr result = new FramedVector3();
+ result->x = newVec(0);
+ result->y = newVec(1);
+ result->z = newVec(2);
+ result->frame = newFrame;
+ return result;
+ }
+
+ Eigen::Matrix4f FramedVector3::__GetRotationBetweenFrames(const std::string &oldFrame, const std::string &newFrame, VirtualRobot::RobotPtr robotState)
+ {
+ Eigen::Matrix4f toNewFrame = robotState->getRobotNode(oldFrame)->getTransformationTo(robotState->getRobotNode(newFrame));
+ toNewFrame(0,3) = 0;
+ toNewFrame(1,3) = 0;
+ toNewFrame(2,3) = 0;
+
+ return toNewFrame;
+ }
+
+ int FramedVector3::readFromXML(const string &xmlData, const Ice::Current &c)
+ {
+ int result = Vector3::readFromXML(xmlData);
+
+ boost::property_tree::ptree pt = Variant::GetPropertyTree(xmlData);
+
+ frame = (pt.get<std::string>("frame"));
+
+ return result;
+ }
+
+ string FramedVector3::writeAsXML(const Ice::Current &c)
+ {
+ using namespace boost::property_tree;
+ ptree pt = Variant::GetPropertyTree(Vector3::writeAsXML());
+ pt.add("frame", frame);
+
+ std::stringstream stream;
+ boost::property_tree::xml_parser::xml_writer_settings<char> settings('\t', 1);
+ xml_parser::write_xml(stream, pt, settings);
+
+ return stream.str();
+ }
+
+ void FramedVector3::serialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &) const
+ {
+ throw LocalException("NYI");
+ }
+
+ void FramedVector3::deserialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &)
+ {
+ throw LocalException("NYI");
+ }
+
+
+
+ FramedPose::FramedPose() :
+ Pose()
+ {
+ frame = "";
+ }
+
+ FramedPose::FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s) :
+ Pose(m, v)
+ {
+ frame = s;
+ }
+
+ FramedPose::FramedPose(const Eigen::Matrix4f& m, const std::string& s) :
+ Pose(m)
+ {
+ frame = s;
+ }
+
+ std::string FramedPose::getFrame() const
+ {
+ return frame;
+ }
+
+ int FramedPose::readFromXML(const string &xmlData, const Ice::Current &c){
+ boost::property_tree::ptree pt = Variant::GetPropertyTree(xmlData);
+
+
+ Pose::readFromXML(xmlData);
+ frame = (pt.get<std::string>("frame"));
+
+ return 1;
+ }
+
+ string FramedPose::writeAsXML(const Ice::Current &)
+ {
+ using namespace boost::property_tree;
+ std::stringstream stream;
+ stream << Pose::writeAsXML()
+ << "<frame>"<<frame<<"</frame>";
+ return stream.str();
+ }
+
+ void FramedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ Pose::serialize(obj, c);
+ obj->setString("frame", frame);
+ }
+
+ void FramedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ Pose::deserialize(obj);
+ frame = obj->getString("frame");
+ }
+
+
+ FramedPosition::FramedPosition() :
+ Vector3()
+ {
+ frame = "";
+ }
+
+ FramedPosition::FramedPosition(const Eigen::Vector3f &v, const std::string &s) :
+ Vector3(v)
+ {
+ frame = s;
+ }
+
+ FramedPosition::FramedPosition(const Eigen::Matrix4f &m, const std::string &s) :
+ Vector3(m)
+ {
+ frame = s;
+ }
+
+ // this doesnt work for unknown reasons
+// FramedPosition::FramedPosition(const Vector3BasePtr pos, const std::string &frame )
+// {
+// this->x = pos->x;
+// this->y = pos->y;
+// this->z = pos->z;
+// this->frame = frame;
+// }
+
+ std::string FramedPosition::getFrame() const
+ {
+ return this->frame;
+ }
+
+ int FramedPosition::readFromXML(const string &xmlData, const Ice::Current &c){
+ using namespace boost::property_tree;
+ ptree pt;
+ std::stringstream stream;
+ stream << xmlData;
+ xml_parser::read_xml(stream, pt);
+
+ Vector3::readFromXML(xmlData);
+ frame = (pt.get<std::string>("frame"));
+
+ return 1;
+ }
+
+ string FramedPosition::writeAsXML(const Ice::Current &)
+ {
+ using namespace boost::property_tree;
+ std::stringstream stream;
+ stream << Vector3::writeAsXML() <<
+ "<frame>"<<frame<<"</frame>";
+ return stream.str();
+ }
+
+ void FramedPosition::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ Vector3::serialize(obj, c);
+ obj->setString("frame", frame);
+ }
+
+ void FramedPosition::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ Vector3::deserialize(obj);
+ frame = obj->getString("frame");
+ }
+
+
+ FramedOrientation::FramedOrientation() :
+ Quaternion()
+ {
+ frame = "";
+ }
+
+ FramedOrientation::FramedOrientation(const Eigen::Matrix3f &m, const std::string &s) :
+ Quaternion(m)
+ {
+ frame = s;
+ }
+
+ FramedOrientation::FramedOrientation(const Eigen::Matrix4f &m, const std::string &s) :
+ Quaternion(m)
+ {
+ frame = s;
+ }
+
+ // this doesnt work for an unknown reason
+// FramedOrientation::FramedOrientation(const QuaternionBasePtr ori, const std::string &frame )
+// {
+// Matrix3f rot;
+// rot = Quaternionf(ori->qw, ori->qx, ori->qy, ori->qz);
+// FramedOrientation(rot, frame);
+// }
+
+ std::string FramedOrientation::getFrame() const
+ {
+ return this->frame;
+ }
+
+ int FramedOrientation::readFromXML(const string &xmlData, const Ice::Current &c)
+ {
+ using namespace boost::property_tree;
+ ptree pt;
+ std::stringstream stream;
+ stream << xmlData;
+ xml_parser::read_xml(stream, pt);
+
+ Quaternion::readFromXML(xmlData);
+ frame = (pt.get<std::string>("frame"));
+
+ return 1;
+ }
+
+ string FramedOrientation::writeAsXML(const Ice::Current &)
+ {
+ using namespace boost::property_tree;
+ std::stringstream stream;
+ stream << Quaternion::writeAsXML() <<
+ "<frame>" << frame << "</frame>";
+ return stream.str();
+ }
+
+ void FramedOrientation::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ Quaternion::serialize(obj, c);
+ obj->setString("frame", frame);
+ }
+
+ void FramedOrientation::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ Quaternion::deserialize(obj);
+ frame = obj->getString("frame");
+ }
+
+ namespace ArmarPose
+ {
+ VirtualRobot::LinkedCoordinate createLinkedCoordinate(VirtualRobot::RobotPtr &virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation)
+ {
+ VirtualRobot::LinkedCoordinate c(virtualRobot);
+ std::string frame = position->getFrame();
+
+ if(!(frame == orientation->getFrame()))
+ throw armarx::UserException("Error: frames mismatch");
+
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+
+ pose.block<3,3>(0,0) = orientation->toEigen();
+ pose.block<3,1>(0,3) = position->toEigen();
+
+ c.set(frame, pose);
+
+ return c;
+ }
+ }
+
+
+
+
+}
diff --git a/source/RobotAPI/robotstate/remote/ArmarPose.h b/source/RobotAPI/robotstate/remote/ArmarPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..4b5e2378fa02812dbc158a306ded1a35141057f0
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/ArmarPose.h
@@ -0,0 +1,594 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotStateComponent::
+ * @author ( stefan dot ulbrich at kit dot edu)
+ * @date
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotStateComponent_ArmarPose_H
+#define _ARMARX_COMPONENT_RobotStateComponent_ArmarPose_H
+#include <sstream>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <RobotAPI/interface/robotstate/RobotState.h>
+#include <Core/observers/variant/Variant.h>
+#include <Core/observers/AbstractObjectSerializer.h>
+#include <VirtualRobot/LinkedCoordinate.h>
+#include <VirtualRobot/LinkedCoordinate.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <Core/core/exceptions/local/ExpressionException.h>
+
+namespace armarx
+{
+ namespace VariantType
+ {
+ // variant types
+ const VariantTypeId Vector3 = Variant::addTypeName("::armarx::Vector3Base");
+ const VariantTypeId Quaternion = Variant::addTypeName("::armarx::QuaternionBase");
+ const VariantTypeId Pose = Variant::addTypeName("::armarx::PoseBase");
+ const VariantTypeId FramedVector3 = Variant::addTypeName("::armarx::FramedVector3Base");
+ const VariantTypeId FramedPose = Variant::addTypeName("::armarx::FramedPoseBase");
+ const VariantTypeId FramedPosition = Variant::addTypeName("::armarx::FramedPositionBase");
+ const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
+ }
+
+
+ /**
+ * @class Vector3
+ * @ingroup Variants
+ * @brief The Vector3 class
+ */
+ class Vector3 :
+ virtual public Vector3Base
+ {
+ public:
+ Vector3();
+ Vector3(const Eigen::Vector3f &);
+ Vector3(const Eigen::Matrix4f &);
+
+ virtual Eigen::Vector3f toEigen() const;
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new Vector3(*this);
+ }
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << toEigen();
+ return s.str();
+ }
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::Vector3;
+ }
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+ /**
+ * @brief Implementation of virtual function to read a Vector3 from an XML-file.
+ *Example xml-layout:
+ * @code
+ * <x>0</x>
+ * <y>0</y>
+ * <z>0</z>
+ * @endcode
+
+ * @param xmlData String with xml-data. NOT a file path!
+ * @return ErrorCode, 1 on Success
+ */
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const Vector3& rhs)
+ {
+ stream << "Vector3: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ };
+
+ public: // serialization
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+ };
+
+ typedef IceInternal::Handle<Vector3> Vector3Ptr;
+
+
+ /**
+ * @class Quaternion
+ * @ingroup Variants
+ * @brief The Quaternion class
+ */
+ class Quaternion :
+ virtual public QuaternionBase
+ {
+ public:
+ Quaternion();
+ Quaternion(const Eigen::Matrix4f &);
+ Quaternion(const Eigen::Matrix3f &);
+ Quaternion(const Eigen::Quaternionf &);
+
+ Eigen::Matrix3f toEigen() const;
+ Eigen::Matrix3f slerp(float, const Eigen::Matrix3f &);
+
+ static Eigen::Matrix3f slerp(float, const Eigen::Matrix3f &, const Eigen::Matrix3f &);
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new Quaternion(*this);
+ }
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << toEigen();
+ return s.str();
+ }
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::Quaternion;
+ }
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+ /**
+ * @brief Implementation of virtual function to read a Quaternion from an XML-file.
+ *Example xml-layout:
+ * @code
+ * <qw>0</qw>
+ * <qx>0</qx>
+ * <qy>0</qy>
+ * <qz>0</qz>
+ * @endcode
+ *
+ *OR in AngleAxis-Notation with radiant
+ *
+ * @code
+ * <angle>0.5</qw>
+ * <x>0.2</x>
+ * <y>0.1</y>
+ * <z>0.7</z>
+ * @endcode
+ *
+ * @param xmlData String with xml-data. NOT a file path!
+ * @param c
+ * @return ErrorCode, 1 on Success
+ */
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const Quaternion& rhs)
+ {
+ stream << "Quaternion: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ };
+
+ public: // serialization
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+
+ private:
+ void init(const Eigen::Matrix3f &);
+ void init(const Eigen::Quaternionf &);
+ };
+
+ typedef IceInternal::Handle<Quaternion> QuaternionPtr;
+
+
+ /**
+ * @class Pose
+ * @ingroup Variants
+ * @brief The Pose class
+ */
+ class Pose :
+ virtual public PoseBase
+ {
+ public:
+ Pose();
+ Pose(const Eigen::Matrix4f &);
+ Pose(const Eigen::Matrix3f &, const Eigen::Vector3f &);
+
+ virtual Eigen::Matrix4f toEigen() const;
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new Pose(*this);
+ }
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << toEigen();
+ return s.str();
+ }
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::Pose;
+ }
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+ /**
+ * @brief Implementation of virtual function to read a FramedOrientation from an XML-file.
+ *Example xml-layout:
+ * @code
+ * <frame>leftHand</frame>
+ * <x>0</x>
+ * <y>0</y>
+ * <z>0</z>
+ * <qw>0.45</qw>
+ * <qx>0.2</qx>
+ * <qy>0</qy>
+ * <qz>0</qz>
+ * @endcode
+
+ * @param xmlData String with xml-data. NOT a file path!
+ * @param c
+ * @return ErrorCode, 1 on Success
+ * @see Quaternion::readFromXml() for AxisAngle-Notation
+ */
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const Pose& rhs)
+ {
+ stream << "Pose: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ };
+
+ public: // serialization
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+
+ private:
+ QuaternionPtr c_orientation;
+ Vector3Ptr c_position;
+ };
+
+ typedef IceInternal::Handle<Pose> PosePtr;
+
+
+
+ /**
+ * @class FramedPose
+ * @ingroup Variants
+ * @brief The FramedPose class
+ */
+ class FramedPose :
+ virtual public FramedPoseBase,
+ virtual public Pose
+ {
+ public:
+ FramedPose();
+ FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame);
+ FramedPose(const Eigen::Matrix4f& m, const std::string& frame);
+
+ std::string getFrame() const;
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new FramedPose(*this);
+ }
+
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << toEigen() << std::endl << "frame: " << getFrame();
+ return s.str();
+ }
+
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::FramedPose;
+ }
+
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+ /**
+ * @brief Implementation of virtual function to read a FramedPose from an XML-file.
+ *Example xml-layout:
+ * @code
+ * <frame>leftHand</frame>
+ * <x>0</x>
+ * <y>0</y>
+ * <z>0</z>
+ * @endcode
+
+ * @param xmlData String with xml-data. NOT a file path!
+ * @return ErrorCode, 1 on Success
+ */
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const FramedPose& rhs)
+ {
+ stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ };
+
+ public:
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+ };
+
+ typedef IceInternal::Handle<FramedPose> FramedPosePtr;
+
+
+ /**
+ * @class FramedVector3
+ * @brief FramedVector3 is a 3 dimensional vector with a reference frame.
+ * @ingroup Variants
+ * The reference frame can be used to change the coordinate system to which
+ * the vector relates. The difference to a framed position is, that on frame
+ * changing only the orientation of the vector is changed. The translation
+ * part remains unchanged. This class is usefull e.g. for forces, torques and tcp
+ * velocities.
+ *
+ * @see Vector3, FramedPosition
+ */
+ class FramedVector3;
+ typedef IceInternal::Handle<FramedVector3> FramedVector3Ptr;
+
+ class FramedVector3 :
+ virtual public FramedVector3Base,
+ virtual public Vector3
+ {
+ public:
+ FramedVector3();
+ FramedVector3(const FramedVector3& source);
+ FramedVector3(const Eigen::Vector3f & vec, const std::string &frame );
+
+ std::string getFrame() const;
+ static FramedVector3Ptr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedVector3& framedVec, const std::string &newFrame);
+
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new FramedVector3(*this);
+ }
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << toEigen() << std::endl << "frame: " << getFrame();
+ return s.str();
+ }
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::FramedVector3;
+ }
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+ /**
+ * @brief Implementation of virtual function to read a FramedVector3 from an XML-file.
+ *Example xml-layout:
+ * @code
+ * <frame>leftHand</frame>
+ * <x>0</x>
+ * <y>0</y>
+ * <z>0</z>
+ * @endcode
+
+ * @param xmlData String with xml-data. NOT a file path!
+ * @return ErrorCode, 1 on Success
+ */
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const FramedVector3& rhs)
+ {
+ stream << "FramedVector3: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
+ public: // serialization
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+ private:
+
+ static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string &oldFrame, const std::string &newFrame, VirtualRobot::RobotPtr robotState);
+ };
+
+
+
+ /**
+ * @class FramedPosition
+ * @ingroup Variants
+ * @brief The FramedPosition class
+ */
+ class FramedPosition :
+ virtual public FramedPositionBase,
+ virtual public Vector3
+ {
+ public:
+ FramedPosition( );
+ FramedPosition(const Eigen::Vector3f &, const std::string &frame );
+ FramedPosition(const Eigen::Matrix4f &, const std::string &frame );
+ //FramedPosition(const Vector3BasePtr pos, const std::string &frame ); // this doesnt work for unknown reasons
+ std::string getFrame() const;
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new FramedPosition(*this);
+ }
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << toEigen() << std::endl << "frame: " << getFrame();
+ return s.str();
+ }
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::FramedPosition;
+ }
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+ /**
+ * @brief Implementation of virtual function to read a FramedPosition from an XML-file.
+ *Example xml-layout:
+ * @code
+ * <frame>leftHand</frame>
+ * <x>0</x>
+ * <y>0</y>
+ * <z>0</z>
+ * @endcode
+
+ * @param xmlData String with xml-data. NOT a file path!
+ * @return ErrorCode, 1 on Success
+ */
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const FramedPosition& rhs)
+ {
+ stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ };
+
+ public: // serialization
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+ };
+
+ typedef IceInternal::Handle<FramedPosition> FramedPositionPtr;
+
+
+ /**
+ * @class FramedOrientation
+ * @ingroup Variants
+ * @brief The FramedOrientation class
+ */
+ class FramedOrientation :
+ virtual public FramedOrientationBase,
+ virtual public Quaternion
+ {
+ public:
+ FramedOrientation();
+ FramedOrientation(const Eigen::Matrix4f &, const std::string &frame );
+ FramedOrientation(const Eigen::Matrix3f &, const std::string &frame );
+ // this doesnt work for an unknown reason
+ //FramedOrientation(const QuaternionBasePtr ori, const std::string &frame );
+ std::string getFrame()const ;
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new FramedOrientation(*this);
+ }
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << toEigen() << std::endl << "frame: " << getFrame();
+ return s.str();
+ }
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::FramedOrientation;
+ }
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+ /**
+ * @brief Implementation of virtual function to read a FramedOrientation from an XML-file.
+ *Example xml-layout:
+ * @code
+ * <frame>leftHand</frame>
+ * <qw>0.45</qw>
+ * <qx>0.2</qx>
+ * <qy>0</qy>
+ * <qz>0</qz>
+ * @endcode
+
+ * @param xmlData String with xml-data. NOT a file path!
+ * @param c
+ * @return ErrorCode, 1 on Success
+ */
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const FramedOrientation& rhs)
+ {
+ stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ };
+
+ public: // serialization
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+ };
+
+ typedef IceInternal::Handle<FramedOrientation> FramedOrientationPtr;
+
+
+ namespace ArmarPose
+ {
+ VirtualRobot::LinkedCoordinate createLinkedCoordinate(VirtualRobot::RobotPtr &virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation);
+ }
+}
+#endif
diff --git a/source/RobotAPI/robotstate/remote/CMakeLists.txt b/source/RobotAPI/robotstate/remote/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e2467fb00728633d76cc178e089e80d42b0f90e0
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/CMakeLists.txt
@@ -0,0 +1,37 @@
+
+armarx_set_target("Core Library: ArmarXCoreRemoteRobot")
+find_package(Eigen3 QUIET)
+if (NOT Simox_FOUND)
+ find_package(Simox 2.3.0 QUIET)
+endif()
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+include_directories(${Eigen3_INCLUDE_DIR})
+include_directories(${Simox_INCLUDE_DIRS})
+
+set(LIB_NAME RobotAPIRemoteRobot)
+set(LIB_VERSION 0.1.0)
+set(LIB_SOVERSION 0)
+
+set(LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers RobotAPIInterfaces
+ ${Simox_LIBRARIES} )
+
+set(LIB_FILES ArmarPose.cpp
+ LinkedPose.cpp
+ RobotStateObserver.cpp
+ RemoteRobot.cpp
+ RemoteRobotNode.cpp
+ checks/ConditionCheckMagnitudeChecks.cpp
+ )
+set(LIB_HEADERS ArmarPose.h
+ LinkedPose.h
+ RobotStateObserver.h
+ RemoteRobot.h
+ checks/ConditionCheckEqualsPose.h
+ checks/ConditionCheckEqualsPoseWithTolerance.h
+ checks/ConditionCheckMagnitudeChecks.h
+ RobotStateObjectFactories.h)
+
+armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/robotstate/remote/LinkedPose.cpp b/source/RobotAPI/robotstate/remote/LinkedPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..23de4ee3ea5280558a126b45779cada9c132e5c0
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/LinkedPose.cpp
@@ -0,0 +1,199 @@
+#include "LinkedPose.h"
+
+#include "RemoteRobot.h"
+#include "../SharedRobotServants.h"
+
+#include <Core/core/logging/Logging.h>
+
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+
+#include <Eigen/Geometry>
+
+
+namespace armarx {
+
+ LinkedPose::LinkedPose() :
+ Pose(),
+ FramedPose()
+ {
+ this->referenceRobot = NULL;
+ }
+
+ LinkedPose::LinkedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
+ Pose(m, v),
+ FramedPose(m, v, s)
+ {
+ referenceRobot->ref();
+ this->referenceRobot = referenceRobot;
+ }
+
+ LinkedPose::LinkedPose(const Eigen::Matrix4f& m, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
+ Pose(m),
+ FramedPose(m, s)
+ {
+ referenceRobot->ref();
+ this->referenceRobot = referenceRobot;
+ }
+
+ LinkedPose::~LinkedPose()
+ {
+ try{
+ if(referenceRobot)
+ {
+ referenceRobot->unref();
+ }
+ }
+ catch(...)
+ {
+ handleExceptions();
+ }
+ }
+
+
+ VirtualRobot::LinkedCoordinate LinkedPose::createLinkedCoordinate()
+ {
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ VirtualRobot::LinkedCoordinate c(sharedRobot);
+ std::string frame = this->getFrame();
+
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+
+ pose.block<3,3>(0,0) = QuaternionPtr::dynamicCast(orientation)->toEigen();
+ pose.block<3,1>(0,3) = Vector3Ptr::dynamicCast(position)->toEigen();
+
+ c.set(frame, pose);
+
+ return c;
+ }
+
+
+ int LinkedPose::readFromXML(const std::string &xmlData, const Ice::Current &c){
+ using namespace boost::property_tree;
+ ptree pt;
+ std::stringstream stream;
+ stream << xmlData;
+ xml_parser::read_xml(stream, pt);
+
+ Pose::readFromXML(xmlData);
+ frame = (pt.get<std::string>("frame"));
+
+ std::string remoteRobotId = pt.get<std::string>("referenceRobot");
+ referenceRobot = SharedRobotInterfacePrx::uncheckedCast(c.adapter->getCommunicator()->stringToProxy(remoteRobotId));
+ if(!referenceRobot)
+ {
+ ARMARX_ERROR_S << "ReferenceRobot for LinkedPose not registered: " << remoteRobotId << flush;
+ return 0;
+ }
+
+ return 1;
+ }
+
+ std::string LinkedPose::writeAsXML(const Ice::Current& c)
+ {
+ using namespace boost::property_tree;
+ std::stringstream stream;
+ stream << FramedPose::writeAsXML()
+ << "<referenceRobot>" << "Not serializable" <<"</referenceRobot>";
+ return stream.str();
+ }
+
+ void LinkedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ Pose::serialize(obj, c);
+ obj->setString("referenceRobot", "");
+ }
+
+ void LinkedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ FramedPose::deserialize(obj);
+
+ std::string remoteRobotId = obj->getString("referenceRobot");
+ referenceRobot = SharedRobotInterfacePrx::uncheckedCast(c.adapter->getCommunicator()->stringToProxy(remoteRobotId));
+ if(!referenceRobot)
+ {
+ ARMARX_ERROR_S << "ReferenceRobot for LinkedPose not registered: " << remoteRobotId << flush;
+ }
+ }
+
+ LinkedVector3::LinkedVector3()
+ {
+ }
+
+ LinkedVector3::LinkedVector3(const LinkedVector3 &source) :
+ IceUtil::Shared(source),
+ Vector3Base(source),
+ FramedVector3Base(source),
+ LinkedVector3Base(source),
+ Vector3(source),
+ FramedVector3(source)
+ {
+ referenceRobot = source.referenceRobot;
+ }
+
+ LinkedVector3::LinkedVector3(const Eigen::Vector3f &v, const std::string &frame, const SharedRobotInterfacePrx &referenceRobot) :
+ FramedVector3(v, frame)
+ {
+ referenceRobot->ref();
+ this->referenceRobot = referenceRobot;
+ }
+
+ LinkedVector3::~LinkedVector3()
+ {
+ try{
+ if(referenceRobot)
+ {
+ referenceRobot->unref();
+ }
+ }
+ catch(...)
+ {
+ handleExceptions();
+ }
+ }
+
+ void LinkedVector3::changeFrame(const std::string &newFrame, const Ice::Current &c)
+ {
+ if(newFrame == frame)
+ return;
+
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+
+ FramedVector3Ptr frVec = ChangeFrame(sharedRobot, *this, newFrame);
+ x = frVec->x;
+ y = frVec->y;
+ z = frVec->z;
+ frame = frVec->frame;
+ }
+
+ int LinkedVector3::readFromXML(const std::string &xmlData, const Ice::Current &c)
+ {
+ throw LocalException("LinkedVector3 cannot be serialized! Serialize FramedVector3");
+
+ }
+
+ std::string LinkedVector3::writeAsXML(const Ice::Current &c)
+ {
+ throw LocalException("LinkedVector3 cannot be deserialized! Serialize FramedVector3");
+
+ }
+
+ void LinkedVector3::serialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &) const
+ {
+ throw LocalException("LinkedVector3 cannot be serialized! Serialize FramedVector3");
+ }
+
+ void LinkedVector3::deserialize(const ObjectSerializerBasePtr &serializer, const Ice::Current &)
+ {
+ throw LocalException("LinkedVector3 cannot be deserialized! Deserialize FramedVector3");
+ }
+
+
+
+
+
+}
diff --git a/source/RobotAPI/robotstate/remote/LinkedPose.h b/source/RobotAPI/robotstate/remote/LinkedPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..107617e9e22f766ad6ad081daec7ae2bb664cfd5
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/LinkedPose.h
@@ -0,0 +1,194 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotStateComponent::
+ * @author ( stefan dot ulbrich at kit dot edu)
+ * @date
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_RobotState_LinkedPose_H
+#define _ARMARX_RobotState_LinkedPose_H
+
+#include "ArmarPose.h"
+
+#include <RobotAPI/interface/robotstate/LinkedPoseBase.h>
+#include <RobotAPI/interface/robotstate/RobotState.h>
+
+#include <Core/observers/AbstractObjectSerializer.h>
+#include <Core/observers/variant/Variant.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/LinkedCoordinate.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <sstream>
+
+
+namespace armarx
+{
+ namespace VariantType
+ {
+ // variant types
+ const VariantTypeId LinkedPose = Variant::addTypeName("::armarx::LinkedPoseBase");
+ const VariantTypeId LinkedVector3 = Variant::addTypeName("::armarx::LinkedVector3Base");
+ }
+
+
+ /**
+ * @class LinkedPose
+ * @ingroup Variants
+ * @brief The LinkedPose class
+ */
+ class LinkedPose :
+ virtual public LinkedPoseBase,
+ virtual public FramedPose
+ {
+ public:
+ LinkedPose();
+ LinkedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
+ LinkedPose(const Eigen::Matrix4f& m, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
+
+ virtual ~LinkedPose();
+
+ VirtualRobot::LinkedCoordinate createLinkedCoordinate();
+
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new LinkedPose(*this);
+ }
+
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << FramedPose::toEigen() << std::endl << "reference robot: " << referenceRobot;
+ return s.str();
+ }
+
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::LinkedPose;
+ }
+
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+ /**
+ * @brief Implementation of virtual function to read a LinkedPose from an XML-file.
+ *Example xml-layout:
+ * @code
+ * <referenceRobot>5029a22c-e333-4f87-86b1-cd5e0fcce509</referenceRobot>
+ * <frame>leftHand</frame>
+ * <x>0</x>
+ * <y>0</y>
+ * <z>0</z>
+ * @endcode
+
+ * @param xmlData String with xml-data. NOT a file path!
+ * @return ErrorCode, 1 on Success
+ */
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const LinkedPose& rhs)
+ {
+ stream << "LinkedPose: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ };
+
+ public:
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+ };
+
+ typedef IceInternal::Handle<LinkedPose> LinkedPosePtr;
+
+ /**
+ * @class LinkedVector3
+ * @ingroup Variants
+ * @brief The LinkedVector3 class
+ */
+ class LinkedVector3 :
+ virtual public LinkedVector3Base,
+ virtual public FramedVector3
+ {
+ public:
+ LinkedVector3();
+ LinkedVector3(const LinkedVector3& source);
+ LinkedVector3(const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
+
+ virtual ~LinkedVector3();
+
+ void changeFrame(const std::string &newFrame, const Ice::Current &c = Ice::Current());
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return new LinkedVector3(*this);
+ }
+
+ std::string output(const Ice::Current& c = ::Ice::Current()) const
+ {
+ std::stringstream s;
+ s << FramedVector3::toEigen() << std::endl << "reference robot: " << referenceRobot;
+ return s.str();
+ }
+
+ VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const
+ {
+ return VariantType::LinkedVector3;
+ }
+
+ bool validate(const Ice::Current& c = ::Ice::Current())
+ {
+ return true;
+ }
+
+
+ int readFromXML(const std::string &xmlData, const Ice::Current &c = ::Ice::Current());
+ std::string writeAsXML(const Ice::Current &c = ::Ice::Current());
+
+ friend std::ostream &operator<<(std::ostream &stream, const LinkedVector3& rhs)
+ {
+ stream << "LinkedVector3: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ };
+
+ virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
+ virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+
+ };
+ typedef IceInternal::Handle<LinkedVector3> LinkedVector3Ptr;
+}
+
+#endif
diff --git a/source/RobotAPI/robotstate/remote/RemoteRobot.cpp b/source/RobotAPI/robotstate/remote/RemoteRobot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53302b488e0d5371e370134fa7902bdc1ac64d06
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/RemoteRobot.cpp
@@ -0,0 +1,364 @@
+#include "RemoteRobot.h"
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
+#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
+#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
+#include <Eigen/Geometry>
+
+//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj << endl;
+#define DMES(Obj) ;
+
+using namespace std;
+using namespace boost;
+using namespace VirtualRobot;
+using namespace Eigen;
+
+namespace armarx{
+
+RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
+ Robot(),
+ _robot(robot)
+{
+ _robot->ref();
+}
+
+RemoteRobot::~RemoteRobot()
+{
+ _robot->unref();
+}
+
+/*Matrix4f RemoteRobot::Pose2Matrix4f(PosePtr p){
+ Matrix4f m = Matrix4f::Identity();
+ Matrix3f rot; // Matrix3f rot = Quaternionf .... does NOT work!
+ rot =Quaternionf(p->orientation->qw,p->orientation->qx,p->orientation->qy,p->orientation->qz);
+ m.block<3,3>(0,0) = rot;
+ m.block<3,1>(0,3) << p->position->x,p->position->y,p->position->z;
+ return m;
+}*/
+
+RobotNodePtr RemoteRobot::getRootNode()
+{
+ if (!_root)
+ _root = RemoteRobot::createRemoteRobotNode(_robot->getRootNode(),shared_from_this());
+ return _root;
+}
+
+bool RemoteRobot::hasRobotNode( const string &robotNodeName )
+{
+ if (_cachedNodes.find(name)==_cachedNodes.end())
+ return _robot->hasRobotNode(robotNodeName);
+ else
+ return true;
+}
+
+
+bool RemoteRobot::hasRobotNode( RobotNodePtr robotNode )
+{
+ return this->hasRobotNode(robotNode->getName());
+
+ /*
+ * This just does nost work. because you cannot tell wheter RemoteRobotNode<RobotNodeRevolute> or another type is used
+ * perhaps you can infer the actual RobotNodeType somehow. Until now we just check for names which is
+ * much faster!
+ *
+ if ( (_cachedNodes.find(name)==_cachedNodes.end()) || _robot->hasRobotNode(robotNode->getName())) {
+ shared_ptr<RemoteRobotNode> remoteNode(dynamic_pointer_cast<RemoteRobotNode>(robotNode));
+ if (! remoteNode) return false;
+
+ SharedRobotNodeInterfacePrx sharedNode = remoteNode->getSharedNode();
+ SharedRobotNodeInterfacePrx otherSharedNode = dynamic_pointer_cast<RemoteRobotNode>(this->getRobotNode(robotNodeName))->getSharedNode();
+ if (sharedNode == otherSharedNode)
+ return true;
+ }
+
+ return false;
+ */
+}
+
+
+RobotNodePtr RemoteRobot::getRobotNode(const string &robotNodeName)
+{
+ DMES((format("Node: %1%") % robotNodeName));
+
+ if (_cachedNodes.find(robotNodeName)==_cachedNodes.end())
+ {
+ DMES("No cache hit");
+ _cachedNodes[robotNodeName]=RemoteRobot::createRemoteRobotNode(_robot->getRobotNode(robotNodeName),
+ shared_from_this());
+ }
+ else
+ {
+ DMES("Cache hit");
+ }
+ return _cachedNodes[robotNodeName];
+}
+
+void RemoteRobot::getRobotNodes(vector< RobotNodePtr > &storeNodes, bool clearVector)
+{
+ if (clearVector) storeNodes.clear();
+ NameList nodes = _robot->getRobotNodes();
+ BOOST_FOREACH(string name, nodes){
+ storeNodes.push_back(this->getRobotNode(name));
+ }
+}
+
+bool RemoteRobot::hasRobotNodeSet( const string &name )
+{
+ return _robot->hasRobotNodeSet(name);
+}
+
+RobotNodeSetPtr RemoteRobot::getRobotNodeSet(const string &nodeSetName)
+{
+ vector<RobotNodePtr> storeNodes;
+ RobotNodeSetInfoPtr info = _robot->getRobotNodeSet(nodeSetName);
+ return RobotNodeSet::createRobotNodeSet(
+ shared_from_this(),nodeSetName, info->names, info->rootName, info->tcpName);
+}
+
+
+void RemoteRobot::getRobotNodeSets(vector<RobotNodeSetPtr> &storeNodeSet)
+{
+ NameList sets= _robot->getRobotNodeSets();
+
+ BOOST_FOREACH(string name, sets){
+ storeNodeSet.push_back(this->getRobotNodeSet(name));
+ }
+}
+
+Matrix4f RemoteRobot::getGlobalPose()
+{
+ PosePtr p = PosePtr::dynamicCast(_robot->getGlobalPose());
+ return p->toEigen(); // convert to eigen first
+}
+
+string RemoteRobot::getName()
+{
+ return _robot->getName();
+}
+
+VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, RobotPtr robo)
+{
+ static int nonameCounter = 0;
+ if (!remoteNode || !robo)
+ {
+ ARMARX_ERROR_S << " NULL data " << endl;
+ return VirtualRobot::RobotNodePtr();
+ }
+ VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
+ VirtualRobot::RobotNodeFactoryPtr fixedNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeFixedFactory::getName(), NULL);
+ VirtualRobot::RobotNodeFactoryPtr prismaticNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodePrismaticFactory::getName(), NULL);
+
+ Eigen::Matrix4f idMatrix = Eigen::Matrix4f::Identity();
+ Eigen::Vector3f idVec3 = Eigen::Vector3f::Zero();
+ std::string name = remoteNode->getName();
+ if (name.empty())
+ {
+ ARMARX_LOG_S << "Node without name!!!" << endl;
+ std::stringstream ss;
+ ss << "robot_node_" << nonameCounter;
+ nonameCounter++;
+ name = ss.str();
+ }
+
+ VirtualRobot::RobotNodePtr result;
+ PosePtr lTbase = PosePtr::dynamicCast(remoteNode->getLocalTransformation());
+ Eigen::Matrix4f localTransform = lTbase->toEigen();
+
+ //float jv = remoteNode->getJointValue();
+ float jvLo = remoteNode->getJointLimitLow();
+ float jvHi = remoteNode->getJointLimitHigh();
+ float jointOffset = 0;//remoteNode->getJointOffset();
+
+ JointType jt = remoteNode->getType();
+ switch (jt)
+ {
+ case ePrismatic:
+ {
+ Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointTranslationDirection());
+ Eigen::Vector3f axis = axisBase->toEigen();
+ // convert axis to local coord system
+ Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
+ result4f.segment(0,3) = axis;
+ PosePtr gp = PosePtr::dynamicCast(remoteNode->getGlobalPose());
+ result4f = gp->toEigen().inverse() * result4f;
+ axis = result4f.block(0,0,3,1);
+
+ result = prismaticNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
+ jvLo, jvHi, jointOffset, idMatrix, idVec3, axis);
+ }
+ break;
+ case eFixed:
+ result = fixedNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(), 0,
+ 0, 0, localTransform, idVec3, idVec3);
+ break;
+ case eRevolute:
+ {
+ Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointRotationAxis());
+ Eigen::Vector3f axis = axisBase->toEigen();
+ // convert axis to local coord system
+ Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
+ result4f.segment(0,3) = axis;
+ PosePtr gp = PosePtr::dynamicCast(remoteNode->getGlobalPose());
+ result4f = gp->toEigen().inverse() * result4f;
+ axis = result4f.block(0,0,3,1);
+
+ result = revoluteNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
+ jvLo, jvHi, jointOffset, localTransform, axis, idVec3);
+ }
+ break;
+ default:
+ ARMARX_ERROR_S << "JointType nyi..." << endl;
+ return VirtualRobot::RobotNodePtr();
+ break;
+ }
+ robo->registerRobotNode(result);
+ allNodes.push_back(result);
+
+
+ // setup joint->nextNodes children
+ std::vector<std::string> childrenBase = remoteNode->getChildren();
+ std::vector<std::string> children;
+
+ // check for RobotNodes (sensors do also register as children!)
+ for (size_t i=0;i<childrenBase.size();i++)
+ {
+ if (_robot->hasRobotNode(childrenBase[i]))
+ {
+ SharedRobotNodeInterfacePrx rnRemote = _robot->getRobotNode(childrenBase[i]);
+ VirtualRobot::RobotNodePtr localNode = createLocalNode(rnRemote, allNodes, childrenMap, robo);
+ /* boost::shared_ptr< RemoteRobotNode<VirtualRobotNodeType> > rnRemote = boost::dynamic_pointer_cast<RemoteRobotNode>(rnRemoteBase);
+ if (!rnRemote)
+ {
+ ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i] << endl;
+ continue;
+ }*/
+
+
+ if (!localNode)
+ {
+ ARMARX_ERROR_S << "Could not create local node: " << children[i] << endl;
+ continue;
+ }
+ children.push_back(childrenBase[i]);
+ }
+ }
+ childrenMap[result] = children;
+ return result;
+}
+
+VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
+{
+ std::string robotType = getName();
+ std::string robotName = getName();
+ VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName,robotType));
+
+ //RobotNodePtr
+ SharedRobotNodeInterfacePrx root = _robot->getRootNode();
+
+
+ std::vector<VirtualRobot::RobotNodePtr> allNodes;
+ std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > childrenMap;
+
+ VirtualRobot::RobotNodePtr rootLocal = createLocalNode(root, allNodes, childrenMap, robo);
+
+ bool res = VirtualRobot::RobotFactory::initializeRobot(robo, allNodes, childrenMap, rootLocal);
+ if (!res)
+ {
+ ARMARX_ERROR_S << "Failed to initialize local robot..." << endl;
+ return VirtualRobot::RobotPtr();
+ }
+
+ // clone rns
+ std::vector<std::string> rns = _robot->getRobotNodeSets();
+ for (size_t i=0;i<rns.size();i++)
+ {
+ RobotNodeSetInfoPtr rnsInfo = _robot->getRobotNodeSet(rns[i]);
+ RobotNodeSet::createRobotNodeSet(robo,rnsInfo->name,rnsInfo->names,rnsInfo->rootName,rnsInfo->tcpName,true);
+ }
+
+ return robo;
+}
+
+VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string &filename)
+{
+ ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
+ VirtualRobot::RobotPtr result;
+
+ if (!robotStatePrx)
+ {
+ ARMARX_ERROR_S << "NULL robotStatePrx. Aborting..." << endl;
+ return result;
+ }
+
+ if (filename.empty())
+ {
+ RemoteRobotPtr rob(new RemoteRobot(robotStatePrx->getSynchronizedRobot()));
+ result = rob->createLocalClone();
+ if (!result)
+ {
+ ARMARX_ERROR_S << "Could not create local clone. Aborting..." << endl;
+ return result;
+ }
+ } else
+ {
+ result = RobotIO::loadRobot(filename);
+ if (!result)
+ {
+ ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
+ return result;
+ }
+ }
+ synchronizeLocalClone(result,robotStatePrx);
+ return result;
+}
+
+bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
+{
+ if (!robotStatePrx || !robot)
+ {
+ ARMARX_ERROR_S << "NULL data. Aborting..." << endl;
+ return false;
+ }
+ RobotConfigPtr c(new RobotConfig(robot,"synchronizeLocalClone"));
+ NameValueMap jv = robotStatePrx->getSynchronizedRobot()->getConfig();
+ for ( NameValueMap::const_iterator it = jv.begin(); it!=jv.end(); it++ )
+ {
+ // joint values
+ std::string jointName = it->first;
+ float jointAngle = it->second;
+ if (!c->setConfig(jointName,jointAngle))
+ {
+ ARMARX_WARNING_S << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+ }
+ }
+ robot->setConfig(c);
+ return true;
+}
+
+
+// Private (unused methods)
+
+bool RemoteRobot::hasEndEffector(const string& endEffectorName)
+{
+ return false;
+}
+
+EndEffectorPtr RemoteRobot::getEndEffector(const string& endEffectorName)
+{
+ return EndEffectorPtr();
+}
+
+void RemoteRobot::getEndEffectors(vector<EndEffectorPtr> &storeEEF){}
+void RemoteRobot::setRootNode(RobotNodePtr node){}
+void RemoteRobot::registerRobotNode(RobotNodePtr node){}
+void RemoteRobot::deregisterRobotNode(RobotNodePtr node){}
+void RemoteRobot::registerRobotNodeSet(RobotNodeSetPtr nodeSet){}
+void RemoteRobot::deregisterRobotNodeSet(RobotNodeSetPtr nodeSet){}
+void RemoteRobot::registerEndEffector(EndEffectorPtr endEffector){}
+void RemoteRobot::setGlobalPose(const Eigen::Matrix4f &globalPose, bool applyValues){}
+
+}
diff --git a/source/RobotAPI/robotstate/remote/RemoteRobot.h b/source/RobotAPI/robotstate/remote/RemoteRobot.h
new file mode 100644
index 0000000000000000000000000000000000000000..b0b6341107b10cd81c79bba011671a12bb235ae1
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/RemoteRobot.h
@@ -0,0 +1,232 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ExampleClient::
+ * @author (stefan dot ulbrich at kit dot edu)
+ * @date
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_REMOTE_ROBOT_H__
+#define _ARMARX_REMOTE_ROBOT_H__
+
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
+#include <VirtualRobot/Nodes/RobotNodeFixed.h>
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include "ArmarPose.h"
+
+#include <RobotAPI/interface/robotstate/RobotState.h>
+
+// boost
+#include <boost/utility/enable_if.hpp>
+#include <boost/type_traits/is_base_of.hpp>
+
+namespace armarx
+{
+ // forward declaration of RemoteRobotNode
+ template<class VirtualRobotNodeType> class RemoteRobotNode;
+
+ /** @brief RemoteRobotNodeInitializer is used to initialize the robot node for a given node type.
+ * For each robot type to be supported make a specialization of the initialize function.
+ * Currently supports: RobotNodeRevolute, RobotNodePrismatic, RobotNodeFixed. Node type specific
+ * initializations go here.
+ */
+ template<typename VirtualRobotNodeType>
+ struct RemoteRobotNodeInitializer
+ {
+ static void initialize(RemoteRobotNode<VirtualRobotNodeType>* remoteNode);
+ };
+
+ // specializations
+ template<>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode);
+ template<>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode);
+ template<>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode);
+
+
+ /** @brief Mimics the behaviour of robot nodes while redirecting everything to Ice proxies.
+ * @tparam VirtualRobotNodeType Must be a descendant of VirtualRobot::RobotNode
+ * @details This class is for <b> internal use only</b> as classes cannot be referenced!
+ */
+ template<class VirtualRobotNodeType>
+ class RemoteRobotNode :
+ public VirtualRobotNodeType
+ {
+ friend struct RemoteRobotNodeInitializer<VirtualRobotNodeType>;
+
+ public:
+ RemoteRobotNode(SharedRobotNodeInterfacePrx node, VirtualRobot::RobotPtr vr) :
+ _node(node)
+ {
+ _node->ref();
+ this->name=_node->getName();
+ this->robot = vr;
+ _node->getJointValueOffest();
+ setJointLimits(_node->getJointLimitLow(), _node->getJointLimitHigh());
+
+ this->collisionChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
+
+ RemoteRobotNodeInitializer<VirtualRobotNodeType>::initialize(this);
+ }
+
+ virtual ~RemoteRobotNode();
+
+ virtual float getJointValue() const;
+ virtual float getJointLimitHi() const;
+ virtual float getJointLimitLo() const;
+
+ //virtual Eigen::Matrix4f getPostJointTransformation();
+ virtual Eigen::Matrix4f getLocalTransformation();
+
+ virtual Eigen::Matrix4f getGlobalPose() const;
+ virtual Eigen::Matrix4f getGlobalPoseVisualization() const;
+ virtual Eigen::Matrix4f getGlobalPoseJoint() const;
+ virtual bool hasChildNode(const std::string &child, bool recursive = false) const;
+
+ virtual std::vector<VirtualRobot::RobotNodePtr> getAllParents( VirtualRobot::RobotNodeSetPtr rns );
+ virtual VirtualRobot::RobotNodePtr getParent();
+ inline SharedRobotNodeInterfacePrx getSharedRobotNode()
+ {
+ return _node;
+ }
+
+ virtual std::vector<std::string> getChildrenNames() const;
+
+ protected:
+ ///////////////////////// SETUP ////////////////////////////////////
+ virtual void setJointLimits(float lo, float hi);
+ //virtual void setPostJointTransformation(const Eigen::Matrix4f &trafo);
+ virtual void setLocalTransformation(const Eigen::Matrix4f &trafo);
+
+ virtual std::string getParentName() const;
+ virtual std::vector< VirtualRobot::SceneObjectPtr> getChildren() const;
+
+ virtual void updateTransformationMatrices();
+ virtual void updateTransformationMatrices(const Eigen::Matrix4f &globalPose);
+
+
+ virtual bool hasChildNode(const VirtualRobot::RobotNodePtr child, bool recursive = false) const;
+ virtual void addChildNode(VirtualRobot::RobotNodePtr child);
+ virtual bool initialize(VirtualRobot::RobotNodePtr parent, bool initializeChildren = false);
+ virtual void reset();
+ virtual void setGlobalPose( const Eigen::Matrix4f &pose );
+ virtual void setJointValue(float q, bool updateTransformations = true, bool clampToLimits = true);
+
+ SharedRobotNodeInterfacePrx _node;
+ };
+
+ /** @brief Mimics the behaviour of the VirtualRobot::Robot class while redirecting everything to an Ice proxy.
+ * @details For a description of the members please refer to the Simox documentation (VirtualRobot::Robot).
+ * Note that not the complete interface has been made available yet. These functions are marked as protected.
+ */
+ class RemoteRobot : public VirtualRobot::Robot
+ {
+ public:
+ RemoteRobot (SharedRobotInterfacePrx robot);
+ ~RemoteRobot();
+
+ static Eigen::Matrix4f Pose2Matrix4f(PosePtr p);
+
+ virtual VirtualRobot::RobotNodePtr getRootNode();
+
+ virtual bool hasRobotNode( const std::string &robotNodeName );
+ virtual bool hasRobotNode( VirtualRobot::RobotNodePtr );
+
+ virtual VirtualRobot::RobotNodePtr getRobotNode(const std::string &robotNodeName);
+ virtual void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr > &storeNodes, bool clearVector=true);
+
+ virtual bool hasRobotNodeSet( const std::string &name );
+ virtual VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string &nodeSetName);
+ virtual void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr> &storeNodeSet);
+
+ virtual Eigen::Matrix4f getGlobalPose();
+
+
+
+ /// Use this method to share the robot instance over Ice.
+ inline SharedRobotInterfacePrx getSharedRobot()
+ {
+ return this->_robot;
+ }
+
+ std::string getName();
+
+ /*!
+ Creates a local robot that is synchronized once but not updated any more. You can use the synchronizeLocalClone method to update the joint values.
+ Note that only the kinematic structure is cloned, but the visualization files, collision mdoels, end effectors, etc are not copied.
+ This means you can use this model for kinematic computations (e.g. coordinate transformations) but not for advanced features like collision detection.
+ In order to get a fully featured robot model you can pass a filename to VirtualRobot::Robot,
+ but then you need to make sure that the loaded model is identical to the model of the remote robot (otherwise errors will occur).
+ */
+ static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string &filename=std::string());
+
+ /*!
+ Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.
+ The local clone must have the identical structure as the remote robot model, otherwise an error will be reported.
+ */
+ static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx);
+
+ // VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging
+
+ //! Clones the structure of this remote robot to a local instance
+ VirtualRobot::RobotPtr createLocalClone();
+ protected:
+
+ /// Not implemented yet
+ virtual bool hasEndEffector(const std::string& endEffectorName);
+ /// Not implemented yet
+ virtual VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName);
+ /// Not implemented yet
+ virtual void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr> &storeEEF);
+
+ /// Not implemented yet
+ virtual void setRootNode(VirtualRobot::RobotNodePtr node);
+ /// Not implemented yet
+ virtual void registerRobotNode(VirtualRobot::RobotNodePtr node);
+ /// Not implemented yet
+ virtual void deregisterRobotNode(VirtualRobot::RobotNodePtr node);
+ /// Not implemented yet
+ virtual void registerRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet);
+ /// Not implemented yet
+ virtual void deregisterRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet);
+ /// Not implemented yet
+ virtual void registerEndEffector(VirtualRobot::EndEffectorPtr endEffector);
+ /// Not implemented yet
+ virtual void setGlobalPose(const Eigen::Matrix4f &globalPose, bool applyValues = true);
+
+ VirtualRobot::RobotNodePtr createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr> &allNodes, std::map<VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, VirtualRobot::RobotPtr robo);
+
+ protected:
+ SharedRobotInterfacePrx _robot;
+ std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes;
+ VirtualRobot::RobotNodePtr _root;
+
+ static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr);
+ };
+
+ typedef boost::shared_ptr<RemoteRobot> RemoteRobotPtr;
+}
+
+#endif
diff --git a/source/RobotAPI/robotstate/remote/RemoteRobotNode.cpp b/source/RobotAPI/robotstate/remote/RemoteRobotNode.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e597d92453376cf4264aefb8ca97122d472753f8
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/RemoteRobotNode.cpp
@@ -0,0 +1,179 @@
+#include "RemoteRobot.h"
+#include <boost/foreach.hpp>
+#include <boost/weak_ptr.hpp>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <Core/interface/core/BasicTypes.h>
+
+namespace armarx
+{
+
+using namespace std;
+using namespace boost;
+using namespace VirtualRobot;
+using namespace Eigen;
+
+RobotNodePtr RemoteRobot::createRemoteRobotNode(SharedRobotNodeInterfacePrx node, RobotPtr robot)
+{
+ switch (node->getType()) {
+ case ePrismatic:
+ return RobotNodePtr(new RemoteRobotNode<RobotNodePrismatic>(node,robot));
+ case eRevolute:
+ return RobotNodePtr(new RemoteRobotNode<RobotNodeRevolute>(node,robot));
+ case eFixed:
+ return RobotNodePtr(new RemoteRobotNode<RobotNodeFixed>(node,robot));
+ default:
+ break; // thow an exception
+ }
+ return RobotNodePtr();
+}
+
+
+template<>
+void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode)
+{
+ // set rotation axis
+ remoteNode->jointRotationAxis = remoteNode->getGlobalPoseJoint().block<3,3>(0,0).inverse() * Vector3Ptr::dynamicCast(remoteNode->_node->getJointRotationAxis())->toEigen();
+}
+
+template<>
+void RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode)
+{
+ // set translation direction
+ remoteNode->jointTranslationDirection = remoteNode->getGlobalPose().block<3,3>(0,0).inverse() * Vector3Ptr::dynamicCast(remoteNode->_node->getJointTranslationDirection())->toEigen();
+}
+
+template<>
+void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode)
+{
+ // nothing to do for fixed joints
+}
+
+
+template<class RobotNodeType>
+RemoteRobotNode<RobotNodeType>::~RemoteRobotNode(){
+ _node->unref();
+}
+
+template<class RobotNodeType>
+float RemoteRobotNode<RobotNodeType>::getJointValue() const{
+ return _node->getJointValue();
+}
+
+template<class RobotNodeType>
+float RemoteRobotNode<RobotNodeType>::getJointLimitHi() const
+{
+ return _node->getJointLimitHigh();
+}
+template<class RobotNodeType>
+float RemoteRobotNode<RobotNodeType>::getJointLimitLo() const
+{
+ return _node->getJointLimitLow();
+}
+/*
+template<class RobotNodeType>
+Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getPostJointTransformation(){
+ return PosePtr::dynamicCast(_node->getPostJointTransformation())->toEigen();
+}*/
+
+template<class RobotNodeType>
+Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getLocalTransformation(){
+ return PosePtr::dynamicCast(_node->getLocalTransformation())->toEigen();
+}
+
+template<class RobotNodeType>
+Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getGlobalPose() const {
+ return PosePtr::dynamicCast(_node->getGlobalPose())->toEigen();
+}
+
+template<class RobotNodeType>
+Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getGlobalPoseVisualization() const {
+ return PosePtr::dynamicCast(_node->getGlobalPose())->toEigen();
+}
+template<class RobotNodeType>
+Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getGlobalPoseJoint() const {
+ return PosePtr::dynamicCast(_node->getGlobalPoseJoint())->toEigen();
+}
+
+template<class RobotNodeType>
+bool RemoteRobotNode<RobotNodeType>::hasChildNode(const RobotNodePtr child, bool recursive) const{
+ return false;
+}
+template<class RobotNodeType>
+bool RemoteRobotNode<RobotNodeType>::hasChildNode(const std::string &child, bool recursive) const{
+ return _node->hasChild(child,recursive);
+}
+
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::setJointLimits(float lo, float hi){
+}
+
+template<class RobotNodeType>
+vector<RobotNodePtr> RemoteRobotNode<RobotNodeType>::getAllParents( RobotNodeSetPtr rns ){
+ NameList nodes = _node->getAllParents(rns->getName());
+ vector<RobotNodePtr> result;
+ RobotPtr robot = this->robot.lock();
+
+ BOOST_FOREACH(string name, nodes){
+ result.push_back(robot->getRobotNode(name));
+ }
+ return result;
+}
+
+template<class RobotNodeType>
+RobotNodePtr RemoteRobotNode<RobotNodeType>::getParent(){
+ return this->robot.lock()->getRobotNode(_node->getParent());
+}
+/*
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::setPostJointTransformation(const Eigen::Matrix4f &trafo){
+}*/
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::setLocalTransformation(const Eigen::Matrix4f &trafo){
+}
+template<class RobotNodeType>
+std::vector<std::string> RemoteRobotNode<RobotNodeType>::getChildrenNames() const{
+ return _node->getChildren();
+}
+template<class RobotNodeType>
+std::string RemoteRobotNode<RobotNodeType>::getParentName() const{
+ return _node->getParent();
+}
+template<class RobotNodeType>
+std::vector<SceneObjectPtr> RemoteRobotNode<RobotNodeType>::getChildren() const{
+ NameList nodes = _node->getChildren();
+ vector<SceneObjectPtr> result;
+ BOOST_FOREACH(string name, nodes){
+ result.push_back(this->robot.lock()->getRobotNode(name));
+ }
+ return result;
+}
+
+
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::updateTransformationMatrices(){
+}
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::updateTransformationMatrices(const Eigen::Matrix4f &globalPose){
+}
+
+
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::addChildNode(RobotNodePtr child){
+}
+template<class RobotNodeType>
+bool RemoteRobotNode<RobotNodeType>::initialize(RobotNodePtr parent, bool initializeChildren){
+ return false;
+}
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::reset(){
+}
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::setGlobalPose( const Eigen::Matrix4f &pose ){
+}
+template<class RobotNodeType>
+void RemoteRobotNode<RobotNodeType>::setJointValue(float q, bool updateTransformations, bool clampToLimits){
+}
+
+}
diff --git a/source/RobotAPI/robotstate/remote/RobotStateObjectFactories.h b/source/RobotAPI/robotstate/remote/RobotStateObjectFactories.h
new file mode 100644
index 0000000000000000000000000000000000000000..75fd46891106e162941b6b8c0ddf155e94711d10
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/RobotStateObjectFactories.h
@@ -0,0 +1,178 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Kai Welke (welke _at_ kit _dot_ edu)
+* @date 2011 Kai Welke
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_ROBOTSTATE_OBJECT_FACTORIES_H
+#define _ARMARX_CORE_ROBOTSTATE_OBJECT_FACTORIES_H
+
+#include <Core/core/system/FactoryCollectionBase.h>
+#include <RobotAPI/interface/robotstate/PoseBase.h>
+#include <RobotAPI/interface/robotstate/RobotState.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/interface/robotstate/LinkedPoseBase.h>
+#include <RobotAPI/robotstate/remote/LinkedPose.h>
+#include <Ice/Ice.h>
+
+namespace armarx
+{
+ class QuaternionObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::QuaternionBase::ice_staticId());
+ return new Quaternion();
+ }
+ virtual void destroy()
+ {}
+ };
+
+ class Vector3ObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::Vector3::ice_staticId());
+ return new Vector3();
+ }
+ virtual void destroy()
+ {}
+ };
+
+ class FramedVector3ObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::FramedVector3Base::ice_staticId());
+ return new FramedVector3();
+ }
+ virtual void destroy()
+ {}
+ };
+
+ class LinkedVector3ObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::LinkedVector3Base::ice_staticId());
+ return new LinkedVector3();
+ }
+ virtual void destroy()
+ {}
+ };
+
+ class PoseObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::PoseBase::ice_staticId());
+ return new Pose();
+ }
+ virtual void destroy()
+ {}
+ };
+
+ class FramedPoseObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::FramedPoseBase::ice_staticId());
+ return new FramedPose();
+ }
+ virtual void destroy()
+ {}
+ };
+
+ class FramedPositionObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::FramedPositionBase::ice_staticId());
+ return new FramedPosition();
+ }
+ virtual void destroy()
+ {}
+ };
+
+ class FramedOrientationObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::FramedOrientationBase::ice_staticId());
+ return new FramedOrientation();
+ }
+ virtual void destroy()
+ {}
+ };
+
+ class LinkedPoseObjectFactory : public Ice::ObjectFactory
+ {
+ public:
+ virtual Ice::ObjectPtr create(const std::string& type)
+ {
+ assert(type == armarx::LinkedPoseBase::ice_staticId());
+ return new LinkedPose();
+ }
+ virtual void destroy()
+ {}
+ };
+
+
+
+ namespace ObjectFactories
+ {
+
+ /**
+ * @class RobotStateObjectFactories
+ */
+ class RobotStateObjectFactories : public FactoryCollectionBase
+ {
+ public:
+ ObjectFactoryMap getFactories()
+ {
+ ObjectFactoryMap map;
+
+ map.insert(std::make_pair(armarx::Vector3Base::ice_staticId(), new Vector3ObjectFactory));
+ map.insert(std::make_pair(armarx::FramedVector3Base::ice_staticId(), new FramedVector3ObjectFactory));
+ map.insert(std::make_pair(armarx::LinkedVector3Base::ice_staticId(), new LinkedVector3ObjectFactory));
+ map.insert(std::make_pair(armarx::QuaternionBase::ice_staticId(), new QuaternionObjectFactory));
+ map.insert(std::make_pair(armarx::PoseBase::ice_staticId(), new PoseObjectFactory));
+ map.insert(std::make_pair(armarx::FramedPoseBase::ice_staticId(), new FramedPoseObjectFactory));
+ map.insert(std::make_pair(armarx::FramedOrientationBase::ice_staticId(), new FramedOrientationObjectFactory));
+ map.insert(std::make_pair(armarx::FramedPositionBase::ice_staticId(), new FramedPositionObjectFactory));
+ map.insert(std::make_pair(armarx::LinkedPoseBase::ice_staticId(), new LinkedPoseObjectFactory));
+
+ return map;
+ }
+ };
+ const FactoryCollectionBaseCleanUp RobotStateObjectFactoriesVar = FactoryCollectionBase::addToPreregistration(new RobotStateObjectFactories());
+
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/robotstate/remote/RobotStateObserver.cpp b/source/RobotAPI/robotstate/remote/RobotStateObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f7de0260562933f47fd3c9853015b0170b84b77d
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/RobotStateObserver.cpp
@@ -0,0 +1,175 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Stefan Ulbrich <stefan dot ulbrich at kit dot edu>, Kai Welke (welke _at_ kit _dot_ edu)
+* @date 2011 Kai Welke
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "RobotStateObserver.h"
+#include <RobotAPI/robotstate/RobotStateComponent.h>
+#include <Core/observers/checks/ConditionCheckEquals.h>
+#include <Core/observers/checks/ConditionCheckInRange.h>
+#include <Core/observers/checks/ConditionCheckLarger.h>
+#include <Core/observers/checks/ConditionCheckSmaller.h>
+
+#include <RobotAPI/robotstate/remote/RemoteRobot.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+
+
+using namespace armarx;
+
+// ********************************************************************
+// observer framework hooks
+// ********************************************************************
+void RobotStateObserver::onInitObserver()
+{
+ // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
+ // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
+ std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+
+ this->server = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
+ this->robot.reset(new RemoteRobot(server->getSynchronizedRobot()));
+
+ if(robotNodeSetName == "")
+ {
+ throw UserException("RobotNodeSet not defined");
+ }
+ VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = this->robot->getRobotNodeSet(robotNodeSetName);
+
+ std::vector< VirtualRobot::RobotNodePtr > robotNodes;
+ robotNodes = robotNodeSetPtr->getAllRobotNodes();
+
+
+ // register all channels
+ offerChannel("positions","Link positions of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("orientations","Link orientations of the" + robotNodeSetName + "kinematic chain");
+
+
+ // register all data fields
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ {
+ std::string jointName = (*it)->getName();
+ std::cout << "* " << jointName << std::endl;
+ offerDataField("positions", jointName, VariantType::FramedPosition, "" + jointName + "");
+ offerDataField("orientations", jointName, VariantType::FramedOrientation, "" + jointName + "");
+
+ // create lookup map
+ namesToNodes.insert(make_pair(jointName, *it));
+ }
+
+ // register all checks
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("inrange", new ConditionCheckInRange());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+ usingTopic(robotNodeSetName + "State");
+
+}
+
+void RobotStateObserver::onConnectObserver()
+{
+}
+
+// ********************************************************************
+// KinematicUnitListener interface implementation
+// ********************************************************************
+void RobotStateObserver::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c)
+{
+}
+
+void RobotStateObserver::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c)
+{
+ if(!aValueChanged)
+ return;
+ // update current model
+ updateRobotModel(jointAngles);
+
+ // update datafields
+ updatePoses();
+
+ // update channels
+ updateChannel("positions");
+ updateChannel("orientations");
+}
+
+void RobotStateObserver::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c)
+{
+}
+
+void RobotStateObserver::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c)
+{
+}
+
+void RobotStateObserver::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c)
+{
+}
+
+void RobotStateObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c)
+{
+}
+
+void RobotStateObserver::reportJointStatuses(const NameStatusMap &jointStatuses, bool aValueChanged, const Ice::Current &c)
+{
+}
+
+// ********************************************************************
+// private methods
+// ********************************************************************
+void RobotStateObserver::updateRobotModel(const NameValueMap& nameValueMap)
+{
+ // update all nodes contained in the valueMap
+ NameValueMap::const_iterator iter = nameValueMap.begin();
+
+ while(iter != nameValueMap.end())
+ {
+ VirtualRobot::RobotNodePtr robotNode = namesToNodes[iter->first];
+ robotNode->setJointValue(iter->second);
+
+ iter++;
+ }
+}
+
+void RobotStateObserver::updatePoses()
+{
+ // go through all nodes
+ VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = this->robot->getRobotNodeSet(robotNodeSetName);
+
+ std::vector< VirtualRobot::RobotNodePtr > robotNodes;
+ robotNodes = robotNodeSetPtr->getAllRobotNodes();
+
+ // set all data fields
+ // TODO: maybe global pose is not correct!!!
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ {
+ std::string jointName = (*it)->getName();
+ setDataField("positions", jointName, FramedPositionPtr(new FramedPosition((*it)->getGlobalPose(), "global")));
+ setDataField("orientations", jointName, FramedOrientationPtr(new FramedOrientation((*it)->getGlobalPose(), "global")));
+ }
+}
+
+PropertyDefinitionsPtr RobotStateObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new RobotStatePropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/robotstate/remote/RobotStateObserver.h b/source/RobotAPI/robotstate/remote/RobotStateObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..16c6698ee09d2501d4db4492b9de1f9f499dffff
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/RobotStateObserver.h
@@ -0,0 +1,87 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Stefan Ulbrich <stefan dot ulbrich at kit dot edu>, Kai Welke (welke _at_ kit _dot_ edu)
+* @date 2011 Kai Welke
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_KINEMATIC_UNIT_OBSERVER_H
+#define _ARMARX_CORE_KINEMATIC_UNIT_OBSERVER_H
+
+#include <Core/core/system/ImportExport.h>
+#include <Core/core/Component.h>
+#include <Core/interface/observers/VariantBase.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/robotstate/RobotStateObserverInterface.h>
+#include <RobotAPI/interface/robotstate/RobotState.h>
+#include <Core/observers/ConditionCheck.h>
+#include <Core/observers/Observer.h>
+
+#include <string>
+
+
+#include <VirtualRobot/VirtualRobot.h>
+
+namespace armarx
+{
+
+ /**
+ * ArmarX RobotStateObserver.
+ *
+ * The RobotStateObserver allows to install conditions on all channel reported by the KinematicUnit.
+ * These include joint angles, velocities, torques and motor temperatures
+ *
+ * The RobotStateObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
+ * which are observer by the unit are define by a robotnodeset
+ */
+ class ARMARXCORE_IMPORT_EXPORT RobotStateObserver :
+ virtual public Observer,
+ virtual public RobotStateObserverInterface
+ {
+ public:
+ // framework hooks
+ void onInitObserver();
+ void onConnectObserver();
+
+ // slice interface implementation
+ void reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current& c = Ice::Current());
+
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ virtual std::string getDefaultName() const { return "RobotStateObserver"; };
+
+ protected:
+ void updateRobotModel(const NameValueMap& nameValueMap);
+ void updatePoses();
+
+ private:
+ std::string robotNodeSetName;
+ RobotStateComponentInterfacePrx server;
+ VirtualRobot::RobotPtr robot;
+ std::map<std::string,VirtualRobot::RobotNodePtr> namesToNodes;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/robotstate/remote/checks/ConditionCheckEqualsPose.h b/source/RobotAPI/robotstate/remote/checks/ConditionCheckEqualsPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..42a683456f77128c6f12aa32543f92fbdd7f7960
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/checks/ConditionCheckEqualsPose.h
@@ -0,0 +1,75 @@
+#ifndef _ARMARX_CORE_CONDITIONCHECKEQUALSPOSE_H
+#define _ARMARX_CORE_CONDITIONCHECKEQUALSPOSE_H
+
+#include <Core/core/system/ImportExport.h>
+#include <Core/observers/ConditionCheck.h>
+#include <Core/robotstate/remote/ArmarPose.h>
+
+namespace armarx
+{
+ class ARMARXCORE_IMPORT_EXPORT ConditionCheckEqualsPose :
+ public ConditionCheck
+ {
+ public:
+ ConditionCheckEqualsPose()
+ {
+ setNumberParameters(1);
+ addSupportedType(VariantType::FramedPosition, createParameterTypeList(1, VariantType::FramedPosition);
+ addSupportedType(VariantType::FramedOrientation, createParameterTypeList(1, VariantType::FramedOrientation);
+ addSupportedType(VariantType::Vector3, createParameterTypeList(1, VariantType::Vector3);
+ addSupportedType(VariantType::Quaternion, createParameterTypeList(1, VariantType::Quaternion);
+ addSupportedType(VariantType::Pose, createParameterTypeList(1, VariantType::Pose);
+ }
+
+ ConditionCheck* clone()
+ {
+ return new ConditionCheckEqualsPose(*this);
+ }
+
+ bool evaluate(const StringVariantMap &dataFields)
+ {
+ if(dataFields.size() != 1)
+ {
+ printf("Size of dataFields: %d\n", (int)dataFields.size());
+ throw InvalidConditionException("Wrong number of datafields for condition equals ");
+ }
+
+ Variant& value = dataFields.begin()->second;
+ VariantTypeId type = value.getType();
+
+ if(type == VariantType::Vector3)
+ return (sqrt(((value.x-getParameter(0).getClass<Vector3>().x)*(value.x-getParameter(0).getClass<Vector3>().x))+
+ ((value.y-getParameter(0).getClass<Vector3>().y)*(value.y-getParameter(0).getClass<Vector3>().y))+
+ ((value.z-getParameter(0).getClass<Vector3>().x)*(value.x-getParameter(0).getClass<Vector3>().z))) == 0);
+
+ if(type == VariantType::FramedPosition)
+ return (sqrt(((value.x-getParameter(0).getClass<FramedPosition>().x)*(value.x-getParameter(0).getClass<FramedPosition>().x))+
+ ((value.y-getParameter(0).getClass<FramedPosition>().y)*(value.y-getParameter(0).getClass<FramedPosition>().y))+
+ ((value.z-getParameter(0).getClass<FramedPosition>().x)*(value.x-getParameter(0).getClass<FramedPosition>().z))) == 0);
+
+ if(type == VariantType::Quaternion)
+ return (sqrt(((value.qw-getParameter(0).getClass<Quaternion>().qw)*(value.qw-getParameter(0).getClass<Quaternion>().qw))+
+ ((value.qx-getParameter(0).getClass<Quaternion>().qx)*(value.qx-getParameter(0).getClass<Quaternion>().qx))+
+ ((value.qy-getParameter(0).getClass<Quaternion>().qy)*(value.qy-getParameter(0).getClass<Quaternion>().qy))+
+ ((value.qz-getParameter(0).getClass<Quaternion>().qx)*(value.qx-getParameter(0).getClass<Quaternion>().qz))) == 0);
+
+ if(type == VariantType::FramedOrientation)
+ return (sqrt(((value.qw-getParameter(0).getClass<FramedOrientation>().qw)*(value.qw-getParameter(0).getClass<FramedOrientation>().qw))+
+ ((value.qx-getParameter(0).getClass<FramedOrientation>().qx)*(value.qx-getParameter(0).getClass<FramedOrientation>().qx))+
+ ((value.qy-getParameter(0).getClass<FramedOrientation>().qy)*(value.qy-getParameter(0).getClass<FramedOrientation>().qy))+
+ ((value.qz-getParameter(0).getClass<FramedOrientation>().qx)*(value.qx-getParameter(0).getClass<FramedOrientation>().qz))) == 0);
+
+ if(type == VariantType::Pose)
+ return (sqrt(((value.x-getParameter(0).getClass<Pose>().x)*(value.x-getParameter(0).getClass<Pose>().x))+
+ ((value.y-getParameter(0).getClass<Pose>().y)*(value.y-getParameter(0).getClass<Pose>().y))+
+ ((value.z-getParameter(0).getClass<Pose>().x)*(value.x-getParameter(0).getClass<Pose>().z)))+
+ sqrt(((value.qw-getParameter(0).getClass<Pose>().qw)*(value.qw-getParameter(0).getClass<Pose>().qw))+
+ ((value.qx-getParameter(0).getClass<Pose>().qx)*(value.qx-getParameter(0).getClass<Pose>().qx))+
+ ((value.qy-getParameter(0).getClass<Pose>().qy)*(value.qy-getParameter(0).getClass<Pose>().qy))+
+ ((value.qz-getParameter(0).getClass<Pose>().qx)*(value.qx-getParameter(0).getClass<Pose>().qz))) == 0);
+
+ return false;
+ }
+ };
+}
+#endif // CONDITIONCHECKEQUALSPOSITION_H
diff --git a/source/RobotAPI/robotstate/remote/checks/ConditionCheckEqualsPoseWithTolerance.h b/source/RobotAPI/robotstate/remote/checks/ConditionCheckEqualsPoseWithTolerance.h
new file mode 100644
index 0000000000000000000000000000000000000000..f00a496cd9062938dc81203601b5e915857b009e
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/checks/ConditionCheckEqualsPoseWithTolerance.h
@@ -0,0 +1,102 @@
+#ifndef CONDITIONCHECKEQUALSPOSITIONWITHTOLERANCE_H
+#define CONDITIONCHECKEQUALSPOSITIONWITHTOLERANCE_H
+
+#include <Core/core/system/ImportExport.h>
+#include <Core/observers/ConditionCheck.h>
+#include <Core/robotstate/remote/ArmarPose.h>
+
+namespace armarx
+{
+ class ARMARXCORE_IMPORT_EXPORT ConditionCheckEqualsPoseWithTolerance :
+ public ConditionCheck
+ {
+ public:
+ ConditionCheckEqualsPoseWithTolerance()
+ {
+ setNumberParameters(2);
+ addSupportedType(VariantType::FramedPosition, createParameterTypeList(2, VariantType::FramedPosition, VariantType::Float));
+ addSupportedType(VariantType::FramedOrientation, createParameterTypeList(2, VariantType::FramedOrientation, VariantType::Float));
+ addSupportedType(VariantType::Vector3, createParameterTypeList(2, VariantType::Vector3, VariantType::Float));
+ addSupportedType(VariantType::Quaternion, createParameterTypeList(2, VariantType::Quaternion, VariantType::Float));
+ addSupportedType(VariantType::Pose, createParameterTypeList(2, VariantType::Pose, VariantType::Float));
+ }
+
+ ConditionCheck* clone()
+ {
+ return new ConditionCheckEqualsPoseWithTolerance(*this);
+ }
+
+ bool evaluate(const StringVariantMap &dataFields)
+ {
+ if(dataFields.size() != 1)
+ {
+ printf("Size of dataFields: %d\n", (int)dataFields.size());
+ throw InvalidConditionException("Wrong number of datafields for condition equals ");
+ }
+
+ Variant& value = dataFields.begin()->second;
+ VariantTypeId type = value.getType();
+
+ if(type == VariantType::Vector3)
+ {
+ Vector3Ptr & typedValue = value.getClass<Vector3>();
+ Vector3Ptr & param = getParameter(0).getClass<Vector3>();
+ return (sqrt(((typedValue->x-param->x)*(typedValue->x-param->x))+
+ ((typedValue->y-param->y)*(typedValue->y-param->y))+
+ ((typedValue->z-param->x)*(typedValue->x-param->z))) < getParameter(1).getFloat());
+ }
+
+ if(type == VariantType::Quaternion)
+ {
+ QuaternionPtr & typedValue = value.getClass<Quaternion>();
+ QuaternionPtr & param = getParameter(0).getClass<Quaternion>();
+ return (sqrt(((typedValue->qw-param->qw)*(typedValue->qw-param->qw))+
+ ((typedValue->qx-param->qx)*(typedValue->qx-param->qx))+
+ ((typedValue->qy-param->qy)*(typedValue->qy-param->qy))+
+ ((typedValue->qz-param->qx)*(typedValue->qx-param->qz))) < getParameter(1).getFloat());
+ }
+
+
+ if(type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr & typedValue = value.getClass<FramedPosition>();
+ FramedPositionPtr & param = getParameter(0).getClass<FramedPosition>();
+ return param->getFrame() == typedValue->getFrame()
+ && (sqrt(((typedValue->x-param->x)*(typedValue->x-param->x))+
+ ((typedValue->y-param->y)*(typedValue->y-param->y))+
+ ((typedValue->z-param->x)*(typedValue->x-param->z))) < getParameter(1).getFloat());
+ }
+
+
+ if(type == VariantType::FramedOrientation)
+ {
+ FramedOrientationPtr & typedValue = value.getClass<FramedOrientation>();
+ FramedOrientationPtr & param = getParameter(0).getClass<FramedOrientation>();
+ return param->getFrame() == typedValue->getFrame()
+ &&(sqrt(((typedValue->qw-param->qw)*(typedValue->qw-param->qw))+
+ ((typedValue->qx-param->qx)*(typedValue->qx-param->qx))+
+ ((typedValue->qy-param->qy)*(typedValue->qy-param->qy))+
+ ((typedValue->qz-param->qx)*(typedValue->qx-param->qz))) < getParameter(1).getFloat());
+ }
+
+
+ if(type == VariantType::Pose)
+ {
+ // TODO: This comparison is bullshit!
+ PosePtr & typedValue = value.getClass<Pose>();
+ PosePtr & param = getParameter(0).getClass<Pose>();
+ return (sqrt(((typedValue->position->x-param->position->x)*(typedValue->position->x-param->position->x))+
+ ((typedValue->position->y-param->position->y)*(typedValue->position->y-param->position->y))+
+ ((typedValue->position->z-param->position->x)*(typedValue->position->x-param->position->z)))+
+ sqrt(((typedValue->orientation->qw-param->orientation->qw)*(typedValue->orientation->qw-param->orientation->qw))+
+ ((typedValue->orientation->qx-param->orientation->qx)*(typedValue->orientation->qx-param->orientation->qx))+
+ ((typedValue->orientation->qy-param->orientation->qy)*(typedValue->orientation->qy-param->orientation->qy))+
+ ((typedValue->orientation->qz-param->orientation->qx)*(typedValue->orientation->qx-param->orientation->qz))) < getParameter(1).getFloat());
+ }
+
+ return false;
+ }
+ };
+}
+
+#endif // CONDITIONCHECKEQUALSPOSITIONWITHTOLERANCE_H
diff --git a/source/RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.cpp b/source/RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b35ef5228d5b2fd241ed79a2061f4a3e5269f8af
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.cpp
@@ -0,0 +1,70 @@
+#include "ConditionCheckMagnitudeChecks.h"
+
+namespace armarx {
+
+ ConditionCheckMagnitudeLarger::ConditionCheckMagnitudeLarger()
+ {
+ setNumberParameters(1);
+ addSupportedType(VariantType::Vector3, createParameterTypeList(1, VariantType::Float));
+ addSupportedType(VariantType::FramedVector3, createParameterTypeList(1, VariantType::Float));
+ addSupportedType(VariantType::LinkedVector3, createParameterTypeList(1, VariantType::Float));
+
+ }
+
+ ConditionCheck* ConditionCheckMagnitudeLarger::clone()
+ {
+ return new ConditionCheckMagnitudeLarger(*this);
+ }
+
+ bool ConditionCheckMagnitudeLarger::evaluate(const StringVariantMap &dataFields)
+ {
+
+ if(dataFields.size() != 1)
+ {
+ printf("Size of dataFields: %d\n", (int)dataFields.size());
+ throw InvalidConditionException("Wrong number of datafields for condition equals ");
+ }
+
+ const Variant& value = dataFields.begin()->second;
+ VariantTypeId type = value.getType();
+
+ if(type == VariantType::Vector3)
+ {
+ Eigen::Vector3f vec = value.getClass<Vector3>()->toEigen();
+ Eigen::Vector3f vec2 = getParameter(0).getClass<Vector3>()->toEigen();
+ return (vec-vec2).norm() > getParameter(1).getFloat();
+ }
+
+ if(type == VariantType::FramedVector3)
+ {
+ FramedVector3Ptr fV1 = value.getClass<FramedVector3>();
+ FramedVector3Ptr fV2 = getParameter(0).getClass<FramedVector3>();
+ if(fV1->frame != fV2->frame)
+ {
+ ARMARX_WARNING_S << deactivateSpam(3) << "Cannot compare FramedVector3 with different reference frames, because no robot is here available. Use LinkedVector3 instead or provide in correct frames";
+ return false;
+ }
+
+ Eigen::Vector3f vec = value.getClass<FramedVector3>()->toEigen();
+ Eigen::Vector3f vec2 = getParameter(0).getClass<FramedVector3>()->toEigen();
+ return (vec-vec2).norm() > getParameter(1).getFloat();
+ }
+
+ if(type == VariantType::LinkedVector3)
+ {
+ LinkedVector3Ptr lV1 = value.getClass<LinkedVector3>();
+ LinkedVector3Ptr lV2 = getParameter(0).getClass<LinkedVector3>();
+ if(lV1->frame != lV2->frame)
+ lV1->changeFrame(lV2->frame);
+
+ Eigen::Vector3f vec = value.getClass<LinkedVector3>()->toEigen();
+ Eigen::Vector3f vec2 = getParameter(0).getClass<LinkedVector3>()->toEigen();
+ return (vec-vec2).norm() > getParameter(1).getFloat();
+ }
+
+ return false;
+ }
+
+
+
+}
diff --git a/source/RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h b/source/RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8c76a87be1f891ed5daff8b4f7ffbb013dc00ed
--- /dev/null
+++ b/source/RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h
@@ -0,0 +1,22 @@
+#ifndef ARMARX_CONDITIONCHECKMAGNITUDECHECKS_H
+#define ARMARX_CONDITIONCHECKMAGNITUDECHECKS_H
+
+#include <Core/core/system/ImportExport.h>
+#include <Core/observers/ConditionCheck.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/robotstate/remote/LinkedPose.h>
+
+namespace armarx {
+
+ class ConditionCheckMagnitudeLarger : public ConditionCheck, Logging
+ {
+ public:
+ ConditionCheckMagnitudeLarger();
+
+ ConditionCheck *clone();
+ bool evaluate(const StringVariantMap &dataFields);
+ };
+
+
+}
+#endif
diff --git a/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
index 60a123777500c06dc11a3de053210af82e1648be..8345036888870295fc582c90ea9593f421a14581 100644
--- a/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
+++ b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
@@ -28,10 +28,10 @@
#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
-#include <Core/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
#include <VirtualRobot/IK/DifferentialIK.h>
-#include <Core/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/core/RobotStatechartContext.h>
diff --git a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
index c869ee5ea6caab7f948a6b2f1367821f57293ab5..b70d3f73a0f39d16ec58d4d0ad65297c0ebf80d0 100644
--- a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
+++ b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
@@ -27,10 +27,10 @@
#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
-#include <Core/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
#include <VirtualRobot/IK/DifferentialIK.h>
-#include <Core/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
namespace armarx
{
diff --git a/source/RobotAPI/statecharts/MovePlatform/PlatformContext.h b/source/RobotAPI/statecharts/MovePlatform/PlatformContext.h
index c1321c03e8c28177015bf3df4071373241d2c03e..e0e419bea7e119f16504195bf3562bc21d96665b 100644
--- a/source/RobotAPI/statecharts/MovePlatform/PlatformContext.h
+++ b/source/RobotAPI/statecharts/MovePlatform/PlatformContext.h
@@ -28,11 +28,11 @@
#include <Core/core/Component.h>
#include <Core/core/system/ImportExportComponent.h>
#include <Core/statechart/StatechartContext.h>
-#include <Core/robotstate/remote/RemoteRobot.h>
-#include <Core/interface/units/PlatformUnitInterface.h>
-#include <Core/interface/observers/PlatformUnitObserverInterface.h>
+#include <RobotAPI/robotstate/remote/RemoteRobot.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/interface/observers/PlatformUnitObserverInterface.h>
-#include <Core/units/PlatformUnitObserver.h>
+#include <RobotAPI/units/PlatformUnitObserver.h>
//#include <VirtualRobot/VirtualRobot.h>
#include <IceUtil/Time.h>
diff --git a/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp b/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp
index 9533b6d0f957e464b1ca0b1bbf1287ce7364219e..b2cbb39913e1248e5ab7a353ef7e5a80725cb74c 100644
--- a/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp
+++ b/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp
@@ -27,12 +27,12 @@
#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
-#include <Core/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
#include <Core/core/ManagedIceObject.h>
-#include <Core/robotstate/remote/RobotStateObjectFactories.h>
+#include <RobotAPI/robotstate/remote/RobotStateObjectFactories.h>
#include <Core/core/CoreObjectFactories.h>
#include <Core/observers/ObserverObjectFactories.h>
-#include <Core/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <boost/algorithm/string.hpp>
diff --git a/source/RobotAPI/statecharts/OpenHand/OpenHand.cpp b/source/RobotAPI/statecharts/OpenHand/OpenHand.cpp
index 74b3bdb8507ad92d5025a9721ba7aae8ae93f935..0d98395ae75b6ad530fce1dc953cb6716a0bd04b 100644
--- a/source/RobotAPI/statecharts/OpenHand/OpenHand.cpp
+++ b/source/RobotAPI/statecharts/OpenHand/OpenHand.cpp
@@ -28,10 +28,10 @@
#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
-#include <Core/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
#include <VirtualRobot/IK/DifferentialIK.h>
-#include <Core/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/core/RobotStatechartContext.h>
diff --git a/source/RobotAPI/units/CMakeLists.txt b/source/RobotAPI/units/CMakeLists.txt
index 0bc0993563f552e2dd27def2a175066064346bfe..fd4a6fda6ed0eee68f165b88c9f6c7510ef8893c 100644
--- a/source/RobotAPI/units/CMakeLists.txt
+++ b/source/RobotAPI/units/CMakeLists.txt
@@ -19,7 +19,13 @@ set(LIB_NAME RobotAPIUnits)
set(LIB_VERSION 0.1.0)
set(LIB_SOVERSION 0)
-set(LIBS RobotAPIInterfaces RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreEigen3Variants ArmarXCoreRemoteRobot ${Simox_LIBRARIES})
+set(LIBS RobotAPIInterfaces
+ RobotAPICore
+ ArmarXInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+ ArmarXCoreEigen3Variants
+ RobotAPIRemoteRobot ${Simox_LIBRARIES})
set(LIB_HEADERS
ForceTorqueObserver.h
@@ -29,6 +35,17 @@ set(LIB_HEADERS
HapticObserver.h
TCPControlUnit.h
TCPControlUnitObserver.h
+
+ HandUnit.h
+ HardwareUnit.h
+ KinematicUnit.h
+ KinematicUnitSimulation.h
+ PlatformUnit.h
+ PlatformUnitSimulation.h
+ TCPMoverUnit.h
+ KinematicUnitObserver.h
+ PlatformUnitObserver.h
+ SensorActorUnit.h
)
set(LIB_FILES
@@ -39,6 +56,17 @@ set(LIB_FILES
HapticObserver.cpp
TCPControlUnit.cpp
TCPControlUnitObserver.cpp
+
+ HandUnit.cpp
+ HardwareUnit.cpp
+ KinematicUnit.cpp
+ KinematicUnitSimulation.cpp
+ PlatformUnit.cpp
+ PlatformUnitSimulation.cpp
+ TCPMoverUnit.cpp
+ KinematicUnitObserver.cpp
+ PlatformUnitObserver.cpp
+ SensorActorUnit.cpp
)
armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/units/ForceTorqueObserver.cpp b/source/RobotAPI/units/ForceTorqueObserver.cpp
index df83a533c9b252f5a6253d6ccc98395fc751a27b..d469ac1135300ddedb518ee26f56031483647b03 100644
--- a/source/RobotAPI/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/units/ForceTorqueObserver.cpp
@@ -5,9 +5,9 @@
#include <Core/observers/checks/ConditionCheckInRange.h>
#include <Core/observers/checks/ConditionCheckLarger.h>
#include <Core/observers/checks/ConditionCheckSmaller.h>
-#include <Core/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+#include <RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
-#include <Core/robotstate/remote/RobotStateObjectFactories.h>
+#include <RobotAPI/robotstate/remote/RobotStateObjectFactories.h>
#define RAWFORCE "_rawforcevectors"
#define OFFSETFORCE "_forceswithoffsetvectors"
diff --git a/source/RobotAPI/units/ForceTorqueUnit.h b/source/RobotAPI/units/ForceTorqueUnit.h
index ca4f4a83f0031775b4982639f8276d287f92cb5b..df6b0566f27b05f72fcfda5e01a137b0cac5869c 100644
--- a/source/RobotAPI/units/ForceTorqueUnit.h
+++ b/source/RobotAPI/units/ForceTorqueUnit.h
@@ -28,7 +28,7 @@
#include <Core/core/Component.h>
#include <Core/core/application/properties/Properties.h>
#include <Core/core/system/ImportExportComponent.h>
-#include "Core/units/SensorActorUnit.h"
+#include "RobotAPI/units/SensorActorUnit.h"
#include <RobotAPI/interface/units/ForceTorqueUnit.h>
diff --git a/source/RobotAPI/units/HandUnit.cpp b/source/RobotAPI/units/HandUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8545fa4352f89820c70a3d976c696823d1742b1f
--- /dev/null
+++ b/source/RobotAPI/units/HandUnit.cpp
@@ -0,0 +1,136 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Manfred Kroehnert (Manfred dot Kroehnert at kit dot edu)
+ * @date 2013
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include "HandUnit.h"
+
+#include <Core/core/system/ArmarXDataPath.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <vector>
+
+using namespace armarx;
+using namespace VirtualRobot;
+
+
+void HandUnit::onInitComponent()
+{
+ std::string endeffectorFile = getProperty<std::string>("EndeffectorFile").getValue();
+ std::string endeffectorName = getProperty<std::string>("EndeffectorName").getValue();
+
+ if (!ArmarXDataPath::getAbsolutePath(endeffectorFile, endeffectorFile))
+ {
+ throw UserException("Could not find robot file " + endeffectorFile);
+ }
+
+
+ try
+ {
+ robot = VirtualRobot::RobotIO::loadRobot(endeffectorFile, VirtualRobot::RobotIO::eStructure);
+ }
+ catch(VirtualRobot::VirtualRobotException& e)
+ {
+ throw UserException(e.what());
+ }
+
+ if(endeffectorName == "")
+ {
+ throw UserException("EndeffectorName not defined");
+ }
+ if(!robot->hasEndEffector(endeffectorName))
+ {
+ throw UserException("Robot does not contain an endeffector with name: " + endeffectorName);
+ }
+
+ const std::vector<std::string> endeffectorPreshapes = robot->getEndEffector(endeffectorName)->getPreshapes();
+
+ preshapeNames = new SingleTypeVariantList(VariantType::String);
+ std::vector<std::string>::const_iterator iter = endeffectorPreshapes.begin();
+ while(iter != endeffectorPreshapes.end())
+ {
+ Variant currentPreshape;
+ currentPreshape.setString(*iter);
+ preshapeNames->addVariant(currentPreshape);
+ iter++;
+ }
+
+
+ // component dependencies
+ listenerChannelName = endeffectorName + "State";
+ offeringTopic(listenerChannelName);
+
+ this->onInitHandUnit();
+}
+
+
+void HandUnit::onConnectComponent()
+{
+ ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
+ listenerPrx = getTopic<HandUnitListenerPrx>(listenerChannelName);
+
+ this->onStartHandUnit();
+}
+
+
+void HandUnit::onExitComponent()
+{
+ this->onExitHandUnit();
+}
+
+
+void HandUnit::open(const Ice::Current& c)
+{
+}
+
+
+void HandUnit::close(const Ice::Current& c)
+{
+}
+
+
+void HandUnit::preshape(const std::string& preshapeName, const Ice::Current& c)
+{
+}
+
+
+SingleTypeVariantListBasePtr HandUnit::getPreshapeNames(const Ice::Current& c)
+{
+ return preshapeNames;
+}
+
+
+PropertyDefinitionsPtr HandUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new HandUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
+
+
+NameValueMap HandUnit::getPreshapeJointValues(const std::string &preshapeName, const Ice::Current &)
+{
+ EndEffectorPtr efp = robot->getEndEffector(getProperty<std::string>("EndeffectorName").getValue());
+ RobotConfigPtr rc = efp->getPreshape(preshapeName);
+ return rc->getRobotNodeJointValueMap();
+}
diff --git a/source/RobotAPI/units/HandUnit.h b/source/RobotAPI/units/HandUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..a73ee435eb585bdd8621a88cec76600e78516287
--- /dev/null
+++ b/source/RobotAPI/units/HandUnit.h
@@ -0,0 +1,160 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Manfred Kroehnert (manfred dot kroehnert at kit dot edu)
+ * @date 2013
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_UNITS_HANDUNIT_H
+#define _ARMARX_CORE_UNITS_HANDUNIT_H
+
+#include <RobotAPI/units/SensorActorUnit.h>
+
+#include <RobotAPI/interface/units/HandUnitInterface.h>
+
+#include <Core/core/application/properties/Properties.h>
+#include <Core/core/system/ImportExportComponent.h>
+#include <Core/observers/variant/SingleTypeVariantList.h>
+
+
+namespace VirtualRobot
+{
+ class Robot;
+ typedef boost::shared_ptr<Robot> RobotPtr;
+}
+namespace armarx
+{
+ /**
+ * @class HandUnitPropertyDefinitions
+ * @brief Defines all necessary properties for armarx::HandUnit
+ * @ingroup SensorActorUnits
+ */
+ class HandUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ HandUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("EndeffectorFile","VirtualRobot XML file in which the endeffector is is stored.");
+ defineRequiredProperty<std::string>("EndeffectorName","Name of the endeffector as stored in the XML file (will publish values on EndeffectorName + 'State')");
+ }
+ };
+
+
+ /**
+ * @class HandUnit
+ * @brief This class defines an interface for providing high level access to robot hands
+ * @ingroup SensorActorUnits
+ *
+ * An instance of a HandUnit provides means to open, close, and preshape hands.
+ * It uses the HandUnitListener Ice interface to report updates of its current state
+ *
+ */
+ class HandUnit :
+ virtual public HandUnitInterface,
+ virtual public SensorActorUnit
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() const
+ {
+ return "HandUnit";
+ }
+
+ /**
+ * Retrieve proxy for publishing State information and call
+ * armarx::PlatformUnit::onInitPlatformUnit().
+ * @see armarx::Component::onInitComponent()
+ */
+ virtual void onInitComponent();
+ /**
+ * Calls armarx::PlatformUnit::onStartPlatformUnit().
+ * @see armarx::Component::onConnectComponent()
+ */
+ virtual void onConnectComponent();
+ /**
+ * Calls armarx::PlatformUnit::onExitPlatformUnit().
+ * @see armarx::Component::onExitComponent()
+ */
+ virtual void onExitComponent();
+
+ /**
+ *
+ */
+ virtual void onInitHandUnit() = 0;
+ /**
+ *
+ */
+ virtual void onStartHandUnit() = 0;
+ /**
+ *
+ */
+ virtual void onExitHandUnit() = 0;
+
+ /**
+ * Send command to the hand to open all fingers.
+ */
+ virtual void open(const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * Send command to the hand to close all fingers.
+ */
+ virtual void close(const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * Send command to the hand to form a specific preshape position.
+ */
+ virtual void preshape(const std::string& preshapeName, const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * @return a list of strings for preshape positions which can be used together with HandUnit::preshape().
+ */
+ virtual SingleTypeVariantListBasePtr getPreshapeNames(const Ice::Current& c = ::Ice::Current());
+
+ NameValueMap getPreshapeJointValues(const std::string &preshapeName, const Ice::Current &);
+
+
+ /**
+ * @see armarx::PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ /**
+ * HandUnitListener proxy for publishing state updates
+ */
+ HandUnitListenerPrx listenerPrx;
+ /**
+ * Ice Topic name on which armarx::HandUnit::listenerPrx publishes information
+ */
+ std::string listenerChannelName;
+ /**
+ * List containing the names of all valid preshapes.
+ */
+ SingleTypeVariantListPtr preshapeNames;
+
+ VirtualRobot::RobotPtr robot;
+
+
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/units/HapticObserver.cpp b/source/RobotAPI/units/HapticObserver.cpp
index 033532980dbb7f79334d1196ebf2edf4d503bb24..cc83771e237dfbd9b62cdf110ea49e94f58ffcb6 100644
--- a/source/RobotAPI/units/HapticObserver.cpp
+++ b/source/RobotAPI/units/HapticObserver.cpp
@@ -5,9 +5,9 @@
#include <Core/observers/checks/ConditionCheckInRange.h>
#include <Core/observers/checks/ConditionCheckLarger.h>
#include <Core/observers/checks/ConditionCheckSmaller.h>
-#include <Core/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+#include <RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
-#include <Core/robotstate/remote/RobotStateObjectFactories.h>
+#include <RobotAPI/robotstate/remote/RobotStateObjectFactories.h>
#include <Eigen/Dense>
#include <Core/observers/variant/TimestampVariant.h>
diff --git a/source/RobotAPI/units/HapticUnit.h b/source/RobotAPI/units/HapticUnit.h
index 1d8c58283f066eb54a42673b6a90a1586b720d70..1694550e465636ac8e3839a452ea40fda2661682 100644
--- a/source/RobotAPI/units/HapticUnit.h
+++ b/source/RobotAPI/units/HapticUnit.h
@@ -27,7 +27,7 @@
#include <Core/core/Component.h>
#include <Core/core/application/properties/Properties.h>
#include <Core/core/system/ImportExportComponent.h>
-#include "Core/units/SensorActorUnit.h"
+#include "RobotAPI/units/SensorActorUnit.h"
#include <RobotAPI/interface/units/HapticUnit.h>
diff --git a/source/RobotAPI/units/HardwareUnit.cpp b/source/RobotAPI/units/HardwareUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0aae5cc96d5d3acb5c47bda6955b3dae9872acd0
--- /dev/null
+++ b/source/RobotAPI/units/HardwareUnit.cpp
@@ -0,0 +1,94 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core
+ * @author Matthias Hadlich (matthias dot hadlich at student dot kit dot edu)
+ * @copyright 2014 Matthias Hadlich
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef HARDWAREUNIT_CPP
+#define HARDWAREUNIT_CPP
+
+#include "HardwareUnit.h"
+#include <vector>
+#include <string>
+
+using namespace std;
+using namespace armarx;
+
+
+void HardwareUnit::onInitComponent() {
+ this->onInitHardwareUnit();
+ offeringTopic("HardwareValues");
+}
+
+void HardwareUnit::onConnectComponent() {
+
+ ARMARX_INFO << flush;
+
+ // TODO: muss das hier gleich sein wie der Channel der Kinematicunit?!?!
+ //std::string hardwareChannel = "hardware";
+
+ hardwareValuePrx = getTopic<HardwareInterfacePrx>("HardwareValues");
+ ARMARX_INFO << "subscribing topic: " << "HardwareValues" << flush;
+
+
+
+ this->onConnectHardwareUnit();
+}
+
+void HardwareUnit::onExitComponent() {
+ this->onExitHardwareUnit();
+}
+
+
+BusNames HardwareUnit::getConfiguredBusses( const Ice::Current& c) {
+ return *(new vector<string>());
+}
+BusInformation HardwareUnit::getBusInformation(const std::string& bus, const Ice::Current& c){
+ return *(new BusInformation);
+
+}
+DeviceNames HardwareUnit::getDevicesOnBus(const std::string &bus, const Ice::Current& c ){
+ return *(new vector<string>());
+}
+
+DeviceInformation HardwareUnit::getDeviceInformation(const std::string& device, const Ice::Current& c ) {
+ return *(new DeviceInformation);
+}
+
+int HardwareUnit::performBasicOperation( BasicOperation& operation, const Ice::Current& c) {
+
+ return 0;
+}
+
+int HardwareUnit::sendCommand(const std::string& command, const Ice::Current& c ) {
+
+ return 0;
+}
+
+
+PropertyDefinitionsPtr HardwareUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new HardwareUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
+
+
+
+#endif
diff --git a/source/RobotAPI/units/HardwareUnit.h b/source/RobotAPI/units/HardwareUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..eab13210ba1099e44a277b721f4e271484c04724
--- /dev/null
+++ b/source/RobotAPI/units/HardwareUnit.h
@@ -0,0 +1,88 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Core
+ * @author Matthias Hadlich (matthias dot hadlich at student dot kit dot edu)
+ * @copyright 2014 Matthias Hadlich
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _HARDWAREUNIT_H
+#define _HARDWAREUNIT_H
+
+#include <string>
+
+#include <Core/core/Component.h>
+#include <Core/core/application/properties/Properties.h>
+#include <Core/core/system/ImportExportComponent.h>
+
+#include <RobotAPI/interface/hardware/HardwareInterface.h>
+
+namespace armarx {
+
+ class HardwareUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ HardwareUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+
+ //defineRequiredProperty<std::string>("RobotNodeSetName","Robot node name");
+ //defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
+ }
+ };
+
+class HardwareUnit :
+ virtual public HardwareInterface, virtual public Component
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() const { return "HardwareUnit"; }
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onExitComponent();
+
+ virtual void onInitHardwareUnit() = 0;
+ virtual void onConnectHardwareUnit() = 0;
+ virtual void onExitHardwareUnit() = 0;
+
+ virtual BusNames getConfiguredBusses( const Ice::Current& c = ::Ice::Current());
+ virtual BusInformation getBusInformation(const std::string &bus, const Ice::Current& c = ::Ice::Current());
+ virtual DeviceNames getDevicesOnBus(const std::string &bus, const Ice::Current& c = ::Ice::Current());
+ virtual DeviceInformation getDeviceInformation(const std::string &device, const Ice::Current& c = ::Ice::Current());
+
+ virtual int performBasicOperation(BasicOperation &operation, const Ice::Current& c = ::Ice::Current());
+ virtual int sendCommand(const std::string &command, const Ice::Current& c = ::Ice::Current());
+
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ std::string channel;
+ HardwareInterfacePrx hardwareValuePrx; // gets commands from hardware
+
+ };
+}
+
+
+
+
+#endif
diff --git a/source/RobotAPI/units/HeadIKUnit.h b/source/RobotAPI/units/HeadIKUnit.h
index b9365e1e15b17aa0f289f2931e9826c9b71cb4fe..1862188317a891a9bf08fc3d59d4cb02b248e425 100644
--- a/source/RobotAPI/units/HeadIKUnit.h
+++ b/source/RobotAPI/units/HeadIKUnit.h
@@ -27,8 +27,8 @@
#include <Core/core/Component.h>
#include <Core/core/services/tasks/PeriodicTask.h>
-#include <Core/robotstate/remote/ArmarPose.h>
-#include <Core/robotstate/remote/RemoteRobot.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/robotstate/remote/RemoteRobot.h>
#include <RobotAPI/interface/units/HeadIKUnit.h>
diff --git a/source/RobotAPI/units/KinematicUnit.cpp b/source/RobotAPI/units/KinematicUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b360f799b39e4cc34ade44cac5c3ed6c974ca8b0
--- /dev/null
+++ b/source/RobotAPI/units/KinematicUnit.cpp
@@ -0,0 +1,115 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Christian Boege (boege at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include "KinematicUnit.h"
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+#include <Core/core/system/ArmarXDataPath.h>
+
+using namespace armarx;
+
+
+void KinematicUnit::onInitComponent()
+{
+ // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
+ // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
+ std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+
+ if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile))
+ {
+ throw UserException("Could not find robot file " + robotFile);
+ }
+
+ // read the robot node set and initialize the joints of this kinematic unit
+ try
+ {
+ robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ }
+ catch(VirtualRobot::VirtualRobotException& e)
+ {
+ throw UserException(e.what());
+ }
+ if(robotNodeSetName == "")
+ {
+ throw UserException("RobotNodeSet not defined");
+ }
+
+ VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
+ robotNodes = robotNodeSetPtr->getAllRobotNodes();
+
+ // component dependencies
+ listenerName = robotNodeSetName + "State";
+ offeringTopic(listenerName);
+
+ this->onInitKinematicUnit();
+}
+
+
+void KinematicUnit::onConnectComponent()
+{
+ ARMARX_INFO << "setting report topic to " << listenerName << flush;
+ listenerPrx = getTopic<KinematicUnitListenerPrx>(listenerName);
+
+ this->onStartKinematicUnit();
+}
+
+
+void KinematicUnit::onExitComponent()
+{
+ this->onExitKinematicUnit();
+}
+
+
+void KinematicUnit::switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c)
+{
+}
+
+void KinematicUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
+{
+}
+
+void KinematicUnit::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
+{
+}
+
+void KinematicUnit::requestKinematicUnit(const StringSequence& nodes, const Ice::Current& c)
+{
+ SensorActorUnit::request(c);
+}
+
+void KinematicUnit::releaseKinematicUnit(const StringSequence& nodes, const Ice::Current& c)
+{
+ SensorActorUnit::release(c);
+}
+
+PropertyDefinitionsPtr KinematicUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new KinematicUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/units/KinematicUnit.h b/source/RobotAPI/units/KinematicUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..084106dfe706d9ab80c391332359c792f6fa654d
--- /dev/null
+++ b/source/RobotAPI/units/KinematicUnit.h
@@ -0,0 +1,108 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Christian Boege (boege at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_KINEMATIC_UNIT_H
+#define _ARMARX_COMPONENT_KINEMATIC_UNIT_H
+
+#include <RobotAPI/units/SensorActorUnit.h>
+
+#include <Core/core/Component.h>
+#include <Core/core/application/properties/Properties.h>
+#include <Core/core/system/ImportExportComponent.h>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <vector>
+
+namespace armarx
+{
+ /**
+ * @class KinematicUnitPropertyDefinitions
+ * @brief
+ * @ingroup SensorActorUnits
+ */
+ class KinematicUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ KinematicUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("RobotNodeSetName","Robot node name");
+ defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
+ }
+ };
+
+
+ /**
+ * @class KinematicUnit
+ * @brief
+ * @ingroup SensorActorUnits
+ *
+ * KinematicUnits are SensorActorUnits which provide sensory data in terms of joints angles, joint velocities and joint forces.
+ * Further target joint angles and velocities can be controlled.
+ *
+ * The KinematicUnit retrieves its configuration from a VirtualRobot robot model. Within the model, the joints, the unit
+ * controls are defined with a RobotNodeSet.
+ */
+ class KinematicUnit :
+ virtual public KinematicUnitInterface,
+ virtual public SensorActorUnit
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() const { return "KinematicUnit"; }
+
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onExitComponent();
+
+ virtual void onInitKinematicUnit() = 0;
+ virtual void onStartKinematicUnit() = 0;
+ virtual void onExitKinematicUnit() = 0;
+
+ // proxy implementation
+ virtual void requestKinematicUnit(const StringSequence& nodes, const Ice::Current& c = ::Ice::Current());
+ virtual void releaseKinematicUnit(const StringSequence& nodes, const Ice::Current& c = ::Ice::Current());
+ virtual void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = ::Ice::Current());
+ virtual void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = ::Ice::Current());
+ virtual void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = ::Ice::Current());
+
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ KinematicUnitListenerPrx listenerPrx;
+
+ VirtualRobot::RobotPtr robot;
+ std::string robotNodeSetName;
+ std::string listenerName;
+ std::vector< VirtualRobot::RobotNodePtr > robotNodes;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/units/KinematicUnitObserver.cpp b/source/RobotAPI/units/KinematicUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0bb77d15aa50362a7882ebd303451396e83e1c6
--- /dev/null
+++ b/source/RobotAPI/units/KinematicUnitObserver.cpp
@@ -0,0 +1,176 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Kai Welke (welke _at_ kit _dot_ edu)
+* @date 2011 Kai Welke
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "KinematicUnitObserver.h"
+#include "KinematicUnit.h"
+#include <Core/observers/checks/ConditionCheckValid.h>
+#include <Core/observers/checks/ConditionCheckUpdated.h>
+#include <Core/observers/checks/ConditionCheckEquals.h>
+#include <Core/observers/checks/ConditionCheckInRange.h>
+#include <Core/observers/checks/ConditionCheckLarger.h>
+#include <Core/observers/checks/ConditionCheckSmaller.h>
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/core/system/ArmarXDataPath.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+
+using namespace armarx;
+
+// ********************************************************************
+// observer framework hooks
+// ********************************************************************
+void KinematicUnitObserver::onInitObserver()
+{
+ // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
+ // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
+ std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+
+ if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile))
+ {
+ throw UserException("Could not find robot file " + robotFile);
+ }
+
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ if(robotNodeSetName == "")
+ {
+ throw UserException("RobotNodeSet not defined");
+ }
+ VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
+
+ std::vector< VirtualRobot::RobotNodePtr > robotNodes;
+ robotNodes = robotNodeSetPtr->getAllRobotNodes();
+
+ // register all channels
+ offerChannel("jointangles","Joint values of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointvelocities","Joint velocities 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");
+
+
+ // register all data fields
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ {
+ std::string jointName = (*it)->getName();
+ ARMARX_VERBOSE << "* " << jointName << std::endl;
+ 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");
+ }
+
+ // register all checks
+ offerConditionCheck("valid", new ConditionCheckValid());
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("inrange", new ConditionCheckInRange());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+ usingTopic(robotNodeSetName + "State");
+}
+
+void KinematicUnitObserver::onConnectObserver()
+{
+
+}
+
+// ********************************************************************
+// KinematicUnitListener interface implementation
+// ********************************************************************
+void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c)
+{
+ // TODO: offer this as channel???
+
+}
+
+void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c)
+{
+ if(jointAngles.size() == 0)
+ return;
+ nameValueMapToDataFields("jointangles", jointAngles);
+
+
+ updateChannel("jointangles");
+}
+
+void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c)
+{
+ if(jointVelocities.size() == 0)
+ return;
+ nameValueMapToDataFields("jointvelocities", jointVelocities);
+ updateChannel("jointvelocities");
+}
+
+void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c)
+{
+ if(jointTorques.size() == 0)
+ return;
+ nameValueMapToDataFields("jointtorques", jointTorques);
+ updateChannel("jointtorques");
+}
+
+void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c)
+{
+ if(jointCurrents.size() == 0)
+ return;
+ nameValueMapToDataFields("jointcurrents", jointCurrents);
+ updateChannel("jointcurrents");
+}
+
+void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c)
+{
+ if(jointMotorTemperatures.size() == 0)
+ return;
+ nameValueMapToDataFields("jointmotortemperatures", jointMotorTemperatures);
+ updateChannel("jointmotortemperatures");
+}
+
+void KinematicUnitObserver::reportJointStatuses(const NameStatusMap &jointStatuses, bool aValueChanged, const Ice::Current &c)
+{
+}
+
+// ********************************************************************
+// private methods
+// ********************************************************************
+void KinematicUnitObserver::nameValueMapToDataFields(const std::string &channelName, const NameValueMap& nameValueMap)
+{
+ NameValueMap::const_iterator iter = nameValueMap.begin();
+
+ while(iter != nameValueMap.end())
+ {
+ setDataFieldFlatCopy(channelName, iter->first, new Variant(iter->second));
+ iter++;
+ }
+}
+
+PropertyDefinitionsPtr KinematicUnitObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new KinematicUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/units/KinematicUnitObserver.h b/source/RobotAPI/units/KinematicUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..80dde03445f3b6fe287f5b8b59b7ca0578a3dd50
--- /dev/null
+++ b/source/RobotAPI/units/KinematicUnitObserver.h
@@ -0,0 +1,130 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Kai Welke (welke _at_ kit _dot_ edu)
+* @date 2011 Kai Welke
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_KINEMATIC_UNIT_OBSERVER_H
+#define _ARMARX_CORE_KINEMATIC_UNIT_OBSERVER_H
+
+#include <Core/core/system/ImportExport.h>
+#include <Core/core/Component.h>
+#include <Core/core/application/properties/Properties.h>
+#include <Core/interface/observers/VariantBase.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+#include <Core/observers/ConditionCheck.h>
+#include <Core/observers/Observer.h>
+
+#include <string>
+
+namespace armarx
+{
+ ARMARX_CREATE_CHECK(KinematicUnitObserver, equals)
+ ARMARX_CREATE_CHECK(KinematicUnitObserver, larger)
+
+ /**
+ * @class KinematicUnitObserver
+ * @brief
+ * @ingroup SensorActorUnits
+ *
+ * The KinematicUnitObserver allows to install conditions on all channel reported by the KinematicUnit.
+ * These include joint angles, velocities, torques and motor temperatures
+ *
+ * The KinematicUnitObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
+ * which are observer by the unit are define by a robotnodeset
+ */
+ class ARMARXCORE_IMPORT_EXPORT KinematicUnitObserver :
+ virtual public Observer,
+ virtual public KinematicUnitObserverInterface
+ {
+ public:
+ // framework hooks
+ void onInitObserver();
+ void onConnectObserver();
+
+ // slice interface implementation
+ void reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+
+ virtual std::string getDefaultName() const { return "KinematicUnitObserver"; };
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ void nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap);
+
+ private:
+ std::string robotNodeSetName;
+ };
+
+ /**
+ @class KinematicUnitDatafieldCreator
+ @brief
+ @ingroup SensorActorUnits
+ */
+ class KinematicUnitDatafieldCreator
+ {
+ public:
+ KinematicUnitDatafieldCreator(const std::string &channelName): _channelName(channelName){}
+
+ /**
+ * @brief Function to create a Channel Representation for a KinematicUnitObserver
+ * @param kinematicUnitOberserverName Name of the KinematicUnitObserver
+ * @param jointName Name of a joint of the robot like it is specified
+ * in the simox-robot-xml-file
+ * @return returns a ChannelRepresentationPtr
+ */
+ DataFieldIdentifier getDatafield(const std::string &kinematicUnitObserverName, const std::string &jointName) const
+ {
+ if(kinematicUnitObserverName.empty())
+ throw LocalException("kinematicUnitObserverName must not be empty!");
+ if(jointName.empty())
+ throw LocalException("jointName must not be empty!");
+ return DataFieldIdentifier(kinematicUnitObserverName, _channelName, jointName);
+ }
+
+ private:
+ std::string _channelName;
+ };
+
+
+ namespace channels{
+
+ namespace KinematicUnitObserver{
+ const KinematicUnitDatafieldCreator jointAngles("jointAngles");
+ const KinematicUnitDatafieldCreator jointVelocities("jointVelocities");
+ const KinematicUnitDatafieldCreator jointTorques("jointTorques");
+ const KinematicUnitDatafieldCreator jointCurrents("jointCurrents");
+ const KinematicUnitDatafieldCreator jointTemperatures("jointTemperatures");
+ }
+ }
+
+}
+
+#endif
diff --git a/source/RobotAPI/units/KinematicUnitSimulation.cpp b/source/RobotAPI/units/KinematicUnitSimulation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6f4af469d18319f503b48c4ba6d938bcfddf9d42
--- /dev/null
+++ b/source/RobotAPI/units/KinematicUnitSimulation.cpp
@@ -0,0 +1,319 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Christian Boege (boege at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include "KinematicUnitSimulation.h"
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+#include <Core/core/system/ArmarXDataPath.h>
+
+#include <algorithm>
+
+using namespace armarx;
+
+void KinematicUnitSimulation::onInitKinematicUnit()
+{
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ {
+ std::string jointName = (*it)->getName();
+ jointInfos[jointName].limitLo = (*it)->getJointLimitLo();
+ jointInfos[jointName].limitHi = (*it)->getJointLimitHi();
+ if ((*it)->isRotationalJoint())
+ {
+ if (jointInfos[jointName].limitHi - jointInfos[jointName].limitLo >= float(M_PI * 2))
+ {
+ jointInfos[jointName].reset();
+ }
+ }
+ ARMARX_VERBOSE << jointName << " with limits: lo: " << jointInfos[jointName].limitLo << " hi: " << jointInfos[jointName].limitHi;
+ }
+
+ {
+ // duplicate the loop because of locking
+ boost::mutex::scoped_lock(jointStatesMutex);
+ // initialize JoinStates
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ {
+ std::string jointName = (*it)->getName();
+ jointStates[jointName] = JointState();
+ jointStates[jointName].init();
+ }
+ }
+
+ intervalMs = getProperty<int>("IntervalMs").getValue();
+ ARMARX_VERBOSE << "Starting kinematic unit simulation with " << intervalMs << " ms interval";
+ simulationTask = new PeriodicTask<KinematicUnitSimulation>(this, &KinematicUnitSimulation::simulationFunction, intervalMs, false, "KinematicUnitSimulation");
+}
+
+void KinematicUnitSimulation::onStartKinematicUnit()
+{
+ lastExecutionTime = IceUtil::Time::now();
+ simulationTask->start();
+}
+
+
+void KinematicUnitSimulation::onExitKinematicUnit()
+{
+ simulationTask->stop();
+}
+
+
+void KinematicUnitSimulation::simulationFunction()
+{
+ // the time it took until this method was called again
+ double timeDeltaInSeconds = (IceUtil::Time::now() - lastExecutionTime).toMilliSecondsDouble() / 1000.0;
+ lastExecutionTime = IceUtil::Time::now();
+ // name value maps for reporting
+ NameValueMap actualJointAngles;
+ NameValueMap actualJointVelocities;
+
+ bool aPosValueChanged = false;
+ bool aVelValueChanged = false;
+
+ {
+ boost::mutex::scoped_lock(jointStatesMutex);
+ for ( JointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
+ {
+
+ float newAngle = 0.0f;
+ // calculate joint positions if joint is in velocity control mode
+ if ( it->second.controlMode == eVelocityControl )
+ {
+ double change = (it->second.velocity * timeDeltaInSeconds);
+ newAngle = it->second.angle + change;
+ }
+ else if(it->second.controlMode == ePositionControl)
+ {
+ newAngle = it->second.angle;
+ }
+
+ // constrain the angle
+ JointInfo & jointInfo = jointInfos[it->first];
+ float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : 2.0 * M_PI;
+ float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -2.0 * M_PI;
+ newAngle = std::max<float>(newAngle, minAngle);
+ newAngle = std::min<float>(newAngle, maxAngle);
+
+ // Check if joint existed before
+ JointStates::iterator itPrev = previousJointStates.find(it->first);
+ if(itPrev == previousJointStates.end())
+ {
+ aPosValueChanged = aVelValueChanged = true;
+ }
+
+ // check if the angle has changed
+ if(fabs(previousJointStates[it->first].angle - newAngle) > 0.00000001 )
+ {
+ aPosValueChanged = true;
+ // set the new joint angle locally for the next simulation iteration
+ it->second.angle = newAngle;
+ }
+ // check if velocities have changed
+ if(fabs(previousJointStates[it->first].velocity - it->second.velocity) > 0.00000001 )
+ {
+ aVelValueChanged = true;
+ }
+
+
+ actualJointAngles[it->first] = newAngle;
+ actualJointVelocities[it->first] = it->second.velocity;
+ }
+ previousJointStates = jointStates;
+ }
+
+ listenerPrx->reportJointAngles(actualJointAngles, aPosValueChanged);
+ listenerPrx->reportJointVelocities(actualJointVelocities, aVelValueChanged);
+}
+
+void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c)
+{
+ bool aValueChanged = false;
+ NameControlModeMap changedControlModes;
+ {
+ boost::mutex::scoped_lock(jointStatesMutex);
+ for ( NameControlModeMap::const_iterator it = targetJointModes.begin(); it!=targetJointModes.end(); it++ )
+ {
+ // target values
+ std::string targetJointName = it->first;
+ ControlMode targetControlMode = it->second;
+
+ JointStates::iterator iterJoints = jointStates.find(targetJointName);
+
+ // check whether there is a joint with this name and set joint angle
+ if(iterJoints != jointStates.end())
+ {
+ if(iterJoints->second.controlMode != targetControlMode)
+ aValueChanged = true;
+ iterJoints->second.controlMode = targetControlMode;
+ changedControlModes.insert(*it);
+ }
+ }
+ }
+
+ // only report control modes which have been changed
+ if (aValueChanged)
+ listenerPrx->reportControlModeChanged(changedControlModes, aValueChanged);
+}
+
+
+void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
+{
+
+ boost::mutex::scoped_lock(jointStatesMutex);
+ // name value maps for reporting
+ NameValueMap actualJointAngles;
+ bool aValueChanged = false;
+ for ( NameValueMap::const_iterator it = targetJointAngles.begin(); it!=targetJointAngles.end(); it++ )
+ {
+ // target values
+ std::string targetJointName = it->first;
+ float targetJointAngle = it->second;
+
+ // check whether there is a joint with this name and set joint angle
+ JointStates::iterator iterJoints = jointStates.find(targetJointName);
+ if(iterJoints != jointStates.end())
+ {
+ if (fabs(iterJoints->second.angle - targetJointAngle)>0)
+ aValueChanged = true;
+ if (jointInfos[targetJointName].hasLimits())
+ {
+ targetJointAngle = std::max<float>(targetJointAngle, jointInfos[targetJointName].limitLo);
+ targetJointAngle = std::min<float>(targetJointAngle, jointInfos[targetJointName].limitHi);
+ }
+ iterJoints->second.angle = targetJointAngle;
+ }
+ else
+ ARMARX_WARNING << "Joint not found!";
+ }
+ if (aValueChanged)
+ listenerPrx->reportJointAngles(actualJointAngles, aValueChanged);
+}
+
+void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
+{
+ boost::mutex::scoped_lock(jointStatesMutex);
+ NameValueMap actualJointVelocities;
+ bool aValueChanged = false;
+ for ( NameValueMap::const_iterator it = targetJointVelocities.begin(); it!=targetJointVelocities.end(); it++ )
+ {
+ // target values
+ std::string targetJointName = it->first;
+ float targetJointVelocity = it->second;
+
+ JointStates::iterator iterJoints = jointStates.find(targetJointName);
+ // check whether there is a joint with this name and set joint angle
+ if(iterJoints != jointStates.end())
+ {
+ if (fabs(iterJoints->second.velocity - targetJointVelocity) > 0)
+ aValueChanged = true;
+ iterJoints->second.velocity = targetJointVelocity;
+ actualJointVelocities[targetJointName] = iterJoints->second.velocity;
+ }
+ }
+ if (aValueChanged)
+ listenerPrx->reportJointVelocities(actualJointVelocities, aValueChanged);
+}
+
+void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c)
+{
+ boost::mutex::scoped_lock(jointStatesMutex);
+ NameValueMap actualJointTorques;
+ bool aValueChanged = false;
+ for ( NameValueMap::const_iterator it = targetJointTorques.begin(); it!=targetJointTorques.end(); it++ )
+ {
+ // target values
+ std::string targetJointName = it->first;
+ float targetJointTorque = it->second;
+
+ JointStates::iterator iterJoints = jointStates.find(targetJointName);
+ // check whether there is a joint with this name and set joint angle
+ if(iterJoints != jointStates.end())
+ {
+ if (fabs(iterJoints->second.torque - targetJointTorque) > 0)
+ aValueChanged = true;
+ iterJoints->second.torque = targetJointTorque;
+ actualJointTorques[targetJointName] = iterJoints->second.torque;
+ }
+ }
+ if (aValueChanged)
+ listenerPrx->reportJointTorques(actualJointTorques, aValueChanged);
+}
+
+void KinematicUnitSimulation::setJointAccelerations(const NameValueMap& targetJointAccelerations, const Ice::Current& c)
+{
+}
+
+void KinematicUnitSimulation::setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current& c)
+{
+}
+
+void KinematicUnitSimulation::stop(const Ice::Current &c)
+{
+
+ boost::mutex::scoped_lock(jointStatesMutex);
+ for ( JointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
+ {
+ it->second.velocity = 0;
+ }
+ SensorActorUnit::stop(c);
+}
+
+
+PropertyDefinitionsPtr KinematicUnitSimulation::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(
+ new KinematicUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+}
+
+
+bool mapEntryEqualsString(std::pair<std::string, JointState> iter, std::string compareString)
+{
+ return (iter.first == compareString);
+}
+
+void KinematicUnitSimulation::requestJoints(const StringSequence& joints, const Ice::Current& c)
+{
+ ARMARX_INFO << flush;
+ // if one of the joints belongs to this unit, lock access to this unit for other components except for the requesting one
+ JointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
+ if (jointStates.end() != it)
+ {
+ KinematicUnit::request(c);
+ }
+}
+
+void KinematicUnitSimulation::releaseJoints(const StringSequence& joints, const Ice::Current& c)
+{
+ ARMARX_INFO << flush;
+ // if one of the joints belongs to this unit, unlock access
+ JointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
+ if (jointStates.end() != it)
+ {
+ KinematicUnit::release(c);
+ }
+}
diff --git a/source/RobotAPI/units/KinematicUnitSimulation.h b/source/RobotAPI/units/KinematicUnitSimulation.h
new file mode 100644
index 0000000000000000000000000000000000000000..8765110f8417f7ea27fbdd4c21b336611767dc4e
--- /dev/null
+++ b/source/RobotAPI/units/KinematicUnitSimulation.h
@@ -0,0 +1,162 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Christian Boege (boege dot at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_KINEMATIC_UNIT_SIMULATION_H
+#define _ARMARX_COMPONENT_KINEMATIC_UNIT_SIMULATION_H
+
+#include "KinematicUnit.h"
+
+#include <Core/core/services/tasks/PeriodicTask.h>
+#include <Core/core/system/ImportExportComponent.h>
+
+#include <IceUtil/Time.h>
+
+namespace armarx
+{
+ /**
+ * @class JointState.
+ * @brief State of a joint.
+ * @ingroup SensorActorUnits
+ *
+ * Includes the control mode, the joint angle, velocitym torquem and motor temperature.
+ * Controlmode defaults to ePositionControl. All other values to zero.
+ */
+ class JointState
+ {
+ public:
+ JointState()
+ {
+ init();
+ }
+
+ void init()
+ {
+ controlMode = ePositionControl;
+ angle = 0.0f;
+ velocity = 0.0f;
+ torque = 0.0f;
+ temperature = 0.0f;
+ }
+
+ ControlMode controlMode;
+ float angle;
+ float velocity;
+ float torque;
+ float temperature;
+ };
+ typedef std::map<std::string, JointState> JointStates;
+
+
+ /**
+ * @class JointInfo.
+ * @brief
+ * @ingroup SensorActorUnits
+ *
+ * Joint info including lower and upper joint limits (rad). No limits set by default (lo=hi).
+ */
+ class JointInfo
+ {
+ public:
+ JointInfo()
+ {
+ reset();
+ }
+
+ void reset()
+ {
+ limitLo = 0.0f;
+ limitHi = 0.0f;
+ }
+ bool hasLimits() const
+ {
+ return (limitLo != limitHi);
+ }
+
+ float limitLo;
+ float limitHi;
+ };
+ typedef std::map<std::string, JointInfo> JointInfos;
+
+ /**
+ * @class KinematicUnitSimulationPropertyDefinitions
+ * @brief
+ * @ingroup SensorActorUnits
+ */
+ class KinematicUnitSimulationPropertyDefinitions :
+ public KinematicUnitPropertyDefinitions
+ {
+ public:
+ KinematicUnitSimulationPropertyDefinitions(std::string prefix) :
+ KinematicUnitPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
+ }
+ };
+
+ /**
+ * @class KinematicUnitSimulation
+ * @brief
+ * @ingroup SensorActorUnits
+ */
+ class KinematicUnitSimulation :
+ virtual public KinematicUnit
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() const { return "KinematicUnitSimulation"; }
+
+ virtual void onInitKinematicUnit();
+ virtual void onStartKinematicUnit();
+ virtual void onExitKinematicUnit();
+
+ void simulationFunction();
+
+ // proxy implementation
+ virtual void requestJoints(const StringSequence& joints, const Ice::Current& c = ::Ice::Current());
+ virtual void releaseJoints(const StringSequence& joints, const Ice::Current& c = ::Ice::Current());
+ virtual void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = ::Ice::Current());
+ virtual void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = ::Ice::Current());
+ virtual void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = ::Ice::Current());
+ virtual void setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c = ::Ice::Current());
+ virtual void setJointAccelerations(const NameValueMap& targetJointAccelerations, const Ice::Current& c = ::Ice::Current());
+ virtual void setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current& c = ::Ice::Current());
+ void stop(const Ice::Current &c = Ice::Current());
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ JointStates jointStates;
+ JointStates previousJointStates;
+ JointInfos jointInfos;
+ boost::mutex jointStatesMutex;
+ IceUtil::Time lastExecutionTime;
+ int intervalMs;
+
+ PeriodicTask<KinematicUnitSimulation>::pointer_type simulationTask;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/units/PlatformUnit.cpp b/source/RobotAPI/units/PlatformUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf5b2cd760b6db168c8f1856e7deabbb283c256b
--- /dev/null
+++ b/source/RobotAPI/units/PlatformUnit.cpp
@@ -0,0 +1,67 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Manfred Kroehnert (Manfred dot Kroehnert at kit dot edu)
+ * @date 2013
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include "PlatformUnit.h"
+
+
+using namespace armarx;
+
+
+void PlatformUnit::onInitComponent()
+{
+ std::string platformName = getProperty<std::string>("PlatformName").getValue();
+
+ // component dependencies
+ listenerChannelName = platformName + "State";
+ offeringTopic(listenerChannelName);
+
+ this->onInitPlatformUnit();
+}
+
+
+void PlatformUnit::onConnectComponent()
+{
+ ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
+ listenerPrx = getTopic<PlatformUnitListenerPrx>(listenerChannelName);
+
+ this->onStartPlatformUnit();
+}
+
+
+void PlatformUnit::onExitComponent()
+{
+ this->onExitPlatformUnit();
+}
+
+
+void PlatformUnit::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current &c)
+{
+}
+
+
+PropertyDefinitionsPtr PlatformUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new PlatformUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/units/PlatformUnit.h b/source/RobotAPI/units/PlatformUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..73e0f1fb75dfdb97aec528d58e42d0971cdb7850
--- /dev/null
+++ b/source/RobotAPI/units/PlatformUnit.h
@@ -0,0 +1,130 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Manfred Kroehnert (Manfred dot Kroehnert at kit dot edu)
+ * @date 2013
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_UNITS_PLATFROMUNIT_H
+#define _ARMARX_CORE_UNITS_PLATFROMUNIT_H
+
+#include <RobotAPI/units/SensorActorUnit.h>
+
+#include <Core/core/application/properties/Properties.h>
+#include <Core/core/system/ImportExportComponent.h>
+
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+
+#include <vector>
+
+namespace armarx
+{
+ /**
+ * @class PlatformUnitPropertyDefinitions
+ * @brief Defines all necessary properties for armarx::PlatformUnit
+ * @ingroup SensorActorUnits
+ */
+ class PlatformUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ PlatformUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("PlatformName", "Platform", "Name of the platform (will publish values on PlatformName + 'State')");
+ }
+ };
+
+
+ /**
+ * @class PlatformUnit
+ * @brief This class defines an interface for providing high level access to robot platforms
+ * @ingroup SensorActorUnits
+ *
+ * An instance of a PlatformUnit provides means to set target positions.
+ * It uses the PlatformUnitListener Ice interface to report updates of its current state.
+ *
+ */
+ class PlatformUnit :
+ virtual public PlatformUnitInterface,
+ virtual public SensorActorUnit
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() const
+ {
+ return "PlatformUnit";
+ }
+
+ /**
+ * Retrieve proxy for publishing State information and call
+ * armarx::PlatformUnit::onInitPlatformUnit().
+ * @see armarx::Component::onInitComponent()
+ */
+ virtual void onInitComponent();
+ /**
+ * Calls armarx::PlatformUnit::onStartPlatformUnit().
+ * @see armarx::Component::onConnectComponent()
+ */
+ virtual void onConnectComponent();
+ /**
+ * Calls armarx::PlatformUnit::onExitPlatformUnit().
+ * @see armarx::Component::onExitComponent()
+ */
+ virtual void onExitComponent();
+
+ /**
+ *
+ */
+ virtual void onInitPlatformUnit() = 0;
+ /**
+ *
+ */
+ virtual void onStartPlatformUnit() = 0;
+ /**
+ *
+ */
+ virtual void onExitPlatformUnit() = 0;
+
+ /**
+ * Set a new target position and orientation for the platform.
+ * The platform will move until it reaches the specified target with the specified accuracy.
+ */
+ virtual void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation,
+ Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = ::Ice::Current());
+
+
+ /**
+ * @see armarx::PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ /**
+ * PlatformUnitListener proxy for publishing state updates
+ */
+ PlatformUnitListenerPrx listenerPrx;
+ /**
+ * Ice Topic name on which armarx::PlatformUnit::listenerPrx publishes information
+ */
+ std::string listenerChannelName;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/units/PlatformUnitObserver.cpp b/source/RobotAPI/units/PlatformUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a7485e323f21a29ce1d889880adf92485ff7ca8d
--- /dev/null
+++ b/source/RobotAPI/units/PlatformUnitObserver.cpp
@@ -0,0 +1,113 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Kai Welke (welke _at_ kit _dot_ edu)
+* @date 2011 Kai Welke
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "PlatformUnitObserver.h"
+
+#include "PlatformUnit.h"
+
+#include <Core/observers/checks/ConditionCheckUpdated.h>
+#include <Core/observers/checks/ConditionCheckEquals.h>
+#include <Core/observers/checks/ConditionCheckInRange.h>
+#include <Core/observers/checks/ConditionCheckLarger.h>
+#include <Core/observers/checks/ConditionCheckSmaller.h>
+#include <Core/observers/checks/ConditionCheckValid.h>
+
+
+using namespace armarx;
+
+// ********************************************************************
+// observer framework hooks
+// ********************************************************************
+void PlatformUnitObserver::onInitObserver()
+{
+ // TODO: check if platformNodeName exists?
+ platformNodeName = getProperty<std::string>("PlatformName").getValue();
+
+ // register all channels
+ offerChannel("platformPose", "Current Position of " + platformNodeName);
+ offerChannel("targetPose", "Target Position of " + platformNodeName);
+ offerChannel("platformVelocity", "Current velocity of " + platformNodeName);
+
+ // register all data fields
+ offerDataField("platformPose", "positionX", VariantType::Float, "Current X position of " + platformNodeName + " in mm");
+ offerDataField("platformPose", "positionY", VariantType::Float, "Current Y position of " + platformNodeName + " in mm");
+ offerDataField("platformPose", "rotation", VariantType::Float, "Current Rotation of " + platformNodeName + " in radian");
+
+ offerDataField("targetPose", "positionX", VariantType::Float, "Target X position of " + platformNodeName + " in mm");
+ offerDataField("targetPose", "positionY", VariantType::Float, "Target Y position of " + platformNodeName + " in mm");
+ offerDataField("targetPose", "rotation", VariantType::Float, "Target Rotation of " + platformNodeName + " in radian");
+
+ offerDataField("platformVelocity", "velocityX", VariantType::Float, "Current X velocity of " + platformNodeName + " in mm/s");
+ offerDataField("platformVelocity", "velocityY", VariantType::Float, "Current Y velocity of " + platformNodeName + " in mm/s");
+ offerDataField("platformVelocity", "velocityRotation", VariantType::Float, "Current Rotation velocity of " + platformNodeName + " in radian/s");
+
+ // register all checks
+ offerConditionCheck("valid", new ConditionCheckValid());
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("inrange", new ConditionCheckInRange());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+ usingTopic(platformNodeName + "State");
+}
+
+void PlatformUnitObserver::onConnectObserver()
+{
+}
+
+void PlatformUnitObserver::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
+{
+ nameValueMapToDataFields("platformPose", currentPlatformPositionX, currentPlatformPositionY, currentPlatformRotation);
+ updateChannel("platformPose");
+}
+
+void PlatformUnitObserver::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
+{
+ nameValueMapToDataFields("targetPose", newPlatformPositionX, newPlatformPositionY, newPlatformRotation);
+ updateChannel("targetPose");
+}
+
+void PlatformUnitObserver::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+{
+ setDataField("platformVelocity", "velocityX", currentPlatformVelocityX);
+ setDataField("platformVelocity", "velocityY", currentPlatformVelocityY);
+ setDataField("platformVelocity", "velocityRotation", currentPlatformVelocityRotation);
+ updateChannel("platformVelocity");
+}
+
+// ********************************************************************
+// private methods
+// ********************************************************************
+void PlatformUnitObserver::nameValueMapToDataFields(std::string channelName, ::Ice::Float platformPositionX, ::Ice::Float platformPositionY, ::Ice::Float platformRotation)
+{
+ setDataField(channelName, "positionX", platformPositionX);
+ setDataField(channelName, "positionY", platformPositionY);
+ setDataField(channelName, "rotation", platformRotation);
+}
+
+PropertyDefinitionsPtr PlatformUnitObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new PlatformUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/units/PlatformUnitObserver.h b/source/RobotAPI/units/PlatformUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ac71cbe329ce677e9cf7a8a49ab675a3d079e8e
--- /dev/null
+++ b/source/RobotAPI/units/PlatformUnitObserver.h
@@ -0,0 +1,131 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Kai Welke (welke _at_ kit _dot_ edu)
+* @date 2011 Kai Welke
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_CORE_KINEMATIC_UNIT_OBSERVER_H
+#define _ARMARX_CORE_KINEMATIC_UNIT_OBSERVER_H
+
+#include <Core/core/system/ImportExport.h>
+#include <Core/core/Component.h>
+
+#include <RobotAPI/interface/observers/PlatformUnitObserverInterface.h>
+#include <Core/interface/observers/VariantBase.h>
+
+#include <Core/observers/ConditionCheck.h>
+#include <Core/observers/Observer.h>
+
+#include <string>
+
+namespace armarx
+{
+ ARMARX_CREATE_CHECK(PlatformUnitObserver, equals)
+ ARMARX_CREATE_CHECK(PlatformUnitObserver, larger)
+ ARMARX_CREATE_CHECK(PlatformUnitObserver, inrange)
+
+ /**
+ * @class PlatformUnitObserver
+ * @brief
+ * @ingroup SensorActorUnits
+ *
+ * The PlatformUnitObserver allows the installation of conditions on all channel reported by the PlatformUnit.
+ * These include current and target position.
+ *
+ * The PlatformUnitObserver retrieves its platform configuration via properties.
+ * The name must exist in the used Simox robot model file.
+ */
+ class ARMARXCORE_IMPORT_EXPORT PlatformUnitObserver :
+ virtual public Observer,
+ virtual public PlatformUnitObserverInterface
+ {
+ public:
+ // framework hooks
+ void onInitObserver();
+ void onConnectObserver();
+
+ // slice interface implementation
+ void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = ::Ice::Current());
+ void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current());
+ void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current());
+
+ virtual std::string getDefaultName() const
+ {
+ return "PlatformUnitObserver";
+ };
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ void nameValueMapToDataFields(std::string channelName, ::Ice::Float platformPositionX, ::Ice::Float platformPositionY, ::Ice::Float platformRotation);
+
+ private:
+ std::string platformNodeName;
+ };
+
+ /**
+ @class PlatformUnitDatafieldCreator
+ @brief
+ @ingroup SensorActorUnits
+ */
+ class PlatformUnitDatafieldCreator
+ {
+ public:
+ PlatformUnitDatafieldCreator(const std::string &channelName) :
+ channelName(channelName)
+ {}
+
+ /**
+ * @brief Function to create a Channel Representation for a PlatformUnitObserver
+ * @param platformUnitOberserverName Name of the PlatformUnitObserver
+ * @param platformName Name of the platform of the robot like it is specified
+ * in the simox-robot-xml-file
+ * @return returns a ChannelRepresentationPtr
+ */
+ DataFieldIdentifier getDatafield(const std::string& platformUnitObserverName, const std::string& platformName) const
+ {
+ if(platformUnitObserverName.empty())
+ throw LocalException("kinematicUnitObserverName must not be empty!");
+ if(platformName.empty())
+ throw LocalException("jointName must not be empty!");
+ return DataFieldIdentifier(platformUnitObserverName, channelName, platformName);
+ }
+
+ private:
+ std::string channelName;
+ };
+
+
+ namespace channels
+ {
+ namespace PlatformUnitObserver
+ {
+ const PlatformUnitDatafieldCreator platformPose("platformPose");
+ const PlatformUnitDatafieldCreator targetPose("targetPose");
+ const PlatformUnitDatafieldCreator platformVelocity("platformVelocity");
+ }
+ }
+
+}
+
+#endif
diff --git a/source/RobotAPI/units/PlatformUnitSimulation.cpp b/source/RobotAPI/units/PlatformUnitSimulation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7bc242710f461d51b93af44ad853be3e215991c2
--- /dev/null
+++ b/source/RobotAPI/units/PlatformUnitSimulation.cpp
@@ -0,0 +1,140 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Christian Boege (boege at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include "PlatformUnitSimulation.h"
+
+#include <cmath>
+
+using namespace armarx;
+
+void PlatformUnitSimulation::onInitPlatformUnit()
+{
+ referenceFrame = getProperty<std::string>("ReferenceFrame").getValue();
+ targetPositionX = currentPositionX = getProperty<float>("InitialPosition.x").getValue();
+ targetPositionY = currentPositionY = getProperty<float>("InitialPosition.y").getValue();
+ targetRotation = 0.0;
+ targetRotation = currentRotation = getProperty<float>("InitialRotation").getValue();
+
+ linearVelocity = getProperty<float>("LinearVelocity").getValue();
+ angularVelocity = getProperty<float>("AngularVelocity").getValue();
+
+ positionalAccuracy = 0.01;
+
+ intervalMs = getProperty<int>("IntervalMs").getValue();
+ ARMARX_VERBOSE << "Starting platform unit simulation with " << intervalMs << " ms interval";
+ simulationTask = new PeriodicTask<PlatformUnitSimulation>(this, &PlatformUnitSimulation::simulationFunction, intervalMs, false, "PlatformUnitSimulation");
+}
+
+void PlatformUnitSimulation::onStartPlatformUnit()
+{
+ listenerPrx->reportPlatformPose(currentPositionX, currentPositionY, currentRotation);
+ simulationTask->start();
+}
+
+
+void PlatformUnitSimulation::onExitPlatformUnit()
+{
+ simulationTask->stop();
+}
+
+
+void PlatformUnitSimulation::simulationFunction()
+{
+ // the time it took until this method was called again
+ double timeDeltaInSeconds = (IceUtil::Time::now() - lastExecutionTime).toMilliSecondsDouble() / 1000.0;
+ lastExecutionTime = IceUtil::Time::now();
+ {
+ ScopedLock lock(currentPoseMutex);
+ float diff = fabs(targetPositionX - currentPositionX);
+ if (diff > positionalAccuracy)
+ {
+ int sign = copysignf(1.0f, (targetPositionX - currentPositionX));
+ currentPositionX += sign * std::min<float>(linearVelocity * timeDeltaInSeconds, diff);
+ }
+ diff = fabs(targetPositionY - currentPositionY);
+ if (diff > positionalAccuracy)
+ {
+ int sign = copysignf(1.0f, (targetPositionY - currentPositionY));
+ currentPositionY += sign * std::min<float>(linearVelocity * timeDeltaInSeconds, diff);
+ }
+ diff = fabs(targetRotation - currentRotation);
+ if (diff > orientationalAccuracy)
+ {
+ int sign = copysignf(1.0f, (targetRotation - currentRotation));
+ currentRotation += sign * std::min<float>(angularVelocity * timeDeltaInSeconds, diff);
+ // stay between +/- M_2_PI
+ if(currentRotation > 2 * M_PI)
+ currentRotation -= 2 * M_PI;
+ if(currentRotation < -2 * M_PI)
+ currentRotation += 2 * M_PI;
+ }
+ }
+ listenerPrx->reportPlatformPose(currentPositionX, currentPositionY, currentRotation);
+}
+
+void PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
+{
+ {
+ ScopedLock lock(currentPoseMutex);
+ targetPositionX = targetPlatformPositionX;
+ targetPositionY = targetPlatformPositionY;
+ targetRotation = targetPlatformRotation;
+ this->positionalAccuracy = positionalAccuracy;
+ this->orientationalAccuracy = orientationalAccuracy;
+ }
+ listenerPrx->reportNewTargetPose(targetPositionX, targetPositionY, targetRotation);
+}
+
+
+PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(
+ new PlatformUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+}
+
+
+void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current &c)
+{
+ throw LocalException("NYI");
+}
+
+void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current &c)
+{
+ float targetPositionX, targetPositionY, targetRotation;
+ {
+ ScopedLock lock(currentPoseMutex);
+ targetPositionX = currentPositionX + targetPlatformOffsetX;
+ targetPositionY = currentPositionY + targetPlatformOffsetY;
+ targetRotation = currentRotation + targetPlatformOffsetRotation;
+ }
+ moveTo(targetPositionX, targetPositionY, targetRotation, positionalAccuracy, orientationalAccuracy);
+}
+
+void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current &c)
+{
+ ScopedLock lock(currentPoseMutex);
+ linearVelocity = positionalVelocity;
+ angularVelocity = orientaionalVelocity;
+}
+
diff --git a/source/RobotAPI/units/PlatformUnitSimulation.h b/source/RobotAPI/units/PlatformUnitSimulation.h
new file mode 100644
index 0000000000000000000000000000000000000000..8260c99662d23e27140d9c816762c91af3248fbc
--- /dev/null
+++ b/source/RobotAPI/units/PlatformUnitSimulation.h
@@ -0,0 +1,117 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Christian Boege (boege dot at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_KINEMATIC_UNIT_SIMULATION_H
+#define _ARMARX_COMPONENT_KINEMATIC_UNIT_SIMULATION_H
+
+#include "PlatformUnit.h"
+
+#include <Core/core/services/tasks/PeriodicTask.h>
+#include <Core/core/system/ImportExportComponent.h>
+
+#include <IceUtil/Time.h>
+
+#include <string>
+
+namespace armarx
+{
+ /**
+ * @class PlatformUnitSimulationPropertyDefinitions
+ * @brief
+ * @ingroup SensorActorUnits
+ */
+ class PlatformUnitSimulationPropertyDefinitions :
+ public PlatformUnitPropertyDefinitions
+ {
+ public:
+ PlatformUnitSimulationPropertyDefinitions(std::string prefix) :
+ PlatformUnitPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
+ defineOptionalProperty<float>("InitialRotation", 0, "Initial rotation of the platform.");
+ defineOptionalProperty<float>("InitialPosition.x", 0, "Initial x position of the platform.");
+ defineOptionalProperty<float>("InitialPosition.y", 0, "Initial y position of the platform.");
+ defineOptionalProperty<std::string>("ReferenceFrame", "Platform", "Reference frame in which the platform position is reported.");
+ defineOptionalProperty<float>("LinearVelocity", 1000.0, "Linear velocity of the platform (default: 1000 mm/sec).");
+ defineOptionalProperty<float>("AngularVelocity", 1.5, "Angular velocity of the platform (default: 1.5 rad/sec).");
+ }
+ };
+
+ /**
+ * @class PlatformUnitSimulation
+ * @brief
+ * @ingroup SensorActorUnits
+ */
+ class PlatformUnitSimulation :
+ virtual public PlatformUnit
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() const
+ {
+ return "PlatformUnitSimulation";
+ }
+
+ virtual void onInitPlatformUnit();
+ virtual void onStartPlatformUnit();
+ virtual void onExitPlatformUnit();
+
+ void simulationFunction();
+
+ // proxy implementation
+ void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = ::Ice::Current());
+ void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current &c = Ice::Current());
+ void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current &c = Ice::Current());
+ void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current &c = Ice::Current());
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ boost::mutex currentPoseMutex;
+ IceUtil::Time lastExecutionTime;
+ int intervalMs;
+
+ ::Ice::Float targetPositionX;
+ ::Ice::Float targetPositionY;
+ ::Ice::Float currentPositionX;
+ ::Ice::Float currentPositionY;
+ ::Ice::Float targetRotation;
+ ::Ice::Float currentRotation;
+
+ ::Ice::Float linearVelocity;
+ ::Ice::Float angularVelocity;
+
+ ::Ice::Float positionalAccuracy;
+ ::Ice::Float orientationalAccuracy;
+
+ std::string referenceFrame;
+
+ PeriodicTask<PlatformUnitSimulation>::pointer_type simulationTask;
+
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/units/SensorActorUnit.cpp b/source/RobotAPI/units/SensorActorUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25ef13d932328fc40630ce0c0c7f27869c673a4c
--- /dev/null
+++ b/source/RobotAPI/units/SensorActorUnit.cpp
@@ -0,0 +1,157 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarXCore::untis
+* @author Kai Welke (welke@kit.edu)
+* @date 2011
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "SensorActorUnit.h"
+
+using namespace armarx;
+
+#define CALLINFO //ARMARX_INFO <<__FILE__ << ":" << __PRETTY_FUNCTION__ << flush;
+
+SensorActorUnit::SensorActorUnit()
+{
+ executionState = eUnitConstructed;
+}
+
+SensorActorUnit::~SensorActorUnit()
+{
+
+}
+
+void SensorActorUnit::init(const Ice::Current& c)
+{
+ std::string currentName = c.adapter->getName();
+ Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
+ if(ownerId == currentId)
+ {
+ CALLINFO
+ if(executionState == eUnitConstructed)
+ {
+ executionState = eUnitInitialized;
+ onInit();
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " is already initialized." << armarx::flush;
+ }
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be initialized while unrequested." << armarx::flush;
+ }
+}
+
+void SensorActorUnit::start(const Ice::Current& c)
+{
+ std::string currentName = c.adapter->getName();
+ Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
+ if(ownerId == currentId)
+ {
+ CALLINFO
+ if( (executionState == eUnitInitialized ) || (executionState == eUnitStopped) )
+ {
+ executionState = eUnitStarted;
+ onStart();
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " could not be started." << armarx::flush;
+ }
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be started while unrequested." << armarx::flush;
+ }
+}
+
+void SensorActorUnit::stop(const Ice::Current& c)
+{
+ std::string currentName = c.adapter->getName();
+ Ice::Identity currentId = c.adapter->getCommunicator()->stringToIdentity(currentName);
+ if(ownerId == currentId)
+ {
+ CALLINFO
+ if(executionState == eUnitStarted)
+ {
+ executionState = eUnitStopped;
+ onStop();
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " could not be stopped." << armarx::flush;
+ }
+ }
+ else
+ {
+ ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be stopped while unrequested. " << armarx::flush;
+ }
+}
+
+UnitExecutionState SensorActorUnit::getExecutionState(const Ice::Current& c)
+{
+ CALLINFO
+ return executionState;
+}
+
+
+void SensorActorUnit::request(const Ice::Current& c)
+{
+ CALLINFO
+ // try to lock unit
+ if(!unitMutex.try_lock())
+ {
+ ARMARX_LOG << armarx::eERROR << "Trying to request already owned unit " << getName() << "\n" << armarx::flush;
+ throw ResourceUnavailableException("Trying to request already owned unit");
+ }
+
+ // retrieve owner id from current connection
+ std::string ownerName = c.adapter->getName();
+ ownerId = c.adapter->getCommunicator()->stringToIdentity(ownerName);
+
+ ARMARX_INFO << "unit requested by "<< ownerName << flush;
+}
+
+void SensorActorUnit::release(const Ice::Current& c)
+{
+ CALLINFO
+ // retrieve owner id from current connection
+ std::string callerName = c.adapter->getName();
+ Ice::Identity callerId = c.adapter->getCommunicator()->stringToIdentity(callerName);
+
+ if(!(ownerId == callerId))
+ {
+ ARMARX_LOG << armarx::eERROR << "Trying to release unit owned by another component" << armarx::flush;
+ throw ResourceNotOwnedException("Trying to release unit owned by another component");
+ }
+
+ // unlock mutex
+ ownerId = c.adapter->getCommunicator()->stringToIdentity(" ");
+ unitMutex.unlock();
+
+ ARMARX_INFO << "unit released" << flush;
+}
+
+void SensorActorUnit::onExitComponent()
+{
+ unitMutex.try_lock(); // try to lock, to ensure that it is locked on unlock call
+ unitMutex.unlock();
+}
diff --git a/source/RobotAPI/units/SensorActorUnit.h b/source/RobotAPI/units/SensorActorUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..1436072d6240c27c54328a52b09af612263197d9
--- /dev/null
+++ b/source/RobotAPI/units/SensorActorUnit.h
@@ -0,0 +1,142 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarXCore::utils
+* @author Kai Welke (welke@kit.edu)
+* @date 2011
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef SENSOR_ACTOR_UNIT_H
+#define SENSOR_ACTOR_UNIT_H
+
+#include <Core/core/system/ImportExport.h>
+#include <Core/core/Component.h>
+#include <Core/core/exceptions/Exception.h>
+#include <RobotAPI/interface/units/UnitInterface.h>
+
+/**
+ @defgroup SensorActorUnits ArmarX Sensor-Actor Units
+
+ Sensor-Actor units
+
+ @li provide resource management (e.g. exclusive access) @see KinematicUnit
+ @li receive control data through Ice remote procedure calls (e.g. setTargetJointAngles(...)) @see KinematicUnit
+ @li publish sensor data through Ice topics @see KinematicUnitListener
+ */
+
+namespace armarx
+{
+ /**
+ * Base Class for all ArmarX SensorActorUnits.
+ *
+ * SensorActorUnits are ArmarX component which provide an actor API and a sensor data stream.
+ * The SensorActorUnit provides basic execution state handling and concurrent access handling.
+ */
+ class ARMARXCORE_IMPORT_EXPORT SensorActorUnit :
+ virtual public SensorActorUnitInterface,
+ virtual public Component
+ {
+ public:
+ /**
+ * Constructs a SensorActorUnit.
+ */
+ SensorActorUnit();
+ /**
+ * Destructor of SensorActorUnit.
+ */
+ virtual ~SensorActorUnit();
+
+ /**
+ * Set execution state to eInitialized.
+ *
+ * Assures that init is called only once and the calls subclass method onInit().
+ *
+ * @param c Ice context provided by the Ice framework
+ */
+ virtual void init(const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * Set execution state to eStarted
+ *
+ * Start streaming of sensory data and acceptance of control data.
+ *
+ * Start can be called if the unit is initialized and not started yet (stopped).
+ * Calls subclass method inStart().
+ *
+ * @param c Ice context provided by the Ice framework
+ */
+ virtual void start(const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * Set execution state to eStopped
+ *
+ * Stop streaming of sensory data and acceptance of control data.
+ *
+ * Stop can be called if the unit is started.
+ * Calls subclass method onStop()
+ *
+ * @param c Ice context provided by the Ice framework
+ */
+ virtual void stop(const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * Retrieve current execution state
+ *
+ * @param c Ice context provided by the Ice framework
+ * @return current execution state
+ */
+ UnitExecutionState getExecutionState(const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * Request exclusive access to current unit. Throws ResourceUnavailableException on error.
+ *
+ * @param c Ice context provided by the Ice framework
+ */
+ virtual void request(const Ice::Current& c = ::Ice::Current());
+
+ /**
+ * Release exclusive access to current unit. Throws ResourceUnavailableException on error.
+ *
+ * @param c Ice context provided by the Ice framework
+ */
+ virtual void release(const Ice::Current& c = ::Ice::Current());
+
+ protected:
+ void onExitComponent();
+ /**
+ * callback onInit for subclass hook. See init().
+ */
+ virtual void onInit() {};
+
+ /**
+ * callback onStart for subclass hook. See start().
+ */
+ virtual void onStart() {};
+
+ /**
+ * callback onStop for subclass hook. See stop().
+ */
+ virtual void onStop() {};
+
+ boost::mutex unitMutex;
+ Ice::Identity ownerId;
+ UnitExecutionState executionState;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 7c49c31783e36d54fd4baeb8a5927601dcc21360..a0f8424a7232b2d55519e5b853472e84064db02d 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -22,7 +22,7 @@
*/
#include "TCPControlUnit.h"
-#include <Core/robotstate/remote/LinkedPose.h>
+#include <RobotAPI/robotstate/remote/LinkedPose.h>
#include <boost/unordered_map.hpp>
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index be466c92686f8f0933a8c3906b9f46026d2d6fe7..3349a4178c81b1a06b447808cf2704ea1e1f1832 100644
--- a/source/RobotAPI/units/TCPControlUnit.h
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -25,12 +25,12 @@
#define _ARMARX_TCPCONTROLUNIT_H
#include <RobotAPI/interface/units/TCPControlUnit.h>
-#include <Core/robotstate/remote/ArmarPose.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
#include <Core/core/services/tasks/PeriodicTask.h>
#include <Core/core/Component.h>
#include <VirtualRobot/IK/DifferentialIK.h>
-#include <Core/robotstate/remote/RemoteRobot.h>
+#include <RobotAPI/robotstate/remote/RemoteRobot.h>
#include <Core/interface/observers/ObserverInterface.h>
namespace armarx {
diff --git a/source/RobotAPI/units/TCPControlUnitObserver.cpp b/source/RobotAPI/units/TCPControlUnitObserver.cpp
index 3514c6b9b0528b73e7cac2497810a60abe7c0e4b..685c73779a4a4fd51c7be46ed429f20899e6b88d 100644
--- a/source/RobotAPI/units/TCPControlUnitObserver.cpp
+++ b/source/RobotAPI/units/TCPControlUnitObserver.cpp
@@ -29,7 +29,7 @@
#include <Core/observers/checks/ConditionCheckInRange.h>
#include <Core/observers/checks/ConditionCheckLarger.h>
#include <Core/observers/checks/ConditionCheckSmaller.h>
-#include <Core/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+#include <RobotAPI/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
#include <Core/core/exceptions/local/ExpressionException.h>
#define TCP_POSE_CHANNEL "TCPPose"
diff --git a/source/RobotAPI/units/TCPMoverUnit.cpp b/source/RobotAPI/units/TCPMoverUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc16e5a605ba849692d16c3e609a589905bdfdda
--- /dev/null
+++ b/source/RobotAPI/units/TCPMoverUnit.cpp
@@ -0,0 +1,319 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Christian Boege (boege at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include "TCPMoverUnit.h"
+
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
+#include <Core/core/system/ArmarXDataPath.h>
+
+//VirtualRobot
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/LinkedCoordinate.h>
+
+
+
+
+/** VirtualRobot headers **/
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <Eigen/Geometry>
+
+using namespace armarx;
+using namespace std;
+using namespace VirtualRobot;
+
+
+TCPMoverUnit::TCPMoverUnit():
+ selectedHand(eNone)
+
+{
+ handData[eLeftHand];
+ handData[eRightHand];
+}
+
+
+void TCPMoverUnit::onInitComponent()
+{
+ kinematicUnitFile = getProperty<std::string>("RobotFileName").getValue();
+ kinematicUnitName = getProperty<std::string>("RobotNodeSetName").getValue();
+ if (!ArmarXDataPath::getAbsolutePath(kinematicUnitFile,kinematicUnitFile))
+ {
+ throw UserException("Could not find robot file " + kinematicUnitFile);
+ }
+ // Load Robot
+ ARMARX_VERBOSE << "Loading KinematicUnit " << kinematicUnitName << " from '" << kinematicUnitFile << "' ..." << flush;
+
+
+ robot = RobotIO::loadRobot(kinematicUnitFile);
+ if (!robot)
+ {
+ ARMARX_ERROR << "Could not find Robot XML file with name '" << kinematicUnitFile << "'" << flush;
+ }
+
+
+ usingProxy(kinematicUnitName);
+
+ periodicTask = new PeriodicTask<TCPMoverUnit>(this, &TCPMoverUnit::periodicExec, 40, false, "TCPMoverUnit");
+}
+
+
+void TCPMoverUnit::onConnectComponent()
+{
+ kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+
+
+ handData[eLeftHand].setTCPNodeSet(robot->getRobotNodeSet("LARM"));
+ handData[eRightHand].setTCPNodeSet(robot->getRobotNodeSet("RARM"));
+ resetToHomePosition(eLeftHand);
+ resetToHomePosition(eRightHand);
+
+ ARMARX_VERBOSE << "loaded";
+ periodicTask->start();
+}
+
+
+void TCPMoverUnit::onExitComponent()
+{
+ try{
+ if(robotRequested && kinematicUnitPrx)
+ kinematicUnitPrx->release();
+ }catch(Ice::NotRegisteredException e)
+ {
+// ARMARX_ERROR << "Lost connection - shutting down" << flush;
+// this->interrupt();
+ }
+ periodicTask->stop();
+}
+
+
+void TCPMoverUnit::periodicExec()
+{
+ this->calculateNewHandPosition(eLeftHand);
+ this->calculateNewHandPosition(eRightHand);
+ return;
+
+
+}
+
+void TCPMoverUnit::request(const Ice::Current &)
+{
+ try{
+ kinematicUnitPrx->request();
+ robotRequested = true;
+ }catch(ResourceUnavailableException &e)
+ {
+ ARMARX_WARNING << "Resource is unavailable" << flush;
+ }
+}
+
+void TCPMoverUnit::release(const Ice::Current &)
+{
+ try
+ {
+ kinematicUnitPrx->release();
+ robotRequested = false;
+ }
+ catch(ResourceNotOwnedException &e)
+ {
+ ARMARX_WARNING << "Resource is not owned" << flush;
+ }
+}
+
+
+void TCPMoverUnit::resetArmToHomePosition(HandSelection selectedHand, const Ice::Current &)
+{
+ resetToHomePosition(selectedHand);
+}
+
+
+void TCPMoverUnit::setCartesianTCPVelocity(HandSelection selectedHand, Ice::Float x, Ice::Float y, Ice::Float z, Ice::Float speedFactor, const Ice::Current &)
+{
+ if(selectedHand != eLeftHand && selectedHand != eRightHand)
+ throw LocalException("Please select a specific hand.");
+ if(handData.find(selectedHand) == handData.end())
+ throw LocalException("HandData not found.");
+ HandData &hand = handData[selectedHand];
+ ARMARX_VERBOSE << "setting new cartesian tcp velocity";
+ this->selectedHand = selectedHand;
+ hand.velocities[0] = x*speedFactor;
+ hand.velocities[1] = y*speedFactor;
+ hand.velocities[2] = z*speedFactor;
+
+}
+
+
+void TCPMoverUnit::setTCPOrientation(HandSelection selectedHand, Ice::Float alpha, Ice::Float beta, Ice::Float gamma, const Ice::Current &)
+{
+ if(selectedHand != eLeftHand && selectedHand != eRightHand)
+ throw LocalException("Please select a specific hand.");
+ if(handData.find(selectedHand) == handData.end())
+ throw LocalException("HandData not found.");
+ HandData &hand = handData[selectedHand];
+ ARMARX_VERBOSE << "setting new tcp orientation";
+ this->selectedHand = selectedHand;
+ hand.orientation[0] = alpha;
+ hand.orientation[1] = beta;
+ hand.orientation[2] = gamma;
+}
+
+
+void TCPMoverUnit::setPlatformVelocities(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current &)
+{
+}
+
+void TCPMoverUnit::setRotationMat(Eigen::Matrix4f &matrix, float alpha, float beta, float gamma)
+{
+ using namespace Eigen;
+ const float sinfalpha = sin(alpha);
+ const float cosfalpha = cos(alpha);
+ const float sinfbeta = sin(beta);
+ const float cosfbeta = cos(beta);
+ const float sinfgamma = sin(gamma);
+ const float cosfgamma = cos(gamma);
+
+ matrix(0,0) = cosfbeta * cosfgamma;
+ matrix(0,1) = - cosfbeta * sinfgamma;
+ matrix(0,2) = sinfbeta;
+ matrix(1,0) = cosfalpha * sinfgamma + sinfalpha * sinfbeta * cosfgamma;
+ matrix(1,1) = cosfalpha * cosfgamma - sinfalpha * sinfbeta * sinfgamma;
+ matrix(1,2) = - sinfalpha * cosfbeta;
+ matrix(2,0) = sinfalpha * sinfgamma - cosfalpha * sinfbeta * cosfgamma;
+ matrix(2,1) = sinfalpha * cosfgamma + cosfalpha * sinfbeta * sinfgamma;
+ matrix(2,2) = cosfalpha * cosfbeta;
+}
+
+
+void TCPMoverUnit::resetToHomePosition(HandSelection selectedHand)
+{
+ if(selectedHand != eLeftHand && selectedHand != eRightHand)
+ throw LocalException("Please select a specific hand.");
+ if(handData.find(selectedHand) == handData.end())
+ throw LocalException("HandData not found.");
+ HandData &hand = handData[selectedHand];
+ hand.orientation.resize(3, 0.f);
+ std::vector<float> targetVec(19,0.f);
+ targetVec[11] = -0.009199;
+ targetVec[14] = -3.14/2;
+ targetVec[15] = -90.f/180*3.145;
+ if(selectedHand == eRightHand)
+ targetVec[15] *= -1;
+
+ robot->setJointValues(hand.getTCPNodeSet(),targetVec);
+ hand.standardRot = hand.getTCPNodeSet()->getTCP()->getGlobalPoseVisualization();
+}
+
+void TCPMoverUnit::setHandConfiguration(HandSelection, Ice::Int, const Ice::Current &)
+{
+ ARMARX_INFO << "not yet implemented" << std::endl;
+}
+
+
+void TCPMoverUnit::calculateNewHandPosition(HandSelection selectedHand)
+{
+ if(selectedHand != eLeftHand && selectedHand != eRightHand)
+ throw LocalException("Please select a specific hand.");
+ if(handData.find(selectedHand) == handData.end())
+ throw LocalException("HandData not found.");
+ HandData &hand = handData[selectedHand];
+ RobotNodePtr tcpNode = hand.getTCPNodeSet()->getTCP();
+ using namespace Eigen;
+
+
+ Matrix4f pose = tcpNode->getGlobalPoseVisualization();
+// cout << "angles: \n" << orientation[0] << ", " << orientation[1] << ", " << orientation[2] << endl;
+ Matrix4f targetPose;
+ Matrix4f xRot;
+ xRot << 1, 0, 0, 0,
+ 0, cos(hand.orientation[0]), -sin(hand.orientation[0]), 0,
+ 0, sin(hand.orientation[0]), cos(hand.orientation[0]), 0,
+ 0, 0, 0, 1;
+
+ Matrix4f yRot;
+ yRot << cos(hand.orientation[1]), 0, sin(hand.orientation[1]), 0,
+ 0, 1, 0, 0,
+ -sin(hand.orientation[1]), 0, cos(hand.orientation[1]), 0,
+ 0, 0, 0, 1;
+
+
+ Matrix4f zRot;
+ zRot <<
+ cos(hand.orientation[2]), -sin(hand.orientation[2]), 0, 0,
+ sin(hand.orientation[2]), cos(hand.orientation[2]), 0, 0,
+ 0, 0, 1, 0,
+ 0, 0, 0, 1;
+
+
+ targetPose = xRot*yRot*zRot*hand.standardRot;
+ targetPose(0,3) = pose(0,3) + hand.velocities[0];
+ targetPose(1,3) = pose(1,3) + hand.velocities[1];
+ targetPose(2,3) = pose(2,3) + hand.velocities[2];
+// std::cout << targetPose << std::endl;
+ hand.ik->setGoal(targetPose, tcpNode, IKSolver::All);
+
+ VectorXf errorJoint(hand.getTCPNodeSet()->getSize());
+ errorJoint = hand.ik->computeStep(0.1f);
+
+ // assign new values to local robot and to kinematicUnit
+ std::vector<float> angles = hand.getTCPNodeSet()->getJointValues();
+ std::vector<float> targetAngles(hand.getTCPNodeSet()->getSize());
+ NameValueMap targetMap;
+
+ for(unsigned int i = 0; i < hand.getTCPNodeSet()->getSize(); i++)
+ {
+ float newAngle = angles[i] + errorJoint(i);
+ targetAngles[i] = newAngle;
+ targetMap[hand.getTCPNodeSet()->getNode(i)->getName()] = newAngle;
+ //ARMARX_VERBOSE << tcpNodeSet->getNode(i)->getName() << ": " << newAngle;
+ }
+ robot->setJointValues(hand.getTCPNodeSet(), targetAngles);
+
+ try{
+ kinematicUnitPrx->setJointAngles(targetMap);
+ }catch(Ice::NotRegisteredException e)
+ {
+ ARMARX_ERROR << "Lost connection - shutting down" << flush;
+ this->terminate();
+ }
+}
+
+
+void TCPMoverUnit::lookWithHeadTo(Ice::Float y, Ice::Float, Ice::Float, const Ice::Current &)
+{
+}
+
+
+void TCPMoverUnit::HandData::setTCPNodeSet(RobotNodeSetPtr tcpNodeSet)
+{
+ this->tcpNodeSet = tcpNodeSet; ik = VirtualRobot::DifferentialIKPtr(new DifferentialIK(tcpNodeSet));
+}
+
+
+PropertyDefinitionsPtr TCPMoverUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new TCPMoverUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/units/TCPMoverUnit.h b/source/RobotAPI/units/TCPMoverUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6b91cbd8fd9ef2dcfb9fb6e5a08e3f5a0f9c9f5
--- /dev/null
+++ b/source/RobotAPI/units/TCPMoverUnit.h
@@ -0,0 +1,156 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarXCore::units
+ * @author Christian Boege (boege dot at kit dot edu)
+ * @date 2011
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_TCPMOVER_UNIT_H
+#define _ARMARX_COMPONENT_TCPMOVER_UNIT_H
+
+#include "KinematicUnit.h"
+
+#include <Core/core/Component.h>
+#include <Core/core/services/tasks/PeriodicTask.h>
+#include <Core/core/application/properties/Properties.h>
+#include <Core/core/system/ImportExportComponent.h>
+#include <RobotAPI/interface/units/TCPMoverUnitInterface.h>
+
+//
+#include <IceUtil/Time.h>
+
+//
+#include <Eigen/Core>
+
+#ifndef WIN32
+#define KINEMATIC_UNIT_FILE_DEFAULT std::string(std::getenv("ArmarXHome_DIR")?std::getenv("ArmarXHome_DIR"):"../../")+std::string("/data/ArmarIV/RobotModel/ArmarIV.xml")
+#else
+#define KINEMATIC_UNIT_FILE_DEFAULT "/org/share/staff/home/waechter/projects/armarx/trunk/data/ArmarIV/RobotModel/ArmarIV.xml"
+#endif
+#define KINEMATIC_UNIT_NAME_DEFAULT "RobotKinematicUnit"
+
+namespace VirtualRobot {
+ class Robot;
+ class RobotNodeSet;
+ class DifferentialIK;
+ typedef boost::shared_ptr<Robot> RobotPtr;
+ typedef boost::shared_ptr<RobotNodeSet> RobotNodeSetPtr;
+ typedef boost::shared_ptr<DifferentialIK> DifferentialIKPtr;
+
+}
+
+namespace armarx
+{
+
+ /**
+ * @class TCPMoverUnitPropertyDefinitions
+ * @brief
+ * @ingroup SensorActorUnits
+ */
+ class TCPMoverUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ TCPMoverUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("RobotNodeSetName", KINEMATIC_UNIT_NAME_DEFAULT, "Robot node name");
+ defineOptionalProperty<std::string>("RobotFileName", KINEMATIC_UNIT_FILE_DEFAULT, "Robot file name, e.g. robot_model.xml");
+ }
+ };
+
+
+ /**
+ @class TCPMoverUnit
+ @brief
+ @ingroup SensorActorUnits
+ */
+ class TCPMoverUnit :
+ virtual public Component,
+ virtual public TCPMoverUnitInterface
+ {
+ public:
+ struct HandData{
+ HandSelection hand;
+ std::vector<float> velocities;
+ std::vector<float> orientation;
+ Eigen::Matrix4f standardRot;
+ VirtualRobot::DifferentialIKPtr ik;
+
+
+ HandData():
+ hand(eNone),
+ velocities(3, 0.f),
+ orientation(3, 0.f)
+ {}
+
+ void setTCPNodeSet( VirtualRobot::RobotNodeSetPtr tcpNodeSet);
+ VirtualRobot::RobotNodeSetPtr getTCPNodeSet(){ return tcpNodeSet;}
+
+ protected:
+ VirtualRobot::RobotNodeSetPtr tcpNodeSet;
+
+ };
+
+ TCPMoverUnit();
+ // inherited from Component
+ virtual std::string getDefaultName() const { return "TCPMoverUnit"; }
+
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+ virtual void onExitComponent();
+
+ virtual void periodicExec();
+
+ void request(const Ice::Current &);
+ void release(const Ice::Current &);
+ void resetArmToHomePosition(HandSelection selectedHand, const Ice::Current &);
+ void lookWithHeadTo(Ice::Float x, Ice::Float y, Ice::Float z, const Ice::Current &);
+ void setCartesianTCPVelocity(HandSelection selectedHand, Ice::Float x, Ice::Float y, Ice::Float z, Ice::Float speedFactor, const Ice::Current &);
+ void setTCPOrientation(HandSelection selectedHand, Ice::Float alpha, Ice::Float beta, Ice::Float gamma, const Ice::Current &);
+ void setPlatformVelocities(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current &);
+ void setRotationMat(Eigen::Matrix4f &matrix, float alpha, float beta, float gamma);
+ void resetToHomePosition(HandSelection selectedHand);
+ void setHandConfiguration(HandSelection, Ice::Int, const Ice::Current &);
+
+
+ void stopArm(HandSelection, const Ice::Current &){}
+ void stopHead(const Ice::Current &){}
+ void stopPlatform(const Ice::Current &){}
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ void calculateNewHandPosition(HandSelection selectedHand);
+
+ private:
+ std::map<HandSelection, HandData> handData;
+ IceUtil::Time lastExec;
+ HandSelection selectedHand;
+ NameValueMap velocityMap;
+ bool robotRequested;
+
+ std::string kinematicUnitName;
+ std::string kinematicUnitFile;
+ VirtualRobot::RobotPtr robot;
+ KinematicUnitInterfacePrx kinematicUnitPrx;
+ PeriodicTask<TCPMoverUnit>::pointer_type periodicTask;
+ };
+}
+
+#endif