diff --git a/CMakeLists.txt b/CMakeLists.txt
index f6aa2024fb4bb00397189d1dc3d17239ff5d4a2f..49090a7ca5ada7076b62af3306f636ccb2264ab3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,7 +14,7 @@ set(ARMARX_ENABLE_AUTO_CODE_FORMATTING TRUE)
armarx_project("RobotAPI")
depends_on_armarx_package(ArmarXGui "OPTIONAL")
-set(ArmarX_Simox_VERSION 2.3.25)
+set(ArmarX_Simox_VERSION 2.3.34)
find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
if (Simox_FOUND)
diff --git a/build/.gitkeep b/build/.gitkeep
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index f316bed3e6dfb25dff5297b1cd06e50bb42e2cac..6fc938f2aa15c131f1d83050124bc4bcc15030ca 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -113,6 +113,14 @@
<onConnect>remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));</onConnect>
<stateMethod header="const VirtualRobot::RobotPtr getRobot() const">return %getContext%->getRobot();</stateMethod>
</Proxy>
+ <Proxy include="RobotAPI/interface/components/ViewSelectionInterface.h"
+ humanName="Automatic View Selection"
+ typeName="ViewSelectionInterfacePrx"
+ memberName="viewSelection"
+ getterName="getViewSelection"
+ propertyName="ViewSelectionName"
+ propertyIsOptional="true"
+ propertyDefaultValue="ViewSelection" />
<Topic include="RobotAPI/interface/visualization/DebugDrawerInterface.h"
humanName="Debug Drawer Topic"
typeName="DebugDrawerInterfacePrx"
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII.xml b/data/RobotAPI/robots/Armar3/ArmarIII.xml
index 041a490471e1605a95d739bb98fb90c94b11fea1..32a21df52906290dfae10c0bbbb98c8da5335959 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII.xml
@@ -123,7 +123,7 @@
<RobotNode name="Hip Yaw">
<Joint type="revolute">
<Axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-90" hi="90"/>
+ <Limits unit="degree" lo="-130" hi="130"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
<MaxTorque value="3000"/>
diff --git a/data/RobotAPI/tests/robotmodel/ArmarIII-Head.xml b/data/RobotAPI/tests/robotmodel/ArmarIII-Head.xml
deleted file mode 100644
index bf0056f808b59d2a02aeafa36c8f5657ad450479..0000000000000000000000000000000000000000
--- a/data/RobotAPI/tests/robotmodel/ArmarIII-Head.xml
+++ /dev/null
@@ -1,472 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-
-<Robot Type="ArmarIII Head" RootNode="Head Base">
-
- <RobotNode name="Head Base">
- <Transform>
- <DH theta="90" d="0" a="0" alpha="0" unitsangle="degree" unitslength="mm"/>
- </Transform>
-
- <Visualization>
- <File type="Inventor">fullmodel/head_base.iv</File>
- </Visualization>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="4" units="kg" />
- </Physics>
-
- <CollisionModel>
- <File type="Inventor">convexModel/head_base.iv</File>
- </CollisionModel>
-
- <Child name="Neck_1_Pitch"/>
- </RobotNode>
-
- <RobotNode name="Neck_1_Pitch">
- <Transform>
- <DH theta="0" d="0" a="0" alpha="90" unitsangle="degree" unitslength="mm"/>
- <DH theta="90" d="0" a="0" alpha="0" unitsangle="degree" unitslength="mm"/>
- </Transform>
-
-
- <Joint type="revolute">
- <!--DH theta="90" d="0" a="0" alpha="90" unitsangle="degree" unitslength="mm"/-->
- <axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-45" hi="45"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
-
- <Visualization>
- <File type="Inventor">fullmodel/neck_pitch_link.iv</File>
- </Visualization>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="4" units="kg" />
- <IgnoreCollision name="Hip Yaw"/>
- </Physics>
-
- <CollisionModel>
- <File type="Inventor">convexModel/neck_pitch_link.iv</File>
- </CollisionModel>
-
- <Child name="Neck_2_Roll"/>
- </RobotNode>
-
- <RobotNode name="Neck_2_Roll">
- <Transform>
- <DH a="0" d="0" theta="0" alpha="90" unitsangle="degree" unitslength="mm"/>
- <DH a="0" d="0" theta="90" alpha="0" unitsangle="degree" unitslength="mm"/>
- </Transform>
- <Joint type="revolute">
- <!--DH theta="90" d="0" a="0" alpha="90" unitsangle="degree" unitslength="mm"/-->
- <axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-45" hi="45"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
-
- <Visualization>
- <File type="Inventor">fullmodel/neck_roll_link.iv</File>
- </Visualization>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="2" units="kg" />
- <IgnoreCollision name="Hip Yaw"/>
- <IgnoreCollision name="Head Base"/>
- </Physics>
-
- <CollisionModel>
- <File type="Inventor">convexModel/neck_roll_link.iv</File>
- </CollisionModel>
-
- <Child name="Neck_3_Yaw"/>
- </RobotNode>
-
- <RobotNode name="Neck_3_Yaw">
- <Transform>
- <DH a="0" d="0" theta="0" alpha="90" unitsangle="degree" unitslength="mm"/>
- <DH a="0" d="0" theta="90" alpha="0" unitsangle="degree" unitslength="mm"/>
- </Transform>
- <Joint type="revolute">
- <!--DH theta="90" d="120" a="0" alpha="90" unitsangle="degree" unitslength="mm"/-->
- <axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-45" hi="45"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
-
- <Visualization>
- <File type="Inventor">fullmodel/neck_yaw_link.iv</File>
- </Visualization>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="2" units="kg" />
- </Physics>
-
- <CollisionModel>
- <File type="Inventor">convexModel/neck_yaw_link.iv</File>
- </CollisionModel>
-
- <Child name="Head_Tilt"/>
- </RobotNode>
-
- <RobotNode name="Head_Tilt">
- <Transform>
- <DH theta="0" d="120" a="0" alpha="90" unitsangle="degree" unitslength="mm"/>
- </Transform>
- <Joint type="revolute">
- <!--DH theta="0" d="0" a="0" alpha="-90" unitsangle="degree" unitslength="mm"/-->
- <axis x="0" y="0" z="1"/>
- <!--<Limits unit="degree" lo="-45" hi="45"/>/-->
- <Limits unit="degree" lo="0" hi="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="6.79877000" units="kg" />
- <IgnoreCollision name="Neck_2_Roll"/>
- <IgnoreCollision name="Hip Yaw"/>
- </Physics>
-
- <Visualization>
- <File type="Inventor">fullmodel/head.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/head.iv</File>
- </CollisionModel>
-
- <Child name="Head Center1"/>
- </RobotNode>
-
- <RobotNode name="Head Center1">
- <Transform>
- <DH theta="0" d="0" a="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
- </Transform>
- <Child name="Head Center"/>
- </RobotNode>
-
-
- <RobotNode name="Head Center">
- <Transform>
- <DH theta="0" d="54.5" a="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
- </Transform>
- <Child name="Cameras"/>
- <Child name="Jaw"/>
- <Child name="Kinect"/>
- <Child name="DepthCamera"/>
- </RobotNode>
-
- <RobotNode name="Jaw">
- <Joint type="revolute">
- <axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-30" hi="30"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
- <Visualization>
- <File type="Inventor">fullmodel/jaw.iv</File>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="Cameras">
- <Transform>
- <Translation x="100" y="0" z="0" units='mm'/>
- </Transform>
- <Joint type="revolute">
- <Axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-30" hi="45"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
-
- <CollisionModel>
- <File type="Inventor">iv_primitives/cyl2.wrl</File>
- </CollisionModel>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="0.2" units="kg" />
- <IgnoreCollision name="Neck_2_Roll"/>
- <IgnoreCollision name="Hip Yaw"/>
- <IgnoreCollision name="Head_Tilt"/>
- </Physics>
-
- <Child name="Eye_Left_Dummy"/>
- <Child name="Eye_Right_Dummy"/>
- <Child name="VirtualCentralGaze"/>
- </RobotNode>
-
- <RobotNode name="VirtualCentralGaze">
- <Joint type="prismatic">
- <TranslationDirection x="1" y="0" z="0"/>
- <Limits unit="mm" lo="0" hi="10000"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="3"/>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="Kinect">
- <Transform>
- <Translation x="140" y="-100" z="0" units='mm'/>
- </Transform>
- <Joint type="prismatic">
- <TranslationDirection x="1" y="0" z="0"/>
- <Limits unit="mm" lo="0" hi="10000"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="3"/>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="DepthCamera">
- <Transform>
- <Translation x="130" y="-80" z="0" units='mm'/>
- <rollpitchyaw roll="0" pitch="90" yaw="0" unitsAngle="degree"/>
- <rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
- <rollpitchyaw roll="30" pitch="0" yaw="0" unitsAngle="degree"/>
- </Transform>
-
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="3"/>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="Eye_Left_Dummy">
- <Transform>
- <DH a="0" d="46.5" theta="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
- </Transform>
- <Child name="Eye_Left"/>
- </RobotNode>
-
- <RobotNode name="Eye_Right_Dummy">
- <Transform>
- <DH a="0" d="-46.5" theta="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
- </Transform>
- <Child name="Eye_Right"/>
- </RobotNode>
-
- <RobotNode name="Eye_Left">
- <Joint type="revolute">
- <Axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-30" hi="30"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
- <Visualization>
- <File type="Inventor">fullmodel/eye_l.iv</File>
- </Visualization>
-
-
- <CollisionModel>
- <File type="Inventor">iv_primitives/cyl1.wrl</File>
- </CollisionModel>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="0.1" units="kg" />
- <IgnoreCollision name="Cameras"/>
- <IgnoreCollision name="Head_Tilt"/>
- </Physics>
-
- <Child name="Lid Left Bottom"/>
- <Child name="Lid Left Top"/>
- <Child name="EyeLeftCameraPreTransformation"/>
- </RobotNode>
-
- <RobotNode name="Eye_Right">
- <Joint type="revolute">
- <Axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-30" hi="30"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
- <Visualization>
- <File type="Inventor">fullmodel/eye_r.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">iv_primitives/cyl1.wrl</File>
- </CollisionModel>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="0.1" units="kg" />
- <IgnoreCollision name="Cameras"/>
- <IgnoreCollision name="Head_Tilt"/>
- </Physics>
-
- <Child name="Lid Right Bottom"/>
- <Child name="Lid Right Top"/>
- <Child name="EyeRightCameraPreTransformation"/>
- </RobotNode>
-
- <RobotNode name="EyeLeftCameraPreTransformation">
- <Transform>
- <translation x="20" y="0" z="0"/>
- <RollPitchYaw roll="0" pitch="90" yaw ="0" units="degree"/>
- </Transform>
- <Child name="EyeLeftCamera"/>
- <Child name="EyeLeftCameraSim"/>
- </RobotNode>
-
- <RobotNode name="EyeLeftCamera">
- <Transform>
- <RollPitchYaw roll="0" pitch="0" yaw ="90" units="degree"/>
- </Transform>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="5"/>
- </Visualization>
- <Child name="HeadMotionMeasurementTCP"/>
- <!--Sensor type="Camera" name="LeftEyeCam">
- </Sensor-->
- </RobotNode>
-
- <RobotNode name="EyeLeftCameraSim">
- <Transform>
- <RollPitchYaw roll="0" pitch="0" yaw ="180" units="degree"/>
- </Transform>
- </RobotNode>
-
- <RobotNode name="HeadMotionMeasurementTCP">
- <Transform>
- <translation x="0" y="0" z="1000"/>
- </Transform>
- </RobotNode>
-
- <RobotNode name="Lid Left Bottom">
- <Joint type="revolute">
- <Axis x="0" y="1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
- <Visualization>
- <File type="Inventor">fullmodel/lid_l_bottom.iv</File>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="Lid Left Top">
- <Joint type="revolute">
- <Axis x="0" y="1" z="0"/>
- </Joint>
- <Visualization>
- <File type="Inventor">fullmodel/lid_l_top.iv</File>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="EyeRightCameraPreTransformation">
- <Transform>
- <translation x="20" y="0" z="0"/>
- <RollPitchYaw roll="0" pitch="90" yaw ="0" units="degree"/>
- </Transform>
- <Child name="EyeRightCamera"/>
- <Child name="EyeRightCameraSim"/>
- </RobotNode>
-
- <RobotNode name="EyeRightCamera">
- <Transform>
- <RollPitchYaw roll="0" pitch="0" yaw ="90" units="degree"/>
- </Transform>
- <!--Sensor type="Camera" name="RightEyeCam">
- </Sensor-->
- </RobotNode>
-
- <RobotNode name="EyeRightCameraSim">
- <Transform>
- <RollPitchYaw roll="0" pitch="0" yaw ="180" units="degree"/>
- </Transform>
- </RobotNode>
-
- <RobotNode name="Lid Right Bottom">
- <Joint type="revolute">
- <Axis x="0" y="1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
- <Visualization>
- <File type="Inventor">fullmodel/lid_r_bottom.iv</File>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="Lid Right Top">
- <Joint type="revolute">
- <Axis x="0" y="1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="5000"/>
- </Joint>
- <Visualization>
- <File type="Inventor">fullmodel/lid_r_top.iv</File>
- </Visualization>
- </RobotNode>
-
- <RobotNodeSet name="Head" kinematicRoot="Neck_1_Pitch">
- <Node name="Neck_1_Pitch"/>
- <Node name="Neck_2_Roll"/>
- <Node name="Neck_3_Yaw"/>
- <Node name="Head_Tilt"/>
- <Node name="Cameras"/>
- <Node name="Eye_Left"/>
- <Node name="Eye_Right"/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name="Neck" kinematicRoot="Neck_1_Pitch">
- <Node name="Neck_1_Pitch"/>
- <Node name="Neck_2_Roll"/>
- <Node name="Neck_3_Yaw"/>
- <Node name="Head_Tilt"/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name="IKVirtualGaze" kinematicRoot="Neck_1_Pitch" tcp="VirtualCentralGaze">
- <Node name="Neck_1_Pitch"/>
- <Node name="Neck_2_Roll"/>
- <Node name="Neck_3_Yaw"/>
- <Node name="Cameras"/>
- <Node name="VirtualCentralGaze"/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name="IKVirtualGazeKinect" kinematicRoot="Neck_1_Pitch" tcp="Kinect">
- <Node name="Neck_1_Pitch"/>
- <Node name="Neck_2_Roll"/>
- <Node name="Neck_3_Yaw"/>
- <Node name="Kinect"/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name="IKVirtualGaze_WithHeadTilt" kinematicRoot="Neck_1_Pitch" tcp="VirtualCentralGaze">
- <Node name="Neck_1_Pitch"/>
- <Node name="Neck_2_Roll"/>
- <Node name="Neck_3_Yaw"/>
- <Node name="Head_Tilt"/>
- <Node name="Cameras"/>
- <Node name="VirtualCentralGaze"/>
- </RobotNodeSet>
-
-</Robot>
diff --git a/data/RobotAPI/tests/robotmodel/ArmarIII-LeftArm.xml b/data/RobotAPI/tests/robotmodel/ArmarIII-LeftArm.xml
deleted file mode 100644
index f4e74ae6cf4fa6b05f6dec3d2f02587d930ca626..0000000000000000000000000000000000000000
--- a/data/RobotAPI/tests/robotmodel/ArmarIII-LeftArm.xml
+++ /dev/null
@@ -1,274 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-
-<Robot Type="ArmarIII LeftArm" RootNode="Left Arm Base">
-
- <RobotNode name="Left Arm Base">
- <!-- Trafo BTo0; COS 0; DOF q1 -->
- <Transform>
- <DH a="0" d="0" theta="-90" alpha="90" units="degree"/>
- </Transform>
- <Child name="Shoulder 1 L"/>
- </RobotNode>
-
- <RobotNode name="Shoulder 1 L">
- <!-- Trafo BTo0; COS 0; DOF q1 -->
- <Transform>
- <DH a="0" d="0" theta="90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="0" theta="90" alpha="90" units="degree"/-->
- <axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-107" hi="42"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="3.65" units="kg" />
- </Physics>
- <Visualization enable="true">
- <File type="Inventor">fullmodel/shoulder_l.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/shoulder_l.iv</File>
- </CollisionModel>
- <Child name="Shoulder 2 L"/>
- </RobotNode>
-
- <RobotNode name="Shoulder 2 L">
- <!-- Trafo 0To1; COS 1; DOF q2 -->
- <Transform>
- <DH a="0" d="0" theta="0" alpha="90" units="degree"/>
- <DH a="0" d="0" theta="75" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="0" theta="75" alpha="90" units="degree"/-->
- <axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-14" hi="95"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="2.5" units="kg" />
- </Physics>
- <Visualization enable="true">
- <File type="Inventor">fullmodel/shoulder2_l_rot.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/shoulder2_l_rot.iv</File>
- </CollisionModel>
- <Child name="Upperarm L"/>
- </RobotNode>
-
- <RobotNode name="Upperarm L">
- <!-- Trafo 1To2; COS 2; DOF q3 -->
- <Transform>
- <DH a="0" d="0" theta="0" alpha="90" units="degree"/>
- <DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="20" d="-310" theta="-90" alpha="90" units="degree"/-->
- <axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-70" hi="109"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="1.55685000" units="kg" />
- <IgnoreCollision name="Shoulder 1 L"/>
- </Physics>
- <Visualization enable="true">
- <File type="Inventor">fullmodel/upperarm_l.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/upperarm_l.iv</File>
- </CollisionModel>
- <Child name="Elbow L"/>
- </RobotNode>
-
- <RobotNode name="Elbow L">
- <Transform>
- <DH a="20" d="-310" theta="0" alpha="90" units="degree"/>
- <DH a="0" d="0" theta="90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="-7.5" theta="90" alpha="-90" units="degree"/-->
- <axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-120" hi="40"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- <File type="Inventor">fullmodel/elbow_l.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/elbow_l.iv</File>
- </CollisionModel>
- <Physics>
- <CoM location="Joint"/>
- <Mass value="1.15744000" units="kg" />
- </Physics>
- <Child name="Underarm L"/>
- </RobotNode>
-
- <RobotNode name="Underarm L">
- <Transform>
- <DH a="0" d="-7.5" theta="0" alpha="-90" units="degree"/>
- <DH a="0" d="0" theta="90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="-240" theta="90" alpha="-90" units="degree"/-->
- <axis x="0" y="0" z="-1"/>
- <!--Limits unit="degree" lo="-57.29" hi="174.48"/-->
- <Limits unit="degree" lo="-90" hi="200"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Visualization enable="true">
- <File type="Inventor">fullmodel/underarm_l.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/underarm_l.iv</File>
- </CollisionModel>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="2.26566087" units="kg" />
- </Physics>
- <Child name="Wrist 1 L"/>
- </RobotNode>
-
- <RobotNode name="Wrist 1 L">
- <Transform>
- <DH a="0" d="-240" theta="0" alpha="-90" units="degree"/>
- <DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="0" theta="-90" alpha="-90" units="degree"/-->
- <axis x="0" y="0" z="-1"/>
- <Limits unit="degree" lo="-30" hi="30"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="1.29945309" units="kg" />
- </Physics>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- <File type="Inventor">fullmodel/wrist1_l.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/wrist1_l.iv</File>
- </CollisionModel>
- <Child name="Wrist 2 L"/>
- </RobotNode>
-
- <RobotNode name="Wrist 2 L">
- <Transform>
- <DH a="0" d="0" theta="0" alpha="-90" units="degree"/>
- <DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="0" theta="-90" alpha="90" units="degree"/-->
- <axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-38" hi="50"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- <File type="Inventor">fullmodel/wrist2_l.iv</File>
- </Visualization>
- <!-- bullet doesn't like two models connected with a fixed joint -> disable this collisison model and just use the hand palm model-->
- <!--CollisionModel>
- <File type="Inventor">convexModel/wrist2_l.iv</File>
- </CollisionModel>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="2.59945309" units="kg" />
- <IgnoreCollision name="Underarm L"/>
- </Physics-->
- <Sensor type="forcetorque">
- <!--Transform>
- <Translation x="0" y="-40" z="0"/>
- </Transform-->
- </Sensor>
- <Child name="EndArmL"/>
- </RobotNode>
-
- <!--RobotNode name="FTSensorL">
- <Transform>
- <Translation x="0" y="-40" z="0"/>
- </Transform>
- <Sensor type="forcetorque"/>
- <Child name="EndArmL"/>
- </RobotNode-->
-
- <RobotNode name="EndArmL">
- <Transform>
- <Translation x="0" y="-40" z="0"/>
- <DH a="0" d="0" theta="0" alpha="90" units="degree"/>
- </Transform>
- <Visualization enable="false">
- <CoordinateAxis type="Inventor" enable="true" scaling="2"/>
- </Visualization>
- <ChildFromRobot>
- <File importEEF="true">ArmarIII-LeftHand.xml</File>
- </ChildFromRobot>
- </RobotNode>
-
-
- <RobotNodeSet name="LeftArm" kinematicRoot="Left Arm Base" tcp="TCP L">
- <Node name="Shoulder 1 L"/>
- <Node name="Shoulder 2 L"/>
- <Node name="Upperarm L"/>
- <Node name="Elbow L"/>
- <Node name="Underarm L"/>
- <Node name="Wrist 1 L"/>
- <Node name="Wrist 2 L"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="LeftArmElbow" kinematicRoot="Left Arm Base" tcp="Upperarm L">
- <Node name="Shoulder 1 L"/>
- <Node name="Shoulder 2 L"/>
- <Node name="Upperarm L"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="LeftArmColModel" kinematicRoot="Left Arm Base" tcp="TCP L">
- <Node name="Upperarm L"/>
- <Node name="Underarm L"/>
- <Node name="Wrist 2 L"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="LeftArmHandColModel" kinematicRoot="Left Arm Base" tcp="TCP L">
- <Node name="Upperarm L"/>
- <Node name="Underarm L"/>
- <Node name="Hand Palm 1 L"/>
- <Node name="Hand Palm 2 L"/>
- <Node name="Thumb L J0"/>
- <Node name="Thumb L J1"/>
- <Node name="Index L J0"/>
- <Node name="Index L J1"/>
- <Node name="Middle L J0"/>
- <Node name="Middle L J1"/>
- <Node name="Ring L J0"/>
- <Node name="Ring L J1"/>
- <Node name="Pinky L J0"/>
- <Node name="Pinky L J1"/>
- </RobotNodeSet>
-
-
-
-</Robot>
diff --git a/data/RobotAPI/tests/robotmodel/ArmarIII-LeftHand.xml b/data/RobotAPI/tests/robotmodel/ArmarIII-LeftHand.xml
deleted file mode 100644
index e7a1bc5aa1d9596c028c2784ee4aee7163174b2a..0000000000000000000000000000000000000000
--- a/data/RobotAPI/tests/robotmodel/ArmarIII-LeftHand.xml
+++ /dev/null
@@ -1,469 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-
-<Robot Type="ArmarIII LeftHand" RootNode="Hand L Base">
-
- <RobotNode name="Hand L Base">
- <Child name="TCP L"/>
- <Child name="GCP L"/>
- <Child name="Marker L"/>
- <Child name="left_hand_configuration_actual_float"/>
- <Child name="Hand Palm 1 L"/>
- </RobotNode>
-
- <RobotNode name="TCP L">
- <Transform>
- <Translation x="0" y="0" z="130"/>
- </Transform>
- </RobotNode>
-
- <RobotNode name="GCP L">
- <Transform>
- <Translation X="-40" Y="-20" Z="90"/>
- <rollpitchyaw roll="0" pitch="-45" yaw="0" units="degree"/>
- </Transform>
- </RobotNode>
-
- <RobotNode name="Marker L">
- <Transform>
- <Translation x="-35" y="45" z="65"/>
- </Transform>
- <Visualization enable="false">
- <CoordinateAxis type="Inventor" enable="true" scaling="1"/>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="left_hand_configuration_actual_float">
- <!--Joint type="revolute">
- <Axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-20" hi="20"/>
- </Joint-->
- </RobotNode>
-
- <RobotNode name="Hand Palm 1 L">
- <Transform>
- <Translation x="0" y="0" z="36"/>
- </Transform>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/palm1_l.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/palm1_l.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="800" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Hand Palm 2 L"/>
- <Child name="Thumb L"/>
- </RobotNode>
-
- <RobotNode name="Hand Palm 2 L">
- <Transform>
- <Translation x="-8.7" y="13.5" z="29.25"/>
- </Transform>
- <Joint type="revolute">
- <Axis x="0" y="-1" z="0"/>
- <Limits unit="degree" lo="-90" hi="90"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/palm2_l.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/palm2_l.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="800" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Pinky L"/>
- <Child name="Ring L"/>
- <Child name="Middle L"/>
- <Child name="Index L"/>
- </RobotNode>
-
- <RobotNode name="Thumb L">
- <Transform>
- <Translation x="-48.9" y="0" z="29.25"/>
- </Transform>
- <Child name="Thumb L J0"/>
- </RobotNode>
-
- <RobotNode name="Thumb L J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/thumb_l1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/thumb_l1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Thumb L J1"/>
- </RobotNode>
-
- <RobotNode name="Thumb L J1">
- <Transform>
- <Translation x="-40.2" y="0" z="0"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/thumb_l2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/thumb_l2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNode name="Pinky L">
- <Transform>
- <Translation x="0" y="-73" z="40.4"/>
- </Transform>
-
- <Child name="Pinky L J0"/>
- </RobotNode>
-
- <RobotNode name="Pinky L J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/pinky_l1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/pinky_l1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Pinky L J1"/>
- </RobotNode>
-
- <RobotNode name="Pinky L J1">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/pinky_l2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/pinky_l2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNode name="Ring L">
- <Transform>
- <Translation x="0" y="-51" z="40.4"/>
- </Transform>
-
- <Child name="Ring L J0"/>
- </RobotNode>
-
- <RobotNode name="Ring L J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/ring_l1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/ring_l1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Ring L J1"/>
- </RobotNode>
-
- <RobotNode name="Ring L J1">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/ring_l2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/ring_l2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNode name="Middle L">
- <Transform>
- <Translation x="0" y="-27" z="40.4"/>
- </Transform>
-
- <Child name="Middle L J0"/>
- </RobotNode>
-
- <RobotNode name="Middle L J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/middle_l1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/middle_l1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Middle L J1"/>
- </RobotNode>
-
- <RobotNode name="Middle L J1">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/middle_l2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/middle_l2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNode name="Index L">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
-
- <Child name="Index L J0"/>
- </RobotNode>
-
- <RobotNode name="Index L J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/index_l1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/index_l1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Index L J1"/>
- </RobotNode>
-
- <RobotNode name="Index L J1">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="180"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/index_l2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/index_l2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNodeSet name="LeftHandColModel" kinematicRoot="Hand L Base" tcp="TCP L">
- <Node name="Hand Palm 1 L"/>
- <Node name="Hand Palm 2 L"/>
- <Node name="Thumb L J0"/>
- <Node name="Thumb L J1"/>
- <Node name="Index L J0"/>
- <Node name="Index L J1"/>
- <Node name="Middle L J0"/>
- <Node name="Middle L J1"/>
- <Node name="Ring L J0"/>
- <Node name="Ring L J1"/>
- <Node name="Pinky L J0"/>
- <Node name="Pinky L J1"/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name="LeftHandJoints" kinematicRoot="Hand L Base" tcp="TCP L">
- <Node name="Thumb L J0"/>
- <Node name="Thumb L J1"/>
- <Node name="Index L J0"/>
- <Node name="Index L J1"/>
- <Node name="Middle L J0"/>
- <Node name="Middle L J1"/>
- <Node name="Ring L J0"/>
- <Node name="Ring L J1"/>
- <Node name="Pinky L J0"/>
- <Node name="Pinky L J1"/>
- </RobotNodeSet>
-
- <Endeffector name="TCP L" base="Hand L Base" tcp="TCP L" gcp="GCP L">
- <Preshape name="Open Preshape">
- <Node name="Thumb L J0" unit="radian" value="0"/>
- <Node name="Thumb L J1" unit="radian" value="0"/>
- <Node name="Index L J0" unit="radian" value="0"/>
- <Node name="Index L J1" unit="radian" value="0"/>
- <Node name="Middle L J0" unit="radian" value="0"/>
- <Node name="Middle L J1" unit="radian" value="0"/>
- <Node name="Ring L J0" unit="radian" value="0"/>
- <Node name="Ring L J1" unit="radian" value="0"/>
- <Node name="Pinky L J0" unit="radian" value="0"/>
- <Node name="Pinky L J1" unit="radian" value="0"/>
- </Preshape>
- <Preshape name="Close Preshape">
- <Node name="Thumb L J0" unit="radian" value="1"/>
- <Node name="Thumb L J1" unit="radian" value="1"/>
- <Node name="Index L J0" unit="radian" value="1"/>
- <Node name="Index L J1" unit="radian" value="1"/>
- <Node name="Middle L J0" unit="radian" value="1"/>
- <Node name="Middle L J1" unit="radian" value="1"/>
- <Node name="Ring L J0" unit="radian" value="1"/>
- <Node name="Ring L J1" unit="radian" value="1"/>
- <Node name="Pinky L J0" unit="radian" value="1"/>
- <Node name="Pinky L J1" unit="radian" value="1"/>
- </Preshape>
- <Static>
- <Node name="Hand Palm 1 L"/>
- <Node name="Hand Palm 2 L"/>
- </Static>
- <Actor name="Thumb Left">
- <Node name="Thumb L J0" considerCollisions="Actors"/>
- <Node name="Thumb L J1" considerCollisions="All"/>
- </Actor>
- <Actor name="Index Left">
- <Node name="Index L J0" considerCollisions="Actors"/>
- <Node name="Index L J1" considerCollisions="All"/>
- </Actor>
- <Actor name="Middle Left">
- <Node name="Middle L J0" considerCollisions="Actors"/>
- <Node name="Middle L J1" considerCollisions="All"/>
- </Actor>
- <Actor name="Ring Left">
- <Node name="Ring L J0" considerCollisions="Actors"/>
- <Node name="Ring L J1" considerCollisions="All"/>
- </Actor>
- <Actor name="Pinky Left">
- <Node name="Pinky L J0" considerCollisions="Actors"/>
- <Node name="Pinky L J1" considerCollisions="All"/>
- </Actor>
- </Endeffector>
-
-</Robot>
diff --git a/data/RobotAPI/tests/robotmodel/ArmarIII-RightArm.xml b/data/RobotAPI/tests/robotmodel/ArmarIII-RightArm.xml
deleted file mode 100644
index cf073e6d5a1b997cb4887aa97f743faeedb9018a..0000000000000000000000000000000000000000
--- a/data/RobotAPI/tests/robotmodel/ArmarIII-RightArm.xml
+++ /dev/null
@@ -1,255 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-
-<Robot Type="ArmarIII RightArm" RootNode="Right Arm Base">
-
- <RobotNode name="Right Arm Base">
- <Transform>
- <DH a="0" d="0" theta="-90" alpha="90" units="degree"/>
- </Transform>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- </Visualization>
- <Child name="Shoulder 1 R"/>
- </RobotNode>
-
- <RobotNode name="Shoulder 1 R">
- <Transform>
- <DH a="0" d="0" theta="90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="0" theta="90" alpha="90" units="degree"/-->
- <Limits unit="degree" lo="-107" hi="42"/>
- <MaxVelocity unit="radian" value="0.5"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="3.65" units="kg" />
- </Physics>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="4"/>
- <File type="Inventor">fullmodel/shoulder_r.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/shoulder_r.iv</File>
- </CollisionModel>
- <Child name="Shoulder 2 R"/>
- </RobotNode>
-
- <RobotNode name="Shoulder 2 R">
- <Transform>
- <DH a="0" d="0" theta="0" alpha="90" units="degree"/>
- <DH a="0" d="0" theta="105" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="0" theta="105" alpha="90" units="degree"/-->
- <Limits unit="degree" lo="-14" hi="95"/>
- <MaxVelocity unit="radian" value="0.5"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="2.5" units="kg" />
- </Physics>
- <Visualization enable="true">
- <File type="Inventor">fullmodel/shoulder2_r_rot.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/shoulder2_r_rot.iv</File>
- </CollisionModel>
- <Child name="Upperarm R"/>
- </RobotNode>
-
- <RobotNode name="Upperarm R">
- <Transform>
- <DH a="0" d="0" theta="0" alpha="90" units="degree"/>
- <DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="20" d="-310" theta="-90" alpha="90" units="degree"/-->
- <Limits unit="degree" lo="-109" hi="70"/>
- <MaxVelocity unit="radian" value="0.5"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="1.55685000" units="kg" />
- <IgnoreCollision name="Shoulder 1 R"/>
- </Physics>
- <Visualization enable="true">
- <File type="Inventor">fullmodel/upperarm_r.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/upperarm_r.iv</File>
- </CollisionModel>
- <Child name="Elbow R"/>
- </RobotNode>
-
- <RobotNode name="Elbow R">
- <Transform>
- <DH a="20" d="-310" theta="0" alpha="90" units="degree"/>
- <DH a="0" d="0" theta="90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="7.5" theta="90" alpha="-90" units="degree"/-->
- <Limits unit="degree" lo="-120" hi="40"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="4"/>
- <File type="Inventor">fullmodel/elbow_r.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/elbow_r.iv</File>
- </CollisionModel>
- <Physics>
- <CoM location="Joint"/>
- <Mass value="1.15744000" units="kg" />
- </Physics>
- <Child name="Underarm R"/>
- </RobotNode>
-
- <RobotNode name="Underarm R">
- <Transform>
- <DH a="0" d="7.5" theta="0" alpha="-90" units="degree"/>
- <DH a="0" d="0" theta="90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="-240" theta="90" alpha="-90" units="degree"/-->
-
- <Limits unit="degree" lo="-90" hi="200"/>
- <MaxVelocity unit="radian" value="0.5"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Visualization enable="true">
- <File type="Inventor">fullmodel/underarm_r.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/underarm_r.iv</File>
- </CollisionModel>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="2.26566087" units="kg" />
- </Physics>
- <Child name="Wrist 1 R"/>
- </RobotNode>
-
- <RobotNode name="Wrist 1 R">
- <Transform>
- <DH a="0" d="-240" theta="0" alpha="-90" units="degree"/>
- <DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="0" theta="-90" alpha="-90" units="degree"/-->
- <Limits unit="degree" lo="-22.3" hi="22.8"/>
- <MaxVelocity unit="radian" value="0.5"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="1.29945309" units="kg" /><!--1.13047000-->
- </Physics>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="2"/>
- <File type="Inventor">fullmodel/wrist1_r.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/wrist1_r.iv</File>
- </CollisionModel>
- <Child name="Wrist 2 R"/>
- </RobotNode>
-
- <RobotNode name="Wrist 2 R">
- <Transform>
- <DH a="0" d="0" theta="0" alpha="-90" units="degree"/>
- <DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
- </Transform>
- <Joint type="revolute">
- <!--DH a="0" d="0" theta="-90" alpha="90" units="degree"/-->
- <Limits unit="degree" lo="-38" hi="50"/>
- <MaxVelocity unit="radian" value="0.5"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="2"/>
- <File type="Inventor">fullmodel/wrist2_r.iv</File>
- </Visualization>
- <!-- bullet doesn't like two models connected with a fixed joint -> disable this collisison model and just use the hand palm model-->
- <!--CollisionModel>
- <File type="Inventor">convexModel/wrist2_r.iv</File>
- </CollisionModel>
-
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="2.59945309" units="kg" />
- <IgnoreCollision name="Underarm R"/>
- </Physics-->
- <Sensor type="forcetorque">
- <!--Transform>
- <Translation x="0" y="-40" z="0"/>
- </Transform-->
- </Sensor>
- <Child name="EndArmR"/>
- </RobotNode>
-
- <!--RobotNode name="FTSensorR">
- <Transform>
- <Translation x="0" y="-40" z="0"/>
- </Transform>
- <Sensor type="forcetorque"/>
- <Child name="EndArmR"/>
- </RobotNode-->
-
- <RobotNode name="EndArmR">
- <Transform>
- <Translation x="0" y="-40" z="0"/>
- <DH a="0" d="0" theta="0" alpha="90" units="degree"/>
- </Transform>
- <ChildFromRobot>
- <File importEEF="true">ArmarIII-RightHand.xml</File>
- </ChildFromRobot>
- </RobotNode>
-
- <RobotNodeSet name="RightArm" kinematicRoot="Right Arm Base" tcp="TCP R">
- <Node name="Shoulder 1 R"/>
- <Node name="Shoulder 2 R"/>
- <Node name="Upperarm R"/>
- <Node name="Elbow R"/>
- <Node name="Underarm R"/>
- <Node name="Wrist 1 R"/>
- <Node name="Wrist 2 R"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="RightArmColModel" kinematicRoot="Right Arm Base" tcp="TCP R">
- <Node name="Upperarm R"/>
- <Node name="Underarm R"/>
- <Node name="Wrist 2 R"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="RightArmHandColModel" kinematicRoot="Right Arm Base" tcp="TCP R">
- <Node name="Upperarm R"/>
- <Node name="Underarm R"/>
- <Node name="Hand Palm 1 R"/>
- <Node name="Hand Palm 2 R"/>
- <Node name="Thumb R J0"/>
- <Node name="Thumb R J1"/>
- <Node name="Index R J0"/>
- <Node name="Index R J1"/>
- <Node name="Middle R J0"/>
- <Node name="Middle R J1"/>
- <Node name="Ring R J0"/>
- <Node name="Ring R J1"/>
- <Node name="Pinky R J0"/>
- <Node name="Pinky R J1"/>
- </RobotNodeSet>
-
-</Robot>
diff --git a/data/RobotAPI/tests/robotmodel/ArmarIII-RightHand.xml b/data/RobotAPI/tests/robotmodel/ArmarIII-RightHand.xml
deleted file mode 100644
index 9bd73d4a3544b4ac1329c0eb919aee7828212bba..0000000000000000000000000000000000000000
--- a/data/RobotAPI/tests/robotmodel/ArmarIII-RightHand.xml
+++ /dev/null
@@ -1,499 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-
-<Robot Type="ArmarIII RightHand" RootNode="Hand R Base">
-
- <RobotNode name="Hand R Base">
- <Child name="TCP R"/>
- <Child name="GCP R"/>
- <Child name="Marker R"/>
- <Child name="right_hand_configuration_actual_float"/>
- <Child name="Hand Palm 1 R"/>
- </RobotNode>
-
- <RobotNode name="TCP R">
- <Transform>
- <Translation x="0" y="0" z="130"/>
- </Transform>
- <Visualization enable="false">
- <CoordinateAxis type="Inventor" enable="true" scaling="1"/>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="GCP R">
- <Transform>
- <Translation X="-40" Y="20" Z="90"/>
- <rollpitchyaw roll="0" pitch="-45" yaw="0" units="degree"/>
- </Transform>
- </RobotNode>
-
- <RobotNode name="Marker R">
- <Transform>
- <Translation x="-35" y="-45" z="65"/>
- </Transform>
- <Visualization enable="false">
- <CoordinateAxis type="Inventor" enable="true" scaling="1"/>
- </Visualization>
- </RobotNode>
-
- <RobotNode name="right_hand_configuration_actual_float">
- <!--Joint type="revolute">
- <Axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-20" hi="20"/>
- </Joint-->
- </RobotNode>
-
- <RobotNode name="Hand Palm 1 R">
-
- <Transform>
- <Translation x="0" y="0" z="36"/>
- </Transform>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/palm1_r.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/palm1_r.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="800" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Hand Palm 2 R"/>
- <Child name="Thumb R"/>
- </RobotNode>
-
- <RobotNode name="Hand Palm 2 R">
- <Transform>
- <Translation x="-8.7" y="-13.5" z="29.25"/>
- </Transform>
- <Joint type="revolute">
- <Axis x="0" y="-1" z="0"/>
- <Limits unit="degree" lo="-90" hi="90"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/palm2_r.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/palm2_r.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="800" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Pinky R"/>
- <Child name="Ring R"/>
- <Child name="Middle R"/>
- <Child name="Index R"/>
- </RobotNode>
-
- <RobotNode name="Thumb R">
- <Transform>
- <Translation x="-48.9" y="0" z="29.25"/>
- </Transform>
- <Child name="Thumb R J0"/>
- </RobotNode>
-
- <RobotNode name="Thumb R J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/thumb_r1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/thumb_r1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Thumb R J1"/>
- </RobotNode>
-
- <RobotNode name="Thumb R J1">
- <Transform>
- <Translation x="-40.2" y="0" z="0"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/thumb_r2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/thumb_r2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNode name="Pinky R">
- <Transform>
- <Translation x="0" y="73" z="40.4"/>
- </Transform>
-
- <Child name="Pinky R J0"/>
- </RobotNode>
-
- <RobotNode name="Pinky R J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/pinky_r1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/pinky_r1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Pinky R J1"/>
- </RobotNode>
-
- <RobotNode name="Pinky R J1">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/pinky_r2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/pinky_r2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNode name="Ring R">
-
- <Transform>
- <Translation x="0" y="51" z="40.4"/>
- </Transform>
-
- <Child name="Ring R J0"/>
- </RobotNode>
-
- <RobotNode name="Ring R J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/ring_r1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/ring_r1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Ring R J1"/>
- </RobotNode>
-
- <RobotNode name="Ring R J1">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/ring_r2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/ring_r2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNode name="Middle R">
- <Transform>
- <Translation x="0" y="27" z="40.4"/>
- </Transform>
-
- <Child name="Middle R J0"/>
- </RobotNode>
-
- <RobotNode name="Middle R J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/middle_r1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/middle_r1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Middle R J1"/>
- </RobotNode>
-
- <RobotNode name="Middle R J1">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/middle_r2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/middle_r2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
- </RobotNode>
-
- <RobotNode name="Index R">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
-
- <Child name="Index R J0"/>
- </RobotNode>
-
- <RobotNode name="Index R J0">
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/index_r1.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/index_r1.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Index R J1"/>
- </RobotNode>
-
- <RobotNode name="Index R J1">
- <Transform>
- <Translation x="0" y="0" z="40.2"/>
- </Transform>
- <Joint type="revolute">
- <Limits unit="degree" lo="0" hi="90"/>
- <Axis x="0" y="-1" z="0"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <File type="Inventor">fullmodel/index_r2.iv</File>
- </Visualization>
-
- <CollisionModel>
- <File type="Inventor">convexModel/index_r2.iv</File>
- </CollisionModel>
-
- <Physics>
- <Mass value="500" unit="g"/>
- <CoM location="VisualizationBBoxCenter"/>
- </Physics>
-
- <Child name="Index R Fingertip"/>
- </RobotNode>
-
- <RobotNode name="Index R Fingertip">
- <Transform>
- <Translation x="-5" y="0" z="40.2"/>
- </Transform>
- <Visualization enable="false">
- <CoordinateAxis type="Inventor" enable="true" scaling="1"/>
- </Visualization>
- </RobotNode>
-
- <RobotNodeSet name="RightHandColModel" kinematicRoot="Hand R Base" tcp="TCP R">
- <Node name="Hand Palm 1 R"/>
- <Node name="Hand Palm 2 R"/>
- <Node name="Thumb R J0"/>
- <Node name="Thumb R J1"/>
- <Node name="Index R J0"/>
- <Node name="Index R J1"/>
- <Node name="Middle R J0"/>
- <Node name="Middle R J1"/>
- <Node name="Ring R J0"/>
- <Node name="Ring R J1"/>
- <Node name="Pinky R J0"/>
- <Node name="Pinky R J1"/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name="RightHandJoints" kinematicRoot="Hand R Base" tcp="TCP R">
- <Node name="Thumb R J0"/>
- <Node name="Thumb R J1"/>
- <Node name="Index R J0"/>
- <Node name="Index R J1"/>
- <Node name="Middle R J0"/>
- <Node name="Middle R J1"/>
- <Node name="Ring R J0"/>
- <Node name="Ring R J1"/>
- <Node name="Pinky R J0"/>
- <Node name="Pinky R J1"/>
- </RobotNodeSet>
-
- <Endeffector name="TCP R" base="Hand R Base" tcp="TCP R" gcp="GCP R">
- <Preshape name="Open Preshape">
- <Node name="Thumb R J0" unit="radian" value="0"/>
- <Node name="Thumb R J1" unit="radian" value="0"/>
- <Node name="Index R J0" unit="radian" value="0"/>
- <Node name="Index R J1" unit="radian" value="0"/>
- <Node name="Middle R J0" unit="radian" value="0"/>
- <Node name="Middle R J1" unit="radian" value="0"/>
- <Node name="Ring R J0" unit="radian" value="0"/>
- <Node name="Ring R J1" unit="radian" value="0"/>
- <Node name="Pinky R J0" unit="radian" value="0"/>
- <Node name="Pinky R J1" unit="radian" value="0"/>
- </Preshape>
- <Preshape name="Close Preshape">
- <Node name="Thumb R J0" unit="radian" value="1"/>
- <Node name="Thumb R J1" unit="radian" value="1"/>
- <Node name="Index R J0" unit="radian" value="1"/>
- <Node name="Index R J1" unit="radian" value="1"/>
- <Node name="Middle R J0" unit="radian" value="1"/>
- <Node name="Middle R J1" unit="radian" value="1"/>
- <Node name="Ring R J0" unit="radian" value="1"/>
- <Node name="Ring R J1" unit="radian" value="1"/>
- <Node name="Pinky R J0" unit="radian" value="1"/>
- <Node name="Pinky R J1" unit="radian" value="1"/>
- </Preshape>
- <Preshape name="Julia Preshape">
- <Node name="Thumb R J0" unit="radian" value="0"/>
- <Node name="Thumb R J1" unit="radian" value="0"/>
- <Node name="Index R J0" unit="radian" value="0"/>
- <Node name="Index R J1" unit="radian" value="0"/>
- <Node name="Middle R J0" unit="radian" value="1.5"/>
- <Node name="Middle R J1" unit="radian" value="1.5"/>
- <Node name="Ring R J0" unit="radian" value="1.5"/>
- <Node name="Ring R J1" unit="radian" value="1.5"/>
- <Node name="Pinky R J0" unit="radian" value="0"/>
- <Node name="Pinky R J1" unit="radian" value="0"/>
- </Preshape>
-
-
- <Static>
- <Node name="Hand Palm 1 R"/>
- <Node name="Hand Palm 2 R"/>
- </Static>
- <Actor name="Thumb Right">
- <Node name="Thumb R J0" considerCollisions="Actors"/>
- <Node name="Thumb R J1" considerCollisions="All"/>
- </Actor>
- <Actor name="Index Right">
- <Node name="Index R J0" considerCollisions="Actors"/>
- <Node name="Index R J1" considerCollisions="All"/>
- </Actor>
- <Actor name="Middle Right">
- <Node name="Middle R J0" considerCollisions="Actors"/>
- <Node name="Middle R J1" considerCollisions="All"/>
- </Actor>
- <Actor name="Ring Right">
- <Node name="Ring R J0" considerCollisions="Actors"/>
- <Node name="Ring R J1" considerCollisions="All"/>
- </Actor>
- <Actor name="Pinky Right">
- <Node name="Pinky R J0" considerCollisions="Actors"/>
- <Node name="Pinky R J1" considerCollisions="All"/>
- </Actor>
- </Endeffector>
-
-</Robot>
diff --git a/data/RobotAPI/tests/robotmodel/ArmarIII.xml b/data/RobotAPI/tests/robotmodel/ArmarIII.xml
deleted file mode 100644
index ff794e8e0b979ce95e9fcd1b4bfc773e25d66257..0000000000000000000000000000000000000000
--- a/data/RobotAPI/tests/robotmodel/ArmarIII.xml
+++ /dev/null
@@ -1,395 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-
-<Robot Type="Armar3" RootNode="Armar3_Base">
-
- <RobotNode name="Armar3_Base">
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="20"/>
- </Visualization>
- <Child name="Dummy_Platform"/>
- <!--Child name="Platform"/-->
- </RobotNode>
-
- <RobotNode name="Dummy_Platform">
- <Transform>
- <DH a="0" d="0" theta="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
- </Transform>
- <Child name="X_Platform"/>
- </RobotNode>
-
- <RobotNode name="X_Platform">
- <Transform>
- <DH a="0" d="0" theta="90" alpha="-90" unitsangle="degree" unitslength="mm"/>
- <!--Limits unit="mm" lo="-10000" hi="10000"/-->
- </Transform>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- </Visualization>
- <Child name="Y_Platform"/>
- </RobotNode>
-
- <RobotNode name="Y_Platform">
- <Transform>
- <DH a="0" d="0" theta="-90" alpha="90" unitsangle="degree" unitslength="mm"/>
- <!--Limits unit="mm" lo="-10000" hi="10000"/-->
- </Transform>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- </Visualization>
- <Child name="Yaw_Platform"/>
- </RobotNode>
-
- <RobotNode name="Yaw_Platform">
- <Transform>
- <DH a="0" d="0" theta="-90" alpha="0" unitsangle="degree" unitslength="mm"/>
- <!--Limits unit="mm" lo="-10000" hi="10000"/-->
- </Transform>
- <Joint type="revolute">
- <axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-360" hi="360"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- </Visualization>
- <Child name="Platform"/>
- </RobotNode>
-
- <RobotNode name="Platform"> <!-- visualization via 3D-model at the very end of the kinematic chain-->
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- <File type="Inventor">fullmodel/platform.iv</File>
- </Visualization>
- <Physics>
- <CoM location="Joint" x="0" y="0" z="200"/>
- <Mass value="100" units="kg" />
- </Physics>
- <CollisionModel>
- <File type="Inventor">convexModel/platform.iv</File>
- </CollisionModel>
- <Child name="Hip Pitch"/>
- </RobotNode>
-
- <RobotNode name="Hip Pitch">
- <Transform>
- <Translation x="0" y="158" z="890" units='mm'/>
- </Transform>
- <Joint type="revolute">
- <Axis x="1" y="0" z="0"/>
- <Limits unit="degree" lo="-45" hi="45"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- <File type="Inventor">fullmodel/platform_pitch_link.iv</File>
- </Visualization>
- <Physics>
- <Mass value="10" units="kg" />
- </Physics>
- <CollisionModel>
- <File type="Inventor">convexModel/platform_pitch_link.iv</File>
- </CollisionModel>
- <Child name="Hip Roll"/>
- </RobotNode>
-
- <RobotNode name="Hip Roll">
- <Joint type="revolute">
- <Axis x="0" y="1" z="0"/>
- <Limits unit="degree" lo="-45" hi="45"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
-
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- <File type="Inventor">fullmodel/platform_roll_link.iv</File>
- </Visualization>
- <Physics>
- <Mass value="10" units="kg" />
- <IgnoreCollision name="Platform"/>
- </Physics>
- <CollisionModel>
- <File type="Inventor">convexModel/platform_roll_link.iv</File>
- </CollisionModel>
- <Child name="Hip Yaw"/>
- </RobotNode>
-
- <RobotNode name="Hip Yaw">
- <Joint type="revolute">
- <Axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-45" hi="45"/>
- <MaxVelocity unit="radian" value="1"/>
- <MaxAcceleration value="10"/>
- <MaxTorque value="20000"/>
- </Joint>
- <Physics>
- <CoM location="VisualizationBBoxCenter"/>
- <Mass value="15.415" units="kg" />
- <IgnoreCollision name="Hip Pitch"/>
- <IgnoreCollision name="Platform"/>
- </Physics>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- <File type="Inventor">fullmodel/torso.iv</File>
- </Visualization>
- <CollisionModel>
- <File type="Inventor">convexModel/torso.iv</File>
- </CollisionModel>
- <Child name="Center of Arms"/>
- </RobotNode>
-
- <RobotNode name="Center of Arms">
- <Transform>
- <Translation x="0" y="-35" z="485" units='mm'/>
- </Transform>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- </Visualization>
-
- <Child name="TrafoToHead"/>
- <Child name="TrafoToLeftArm"/>
- <Child name="TrafoToRightArm"/>
- </RobotNode>
-
- <RobotNode name="TrafoToHead">
- <Transform>
- <Translation x="0" y="0" z="118" units='mm'/>
- </Transform>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- </Visualization>
-
- <ChildFromRobot>
- <File importEEF="true">ArmarIII-Head.xml</File>
- </ChildFromRobot>
- </RobotNode>
-
- <RobotNode name="TrafoToLeftArm">
- <Transform>
- <Translation x="-232" y="0" z="0" units='mm'/>
- </Transform>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- </Visualization>
-
- <ChildFromRobot>
- <File importEEF="true">ArmarIII-LeftArm.xml</File>
- </ChildFromRobot>
- </RobotNode>
-
- <RobotNode name="TrafoToRightArm">
- <Transform>
- <Translation x="232" y="0" z="0" units='mm'/>
- </Transform>
- <Visualization enable="true">
- <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
- </Visualization>
-
- <ChildFromRobot>
- <File importEEF="true">ArmarIII-RightArm.xml</File>
- </ChildFromRobot>
- </RobotNode>
-
- <RobotNodeSet name="Torso" kinematicRoot="Platform">
- <Node name="Hip Pitch"/>
- <Node name="Hip Roll"/>
- <Node name="Hip Yaw"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="TorsoHeadColModel">
- <Node name="Hip Yaw"/>
- <Node name="Head_Tilt"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="PlatformTorsoColModel">
- <Node name="Platform"/>
- <Node name="Hip Yaw"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="PlatformTorsoHeadColModel">
- <Node name="Platform"/>
- <Node name="Hip Yaw"/>
- <Node name="Head_Tilt"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="TorsoLeftArm" kinematicRoot="Platform" tcp="TCP L">
- <Node name="Hip Pitch"/>
- <Node name="Hip Roll"/>
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 L"/>
- <Node name="Shoulder 2 L"/>
- <Node name="Upperarm L"/>
- <Node name="Elbow L"/>
- <Node name="Underarm L"/>
- <Node name="Wrist 1 L"/>
- <Node name="Wrist 2 L"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="HipYawLeftArm" kinematicRoot="Platform" tcp="TCP L">
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 L"/>
- <Node name="Shoulder 2 L"/>
- <Node name="Upperarm L"/>
- <Node name="Elbow L"/>
- <Node name="Underarm L"/>
- <Node name="Wrist 1 L"/>
- <Node name="Wrist 2 L"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="TorsoRightArm" kinematicRoot="Platform" tcp="TCP R">
- <Node name="Hip Pitch"/>
- <Node name="Hip Roll"/>
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 R"/>
- <Node name="Shoulder 2 R"/>
- <Node name="Upperarm R"/>
- <Node name="Elbow R"/>
- <Node name="Underarm R"/>
- <Node name="Wrist 1 R"/>
- <Node name="Wrist 2 R"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="TorsoRightArmIndexFingertip" kinematicRoot="Platform" tcp="Index R Fingertip">
- <Node name="Hip Pitch"/>
- <Node name="Hip Roll"/>
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 R"/>
- <Node name="Shoulder 2 R"/>
- <Node name="Upperarm R"/>
- <Node name="Elbow R"/>
- <Node name="Underarm R"/>
- <Node name="Wrist 1 R"/>
- <Node name="Wrist 2 R"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="HipYawRightArm" kinematicRoot="Platform" tcp="TCP R">
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 R"/>
- <Node name="Shoulder 2 R"/>
- <Node name="Upperarm R"/>
- <Node name="Elbow R"/>
- <Node name="Underarm R"/>
- <Node name="Wrist 1 R"/>
- <Node name="Wrist 2 R"/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name="TorsoBothArms" kinematicRoot="Platform">
- <Node name="Hip Pitch"/>
- <Node name="Hip Roll"/>
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 L"/>
- <Node name="Shoulder 2 L"/>
- <Node name="Upperarm L"/>
- <Node name="Elbow L"/>
- <Node name="Underarm L"/>
- <Node name="Wrist 1 L"/>
- <Node name="Wrist 2 L"/>
- <Node name="Shoulder 1 R"/>
- <Node name="Shoulder 2 R"/>
- <Node name="Upperarm R"/>
- <Node name="Elbow R"/>
- <Node name="Underarm R"/>
- <Node name="Wrist 1 R"/>
- <Node name="Wrist 2 R"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="Robot" kinematicRoot="Platform">
- <Node name="Neck_1_Pitch"/>
- <Node name="Neck_2_Roll"/>
- <Node name="Neck_3_Yaw"/>
- <Node name="Head_Tilt"/>
- <Node name="Cameras"/>
- <Node name="Eye_Left"/>
- <Node name="Eye_Right"/>
- <Node name="VirtualCentralGaze"/>
- <Node name="Hip Pitch"/>
- <Node name="Hip Roll"/>
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 L"/>
- <Node name="Shoulder 2 L"/>
- <Node name="Upperarm L"/>
- <Node name="Elbow L"/>
- <Node name="Underarm L"/>
- <Node name="Wrist 1 L"/>
- <Node name="Wrist 2 L"/>
- <Node name="Shoulder 1 R"/>
- <Node name="Shoulder 2 R"/>
- <Node name="Upperarm R"/>
- <Node name="Elbow R"/>
- <Node name="Underarm R"/>
- <Node name="Wrist 1 R"/>
- <Node name="Wrist 2 R"/>
- <Node name="Hand Palm 1 L"/>
- <Node name="Hand Palm 2 L"/>
- <Node name="Thumb L J0"/>
- <Node name="Thumb L J1"/>
- <Node name="Index L J0"/>
- <Node name="Index L J1"/>
- <Node name="Middle L J0"/>
- <Node name="Middle L J1"/>
- <Node name="Ring L J0"/>
- <Node name="Ring L J1"/>
- <Node name="Pinky L J0"/>
- <Node name="Pinky L J1"/>
- <Node name="Hand Palm 1 R"/>
- <Node name="Hand Palm 2 R"/>
- <Node name="Thumb R J0"/>
- <Node name="Thumb R J1"/>
- <Node name="Index R J0"/>
- <Node name="Index R J1"/>
- <Node name="Middle R J0"/>
- <Node name="Middle R J1"/>
- <Node name="Ring R J0"/>
- <Node name="Ring R J1"/>
- <Node name="Pinky R J0"/>
- <Node name="Pinky R J1"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="PlatformYawTorsoLeftArm" kinematicRoot="Armar3_Base" tcp="TCP L">
- <Node name="Yaw_Platform"/>
- <Node name="Hip Pitch"/>
- <Node name="Hip Roll"/>
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 L"/>
- <Node name="Shoulder 2 L"/>
- <Node name="Upperarm L"/>
- <Node name="Elbow L"/>
- <Node name="Underarm L"/>
- <Node name="Wrist 1 L"/>
- <Node name="Wrist 2 L"/>
- </RobotNodeSet>
-
- <RobotNodeSet name="PlatformYawTorsoRightArm" kinematicRoot="Armar3_Base" tcp="TCP R">
- <Node name="Yaw_Platform"/>
- <Node name="Hip Pitch"/>
- <Node name="Hip Roll"/>
- <Node name="Hip Yaw"/>
- <Node name="Shoulder 1 R"/>
- <Node name="Shoulder 2 R"/>
- <Node name="Upperarm R"/>
- <Node name="Elbow R"/>
- <Node name="Underarm R"/>
- <Node name="Wrist 1 R"/>
- <Node name="Wrist 2 R"/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name="HeadMotionMeasurementChain" kinematicRoot="Neck_1_Pitch" tcp="HeadMotionMeasurementTCP">
- <Node name="Neck_1_Pitch"/>
- <Node name="Neck_2_Roll"/>
- <Node name="Neck_3_Yaw"/>
- <Node name="Head_Tilt"/>
- <Node name="Cameras"/>
- <Node name="Eye_Left"/>
- <Node name="HeadMotionMeasurementTCP"/>
- </RobotNodeSet>
-
-
-</Robot>
diff --git a/etc/cmake/ArmarXPackageVersion.cmake b/etc/cmake/ArmarXPackageVersion.cmake
index 278af65627cbc1601bc0047131d482b84a116382..beeea8785801fcb7c91a9b1a4012e51c72b66e7e 100644
--- a/etc/cmake/ArmarXPackageVersion.cmake
+++ b/etc/cmake/ArmarXPackageVersion.cmake
@@ -1,7 +1,7 @@
# armarx version file
set(ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR "0")
set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "8")
-set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "4")
+set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "5")
set(ARMARX_PACKAGE_LIBRARY_VERSION "${ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_MINOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_PATCH}")
diff --git a/etc/doxygen/pages/GuiPlugins.dox b/etc/doxygen/pages/GuiPlugins.dox
index 5aea7e242f7e7a0711e4cbbf78c4bbdc32fa1dbe..3adafb1762821dbbfaa0b581e2d50c680791c522 100644
--- a/etc/doxygen/pages/GuiPlugins.dox
+++ b/etc/doxygen/pages/GuiPlugins.dox
@@ -16,4 +16,6 @@ The following Gui Plugins are available:
\subpage RobotAPI-GuiPlugins-PlatformUnitPlugin
\subpage RobotAPI-GuiPlugins-RobotViewerPlugin
+
+\subpage ArmarXGui-GuiPlugins-DebugDrawerViewer
*/
diff --git a/etc/doxygen/pages/RemoteRobot.dox b/etc/doxygen/pages/RemoteRobot.dox
index d94a1b5688537bb0b127cbffeceaa87d10ef2628..b135e1008d9fb7b7add46ab8eaa7239b4dbc03bf 100644
--- a/etc/doxygen/pages/RemoteRobot.dox
+++ b/etc/doxygen/pages/RemoteRobot.dox
@@ -60,11 +60,11 @@ You can create a local clone really easy like this:
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/interface/core/RobotState.h>
-VirtualRobot::RobotPtr localRobot(RemoteRobot::createLocalClone(robotStateComponentPrx));
+VirtualRobot::RobotPtr localRobot(RemoteRobot::createLocalClone(robotStateComponentPrx, robotStateComponentPrx->getRobotFilename(), robotStateComponentPrx->getArmarXPackages()));
\endcode
Now, you have a local robot clone with the robot state from the timestamp where the clone method was called.
This offers only basic functionality of the RobotPtr like joint names, joint values, robot nodes, robot node sets, robot node poses and transformations.
-If you need all functionality, pass the filepath to the clone method (this path can be retrieved from the RobotStateComponent if both processes are on the same filesystem or have the same filesystem structure).
+If you pass in an empty filename (or omit) to the clone method only limited functionality is available (only coordinate transformations).
This clone call should be done once, since it can take a moment.
In each cycle of a control loop, the local robot should just be synced to retrieve the current joint values:
\code
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index c6e30a4ba6c962ed6e64e8ef245d78fd4191bd4a..736267ef643f75f212ba27af7efa84ff362f9464 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -20,3 +20,5 @@ add_subdirectory(ForceTorqueUnitSimulation)
add_subdirectory(XsensIMU)
add_subdirectory(InertialMeasurementUnitObserver)
+
+add_subdirectory(ViewSelection)
diff --git a/source/RobotAPI/applications/ViewSelection/CMakeLists.txt b/source/RobotAPI/applications/ViewSelection/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e9617c08694be32ad5ed3f2bc137b3c1bec37390
--- /dev/null
+++ b/source/RobotAPI/applications/ViewSelection/CMakeLists.txt
@@ -0,0 +1,19 @@
+armarx_component_set_name("ViewSelectionApp")
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+if(Eigen3_FOUND)
+ include_directories(${Eigen3_INCLUDE_DIR})
+endif()
+
+
+set(COMPONENT_LIBS
+ ViewSelection
+ RobotAPIInterfaces RobotAPICore
+ ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers
+)
+
+
+set(EXE_SOURCE main.cpp ViewSelectionApp.h)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/ViewSelection/ViewSelectionApp.h b/source/RobotAPI/applications/ViewSelection/ViewSelectionApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..f26a5682222cf775fe27d4e087e26bb9916aab2f
--- /dev/null
+++ b/source/RobotAPI/applications/ViewSelection/ViewSelectionApp.h
@@ -0,0 +1,54 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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 RobotComponents::application::ViewSelection
+ * @author David Schiebener (schiebener qt kit dot edu)
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_APPLICATION_RobotComponents_ViewSelection_H
+#define _ARMARX_APPLICATION_RobotComponents_ViewSelection_H
+
+
+#include <ArmarXCore/core/application/Application.h>
+#include <RobotAPI/components/ViewSelection/ViewSelection.h>
+
+namespace armarx
+{
+ /**
+ * @class ViewSelectionApp
+ * @brief A brief description
+ *
+ * Detailed Description
+ */
+ class ViewSelectionApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties)
+ {
+ registry->addObject(Component::create<ViewSelection>(properties));
+ }
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/applications/ViewSelection/main.cpp b/source/RobotAPI/applications/ViewSelection/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01e91acc5430646d786aa12fd1207487e0da7a93
--- /dev/null
+++ b/source/RobotAPI/applications/ViewSelection/main.cpp
@@ -0,0 +1,34 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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 RobotComponents::application::ViewSelection
+ * @author David Schiebener (schiebener qt kit dot edu)
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ViewSelectionApp.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::ViewSelectionApp > ();
+ app->setName("ViewSelection");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index fb09cfb0a45c54b1fd2f503b76a857ccd73a87e4..bf391c4c6bea7637e8137f4319e1e1a4e032d731 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -1,5 +1,5 @@
-
add_subdirectory(units)
add_subdirectory(DebugDrawer)
add_subdirectory(RobotState)
-
+add_subdirectory(EarlyVisionGraph)
+add_subdirectory(ViewSelection)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 31d5d60da516783f06524f104b677b04c64bad7a..270193c7952c20ae7a5b9c9c47e1ad3e739af027 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -251,7 +251,6 @@ namespace armarx
DebugDrawerComponent::~DebugDrawerComponent()
{
- std::cout << "DESTRUCTOR debug drawer" << std::endl;
/*{
ScopedRecursiveLockPtr l = getScopedLock();
layerMainNode->unref();
@@ -296,6 +295,7 @@ namespace armarx
return;
}
+ ScopedLock lock(drawer->timerSensorMutex);
drawer->updateVisualization();
}
@@ -333,6 +333,7 @@ namespace armarx
//ARMARX_DEBUG << "onExitComponent";
if (timerSensor)
{
+ ScopedLock lock(timerSensorMutex);
SoSensorManager* sensor_mgr = SoDB::getSensorManager();
sensor_mgr->removeTimerSensor(timerSensor);
}
@@ -758,17 +759,17 @@ namespace armarx
TriMeshModelPtr triMesh = TriMeshModelPtr(new TriMeshModel());
- for(DrawColor color : d.triMesh.colors)
+ for (DrawColor color : d.triMesh.colors)
{
triMesh->addColor(VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, color.a));
}
- for(DebugDrawerVertex v : d.triMesh.vertices)
+ for (DebugDrawerVertex v : d.triMesh.vertices)
{
triMesh->addVertex(Eigen::Vector3f(v.x, v.y, v.z));
}
- for(DebugDrawerFace f : d.triMesh.faces)
+ for (DebugDrawerFace f : d.triMesh.faces)
{
MathTools::TriangleFace face;
@@ -1890,14 +1891,12 @@ namespace armarx
void DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
{
ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ //updates should only contain elements to add for each layer after the last clear for this layer was executed
+ //this prevents a sequence add,clear,add to result in an empty layer
+ //=>remove all objects pending for this layer
+ removeAccumulatedData(layerName);
+ accumulatedUpdateData.clearLayers.emplace(layerName);
- // check if already requested
- if (std::find(accumulatedUpdateData.clearLayers.begin(), accumulatedUpdateData.clearLayers.end(), layerName) != accumulatedUpdateData.clearLayers.end())
- {
- return;
- }
-
- accumulatedUpdateData.clearLayers.push_back(layerName);
}
void DebugDrawerComponent::clearLayerQt(const std::string& layerName)
@@ -1924,8 +1923,6 @@ namespace armarx
selectionNode->deselectAll();
- removeAccumulatedData(layerName);
-
Layer& layer = layers.at(layerName);
for (const auto & i : layer.addedCoordVisualizations)
@@ -2063,120 +2060,109 @@ namespace armarx
ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
- for (auto i = accumulatedUpdateData.coord.begin(); i != accumulatedUpdateData.coord.end(); i++)
+ // check for clear&remove
+ //(updates only contain elements to add for each layer after the last clear for this layer was executed)
+ //this prevents a sequence add,clear,add to result in an empty layer
+ for (const auto & layer : accumulatedUpdateData.clearLayers)
{
- drawCoordSystem(i->second);
+ clearLayerQt(layer);
}
+ accumulatedUpdateData.clearLayers.clear();
- accumulatedUpdateData.coord.clear();
+ for (const auto & layer : accumulatedUpdateData.removeLayers)
+ {
+ removeLayerQt(layer);
+ }
+ accumulatedUpdateData.removeLayers.clear();
- for (auto i = accumulatedUpdateData.box.begin(); i != accumulatedUpdateData.box.end(); i++)
+ //add elements
+ for (auto & e : accumulatedUpdateData.coord)
{
- drawBox(i->second);
+ drawCoordSystem(e.second);
}
+ accumulatedUpdateData.coord.clear();
+ for (auto & e : accumulatedUpdateData.box)
+ {
+ drawBox(e.second);
+ }
accumulatedUpdateData.box.clear();
- for (auto i = accumulatedUpdateData.line.begin(); i != accumulatedUpdateData.line.end(); i++)
+ for (auto & e : accumulatedUpdateData.line)
{
- drawLine(i->second);
+ drawLine(e.second);
}
-
accumulatedUpdateData.line.clear();
- for (auto i = accumulatedUpdateData.lineSet.begin(); i != accumulatedUpdateData.lineSet.end(); i++)
+ for (auto & e : accumulatedUpdateData.lineSet)
{
- drawLineSet(i->second);
+ drawLineSet(e.second);
}
-
accumulatedUpdateData.lineSet.clear();
- for (auto i = accumulatedUpdateData.sphere.begin(); i != accumulatedUpdateData.sphere.end(); i++)
+ for (auto & e : accumulatedUpdateData.sphere)
{
- drawSphere(i->second);
+ drawSphere(e.second);
}
-
accumulatedUpdateData.sphere.clear();
- for (auto i = accumulatedUpdateData.cylinder.begin(); i != accumulatedUpdateData.cylinder.end(); i++)
+ for (auto & e : accumulatedUpdateData.cylinder)
{
- drawCylinder(i->second);
+ drawCylinder(e.second);
}
-
accumulatedUpdateData.cylinder.clear();
- for (auto i = accumulatedUpdateData.text.begin(); i != accumulatedUpdateData.text.end(); i++)
+ for (auto & e : accumulatedUpdateData.text)
{
- drawText(i->second);
+ drawText(e.second);
}
-
accumulatedUpdateData.text.clear();
- for (auto i = accumulatedUpdateData.pointcloud.begin(); i != accumulatedUpdateData.pointcloud.end(); i++)
+ for (auto & e : accumulatedUpdateData.pointcloud)
{
- drawPointCloud(i->second);
+ drawPointCloud(e.second);
}
-
accumulatedUpdateData.pointcloud.clear();
- for (auto i = accumulatedUpdateData.polygons.begin(); i != accumulatedUpdateData.polygons.end(); i++)
+ for (auto & e : accumulatedUpdateData.polygons)
{
- drawPolygon(i->second);
+ drawPolygon(e.second);
}
-
accumulatedUpdateData.polygons.clear();
- for (auto i = accumulatedUpdateData.arrows.begin(); i != accumulatedUpdateData.arrows.end(); i++)
+ for (auto & e : accumulatedUpdateData.arrows)
{
- drawArrow(i->second);
+ drawArrow(e.second);
}
-
accumulatedUpdateData.arrows.clear();
for (auto i = accumulatedUpdateData.triMeshes.begin(); i != accumulatedUpdateData.triMeshes.end(); i++)
{
drawTriMesh(i->second);
}
-
accumulatedUpdateData.triMeshes.clear();
- for (auto i = accumulatedUpdateData.robots.begin(); i != accumulatedUpdateData.robots.end(); i++)
+ for (auto & e : accumulatedUpdateData.robots)
{
- ARMARX_DEBUG << "update visu / drawRobot for robot " << i->first;
- ensureExistingRobotNodes(i->second);
+ ARMARX_DEBUG << "update visu / drawRobot for robot " << e.first;
+ ensureExistingRobotNodes(e.second);
- drawRobot(i->second);
+ drawRobot(e.second);
}
-
accumulatedUpdateData.robots.clear();
- for (auto i = accumulatedUpdateData.coloredpointcloud.begin(); i != accumulatedUpdateData.coloredpointcloud.end(); i++)
+ for (auto & e : accumulatedUpdateData.coloredpointcloud)
{
- drawColoredPointCloud(i->second);
+ drawColoredPointCloud(e.second);
}
-
accumulatedUpdateData.coloredpointcloud.clear();
- for (auto i = accumulatedUpdateData.colored24Bitpointcloud.begin(); i != accumulatedUpdateData.colored24Bitpointcloud.end(); i++)
+ for (auto & e : accumulatedUpdateData.colored24Bitpointcloud)
{
- draw24BitColoredPointCloud(i->second);
+ draw24BitColoredPointCloud(e.second);
}
-
accumulatedUpdateData.colored24Bitpointcloud.clear();
- // check for clear and remove
- for (auto i = accumulatedUpdateData.clearLayers.begin(); i != accumulatedUpdateData.clearLayers.end(); i++)
- {
- clearLayerQt(*i);
- }
- accumulatedUpdateData.clearLayers.clear();
-
- for (auto i = accumulatedUpdateData.removeLayers.begin(); i != accumulatedUpdateData.removeLayers.end(); i++)
- {
- removeLayerQt(*i);
- }
- accumulatedUpdateData.removeLayers.clear();
-
onUpdateVisualization();
}
@@ -2189,13 +2175,11 @@ namespace armarx
void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
{
ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- // check if already requested
- if (std::find(accumulatedUpdateData.removeLayers.begin(), accumulatedUpdateData.removeLayers.end(), layerName) != accumulatedUpdateData.removeLayers.end())
- {
- return;
- }
-
- accumulatedUpdateData.removeLayers.push_back(layerName);
+ //updates should only contain elements to add for each layer after the last clear/remove for this layer was executed
+ //this prevents a sequence add,clear,add to result in an empty layer
+ //=>remove all objects pending for this layer
+ removeAccumulatedData(layerName);
+ accumulatedUpdateData.removeLayers.emplace(layerName);
}
void DebugDrawerComponent::removeLayerQt(const std::string& layerName)
@@ -2226,208 +2210,38 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
+ template<class UpdateDataType>
+ void removeUpdateDataFromMap(const std::string& layerName, std::map<std::string, UpdateDataType>& map)
{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__";
- {
- auto i1 = accumulatedUpdateData.coord.begin();
-
- while (i1 != accumulatedUpdateData.coord.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.coord.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.box.begin();
-
- while (i1 != accumulatedUpdateData.box.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.box.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.line.begin();
-
- while (i1 != accumulatedUpdateData.line.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.line.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.lineSet.begin();
-
- while (i1 != accumulatedUpdateData.lineSet.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.lineSet.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.sphere.begin();
-
- while (i1 != accumulatedUpdateData.sphere.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.sphere.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.cylinder.begin();
-
- while (i1 != accumulatedUpdateData.cylinder.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.cylinder.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.text.begin();
-
- while (i1 != accumulatedUpdateData.text.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.text.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.pointcloud.begin();
-
- while (i1 != accumulatedUpdateData.pointcloud.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.pointcloud.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.polygons.begin();
-
- while (i1 != accumulatedUpdateData.polygons.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.polygons.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.triMeshes.begin();
-
- while (i1 != accumulatedUpdateData.triMeshes.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.triMeshes.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
+ auto it = map.begin();
+ while (it != map.end())
{
- auto i1 = accumulatedUpdateData.arrows.begin();
-
- while (i1 != accumulatedUpdateData.arrows.end())
+ auto curr = it++;
+ if (curr->second.layerName == layerName)
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.arrows.erase(i1);
- }
- else
- {
- i1++;
- }
+ map.erase(curr);
}
}
+ }
- {
- ARMARX_DEBUG << "Removing all accumulated data for robot, nr of robots:" << accumulatedUpdateData.robots.size();
-
- auto i1 = accumulatedUpdateData.robots.begin();
-
- while (i1 != accumulatedUpdateData.robots.end())
- {
- ARMARX_DEBUG << "checking accumulated data " << i1->first;
-
- if (boost::starts_with(i1->first, entryName))
- {
- ARMARX_DEBUG << "removing accumulated data for " << i1->first;
- i1 = accumulatedUpdateData.robots.erase(i1);
- }
- else
- {
- i1++;
- }
- }
+ void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
+ {
+ ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- ARMARX_DEBUG << "end, nr of robots:" << accumulatedUpdateData.robots.size();
- }
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.coord);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.line);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.lineSet);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.box);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.text);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.sphere);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.cylinder);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.pointcloud);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.coloredpointcloud);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.colored24Bitpointcloud);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.polygons);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.arrows);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.robots);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.triMeshes);
onRemoveAccumulatedData(layerName);
@@ -2461,7 +2275,6 @@ namespace armarx
::armarx::LayerInformationSequence seq {};
ScopedRecursiveLockPtr l = getScopedVisuLock();
- // @@@ TODO: FIXME: count does not include all maps!
for (const auto & layer : layers)
{
int count = layer.second.addedCoordVisualizations.size() +
@@ -2472,9 +2285,11 @@ namespace armarx
layer.second.addedSphereVisualizations.size() +
layer.second.addedCylinderVisualizations.size() +
layer.second.addedPointCloudVisualizations.size() +
- layer.second.addedArrowVisualizations.size() +
+ layer.second.addedColoredPointCloudVisualizations.size() +
+ layer.second.added24BitColoredPointCloudVisualizations.size() +
layer.second.addedPolygonVisualizations.size() +
layer.second.addedTriMeshVisualizations.size() +
+ layer.second.addedArrowVisualizations.size() +
layer.second.addedRobotVisualizations.size() +
layer.second.addedCustomVisualizations.size();
::armarx::LayerInformation info = {layer.first, layer.second.visible, count};
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index dbf74aacdb8907dc0714f916104e43938e556bb7..bef471367c53318c2a798132f4d156fd699b1ede 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -298,6 +298,9 @@ namespace armarx
std::string layerName;
std::string name;
bool active;
+ /**
+ * @brief Whether an existing visu should be updated
+ */
bool update;
};
@@ -418,8 +421,8 @@ namespace armarx
std::map<std::string, ArrowData> arrows;
std::map<std::string, RobotData> robots;
- std::vector< std::string > clearLayers;
- std::vector< std::string > removeLayers;
+ std::set< std::string > clearLayers;
+ std::set< std::string > removeLayers;
};
UpdateData accumulatedUpdateData;
@@ -547,6 +550,7 @@ namespace armarx
float cycleTimeMS;
//PeriodicTask<DebugDrawerComponent>::pointer_type execTaskVisuUpdates;
SoTimerSensor* timerSensor;
+ Mutex timerSensorMutex;
void removeAccumulatedData(const std::string& layerName);
};
diff --git a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3398169a541f572de495643794620d2d489087f0
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
@@ -0,0 +1,34 @@
+armarx_component_set_name("EarlyVisionGraph")
+
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+if(Eigen3_FOUND)
+ include_directories(${Eigen3_INCLUDE_DIR})
+endif()
+
+
+set(COMPONENT_LIBS "")
+
+
+set(SOURCES
+./MathTools.cpp
+./SphericalGraph.cpp
+./IntensityGraph.cpp
+./GraphLookupTable.cpp
+./GraphPyramidLookupTable.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+./Structs.h
+./MathTools.h
+./SphericalGraph.h
+./IntensityGraph.h
+./GraphLookupTable.h
+./GraphPyramidLookupTable.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf424e5b3f05a37b6d55f52d8e6ebb2fb7bbd2b0
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp
@@ -0,0 +1,351 @@
+// *****************************************************************
+// Filename: GraphGenerator.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 08.07.2008
+// *****************************************************************
+
+#include "GraphGenerator.h"
+
+vector<Vec3d> CGraphGenerator::m_Vertices;
+vector<Face> CGraphGenerator::m_Faces;
+
+
+void CGraphGenerator::generateGraph(CSphericalGraph* pPlainGraph, EPolyhedronType nBaseType, int nLevels, float fRadius)
+{
+ initBasePolyhedron(nBaseType);
+ projectToSphere(fRadius);
+
+ for (int L = 0 ; L < nLevels ; L++)
+ {
+ subDivide();
+ projectToSphere(fRadius);
+ }
+
+ buildNodesAndEdges(pPlainGraph);
+}
+
+void CGraphGenerator::initBasePolyhedron(EPolyhedronType nBaseType)
+{
+ m_Vertices.clear();
+ m_Faces.clear();
+
+ switch (nBaseType)
+ {
+ case eTetrahedron:
+ generateTetrahedron();
+ break;
+
+ case eOctahedron:
+ generateOctahedron();
+ break;
+
+ case eIcosahedron:
+ generateIcosahedron();
+ break;
+
+ default:
+ printf("Wrong base type\n");
+ break;
+ }
+}
+
+void CGraphGenerator::generateTetrahedron()
+{
+ // vertices of a tetrahedron
+ float fSqrt3 = sqrt(3.0);
+ Vec3d v[4];
+
+ Math3d::SetVec(v[0], fSqrt3, fSqrt3, fSqrt3);
+ Math3d::SetVec(v[1], -fSqrt3, -fSqrt3, fSqrt3);
+ Math3d::SetVec(v[2], -fSqrt3, fSqrt3, -fSqrt3);
+ Math3d::SetVec(v[3], fSqrt3, -fSqrt3, -fSqrt3);
+
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ m_Vertices.push_back(v[i]);
+ }
+
+ // structure describing a tetrahedron
+ Face f[4];
+ f[0].set(1, 2, 3);
+ f[1].set(1, 4, 2);
+ f[2].set(3, 2, 4);
+ f[3].set(4, 1, 3);
+
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ f[i].m_n1--;
+ f[i].m_n2--;
+ f[i].m_n3--;
+ m_Faces.push_back(f[i]);
+ }
+}
+
+void CGraphGenerator::generateOctahedron()
+{
+ // Six equidistant points lying on the unit sphere
+ Vec3d v[6];
+ Math3d::SetVec(v[0], 1, 0, 0);
+ Math3d::SetVec(v[1], -1, 0, 0);
+ Math3d::SetVec(v[2], 0, 1, 0);
+ Math3d::SetVec(v[3], 0, -1, 0);
+ Math3d::SetVec(v[4], 0, 0, 1);
+ Math3d::SetVec(v[5], 0, 0, -1);
+
+ for (int i = 0 ; i < 6 ; i++)
+ {
+ m_Vertices.push_back(v[i]);
+ }
+
+ // Join vertices to create a unit octahedron
+ Face f[8];
+ f[0].set(1, 5, 3);
+ f[1].set(3, 5, 2);
+ f[2].set(2, 5, 4);
+ f[3].set(4, 5, 1);
+ f[4].set(1, 3, 6);
+ f[5].set(3, 2, 6);
+ f[6].set(2, 4, 6);
+ f[7].set(4, 1, 6);
+
+ for (int i = 0 ; i < 8 ; i++)
+ {
+ f[i].m_n1--;
+ f[i].m_n2--;
+ f[i].m_n3--;
+ m_Faces.push_back(f[i]);
+ }
+}
+
+void CGraphGenerator::generateIcosahedron()
+{
+ // Twelve vertices of icosahedron on unit sphere
+ float tau = 0.8506508084;
+ float one = 0.5257311121;
+
+ Vec3d v[12];
+
+ Math3d::SetVec(v[0], tau, one, 0);
+ Math3d::SetVec(v[1], -tau, one, 0);
+ Math3d::SetVec(v[2], -tau, -one, 0);
+ Math3d::SetVec(v[3], tau, -one, 0);
+ Math3d::SetVec(v[4], one, 0, tau);
+ Math3d::SetVec(v[5], one, 0, -tau);
+ Math3d::SetVec(v[6], -one, 0, -tau);
+ Math3d::SetVec(v[7], -one, 0, tau);
+ Math3d::SetVec(v[8], 0, tau, one);
+ Math3d::SetVec(v[9], 0, -tau, one);
+ Math3d::SetVec(v[10], 0, -tau, -one);
+ Math3d::SetVec(v[11], 0, tau, -one);
+
+ for (int i = 0 ; i < 12 ; i++)
+ {
+ m_Vertices.push_back(v[i]);
+ }
+
+
+ // Structure for unit icosahedron
+ Face f[20];
+ f[0].set(5, 8, 9);
+ f[1].set(5, 10, 8);
+ f[2].set(6, 12, 7);
+ f[3].set(6, 7, 11);
+ f[4].set(1, 4, 5);
+ f[5].set(1, 6, 4);
+ f[6].set(3, 2, 8);
+ f[7].set(3, 7, 2);
+ f[8].set(9, 12, 1);
+ f[9].set(9, 2, 12);
+ f[10].set(10, 4, 11);
+ f[11].set(10, 11, 3);
+ f[12].set(9, 1, 5);
+ f[13].set(12, 6, 1);
+ f[14].set(5, 4, 10);
+ f[15].set(6, 11, 4);
+ f[16].set(8, 2, 9);
+ f[17].set(7, 12, 2);
+ f[18].set(8, 10, 3);
+ f[19].set(7, 3, 11);
+
+ for (int i = 0 ; i < 20 ; i++)
+ {
+ f[i].m_n1--;
+ f[i].m_n2--;
+ f[i].m_n3--;
+ m_Faces.push_back(f[i]);
+ }
+}
+
+void CGraphGenerator::subDivide()
+{
+ // buffer new faces
+ vector<Face> faces;
+
+ // gp through all faces and subdivide
+ for (int f = 0 ; f < int(m_Faces.size()) ; f++)
+ {
+ // get the triangle vertex indices
+ int nA = m_Faces.at(f).m_n1;
+ int nB = m_Faces.at(f).m_n2;
+ int nC = m_Faces.at(f).m_n3;
+
+ // get the triangle vertex coordinates
+ Vec3d a = m_Vertices.at(nA);
+ Vec3d b = m_Vertices.at(nB);
+ Vec3d c = m_Vertices.at(nC);
+
+ // Now find the midpoints between vertices
+ Vec3d a_m, b_m, c_m;
+ Math3d::AddVecVec(a, b, a_m);
+ Math3d::MulVecScalar(a_m, 0.5, a_m);
+ Math3d::AddVecVec(b, c, b_m);
+ Math3d::MulVecScalar(b_m, 0.5, b_m);
+ Math3d::AddVecVec(c, a, c_m);
+ Math3d::MulVecScalar(c_m, 0.5, c_m);
+ /* printf("a: %f,%f,%f\n", a.x,a.y,a.z);
+ printf("b: %f,%f,%f\n", b.x,b.y,b.z);
+ printf("c: %f,%f,%f\n", c.x,c.y,c.z);
+
+ printf("a<->b: %f,%f,%f\n", a_m.x,a_m.y,a_m.z);
+ printf("b<->c: %f,%f,%f\n", b_m.x,b_m.y,b_m.z);
+ printf("c<->a: %f,%f,%f\n", c_m.x,c_m.y,c_m.z);*/
+
+ // add vertices
+ int nA_m, nB_m, nC_m;
+
+ int nIndex;
+ nIndex = findVertex(a_m, 0.01);
+
+ if (nIndex == -1)
+ {
+ m_Vertices.push_back(a_m);
+ nA_m = m_Vertices.size() - 1;
+ }
+ else
+ {
+ nA_m = nIndex;
+ }
+
+ nIndex = findVertex(b_m, 0.01);
+
+ if (nIndex == -1)
+ {
+ m_Vertices.push_back(b_m);
+ nB_m = m_Vertices.size() - 1;
+ }
+ else
+ {
+ nB_m = nIndex;
+ }
+
+ nIndex = findVertex(c_m, 0.01);
+
+ if (nIndex == -1)
+ {
+ m_Vertices.push_back(c_m);
+ nC_m = m_Vertices.size() - 1;
+ }
+ else
+ {
+ nC_m = nIndex;
+ }
+
+ // Create new faces with orig vertices plus midpoints
+ Face f[4];
+ f[0].set(nA, nA_m, nC_m);
+ f[1].set(nA_m, nB, nB_m);
+ f[2].set(nC_m, nB_m, nC);
+ f[3].set(nA_m, nB_m, nC_m);
+
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ faces.push_back(f[i]);
+ }
+ }
+
+ // assign new faces
+ m_Faces = faces;
+}
+
+void CGraphGenerator::projectToSphere(float fRadius)
+{
+ TSphereCoord coord;
+ Vec3d point;
+ coord.fDistance = 1.0f;
+
+ for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ {
+ float X = m_Vertices.at(v).x;
+ float Y = m_Vertices.at(v).y;
+ float Z = m_Vertices.at(v).z;
+
+ float x, y, z;
+
+ // Convert Cartesian X,Y,Z to spherical (radians)
+ float theta = atan2(Y, X);
+ float phi = atan2(sqrt(X * X + Y * Y), Z);
+
+ // Recalculate X,Y,Z for constant r, given theta & phi.
+ x = fRadius * sin(phi) * cos(theta);
+ y = fRadius * sin(phi) * sin(theta);
+ z = fRadius * cos(phi);
+
+ m_Vertices.at(v).x = x;
+ m_Vertices.at(v).y = y;
+ m_Vertices.at(v).z = z;
+ }
+}
+
+void CGraphGenerator::buildNodesAndEdges(CSphericalGraph* pGraph)
+{
+ // add nodes
+ TSphereCoord coord;
+
+ for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ {
+ // printf("Vertex: %f,%f,%f\n", m_Vertices.at(v).x, m_Vertices.at(v).y, m_Vertices.at(v).z);
+ MathTools::convert(m_Vertices.at(v), coord);
+ CSGNode* pNode = pGraph->getNewNode();
+ pNode->setPosition(coord);
+ pGraph->addNode(pNode);
+ }
+
+ // add edges
+ // go through all faces (assumes check for duplicate edges in spherical graph!?)
+ for (int f = 0 ; f < int(m_Faces.size()) ; f++)
+ {
+ // printf(" %d,%d,%d\n", m_Faces.at(f).m_n1, m_Faces.at(f).m_n2, m_Faces.at(f).m_n3);
+ pGraph->addEdge(m_Faces.at(f).m_n1, m_Faces.at(f).m_n2);
+ pGraph->addEdge(m_Faces.at(f).m_n2, m_Faces.at(f).m_n3);
+ pGraph->addEdge(m_Faces.at(f).m_n3, m_Faces.at(f).m_n1);
+ }
+}
+
+int CGraphGenerator::findVertex(Vec3d position, float fEpsilon)
+{
+ float fDistance;
+ float fMinDistance = FLT_MAX;
+ int nIndex = -1;
+
+ for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ {
+ fDistance = Math3d::Angle(position, m_Vertices.at(v));
+
+ if (fDistance < fMinDistance)
+ {
+ fMinDistance = fDistance;
+ nIndex = v;
+ }
+ }
+
+ if (fMinDistance > fEpsilon)
+ {
+ nIndex = -1;
+ }
+
+ return nIndex;
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..7904916daf5bb2f4c16f0c7f3abdf43b9b31c4b9
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h
@@ -0,0 +1,85 @@
+// *****************************************************************
+// Filename: GraphGenerator.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 08.07.2008
+// *****************************************************************
+
+#ifndef _GRAPH_GENERATOR_H_
+#define _GRAPH_GENERATOR_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "Base/DataStructures/Graph/SphericalGraph.h"
+#include "Base/Math/MathTools.h"
+#include "Math/Math3d.h"
+#include <vector>
+#include <float.h>
+
+using namespace std;
+
+// *****************************************************************
+// enums
+// *****************************************************************
+enum EPolyhedronType
+{
+ eTetrahedron,
+ eOctahedron,
+ eIcosahedron
+};
+
+// *****************************************************************
+// implementation of class Face
+// *****************************************************************
+class Face
+{
+public:
+ Face() {};
+ Face(int n1, int n2, int n3)
+ {
+ set(n1, n2, n3);
+ };
+ ~Face() {};
+
+ void set(int n1, int n2, int n3)
+ {
+ m_n1 = n1;
+ m_n2 = n2 ;
+ m_n3 = n3;
+ };
+
+ int m_n1;
+ int m_n2;
+ int m_n3;
+};
+
+// *****************************************************************
+// implementation of CGraphGenerator
+// *****************************************************************
+class CGraphGenerator
+{
+public:
+ static void generateGraph(CSphericalGraph* pPlainGraph, EPolyhedronType nBaseType, int nLevels, float fRadius);
+
+private:
+ static void initBasePolyhedron(EPolyhedronType nBaseType);
+ static void generateTetrahedron();
+ static void generateOctahedron();
+ static void generateIcosahedron();
+ static void subDivide();
+ static void projectToSphere(float fRadius);
+ static void buildNodesAndEdges(CSphericalGraph* pGraph);
+ static int findVertex(Vec3d position, float fEpsilon);
+
+private:
+ CGraphGenerator() {};
+ ~CGraphGenerator() {};
+
+ static vector<Vec3d> m_Vertices;
+ static vector<Face> m_Faces;
+};
+
+#endif /* _GRAPH_GENERATOR_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a6e856c5614ca95e7184c63f2da94776bd722bcd
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp
@@ -0,0 +1,238 @@
+// *****************************************************************
+// Filename: GraphLookupTable.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 10.10.2008
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "GraphLookupTable.h"
+#include "MathTools.h"
+#include <float.h>
+
+// *****************************************************************
+// implementation of CGraphLookupTable
+// *****************************************************************
+// construction / destruction
+CGraphLookupTable::CGraphLookupTable(int nMaxZenithBins, int nMaxAzimuthBins)
+{
+ m_nMaxZenithBins = nMaxZenithBins;
+ m_nMaxAzimuthBins = nMaxAzimuthBins;
+
+ buildMemory();
+}
+
+CGraphLookupTable::~CGraphLookupTable()
+{
+ cleanMemory();
+}
+
+void CGraphLookupTable::buildLookupTable(CSphericalGraph* pGraph)
+{
+ reset();
+
+ TNodeList* pNodes = pGraph->getNodes();
+ int nSize = pNodes->size();
+
+ for (int n = 0 ; n < nSize ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ addEntry(pNode->getPosition(), n);
+ }
+
+ m_pGraph = pGraph;
+}
+
+int CGraphLookupTable::getClosestNode(TSphereCoord position)
+{
+ bool bExact;
+ return getClosestNode(position, bExact);
+}
+
+int CGraphLookupTable::getClosestNode(Eigen::Vector3d position)
+{
+ bool bExact;
+ return getClosestNode(position, bExact);
+}
+
+int CGraphLookupTable::getClosestNode(Eigen::Vector3d position, bool& bExact)
+{
+ TSphereCoord coords;
+ MathTools::convert(position, coords);
+
+ return getClosestNode(coords, bExact);
+}
+
+
+int CGraphLookupTable::getClosestNode(TSphereCoord position, bool& bExact)
+{
+ int nIndex1, nIndex2;
+ nIndex1 = getZenithBinIndex(position.fPhi);
+ nIndex2 = getAzimuthBinIndex(position.fTheta, position.fPhi);
+
+ list<int>::iterator iter = m_ppTable[nIndex1][nIndex2].begin();
+
+ float fMinDistance = FLT_MAX;
+ int nBestIndex = -1;
+ float fDistance;
+
+ while (iter != m_ppTable[nIndex1][nIndex2].end())
+ {
+ fDistance = MathTools::getDistanceOnArc(position, m_pGraph->getNode(*iter)->getPosition());
+
+ if (fDistance < fMinDistance)
+ {
+ fMinDistance = fDistance;
+ nBestIndex = *iter;
+ }
+
+ iter++;
+ }
+
+ // retrieve distance to bounding
+ float fMinPhi, fMaxPhi, fMinTheta, fMaxTheta;
+
+ float fTestValues[4];
+ // printf("Current position: phi %f, theta %f\n", position.fPhi, position.fTheta);
+
+ getBinMinMaxValues(nIndex1, nIndex2, fMinPhi, fMaxPhi, fMinTheta, fMaxTheta);
+ // printf("B Phi: %f - %f\n", fMinPhi, fMaxPhi);
+ // printf("B Theta: %f - %f\n", fMinTheta, fMaxTheta);
+
+ fTestValues[0] = fabs(fMinPhi - position.fPhi) * M_PI / 180.0f;
+ fTestValues[1] = fabs(fMaxPhi - position.fPhi) * M_PI / 180.0f;
+ fTestValues[2] = fabs(fMinTheta - position.fTheta) * M_PI / 180.0f;
+ fTestValues[3] = fabs(fMaxTheta - position.fTheta) * M_PI / 180.0f;
+ bExact = true;
+
+ // printf("AzimuthDistance (min,max,current): (%f,%f,%f)\n", fTestValues[2], fTestValues[3], fMinDistance);
+ // printf("ZenithDistance (min,max,current): (%f,%f,%f)\n", fTestValues[0], fTestValues[1], fMinDistance);
+
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ if (fTestValues[i] < fMinDistance)
+ {
+ bExact = false;
+ }
+ }
+
+ return nBestIndex;
+}
+
+void CGraphLookupTable::reset()
+{
+ float fZenith;
+
+ for (int z = 0 ; z < getNumberZenithBins() ; z++)
+ {
+ fZenith = 180.0f / m_nMaxZenithBins * z;
+
+ for (int a = 0 ; a < getNumberAzimuthBins(fZenith) ; a++)
+ {
+ m_ppTable[z][a].clear();
+ }
+ }
+}
+
+// *****************************************************************
+// private methods
+// *****************************************************************
+void CGraphLookupTable::addEntry(TSphereCoord position, int nIndex)
+{
+ int nIndex1, nIndex2;
+ nIndex1 = getZenithBinIndex(position.fPhi);
+ nIndex2 = getAzimuthBinIndex(position.fTheta, position.fPhi);
+
+ m_ppTable[nIndex1][nIndex2].push_back(nIndex);
+}
+
+void CGraphLookupTable::buildMemory()
+{
+ float fZenith;
+
+ m_ppTable = new list<int>* [m_nMaxZenithBins];
+
+ for (int z = 0 ; z < m_nMaxZenithBins ; z++)
+ {
+ fZenith = 180.0f / m_nMaxZenithBins * z;
+ m_ppTable[z] = new list<int>[getNumberAzimuthBins(fZenith)];
+ }
+}
+
+void CGraphLookupTable::cleanMemory()
+{
+ for (int z = 0 ; z < m_nMaxZenithBins ; z++)
+ {
+ delete [] m_ppTable[z];
+ }
+
+ delete [] m_ppTable;
+}
+
+int CGraphLookupTable::getNumberZenithBins()
+{
+ return m_nMaxZenithBins;
+}
+
+int CGraphLookupTable::getNumberAzimuthBins(float fZenith)
+{
+ int nZenithBin = getZenithBinIndex(fZenith);
+
+ // 0 degree means 1 min bins
+ // 90 degree means max bins
+ int nMinBins = 1;
+ float fNumberBins;
+
+ if (getNumberZenithBins() > 1)
+ {
+ fNumberBins = sin(float(nZenithBin) / (getNumberZenithBins() - 1) * M_PI) * (m_nMaxAzimuthBins - nMinBins) + nMinBins;
+ }
+ else
+ {
+ fNumberBins = 0.5f;
+ }
+
+ return int(fNumberBins + 0.5f);
+}
+
+int CGraphLookupTable::getAzimuthBinIndex(float fAzimuth, float fZenith)
+{
+ float fBinIndex = fAzimuth / 360.0f * (getNumberAzimuthBins(fZenith) - 1) + 0.5f;
+ return int(fBinIndex);
+}
+
+int CGraphLookupTable::getZenithBinIndex(float fZenith)
+{
+ float fBinIndex = fZenith / 180.0f * (m_nMaxZenithBins - 1) + 0.5f;
+ return int(fBinIndex);
+}
+
+void CGraphLookupTable::getBinMinMaxValues(int nZenithBinIndex, int nAzimuthBinIndex, float& fMinZenith, float& fMaxZenith, float& fMinAzimuth, float& fMaxAzimuth)
+{
+ if ((m_nMaxZenithBins - 1) == 0)
+ {
+ fMinZenith = -FLT_MAX;
+ fMaxZenith = FLT_MAX;
+ }
+ else
+ {
+ fMinZenith = (nZenithBinIndex - 0.5f) * 180.0f / (m_nMaxZenithBins - 1);
+ fMaxZenith = (nZenithBinIndex + 0.5f) * 180.0f / (m_nMaxZenithBins - 1);
+ }
+
+ if ((getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1) == 0)
+ {
+ fMinAzimuth = -FLT_MAX;
+ fMaxAzimuth = FLT_MAX;
+ }
+ else
+ {
+ fMinAzimuth = (nAzimuthBinIndex - 0.5f) * 360.0f / (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
+ fMaxAzimuth = (nAzimuthBinIndex + 0.5f) * 360.0f / (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
+ }
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h
new file mode 100644
index 0000000000000000000000000000000000000000..b97a195b358bbc858b11fd2f3afc0bd8a52fe219
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h
@@ -0,0 +1,65 @@
+// *****************************************************************
+// Filename: GraphLookupTable.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 10.10.2008
+// *****************************************************************
+
+#ifndef _GRAPH_LOOKUP_TABLE_H_
+#define _GRAPH_LOOKUP_TABLE_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+#include <list>
+
+// *****************************************************************
+// namespace
+// *****************************************************************
+using namespace std;
+
+// *****************************************************************
+// decleration of CGraphLookupTable
+// *****************************************************************
+class CGraphLookupTable
+{
+public:
+ // construction / destruction
+ CGraphLookupTable(int nMaxZenithBins, int nMaxAzimuthBins);
+ ~CGraphLookupTable();
+
+ // build table
+ void buildLookupTable(CSphericalGraph* pGraph);
+
+ // operation
+ int getClosestNode(Eigen::Vector3d position);
+ int getClosestNode(TSphereCoord position);
+ int getClosestNode(Eigen::Vector3d position, bool& bExact);
+ int getClosestNode(TSphereCoord position, bool& bExact);
+
+private:
+ void buildMemory();
+ void cleanMemory();
+ void addEntry(TSphereCoord position, int nIndex);
+ void reset();
+
+ int getNumberZenithBins();
+ int getNumberAzimuthBins(float fZenith);
+ int getAzimuthBinIndex(float fAzimuth, float fZenith);
+ int getZenithBinIndex(float fZenith);
+
+ void getBinMinMaxValues(int nZenithBinIndex, int nAzimuthBinIndex, float& fMinZenith, float& fMaxZenith, float& fMinAzimuth, float& fMaxAzimuth);
+
+ int m_nMaxZenithBins;
+ int m_nMaxAzimuthBins;
+
+ list<int>** m_ppTable;
+
+ CSphericalGraph* m_pGraph;
+};
+
+
+#endif /* _GRAPH_LOOKUP_TABLE_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..deaeab33e6a845231419d0856a3c8c507ac3ffa4
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp
@@ -0,0 +1,262 @@
+// *****************************************************************
+// Filename: GraphMap.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 9.08.2007
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "GraphMap.h"
+#include "Base/Math/MathTools.h"
+#include "Base/DataStructures/Graph/GraphProcessor.h"
+#include <list>
+#include "Helpers/helpers.h"
+
+using namespace std;
+
+// *****************************************************************
+// implementation of CGraphMap
+// *****************************************************************
+// construction / destruction
+CGraphMap::CGraphMap(CIntensityGraph* pGraph, bool bUseLookup)
+{
+ m_pGraph = pGraph;
+ m_nNumberNodes = pGraph->getNodes()->size();
+
+ m_nWidth = (int) sqrt(float(m_nNumberNodes));
+ m_nHeight = m_nNumberNodes / m_nWidth + 1;
+
+ m_bUseLookup = bUseLookup;
+
+ // init lookup table
+ if (m_bUseLookup)
+ {
+ m_pLookupTable = new CGraphPyramidLookupTable(100, 100);
+ m_pLookupTable->buildLookupTable(pGraph);
+ }
+
+ // create float matrix
+ m_pMap = new CFloatMatrix(m_nWidth, m_nHeight);
+ m_bOwnMemory = true;
+ toMap();
+}
+
+CGraphMap::~CGraphMap()
+{
+ if (m_bOwnMemory)
+ {
+ delete m_pMap;
+ }
+
+ if (m_bUseLookup)
+ {
+ delete m_pLookupTable;
+ }
+}
+
+void CGraphMap::setMap(CFloatMatrix* pMap, bool bUpdateGraph)
+{
+ if (m_bOwnMemory)
+ {
+ delete m_pMap;
+ }
+
+ m_bOwnMemory = false;
+
+ m_pMap = pMap;
+ m_nWidth = pMap->columns;
+ m_nHeight = pMap->rows;
+
+ if (bUpdateGraph)
+ {
+ toGraph();
+ }
+}
+
+CFloatMatrix* CGraphMap::getMap()
+{
+ return m_pMap;
+}
+
+int CGraphMap::getWidth()
+{
+ return m_nWidth;
+}
+
+int CGraphMap::getHeight()
+{
+ return m_nHeight;
+}
+
+// manipulation
+void CGraphMap::applyGaussian(EOperation nOperation, Vec3d position, float fAmplitude, float fVariance)
+{
+ TSphereCoord coord;
+ MathTools::convert(position, coord);
+
+ applyGaussian(nOperation, coord, fAmplitude, fVariance);
+}
+
+void CGraphMap::applyGaussian(EOperation nOperation, TSphereCoord coord, float fAmplitude, float fVariance)
+{
+ int nNodeID = 0;
+
+ if (m_bUseLookup)
+ {
+ nNodeID = m_pLookupTable->getClosestNode(coord);
+ }
+ else
+ {
+ nNodeID = GraphProcessor::findClosestNode(m_pGraph, coord);
+ }
+
+ applyGaussian(nOperation, nNodeID, fAmplitude, fVariance);
+}
+
+void CGraphMap::applyGaussian(EOperation nOperation, int nNodeID, float fAmplitude, float fVariance)
+{
+ list<int> nodes;
+ vector<int> accomplished;
+
+ // store initial node
+ int nFirstNode = nNodeID;
+ TSphereCoord coord = m_pGraph->getNodes()->at(nFirstNode)->getPosition();
+ nodes.push_back(nFirstNode);
+
+ // eat nodelist
+ TSphereCoord cur_sc;
+ Vec3d ref, cur;
+ MathTools::convert(coord, ref);
+ float fIntensity;
+
+ while (nodes.size() > 0)
+ {
+ // retrieve first node
+ int nCurrentNode = nodes.front();
+ nodes.pop_front();
+ accomplished.push_back(nCurrentNode);
+
+ // calculate angle
+ cur_sc = m_pGraph->getNodes()->at(nCurrentNode)->getPosition();
+ MathTools::convert(cur_sc, cur);
+ float fAngle = Math3d::Angle(ref, cur);
+
+ // check if maximum angle
+ if (fAngle > 2.0 * fVariance)
+ {
+ continue;
+ }
+
+ // evaluate gaussian at angle
+ fIntensity = evaluateGaussian(fAngle, fAmplitude, fVariance);
+
+ // update graph and map
+ switch (nOperation)
+ {
+ case eSet:
+ m_pMap->data[nCurrentNode] = fIntensity;
+ break;
+
+ case eAdd:
+ m_pMap->data[nCurrentNode] += fIntensity;
+ break;
+
+ case eMultiply:
+ m_pMap->data[nCurrentNode] *= fIntensity;
+ break;
+
+ case eMax:
+ if (m_pMap->data[nCurrentNode] < fIntensity)
+ {
+ m_pMap->data[nCurrentNode] = fIntensity;
+ }
+
+ break;
+
+ default:
+ printf("Illegal operation\n");
+ break;
+ }
+
+ // add neighbours to nodelist
+ vector<int>* pAdjacency = m_pGraph->getNodeAdjacency(nCurrentNode);
+
+ // check for double nodes
+ int nAdjNode;
+
+ for (int a = 0 ; a < int(pAdjacency->size()) ; a++)
+ {
+ nAdjNode = pAdjacency->at(a);
+
+ // check if we already processed this node
+ bool bNewNode = true;
+
+ for (int c = 0 ; c < int(accomplished.size()) ; c++)
+ if (accomplished.at(c) == nAdjNode)
+ {
+ bNewNode = false;
+ }
+
+ list<int>::iterator iter = nodes.begin();
+
+ while (iter != nodes.end())
+ {
+ if (*iter == nAdjNode)
+ {
+ bNewNode = false;
+ }
+
+ iter++;
+ }
+
+
+ if (bNewNode)
+ {
+ nodes.push_back(nAdjNode);
+ }
+ }
+ }
+}
+
+void CGraphMap::updateGraph()
+{
+ toGraph();
+}
+
+void CGraphMap::updateMap()
+{
+ toMap();
+}
+
+float CGraphMap::evaluateGaussian(float fDist, float fAmplitude, float fVariance)
+{
+ return exp(- (fDist / fVariance * 2.0f)) * fAmplitude;
+}
+
+void CGraphMap::toGraph()
+{
+ float fIntensity;
+
+ for (int n = 0 ; n < m_nNumberNodes ; n++)
+ {
+ fIntensity = m_pMap->data[n];
+ ((CIntensityNode*) m_pGraph->getNodes()->at(n))->setIntensity(fIntensity);
+ }
+}
+
+void CGraphMap::toMap()
+{
+ float fIntensity;
+
+ for (int n = 0 ; n < m_nNumberNodes ; n++)
+ {
+ fIntensity = ((CIntensityNode*) m_pGraph->getNodes()->at(n))->getIntensity();
+ m_pMap->data[n] = fIntensity;
+ }
+}
+
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8533388a4fad98b6e06ab2c26e6cddf45b6dee3
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h
@@ -0,0 +1,74 @@
+// *****************************************************************
+// Filename: GraphMap.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _GRAPH_MAP_H_
+#define _GRAPH_MAP_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "Base/DataStructures/Graph/IntensityGraph.h"
+#include "Base/DataStructures/Graph/GraphPyramidLookupTable.h"
+#include "Math/FloatMatrix.h"
+
+// *****************************************************************
+// enums
+// *****************************************************************
+enum EOperation
+{
+ eSet,
+ eAdd,
+ eMultiply,
+ eMax
+};
+
+// *****************************************************************
+// decleration of CGraphMap
+// *****************************************************************
+class CGraphMap
+{
+public:
+ // construction / destruction
+ CGraphMap(CIntensityGraph* pGraph, bool bUseLookup = false);
+ ~CGraphMap();
+
+ // getter / setter
+ void setMap(CFloatMatrix* pMap, bool bUpdateGraph = true);
+ CFloatMatrix* getMap();
+ int getWidth();
+ int getHeight();
+
+ // manipulation
+ void applyGaussian(EOperation nOperation, Vec3d position, float fAmplitude, float fVariance);
+ void applyGaussian(EOperation nOperation, TSphereCoord coord, float fAmplitude, float fVariance);
+ void applyGaussian(EOperation nOperation, int nNodeID, float fAmplitude, float fVariance);
+
+ // update
+ void updateGraph();
+ void updateMap();
+
+private:
+ float evaluateGaussian(float fDist, float fAmplitude, float fVariance);
+ void toGraph(); // update intensitygraph
+ void toMap(); // update intensitymap
+
+ CSphericalGraph* m_pGraph;
+ int m_nNumberNodes;
+ CFloatMatrix* m_pMap;
+ bool m_bOwnMemory;
+
+ int m_nWidth;
+ int m_nHeight;
+
+ bool m_bUseLookup;
+ CGraphPyramidLookupTable* m_pLookupTable;
+};
+
+
+#endif /* _GRAPH_MAP_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5faffd65d311340c76ee9ea38da2dcc266b86b3
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp
@@ -0,0 +1,307 @@
+// *****************************************************************
+// Filename: GraphProcessor.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+
+// *****************************************************************
+// includes
+// *****************************************************************
+// system
+#include <math.h>
+#include <float.h>
+
+// local includes
+#include "GraphProcessor.h"
+#include "Base/Math/MathTools.h"
+#include "Base/Tools/DebugMemory.h"
+
+void GraphProcessor::transform(CSphericalGraph* pGraph, TTransform transform)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord position, position_tr;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ position = pNode->getPosition();
+ position_tr = MathTools::transform(position, transform);
+ pNode->setPosition(position_tr);
+ }
+}
+
+void GraphProcessor::inverseTransform(CSphericalGraph* pGraph, TTransform transform)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord position, position_tr;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ position = pNode->getPosition();
+ position_tr = MathTools::inverseTransform(position, transform);
+ pNode->setPosition(position_tr);
+ }
+}
+
+void GraphProcessor::invertPhi(CSphericalGraph* pGraph)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord position;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ position = pNode->getPosition();
+ position.fPhi = 360.0f - position.fPhi;
+ pNode->setPosition(position);
+ }
+}
+
+void GraphProcessor::invertTheta(CSphericalGraph* pGraph)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord position;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ position = pNode->getPosition();
+ position.fTheta = 180.0f - position.fTheta;
+ pNode->setPosition(position);
+ }
+}
+
+void GraphProcessor::copyGraphNodesAndEdges(CSphericalGraph* pSrcGraph, CSphericalGraph* pDstGraph)
+{
+ pDstGraph->clear();
+
+ TNodeList* pNodes = pSrcGraph->getNodes();
+ TEdgeList* pEdges = pSrcGraph->getEdges();
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ CSGNode* pNewNode = pDstGraph->getNewNode();
+ pNewNode->setPosition(pNode->getPosition());
+ pDstGraph->addNode(pNewNode);
+ }
+
+ for (int e = 0 ; e < int(pEdges->size()) ; e++)
+ {
+ CSGEdge* pEdge = pEdges->at(e);
+ pDstGraph->addEdge(pEdge->nIndex1, pEdge->nIndex2, pEdge->nLeftFace, pEdge->nRightFace);
+ }
+}
+
+int GraphProcessor::findClosestNode(CSphericalGraph* pGraph, TSphereCoord coord)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ float fMinDistance = FLT_MAX;
+ float fDistance = 0.0f;
+ int nIndex = -1;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ fDistance = MathTools::getDistanceOnArc(pNode->getPosition(), coord);
+
+ if (fDistance < fMinDistance)
+ {
+ fMinDistance = fDistance;
+ nIndex = n;
+ }
+ }
+
+ return nIndex;
+}
+
+void GraphProcessor::findClosestNeighbours(CSphericalGraph* pGraph, int& s, int& t)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ float fMinDistance = FLT_MAX;
+ int nIndex1 = -1;
+ int nIndex2 = -1;
+ float fDistance;
+
+ for (int i = 0 ; i < int(pNodes->size()); i++)
+ {
+ for (int j = 0 ; j < int(pNodes->size()); j++)
+ {
+ if (i != j)
+ {
+ fDistance = MathTools::getDistanceOnArc(pNodes->at(i)->getPosition(), pNodes->at(j)->getPosition());
+
+ if (fDistance < fMinDistance)
+ {
+ fMinDistance = fDistance;
+ nIndex1 = i;
+ nIndex2 = j;
+ }
+ }
+ }
+ }
+
+ s = nIndex1;
+ t = nIndex2;
+}
+
+int GraphProcessor::findEdge(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
+{
+ TEdgeList* pEdges = pGraph->getEdges();
+
+ for (int e = 0 ; e < int(pEdges->size()) ; e++)
+ {
+ if ((pEdges->at(e)->nIndex1 == nIndex1) && (pEdges->at(e)->nIndex2 == nIndex2))
+ {
+ return e;
+ }
+
+ if ((pEdges->at(e)->nIndex1 == nIndex2) && (pEdges->at(e)->nIndex2 == nIndex1))
+ {
+ return e;
+ }
+ }
+
+ // not found
+ return -1;
+}
+
+bool GraphProcessor::insideCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, int nPointIndex, bool& bInside)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord center;
+ float fRadius;
+
+ if (!getCircumcircle(pGraph, nIndex1, nIndex2, nIndex3, center, fRadius))
+ {
+ return false;
+ }
+
+ TSphereCoord point = pNodes->at(nPointIndex)->getPosition();
+
+ // check with give coordinates
+ float fDistance = sqrt((center.fPhi - point.fPhi) * (center.fPhi - point.fPhi) + (center.fTheta - point.fTheta) * (center.fTheta - point.fTheta));
+
+ bInside = (fDistance < (fRadius + 0.0001));
+
+ return true;
+}
+
+bool GraphProcessor::getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, TSphereCoord& center, float& fRadius)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+ float cp;
+
+ TSphereCoord s1, s2, s3;
+ s1 = pNodes->at(nIndex1)->getPosition();
+ s2 = pNodes->at(nIndex2)->getPosition();
+ s3 = pNodes->at(nIndex3)->getPosition();
+
+ cp = MathTools::CalculateCrossProductFromDifference(s1, s2, s3);
+
+ if (cp != 0.0)
+ {
+ float p1Sq, p2Sq, p3Sq;
+ float num;
+ float cx, cy;
+
+ p1Sq = s1.fPhi * s1.fPhi + s1.fTheta * s1.fTheta;
+ p2Sq = s2.fPhi * s2.fPhi + s2.fTheta * s2.fTheta;
+ p3Sq = s3.fPhi * s3.fPhi + s3.fTheta * s3.fTheta;
+
+ num = p1Sq * (s2.fTheta - s3.fTheta) + p2Sq * (s3.fTheta - s1.fTheta) + p3Sq * (s1.fTheta - s2.fTheta);
+ cx = num / (2.0f * cp);
+ num = p1Sq * (s3.fPhi - s2.fPhi) + p2Sq * (s1.fPhi - s3.fPhi) + p3Sq * (s2.fPhi - s1.fPhi);
+ cy = num / (2.0f * cp);
+
+ center.fPhi = cx;
+ center.fTheta = cy;
+
+ fRadius = sqrt((center.fPhi - s1.fPhi) * (center.fPhi - s1.fPhi) + (center.fTheta - s1.fTheta) * (center.fTheta - s1.fTheta)) ;
+ return true;
+ }
+ else
+ {
+ // points lie on a line
+ return false;
+ }
+}
+
+int GraphProcessor::getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+ float fMinRadius = FLT_MAX;
+ float fRadius;
+ int nIndex = -1;
+
+ // go through all points
+ for (int v = 0 ; v < int(pNodes->size()) ; v++)
+ {
+ if ((v == pEdge->nIndex1) || (v == pEdge->nIndex2))
+ {
+ continue;
+ }
+
+ // check if v is already connected to index1, index2
+ if (GraphProcessor::isEdgePresent(pGraph, pEdge->nIndex1, v))
+ {
+ continue;
+ }
+
+ if (GraphProcessor::isEdgePresent(pGraph, pEdge->nIndex2, v))
+ {
+ continue;
+ }
+
+ // check if v is an unconnected node
+ if (pGraph->getNodeAdjacency(v)->size() != 0)
+ {
+ continue;
+ }
+
+ TSphereCoord center;
+
+ getCircumcircle(pGraph, pEdge->nIndex1, pEdge->nIndex2, v, center, fRadius);
+
+ if (fRadius < fMinRadius)
+ {
+ fMinRadius = fRadius;
+ nIndex = v;
+ }
+ }
+
+ return nIndex;
+}
+
+bool GraphProcessor::isEdgePresent(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
+{
+
+ bool bPresent = false;
+
+ vector<int>::iterator iter = pGraph->getNodeAdjacency(nIndex1)->begin();
+
+ while (iter != pGraph->getNodeAdjacency(nIndex1)->end())
+ {
+ if ((*iter) == nIndex2)
+ {
+ bPresent = true;
+ }
+
+ iter++;
+ }
+
+ return bPresent;
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..2a609783b07cef5c6b4f4c77794adde38c0cc017
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h
@@ -0,0 +1,50 @@
+// *****************************************************************
+// Filename: GraphProcessor.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _GRAPH_PROCESSOR_H_
+#define _GRAPH_PROCESSOR_H_
+
+// *****************************************************************
+// forward declarations
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+
+// *****************************************************************
+// namespace GraphProcessor
+// *****************************************************************
+namespace GraphProcessor
+{
+ // graph operations
+ void copyGraphNodesAndEdges(CSphericalGraph* pSrcGraph, CSphericalGraph* pDstGraph);
+
+ // graph coordinate transformations
+ void transform(CSphericalGraph* pGraph, TTransform transform);
+ void inverseTransform(CSphericalGraph* pGraph, TTransform transform);
+ void invertPhi(CSphericalGraph* pGraph);
+ void invertTheta(CSphericalGraph* pGraph);
+
+ // edge operations
+ int findEdge(CSphericalGraph* pGraph, int nIndex1, int nIndex2);
+ bool isEdgePresent(CSphericalGraph* pGraph, int nIndex1, int nIndex2);
+
+ // neighbour and node operations
+ int findClosestNode(CSphericalGraph* pGraph, TSphereCoord coord);
+ void findClosestNeighbours(CSphericalGraph* pGraph, int& s, int& t);
+ int getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge);
+
+ // circumcircle operations
+ bool insideCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, int nPointIndex, bool& bInside);
+ bool getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, TSphereCoord& center, float& fRadius);
+};
+
+#endif /* _GRAPH_PROCESSOR_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..babc001fbcd7286208e2dd033b7a6126a4e61709
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp
@@ -0,0 +1,108 @@
+// *****************************************************************
+// Filename: GraphLookupTable.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 10.10.2008
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include <cstdlib>
+#include "GraphPyramidLookupTable.h"
+#include "MathTools.h"
+
+// *****************************************************************
+// implementation of CGraphLookupTable
+// *****************************************************************
+// construction / destruction
+CGraphPyramidLookupTable::CGraphPyramidLookupTable(int nMaxZenithBins, int nMaxAzimuthBins)
+{
+ m_nMaxZenithBins = nMaxZenithBins;
+ m_nMaxAzimuthBins = nMaxAzimuthBins;
+
+ m_nSubDivision = 2;
+ m_nLevels = 0;
+
+ CGraphLookupTable* pTable;
+
+ while (nMaxZenithBins != 0)
+ {
+ pTable = new CGraphLookupTable(nMaxZenithBins, nMaxAzimuthBins);
+
+ m_Tables.push_back(pTable);
+
+ // printf("Level %d: zenithbins: %d, azimuithbins: %d\n", m_nLevels, nMaxZenithBins, nMaxAzimuthBins);
+
+ m_nLevels++;
+ nMaxZenithBins /= m_nSubDivision;
+ nMaxAzimuthBins /= m_nSubDivision;
+ }
+
+}
+
+CGraphPyramidLookupTable::~CGraphPyramidLookupTable()
+{
+ list<CGraphLookupTable*>::iterator iter = m_Tables.begin();
+
+ while (iter != m_Tables.end())
+ {
+ delete *iter;
+
+ iter++;
+ }
+}
+
+void CGraphPyramidLookupTable::buildLookupTable(CSphericalGraph* pGraph)
+{
+ list<CGraphLookupTable*>::iterator iter = m_Tables.begin();
+
+ while (iter != m_Tables.end())
+ {
+ (*iter)->buildLookupTable(pGraph);
+
+ iter++;
+ }
+}
+
+int CGraphPyramidLookupTable::getClosestNode(Eigen::Vector3d position)
+{
+ TSphereCoord coords;
+ MathTools::convert(position, coords);
+
+ return getClosestNode(coords);
+}
+
+int CGraphPyramidLookupTable::getClosestNode(TSphereCoord position)
+{
+ bool bFinished = false;
+ list<CGraphLookupTable*>::iterator iter = m_Tables.begin();
+ int nIndex = -1;
+
+ int L = 0;
+
+ while (iter != m_Tables.end())
+ {
+ nIndex = (*iter)->getClosestNode(position, bFinished);
+ // printf("Result in level %d: index %d, finished: %d\n",L,nIndex,bFinished);
+
+ if (bFinished)
+ {
+ break;
+ }
+
+ L++;
+ iter++;
+ }
+
+ if (!bFinished)
+ {
+ printf("CGraphPyramidLookupTable:: no closest node found\n");
+ exit(1);
+ }
+
+ return nIndex;
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h
new file mode 100644
index 0000000000000000000000000000000000000000..b871f1d0ae2ec49ec8c078e227b6934c812c593c
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h
@@ -0,0 +1,52 @@
+// *****************************************************************
+// Filename: GraphPyramidLookupTable.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 10.10.2008
+// *****************************************************************
+
+#ifndef _GRAPH_PYRAMID_LOOKUP_TABLE_H_
+#define _GRAPH_PYRAMID_LOOKUP_TABLE_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+#include "GraphLookupTable.h"
+#include <list>
+
+// *****************************************************************
+// namespace
+// *****************************************************************
+using namespace std;
+
+// *****************************************************************
+// decleration of CGraphPyramidLookupTable
+// *****************************************************************
+class CGraphPyramidLookupTable
+{
+public:
+ // construction / destruction
+ CGraphPyramidLookupTable(int nMaxZenithBins, int nMaxAzimuthBins);
+ ~CGraphPyramidLookupTable();
+
+ // build table
+ void buildLookupTable(CSphericalGraph* pGraph);
+
+ // operation
+ int getClosestNode(Eigen::Vector3d position);
+ int getClosestNode(TSphereCoord position);
+
+private:
+ list<CGraphLookupTable*> m_Tables;
+ int m_nSubDivision;
+ int m_nLevels;
+
+ int m_nMaxZenithBins;
+ int m_nMaxAzimuthBins;
+};
+
+
+#endif /* _GRAPH_PYRAMID_LOOKUP_TABLE_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..495ebc8435ee60db4f4ed97681cceb30679d489d
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp
@@ -0,0 +1,423 @@
+// *****************************************************************
+// Filename: GraphProcessor.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+
+// *****************************************************************
+// includes
+// *****************************************************************
+// local includes
+#include "GraphTriangulation.h"
+#include "GraphProcessor.h"
+#include "Base/Math/MathTools.h"
+#include "Base/Tools/DebugMemory.h"
+
+void GraphTriangulation::delaunayTriangulationQuadratic(CSphericalGraph* pGraph)
+{
+ TC3DNodeList* pC3DNodeList = new TC3DNodeList();
+ int nNodes = (int) pGraph->getNodes()->size();
+
+ for (int i = 0; i < nNodes; i++)
+ {
+ // project point to polar coordinates
+ Vec3d cartesian_point;
+ MathTools::convert(pGraph->getNode(i)->getPosition(), cartesian_point);
+
+ CC3DNode* pC3DNode = new CC3DNode(cartesian_point);
+ pC3DNodeList->push_back(pC3DNode);
+ }
+
+ // generate the delaunay triangulation by computing the
+ // convex hull of the sphere
+ CCHGiftWrap giftWrapping = CCHGiftWrap(pC3DNodeList);
+ CSphericalGraph* pCopy = new CSphericalGraph();
+ giftWrapping.generateConvexHull(pCopy);
+
+ // add edges from triangulated graph to pGraph
+ TEdgeList* pEdges = pCopy->getEdges();
+
+ for (int e = 0 ; e < pEdges->size() ; e++)
+ {
+ pGraph->addEdge(pEdges->at(e)->nIndex1, pEdges->at(e)->nIndex2);
+ }
+
+ delete pCopy;
+}
+
+
+void GraphTriangulation::triangulationQuadraticSpherical(CSphericalGraph* pGraph)
+{
+ int nNumberViews = (int) pGraph->getNodes()->size();
+
+ // create graph with nodes and nodes + 360
+ CSphericalGraph* pDoubleGraph = doubleNodes(pGraph);
+
+ // do triangulation on the double size graph
+ //triangulationQuadratic(pDoubleGraph);
+ triangulationQuartic(pDoubleGraph);
+
+ // insert detrmined edges in original graph
+ int nIndex1, nIndex2;
+ CSGEdge* pCurrentEdge;
+
+ for (int e = 0 ; e < int(pDoubleGraph->getEdges()->size()) ; e++)
+ {
+ pCurrentEdge = pDoubleGraph->getEdges()->at(e);
+ nIndex1 = pCurrentEdge->nIndex1;
+ nIndex2 = pCurrentEdge->nIndex2;
+
+ // HACK: delete outer edges
+ if ((pDoubleGraph->getNode(nIndex1)->getPosition().fTheta < 30.0) || (pDoubleGraph->getNode(nIndex1)->getPosition().fTheta > 690.0))
+ {
+ continue;
+ }
+
+ if ((pDoubleGraph->getNode(nIndex2)->getPosition().fTheta < 30.0) || (pDoubleGraph->getNode(nIndex2)->getPosition().fTheta > 690.0))
+ {
+ continue;
+ }
+
+ /* if( (pDoubleGraph->getNode(nIndex1)->getPosition().fPhi < 30.0) || (pDoubleGraph->getNode(nIndex1)->getPosition().fPhi > 330.0))
+ continue;
+ if( (pDoubleGraph->getNode(nIndex2)->getPosition().fPhi < 30.0) || (pDoubleGraph->getNode(nIndex2)->getPosition().fPhi > 330.0))
+ continue;
+ if( (pDoubleGraph->getNode(nIndex1)->getPosition().fPhi < 180.0) && (pDoubleGraph->getNode(nIndex2)->getPosition().fPhi > 180.0))
+ continue;
+ if( (pDoubleGraph->getNode(nIndex2)->getPosition().fPhi < 180.0) && (pDoubleGraph->getNode(nIndex1)->getPosition().fPhi > 180.0))
+ continue;
+ */
+ // eliminate virtual nodes
+ if (nIndex1 >= nNumberViews)
+ {
+ nIndex1 %= nNumberViews;
+ }
+
+ if (nIndex2 >= nNumberViews)
+ {
+ nIndex2 %= nNumberViews;
+ }
+
+ // update graph
+ if (nIndex1 != nIndex2)
+ if (!GraphProcessor::isEdgePresent(pGraph, nIndex1, nIndex2))
+ {
+ pGraph->addEdge(nIndex1, nIndex2);
+ }
+ }
+
+ delete pDoubleGraph;
+}
+
+void GraphTriangulation::triangulationQuartic(CSphericalGraph* pGraph, float fThreshold)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ int nNumberPoints = int(pNodes->size());
+ bool bPointFree;
+ int i, j, k, l;
+
+ for (i = 0 ; i < nNumberPoints - 2 ; i++)
+ {
+ printf("Round %d / %d\n", i, nNumberPoints - 2);
+
+ for (j = i + 1 ; j < nNumberPoints - 1 ; j++)
+ {
+ if (j != i)
+ {
+ for (k = j + 1; k < nNumberPoints; k++)
+ {
+ if (k != i && k != j)
+ {
+
+ // check if this rectangle is point free
+ bPointFree = true;
+
+ for (l = 0; l < nNumberPoints ; l++)
+ {
+ // only if not one of the triangle points
+ if (l != i && l != j && l != k)
+ {
+ bool bInside;
+
+ if (!GraphProcessor::insideCircumcircle(pGraph, i, j, k, l, bInside))
+ {
+ bPointFree = false;
+ break;
+ }
+
+ if (bInside)
+ {
+ bPointFree = false;
+ break;
+ }
+ }
+ }
+
+ if (bPointFree)
+ {
+ TSphereCoord p_i, p_j, p_k;
+ p_i = pNodes->at(i)->getPosition();
+ p_j = pNodes->at(j)->getPosition();
+ p_k = pNodes->at(k)->getPosition();
+
+ // add triangle
+ if (MathTools::getDistanceOnArc(p_i, p_j) < fThreshold)
+ {
+ pGraph->addEdge(i, j);
+ }
+
+ if (MathTools::getDistanceOnArc(p_i, p_k) < fThreshold)
+ {
+ pGraph->addEdge(i, k);
+ }
+
+ if (MathTools::getDistanceOnArc(p_k, p_j) < fThreshold)
+ {
+ pGraph->addEdge(k, j);
+ }
+ }
+
+ }
+ }
+ }
+ }
+ }
+}
+
+
+void GraphTriangulation::triangulationQuadratic(CSphericalGraph* pGraph)
+{
+ TEdgeList* pEdges = pGraph->getEdges();
+
+ int nCurrentEdge;
+ int nFaces = 0;
+ int s = 0, t = 0;
+
+ // find two closest neighbours and add edge to triangulation.
+ GraphProcessor::findClosestNeighbours(pGraph, s, t);
+
+ // Create seed edge
+ int nEdgeIndex = pGraph->addEdge(s , t , -1, -1);
+
+ nCurrentEdge = 0;
+
+ while (nCurrentEdge < int(pEdges->size()))
+ {
+ if (pEdges->at(nCurrentEdge)->nLeftFace == -1)
+ {
+ completeFacet(pGraph, nCurrentEdge, nFaces);
+ }
+
+ if (pEdges->at(nCurrentEdge)->nRightFace == -1)
+ {
+ completeFacet(pGraph, nCurrentEdge, nFaces);
+ }
+
+ nCurrentEdge++;
+ }
+}
+
+void GraphTriangulation::completeFacet(CSphericalGraph* pGraph, int nEdgeIndex, int nFaces)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+ TEdgeList* pEdges = pGraph->getEdges();
+ int s, t, u;
+
+ // Cache s and t.
+ if (pEdges->at(nEdgeIndex)->nLeftFace == -1)
+ {
+ s = pEdges->at(nEdgeIndex)->nIndex1;
+ t = pEdges->at(nEdgeIndex)->nIndex2;
+ }
+ else if (pEdges->at(nEdgeIndex)->nRightFace == -1)
+ {
+ s = pEdges->at(nEdgeIndex)->nIndex2;
+ t = pEdges->at(nEdgeIndex)->nIndex1;
+ }
+ else
+ {
+ // Edge already completed.
+ return;
+ }
+
+
+ // Find a point on left of edge.
+ for (u = 0; u < int(pNodes->size()); u++)
+ {
+ if (u == s || u == t)
+ {
+ continue;
+ }
+
+ // check side
+ float fAngle = MathTools::CalculateCrossProductFromDifference(pNodes->at(s)->getPosition(), pNodes->at(t)->getPosition(), pNodes->at(u)->getPosition());
+
+ if (fAngle > 0.0)
+ {
+ break;
+ }
+ }
+
+ // Find best point on left of edge.
+ int nBestPoint = u;
+ float fCp;
+
+ if (nBestPoint < int(pNodes->size()))
+ {
+ TSphereCoord center;
+ float fRadius;
+ GraphProcessor::getCircumcircle(pGraph, s, t, nBestPoint, center, fRadius);
+
+ for (u = nBestPoint + 1; u < int(pNodes->size()); u++)
+ {
+ if (u == s || u == t)
+ {
+ continue;
+ }
+
+ fCp = MathTools::CalculateCrossProductFromDifference(pNodes->at(s)->getPosition(), pNodes->at(t)->getPosition(), pNodes->at(u)->getPosition());
+
+ if (fCp > 0.0)
+ {
+ bool bInside;
+
+ if (GraphProcessor::insideCircumcircle(pGraph, s, t, nBestPoint, u, bInside))
+ {
+ if (bInside)
+ {
+ nBestPoint = u;
+ }
+ }
+ }
+ }
+ }
+
+ // Add new triangle or update edge info if s-t is on hull.
+ if (nBestPoint < int(pNodes->size()))
+ {
+ // Update face information of edge being completed.
+ updateLeftFace(pGraph, nEdgeIndex, s, t, nFaces);
+ nFaces++;
+
+ // Add new edge or update face info of old edge.
+ nEdgeIndex = GraphProcessor::findEdge(pGraph, nBestPoint, s);
+
+ if (nEdgeIndex == -1)
+ {
+ // New edge.
+ nEdgeIndex = pGraph->addEdge(nBestPoint, s, nFaces, -1);
+ }
+ else
+ {
+ // Old edge.
+ updateLeftFace(pGraph, nEdgeIndex, nBestPoint, s, nFaces);
+ }
+
+ // Add new edge or update face info of old edge.
+ nEdgeIndex = GraphProcessor::findEdge(pGraph, t, nBestPoint);
+
+ if (nEdgeIndex == -1)
+ {
+ // New edge.
+ nEdgeIndex = pGraph->addEdge(t, nBestPoint, nFaces, -1);
+ }
+ else
+ {
+ // Old edge.
+ updateLeftFace(pGraph, nEdgeIndex, t, nBestPoint, nFaces);
+ }
+ }
+ else
+ {
+ updateLeftFace(pGraph, nEdgeIndex, s, t, -2);
+ }
+}
+
+void GraphTriangulation::updateLeftFace(CSphericalGraph* pGraph, int nEdgeIndex, int nIndex1, int nIndex2, int nFace)
+{
+ // vector<TView*>* pViews = &pGraph->getAspectPool()->m_Views;
+ vector<CSGEdge*>* pEdges = pGraph->getEdges();
+
+ bool bValid = false;
+
+ if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex1) && (pEdges->at(nEdgeIndex)->nIndex2 == nIndex2))
+ {
+ bValid = true;
+ }
+
+ if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex2) && (pEdges->at(nEdgeIndex)->nIndex2 == nIndex1))
+ {
+ bValid = true;
+ }
+
+ if (!bValid)
+ {
+ printf("updateLeftFace: adj. matrix and edge table mismatch\n");
+ }
+
+ if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex1) && (pEdges->at(nEdgeIndex)->nLeftFace == -1))
+ {
+ pEdges->at(nEdgeIndex)->nLeftFace = nFace;
+ }
+ else if ((pEdges->at(nEdgeIndex)->nIndex2 == nIndex1) && (pEdges->at(nEdgeIndex)->nRightFace == -1))
+ {
+ pEdges->at(nEdgeIndex)->nRightFace = nFace;
+ }
+
+}
+
+CSphericalGraph* GraphTriangulation::doubleNodes(CSphericalGraph* pSourceGraph)
+{
+ // list to source views
+ TNodeList* pNodes = pSourceGraph->getNodes();
+ int nNumberSourceNodes = (int) pNodes->size();
+
+ // add views to aspectpool
+ vector<TSphereCoord> coordinates;
+
+
+ for (int i = 0 ; i < nNumberSourceNodes ; i++)
+ {
+ coordinates.push_back(pNodes->at(i)->getPosition());
+ }
+
+ TSphereCoord shiftedCoord;
+
+ for (int i = 0 ; i < nNumberSourceNodes ; i++)
+ {
+ shiftedCoord = pNodes->at(i)->getPosition();
+ shiftedCoord.fTheta += 360.0f;
+ coordinates.push_back(shiftedCoord);
+ }
+
+ /* TSphereCoord shifted2Coord, shifted3Coord;
+ for(int i = 0 ; i < nNumberSourceNodes ; i++)
+ {
+ shifted2Coord = pNodes->at(i)->getPosition();
+ shifted2Coord.fPhi += 180.0f;
+ coordinates.push_back(shifted2Coord);
+ }
+
+ for(int i = 0 ; i < nNumberSourceNodes ; i++)
+ {
+ shifted3Coord = pNodes->at(i)->getPosition();
+ shifted3Coord.fPhi += 180.0f;
+ shifted3Coord.fTheta += 360.0f;
+ coordinates.push_back(shifted3Coord);
+ }
+ */
+ CSphericalGraph* pTmpGraph = new CSphericalGraph();
+
+ for (int i = 0 ; i < 2 * nNumberSourceNodes ; i++)
+ {
+ CSGNode* pNode = new CSGNode(coordinates.at(i));
+ pTmpGraph->addNode(pNode);
+ }
+
+ return pTmpGraph;
+}
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h
new file mode 100644
index 0000000000000000000000000000000000000000..62e3dadc5ea464bd731f3c4ac1e364182d5bc171
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h
@@ -0,0 +1,52 @@
+// *****************************************************************
+// Filename: AspectGraphProcessor.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _GRAPH_TRIANGULATION_H_
+#define _GRAPH_TRIANGULATION_H_
+
+// *****************************************************************
+// forward declarations
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+#include "Base/DataStructures/Graph/Convexhull/C3DNode.h"
+#include "Base/DataStructures/Graph/Convexhull/HalfSpace.h"
+#include "Base/DataStructures/Graph/Convexhull/CHGiftWrap.h"
+#include <float.h>
+
+// *****************************************************************
+// namespaces
+// *****************************************************************
+
+// *****************************************************************
+// namespace GraphTriangulation
+// *****************************************************************
+class GraphTriangulation
+{
+public:
+ static void delaunayTriangulationQuadratic(CSphericalGraph* pGraph);
+ // triangulate with O(n^2) spherical
+ static void triangulationQuadraticSpherical(CSphericalGraph* pGraph);
+ // triangulate with O(n^4)
+ static void triangulationQuartic(CSphericalGraph* pGraph, float fThreshold = FLT_MAX);
+ // triangulate with O(n^2)
+ static void triangulationQuadratic(CSphericalGraph* pGraph);
+
+private:
+ static void updateLeftFace(CSphericalGraph* pGraph, int nEdgeIndex, int nIndex1, int nIndex2, int nFace);
+ static void completeFacet(CSphericalGraph* pGraph, int nEdgeIndex, int nFaces);
+
+ static CSphericalGraph* doubleNodes(CSphericalGraph* pSourceGraph);
+
+};
+
+#endif /* _GRAPH_TRIANGULATION_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d48757d107bc3f69b5aa174e772c72d8b96cb86
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp
@@ -0,0 +1,276 @@
+// *****************************************************************
+// Filename: SensoryEgoSphere.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 21.07.2008
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "IntensityGraph.h"
+//#include "Base/Math/DistributePoints.h"
+//#include "MathTools.h"
+//#include "Base/DataStructures/Graph/GraphProcessor.h"
+
+#include <float.h>
+#include <fstream>
+
+// *****************************************************************
+// implementation of CIntensityNode
+// *****************************************************************
+CIntensityNode::CIntensityNode()
+{
+ setIntensity(0.0f);
+}
+
+CIntensityGraph::CIntensityGraph(const CIntensityGraph& prototype)
+ : CSphericalGraph(prototype)
+{
+}
+
+CIntensityNode::CIntensityNode(TSphereCoord coord)
+ : CSGNode(coord)
+{
+ setIntensity(0.0f);
+}
+
+CIntensityNode::~CIntensityNode()
+{
+}
+
+void CIntensityNode::setIntensity(float fIntensity)
+{
+ m_fIntensity = fIntensity;
+}
+
+float CIntensityNode::getIntensity()
+{
+ return m_fIntensity;
+}
+
+// *****************************************************************
+// implementation of CIntensityGraph
+// *****************************************************************
+// *****************************************************************
+// construction / destruction
+// *****************************************************************
+CIntensityGraph::CIntensityGraph()
+{
+ reset();
+}
+
+//CIntensityGraph::CIntensityGraph(int nNumberNodes)
+//{
+// // generate points on sphere
+// points P = generatePoints(nNumberNodes,1000);
+
+// for(int i = 0 ; i < int(P.V.size()) ; i++)
+// {
+// // project point to polar coordinates
+// Vec3d point;
+// Math3d::SetVec(point,P.V.at(i).array()[0],P.V.at(i).array()[1],P.V.at(i).array()[2]);
+// TSphereCoord point_on_sphere;
+// MathTools::convert(point, point_on_sphere);
+
+// CIntensityNode* pNode = new CIntensityNode(point_on_sphere);
+
+// addNode(pNode);
+// }
+
+// // initialize
+// reset();
+//}
+
+CIntensityGraph::CIntensityGraph(string sFilename)
+{
+ printf("Reading intensity graph from: %s\n", sFilename.c_str());
+
+ ifstream infile;
+ infile.open(sFilename.c_str());
+
+ read(infile);
+
+ //printf("Read number nodes: %d\n", getNodes()->size());
+ //printf("Read number edges: %d\n", getEdges()->size());
+}
+
+CIntensityGraph::~CIntensityGraph()
+{
+}
+
+CIntensityGraph& CIntensityGraph::operator= (CIntensityGraph const& rhs)
+{
+ *((CSphericalGraph*) this) = (const CSphericalGraph) rhs;
+ return *this;
+}
+
+// *****************************************************************
+// setting
+// *****************************************************************
+void CIntensityGraph::reset()
+{
+ set(0.0f);
+}
+
+void CIntensityGraph::set(float fIntensity)
+{
+ TNodeList* pNodes = getNodes();
+
+ for (size_t i = 0 ; i < pNodes->size() ; i++)
+ {
+ ((CIntensityNode*) pNodes->at(i))->setIntensity(fIntensity);
+ }
+}
+
+
+// *****************************************************************
+// file io
+// *****************************************************************
+bool CIntensityGraph::read(istream& infile)
+{
+ try
+ {
+ int nNumberNodes;
+
+ // read number of nodes
+ infile >> nNumberNodes;
+
+ // read all nodes
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ readNode(infile);
+ }
+
+ // read number of edges
+ int nNumberEdges;
+ infile >> nNumberEdges;
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ readEdge(infile);
+ }
+
+ }
+ catch (istream::failure e)
+ {
+ printf("ERROR: failed to write FeatureGraph to file\n");
+ return false;
+ }
+
+ return true;
+}
+
+bool CIntensityGraph::readNode(istream& infile)
+{
+ CIntensityNode* pNewNode = getNewNode();
+
+ int nIndex;
+ TSphereCoord pos;
+ float fIntensity;
+
+ infile >> nIndex;
+ infile >> pos.fPhi;
+ infile >> pos.fTheta;
+ infile >> fIntensity;
+
+ pNewNode->setPosition(pos);
+ pNewNode->setIntensity(fIntensity);
+
+ int nTest = addNode(pNewNode);
+
+ if (nTest != nIndex)
+ {
+ printf("Error input file inconsistent: %d %d\n", nTest, nIndex);
+ return false;
+ }
+
+ return true;
+}
+
+bool CIntensityGraph::readEdge(istream& infile)
+{
+ int nIndex1, nIndex2, nLeftFace, nRightFace;
+
+ infile >> nIndex1;
+ infile >> nIndex2;
+
+ infile >> nLeftFace;
+ infile >> nRightFace;
+
+ addEdge(nIndex1, nIndex2, nLeftFace, nRightFace);
+ return true;
+}
+
+
+bool CIntensityGraph::write(ostream& outfile)
+{
+ try
+ {
+ int nNumberNodes = int(m_Nodes.size());
+ // write number of nodes
+ outfile << nNumberNodes << endl;
+
+ // write all nodes
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ writeNode(outfile, n);
+ }
+
+ // write number of edges
+ int nNumberEdges = int(m_Edges.size());
+ outfile << nNumberEdges << endl;
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ writeEdge(outfile, e);
+ }
+
+ }
+ catch (ostream::failure e)
+ {
+ printf("ERROR: failed to write FeatureGraph to file\n");
+ return false;
+ }
+
+ return true;
+}
+
+bool CIntensityGraph::writeNode(ostream& outfile, int n)
+{
+ CIntensityNode* pCurrentNode = (CIntensityNode*) m_Nodes.at(n);
+
+ outfile << pCurrentNode->getIndex() << endl;
+ outfile << pCurrentNode->getPosition().fPhi << endl;
+ outfile << pCurrentNode->getPosition().fTheta << endl;
+ outfile << pCurrentNode->getIntensity() << endl;
+
+ outfile << endl;
+ return true;
+}
+
+bool CIntensityGraph::writeEdge(ostream& outfile, int e)
+{
+ CSGEdge* pCurrentEdge = m_Edges.at(e);
+ outfile << pCurrentEdge->nIndex1 << " ";
+ outfile << pCurrentEdge->nIndex2 << " ";
+
+ outfile << pCurrentEdge->nLeftFace << " ";
+ outfile << pCurrentEdge->nRightFace << " ";
+
+ outfile << endl;
+ return true;
+}
+
+void CIntensityGraph::graphToVec(std::vector<float>& vec)
+{
+ TNodeList* nodes = this->getNodes();
+
+ for (size_t i = 0; i < nodes->size(); i++)
+ {
+ CIntensityNode* node = (CIntensityNode*) nodes->at(i);
+ vec.push_back(node->getIntensity());
+ }
+}
diff --git a/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h
new file mode 100644
index 0000000000000000000000000000000000000000..4436704a40aedd848925bf6d43098a1cc2a669d3
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h
@@ -0,0 +1,97 @@
+// *****************************************************************
+// Filename: IntensityGraph.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2008
+// *****************************************************************
+
+#ifndef _INTENSITY_GRAPH_H_
+#define _INTENSITY_GRAPH_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+#include <string>
+
+using namespace std;
+
+// *****************************************************************
+// namespaces
+// *****************************************************************
+
+// *****************************************************************
+// structures
+// *****************************************************************
+// *****************************************************************
+// definition of nodes
+// *****************************************************************
+class CIntensityNode : public CSGNode
+{
+public:
+ CIntensityNode();
+ CIntensityNode(TSphereCoord coord);
+ ~CIntensityNode();
+
+ void setIntensity(float fIntensity);
+ float getIntensity();
+
+ virtual CSGNode* clone()
+ {
+ CIntensityNode* copy = new CIntensityNode(m_Position);
+ copy->setIndex(m_nIndex);
+ copy->setIntensity(m_fIntensity);
+ return copy;
+ }
+
+private:
+ float m_fIntensity;
+};
+
+
+// *****************************************************************
+// definition of sensory egosphere
+// *****************************************************************
+class CIntensityGraph : public CSphericalGraph
+{
+public:
+ CIntensityGraph();
+ CIntensityGraph(const CIntensityGraph& prototype);
+ //CIntensityGraph(int nNumberNodes);
+ CIntensityGraph(string sFileName);
+ ~CIntensityGraph();
+
+ void reset();
+ void set(float fIntensity);
+
+
+ void graphToVec(std::vector<float>& vec);
+
+ ///////////////////////////////
+ // file io
+ ///////////////////////////////
+ virtual bool read(istream& infile);
+ virtual bool readNode(istream& infile);
+ virtual bool readEdge(istream& infile);
+
+ virtual bool write(ostream& outfile);
+ virtual bool writeNode(ostream& outfile, int n);
+ virtual bool writeEdge(ostream& outfile, int e);
+
+
+ // inherited
+ virtual CIntensityNode* getNewNode()
+ {
+ return new CIntensityNode();
+ }
+
+ ///////////////////////////////
+ // operators
+ ///////////////////////////////
+ CIntensityGraph& operator= (CIntensityGraph const& rhs);
+
+};
+
+#endif // _INTENSITY_GRAPH_H_
diff --git a/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp b/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f430214639804b394067e34c7a8b6d209a4972da
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp
@@ -0,0 +1,488 @@
+// *****************************************************************
+// Filename: AspectGraphProcessor.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "MathTools.h"
+#include <math.h>
+//#include "Base/Tools/DebugMemory.h"
+
+
+// **********************************************************
+// transformation on sphere coordinates and paths
+// **********************************************************
+// returns transformation from p1 -> p2
+TTransform MathTools::determinePathTransformation(TPath* p1, TPath* p2)
+{
+ TSphereCoord start1, start2, end1, end2;
+ start1 = p1->at(0).Position;
+ end1 = p1->at(p1->size() - 1).Position;
+ start2 = p2->at(0).Position;
+ end2 = p2->at(p2->size() - 1).Position;
+
+ return determineTransformation(start1, start2, end1, end2);
+}
+
+TPath* MathTools::transformPath(TTransform transform, TPath* pPath)
+{
+ TPath* pResulting = new TPath();
+
+ if (pPath->size() < 2)
+ {
+ printf("ERROR: path is too short\n");
+ delete pResulting;
+ return NULL;
+ }
+
+ // copy path and transform
+ for (int e = 0 ; e < int(pPath->size()) ; e++)
+ {
+ TPathElement element = pPath->at(e);
+ TSphereCoord res = MathTools::transform(element.Position, transform);
+ element.Position = res;
+
+ pResulting->push_back(element);
+ }
+
+ return pResulting;
+
+}
+
+TPath* MathTools::inverseTransformPath(TTransform transform, TPath* pPath)
+{
+ TPath* pResulting = new TPath();
+
+ if (pPath->size() < 2)
+ {
+ printf("ERROR: path is too short\n");
+ delete pResulting;
+ return NULL;
+ }
+
+ // copy path and transform
+ for (int e = 0 ; e < int(pPath->size()) ; e++)
+ {
+ TPathElement element = pPath->at(e);
+ TSphereCoord res = MathTools::inverseTransform(element.Position, transform);
+ element.Position = res;
+
+ pResulting->push_back(element);
+ }
+
+ return pResulting;
+
+}
+
+TTransform MathTools::determineTransformation(TSphereCoord start1, TSphereCoord start2, TSphereCoord end1, TSphereCoord end2)
+{
+ TTransform result;
+
+ // first determine alpha and beta and beta axis
+ float fAlpha, fBeta;
+ Eigen::Vector3d betaAxis;
+ determineTransformationAlphaBeta(start1, start2, fAlpha, fBeta, betaAxis);
+ result.fAlpha = fAlpha;
+ result.fBeta = fBeta;
+ result.betaAxis = betaAxis;
+
+ // calculate angular for psi rotation
+
+ // apply transform to first path end
+ TSphereCoord end1_transformed = transformAlphaBeta(end1, fAlpha, fBeta, betaAxis);
+
+ Eigen::Vector3d end1_transformed_v, start2_v, end2_v;
+ convert(end1_transformed, end1_transformed_v);
+ convert(start2, start2_v);
+ convert(end2, end2_v);
+
+ // calculate angle between start2 <-> end1_tranformed ; start2 <-> end2
+ Eigen::Vector3d zero = Eigen::Vector3d::Zero();
+
+ Eigen::Vector3d line1 = MathTools::dropAPerpendicular(end1_transformed_v, zero, start2_v);
+ Eigen::Vector3d line2 = MathTools::dropAPerpendicular(end2_v, zero, start2_v);
+ line1 -= end1_transformed_v;
+ line2 -= end2_v;
+
+ float fPsi = MathTools::AngleAndDirection(line1, line2, start2_v) * 180.0 / M_PI;
+
+ result.fPsi = fPsi;
+ result.psiAxis = start2_v;
+
+ return result;
+}
+
+TSphereCoord MathTools::transform(TSphereCoord sc, TTransform transform)
+{
+ // first apply alpha , beta
+ TSphereCoord rotated_ab = transformAlphaBeta(sc, transform.fAlpha, transform.fBeta, transform.betaAxis);
+
+ Eigen::Vector3d rotated_ab_v;
+
+ convert(rotated_ab, rotated_ab_v);
+
+ // now apply final rotation
+ Eigen::AngleAxisd rot(-transform.fPsi * M_PI / 180.0, transform.psiAxis);
+ rotated_ab_v = rot.matrix() * rotated_ab_v;
+
+ TSphereCoord rotated;
+ convert(rotated_ab_v, rotated);
+
+ return rotated;
+}
+
+TSphereCoord MathTools::inverseTransform(TSphereCoord sc, TTransform transform)
+{
+ Eigen::Vector3d sc_v;
+ convert(sc, sc_v);
+
+ // first rotate
+ Eigen::AngleAxisd rot(transform.fPsi * M_PI / 180.0, transform.psiAxis);
+ sc_v = rot.matrix() * sc_v;
+
+ TSphereCoord rotated;
+ convert(sc_v, rotated);
+
+ return inverseTransformAlphaBeta(rotated, transform.fAlpha, transform.fBeta, transform.betaAxis);
+}
+
+// determine alpha and beta of a transform
+// fAlpha: rotation around the z axis in counterclockwise direction considering the positive z-axis
+// fBeta: rotation around the rotated y axis in counterclockwise direction considering the positive y-axis
+void MathTools::determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord sc2, float& fAlpha, float& fBeta, Eigen::Vector3d& betaAxis)
+{
+ Eigen::Vector3d vector1, vector2;
+ convert(sc1, vector1);
+ convert(sc2, vector2);
+
+ // project both vectors to xy plane
+ Eigen::Vector3d vector1_xy, vector2_xy;
+
+ vector1_xy = vector1;
+ vector2_xy = vector2;
+
+ vector1_xy(2) = 0.0;
+ vector2_xy(2) = 0.0;
+
+ // calculate first angle from vec1_projected to vec2_projected with normale z
+ Eigen::Vector3d normal = Eigen::Vector3d::UnitZ();
+
+ if (vector1_xy.norm() == 0.0 || vector2_xy.norm() == 0)
+ {
+ fAlpha = ((sc2.fTheta - sc1.fTheta) / 180.0 * M_PI);
+ }
+ else
+ {
+ fAlpha = MathTools::AngleAndDirection(vector1_xy, vector2_xy, normal);
+ }
+
+ // now rotate first vector with alpha
+ Eigen::Vector3d rotated1, rotated_y;
+
+ // the vector to rotate
+
+ rotated1 = vector1;
+ // set rotation: alpha around z-axis to transform v1 in same plane as v2
+
+ Eigen::AngleAxisd rotation(fAlpha, Eigen::Vector3d::UnitZ());;
+ Eigen::AngleAxisd rotation_yaxis(sc2.fTheta, Eigen::Vector3d::UnitZ());
+
+ // we need normal for angle calculateion, so also rotate x-axis
+ rotated_y << 0.0, 1.0, 0.0;
+
+ // rotate vector and x-axis
+
+
+ rotated1 = rotation.matrix() * rotated1;
+
+ rotated_y = rotation_yaxis.matrix() * rotated_y;
+
+ betaAxis = rotated_y;
+
+ // calculate angle between both vectors
+ fBeta = MathTools::AngleAndDirection(rotated1, vector2, rotated_y);
+
+ fAlpha *= 180.0 / M_PI;
+ fBeta *= 180.0 / M_PI;
+}
+
+TSphereCoord MathTools::transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
+{
+ TSphereCoord result;
+ Eigen::Vector3d vector1, vector1_rotated;
+ convert(sc, vector1);
+
+ // rotate around positive z-axis
+ Eigen::AngleAxisd rotation_alpha(fAlpha / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
+
+ // rotate vector aroun z axis
+ vector1_rotated = rotation_alpha.matrix() * vector1;
+ // rotate vector with beta around betaAxis
+ Eigen::AngleAxisd rot(-fBeta / 180.0 * M_PI, betaAxis);
+
+ vector1_rotated = rot.matrix() * vector1_rotated;
+
+ convert(vector1_rotated, result);
+
+ return result;
+}
+
+TSphereCoord MathTools::inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
+{
+ Eigen::Vector3d vector1;
+ convert(sc, vector1);
+
+ // do inverse of beta axis rotation
+ Eigen::AngleAxisd rot(fBeta / 180.0 * M_PI, betaAxis);
+ vector1 = rot.matrix() * vector1;
+
+ // do inverse of zaxis rotation
+ Eigen::AngleAxisd rotation_alpha(-fAlpha / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
+
+ // rotate vector aroun z axis
+ vector1 = rotation_alpha * vector1;
+
+ TSphereCoord result;
+ convert(vector1, result);
+
+ return result;
+}
+
+// **********************************************************
+// calculations on sphere coordinates
+// **********************************************************
+float MathTools::CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3)
+{
+ float u1, v1, u2, v2;
+
+ u1 = p2.fPhi - p1.fPhi;
+ v1 = p2.fTheta - p1.fTheta;
+ u2 = p3.fPhi - p1.fPhi;
+ v2 = p3.fTheta - p1.fTheta;
+
+ return u1 * v2 - v1 * u2;
+}
+
+double MathTools::Angle(Eigen::Vector3d vec1, Eigen::Vector3d vec2)
+{
+ double r = vec1.dot(vec2) / (vec1.norm() * vec2.norm());
+ r = std::min(1.0, std::max(-1.0, r));
+ return std::acos(r);
+}
+
+float MathTools::getDistanceOnArc(TSphereCoord p1, TSphereCoord p2)
+{
+ Eigen::Vector3d vec1, vec2;
+ p1.fDistance = 1.0f;
+ p2.fDistance = 1.0f;
+
+ convert(p1, vec1);
+ convert(p2, vec2);
+
+ float fAngle = (float) MathTools::Angle(vec1, vec2);
+
+ // length of circular arc
+ // not necessary because Math3d::Angle returns a value in radians
+ //float fDist = float( (2 * M_PI * fAngle ) / 360.0f);
+
+ return fAngle;
+}
+
+float MathTools::getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius)
+{
+ // never use this!!!!
+ TSphereCoord diff;
+ diff.fPhi = p1.fPhi - p2.fPhi;
+ diff.fTheta = p1.fTheta - p2.fTheta;
+
+ float fAngle = sqrt(diff.fPhi * diff.fPhi + diff.fTheta * diff.fTheta);
+
+ return fAngle;
+}
+
+// **********************************************************
+// extended 2d Math
+// **********************************************************
+bool MathTools::IntersectLines(Eigen::Vector2d p1, Eigen::Vector2d m1, Eigen::Vector2d p2, Eigen::Vector2d m2, Eigen::Vector2d& res)
+{
+ // calculate perpendicular bisector of sides
+ float dx = float(p1(0) - p2(0));
+ float dy = float(p1(1) - p2(1));
+
+ // now from
+ // g1: g1(x,y) = (p1x,p1y) + (m1x,m1y) * c1
+ // g2: g2(x,y) = (p2x,p2y) + (m2x,m2y) * c2
+ // -->
+ // g1(x,y) = g2(x,y)
+ // -->
+ // (p1x,p1y) + (m1x,m1y) * c1 = (p2x,p2y) + (m2x,m2y) * c2
+ // (p1x,p1y) - (p2x,p2y) = (dx,dy) = (m2x,m2y) * c2 - (m1x,m1y) * c1
+ // -->
+ // dx = m2x * c2 - m1x * c1, dy = m2y * c2 - m1y * c1
+ // c1 = (m2y * c2 - dy) / m1y
+ // dx = m2x * c2 - m1x * (m2y * c2 - dy) / m1y
+
+ float z = float(dx - (m1(0) * dy) / m1(1));
+ float n = float(m2(0) - (m1(0) * m2(1)) / m1(1));
+
+ float c2;
+
+ if (n == 0)
+ {
+ return false;
+ }
+ else
+ {
+ c2 = z / n;
+ }
+
+ res(0) = p2(0) + c2 * m2(0);
+ res(1) = p2(1) + c2 * m2(1);
+
+ return true;
+}
+
+// **********************************************************
+// extended 3d Math
+// **********************************************************
+// checked and working
+float MathTools::AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vector2, Eigen::Vector3d normal)
+{
+ double fAngle = MathTools::Angle(vector1, vector2);
+
+ Eigen::Vector3d axis = vector1.cross(vector2);
+
+ // compare axis and normal
+ normal.normalize();
+
+ double c = normal.dot(vector1);
+
+ axis += vector1;
+ double d = normal.dot(axis) - c;
+
+
+ if (d < 0)
+ {
+ return 2 * M_PI - fAngle;
+ }
+
+ return (float) fAngle;
+}
+
+// checked and working
+Eigen::Vector3d MathTools::dropAPerpendicular(Eigen::Vector3d point, Eigen::Vector3d linepoint, Eigen::Vector3d linevector)
+{
+ // helping plane normal (ax + by + cz + d = 0)
+ Eigen::Vector3d plane_normal = linevector; // (a,b,c)
+ float d = (float) - plane_normal.dot(point);
+
+ // calculate k from plane and line intersection
+ float z = float(- plane_normal(0) * linepoint(0) - plane_normal(1) * linepoint(1) - plane_normal(2) * linepoint(2) - d);
+ float n = float(plane_normal(0) * linevector(0) + plane_normal(1) * linevector(1) + plane_normal(2) * linevector(2));
+
+ float k = z / n;
+
+ // calculate line from line equation
+ linevector = k * linevector;
+ linevector += linepoint;
+
+ return linevector;
+
+}
+
+// **********************************************************
+// conversions
+// **********************************************************
+// checked and working
+void MathTools::convert(TSphereCoord in, Eigen::Vector3d& out)
+{
+ // alpha is azimuth, beta is polar angle
+ in.fPhi *= M_PI / 180.0f;
+ in.fTheta *= M_PI / 180.0f;
+
+ // OLD and wrong??
+ // out.x = in.fDistance * sin(in.fPhi) * cos(in.fTheta);
+ // out.y = in.fDistance * sin(in.fPhi) * sin(in.fTheta);
+ // out.z = in.fDistance * cos(in.fPhi);
+
+ Eigen::Vector3d vector = Eigen::Vector3d::UnitZ();
+ Eigen::AngleAxisd rotation_y(in.fPhi, Eigen::Vector3d::UnitY());
+ Eigen::AngleAxisd rotation_z(in.fTheta, Eigen::Vector3d::UnitZ());
+
+
+ // first rotate around y axis with phi
+ vector = rotation_y.matrix() * vector;
+
+ // second rotate around z with theta
+ vector = rotation_z.matrix() * vector;
+
+ out = vector;
+}
+
+
+void MathTools::convertWithDistance(TSphereCoord in, Eigen::Vector3d& out)
+{
+ convert(in, out);
+ out *= in.fDistance;
+}
+
+void MathTools::convert(TSphereCoord in, Eigen::Vector3f& out)
+{
+ Eigen::Vector3d vec;
+ convert(in, vec);
+ out = vec.cast<float>();
+}
+
+
+// checked and working
+void MathTools::convert(Eigen::Vector3d in, TSphereCoord& out)
+{
+ // alpha is azimuth, beta is polar angle
+ out.fDistance = in.norm();
+
+ float fAngle = in(2) / out.fDistance;
+
+ if (fAngle < -1.0f)
+ {
+ fAngle = -1.0f;
+ }
+
+ if (fAngle > 1.0f)
+ {
+ fAngle = 1.0f;
+ }
+
+ out.fPhi = (float) acos(fAngle);
+
+ out.fTheta = (float) atan2(in(1), in(0));
+
+ // convert to angles and correct interval
+ out.fPhi /= M_PI / 180.0f;
+ out.fTheta /= M_PI / 180.0f;
+
+ // only positive rotations
+ if (out.fTheta < 0.0)
+ {
+ out.fTheta = 360.0f + out.fTheta;
+ }
+
+ // be aware of limit
+ if (out.fPhi > 180.0)
+ {
+ out.fPhi = out.fPhi - 180.0f;
+ }
+}
+
+
+void MathTools::convert(Eigen::Vector3f in, TSphereCoord& out)
+{
+ Eigen::Vector3d vec(in(0), in(1), in(2));
+ convert(vec, out);
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/MathTools.h b/source/RobotAPI/components/EarlyVisionGraph/MathTools.h
new file mode 100644
index 0000000000000000000000000000000000000000..336629401bf50aed9f10fe1b24b6e04904e8fd0a
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/MathTools.h
@@ -0,0 +1,72 @@
+// *****************************************************************
+// Filename: MathTools.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _MATH_TOOLS_H_
+#define _MATH_TOOLS_H_
+
+// *****************************************************************
+// defines
+// *****************************************************************
+#include <math.h>
+#ifndef M_PI
+#define M_PI 3.141592653589793
+#endif
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "Structs.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// *****************************************************************
+// class MathTools
+// *****************************************************************
+class MathTools
+{
+public:
+ // transformation of coordinates and paths
+ static TTransform determinePathTransformation(TPath* p1, TPath* p2);
+ static TPath* transformPath(TTransform transform, TPath* pPath);
+ static TPath* inverseTransformPath(TTransform transform, TPath* pPath);
+
+ static TTransform determineTransformation(TSphereCoord start1, TSphereCoord start2, TSphereCoord end1, TSphereCoord end2);
+ static TSphereCoord transform(TSphereCoord sc, TTransform transform);
+ static TSphereCoord inverseTransform(TSphereCoord sc, TTransform transform);
+
+ static void determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord sc2, float& fAlpha, float& fBeta, Eigen::Vector3d& betaAxis);
+ static TSphereCoord transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis);
+ static TSphereCoord inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis);
+
+ // calculations on sphere coordimates
+ static float CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3);
+ static float getDistanceOnArc(TSphereCoord p1, TSphereCoord p2);
+ static float getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius);
+
+ // extended 2d math
+ static bool IntersectLines(Eigen::Vector2d p1, Eigen::Vector2d m1, Eigen::Vector2d p2, Eigen::Vector2d m2, Eigen::Vector2d& res);
+
+ // extended 3d math
+ static Eigen::Vector3d dropAPerpendicular(Eigen::Vector3d point, Eigen::Vector3d linepoint, Eigen::Vector3d linevector);
+ static float AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vector2, Eigen::Vector3d normal);
+
+ // coorrdinate conversion
+ static void convert(TSphereCoord in, Eigen::Vector3d& out);
+ static void convert(TSphereCoord in, Eigen::Vector3f& out);
+ static void convert(Eigen::Vector3d in, TSphereCoord& out);
+ static void convert(Eigen::Vector3f in, TSphereCoord& out);
+ static void convertWithDistance(TSphereCoord in, Eigen::Vector3d& out);
+
+ static double Angle(Eigen::Vector3d vec1, Eigen::Vector3d vec2);
+
+
+};
+
+#endif /* _MATH_TOOLS_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31b5b39a48ff36b62df0af05491778fe63690218
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp
@@ -0,0 +1,314 @@
+// *****************************************************************
+// Filename: AspectGraph.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+//#include "Base/Tools/DebugMemory.h"
+
+// *****************************************************************
+// construction / destruction
+// *****************************************************************
+CSphericalGraph::CSphericalGraph()
+{
+ clear();
+}
+
+CSphericalGraph::CSphericalGraph(const CSphericalGraph& prototype)
+{
+ *this = prototype;
+}
+
+CSphericalGraph::~CSphericalGraph()
+{
+ clear();
+}
+
+CSphericalGraph& CSphericalGraph::operator= (CSphericalGraph const& rhs)
+{
+ clear();
+
+ // copy all nodes
+ const TNodeList* nodes = &rhs.m_Nodes;
+
+ int nNumberNodes = int(nodes->size());
+
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ addNode(nodes->at(n)->clone());
+ }
+
+ // copy all edges
+ const TEdgeList* edges = &rhs.m_Edges;
+
+ int nNumberEdges = int(edges->size());
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ CSGEdge* edge = edges->at(e);
+ addEdge(edge->nIndex1, edge->nIndex2, edge->nLeftFace, edge->nRightFace);
+ }
+
+ return *this;
+}
+
+
+// *****************************************************************
+// control
+// *****************************************************************
+void CSphericalGraph::clear()
+{
+ // clear edges
+ vector<CSGEdge*>::iterator iter = m_Edges.begin();
+
+ while (iter != m_Edges.end())
+ {
+ delete *iter;
+ iter++;
+ }
+
+ m_Edges.clear();
+
+ // clear nodes
+ vector<CSGNode*>::iterator iter_node = m_Nodes.begin();
+
+ while (iter_node != m_Nodes.end())
+ {
+ delete *iter_node;
+ iter_node++;
+ }
+
+ m_Nodes.clear();
+}
+
+// *****************************************************************
+// graph building
+// *****************************************************************
+// nodes
+int CSphericalGraph::addNode(CSGNode* pNode)
+{
+ m_Nodes.push_back(pNode);
+ pNode->setIndex(int(m_Nodes.size() - 1));
+
+ return pNode->getIndex();
+}
+
+// edges
+int CSphericalGraph::addEdge(int nIndex1, int nIndex2)
+{
+ return addEdge(nIndex1, nIndex2, -1, -1);
+}
+
+int CSphericalGraph::addEdge(int nIndex1, int nIndex2, int nLeftFace, int nRightFace)
+{
+ CSGEdge* pEdge = new CSGEdge;
+ pEdge->nIndex1 = nIndex1;
+ pEdge->nIndex2 = nIndex2;
+ pEdge->nLeftFace = nLeftFace;
+ pEdge->nRightFace = nRightFace;
+
+ // printf("Adding edge %d --> %d\n",nIndex1,nIndex2);
+
+ // add to list of edges
+ m_Edges.push_back(pEdge);
+
+ // update node adjacency list
+ addNodeAdjacency(nIndex1, nIndex2);
+ addNodeAdjacency(nIndex2, nIndex1);
+
+ return int(m_Edges.size()) - 1;
+}
+
+void CSphericalGraph::addNodeAdjacency(int nNode, int nAdjacency)
+{
+ if (nNode == nAdjacency)
+ {
+ printf("Error: adding node as adjacenca\n");
+ }
+
+ // check if present
+ bool bPresent = false;
+
+ for (int n = 0 ; n < int(m_NodeAdjacency[nNode].size()) ; n++)
+ {
+ if (m_NodeAdjacency[nNode].at(n) == nAdjacency)
+ {
+ bPresent = true;
+ }
+ }
+
+ if (!bPresent)
+ {
+ m_NodeAdjacency[nNode].push_back(nAdjacency);
+ }
+}
+
+// *****************************************************************
+// member access
+// *****************************************************************
+TEdgeList* CSphericalGraph::getEdges()
+{
+ return &m_Edges;
+}
+
+TNodeList* CSphericalGraph::getNodes()
+{
+ return &m_Nodes;
+}
+
+CSGEdge* CSphericalGraph::getEdge(int nEdgeIndex)
+{
+ return m_Edges.at(nEdgeIndex);
+}
+
+CSGNode* CSphericalGraph::getNode(int nIndex)
+{
+ return m_Nodes.at(nIndex);
+}
+
+vector<int>* CSphericalGraph::getNodeAdjacency(int nIndex)
+{
+ return &m_NodeAdjacency[nIndex];
+}
+
+// *****************************************************************
+// file io
+// *****************************************************************
+bool CSphericalGraph::read(istream& infile)
+{
+ try
+ {
+ int nNumberNodes;
+
+ // read number of nodes
+ infile >> nNumberNodes;
+
+ // read all nodes
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ readNode(infile);
+ }
+
+ // read number of edges
+ int nNumberEdges;
+ infile >> nNumberEdges;
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ readEdge(infile);
+ }
+
+ }
+ catch (istream::failure e)
+ {
+ printf("ERROR: failed to write FeatureGraph to file\n");
+ return false;
+ }
+
+ return true;
+}
+
+bool CSphericalGraph::readNode(istream& infile)
+{
+ CSGNode* pNewNode = (CSGNode*) getNewNode();
+
+ int nIndex;
+ TSphereCoord pos;
+
+ infile >> nIndex;
+ infile >> pos.fPhi;
+ infile >> pos.fTheta;
+
+ pNewNode->setPosition(pos);
+
+ if (addNode(pNewNode) != nIndex)
+ {
+ printf("Error input file inconsistent\n");
+ return false;
+ }
+
+ return true;
+}
+
+
+
+bool CSphericalGraph::readEdge(istream& infile)
+{
+ int nIndex1, nIndex2, nLeftFace, nRightFace;
+
+ infile >> nIndex1;
+ infile >> nIndex2;
+
+ infile >> nLeftFace;
+ infile >> nRightFace;
+
+ addEdge(nIndex1, nIndex2, nLeftFace, nRightFace);
+ return true;
+}
+
+
+bool CSphericalGraph::write(ostream& outfile)
+{
+ try
+ {
+ int nNumberNodes = int(m_Nodes.size());
+ // write number of nodes
+ outfile << nNumberNodes << endl;
+
+ // write all nodes
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ writeNode(outfile, n);
+ }
+
+ // write number of edges
+ int nNumberEdges = int(m_Edges.size());
+ outfile << nNumberEdges << endl;
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ writeEdge(outfile, e);
+ }
+
+ }
+ catch (ostream::failure e)
+ {
+ printf("ERROR: failed to write FeatureGraph to file\n");
+ return false;
+ }
+
+ return true;
+}
+
+bool CSphericalGraph::writeNode(ostream& outfile, int n)
+{
+ CSGNode* pCurrentNode = m_Nodes.at(n);
+
+ outfile << pCurrentNode->getIndex() << endl;
+ outfile << pCurrentNode->getPosition().fPhi << endl;
+ outfile << pCurrentNode->getPosition().fTheta << endl;
+
+ outfile << endl;
+ return true;
+}
+
+bool CSphericalGraph::writeEdge(ostream& outfile, int e)
+{
+ CSGEdge* pCurrentEdge = m_Edges.at(e);
+ outfile << pCurrentEdge->nIndex1 << " ";
+ outfile << pCurrentEdge->nIndex2 << " ";
+
+ outfile << pCurrentEdge->nLeftFace << " ";
+ outfile << pCurrentEdge->nRightFace << " ";
+
+ outfile << endl;
+ return true;
+}
diff --git a/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h
new file mode 100644
index 0000000000000000000000000000000000000000..fc8ffdbe823dbec9f5e7864216020f11fe5aabb1
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h
@@ -0,0 +1,165 @@
+// *****************************************************************
+// Filename: AspectGraph.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _SPHERICAL_GRAPH_H_
+#define _SPHERICAL_GRAPH_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+// stl includes
+#include <vector>
+#include <istream>
+#include <ostream>
+
+// local includes
+#include "Structs.h"
+
+// *****************************************************************
+// namespaces
+// *****************************************************************
+using namespace std;
+
+// *****************************************************************
+// defines
+// *****************************************************************
+#define MAX_NODES 100000
+
+// *****************************************************************
+// nodes and edges
+// *****************************************************************
+class CSGEdge
+{
+public:
+ // indices of nodes
+ int nIndex1;
+ int nIndex2;
+
+ // indices of faces (needed in triangulation
+ int nLeftFace;
+ int nRightFace;
+};
+
+class CSGNode
+{
+public:
+ CSGNode() {}
+ CSGNode(TSphereCoord position)
+ {
+ m_Position = position;
+ }
+ virtual ~CSGNode() {}
+
+ TSphereCoord getPosition()
+ {
+ return m_Position;
+ }
+ void setPosition(TSphereCoord position)
+ {
+ m_Position = position;
+ }
+
+ int getIndex()
+ {
+ return m_nIndex;
+ }
+ void setIndex(int nIndex)
+ {
+ m_nIndex = nIndex;
+ }
+
+ virtual CSGNode* clone()
+ {
+ CSGNode* copy = new CSGNode(m_Position);
+ copy->setIndex(m_nIndex);
+ return copy;
+ }
+
+protected:
+ TSphereCoord m_Position;
+ int m_nIndex;
+
+};
+
+// *****************************************************************
+// types
+// *****************************************************************
+typedef vector<CSGEdge*> TEdgeList;
+typedef vector<CSGNode*> TNodeList;
+
+// *****************************************************************
+// definition of class CSphericalGraph
+// *****************************************************************
+class CSphericalGraph
+{
+public:
+ ///////////////////////////////
+ // construction / destruction
+ ///////////////////////////////
+ CSphericalGraph();
+ CSphericalGraph(const CSphericalGraph& prototype);
+
+ virtual ~CSphericalGraph();
+ virtual CSGNode* getNewNode()
+ {
+ return new CSGNode();
+ }
+
+ ///////////////////////////////
+ // aspect graph manipulation
+ ///////////////////////////////
+ // clear graph
+ void clear();
+
+ // add nodes
+ int addNode(CSGNode* pNode);
+
+ // add edges
+ int addEdge(int nIndex1, int nIndex2);
+ int addEdge(int nIndex1, int nIndex2, int nLeftFace, int nRightFace);
+
+ // node adjacency (usually only required by addEdge)
+ void addNodeAdjacency(int nNode, int nAdjacency);
+
+ ///////////////////////////////
+ // member access
+ ///////////////////////////////
+ TEdgeList* getEdges();
+ TNodeList* getNodes();
+
+ CSGEdge* getEdge(int nEdgeIndex);
+ CSGNode* getNode(int nIndex);
+ vector<int>* getNodeAdjacency(int nIndex);
+
+ ///////////////////////////////
+ // file io
+ ///////////////////////////////
+ virtual bool read(istream& infile);
+ virtual bool readNode(istream& infile);
+ virtual bool readEdge(istream& infile);
+
+ virtual bool write(ostream& outfile);
+ virtual bool writeNode(ostream& outfile, int n);
+ virtual bool writeEdge(ostream& outfile, int e);
+
+
+
+ ///////////////////////////////
+ // operators
+ ///////////////////////////////
+ CSphericalGraph& operator= (CSphericalGraph const& rhs);
+
+protected:
+ TEdgeList m_Edges;
+ TNodeList m_Nodes;
+ vector<int> m_NodeAdjacency[MAX_NODES];
+
+};
+
+#endif /* _SPHERICAL_GRAPH_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/Structs.h b/source/RobotAPI/components/EarlyVisionGraph/Structs.h
new file mode 100644
index 0000000000000000000000000000000000000000..64e071bcddd59b4dc5bb7517a8d8611a95a04f76
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/Structs.h
@@ -0,0 +1,85 @@
+// *****************************************************************
+// Filename: Structs.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _STRUCTS_H_
+#define _STRUCTS_H_
+
+#include <vector>
+#include <Eigen/Core>
+
+using namespace std;
+
+// *****************************************************************
+// structs
+// *****************************************************************
+
+// sphere coordinates with the following convention:
+// phi: rotation from positive z-axis (pointing up) to the vector (p-o) (around the rotated y-axis)
+// theta: rotation from the positive x-axis to the projection of the point in xy plane (around z-axis)
+struct TSphereCoord
+{
+ float fDistance;
+ float fPhi; // zenith
+ float fTheta; // azimuth
+};
+
+// transformation from one sphere to another
+// both spheres wiht same radius
+// psi: orientation of the view = rotation around the rotated (by spherical coords) x-axis
+struct TSphereTransform
+{
+ TSphereCoord fPosition;
+ float fPsi; // orientation
+};
+
+// transformation from one sphere to another
+// both spheres wiht same radius
+// fAlpha: rotation around the z axis in counterclockwise direction considering the positive z-axis
+// fBeta: rotation around the rotated y axis in counterclockwise direction considering the positive y-axis
+struct TTransform
+{
+ float fAlpha;
+ float fBeta;
+ Eigen::Vector3d betaAxis;
+ float fPsi;
+ Eigen::Vector3d psiAxis;
+};
+
+// *****************************************************************
+// structs
+// *****************************************************************
+struct TPathElement
+{
+ TSphereCoord Position;
+ int nNodeIndex;
+ int nHits;
+ float fBestSimilarity;
+};
+
+typedef vector<TPathElement> TPath;
+
+struct THypothesis
+{
+ int nID;
+ TPath Path;
+ // path look-a-like rating
+ float fRating;
+ // visual similarity of last comparison
+ float fSimilarity;
+ // overall visual similarity
+ float fAccSimilarity;
+ // overall rating
+ float fOverallRating;
+ TTransform Transform;
+ bool bValidTransform;
+};
+
+
+
+#endif /* _STRUCTS_H_ */
diff --git a/source/RobotAPI/components/ViewSelection/CMakeLists.txt b/source/RobotAPI/components/ViewSelection/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3fb96c045bcebbde9a8886499180d16df3990edd
--- /dev/null
+++ b/source/RobotAPI/components/ViewSelection/CMakeLists.txt
@@ -0,0 +1,29 @@
+armarx_component_set_name("ViewSelection")
+
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+if(Eigen3_FOUND)
+ include_directories(${Eigen3_INCLUDE_DIR})
+endif()
+
+
+set(COMPONENT_LIBS
+ RobotAPIInterfaces RobotAPICore EarlyVisionGraph
+ ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers
+ ${Simox_LIBRARIES}
+)
+
+set(SOURCES
+./ViewSelection.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+./ViewSelection.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3ab494165047001c86104c218febb043c2cbcc2e
--- /dev/null
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
@@ -0,0 +1,329 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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 RobotComponents::ViewSelection
+ * @author David Schiebener (schiebener at kit dot edu)
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ViewSelection.h"
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+
+using namespace armarx;
+
+
+void ViewSelection::onInitComponent()
+{
+ usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
+ usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
+
+ offeringTopic("DebugDrawerUpdates");
+
+ headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
+ headFrameName = getProperty<std::string>("HeadFrameName").getValue();
+ cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
+ drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();
+
+ std::string graphFileName = "RobotComponents/ViewSelection/graph40k.gra";
+
+ armarx::CMakePackageFinder finder("RobotComponents");
+ ArmarXDataPath::addDataPaths(finder.getDataDir());
+
+ if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
+ {
+ visibilityMaskGraph = new CIntensityGraph(graphFileName);
+ TNodeList* nodes = visibilityMaskGraph->getNodes();
+ const float maxOverallHeadTiltAngle = getProperty<float>("MaxOverallHeadTiltAngle").getValue();
+ const float borderAreaAngle = 10.0f;
+ const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
+
+ for (size_t i = 0; i < nodes->size(); i++)
+ {
+ CIntensityNode* node = (CIntensityNode*) nodes->at(i);
+
+ if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle - borderAreaAngle)
+ {
+ node->setIntensity(1.0f);
+ }
+ else if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle)
+ {
+ node->setIntensity(1.0f - (fabs(node->getPosition().fPhi - centralAngle) - (maxOverallHeadTiltAngle - borderAreaAngle)) / borderAreaAngle);
+ }
+ else
+ {
+ node->setIntensity(0.0f);
+ }
+ }
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not find required1 graph file";
+ handleExceptions();
+ }
+
+ sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
+ doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
+
+ timeOfLastViewChange = IceUtil::Time::now();
+
+ // this is robot model specific: offset from the used head coordinate system to the actual
+ // head center where the eyes are assumed to be located. Here it is correct for ARMAR-III
+ offsetToHeadCenter << 0, 0, 150;
+
+ processorTask = new PeriodicTask<ViewSelection>(this, &ViewSelection::process, 30);
+ processorTask->setDelayWarningTolerance(sleepingTimeBetweenViewDirectionChanges + 100);
+}
+
+
+void ViewSelection::onConnectComponent()
+{
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+
+ headIKUnitProxy = getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
+ headIKUnitProxy->request();
+
+ drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
+
+ processorTask->start();
+}
+
+
+void ViewSelection::onDisconnectComponent()
+{
+ processorTask->stop();
+
+ if (drawer)
+ {
+ drawer->removeArrowDebugLayerVisu("HeadRealViewDirection");
+ }
+
+ try
+ {
+ headIKUnitProxy->release();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING_S << "An exception occured during onDisconnectComponent()";
+ handleExceptions();
+ }
+}
+
+
+void ViewSelection::onExitComponent()
+{
+ delete visibilityMaskGraph;
+}
+
+
+ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
+{
+ SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
+ TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
+
+ // find the direction with highest saliency
+ int maxIndex = -1;
+ float maxSaliency = 0;
+ for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
+ {
+ float saliency = 0.0f;
+ for (const auto & p : saliencyMaps)
+ {
+ saliency += p.second[i];
+ }
+
+ CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
+ saliency *= visibilityNode->getIntensity();
+
+ if (saliency > maxSaliency)
+ {
+ maxSaliency = saliency;
+ maxIndex = i;
+ }
+ }
+
+ ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
+
+ // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
+ int timeSinceLastViewChange = (IceUtil::Time::now() - timeOfLastViewChange).toMilliSeconds();
+ float saliencyThreshold = 0;
+
+ if (timeSinceLastViewChange > 0)
+ {
+ saliencyThreshold = 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
+ }
+
+ if (maxSaliency > saliencyThreshold)
+ {
+ Eigen::Vector3f target;
+ MathTools::convert(visibilityMaskGraphNodes->at(maxIndex)->getPosition(), target);
+ const float distanceFactor = 1500;
+ target = distanceFactor * target + offsetToHeadCenter;
+ FramedPositionPtr viewTargetPositionPtr = new FramedPosition(target, headFrameName, robotPrx->getName());
+
+ ViewTargetBasePtr viewTarget = new ViewTargetBase();
+ viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY; // * maxSaliency;
+ viewTarget->timeAdded = new TimestampVariant(IceUtil::Time::now());
+ viewTarget->target = viewTargetPositionPtr;
+ viewTarget->duration = 0;
+
+ return viewTarget;
+ }
+
+ return nullptr;
+}
+
+
+
+void ViewSelection::process()
+{
+ ViewTargetBasePtr viewTarget;
+
+ if (doAutomaticViewSelection)
+ {
+ viewTarget = nextAutomaticViewTarget();
+ }
+
+ if (!manualViewTargets.empty())
+ {
+ ScopedLock lock(manualViewTargetsMutex);
+
+ ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
+
+ CompareViewTargets c;
+
+ if (!viewTarget)
+ {
+ viewTarget = manualViewTarget;
+ manualViewTargets.pop();
+ }
+ else if (c(viewTarget, manualViewTarget))
+ {
+ viewTarget = manualViewTarget;
+ manualViewTargets.pop();
+ }
+ }
+
+ if (viewTarget)
+ {
+ SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
+ FramedPositionPtr viewTargetPositionPtr = new FramedPosition(FramedPositionPtr::dynamicCast(viewTarget->target)->toEigen(), viewTarget->target->frame, robotPrx->getName());
+
+ if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
+ {
+ float arrowLength = 1500.0f;
+ Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
+ FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
+ drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, arrowLength, 5);
+ Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
+ Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
+ drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, arrowLength, 5);
+ }
+
+ ARMARX_INFO << "Looking at target " << viewTargetPositionPtr->output();
+ headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
+
+ timeOfLastViewChange = IceUtil::Time::now();
+
+
+ float duration = viewTarget->duration;
+ if (!viewTarget->duration)
+ {
+ duration = sleepingTimeBetweenViewDirectionChanges;
+ }
+
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(duration));
+ }
+ else
+ {
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5));
+ }
+}
+
+
+
+void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
+{
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ //SharedRobotInterfacePrx sharedRobotInterface = robotStateComponent->getRobotSnapshot();
+
+ ViewTargetBasePtr viewTarget = new ViewTargetBase();
+ viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY + 1;
+ viewTarget->timeAdded = new TimestampVariant(IceUtil::Time::now());
+ viewTarget->target = target;
+ viewTarget->duration = 0;
+
+ manualViewTargets.push(viewTarget);
+}
+
+void ViewSelection::clearManualViewTargets(const Ice::Current& c)
+{
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ while (!manualViewTargets.empty())
+ {
+ manualViewTargets.pop();
+ }
+
+ // std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> temp;
+ // manualViewTargets.swap(temp);
+}
+
+ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
+{
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ ViewTargetList result;
+
+ std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> temp(manualViewTargets);
+
+ while (!temp.empty())
+ {
+ result.push_back(temp.top());
+
+ temp.pop();
+ }
+
+ return result;
+}
+
+
+
+void armarx::ViewSelection::updateSaliencyMap(const string& name, const FloatSequence& map, const Ice::Current&)
+{
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ saliencyMaps[name] = map;
+}
+
+
+PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new ViewSelectionPropertyDefinitions(getConfigIdentifier()));
+}
+
+
+
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce8977b57ed01e24180a81ad384d1576b43bd909
--- /dev/null
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.h
@@ -0,0 +1,207 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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 RobotComponents::ViewSelection
+ * @author David Schiebener (schiebener at kit dot edu)
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotComponents_ViewSelection_H
+#define _ARMARX_COMPONENT_RobotComponents_ViewSelection_H
+
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+#include <RobotAPI/interface/components/ViewSelectionInterface.h>
+
+#include <RobotAPI/components/units/HeadIKUnit.h>
+
+#include <RobotAPI/components/EarlyVisionGraph/IntensityGraph.h>
+#include <RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h>
+#include <RobotAPI/components/EarlyVisionGraph/MathTools.h>
+
+
+#include <Eigen/Geometry>
+
+
+#include <queue>
+
+namespace armarx
+{
+
+ struct CompareViewTargets
+ {
+ bool operator()(ViewTargetBasePtr const& t1, ViewTargetBasePtr const& t2)
+ {
+ if (t1->priority == t2->priority)
+ {
+ return t1->timeAdded->timestamp < t2->timeAdded->timestamp;
+
+ }
+ return t1->priority < t2->priority;
+ }
+ };
+
+
+ /**
+ * @class ViewSelectionPropertyDefinitions
+ * @brief
+ */
+ class ViewSelectionPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ ViewSelectionPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
+ defineOptionalProperty<std::string>("HeadIKUnitName", "HeadIKUnit", "Name of the head IK unit component that should be used");
+ defineOptionalProperty<std::string>("HeadIKKinematicChainName", "IKVirtualGaze", "Name of the kinematic chain for the head IK");
+ defineOptionalProperty<std::string>("HeadFrameName", "Head Base", "Name of the frame of the head base in the robot model");
+ defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model");
+ defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2500, "Time between two view changes, to keep the head looking into one direction for a while (in ms)");
+ defineOptionalProperty<bool>("ActiveAtStartup", true, "Decide whether the automatic view selection will be activated (can be changed via the proxy during runtime)");
+ defineOptionalProperty<bool>("VisualizeViewDirection", false, "Draw view ray on DebugLayer.");
+ defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 55.0f, "Maximal angle the head and eyes can look down (in degrees)");
+ defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees). Default is looking 20 degrees downwards");
+ defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection");
+ }
+ };
+
+ /**
+ * @defgroup Component-ViewSelection ViewSelection
+ * @ingroup RobotComponents-Components
+ * @brief The ViewSelection component controls the head of the robot with inverse kinematics based on the uncertainty
+ * of the current requested object locations.
+ * The uncertainty of objects grow based on their motion model and the timed passed since the last localization.
+ * It can be activated or deactivated with the Ice interface and given manual target positions to look at.
+ */
+
+ /**
+ * @ingroup Component-ViewSelection
+ * @brief The ViewSelection class
+ */
+ class ViewSelection :
+ virtual public Component,
+ virtual public ViewSelectionInterface
+ {
+ public:
+ /**
+ * @see armarx::ManagedIceObject::getDefaultName()
+ */
+ virtual std::string getDefaultName() const
+ {
+ return "ViewSelection";
+ }
+
+ protected:
+ /**
+ * @see armarx::ManagedIceObject::onInitComponent()
+ */
+ virtual void onInitComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onConnectComponent()
+ */
+ virtual void onConnectComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onDisconnectComponent()
+ */
+ virtual void onDisconnectComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onExitComponent()
+ */
+ virtual void onExitComponent();
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ virtual void addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c = Ice::Current());
+
+ virtual void clearManualViewTargets(const Ice::Current& c = Ice::Current());
+
+ virtual ViewTargetList getManualViewTargets(const Ice::Current& c = Ice::Current());
+
+ virtual void activateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
+ {
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ ARMARX_INFO << "activating automatic view selection";
+ doAutomaticViewSelection = true;
+ }
+
+ virtual void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
+ {
+ boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+
+ ARMARX_INFO << "DEactivating automatic view selection";
+ doAutomaticViewSelection = false;
+ }
+
+ void updateSaliencyMap(const string& name, const FloatSequence& map, const Ice::Current& c = ::Ice::Current());
+
+ private:
+
+ void process();
+
+ ViewTargetBasePtr nextAutomaticViewTarget();
+
+ armarx::PeriodicTask<ViewSelection>::pointer_type processorTask;
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+ HeadIKUnitInterfacePrx headIKUnitProxy;
+ DebugDrawerInterfacePrx drawer;
+
+ std::string headIKKinematicChainName;
+ std::string headFrameName;
+ std::string cameraFrameName;
+
+ CIntensityGraph* visibilityMaskGraph;
+
+ float sleepingTimeBetweenViewDirectionChanges;
+ IceUtil::Time timeOfLastViewChange;
+
+ bool drawViewDirection;
+
+ armarx::Mutex manualViewTargetsMutex;
+ std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> manualViewTargets;
+
+ bool doAutomaticViewSelection;
+
+ Eigen::Vector3f offsetToHeadCenter;
+
+
+ std::map<std::string, std::vector<float>> saliencyMaps;
+
+ };
+
+
+}
+
+#endif
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 088be6ebe83e6fa29a877c92eeb62548115d8492..978fedcf56a92dea71785161f663647355b87a9d 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -34,13 +34,14 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+
#include <algorithm>
using namespace armarx;
void KinematicUnitSimulation::onInitKinematicUnit()
{
-
+ usePDControllerForPosMode = getProperty<bool>("UsePDControllerForJointControl").getValue();
for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
{
std::string jointName = (*it)->getName();
@@ -68,6 +69,10 @@ void KinematicUnitSimulation::onInitKinematicUnit()
std::string jointName = (*it)->getName();
jointStates[jointName] = KinematicUnitSimulationJointState();
jointStates[jointName].init();
+ positionControlPIDController[jointName] = PIDControllerPtr(new PIDController(
+ getProperty<float>("Kp").getValue(),
+ getProperty<float>("Ki").getValue(),
+ getProperty<float>("Kd").getValue()));
}
}
@@ -102,6 +107,10 @@ void KinematicUnitSimulation::simulationFunction()
bool aPosValueChanged = false;
bool aVelValueChanged = false;
+ auto signedMin = [](float newValue, float minAbsValue)
+ {
+ return std::copysign(std::min<float>(fabs(newValue), fabs(minAbsValue)), newValue);
+ };
{
boost::mutex::scoped_lock lock(jointStatesMutex);
@@ -110,27 +119,47 @@ void KinematicUnitSimulation::simulationFunction()
{
float newAngle = 0.0f;
-
+ KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
// calculate joint positions if joint is in velocity control mode
if (it->second.controlMode == eVelocityControl)
{
double change = (it->second.velocity * timeDeltaInSeconds);
double randomNoiseValue = distribution(generator);
- change += randomNoiseValue;
+ change *= 1 + randomNoiseValue;
newAngle = it->second.angle + change;
}
else if (it->second.controlMode == ePositionControl)
{
- newAngle = it->second.angle;
+ if (!usePDControllerForPosMode)
+ {
+ newAngle = it->second.angle;
+ }
+ else
+ {
+ PIDControllerPtr pid = positionControlPIDController[it->first];
+ if (pid)
+ {
+ float velValue = (it->second.velocity != 0.0f ? signedMin(pid->getControlValue(), it->second.velocity) : pid->getControlValue()); //restrain to max velocity
+ velValue *= 1 + distribution(generator);
+ pid->update(it->second.angle);
+ newAngle = it->second.angle +
+ velValue *
+ timeDeltaInSeconds;
+
+ }
+ }
}
// constrain the angle
- KinematicUnitSimulationJointInfo& 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);
+ if (!jointInfo.continuousJoint())
+ {
+ float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : M_PI;
+ float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -M_PI;
+
+ newAngle = std::max<float>(newAngle, minAngle);
+ newAngle = std::min<float>(newAngle, maxAngle);
+ }
// Check if joint existed before
KinematicUnitSimulationJointStates::iterator itPrev = previousJointStates.find(it->first);
@@ -154,6 +183,18 @@ void KinematicUnitSimulation::simulationFunction()
aVelValueChanged = true;
}
+ if (jointInfo.continuousJoint())
+ {
+ if (newAngle < 0)
+ {
+ newAngle = fmod(newAngle - M_PI, 2 * M_PI) + M_PI;
+ }
+ else
+ {
+ newAngle = fmod(newAngle + M_PI, 2 * M_PI) - M_PI;
+ }
+
+ }
actualJointAngles[it->first] = newAngle;
actualJointVelocities[it->first] = it->second.velocity;
@@ -226,15 +267,36 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
{
aValueChanged = true;
}
+ KinematicUnitSimulationJointInfo& jointInfo = jointInfos[targetJointName];
+ if (jointInfo.hasLimits() && !jointInfo.continuousJoint())
+ {
+ targetJointAngle = std::max<float>(targetJointAngle, jointInfo.limitLo);
+ targetJointAngle = std::min<float>(targetJointAngle, jointInfo.limitHi);
+ }
- if (jointInfos[targetJointName].hasLimits())
+ if (!usePDControllerForPosMode)
{
- targetJointAngle = std::max<float>(targetJointAngle, jointInfos[targetJointName].limitLo);
- targetJointAngle = std::min<float>(targetJointAngle, jointInfos[targetJointName].limitHi);
+ iterJoints->second.angle = targetJointAngle;
+ actualJointAngles[targetJointName] = iterJoints->second.angle;
}
+ else
+ {
+ PIDControllerPtr pid = positionControlPIDController[it->first];
+ if (pid)
+ {
+ pid->reset();
+ if (jointInfo.continuousJoint())
+ {
+ float delta = targetJointAngle - iterJoints->second.angle;
+ float signedSmallestDelta = atan2(sin(delta), cos(delta));
+ targetJointAngle = iterJoints->second.angle + signedSmallestDelta;
+ }
+
+ pid->setTarget(targetJointAngle);
- iterJoints->second.angle = targetJointAngle;
- actualJointAngles[targetJointName] = iterJoints->second.angle;
+
+ }
+ }
}
else
{
@@ -362,7 +424,6 @@ bool mapEntryEqualsString(std::pair<std::string, KinematicUnitSimulationJointSta
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
KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
@@ -374,7 +435,6 @@ void KinematicUnitSimulation::requestJoints(const StringSequence& joints, const
void KinematicUnitSimulation::releaseJoints(const StringSequence& joints, const Ice::Current& c)
{
- ARMARX_INFO << flush;
// if one of the joints belongs to this unit, unlock access
KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index 57e3c9f4cc9cee0dde3ab5cdd621aa6e2bc99f5f..87cea5bde4109336bcaab519e72ac9be59854e6a 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -29,6 +29,7 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/libraries/core/PIDController.h>
#include <random>
#include <IceUtil/Time.h>
@@ -93,6 +94,10 @@ namespace armarx
{
return (limitLo != limitHi);
}
+ bool continuousJoint() const
+ {
+ return !hasLimits() || limitLo - limitHi >= 360.0f;
+ }
float limitLo;
float limitHi;
@@ -111,7 +116,12 @@ namespace armarx
KinematicUnitPropertyDefinitions(prefix)
{
defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
- defineOptionalProperty<float>("Noise", 0.0f, "Noise is added in velocity mode with (rand()/RAND_MAX-0.5)*2*Noise. Value in Degree");
+ defineOptionalProperty<float>("Noise", 0.0f, "Gaussian noise is added to the velocity. Value in Degree");
+ defineOptionalProperty<bool>("UsePDControllerForJointControl", true, "If true a PD controller is also used in Position Mode instead of setting the joint angles instantly");
+ defineOptionalProperty<float>("Kp", 3, "proportional gain of the PID position controller.");
+ defineOptionalProperty<float>("Ki", 0.001, "integral gain of the PID position controller.");
+ defineOptionalProperty<float>("Kd", 0.0, "derivative gain of the PID position controller.");
+
}
};
@@ -173,7 +183,8 @@ namespace armarx
int intervalMs;
std::normal_distribution<double> distribution;
std::default_random_engine generator;
-
+ bool usePDControllerForPosMode;
+ std::map<std::string, PIDControllerPtr> positionControlPIDController;
PeriodicTask<KinematicUnitSimulation>::pointer_type simulationTask;
};
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 7b0c9430deac8c04dfabdbef65b08c062c9608f4..b613a04d73437d884c1e2b69d2e72036668a5afa 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -75,8 +75,8 @@ namespace armarx
//remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
robotName = robotStateComponentPrx->getRobotName();
- localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
- jointExistenceCheckRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
+ localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx, robotStateComponentPrx->getRobotFilename(), robotStateComponentPrx->getArmarXPackages());
+ jointExistenceCheckRobot = RemoteRobot::createLocalClone(robotStateComponentPrx, robotStateComponentPrx->getRobotFilename(), robotStateComponentPrx->getArmarXPackages());
localReportRobot = localRobot->clone(localRobot->getName());
@@ -114,7 +114,7 @@ namespace armarx
}
else
{
- ARMARX_ERROR << "Could not find node set with name: " << name;
+ ARMARX_ERROR << "Could not find node with name: " << name;
}
}
}
@@ -493,50 +493,10 @@ namespace armarx
dIK->computeSteps(jointDelta, 0.8f, 0.001, 1, &EDifferentialIK::computeStep); // 1.0, 0.00001, 50
// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
- MatrixXf fullJac = dIK->calcFullJacobian();
- MatrixXf fullJacInv = dIK->computePseudoInverseJacobianMatrix(fullJac);
- Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet, fullJac, fullJacInv);
-
- // for debugging only!
- /*{
- MatrixXf posJac = fullJac.block(0, 0, fullJac.rows(), 3);
- MatrixXf oriJac = fullJac.block(0, 3, fullJac.rows(), 3);
- JacobiSVD<MatrixXf> svdPos(posJac);
- JacobiSVD<MatrixXf> svdOri(oriJac);
- if (svdPos.singularValues()[2] < 90)
- {
- ARMARX_IMPORTANT << deactivateSpam() << "The robot is close to a singularity! Minimal singular value of the position Jacobian: " << svdPos.singularValues()[2];
- }
- if (svdOri.singularValues()[2] < 0.6)
- {
- ARMARX_IMPORTANT << deactivateSpam() << "The robot is close to a singularity! Minimal singular value of the orientation Jacobian: " << svdOri.singularValues()[2];
- }
- }*/
-
- // added by David S
- const float maxJointLimitCompensationRatio = 1.0f;
-
- // if (jointLimitCompensation(0) == jointLimitCompensation(0))
- // {
- // // if (jointLimitCompensation.norm() > maxJointLimitCompensationRatio * jointDelta.norm())
- // // {
- // // jointLimitCompensation = maxJointLimitCompensationRatio * jointDelta.norm() / jointLimitCompensation.norm() * jointLimitCompensation;
- // // }
-
- // jointDelta += jointLimitCompensation;
- // }
-
jointDelta /= (cycleTime * 0.001);
jointDelta = applyMaxJointVelocity(jointDelta, maxJointVelocity);
- // Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
- // const Eigen::Matrix4f mat = robotNodeSet->getTCP()->getGlobalPose() * lastTCPPose.inverse();
- // const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
- // Eigen::Vector3f rpy;
- // VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
- // ARMARX_DEBUG << deactivateSpam(2) << "Current TCP Position: \n" << currentTCPPosition;
- // ARMARX_VERBOSE << deactivateSpam(2)<< "ActualTCPVelocity: " << (rpy/(0.001*cycleTime));
lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
// build name value map
@@ -1366,7 +1326,6 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
const std::string& tcpName = node->getName();
const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robotName);
-
}
// ARMARX_DEBUG << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index eeea47731b80b4c4108789f331f9d877ee789805..14bbb6cd3bab106b126b4a7c5717fcd9ba288e27 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -48,7 +48,6 @@ namespace armarx
defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit Proxy");
defineOptionalProperty<std::string>("RobotStateTopicName", "RobotState", "Name of the RobotComponent State topic.");
defineOptionalProperty<float>("MaxJointVelocity", 30.f / 180 * 3.141, "Maximal joint velocity in rad/sec");
- defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
// defineOptionalProperty<float>("MaximumCommandDelay", 20000, "Delay after which the TCP Control unit releases itself if no new velocity have been set.");
defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.h b/source/RobotAPI/components/units/TCPControlUnitObserver.h
index e3903b39e30d60d91a30acaea2d75e245f5e824f..263833c7e264755af68cd9a649774fda46edfe80 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.h
@@ -38,7 +38,7 @@ namespace armarx
TCPControlUnitObserverPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("TCPControlUnitName", "Name of the TCPControlUnit");
+ defineOptionalProperty<std::string>("TCPControlUnitName", "TCPControlUnit", "Name of the TCPControlUnit");
}
};
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index 423ec3285d801635b8d00309258cebcc9683a053..408c06b2dffd5bbd0eb9e31b583e4a0640b4bd37 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -7,3 +7,5 @@ add_subdirectory(RobotViewerPlugin)
+
+add_subdirectory(DebugDrawerViewer)
\ No newline at end of file
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/CMakeLists.txt b/source/RobotAPI/gui-plugins/DebugDrawerViewer/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5842e4fbd8f0fc57c02dbd315864c9c20db1c9bd
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/CMakeLists.txt
@@ -0,0 +1,40 @@
+
+armarx_set_target("DebugDrawerViewerGuiPlugin")
+
+find_package(Qt4 COMPONENTS QtCore QtGui QtDesigner)
+
+armarx_build_if(QT_FOUND "Qt not available")
+# ArmarXGui gets included through depends_on_armarx_package(ArmarXGui "OPTIONAL")
+# in the toplevel CMakeLists.txt
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+if(QT_FOUND)
+ include(${QT_USE_FILE})
+endif()
+
+set(SOURCES
+./DebugDrawerViewerGuiPlugin.cpp ./DebugDrawerViewerWidgetController.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.cpp @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.cpp
+)
+
+set(HEADERS
+./DebugDrawerViewerGuiPlugin.h ./DebugDrawerViewerWidgetController.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.h @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.h
+)
+
+set(GUI_MOC_HDRS ${HEADERS})
+
+set(GUI_UIS
+./DebugDrawerViewerWidget.ui
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@Widget.ui
+)
+
+# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
+set(COMPONENT_LIBS DebugDrawer ${QT_LIBRARIES})
+
+if(ArmarXGui_FOUND)
+ armarx_gui_library(DebugDrawerViewerGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..741dd98a26bc14b24e45ae236c1d82d858357a63
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp
@@ -0,0 +1,34 @@
+/*
+ * 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 version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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 RobotAPI::gui-plugins::DebugDrawerViewerGuiPlugin
+ * \author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * \date 2016
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DebugDrawerViewerGuiPlugin.h"
+
+#include "DebugDrawerViewerWidgetController.h"
+
+using namespace armarx;
+
+DebugDrawerViewerGuiPlugin::DebugDrawerViewerGuiPlugin()
+{
+ addWidget < DebugDrawerViewerWidgetController > ();
+}
+
+Q_EXPORT_PLUGIN2(armarx_gui_DebugDrawerViewerGuiPlugin, DebugDrawerViewerGuiPlugin)
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..14d7959a43ad66ecac5223c5aa8b5bd6021a6ae8
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.h
@@ -0,0 +1,51 @@
+/*
+ * 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 version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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 RobotAPI::gui-plugins::DebugDrawerViewer
+ * \author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * \date 2016
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_RobotAPI_DebugDrawerViewer_GuiPlugin_H
+#define _ARMARX_RobotAPI_DebugDrawerViewer_GuiPlugin_H
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+namespace armarx
+{
+ /**
+ * \class DebugDrawerViewerGuiPlugin
+ * \ingroup ArmarXGuiPlugins
+ * \brief DebugDrawerViewerGuiPlugin brief description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT DebugDrawerViewerGuiPlugin:
+ public armarx::ArmarXGuiPlugin
+ {
+ public:
+ /**
+ * All widgets exposed by this plugin are added in the constructor
+ * via calls to addWidget()
+ */
+ DebugDrawerViewerGuiPlugin();
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidget.ui b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..9888ab71f5d6e6b6bdfdaa80a74c4f609e37e6cd
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidget.ui
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DebugDrawerViewerWidget</class>
+ <widget class="QWidget" name="DebugDrawerViewerWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>400</width>
+ <height>300</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>DebugDrawerViewerWidget</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QPushButton" name="btnClearAll">
+ <property name="text">
+ <string>Remove All Visualizations</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..df7c61710429535666e6a630e38125b830829c57
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
@@ -0,0 +1,116 @@
+/*
+ * 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 version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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 RobotAPI::gui-plugins::DebugDrawerViewerWidgetController
+ * \author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * \date 2016
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DebugDrawerViewerWidgetController.h"
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <string>
+
+using namespace armarx;
+
+DebugDrawerViewerWidgetController::DebugDrawerViewerWidgetController()
+{
+ rootVisu = NULL;
+ widget.setupUi(getWidget());
+
+}
+
+
+DebugDrawerViewerWidgetController::~DebugDrawerViewerWidgetController()
+{
+
+}
+
+
+void DebugDrawerViewerWidgetController::loadSettings(QSettings* settings)
+{
+
+}
+
+void DebugDrawerViewerWidgetController::saveSettings(QSettings* settings)
+{
+
+}
+
+
+void DebugDrawerViewerWidgetController::onInitComponent()
+{
+ rootVisu = new SoSeparator;
+ rootVisu->ref();
+
+
+ enableMainWidgetAsync(false);
+ connect(widget.btnClearAll, SIGNAL(clicked()), this, SLOT(on_btnClearAll_clicked()), Qt::UniqueConnection);
+}
+
+
+void DebugDrawerViewerWidgetController::onConnectComponent()
+{
+ // create the debugdrawer component
+ std::string debugDrawerComponentName = "GUIDebugDrawer_" + getName();
+ ARMARX_INFO << "Creating component " << debugDrawerComponentName;
+ debugDrawer = Component::create<DebugDrawerComponent>(properties, debugDrawerComponentName);
+
+ if (mutex3D)
+ {
+ //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
+ debugDrawer->setMutex(mutex3D);
+ }
+ else
+ {
+ ARMARX_ERROR << " No 3d mutex available...";
+ }
+
+ ArmarXManagerPtr m = getArmarXManager();
+ m->addObject(debugDrawer, false);
+
+ {
+ boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ rootVisu->addChild(debugDrawer->getVisualization());
+ }
+ enableMainWidgetAsync(true);
+}
+
+
+SoNode *DebugDrawerViewerWidgetController::getScene()
+{
+ return rootVisu;
+}
+
+
+void armarx::DebugDrawerViewerWidgetController::onExitComponent()
+{
+ if (rootVisu)
+ {
+ rootVisu->removeAllChildren();
+ rootVisu->unref();
+ rootVisu = NULL;
+ }
+}
+
+void armarx::DebugDrawerViewerWidgetController::on_btnClearAll_clicked()
+{
+ ARMARX_INFO << "Clearing visu";
+ if(debugDrawer)
+ {
+ debugDrawer->clearAll();
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h
new file mode 100644
index 0000000000000000000000000000000000000000..dc43ee119a2afeddbfbfed7acaf9c72ddd0090dc
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h
@@ -0,0 +1,127 @@
+/*
+ * 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 version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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 RobotAPI::gui-plugins::DebugDrawerViewerWidgetController
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2016
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_RobotAPI_DebugDrawerViewer_WidgetController_H
+#define _ARMARX_RobotAPI_DebugDrawerViewer_WidgetController_H
+
+#include "ui_DebugDrawerViewerWidget.h"
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+namespace armarx
+{
+ /**
+ \page ArmarXGui-GuiPlugins-DebugDrawerViewer DebugDrawerViewer
+ \brief The DebugDrawerViewer listens on the DebugDrawer topic and visualizes all incoming data.
+ This GUI only displays the DebugDrawer content and nothing else.
+ The content is shown in the 3D Viewer Widget.
+
+ API Documentation: \ref DebugDrawerViewerWidgetController
+
+ \see DebugDrawerViewerGuiPlugin
+ */
+
+ /**
+ * \class DebugDrawerViewerWidgetController
+ * \brief DebugDrawerViewerWidgetController
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT
+ DebugDrawerViewerWidgetController:
+ public armarx::ArmarXComponentWidgetController
+ {
+ Q_OBJECT
+
+ public:
+ /**
+ * Controller Constructor
+ */
+ explicit DebugDrawerViewerWidgetController();
+
+ /**
+ * Controller destructor
+ */
+ virtual ~DebugDrawerViewerWidgetController();
+
+ /**
+ * @see ArmarXWidgetController::loadSettings()
+ */
+ virtual void loadSettings(QSettings* settings);
+
+ /**
+ * @see ArmarXWidgetController::saveSettings()
+ */
+ virtual void saveSettings(QSettings* settings);
+
+ /**
+ * Returns the Widget name displayed in the ArmarXGui to create an
+ * instance of this class.
+ */
+ virtual QString getWidgetName() const
+ {
+ return "Visualization.DebugDrawerViewer";
+ }
+
+ /**
+ * \see armarx::Component::onInitComponent()
+ */
+ virtual void onInitComponent();
+
+ /**
+ * \see armarx::Component::onConnectComponent()
+ */
+ virtual void onConnectComponent();
+
+ void onExitComponent();
+
+ public slots:
+ /* QT slot declarations */
+
+ signals:
+ /* QT signal declarations */
+
+ private:
+ /**
+ * Widget Form
+ */
+ Ui::DebugDrawerViewerWidget widget;
+ armarx::DebugDrawerComponentPtr debugDrawer;
+ SoSeparator* rootVisu;
+
+ // ArmarXWidgetController interface
+ public:
+ SoNode* getScene();
+
+
+
+ private slots:
+ void on_btnClearAll_clicked();
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 72df5f416808f1a35d2fc1da762cf32bb2c35e09..f712c2ecd934d4f702c37c3c689b6ff4e2e7fdcc 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -940,14 +940,15 @@ QString KinematicUnitWidgetController::translateStatus(ErrorStatus status)
void KinematicUnitWidgetController::updateJointAnglesTable()
{
- if (!robotNodeSet)
- {
- return;
- }
+
float dirty_squaresum = 0;
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ if (!robotNodeSet)
+ {
+ return;
+ }
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
@@ -983,13 +984,17 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
void KinematicUnitWidgetController::updateJointVelocitiesTable()
{
- if (!getWidget() || !robotNodeSet)
+ if (!getWidget())
{
return;
}
float dirty_squaresum = 0;
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ if (!robotNodeSet)
+ {
+ return;
+ }
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
QTableWidgetItem* newItem;
@@ -1019,12 +1024,13 @@ void KinematicUnitWidgetController::updateJointVelocitiesTable()
void KinematicUnitWidgetController::updateJointTorquesTable()
{
+
+
+ boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (!getWidget() || !robotNodeSet)
{
return;
}
-
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
QTableWidgetItem* newItem;
NameValueMap::const_iterator it;
@@ -1046,12 +1052,13 @@ void KinematicUnitWidgetController::updateJointTorquesTable()
void KinematicUnitWidgetController::updateJointCurrentsTable()
{
+
+
+ boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (!getWidget() || !robotNodeSet)
{
return;
}
-
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
QTableWidgetItem* newItem;
NameValueMap::const_iterator it;
@@ -1073,12 +1080,13 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
void KinematicUnitWidgetController::updateJointMotorTemperaturesTable()
{
+
+ boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (!getWidget() || !robotNodeSet)
{
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
QTableWidgetItem* newItem;
NameValueMap::const_iterator it;
@@ -1198,13 +1206,14 @@ void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& joi
void KinematicUnitWidgetController::updateModel()
{
+
+
+ // ARMARX_INFO << "updateModel()" << flush;
+ boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (!robotNodeSet)
{
return;
}
-
- // ARMARX_INFO << "updateModel()" << flush;
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn2 = robotNodeSet->getAllRobotNodes();
std::vector< RobotNodePtr > usedNodes;
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
index bc2a8c015a2f1537fdc15d620a1f47ff3348f1d1..3bc6d5fb9598f06b3178c078eab9c16ec54649de 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
@@ -99,11 +99,12 @@ namespace armarx
virtual void onDisconnectComponent();
virtual void onExitComponent();
- // inherited of ArmarXWidget
+ // inherited from ArmarXWidget
virtual QString getWidgetName() const
{
- return "RobotControl.RobotViewerGUI";
+ return "Visualization.RobotViewerGUI";
}
+
QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
virtual void loadSettings(QSettings* settings);
virtual void saveSettings(QSettings* settings);
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 250737dce1d6b850218f1c45c00e927c049b46fc..1bd39e90ff1fa82748c5057ca0842cc84a44361a 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -36,6 +36,9 @@ set(SLICE_FILES
units/UnitInterface.ice
units/ATINetFTUnit.ice
+
+ components/ViewSelectionInterface.ice
+
visualization/DebugDrawerInterface.ice
)
#core/RobotIK.ice
diff --git a/source/RobotAPI/interface/components/ViewSelectionInterface.ice b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
new file mode 100755
index 0000000000000000000000000000000000000000..7521a30c834262116fbeaa8f6f13d0ce9e90d2c6
--- /dev/null
+++ b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
@@ -0,0 +1,75 @@
+/**
+* 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 version 2 as
+* published by the Free Software Foundation.
+*
+* 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 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 RobotComponents
+* @author David Schiebener (schiebener at kit dot edu)
+* @author Markus Grotz ( markus dot grotz at kit dot edu )
+* @copyright 2015 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ROBOTCOMPONENTS_COMPONENT_ViewSelection_SLICE_
+#define _ROBOTCOMPONENTS_COMPONENT_ViewSelection_SLICE_
+
+#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+
+module armarx
+{
+
+ const int DEFAULT_VIEWTARGET_PRIORITY = 50;
+
+
+ ["cpp:virtual"]
+ class ViewTargetBase
+ {
+ armarx::FramedPositionBase target;
+
+ TimestampBase validUntil;
+
+ TimestampBase timeAdded;
+
+ float duration;
+
+ int priority;
+
+ string source;
+ };
+
+ sequence<ViewTargetBase> ViewTargetList;
+
+ interface ViewSelectionInterface
+ {
+ void addManualViewTarget(armarx::FramedPositionBase target);
+ void clearManualViewTargets();
+ ViewTargetList getManualViewTargets();
+
+ void activateAutomaticViewSelection();
+ void deactivateAutomaticViewSelection();
+
+ void updateSaliencyMap(string name, FloatSequence map);
+ };
+
+
+};
+
+#endif
+
+
+
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 7a82a5d6a5722b11b06568c33b28483548078aa7..441fb1177b43c79308575dc3a1bdea8072216b95 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -66,6 +66,19 @@ void PIDController::update(double measuredValue, double targetValue)
lastUpdateTime = now;
}
+
+void PIDController::update(double measuredValue)
+{
+ ScopedRecursiveLock lock(mutex);
+ update(measuredValue, target);
+}
+
+void PIDController::setTarget(double newTarget)
+{
+ ScopedRecursiveLock lock(mutex);
+ target = newTarget;
+}
+
void PIDController::update(double deltaSec, double measuredValue, double targetValue)
{
ScopedRecursiveLock lock(mutex);
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index c1c1529a4202cb6436720416222ff9d119c071fc..8e9578b0257259ce74a2fa8944ede3c75b13abed 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -38,6 +38,8 @@ namespace armarx
PIDController(float Kp, float Ki, float Kd);
void update(double deltaSec, double measuredValue, double targetValue);
void update(double measuredValue, double targetValue);
+ void update(double measuredValue);
+ void setTarget(double newTarget);
double getControlValue() const;
void reset();
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index f640b6f7d5ba4c2d724271d1d967fe966f102e70..ba63534b35522ddf271cbc53398d85c4a751284a 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -8,6 +8,8 @@
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
#include <Eigen/Geometry>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj << endl;
#define DMES(Obj) ;
@@ -325,16 +327,18 @@ namespace armarx
return robo;
}
- VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename)
+ VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename, const Ice::StringSeq packages, VirtualRobot::RobotIO::RobotDescription loadMode)
{
- return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling());
+ return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling(), packages, loadMode);
}
- RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename, float scaling)
+ RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename, float scaling, const Ice::StringSeq packages, VirtualRobot::RobotIO::RobotDescription loadMode)
{
+ RobotPtr result;
+
boost::recursive_mutex::scoped_lock cloneLock(m);
ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
- VirtualRobot::RobotPtr result;
+
if (!sharedRobotPrx)
{
@@ -355,25 +359,59 @@ namespace armarx
}
else
{
- result = RobotIO::loadRobot(filename);
+ StringList includePaths;
+ for (const std::string & projectName : packages)
+ {
+ if (projectName.empty())
+ {
+ continue;
+ }
- if (!result)
+ CMakePackageFinder project(projectName);
+ StringList projectIncludePaths;
+ auto pathsString = project.getDataDir();
+ ARMARX_DEBUG_S << "Data paths of ArmarX package " << projectName << ": " << pathsString;
+ boost::split(projectIncludePaths,
+ pathsString,
+ boost::is_any_of(";,"),
+ boost::token_compress_on);
+ ARMARX_DEBUG_S << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
+ includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+
+ }
+
+
+ if (!ArmarXDataPath::getAbsolutePath(filename, filename, includePaths))
{
- ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
+ ARMARX_ERROR_S << "Could find robot file " << filename;
return result;
}
+ result = RobotIO::loadRobot(filename, loadMode);
- if (scaling != 1.0f)
+ if (!result)
{
- ARMARX_INFO_S << "Scaling robot to " << scaling;
- result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
+ ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
+ return result;
}
}
+ if (result && scaling != 1.0f)
+ {
+ ARMARX_INFO_S << "Scaling robot to " << scaling;
+ result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
+ }
+
synchronizeLocalClone(result, sharedRobotPrx);
return result;
}
+ RobotPtr RemoteRobot::createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, RobotIO::RobotDescription loadMode)
+ {
+ return createLocalClone(robotStatePrx, robotStatePrx->getRobotFilename(), robotStatePrx->getArmarXPackages(), loadMode);
+ }
+
+
+
bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
{
return synchronizeLocalClone(robot, robotStatePrx->getSynchronizedRobot());
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index eaab284f832c140d4b594ee597e86e7ae0fd6969..6d9b69c8a491b4d18acadc744935e26c4c1e8816 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -28,6 +28,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
@@ -179,14 +180,27 @@ namespace armarx
/*!
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.
+ Note that only the kinematic structure is cloned, but the visualization files, collision models, 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).
+ In the packages parameter you can pass in ArmarX packages, in which the robot file model might be in.
+ The loadMode specifies in which mode the model should be loaded. Refer to simox for more information (only matters if filename was passed in).
+ @see createLocalCloneFromFile(), synchronizeLocalClone()
*/
- static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string());
+ static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string(), const Ice::StringSeq packages = Ice::StringSeq(), VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
+
+ static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename = std::string(), float scaling = 1.0f, const Ice::StringSeq packages = Ice::StringSeq(), VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
+
+ /**
+ * @brief This is a convenience function for createLocalClone, which automatically get the filename from the RobotStateComponent, loads robot from the file and syncs it once.
+ * @param robotStatePrx proxy to the RobotStateComponent
+ * @param loadMode he loadMode specifies in which mode the model should be loaded. Refer to simox for more information.
+ * @return new robot clone
+ * @see createLocalClone(), synchronizeLocalClone()
+ */
+ static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
- static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string(), float scaling = 1.0f);
/*!
Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.