diff --git a/data/RobotAPI/robots/MMM/mmm.xml b/data/RobotAPI/robots/MMM/mmm.xml
index 862cd928ceb24b45ead631aa46099b48f836d8d0..c386d1b81a4a3863c0744446e73655c88c1384aa 100644
--- a/data/RobotAPI/robots/MMM/mmm.xml
+++ b/data/RobotAPI/robots/MMM/mmm.xml
@@ -12,11 +12,11 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='1'/>
- <limits lo='0' hi='22' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='1' />
+ <limits lo='0' hi='22' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Sensor type='position' name='MARKER_C7'>
<Transform>
@@ -25,7 +25,7 @@
<row2 c1='0' c2='1' c3='0' c4='-38.9454' />
<row3 c1='0' c2='0' c3='1' c4='-0.688374' />
<row4 c1='0' c2='0' c3='0' c4='1' />
- </Matrix4x4>BLNsegment_joint
+ </Matrix4x4>
</Transform>
</Sensor>
<Sensor type='position' name='MARKER_CLAV'>
@@ -38,53 +38,53 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='BLNx_joint'/>
+ <Child name='BLNx_joint' />
</RobotNode>
<RobotNode name='BLNx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.785398' hi='0.261799' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.785398' hi='0.261799' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='BLNz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='BLNz_joint' />
</RobotNode>
<RobotNode name='BLNy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='BUNsegment_joint'/>
+ <Child name='BUNsegment_joint' />
</RobotNode>
<RobotNode name='BLNz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.261799' hi='0.261799' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.261799' hi='0.261799' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='BLNy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='BLNy_joint' />
</RobotNode>
@@ -98,19 +98,19 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='1'/>
- <limits lo='0' hi='40' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='1' />
+ <limits lo='0' hi='40' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.11'/>
- <CoM location='Custom' x='0' y='3.33729e-08' z='-35.8'/>
+ <Mass unit='kg' value='0.11' />
+ <CoM location='Custom' x='0' y='3.33729e-08' z='-35.8' />
<InertiaMatrix>
- <row1 c1='0.000454' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0003637' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0004191'/>
+ <row1 c1='0.000454' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0003637' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0004191' />
</InertiaMatrix>
</Physics>
<Sensor type='position' name='MARKER_L3'>
@@ -123,53 +123,53 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='BPx_joint'/>
+ <Child name='BPx_joint' />
</RobotNode>
<RobotNode name='BPx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.872665' hi='0.610865' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.872665' hi='0.610865' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='BPz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='BPz_joint' />
</RobotNode>
<RobotNode name='BPy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='BTSegment_joint'/>
+ <Child name='BTSegment_joint' />
</RobotNode>
<RobotNode name='BPz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='BPy_joint'/>
+ <CollisionModel>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='BPy_joint' />
</RobotNode>
@@ -183,76 +183,77 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='1'/>
- <limits lo='0' hi='60' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='1' />
+ <limits lo='0' hi='60' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.1'/>
- <CoM location='Custom' x='0' y='3.33729e-08' z='-33.6'/>
+ <Mass unit='kg' value='0.1' />
+ <CoM location='Custom' x='0' y='3.33729e-08' z='-33.6' />
<InertiaMatrix>
- <row1 c1='1.024' c2='0' c3='0'/>
- <row2 c1='0' c2='0.81796' c3='0'/>
- <row3 c1='0' c2='0' c3='0.676'/>
+ <row1 c1='1.024' c2='0' c3='0' />
+ <row2 c1='0' c2='0.81796' c3='0' />
+ <row3 c1='0' c2='0' c3='0.676' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/BTSegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='BTx_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='BTx_joint' />
</RobotNode>
<RobotNode name='BTx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.610865' hi='0.471239' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.610865' hi='0.471239' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='BTz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='BTz_joint' />
</RobotNode>
<RobotNode name='BTy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='collarSegment_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='collarSegment_joint' />
</RobotNode>
<RobotNode name='BTz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.628319' hi='0.628319' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.628319' hi='0.628319' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='BTy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='BTy_joint' />
</RobotNode>
@@ -266,72 +267,73 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='1'/>
- <limits lo='0' hi='30' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='1' />
+ <limits lo='0' hi='30' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.024'/>
- <CoM location='Custom' x='0' y='3.33729e-08' z='-15'/>
+ <Mass unit='kg' value='0.024' />
+ <CoM location='Custom' x='0' y='3.33729e-08' z='-15' />
<InertiaMatrix>
- <row1 c1='0.0287585' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0287585' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0139392'/>
+ <row1 c1='0.0287585' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0287585' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0139392' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/BUNsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='BUNx_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='BUNx_joint' />
</RobotNode>
<RobotNode name='BUNx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.349066' hi='0.523599' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.349066' hi='0.523599' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='BUNz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='BUNz_joint' />
</RobotNode>
<RobotNode name='BUNy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.261799' hi='0.261799' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.261799' hi='0.261799' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='MidHeadSegment_joint'/>
- <Child name='HeadSegment_joint'/>
+ <Child name='MidHeadSegment_joint' />
+ <Child name='HeadSegment_joint' />
</RobotNode>
<RobotNode name='BUNz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='BUNy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='BUNy_joint' />
</RobotNode>
@@ -345,24 +347,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='1'/>
- <limits lo='0' hi='130' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='1' />
+ <limits lo='0' hi='130' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.07'/>
- <CoM location='Custom' x='0' y='3.33729e-08' z='-65'/>
+ <Mass unit='kg' value='0.07' />
+ <CoM location='Custom' x='0' y='3.33729e-08' z='-65' />
<InertiaMatrix>
- <row1 c1='0.612157' c2='0' c3='0'/>
- <row2 c1='0' c2='0.5733' c3='0'/>
- <row3 c1='0' c2='0' c3='0.430612'/>
+ <row1 c1='0.612157' c2='0' c3='0' />
+ <row2 c1='0' c2='0.5733' c3='0' />
+ <row3 c1='0' c2='0' c3='0.430612' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/HeadSegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LBHD'>
<Transform>
<Matrix4x4 units='mm'>
@@ -416,24 +419,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='246' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='246' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.04'/>
- <CoM location='Custom' x='-2.75474e-12' y='1.04413e-05' z='139.5'/>
+ <Mass unit='kg' value='0.04' />
+ <CoM location='Custom' x='-2.75474e-12' y='1.04413e-05' z='139.5' />
<InertiaMatrix>
- <row1 c1='0.237419' c2='0' c3='0'/>
- <row2 c1='0' c2='0.256481' c3='0'/>
- <row3 c1='0' c2='0' c3='0.040572'/>
+ <row1 c1='0.237419' c2='0' c3='0' />
+ <row2 c1='0' c2='0.256481' c3='0' />
+ <row3 c1='0' c2='0' c3='0.040572' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/LAsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LTIP'>
<Transform>
<Matrix4x4 units='mm'>
@@ -444,38 +448,39 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LAx_joint'/>
+ <Child name='LAx_joint' />
</RobotNode>
<RobotNode name='LAx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.698132' hi='0.523599' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.698132' hi='0.523599' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LAz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LAz_joint' />
</RobotNode>
<RobotNode name='LAy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LAy_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LANK'>
<Transform>
<Matrix4x4 units='mm'>
@@ -496,25 +501,25 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LeftFootHeight_joint'/>
- <Child name='LMrot_joint'/>
+ <Child name='LeftFootHeight_joint' />
+ <Child name='LMrot_joint' />
</RobotNode>
<RobotNode name='LAz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.523599' hi='0.523599' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.523599' hi='0.523599' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LAy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LAy_joint' />
</RobotNode>
@@ -528,24 +533,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='188' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='188' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.027'/>
- <CoM location='Custom' x='-8.25728e-12' y='3.81383e-05' z='106'/>
+ <Mass unit='kg' value='0.027' />
+ <CoM location='Custom' x='-8.25728e-12' y='3.81383e-05' z='106' />
<InertiaMatrix>
- <row1 c1='0.0837755' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0940772' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0287506'/>
+ <row1 c1='0.0837755' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0940772' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0287506' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/LEsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LAEL'>
<Transform>
<Matrix4x4 units='mm'>
@@ -576,36 +582,36 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LEx_joint'/>
+ <Child name='LEx_joint' />
</RobotNode>
<RobotNode name='LEx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='2.79253' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='2.79253' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LEz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LEz_joint' />
</RobotNode>
<RobotNode name='LEz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-1.5708' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-1.5708' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LWsegment_joint'/>
+ <Child name='LWsegment_joint' />
</RobotNode>
@@ -619,15 +625,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.785398' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.785398' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LFx_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LTOE'>
<Transform>
<Matrix4x4 units='mm'>
@@ -651,15 +658,16 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='-1' y='0' z='0'/>
- <limits lo='0' hi='52' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='-1' y='0' z='0' />
+ <limits lo='0' hi='52' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LHsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LASI'>
<Transform>
<Matrix4x4 units='mm'>
@@ -690,53 +698,53 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LHx_joint'/>
+ <Child name='LHx_joint' />
</RobotNode>
<RobotNode name='LHx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.872665' hi='1.65806' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.872665' hi='1.65806' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LHz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LHz_joint' />
</RobotNode>
<RobotNode name='LHy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.349066' hi='1.13446' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.349066' hi='1.13446' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LKsegment_joint'/>
+ <Child name='LKsegment_joint' />
</RobotNode>
<RobotNode name='LHz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LHy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LHy_joint' />
</RobotNode>
@@ -750,24 +758,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='245' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='245' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.14'/>
- <CoM location='Custom' x='-2.75474e-12' y='1.04413e-05' z='121.6'/>
+ <Mass unit='kg' value='0.14' />
+ <CoM location='Custom' x='-2.75474e-12' y='1.04413e-05' z='121.6' />
<InertiaMatrix>
- <row1 c1='3.0625' c2='0' c3='0'/>
- <row2 c1='0' c2='3.0625' c3='0'/>
- <row3 c1='0' c2='0' c3='0.636804'/>
+ <row1 c1='3.0625' c2='0' c3='0' />
+ <row2 c1='0' c2='3.0625' c3='0' />
+ <row3 c1='0' c2='0' c3='0.636804' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/LKsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LKNE'>
<Transform>
<Matrix4x4 units='mm'>
@@ -788,19 +797,19 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LKx_joint'/>
+ <Child name='LKx_joint' />
</RobotNode>
<RobotNode name='LKx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-2.26893' hi='0' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-2.26893' hi='0' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LAsegment_joint'/>
+ <Child name='LAsegment_joint' />
</RobotNode>
@@ -814,15 +823,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.261799' hi='0.261799' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.261799' hi='0.261799' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LMrot_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LMT1'>
<Transform>
<Matrix4x4 units='mm'>
@@ -843,7 +853,7 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LFx_joint'/>
+ <Child name='LFx_joint' />
</RobotNode>
@@ -857,15 +867,16 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='-1' y='0' z='0'/>
- <limits lo='0' hi='87' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='-1' y='0' z='0' />
+ <limits lo='0' hi='87' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LSCsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LBAK'>
<Transform>
<Matrix4x4 units='mm'>
@@ -876,53 +887,53 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LSCx_joint'/>
+ <Child name='LSCx_joint' />
</RobotNode>
<RobotNode name='LSCx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='0' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='0' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LSCz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LSCz_joint' />
</RobotNode>
<RobotNode name='LSCy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LSsegment_joint'/>
+ <Child name='LSsegment_joint' />
</RobotNode>
<RobotNode name='LSCz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LSCy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LSCy_joint' />
</RobotNode>
@@ -936,24 +947,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='-1' y='0' z='0'/>
- <limits lo='0' hi='23' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='-1' y='0' z='0' />
+ <limits lo='0' hi='23' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.021'/>
- <CoM location='Custom' x='10' y='4.16334e-14' z='0'/>
+ <Mass unit='kg' value='0.021' />
+ <CoM location='Custom' x='10' y='4.16334e-14' z='0' />
<InertiaMatrix>
- <row1 c1='0.0298116' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0063504' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0298116'/>
+ <row1 c1='0.0298116' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0063504' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0298116' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/LSsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LSHO'>
<Transform>
<Matrix4x4 units='mm'>
@@ -964,53 +976,53 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LSx_joint'/>
+ <Child name='LSx_joint' />
</RobotNode>
<RobotNode name='LSx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-1.22173' hi='3.31613' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-1.22173' hi='3.31613' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LSz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LSz_joint' />
</RobotNode>
<RobotNode name='LSy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='0' hi='2.79253' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='0' hi='2.79253' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LEsegment_joint'/>
+ <Child name='LEsegment_joint' />
</RobotNode>
<RobotNode name='LSz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-1.22173' hi='1.0472' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-1.22173' hi='1.0472' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LSy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LSy_joint' />
</RobotNode>
@@ -1024,24 +1036,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='145' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='145' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.016'/>
- <CoM location='Custom' x='-8.25728e-12' y='3.81383e-05' z='82.6'/>
+ <Mass unit='kg' value='0.016' />
+ <CoM location='Custom' x='-8.25728e-12' y='3.81383e-05' z='82.6' />
<InertiaMatrix>
- <row1 c1='0.0319821' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0340787' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0065229'/>
+ <row1 c1='0.0319821' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0340787' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0065229' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/LWsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LFRA'>
<Transform>
<Matrix4x4 units='mm'>
@@ -1072,44 +1085,45 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LWx_joint'/>
+ <Child name='LWx_joint' />
</RobotNode>
<RobotNode name='LWx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.523599' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.523599' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='LWy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='LWy_joint' />
</RobotNode>
<RobotNode name='LWy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-1.22173' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-1.22173' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LWy_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftHandSegment_joint'/>
- <Child name='LeftFingerJoint50y_joint'/>
- <Child name='LeftFingerJoint31x_joint'/>
- <Child name='LeftFingerJoint21x_joint'/>
- <Child name='LeftFingerJoint11z_joint'/>
- <Child name='LeftFingerJoint40y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftHandSegment_joint' />
+ <Child name='LeftFingerJoint50y_joint' />
+ <Child name='LeftFingerJoint31x_joint' />
+ <Child name='LeftFingerJoint21x_joint' />
+ <Child name='LeftFingerJoint11z_joint' />
+ <Child name='LeftFingerJoint40y_joint' />
</RobotNode>
@@ -1123,13 +1137,13 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='-1' y='0' z='0'/>
- <limits lo='0' hi='15' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='-1' y='0' z='0' />
+ <limits lo='0' hi='15' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LeftEyeSegmentY_joint'/>
+ <Child name='LeftEyeSegmentY_joint' />
</RobotNode>
@@ -1143,15 +1157,16 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='1' z='0'/>
- <limits lo='0' hi='30' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='1' z='0' />
+ <limits lo='0' hi='30' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftEyeSegmentY_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -1165,16 +1180,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint11y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint12y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint12y_joint' />
</RobotNode>
@@ -1188,13 +1204,13 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LeftFingerJoint11y_joint'/>
+ <Child name='LeftFingerJoint11y_joint' />
</RobotNode>
@@ -1208,16 +1224,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint12y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint13y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint13y_joint' />
</RobotNode>
@@ -1231,15 +1248,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint13y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -1253,28 +1271,29 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LeftFingerJoint21y_joint'/>
+ <Child name='LeftFingerJoint21y_joint' />
</RobotNode>
<RobotNode name='LeftFingerJoint21y_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint21y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint22y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint22y_joint' />
</RobotNode>
@@ -1288,16 +1307,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint22y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint23y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint23y_joint' />
</RobotNode>
@@ -1311,15 +1331,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint23y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -1333,28 +1354,29 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LeftFingerJoint31y_joint'/>
+ <Child name='LeftFingerJoint31y_joint' />
</RobotNode>
<RobotNode name='LeftFingerJoint31y_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint31y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint32y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint32y_joint' />
</RobotNode>
@@ -1368,16 +1390,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint32y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint33y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint33y_joint' />
</RobotNode>
@@ -1391,15 +1414,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint33y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -1413,16 +1437,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='0.0872665' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='0.0872665' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint40y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint41x_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint41x_joint' />
</RobotNode>
@@ -1436,28 +1461,29 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LeftFingerJoint41y_joint'/>
+ <Child name='LeftFingerJoint41y_joint' />
</RobotNode>
<RobotNode name='LeftFingerJoint41y_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint41y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint42y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint42y_joint' />
</RobotNode>
@@ -1471,16 +1497,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint42y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint43y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint43y_joint' />
</RobotNode>
@@ -1494,15 +1521,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint43y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -1516,16 +1544,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='0.174533' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='0.174533' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint50y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint51x_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint51x_joint' />
</RobotNode>
@@ -1539,28 +1568,29 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LeftFingerJoint51y_joint'/>
+ <Child name='LeftFingerJoint51y_joint' />
</RobotNode>
<RobotNode name='LeftFingerJoint51y_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint51y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint52y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint52y_joint' />
</RobotNode>
@@ -1574,16 +1604,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint52y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='LeftFingerJoint53y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='LeftFingerJoint53y_joint' />
</RobotNode>
@@ -1597,15 +1628,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/LeftFingerJoint53y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -1619,19 +1651,19 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='39' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='39' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.013'/>
- <CoM location='Custom' x='-8.35798e-08' y='25.225' z='19'/>
+ <Mass unit='kg' value='0.013' />
+ <CoM location='Custom' x='-8.35798e-08' y='25.225' z='19' />
<InertiaMatrix>
- <row1 c1='0.0036504' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0111794' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0096393'/>
+ <row1 c1='0.0036504' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0111794' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0096393' />
</InertiaMatrix>
</Physics>
</RobotNode>
@@ -1647,24 +1679,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='108' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='108' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.006'/>
- <CoM location='Custom' x='-8.25728e-12' y='3.81383e-05' z='53.4'/>
+ <Mass unit='kg' value='0.006' />
+ <CoM location='Custom' x='-8.25728e-12' y='3.81383e-05' z='53.4' />
<InertiaMatrix>
- <row1 c1='0.0021869' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0033304' c3='0'/>
- <row3 c1='0' c2='0' c3='0.001283'/>
+ <row1 c1='0.0021869' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0033304' c3='0' />
+ <row3 c1='0' c2='0' c3='0.001283' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/LeftHandSegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_LHPS'>
<Transform>
<Matrix4x4 units='mm'>
@@ -1708,14 +1741,14 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='1'/>
- <limits lo='0' hi='66' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='1' />
+ <limits lo='0' hi='66' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='LeftEyeSegmentX_joint'/>
- <Child name='RightEyeSegmentX_joint'/>
+ <Child name='LeftEyeSegmentX_joint' />
+ <Child name='RightEyeSegmentX_joint' />
</RobotNode>
@@ -1729,24 +1762,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='246' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='246' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.04'/>
- <CoM location='Custom' x='-3.88578e-13' y='1.04413e-05' z='139.5'/>
+ <Mass unit='kg' value='0.04' />
+ <CoM location='Custom' x='-3.88578e-13' y='1.04413e-05' z='139.5' />
<InertiaMatrix>
- <row1 c1='0.237419' c2='0' c3='0'/>
- <row2 c1='0' c2='0.256481' c3='0'/>
- <row3 c1='0' c2='0' c3='0.040572'/>
+ <row1 c1='0.237419' c2='0' c3='0' />
+ <row2 c1='0' c2='0.256481' c3='0' />
+ <row3 c1='0' c2='0' c3='0.040572' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/RAsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RTIP'>
<Transform>
<Matrix4x4 units='mm'>
@@ -1757,38 +1791,39 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='RAx_joint'/>
+ <Child name='RAx_joint' />
</RobotNode>
<RobotNode name='RAx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.698132' hi='0.523599' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.698132' hi='0.523599' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RAz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RAz_joint' />
</RobotNode>
<RobotNode name='RAy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RAy_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RANK'>
<Transform>
<Matrix4x4 units='mm'>
@@ -1809,25 +1844,25 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='RightFootHeight_joint'/>
- <Child name='RMrot_joint'/>
+ <Child name='RightFootHeight_joint' />
+ <Child name='RMrot_joint' />
</RobotNode>
<RobotNode name='RAz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.523599' hi='0.523599' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.523599' hi='0.523599' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RAy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RAy_joint' />
</RobotNode>
@@ -1841,24 +1876,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='188' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='188' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.027'/>
- <CoM location='Custom' x='0.0203848' y='3.81383e-05' z='106'/>
+ <Mass unit='kg' value='0.027' />
+ <CoM location='Custom' x='0.0203848' y='3.81383e-05' z='106' />
<InertiaMatrix>
- <row1 c1='0.0837755' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0940772' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0287506'/>
+ <row1 c1='0.0837755' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0940772' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0287506' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/REsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RAEL'>
<Transform>
<Matrix4x4 units='mm'>
@@ -1889,36 +1925,36 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='REx_joint'/>
+ <Child name='REx_joint' />
</RobotNode>
<RobotNode name='REx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='2.79253' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='2.79253' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='REz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='REz_joint' />
</RobotNode>
<RobotNode name='REz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-1.5708' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-1.5708' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RWsegment_joint'/>
+ <Child name='RWsegment_joint' />
</RobotNode>
@@ -1932,15 +1968,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.785398' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.785398' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RFx_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RTOE'>
<Transform>
<Matrix4x4 units='mm'>
@@ -1964,15 +2001,16 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='1' y='0' z='0'/>
- <limits lo='0' hi='52' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='1' y='0' z='0' />
+ <limits lo='0' hi='52' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RHsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RASI'>
<Transform>
<Matrix4x4 units='mm'>
@@ -2003,53 +2041,53 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='RHx_joint'/>
+ <Child name='RHx_joint' />
</RobotNode>
<RobotNode name='RHx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.872665' hi='1.65806' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.872665' hi='1.65806' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RHz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RHz_joint' />
</RobotNode>
<RobotNode name='RHy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-1.13446' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-1.13446' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RKsegment_joint'/>
+ <Child name='RKsegment_joint' />
</RobotNode>
<RobotNode name='RHz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RHy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RHy_joint' />
</RobotNode>
@@ -2063,24 +2101,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='245' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='245' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.14'/>
- <CoM location='Custom' x='0.0456274' y='1.04413e-05' z='121.6'/>
+ <Mass unit='kg' value='0.14' />
+ <CoM location='Custom' x='0.0456274' y='1.04413e-05' z='121.6' />
<InertiaMatrix>
- <row1 c1='3.0625' c2='0' c3='0'/>
- <row2 c1='0' c2='3.0625' c3='0'/>
- <row3 c1='0' c2='0' c3='0.636804'/>
+ <row1 c1='3.0625' c2='0' c3='0' />
+ <row2 c1='0' c2='3.0625' c3='0' />
+ <row3 c1='0' c2='0' c3='0.636804' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/RKsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RKNE'>
<Transform>
<Matrix4x4 units='mm'>
@@ -2101,19 +2140,19 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='RKx_joint'/>
+ <Child name='RKx_joint' />
</RobotNode>
<RobotNode name='RKx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-2.26893' hi='0' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-2.26893' hi='0' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RAsegment_joint'/>
+ <Child name='RAsegment_joint' />
</RobotNode>
@@ -2127,15 +2166,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.261799' hi='0.261799' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.261799' hi='0.261799' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RMrot_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RMT1'>
<Transform>
<Matrix4x4 units='mm'>
@@ -2156,7 +2196,7 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='RFx_joint'/>
+ <Child name='RFx_joint' />
</RobotNode>
@@ -2170,15 +2210,16 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='1' y='0' z='0'/>
- <limits lo='0' hi='87' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='1' y='0' z='0' />
+ <limits lo='0' hi='87' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RSCsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RBAK'>
<Transform>
<Matrix4x4 units='mm'>
@@ -2189,53 +2230,53 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='RSCx_joint'/>
+ <Child name='RSCx_joint' />
</RobotNode>
<RobotNode name='RSCx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='0' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='0' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RSCz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RSCz_joint' />
</RobotNode>
<RobotNode name='RSCy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RSsegment_joint'/>
+ <Child name='RSsegment_joint' />
</RobotNode>
<RobotNode name='RSCz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-0.349066' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-0.349066' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RSCy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RSCy_joint' />
</RobotNode>
@@ -2249,24 +2290,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='1' y='0' z='0'/>
- <limits lo='0' hi='23' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='1' y='0' z='0' />
+ <limits lo='0' hi='23' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.021'/>
- <CoM location='Custom' x='-10' y='4.16334e-14' z='0'/>
+ <Mass unit='kg' value='0.021' />
+ <CoM location='Custom' x='-10' y='4.16334e-14' z='0' />
<InertiaMatrix>
- <row1 c1='0.0298116' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0063504' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0298116'/>
+ <row1 c1='0.0298116' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0063504' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0298116' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/RSsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RSHO'>
<Transform>
<Matrix4x4 units='mm'>
@@ -2277,53 +2319,53 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='RSx_joint'/>
+ <Child name='RSx_joint' />
</RobotNode>
<RobotNode name='RSx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-1.22173' hi='3.31613' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-1.22173' hi='3.31613' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RSz_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RSz_joint' />
</RobotNode>
<RobotNode name='RSy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-2.79253' hi='0' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-2.79253' hi='0' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='REsegment_joint'/>
+ <Child name='REsegment_joint' />
</RobotNode>
<RobotNode name='RSz_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='-1.0472' hi='1.22173' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='-1.0472' hi='1.22173' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RSy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RSy_joint' />
</RobotNode>
@@ -2337,24 +2379,25 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='145' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='145' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.016'/>
- <CoM location='Custom' x='0.0203848' y='3.81383e-05' z='82.6'/>
+ <Mass unit='kg' value='0.016' />
+ <CoM location='Custom' x='0.0203848' y='3.81383e-05' z='82.6' />
<InertiaMatrix>
- <row1 c1='0.0319821' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0340787' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0065229'/>
+ <row1 c1='0.0319821' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0340787' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0065229' />
</InertiaMatrix>
</Physics>
<Visualization>
<File type='inventor'>./models/RWsegment_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
<Sensor type='position' name='MARKER_RFRA'>
<Transform>
<Matrix4x4 units='mm'>
@@ -2385,44 +2428,45 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='RWx_joint'/>
+ <Child name='RWx_joint' />
</RobotNode>
<RobotNode name='RWx_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.523599' hi='0.349066' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.523599' hi='0.349066' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<CollisionModel>
- <Primitives>
- <Box width="10" height="10" depth="10"/>
- </Primitives>
- </CollisionModel>
- <Child name='RWy_joint'/>
+ <Primitives>
+ <Box width="10" height="10" depth="10" />
+ </Primitives>
+ </CollisionModel>
+ <Child name='RWy_joint' />
</RobotNode>
<RobotNode name='RWy_joint'>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-1.5708' hi='1.22173' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-1.5708' hi='1.22173' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RWy_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint21x_joint'/>
- <Child name='RightHandSegment_joint'/>
- <Child name='RightFingerJoint11z_joint'/>
- <Child name='RightFingerJoint31x_joint'/>
- <Child name='RightFingerJoint40y_joint'/>
- <Child name='RightFingerJoint50y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint21x_joint' />
+ <Child name='RightHandSegment_joint' />
+ <Child name='RightFingerJoint11z_joint' />
+ <Child name='RightFingerJoint31x_joint' />
+ <Child name='RightFingerJoint40y_joint' />
+ <Child name='RightFingerJoint50y_joint' />
</RobotNode>
@@ -2436,13 +2480,13 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='1' y='0' z='0'/>
- <limits lo='0' hi='15' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='1' y='0' z='0' />
+ <limits lo='0' hi='15' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RightEyeSegmentY_joint'/>
+ <Child name='RightEyeSegmentY_joint' />
</RobotNode>
@@ -2456,15 +2500,16 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='1' z='0'/>
- <limits lo='0' hi='30' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='1' z='0' />
+ <limits lo='0' hi='30' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightEyeSegmentY_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -2478,16 +2523,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint11y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint12y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint12y_joint' />
</RobotNode>
@@ -2501,13 +2547,13 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RightFingerJoint11y_joint'/>
+ <Child name='RightFingerJoint11y_joint' />
</RobotNode>
@@ -2521,16 +2567,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint12y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint13y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint13y_joint' />
</RobotNode>
@@ -2544,15 +2591,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint13y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -2566,28 +2614,29 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RightFingerJoint21y_joint'/>
+ <Child name='RightFingerJoint21y_joint' />
</RobotNode>
<RobotNode name='RightFingerJoint21y_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint21y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint22y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint22y_joint' />
</RobotNode>
@@ -2601,16 +2650,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint22y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint23y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint23y_joint' />
</RobotNode>
@@ -2624,15 +2674,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint23y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -2646,28 +2697,29 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RightFingerJoint31y_joint'/>
+ <Child name='RightFingerJoint31y_joint' />
</RobotNode>
<RobotNode name='RightFingerJoint31y_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint31y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint32y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint32y_joint' />
</RobotNode>
@@ -2681,16 +2733,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint32y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint33y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint33y_joint' />
</RobotNode>
@@ -2704,15 +2757,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint33y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -2726,16 +2780,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='0.0872665' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='0.0872665' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint40y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint41x_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint41x_joint' />
</RobotNode>
@@ -2749,28 +2804,29 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RightFingerJoint41y_joint'/>
+ <Child name='RightFingerJoint41y_joint' />
</RobotNode>
<RobotNode name='RightFingerJoint41y_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint41y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint42y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint42y_joint' />
</RobotNode>
@@ -2784,16 +2840,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint42y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint43y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint43y_joint' />
</RobotNode>
@@ -2807,15 +2864,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint43y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -2829,16 +2887,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='0.174533' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='0.174533' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint50y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint51x_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint51x_joint' />
</RobotNode>
@@ -2852,28 +2911,29 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='0' y='1' z='0'/>
- <limits lo='-0.785398' hi='0.785398' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='1' z='0' />
+ <limits lo='-0.785398' hi='0.785398' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='RightFingerJoint51y_joint'/>
+ <Child name='RightFingerJoint51y_joint' />
</RobotNode>
<RobotNode name='RightFingerJoint51y_joint'>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint51y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint52y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint52y_joint' />
</RobotNode>
@@ -2887,16 +2947,17 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint52y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
- <Child name='RightFingerJoint53y_joint'/>
+ <UseAsCollisionModel />
+ </Visualization>
+ <Child name='RightFingerJoint53y_joint' />
</RobotNode>
@@ -2910,15 +2971,16 @@
</Matrix4x4>
</Transform>
<Joint type='revolute'>
- <axis x='1' y='0' z='0'/>
- <limits lo='0' hi='1.5708' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='1' y='0' z='0' />
+ <limits lo='0' hi='1.5708' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Visualization>
<File type='inventor'>./models/RightFingerJoint53y_joint_visu.wrl</File>
- <UseAsCollisionModel/></Visualization>
+ <UseAsCollisionModel />
+ </Visualization>
</RobotNode>
@@ -2932,22 +2994,22 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='39' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='39' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.013'/>
- <CoM location='Custom' x='-8.35776e-08' y='25.225' z='19'/>
+ <Mass unit='kg' value='0.013' />
+ <CoM location='Custom' x='-8.35776e-08' y='25.225' z='19' />
<InertiaMatrix>
- <row1 c1='0.0036504' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0111794' c3='0'/>
- <row3 c1='0' c2='0' c3='0.0096393'/>
+ <row1 c1='0.0036504' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0111794' c3='0' />
+ <row3 c1='0' c2='0' c3='0.0096393' />
</InertiaMatrix>
</Physics>
- <Child name='RightFootLength_joint'/>
+ <Child name='RightFootLength_joint' />
</RobotNode>
@@ -2961,11 +3023,11 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='1' z='0'/>
- <limits lo='0' hi='68' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='1' z='0' />
+ <limits lo='0' hi='68' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
</RobotNode>
@@ -2980,19 +3042,19 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='-1'/>
- <limits lo='0' hi='108' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='-1' />
+ <limits lo='0' hi='108' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.006'/>
- <CoM location='Custom' x='0.0203848' y='3.81383e-05' z='53.4'/>
+ <Mass unit='kg' value='0.006' />
+ <CoM location='Custom' x='0.0203848' y='3.81383e-05' z='53.4' />
<InertiaMatrix>
- <row1 c1='0.0021869' c2='0' c3='0'/>
- <row2 c1='0' c2='0.0033304' c3='0'/>
- <row3 c1='0' c2='0' c3='0.001283'/>
+ <row1 c1='0.0021869' c2='0' c3='0' />
+ <row2 c1='0' c2='0.0033304' c3='0' />
+ <row3 c1='0' c2='0' c3='0.001283' />
</InertiaMatrix>
</Physics>
<Sensor type='position' name='MARKER_RHPS'>
@@ -3038,19 +3100,19 @@
</Matrix4x4>
</Transform>
<Joint type='prismatic'>
- <translationdirection x='0' y='0' z='1'/>
- <limits lo='0' hi='188' units='mm'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <translationdirection x='0' y='0' z='1' />
+ <limits lo='0' hi='188' units='mm' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
<Physics>
- <Mass unit='kg' value='0.17'/>
- <CoM location='Custom' x='0' y='3.33729e-08' z='-33.84'/>
+ <Mass unit='kg' value='0.17' />
+ <CoM location='Custom' x='0' y='3.33729e-08' z='-33.84' />
<InertiaMatrix>
- <row1 c1='6.37245' c2='0' c3='0'/>
- <row2 c1='0' c2='5.09635' c3='0'/>
- <row3 c1='0' c2='0' c3='4.22532'/>
+ <row1 c1='6.37245' c2='0' c3='0' />
+ <row2 c1='0' c2='5.09635' c3='0' />
+ <row3 c1='0' c2='0' c3='4.22532' />
</InertiaMatrix>
</Physics>
<Sensor type='position' name='MARKER_STRN'>
@@ -3073,504 +3135,501 @@
</Matrix4x4>
</Transform>
</Sensor>
- <Child name='LSCsegment_joint'/>
- <Child name='RSCsegment_joint'/>
- <Child name='BLNsegment_joint'/>
+ <Child name='LSCsegment_joint' />
+ <Child name='RSCsegment_joint' />
+ <Child name='BLNsegment_joint' />
</RobotNode>
<RobotNode name='root_joint'>
<Joint type='revolute'>
- <axis x='0' y='0' z='1'/>
- <limits lo='0' hi='0' units='radian'/>
- <MaxAcceleration value='-1'/>
- <MaxVelocity value='-1'/>
- <MaxTorque value='-1'/>
+ <axis x='0' y='0' z='1' />
+ <limits lo='0' hi='0' units='radian' />
+ <MaxAcceleration value='-1' />
+ <MaxVelocity value='-1' />
+ <MaxTorque value='-1' />
</Joint>
- <Child name='BPSegment_joint'/>
- <Child name='LHsegment_joint'/>
- <Child name='RHsegment_joint'/>
+ <Child name='BPSegment_joint' />
+ <Child name='LHsegment_joint' />
+ <Child name='RHsegment_joint' />
</RobotNode>
-
<RobotNodeSet name='All'>
- <Node name='root_joint'/>
- <Node name='BPSegment_joint'/>
- <Node name='BPx_joint'/>
- <Node name='BPz_joint'/>
- <Node name='BPy_joint'/>
- <Node name='BTSegment_joint'/>
- <Node name='BTx_joint'/>
- <Node name='BTz_joint'/>
- <Node name='BTy_joint'/>
- <Node name='collarSegment_joint'/>
- <Node name='LSCsegment_joint'/>
- <Node name='LSCx_joint'/>
- <Node name='LSCz_joint'/>
- <Node name='LSCy_joint'/>
- <Node name='LSsegment_joint'/>
- <Node name='LSx_joint'/>
- <Node name='LSz_joint'/>
- <Node name='LSy_joint'/>
- <Node name='LEsegment_joint'/>
- <Node name='LEx_joint'/>
- <Node name='LEz_joint'/>
- <Node name='LWsegment_joint'/>
- <Node name='LWx_joint'/>
- <Node name='LWy_joint'/>
- <Node name='LeftHandSegment_joint'/>
- <Node name='LeftFingerJoint50y_joint'/>
- <Node name='LeftFingerJoint51x_joint'/>
- <Node name='LeftFingerJoint51y_joint'/>
- <Node name='LeftFingerJoint52y_joint'/>
- <Node name='LeftFingerJoint53y_joint'/>
- <Node name='LeftFingerJoint31x_joint'/>
- <Node name='LeftFingerJoint31y_joint'/>
- <Node name='LeftFingerJoint32y_joint'/>
- <Node name='LeftFingerJoint33y_joint'/>
- <Node name='LeftFingerJoint21x_joint'/>
- <Node name='LeftFingerJoint21y_joint'/>
- <Node name='LeftFingerJoint22y_joint'/>
- <Node name='LeftFingerJoint23y_joint'/>
- <Node name='LeftFingerJoint11z_joint'/>
- <Node name='LeftFingerJoint11y_joint'/>
- <Node name='LeftFingerJoint12y_joint'/>
- <Node name='LeftFingerJoint13y_joint'/>
- <Node name='LeftFingerJoint40y_joint'/>
- <Node name='LeftFingerJoint41x_joint'/>
- <Node name='LeftFingerJoint41y_joint'/>
- <Node name='LeftFingerJoint42y_joint'/>
- <Node name='LeftFingerJoint43y_joint'/>
- <Node name='RSCsegment_joint'/>
- <Node name='RSCx_joint'/>
- <Node name='RSCz_joint'/>
- <Node name='RSCy_joint'/>
- <Node name='RSsegment_joint'/>
- <Node name='RSx_joint'/>
- <Node name='RSz_joint'/>
- <Node name='RSy_joint'/>
- <Node name='REsegment_joint'/>
- <Node name='REx_joint'/>
- <Node name='REz_joint'/>
- <Node name='RWsegment_joint'/>
- <Node name='RWx_joint'/>
- <Node name='RWy_joint'/>
- <Node name='RightFingerJoint21x_joint'/>
- <Node name='RightFingerJoint21y_joint'/>
- <Node name='RightFingerJoint22y_joint'/>
- <Node name='RightFingerJoint23y_joint'/>
- <Node name='RightHandSegment_joint'/>
- <Node name='RightFingerJoint11z_joint'/>
- <Node name='RightFingerJoint11y_joint'/>
- <Node name='RightFingerJoint12y_joint'/>
- <Node name='RightFingerJoint13y_joint'/>
- <Node name='RightFingerJoint31x_joint'/>
- <Node name='RightFingerJoint31y_joint'/>
- <Node name='RightFingerJoint32y_joint'/>
- <Node name='RightFingerJoint33y_joint'/>
- <Node name='RightFingerJoint40y_joint'/>
- <Node name='RightFingerJoint41x_joint'/>
- <Node name='RightFingerJoint41y_joint'/>
- <Node name='RightFingerJoint42y_joint'/>
- <Node name='RightFingerJoint43y_joint'/>
- <Node name='RightFingerJoint50y_joint'/>
- <Node name='RightFingerJoint51x_joint'/>
- <Node name='RightFingerJoint51y_joint'/>
- <Node name='RightFingerJoint52y_joint'/>
- <Node name='RightFingerJoint53y_joint'/>
- <Node name='BLNsegment_joint'/>
- <Node name='BLNx_joint'/>
- <Node name='BLNz_joint'/>
- <Node name='BLNy_joint'/>
- <Node name='BUNsegment_joint'/>
- <Node name='BUNx_joint'/>
- <Node name='BUNz_joint'/>
- <Node name='BUNy_joint'/>
- <Node name='MidHeadSegment_joint'/>
- <Node name='LeftEyeSegmentX_joint'/>
- <Node name='LeftEyeSegmentY_joint'/>
- <Node name='RightEyeSegmentX_joint'/>
- <Node name='RightEyeSegmentY_joint'/>
- <Node name='HeadSegment_joint'/>
- <Node name='LHsegment_joint'/>
- <Node name='LHx_joint'/>
- <Node name='LHz_joint'/>
- <Node name='LHy_joint'/>
- <Node name='LKsegment_joint'/>
- <Node name='LKx_joint'/>
- <Node name='LAsegment_joint'/>
- <Node name='LAx_joint'/>
- <Node name='LAz_joint'/>
- <Node name='LAy_joint'/>
- <Node name='LeftFootHeight_joint'/>
- <Node name='LMrot_joint'/>
- <Node name='LFx_joint'/>
- <Node name='RHsegment_joint'/>
- <Node name='RHx_joint'/>
- <Node name='RHz_joint'/>
- <Node name='RHy_joint'/>
- <Node name='RKsegment_joint'/>
- <Node name='RKx_joint'/>
- <Node name='RAsegment_joint'/>
- <Node name='RAx_joint'/>
- <Node name='RAz_joint'/>
- <Node name='RAy_joint'/>
- <Node name='RightFootHeight_joint'/>
- <Node name='RightFootLength_joint'/>
- <Node name='RMrot_joint'/>
- <Node name='RFx_joint'/>
+ <Node name='root_joint' />
+ <Node name='BPSegment_joint' />
+ <Node name='BPx_joint' />
+ <Node name='BPz_joint' />
+ <Node name='BPy_joint' />
+ <Node name='BTSegment_joint' />
+ <Node name='BTx_joint' />
+ <Node name='BTz_joint' />
+ <Node name='BTy_joint' />
+ <Node name='collarSegment_joint' />
+ <Node name='LSCsegment_joint' />
+ <Node name='LSCx_joint' />
+ <Node name='LSCz_joint' />
+ <Node name='LSCy_joint' />
+ <Node name='LSsegment_joint' />
+ <Node name='LSx_joint' />
+ <Node name='LSz_joint' />
+ <Node name='LSy_joint' />
+ <Node name='LEsegment_joint' />
+ <Node name='LEx_joint' />
+ <Node name='LEz_joint' />
+ <Node name='LWsegment_joint' />
+ <Node name='LWx_joint' />
+ <Node name='LWy_joint' />
+ <Node name='LeftHandSegment_joint' />
+ <Node name='LeftFingerJoint50y_joint' />
+ <Node name='LeftFingerJoint51x_joint' />
+ <Node name='LeftFingerJoint51y_joint' />
+ <Node name='LeftFingerJoint52y_joint' />
+ <Node name='LeftFingerJoint53y_joint' />
+ <Node name='LeftFingerJoint31x_joint' />
+ <Node name='LeftFingerJoint31y_joint' />
+ <Node name='LeftFingerJoint32y_joint' />
+ <Node name='LeftFingerJoint33y_joint' />
+ <Node name='LeftFingerJoint21x_joint' />
+ <Node name='LeftFingerJoint21y_joint' />
+ <Node name='LeftFingerJoint22y_joint' />
+ <Node name='LeftFingerJoint23y_joint' />
+ <Node name='LeftFingerJoint11z_joint' />
+ <Node name='LeftFingerJoint11y_joint' />
+ <Node name='LeftFingerJoint12y_joint' />
+ <Node name='LeftFingerJoint13y_joint' />
+ <Node name='LeftFingerJoint40y_joint' />
+ <Node name='LeftFingerJoint41x_joint' />
+ <Node name='LeftFingerJoint41y_joint' />
+ <Node name='LeftFingerJoint42y_joint' />
+ <Node name='LeftFingerJoint43y_joint' />
+ <Node name='RSCsegment_joint' />
+ <Node name='RSCx_joint' />
+ <Node name='RSCz_joint' />
+ <Node name='RSCy_joint' />
+ <Node name='RSsegment_joint' />
+ <Node name='RSx_joint' />
+ <Node name='RSz_joint' />
+ <Node name='RSy_joint' />
+ <Node name='REsegment_joint' />
+ <Node name='REx_joint' />
+ <Node name='REz_joint' />
+ <Node name='RWsegment_joint' />
+ <Node name='RWx_joint' />
+ <Node name='RWy_joint' />
+ <Node name='RightFingerJoint21x_joint' />
+ <Node name='RightFingerJoint21y_joint' />
+ <Node name='RightFingerJoint22y_joint' />
+ <Node name='RightFingerJoint23y_joint' />
+ <Node name='RightHandSegment_joint' />
+ <Node name='RightFingerJoint11z_joint' />
+ <Node name='RightFingerJoint11y_joint' />
+ <Node name='RightFingerJoint12y_joint' />
+ <Node name='RightFingerJoint13y_joint' />
+ <Node name='RightFingerJoint31x_joint' />
+ <Node name='RightFingerJoint31y_joint' />
+ <Node name='RightFingerJoint32y_joint' />
+ <Node name='RightFingerJoint33y_joint' />
+ <Node name='RightFingerJoint40y_joint' />
+ <Node name='RightFingerJoint41x_joint' />
+ <Node name='RightFingerJoint41y_joint' />
+ <Node name='RightFingerJoint42y_joint' />
+ <Node name='RightFingerJoint43y_joint' />
+ <Node name='RightFingerJoint50y_joint' />
+ <Node name='RightFingerJoint51x_joint' />
+ <Node name='RightFingerJoint51y_joint' />
+ <Node name='RightFingerJoint52y_joint' />
+ <Node name='RightFingerJoint53y_joint' />
+ <Node name='BLNsegment_joint' />
+ <Node name='BLNx_joint' />
+ <Node name='BLNz_joint' />
+ <Node name='BLNy_joint' />
+ <Node name='BUNsegment_joint' />
+ <Node name='BUNx_joint' />
+ <Node name='BUNz_joint' />
+ <Node name='BUNy_joint' />
+ <Node name='MidHeadSegment_joint' />
+ <Node name='LeftEyeSegmentX_joint' />
+ <Node name='LeftEyeSegmentY_joint' />
+ <Node name='RightEyeSegmentX_joint' />
+ <Node name='RightEyeSegmentY_joint' />
+ <Node name='HeadSegment_joint' />
+ <Node name='LHsegment_joint' />
+ <Node name='LHx_joint' />
+ <Node name='LHz_joint' />
+ <Node name='LHy_joint' />
+ <Node name='LKsegment_joint' />
+ <Node name='LKx_joint' />
+ <Node name='LAsegment_joint' />
+ <Node name='LAx_joint' />
+ <Node name='LAz_joint' />
+ <Node name='LAy_joint' />
+ <Node name='LeftFootHeight_joint' />
+ <Node name='LMrot_joint' />
+ <Node name='LFx_joint' />
+ <Node name='RHsegment_joint' />
+ <Node name='RHx_joint' />
+ <Node name='RHz_joint' />
+ <Node name='RHy_joint' />
+ <Node name='RKsegment_joint' />
+ <Node name='RKx_joint' />
+ <Node name='RAsegment_joint' />
+ <Node name='RAx_joint' />
+ <Node name='RAz_joint' />
+ <Node name='RAy_joint' />
+ <Node name='RightFootHeight_joint' />
+ <Node name='RightFootLength_joint' />
+ <Node name='RMrot_joint' />
+ <Node name='RFx_joint' />
</RobotNodeSet>
<RobotNodeSet name='Joints_Prismatic'>
- <Node name='BPSegment_joint'/>
- <Node name='BTSegment_joint'/>
- <Node name='collarSegment_joint'/>
- <Node name='LSCsegment_joint'/>
- <Node name='LSsegment_joint'/>
- <Node name='LEsegment_joint'/>
- <Node name='LWsegment_joint'/>
- <Node name='LeftHandSegment_joint'/>
- <Node name='RSCsegment_joint'/>
- <Node name='RSsegment_joint'/>
- <Node name='REsegment_joint'/>
- <Node name='RWsegment_joint'/>
- <Node name='RightHandSegment_joint'/>
- <Node name='BLNsegment_joint'/>
- <Node name='BUNsegment_joint'/>
- <Node name='MidHeadSegment_joint'/>
- <Node name='LeftEyeSegmentX_joint'/>
- <Node name='LeftEyeSegmentY_joint'/>
- <Node name='RightEyeSegmentX_joint'/>
- <Node name='RightEyeSegmentY_joint'/>
- <Node name='HeadSegment_joint'/>
- <Node name='LHsegment_joint'/>
- <Node name='LKsegment_joint'/>
- <Node name='LAsegment_joint'/>
- <Node name='LeftFootHeight_joint'/>
- <Node name='RHsegment_joint'/>
- <Node name='RKsegment_joint'/>
- <Node name='RAsegment_joint'/>
- <Node name='RightFootHeight_joint'/>
- <Node name='RightFootLength_joint'/>
+ <Node name='BPSegment_joint' />
+ <Node name='BTSegment_joint' />
+ <Node name='collarSegment_joint' />
+ <Node name='LSCsegment_joint' />
+ <Node name='LSsegment_joint' />
+ <Node name='LEsegment_joint' />
+ <Node name='LWsegment_joint' />
+ <Node name='LeftHandSegment_joint' />
+ <Node name='RSCsegment_joint' />
+ <Node name='RSsegment_joint' />
+ <Node name='REsegment_joint' />
+ <Node name='RWsegment_joint' />
+ <Node name='RightHandSegment_joint' />
+ <Node name='BLNsegment_joint' />
+ <Node name='BUNsegment_joint' />
+ <Node name='MidHeadSegment_joint' />
+ <Node name='LeftEyeSegmentX_joint' />
+ <Node name='LeftEyeSegmentY_joint' />
+ <Node name='RightEyeSegmentX_joint' />
+ <Node name='RightEyeSegmentY_joint' />
+ <Node name='HeadSegment_joint' />
+ <Node name='LHsegment_joint' />
+ <Node name='LKsegment_joint' />
+ <Node name='LAsegment_joint' />
+ <Node name='LeftFootHeight_joint' />
+ <Node name='RHsegment_joint' />
+ <Node name='RKsegment_joint' />
+ <Node name='RAsegment_joint' />
+ <Node name='RightFootHeight_joint' />
+ <Node name='RightFootLength_joint' />
</RobotNodeSet>
<RobotNodeSet name='Joints_Revolute'>
- <Node name='root_joint'/>
- <Node name='BPx_joint'/>
- <Node name='BPz_joint'/>
- <Node name='BPy_joint'/>
- <Node name='BTx_joint'/>
- <Node name='BTz_joint'/>
- <Node name='BTy_joint'/>
- <Node name='LSCx_joint'/>
- <Node name='LSCz_joint'/>
- <Node name='LSCy_joint'/>
- <Node name='LSx_joint'/>
- <Node name='LSz_joint'/>
- <Node name='LSy_joint'/>
- <Node name='LEx_joint'/>
- <Node name='LEz_joint'/>
- <Node name='LWx_joint'/>
- <Node name='LWy_joint'/>
- <Node name='LeftFingerJoint50y_joint'/>
- <Node name='LeftFingerJoint51x_joint'/>
- <Node name='LeftFingerJoint51y_joint'/>
- <Node name='LeftFingerJoint52y_joint'/>
- <Node name='LeftFingerJoint53y_joint'/>
- <Node name='LeftFingerJoint31x_joint'/>
- <Node name='LeftFingerJoint31y_joint'/>
- <Node name='LeftFingerJoint32y_joint'/>
- <Node name='LeftFingerJoint33y_joint'/>
- <Node name='LeftFingerJoint21x_joint'/>
- <Node name='LeftFingerJoint21y_joint'/>
- <Node name='LeftFingerJoint22y_joint'/>
- <Node name='LeftFingerJoint23y_joint'/>
- <Node name='LeftFingerJoint11z_joint'/>
- <Node name='LeftFingerJoint11y_joint'/>
- <Node name='LeftFingerJoint12y_joint'/>
- <Node name='LeftFingerJoint13y_joint'/>
- <Node name='LeftFingerJoint40y_joint'/>
- <Node name='LeftFingerJoint41x_joint'/>
- <Node name='LeftFingerJoint41y_joint'/>
- <Node name='LeftFingerJoint42y_joint'/>
- <Node name='LeftFingerJoint43y_joint'/>
- <Node name='RSCx_joint'/>
- <Node name='RSCz_joint'/>
- <Node name='RSCy_joint'/>
- <Node name='RSx_joint'/>
- <Node name='RSz_joint'/>
- <Node name='RSy_joint'/>
- <Node name='REx_joint'/>
- <Node name='REz_joint'/>
- <Node name='RWx_joint'/>
- <Node name='RWy_joint'/>
- <Node name='RightFingerJoint21x_joint'/>
- <Node name='RightFingerJoint21y_joint'/>
- <Node name='RightFingerJoint22y_joint'/>
- <Node name='RightFingerJoint23y_joint'/>
- <Node name='RightFingerJoint11z_joint'/>
- <Node name='RightFingerJoint11y_joint'/>
- <Node name='RightFingerJoint12y_joint'/>
- <Node name='RightFingerJoint13y_joint'/>
- <Node name='RightFingerJoint31x_joint'/>
- <Node name='RightFingerJoint31y_joint'/>
- <Node name='RightFingerJoint32y_joint'/>
- <Node name='RightFingerJoint33y_joint'/>
- <Node name='RightFingerJoint40y_joint'/>
- <Node name='RightFingerJoint41x_joint'/>
- <Node name='RightFingerJoint41y_joint'/>
- <Node name='RightFingerJoint42y_joint'/>
- <Node name='RightFingerJoint43y_joint'/>
- <Node name='RightFingerJoint50y_joint'/>
- <Node name='RightFingerJoint51x_joint'/>
- <Node name='RightFingerJoint51y_joint'/>
- <Node name='RightFingerJoint52y_joint'/>
- <Node name='RightFingerJoint53y_joint'/>
- <Node name='BLNx_joint'/>
- <Node name='BLNz_joint'/>
- <Node name='BLNy_joint'/>
- <Node name='BUNx_joint'/>
- <Node name='BUNz_joint'/>
- <Node name='BUNy_joint'/>
- <Node name='LHx_joint'/>
- <Node name='LHz_joint'/>
- <Node name='LHy_joint'/>
- <Node name='LKx_joint'/>
- <Node name='LAx_joint'/>
- <Node name='LAz_joint'/>
- <Node name='LAy_joint'/>
- <Node name='LMrot_joint'/>
- <Node name='LFx_joint'/>
- <Node name='RHx_joint'/>
- <Node name='RHz_joint'/>
- <Node name='RHy_joint'/>
- <Node name='RKx_joint'/>
- <Node name='RAx_joint'/>
- <Node name='RAz_joint'/>
- <Node name='RAy_joint'/>
- <Node name='RMrot_joint'/>
- <Node name='RFx_joint'/>
+ <Node name='root_joint' />
+ <Node name='BPx_joint' />
+ <Node name='BPz_joint' />
+ <Node name='BPy_joint' />
+ <Node name='BTx_joint' />
+ <Node name='BTz_joint' />
+ <Node name='BTy_joint' />
+ <Node name='LSCx_joint' />
+ <Node name='LSCz_joint' />
+ <Node name='LSCy_joint' />
+ <Node name='LSx_joint' />
+ <Node name='LSz_joint' />
+ <Node name='LSy_joint' />
+ <Node name='LEx_joint' />
+ <Node name='LEz_joint' />
+ <Node name='LWx_joint' />
+ <Node name='LWy_joint' />
+ <Node name='LeftFingerJoint50y_joint' />
+ <Node name='LeftFingerJoint51x_joint' />
+ <Node name='LeftFingerJoint51y_joint' />
+ <Node name='LeftFingerJoint52y_joint' />
+ <Node name='LeftFingerJoint53y_joint' />
+ <Node name='LeftFingerJoint31x_joint' />
+ <Node name='LeftFingerJoint31y_joint' />
+ <Node name='LeftFingerJoint32y_joint' />
+ <Node name='LeftFingerJoint33y_joint' />
+ <Node name='LeftFingerJoint21x_joint' />
+ <Node name='LeftFingerJoint21y_joint' />
+ <Node name='LeftFingerJoint22y_joint' />
+ <Node name='LeftFingerJoint23y_joint' />
+ <Node name='LeftFingerJoint11z_joint' />
+ <Node name='LeftFingerJoint11y_joint' />
+ <Node name='LeftFingerJoint12y_joint' />
+ <Node name='LeftFingerJoint13y_joint' />
+ <Node name='LeftFingerJoint40y_joint' />
+ <Node name='LeftFingerJoint41x_joint' />
+ <Node name='LeftFingerJoint41y_joint' />
+ <Node name='LeftFingerJoint42y_joint' />
+ <Node name='LeftFingerJoint43y_joint' />
+ <Node name='RSCx_joint' />
+ <Node name='RSCz_joint' />
+ <Node name='RSCy_joint' />
+ <Node name='RSx_joint' />
+ <Node name='RSz_joint' />
+ <Node name='RSy_joint' />
+ <Node name='REx_joint' />
+ <Node name='REz_joint' />
+ <Node name='RWx_joint' />
+ <Node name='RWy_joint' />
+ <Node name='RightFingerJoint21x_joint' />
+ <Node name='RightFingerJoint21y_joint' />
+ <Node name='RightFingerJoint22y_joint' />
+ <Node name='RightFingerJoint23y_joint' />
+ <Node name='RightFingerJoint11z_joint' />
+ <Node name='RightFingerJoint11y_joint' />
+ <Node name='RightFingerJoint12y_joint' />
+ <Node name='RightFingerJoint13y_joint' />
+ <Node name='RightFingerJoint31x_joint' />
+ <Node name='RightFingerJoint31y_joint' />
+ <Node name='RightFingerJoint32y_joint' />
+ <Node name='RightFingerJoint33y_joint' />
+ <Node name='RightFingerJoint40y_joint' />
+ <Node name='RightFingerJoint41x_joint' />
+ <Node name='RightFingerJoint41y_joint' />
+ <Node name='RightFingerJoint42y_joint' />
+ <Node name='RightFingerJoint43y_joint' />
+ <Node name='RightFingerJoint50y_joint' />
+ <Node name='RightFingerJoint51x_joint' />
+ <Node name='RightFingerJoint51y_joint' />
+ <Node name='RightFingerJoint52y_joint' />
+ <Node name='RightFingerJoint53y_joint' />
+ <Node name='BLNx_joint' />
+ <Node name='BLNz_joint' />
+ <Node name='BLNy_joint' />
+ <Node name='BUNx_joint' />
+ <Node name='BUNz_joint' />
+ <Node name='BUNy_joint' />
+ <Node name='LHx_joint' />
+ <Node name='LHz_joint' />
+ <Node name='LHy_joint' />
+ <Node name='LKx_joint' />
+ <Node name='LAx_joint' />
+ <Node name='LAz_joint' />
+ <Node name='LAy_joint' />
+ <Node name='LMrot_joint' />
+ <Node name='LFx_joint' />
+ <Node name='RHx_joint' />
+ <Node name='RHz_joint' />
+ <Node name='RHy_joint' />
+ <Node name='RKx_joint' />
+ <Node name='RAx_joint' />
+ <Node name='RAz_joint' />
+ <Node name='RAy_joint' />
+ <Node name='RMrot_joint' />
+ <Node name='RFx_joint' />
</RobotNodeSet>
+ <RobotNodeSet name='MMMJoints'>
+ <Node name='root_joint' />
+ <Node name='BPx_joint' />
+ <Node name='BPz_joint' />
+ <Node name='BPy_joint' />
+ <Node name='BTx_joint' />
+ <Node name='BTz_joint' />
+ <Node name='BTy_joint' />
+ <Node name='LSCx_joint' />
+ <Node name='LSCz_joint' />
+ <Node name='LSCy_joint' />
+ <Node name='LSx_joint' />
+ <Node name='LSz_joint' />
+ <Node name='LSy_joint' />
+ <Node name='LEx_joint' />
+ <Node name='LEz_joint' />
+ <Node name='LWx_joint' />
+ <Node name='LWy_joint' />
+ <Node name='RSCx_joint' />
+ <Node name='RSCz_joint' />
+ <Node name='RSCy_joint' />
+ <Node name='RSx_joint' />
+ <Node name='RSz_joint' />
+ <Node name='RSy_joint' />
+ <Node name='REx_joint' />
+ <Node name='REz_joint' />
+ <Node name='RWx_joint' />
+ <Node name='RWy_joint' />
+ <Node name='BLNx_joint' />
+ <Node name='BLNz_joint' />
+ <Node name='BLNy_joint' />
+ <Node name='BUNx_joint' />
+ <Node name='BUNz_joint' />
+ <Node name='BUNy_joint' />
+ <Node name='LHx_joint' />
+ <Node name='LHz_joint' />
+ <Node name='LHy_joint' />
+ <Node name='LKx_joint' />
+ <Node name='LAx_joint' />
+ <Node name='LAz_joint' />
+ <Node name='LAy_joint' />
+ <Node name='LMrot_joint' />
+ <Node name='LFx_joint' />
+ <Node name='RHx_joint' />
+ <Node name='RHz_joint' />
+ <Node name='RHy_joint' />
+ <Node name='RKx_joint' />
+ <Node name='RAx_joint' />
+ <Node name='RAz_joint' />
+ <Node name='RAy_joint' />
+ <Node name='RMrot_joint' />
+ <Node name='RFx_joint' />
+ </RobotNodeSet>
+
+
+ <RobotNodeSet name='LeftHipArm' tcp='LeftHandSegment_joint'>
+ <Node name='BTx_joint' />
+ <Node name='BTz_joint' />
+ <Node name='BTy_joint' />
+ <Node name='LSCx_joint' />
+ <Node name='LSCz_joint' />
+ <Node name='LSCy_joint' />
+ <Node name='LSx_joint' />
+ <Node name='LSz_joint' />
+ <Node name='LSy_joint' />
+ <Node name='LEx_joint' />
+ <Node name='LEz_joint' />
+ <Node name='LWx_joint' />
+ <Node name='LWy_joint' />
+ </RobotNodeSet>
- <RobotNodeSet name='MMMJoints'>
- <Node name='root_joint'/>
- <Node name='BPx_joint'/>
- <Node name='BPz_joint'/>
- <Node name='BPy_joint'/>
- <Node name='BTx_joint'/>
- <Node name='BTz_joint'/>
- <Node name='BTy_joint'/>
- <Node name='LSCx_joint'/>
- <Node name='LSCz_joint'/>
- <Node name='LSCy_joint'/>
- <Node name='LSx_joint'/>
- <Node name='LSz_joint'/>
- <Node name='LSy_joint'/>
- <Node name='LEx_joint'/>
- <Node name='LEz_joint'/>
- <Node name='LWx_joint'/>
- <Node name='LWy_joint'/>
- <Node name='RSCx_joint'/>
- <Node name='RSCz_joint'/>
- <Node name='RSCy_joint'/>
- <Node name='RSx_joint'/>
- <Node name='RSz_joint'/>
- <Node name='RSy_joint'/>
- <Node name='REx_joint'/>
- <Node name='REz_joint'/>
- <Node name='RWx_joint'/>
- <Node name='RWy_joint'/>
- <Node name='BLNx_joint'/>
- <Node name='BLNz_joint'/>
- <Node name='BLNy_joint'/>
- <Node name='BUNx_joint'/>
- <Node name='BUNz_joint'/>
- <Node name='BUNy_joint'/>
- <Node name='LHx_joint'/>
- <Node name='LHz_joint'/>
- <Node name='LHy_joint'/>
- <Node name='LKx_joint'/>
- <Node name='LAx_joint'/>
- <Node name='LAz_joint'/>
- <Node name='LAy_joint'/>
- <Node name='LMrot_joint'/>
- <Node name='LFx_joint'/>
- <Node name='RHx_joint'/>
- <Node name='RHz_joint'/>
- <Node name='RHy_joint'/>
- <Node name='RKx_joint'/>
- <Node name='RAx_joint'/>
- <Node name='RAz_joint'/>
- <Node name='RAy_joint'/>
- <Node name='RMrot_joint'/>
- <Node name='RFx_joint'/>
- </RobotNodeSet>
-
-
- <RobotNodeSet name='LeftHipArm' tcp='LeftHandSegment_joint'>
- <Node name='BTx_joint'/>
- <Node name='BTz_joint'/>
- <Node name='BTy_joint'/>
- <Node name='LSCx_joint'/>
- <Node name='LSCz_joint'/>
- <Node name='LSCy_joint'/>
- <Node name='LSx_joint'/>
- <Node name='LSz_joint'/>
- <Node name='LSy_joint'/>
- <Node name='LEx_joint'/>
- <Node name='LEz_joint'/>
- <Node name='LWx_joint'/>
- <Node name='LWy_joint'/>
- </RobotNodeSet>
-
-
-
- <RobotNodeSet name='LeftArm' tcp='LeftHandSegment_joint'>
- <Node name='LSCx_joint'/>
- <Node name='LSCz_joint'/>
- <Node name='LSCy_joint'/>
- <Node name='LSx_joint'/>
- <Node name='LSz_joint'/>
- <Node name='LSy_joint'/>
- <Node name='LEx_joint'/>
- <Node name='LEz_joint'/>
- <Node name='LWx_joint'/>
- <Node name='LWy_joint'/>
- </RobotNodeSet>
+
+ <RobotNodeSet name='LeftArm' tcp='LeftHandSegment_joint'>
+ <Node name='LSCx_joint' />
+ <Node name='LSCz_joint' />
+ <Node name='LSCy_joint' />
+ <Node name='LSx_joint' />
+ <Node name='LSz_joint' />
+ <Node name='LSy_joint' />
+ <Node name='LEx_joint' />
+ <Node name='LEz_joint' />
+ <Node name='LWx_joint' />
+ <Node name='LWy_joint' />
+ </RobotNodeSet>
<RobotNodeSet name="LeftArm-7dof" kinematicRoot="LSsegment_joint" tcp="LeftHandSegment_joint">
- <Node name='LSx_joint'/>
- <Node name='LSz_joint'/>
- <Node name='LSy_joint'/>
- <Node name='LEx_joint'/>
- <Node name='LEz_joint'/>
- <Node name='LWx_joint'/>
- <Node name='LWy_joint'/>
+ <Node name='LSx_joint' />
+ <Node name='LSz_joint' />
+ <Node name='LSy_joint' />
+ <Node name='LEx_joint' />
+ <Node name='LEz_joint' />
+ <Node name='LWx_joint' />
+ <Node name='LWy_joint' />
+ </RobotNodeSet>
+
+ <RobotNodeSet name='RightHipArm' tcp='RightHandSegment_joint'>
+ <Node name='BTx_joint' />
+ <Node name='BTz_joint' />
+ <Node name='BTy_joint' />
+ <Node name='RSCx_joint' />
+ <Node name='RSCz_joint' />
+ <Node name='RSCy_joint' />
+ <Node name='RSx_joint' />
+ <Node name='RSz_joint' />
+ <Node name='RSy_joint' />
+ <Node name='REx_joint' />
+ <Node name='REz_joint' />
+ <Node name='RWx_joint' />
+ <Node name='RWy_joint' />
</RobotNodeSet>
- <RobotNodeSet name='RightHipArm' tcp='RightHandSegment_joint'>
- <Node name='BTx_joint'/>
- <Node name='BTz_joint'/>
- <Node name='BTy_joint'/>
- <Node name='RSCx_joint'/>
- <Node name='RSCz_joint'/>
- <Node name='RSCy_joint'/>
- <Node name='RSx_joint'/>
- <Node name='RSz_joint'/>
- <Node name='RSy_joint'/>
- <Node name='REx_joint'/>
- <Node name='REz_joint'/>
- <Node name='RWx_joint'/>
- <Node name='RWy_joint'/>
- </RobotNodeSet>
-
- <RobotNodeSet name='RightArm' tcp='RightHandSegment_joint'>
- <Node name='RSCx_joint'/>
- <Node name='RSCz_joint'/>
- <Node name='RSCy_joint'/>
- <Node name='RSx_joint'/>
- <Node name='RSz_joint'/>
- <Node name='RSy_joint'/>
- <Node name='REx_joint'/>
- <Node name='REz_joint'/>
- <Node name='RWx_joint'/>
- <Node name='RWy_joint'/>
- </RobotNodeSet>
+ <RobotNodeSet name='RightArm' tcp='RightHandSegment_joint'>
+ <Node name='RSCx_joint' />
+ <Node name='RSCz_joint' />
+ <Node name='RSCy_joint' />
+ <Node name='RSx_joint' />
+ <Node name='RSz_joint' />
+ <Node name='RSy_joint' />
+ <Node name='REx_joint' />
+ <Node name='REz_joint' />
+ <Node name='RWx_joint' />
+ <Node name='RWy_joint' />
+ </RobotNodeSet>
<RobotNodeSet name="RightArm-7dof" kinematicRoot="RSsegment_joint" tcp="RightHandSegment_joint">
- <Node name='RSx_joint'/>
- <Node name='RSz_joint'/>
- <Node name='RSy_joint'/>
- <Node name='REx_joint'/>
- <Node name='REz_joint'/>
- <Node name='RWx_joint'/>
- <Node name='RWy_joint'/>
+ <Node name='RSx_joint' />
+ <Node name='RSz_joint' />
+ <Node name='RSy_joint' />
+ <Node name='REx_joint' />
+ <Node name='REz_joint' />
+ <Node name='RWx_joint' />
+ <Node name='RWy_joint' />
</RobotNodeSet>
- <RobotNodeSet name='Optimization'>
- <!-- Neck -->
- <Node name='BLNx_joint'/>
- <Node name='BLNy_joint'/>
- <Node name='BLNz_joint'/>
-
- <!-- Pelvis -->
- <Node name='BPx_joint'/>
- <Node name='BPy_joint'/>
- <Node name='BPz_joint'/>
-
- <!-- Torso -->
- <Node name='BTx_joint'/>
- <Node name='BTy_joint'/>
- <Node name='BTz_joint'/>
-
- <!-- Head -->
- <Node name='BUNx_joint'/>
- <Node name='BUNy_joint'/>
- <Node name='BUNz_joint'/>
-
- <!-- Left Ankle -->
- <Node name='LAx_joint'/>
- <Node name='LAy_joint'/>
- <Node name='LAz_joint'/>
-
- <!-- Left Elbow -->
- <Node name='LEx_joint'/>
- <Node name='LEz_joint'/>
-
- <!-- Left Hip -->
- <Node name='LHx_joint'/>
- <Node name='LHy_joint'/>
- <Node name='LHz_joint'/>
-
- <!-- Left Knee -->
- <Node name='LKx_joint'/>
-
- <!-- Left Shoulder Outer -->
- <Node name='LSx_joint'/>
- <Node name='LSy_joint'/>
- <Node name='LSz_joint'/>
-
- <!-- Left Wrist -->
- <Node name='LWx_joint'/>
- <Node name='LWy_joint'/>
-
- <!-- Left Foot -->
- <Node name='LFx_joint'/>
- <Node name='LMrot_joint'/>
-
- <!-- Right Ankle -->
- <Node name='RAx_joint'/>
- <Node name='RAy_joint'/>
- <Node name='RAz_joint'/>
-
- <!-- Right Elbow -->
- <Node name='REx_joint'/>
- <Node name='REz_joint'/>
-
- <!-- Right Hip -->
- <Node name='RHx_joint'/>
- <Node name='RHy_joint'/>
- <Node name='RHz_joint'/>
-
- <!-- Right Knee -->
- <Node name='RKx_joint'/>
-
- <!-- Right Shoulder Outer -->
- <Node name='RSx_joint'/>
- <Node name='RSy_joint'/>
- <Node name='RSz_joint'/>
-
- <!-- Right Wrist -->
- <Node name='RWx_joint'/>
- <Node name='RWy_joint'/>
-
- <!-- Right Foot -->
- <Node name='RFx_joint'/>
- <Node name='RMrot_joint'/>
- </RobotNodeSet>
+ <RobotNodeSet name='Optimization'>
+ <!-- Neck -->
+ <Node name='BLNx_joint' />
+ <Node name='BLNy_joint' />
+ <Node name='BLNz_joint' />
+
+ <!-- Pelvis -->
+ <Node name='BPx_joint' />
+ <Node name='BPy_joint' />
+ <Node name='BPz_joint' />
+
+ <!-- Torso -->
+ <Node name='BTx_joint' />
+ <Node name='BTy_joint' />
+ <Node name='BTz_joint' />
+
+ <!-- Head -->
+ <Node name='BUNx_joint' />
+ <Node name='BUNy_joint' />
+ <Node name='BUNz_joint' />
+
+ <!-- Left Ankle -->
+ <Node name='LAx_joint' />
+ <Node name='LAy_joint' />
+ <Node name='LAz_joint' />
+
+ <!-- Left Elbow -->
+ <Node name='LEx_joint' />
+ <Node name='LEz_joint' />
+
+ <!-- Left Hip -->
+ <Node name='LHx_joint' />
+ <Node name='LHy_joint' />
+ <Node name='LHz_joint' />
+
+ <!-- Left Knee -->
+ <Node name='LKx_joint' />
+
+ <!-- Left Shoulder Outer -->
+ <Node name='LSx_joint' />
+ <Node name='LSy_joint' />
+ <Node name='LSz_joint' />
+
+ <!-- Left Wrist -->
+ <Node name='LWx_joint' />
+ <Node name='LWy_joint' />
+
+ <!-- Left Foot -->
+ <Node name='LFx_joint' />
+ <Node name='LMrot_joint' />
+
+ <!-- Right Ankle -->
+ <Node name='RAx_joint' />
+ <Node name='RAy_joint' />
+ <Node name='RAz_joint' />
+
+ <!-- Right Elbow -->
+ <Node name='REx_joint' />
+ <Node name='REz_joint' />
+
+ <!-- Right Hip -->
+ <Node name='RHx_joint' />
+ <Node name='RHy_joint' />
+ <Node name='RHz_joint' />
+
+ <!-- Right Knee -->
+ <Node name='RKx_joint' />
+
+ <!-- Right Shoulder Outer -->
+ <Node name='RSx_joint' />
+ <Node name='RSy_joint' />
+ <Node name='RSz_joint' />
+
+ <!-- Right Wrist -->
+ <Node name='RWx_joint' />
+ <Node name='RWy_joint' />
+
+ <!-- Right Foot -->
+ <Node name='RFx_joint' />
+ <Node name='RMrot_joint' />
+ </RobotNodeSet>
</Robot>
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 9638058676450ed6fcb7d8ff884cf7ebf8beb67e..734be2bc9cf9fa584fe9d76d30935863ea8d8544 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -71,6 +71,9 @@ set(SOURCES
Coin/VisualizationRobot.cpp
Coin/VisualizationPath.cpp
Coin/VisualizationObject.cpp
+ Coin/VisualizationMesh.cpp
+ Coin/VisualizationEllipsoid.cpp
+ Coin/VisualizationCylindroid.cpp
Coin/Visualizer.cpp
Coin/RegisterVisualizationTypes.cpp
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d2cc827a17e3b5e507ead7888d673cb10d849de
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.cpp
@@ -0,0 +1,63 @@
+/**
+ * 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/>.
+ *
+ * @author ( )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+ #include "VisualizationCylindroid.h"
+
+#include <Inventor/nodes/SoMaterial.h>
+
+ bool
+ armarx::viz::coin::VisualizationCylindroid::update(ElementType const& element)
+ {
+ auto color = element.color;
+ constexpr float conv = 1.0f / 255.0f;
+ const float r = color.r * conv;
+ const float g = color.g * conv;
+ const float b = color.b * conv;
+ const float a = color.a * conv;
+
+ VirtualRobot::VisualizationNodePtr cylindroid_node;
+ {
+ // Params.
+ SoMaterial* mat = new SoMaterial;
+ mat->diffuseColor.setValue(r, g, b);
+ mat->ambientColor.setValue(r, g, b);
+ mat->transparency.setValue(1. - a);
+
+ SoSeparator* res = new SoSeparator();
+ res->ref();
+ SoUnits* u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
+ res->addChild(u);
+ res->addChild(VirtualRobot::CoinVisualizationFactory::CreateCylindroid(
+ element.axisLengths.e0, element.axisLengths.e1, element.height, mat));
+
+ cylindroid_node.reset(new VirtualRobot::CoinVisualizationNode(res));
+ res->unref();
+ }
+
+ SoNode* cylindroid = dynamic_cast<VirtualRobot::CoinVisualizationNode&>(*cylindroid_node)
+ .getCoinVisualization();
+
+ node->removeAllChildren();
+ node->addChild(cylindroid);
+
+ return true;
+ }
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
index 478f99c019e82321a860f7b6a4be18454f97064a..ded11d25008b2d2525294ac9d5bd4aa317bc2c16 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
@@ -15,43 +15,6 @@ namespace armarx::viz::coin
{
using ElementType = data::ElementCylindroid;
- bool update(ElementType const& element)
- {
- auto color = element.color;
- constexpr float conv = 1.0f / 255.0f;
- const float r = color.r * conv;
- const float g = color.g * conv;
- const float b = color.b * conv;
- const float a = color.a * conv;
-
- VirtualRobot::VisualizationNodePtr cylindroid_node;
- {
- // Params.
- SoMaterial* mat = new SoMaterial;
- mat->diffuseColor.setValue(r, g, b);
- mat->ambientColor.setValue(r, g, b);
- mat->transparency.setValue(1. - a);
-
- SoSeparator* res = new SoSeparator();
- res->ref();
- SoUnits* u = new SoUnits();
- u->units = SoUnits::MILLIMETERS;
- res->addChild(u);
- res->addChild(VirtualRobot::CoinVisualizationFactory::CreateCylindroid(
- element.axisLengths.e0, element.axisLengths.e1, element.height,
- mat));
-
- cylindroid_node.reset(new VirtualRobot::CoinVisualizationNode(res));
- res->unref();
- }
-
- SoNode* cylindroid = dynamic_cast<VirtualRobot::CoinVisualizationNode&>(
- *cylindroid_node).getCoinVisualization();
-
- node->removeAllChildren();
- node->addChild(cylindroid);
-
- return true;
- }
+ bool update(ElementType const& element);
};
}
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..65fa3c22b1457ca322a83fe86e30463df13e34e3
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.cpp
@@ -0,0 +1,31 @@
+#include "VisualizationEllipsoid.h"
+
+#include <Inventor/nodes/SoComplexity.h>
+#include <Inventor/nodes/SoScale.h>
+#include <Inventor/nodes/SoSphere.h>
+
+armarx::viz::coin::VisualizationEllipsoid::VisualizationEllipsoid()
+{
+ complexity = new SoComplexity();
+ complexity->type.setValue(SoComplexity::OBJECT_SPACE);
+ complexity->value.setValue(1.0f);
+
+ scale = new SoScale;
+
+ sphere = new SoSphere();
+ // We create a unit sphere and create an ellipsoid through scaling
+ sphere->radius.setValue(1.0f);
+
+ node->addChild(complexity);
+ node->addChild(scale);
+ node->addChild(sphere);
+}
+
+bool
+armarx::viz::coin::VisualizationEllipsoid::update(ElementType const& element)
+{
+ scale->scaleFactor.setValue(
+ element.axisLengths.e0, element.axisLengths.e1, element.axisLengths.e2);
+
+ return true;
+}
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
index e0c3e4db489cdbe6a6434616348229b68c552abc..e38337629eebaac5d26dfd893e43e605e57f574a 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
@@ -4,9 +4,9 @@
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <Inventor/nodes/SoComplexity.h>
-#include <Inventor/nodes/SoScale.h>
-#include <Inventor/nodes/SoSphere.h>
+class SoComplexity;
+class SoScale;
+class SoSphere;
namespace armarx::viz::coin
{
@@ -14,29 +14,9 @@ namespace armarx::viz::coin
{
using ElementType = data::ElementEllipsoid;
- VisualizationEllipsoid()
- {
- complexity = new SoComplexity();
- complexity->type.setValue(SoComplexity::OBJECT_SPACE);
- complexity->value.setValue(1.0f);
+ VisualizationEllipsoid();
- scale = new SoScale;
-
- sphere = new SoSphere();
- // We create a unit sphere and create an ellipsoid through scaling
- sphere->radius.setValue(1.0f);
-
- node->addChild(complexity);
- node->addChild(scale);
- node->addChild(sphere);
- }
-
- bool update(ElementType const& element)
- {
- scale->scaleFactor.setValue(element.axisLengths.e0, element.axisLengths.e1, element.axisLengths.e2);
-
- return true;
- }
+ bool update(ElementType const& element);
SoMaterial* material = nullptr;
SoComplexity* complexity = nullptr;
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90a5ae9eef31a378436424ab2f65579192e8da85
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.cpp
@@ -0,0 +1,113 @@
+#include "VisualizationMesh.h"
+
+#include <Inventor/nodes/SoCoordinate3.h>
+#include <Inventor/nodes/SoIndexedFaceSet.h>
+#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoMaterialBinding.h>
+#include <Inventor/nodes/SoNormal.h>
+#include <Inventor/nodes/SoShapeHints.h>
+
+namespace armarx::viz::coin
+{
+
+ VisualizationMesh::VisualizationMesh()
+ {
+ SoMaterialBinding* myBinding = new SoMaterialBinding;
+ myBinding->value = SoMaterialBinding::PER_VERTEX_INDEXED;
+
+ materials = new SoMaterial;
+ coords = new SoCoordinate3;
+
+ SoShapeHints* hints = new SoShapeHints;
+ // Disable back culling and enable two-sided lighting
+ hints->vertexOrdering = SoShapeHints::VertexOrdering::COUNTERCLOCKWISE;
+ hints->shapeType = SoShapeHints::ShapeType::UNKNOWN_SHAPE_TYPE;
+
+ faceSet = new SoIndexedFaceSet;
+
+ node->addChild(myBinding);
+ node->addChild(materials);
+ node->addChild(coords);
+ node->addChild(hints);
+ node->addChild(faceSet);
+ }
+
+ bool
+ VisualizationMesh::update(ElementType const& element)
+ {
+ int colorSize = (int)element.colors.size();
+ bool noColorsArray = colorSize == 0;
+ if (colorSize == 0)
+ {
+ colorSize = 1;
+ }
+ matColor.resize(colorSize);
+ transp.resize(colorSize);
+
+ const float conv = 1.0f / 255.0f;
+ if (noColorsArray)
+ {
+ auto color = element.color;
+ float r = color.r * conv;
+ float g = color.g * conv;
+ float b = color.b * conv;
+ float a = color.a * conv;
+ matColor[0].setValue(r, g, b);
+ transp[0] = 1.0f - a;
+ }
+ else
+ {
+ for (int i = 0; i < colorSize; i++)
+ {
+ auto color = element.colors[i];
+ float r = color.r * conv;
+ float g = color.g * conv;
+ float b = color.b * conv;
+ float a = color.a * conv;
+ matColor[i].setValue(r, g, b);
+ transp[i] = 1.0f - a;
+ }
+ }
+
+ // Define colors for the faces
+ materials->diffuseColor.setValuesPointer(colorSize, matColor.data());
+ materials->ambientColor.setValuesPointer(colorSize, matColor.data());
+ materials->transparency.setValuesPointer(colorSize, transp.data());
+
+ // define vertex array
+ int vertexSize = (int)element.vertices.size();
+ vertexPositions.resize(vertexSize);
+ for (int i = 0; i < vertexSize; i++)
+ {
+ auto v = element.vertices[i];
+ vertexPositions[i].setValue(v.e0, v.e1, v.e2);
+ }
+
+ // Define coordinates for vertices
+ coords->point.setValuesPointer(vertexSize, vertexPositions.data());
+
+ int facesSize = (int)element.faces.size();
+ faces.resize(facesSize * 4);
+ matInx.resize(facesSize * 4);
+
+ for (int i = 0; i < facesSize; i++)
+ {
+ auto& face = element.faces[i];
+
+ faces[i * 4 + 0] = face.v0;
+ faces[i * 4 + 1] = face.v1;
+ faces[i * 4 + 2] = face.v2;
+ faces[i * 4 + 3] = SO_END_FACE_INDEX;
+
+ matInx[i * 4 + 0] = face.c0;
+ matInx[i * 4 + 1] = face.c1;
+ matInx[i * 4 + 2] = face.c2;
+ matInx[i * 4 + 3] = SO_END_FACE_INDEX;
+ }
+
+ faceSet->coordIndex.setValuesPointer(faces.size(), faces.data());
+ faceSet->materialIndex.setValuesPointer(matInx.size(), matInx.data());
+
+ return true;
+ }
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h b/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
index 120bb629a6ddb6008ebeb7780e31d4954441be4e..391fe0e140a2f5e907be36871a86837535a63321 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
@@ -4,11 +4,10 @@
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <Inventor/nodes/SoCoordinate3.h>
-#include <Inventor/nodes/SoIndexedFaceSet.h>
-#include <Inventor/nodes/SoMaterialBinding.h>
-#include <Inventor/nodes/SoNormal.h>
-#include <Inventor/nodes/SoShapeHints.h>
+#include <Inventor/SbColor.h>
+
+class SoCoordinate3;
+class SoIndexedFaceSet;
namespace armarx::viz::coin
{
@@ -16,105 +15,9 @@ namespace armarx::viz::coin
{
using ElementType = data::ElementMesh;
- VisualizationMesh()
- {
- SoMaterialBinding* myBinding = new SoMaterialBinding;
- myBinding->value = SoMaterialBinding::PER_VERTEX_INDEXED;
-
- materials = new SoMaterial;
- coords = new SoCoordinate3;
-
- SoShapeHints* hints = new SoShapeHints;
- // Disable back culling and enable two-sided lighting
- hints->vertexOrdering = SoShapeHints::VertexOrdering::COUNTERCLOCKWISE;
- hints->shapeType = SoShapeHints::ShapeType::UNKNOWN_SHAPE_TYPE;
-
- faceSet = new SoIndexedFaceSet;
-
- node->addChild(myBinding);
- node->addChild(materials);
- node->addChild(coords);
- node->addChild(hints);
- node->addChild(faceSet);
- }
-
- bool update(ElementType const& element)
- {
- int colorSize = (int)element.colors.size();
- bool noColorsArray = colorSize == 0;
- if (colorSize == 0)
- {
- colorSize = 1;
- }
- matColor.resize(colorSize);
- transp.resize(colorSize);
-
- const float conv = 1.0f / 255.0f;
- if (noColorsArray)
- {
- auto color = element.color;
- float r = color.r * conv;
- float g = color.g * conv;
- float b = color.b * conv;
- float a = color.a * conv;
- matColor[0].setValue(r, g, b);
- transp[0] = 1.0f - a;
- }
- else
- {
- for (int i = 0; i < colorSize; i++)
- {
- auto color = element.colors[i];
- float r = color.r * conv;
- float g = color.g * conv;
- float b = color.b * conv;
- float a = color.a * conv;
- matColor[i].setValue(r, g, b);
- transp[i] = 1.0f - a;
- }
- }
-
- // Define colors for the faces
- materials->diffuseColor.setValuesPointer(colorSize, matColor.data());
- materials->ambientColor.setValuesPointer(colorSize, matColor.data());
- materials->transparency.setValuesPointer(colorSize, transp.data());
-
- // define vertex array
- int vertexSize = (int)element.vertices.size();
- vertexPositions.resize(vertexSize);
- for (int i = 0; i < vertexSize; i++)
- {
- auto v = element.vertices[i];
- vertexPositions[i].setValue(v.e0, v.e1, v.e2);
- }
-
- // Define coordinates for vertices
- coords->point.setValuesPointer(vertexSize, vertexPositions.data());
-
- int facesSize = (int)element.faces.size();
- faces.resize(facesSize * 4);
- matInx.resize(facesSize * 4);
-
- for (int i = 0; i < facesSize; i++)
- {
- auto& face = element.faces[i];
-
- faces[i * 4 + 0] = face.v0;
- faces[i * 4 + 1] = face.v1;
- faces[i * 4 + 2] = face.v2;
- faces[i * 4 + 3] = SO_END_FACE_INDEX;
-
- matInx[i * 4 + 0] = face.c0;
- matInx[i * 4 + 1] = face.c1;
- matInx[i * 4 + 2] = face.c2;
- matInx[i * 4 + 3] = SO_END_FACE_INDEX;
- }
-
- faceSet->coordIndex.setValuesPointer(faces.size(), faces.data());
- faceSet->materialIndex.setValuesPointer(matInx.size(), matInx.data());
+ VisualizationMesh();
- return true;
- }
+ bool update(ElementType const& element);
SoMaterial* materials;
SoCoordinate3* coords;
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
index 5f6b068fccb635c596f2534068e4516fec63693d..f27e344ca8ab1256bd618d1f86e5c2496abcbfd1 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
@@ -2,6 +2,9 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/SbColor.h>
+
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
index 5b4afb2bb562e0b52aac7bc954ed3f2526058c0f..16df986fe4ef3efa20e84d91392e1475440aed83 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
@@ -1,11 +1,13 @@
#pragma once
+#include <cfloat>
#include "ElementVisualizer.h"
#include <RobotAPI/interface/ArViz/Elements.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoPointSet.h>
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
index 7ea618b063125dce6507245847688ffe29f69e3d..5c185f4539f265fd1fba2f8807142df1052ba9c7 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
@@ -10,6 +10,7 @@
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoFaceSet.h>
#include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoMaterial.h>
namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPose.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPose.h
index 490e192582f1b89baafdedaf12a03d5a0a55bc93..1e5e9719dbfbc546199417da57823d676fe50f2b 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPose.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPose.h
@@ -6,6 +6,7 @@
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <Inventor/nodes/SoAsciiText.h>
#include <Inventor/nodes/SoCube.h>
+#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoTranslation.h>
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 71dad203ab70fab4b411ad891b55b781b9fc7e39..68cfb856e64ef8c225436dec38e9a5a894c493dc 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -9,6 +9,8 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <Inventor/SbColor.h>
+#include <Inventor/nodes/SoMaterial.h>
namespace armarx::viz::coin
{
diff --git a/source/RobotAPI/components/DebugDrawer/CMakeLists.txt b/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
index 131cc850cb846c673d5147154acc3256c43d50c6..5322817c5cf5a12bcc1cc4f5da8ca8b7c0ecf5a5 100644
--- a/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
+++ b/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
@@ -5,6 +5,7 @@ set(COMPONENT_LIBS ArmarXCore RobotAPIInterfaces RobotAPICore)
set(SOURCES
DebugDrawerComponent.cpp
DebugDrawerHelper.cpp
+ DebugDrawerUtils.cpp
)
set(HEADERS
DebugDrawerComponent.h
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 5cffdc6d755be9696f23479a64b4077380228046..928e2d47b22cfb858ed966a77a93d6e892f7673b 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -30,6 +30,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoMaterial.h>
@@ -51,6 +52,7 @@
#include <Inventor/fields/SoMFVec3f.h>
#include <Inventor/fields/SoMFColor.h>
#include <Inventor/nodes/SoShapeHints.h>
+#include <Inventor/nodes/SoMatrixTransform.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 9b7c3709bc92be7256deae774a11b748be5ee14a..7b2e5af08f2b9399cb5a2b482af3870db0689820 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -626,4 +626,3 @@ namespace armarx
using DebugDrawerComponentPtr = IceInternal::Handle<DebugDrawerComponent>;
}
-
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index e65fa6a4f4d0242b686656493ef93dba95cc5b36..e0293363b3b56891468b8c1a88950e6c7e09ae6e 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -23,6 +23,8 @@
#include "DebugDrawerHelper.h"
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 72a4ee76f56373b6e03697b76a9c45e07a3816dd..9da37492f73fd7b08310591a3ed294f6767acce7 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -23,7 +23,7 @@
#pragma once
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <SimoxUtility/shapes/XYConstrainedOrientedBox.h>
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52b2a5872cec93b896e05e836d41ec32de6506c7
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.cpp
@@ -0,0 +1,49 @@
+#include "DebugDrawerUtils.h"
+
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+
+armarx::DebugDrawerTriMesh
+armarx::DebugDrawerUtils::convertTriMesh(const VirtualRobot::TriMeshModel& trimesh)
+{
+ DebugDrawerTriMesh ddMesh;
+
+ auto addVertex = [&](const Eigen::Vector3f& v)
+ {
+ ddMesh.vertices.push_back({v(0), v(1), v(2)});
+ return (int)ddMesh.vertices.size() - 1;
+ };
+
+ auto addColor = [&](const VirtualRobot::VisualizationFactory::Color& c)
+ {
+ ddMesh.colors.push_back(DrawColor{c.r, c.g, c.b, c.transparency});
+ return (int)ddMesh.colors.size() - 1;
+ };
+ ddMesh.faces.reserve(trimesh.faces.size());
+ ddMesh.vertices.reserve(trimesh.vertices.size());
+ ddMesh.colors.reserve(trimesh.colors.size());
+
+ for (auto& f : trimesh.faces)
+ {
+ DebugDrawerFace f2;
+ f2.vertex1.vertexID = addVertex(trimesh.vertices.at(f.id1));
+ f2.vertex2.vertexID = addVertex(trimesh.vertices.at(f.id2));
+ f2.vertex3.vertexID = addVertex(trimesh.vertices.at(f.id3));
+ if (f.idNormal1 != UINT_MAX)
+ {
+ f2.vertex1.normalID = addVertex(trimesh.normals.at(f.idNormal1));
+ f2.vertex2.normalID = addVertex(trimesh.normals.at(f.idNormal2));
+ f2.vertex3.normalID = addVertex(trimesh.normals.at(f.idNormal3));
+ }
+ else
+ {
+ // f2.vertex1.normalID = f2.vertex2.normalID = f2.vertex3.normalID = addVertex(f.normal);
+ f2.normal = {f.normal.x(), f.normal.y(), f.normal.z()};
+ }
+ f2.vertex1.colorID = addColor(trimesh.colors.at(f.idColor1));
+ f2.vertex2.colorID = addColor(trimesh.colors.at(f.idColor2));
+ f2.vertex3.colorID = addColor(trimesh.colors.at(f.idColor3));
+
+ ddMesh.faces.push_back(f2);
+ }
+ return ddMesh;
+}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
index b50b0a9d98870ca210f1459e4445802d06f3e922..7d20ed5fc22362c478b143fe79984b1c90e7d253 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
@@ -23,58 +23,16 @@
*/
#pragma once
-#include <VirtualRobot/Visualization/TriMeshModel.h>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+
#include <Eigen/Core>
namespace armarx
{
class DebugDrawerUtils
{
public:
- static DebugDrawerTriMesh convertTriMesh(const VirtualRobot::TriMeshModel& trimesh)
- {
- DebugDrawerTriMesh ddMesh;
-
- auto addVertex = [&](const Eigen::Vector3f & v)
- {
- ddMesh.vertices.push_back({v(0), v(1), v(2)});
- return (int)ddMesh.vertices.size() - 1;
- };
-
- auto addColor = [&](const VirtualRobot::VisualizationFactory::Color & c)
- {
- ddMesh.colors.push_back(DrawColor {c.r, c.g, c.b, c.transparency});
- return (int)ddMesh.colors.size() - 1;
- };
- ddMesh.faces.reserve(trimesh.faces.size());
- ddMesh.vertices.reserve(trimesh.vertices.size());
- ddMesh.colors.reserve(trimesh.colors.size());
-
- for (auto& f : trimesh.faces)
- {
- DebugDrawerFace f2;
- f2.vertex1.vertexID = addVertex(trimesh.vertices.at(f.id1));
- f2.vertex2.vertexID = addVertex(trimesh.vertices.at(f.id2));
- f2.vertex3.vertexID = addVertex(trimesh.vertices.at(f.id3));
- if (f.idNormal1 != UINT_MAX)
- {
- f2.vertex1.normalID = addVertex(trimesh.normals.at(f.idNormal1));
- f2.vertex2.normalID = addVertex(trimesh.normals.at(f.idNormal2));
- f2.vertex3.normalID = addVertex(trimesh.normals.at(f.idNormal3));
- }
- else
- {
- // f2.vertex1.normalID = f2.vertex2.normalID = f2.vertex3.normalID = addVertex(f.normal);
- f2.normal = {f.normal.x(), f.normal.y(), f.normal.z()};
- }
- f2.vertex1.colorID = addColor(trimesh.colors.at(f.idColor1));
- f2.vertex2.colorID = addColor(trimesh.colors.at(f.idColor2));
- f2.vertex3.colorID = addColor(trimesh.colors.at(f.idColor3));
-
- ddMesh.faces.push_back(f2);
- }
- return ddMesh;
- }
-
+ static DebugDrawerTriMesh convertTriMesh(const VirtualRobot::TriMeshModel& trimesh);
};
}
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index 8eb4b47b27281850862e414d80654c61639e3ba7..17292ba93b00511d251c50a3241859c38ce127c4 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -28,6 +28,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/math/Helpers.h>
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
index e94e1a7cc2aa544650681583ca6a1aae11f9bef6..783675ba07f54dfe80ab569c8fe65a5b836ca23f 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
@@ -1,6 +1,7 @@
#pragma once
#include <list>
+#include <queue>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index 7fea318b5249b557e92cb1e207f3f04e0de70321..f4854b20d1f62ffff23e7f441d24a736f90ec8c9 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -31,6 +31,7 @@
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/color/cmaps.h>
#include <SimoxUtility/math/pose/pose.h>
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
index c5e6486e7ded27538c715f1ba48531a1fd552b1f..ca3344af472d7fc3ee8b4b2e02a9134d5a05c892 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
@@ -22,6 +22,8 @@
#pragma once
+#include <mutex>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/Frequency.h>
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index ff55654b049adfe37980d243b1717e69c5938d73..ea5d2df495d8f427695f8778562035439f632397 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -18,12 +18,13 @@
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/time/forward_declarations.h>
-#include "RobotAPI/components/ArViz/Client/elements/Robot.h"
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/components/ArViz/Client/elements/Color.h>
+#include <RobotAPI/components/ArViz/Client/elements/Robot.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
@@ -48,6 +49,11 @@ namespace armarx::armem::server::grasp
defs->optional(enableVisualizeKnownGraspCandidates,
"p.enableVisualizeKnownGraspCandidates");
+ defs->optional(enableVisualizeKnownGraspCandidatesPreposes,
+ "p.enableVisualizeKnownGraspCandidatesPreposes");
+ defs->optional(enableVisualizeKnownGraspCandidatesApproach,
+ "p.enableVisualizeKnownGraspCandidatesApproach");
+
defs->optional(frequencyHzVisualizeKnownGraspCandidates,
"p.frequencyHzVisualizeKnownGraspCandidates");
@@ -1016,16 +1022,55 @@ namespace armarx::armem::server::grasp
const Eigen::Isometry3f eef_T_hand_root{
handRootNode->getPoseInFrame(tcp)};
- const Eigen::Isometry3f pose =
+ const Eigen::Isometry3f global_T_grasp_pose =
Eigen::Isometry3f{object.objectPoseGlobal} *
- Eigen::Isometry3f{grasp.pose}.inverse() * eef_T_hand_root;
+ Eigen::Isometry3f{grasp.pose}.inverse();
+
+ // visualize grasp pose
+ {
+ viz::Robot graspHandVisu(visuName + "_grasp");
+ graspHandVisu.useCollisionModel()
+ .file(visuRobotPartXML)
+ .pose(
+ Eigen::Isometry3f{global_T_grasp_pose * eef_T_hand_root}
+ .matrix());
+
+ layer.add(graspHandVisu);
+ }
+
+ // visualize prepose if available
+ if (grasp.prepose.has_value())
+ {
+ const Eigen::Isometry3f global_T_prepose =
+ Eigen::Isometry3f{object.objectPoseGlobal} *
+ Eigen::Isometry3f{grasp.prepose.value()}.inverse();
- viz::Robot handVisu(visuName);
- handVisu.useCollisionModel()
- .file(visuRobotPartXML)
- .pose(pose.matrix());
+ // visualize as prepose as hand
+ if (enableVisualizeKnownGraspCandidatesPreposes)
+ {
+ viz::Robot graspHandVisuPrepose(visuName + "_prepose");
+ graspHandVisuPrepose.useCollisionModel()
+ .file(visuRobotPartXML)
+ .pose(Eigen::Isometry3f{global_T_prepose *
+ eef_T_hand_root}
+ .matrix())
+ .overrideColor(viz::Color::blue());
+
+ layer.add(graspHandVisuPrepose);
+ }
+
+ // visualize approach direction (arrow between poses)
+ if (enableVisualizeKnownGraspCandidatesApproach)
+ {
+ viz::Arrow arrow(visuName + "_approach");
+ arrow
+ .fromTo(global_T_prepose.translation(),
+ global_T_grasp_pose.translation())
+ .color(viz::Color::green());
- layer.add(handVisu);
+ layer.add(arrow);
+ }
+ }
}
}
}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index 40ef25686fc4c8b545bd0aa49ed787c4035ab7c5..7649e1f4ef73da60db7e7e14c3d95c1de004b9ff 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -2,7 +2,7 @@
#include <memory>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
@@ -120,6 +120,10 @@ namespace armarx::armem::server::grasp
bool enableRemoteGui{true};
bool enableVisualizeKnownGraspCandidates = false;
+ bool enableVisualizeKnownGraspCandidatesPreposes = false;
+ bool enableVisualizeKnownGraspCandidatesApproach = false;
+
+
std::size_t frequencyHzVisualizeKnownGraspCandidates = 5;
void visualizeGraspCandidates();
diff --git a/source/RobotAPI/components/ik_demo/IkDemo.cpp b/source/RobotAPI/components/ik_demo/IkDemo.cpp
index 2d60ef2feeae53056890925f44a799a7ea8f5b10..4ff605566ffec008e8cbdd811554d070cbc3f36d 100644
--- a/source/RobotAPI/components/ik_demo/IkDemo.cpp
+++ b/source/RobotAPI/components/ik_demo/IkDemo.cpp
@@ -5,6 +5,7 @@
#include <SimoxUtility/math/pose/invert.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h>
#include <VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h>
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
index 6dcea52a55191f03716aea507d13cd6c831d8d87..4b3e23d3e5c4c24224d44893ee446b8d5f833f1c 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
@@ -1,5 +1,6 @@
#include "Callback.h"
+#include "RobotAPI/libraries/aron/core/data/variant/primitive/String.h"
namespace armarx::skills::provider
{
diff --git a/source/RobotAPI/components/units/HandUnit.cpp b/source/RobotAPI/components/units/HandUnit.cpp
index 0b19a5566bbf07fdd4f6805ff0cbcedc181a150b..28cbd6157b2f03757dd0c6f476a9ea61caa48bbb 100644
--- a/source/RobotAPI/components/units/HandUnit.cpp
+++ b/source/RobotAPI/components/units/HandUnit.cpp
@@ -25,19 +25,20 @@
#include "HandUnit.h"
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <vector>
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/EndEffector/EndEffector.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <vector>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
using namespace armarx;
using namespace VirtualRobot;
-
-void HandUnit::onInitComponent()
+void
+HandUnit::onInitComponent()
{
std::string endeffectorFile = getProperty<std::string>("RobotFileName").getValue();
std::string endeffectorName = getProperty<std::string>("EndeffectorName").getValue();
@@ -50,7 +51,8 @@ void HandUnit::onInitComponent()
try
{
- robot = VirtualRobot::RobotIO::loadRobot(endeffectorFile, VirtualRobot::RobotIO::eStructure);
+ robot =
+ VirtualRobot::RobotIO::loadRobot(endeffectorFile, VirtualRobot::RobotIO::eStructure);
}
catch (VirtualRobot::VirtualRobotException& e)
{
@@ -86,7 +88,7 @@ void HandUnit::onInitComponent()
for (size_t i = 0; i < actors.size(); i++)
{
EndEffectorActorPtr a = actors[i];
- std::vector<EndEffectorActor::ActorDefinition> ads;// = a->getDefinition();
+ std::vector<EndEffectorActor::ActorDefinition> ads; // = a->getDefinition();
for (size_t j = 0; j < ads.size(); j++)
{
@@ -99,7 +101,8 @@ void HandUnit::onInitComponent()
}
}
- const std::vector<std::string> endeffectorPreshapes = robot->getEndEffector(endeffectorName)->getPreshapes();
+ const std::vector<std::string> endeffectorPreshapes =
+ robot->getEndEffector(endeffectorName)->getPreshapes();
shapeNames = new SingleTypeVariantList(VariantType::String);
std::vector<std::string>::const_iterator iter = endeffectorPreshapes.begin();
@@ -120,8 +123,8 @@ void HandUnit::onInitComponent()
this->onInitHandUnit();
}
-
-void HandUnit::onConnectComponent()
+void
+HandUnit::onConnectComponent()
{
ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
listenerPrx = getTopic<HandUnitListenerPrx>(listenerChannelName);
@@ -129,45 +132,48 @@ void HandUnit::onConnectComponent()
this->onStartHandUnit();
}
-
-void HandUnit::onExitComponent()
+void
+HandUnit::onExitComponent()
{
this->onExitHandUnit();
}
-
-
-void HandUnit::setShape(const std::string& shapeName, const Ice::Current& c)
+void
+HandUnit::setShape(const std::string& shapeName, const Ice::Current& c)
{
}
-void HandUnit::setShapeWithObjectInstance(const std::string& shapeName, const std::string& objectInstanceName, const Ice::Current& c)
+void
+HandUnit::setShapeWithObjectInstance(const std::string& shapeName,
+ const std::string& objectInstanceName,
+ const Ice::Current& c)
{
ARMARX_WARNING << "setShapeWithObjectInstance Function not implemented!";
}
-
-SingleTypeVariantListBasePtr HandUnit::getShapeNames(const Ice::Current& c)
+SingleTypeVariantListBasePtr
+HandUnit::getShapeNames(const Ice::Current& c)
{
return shapeNames;
}
-
-PropertyDefinitionsPtr HandUnit::createPropertyDefinitions()
+PropertyDefinitionsPtr
+HandUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new HandUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new HandUnitPropertyDefinitions(getConfigIdentifier()));
}
-
-NameValueMap HandUnit::getShapeJointValues(const std::string& shapeName, const Ice::Current&)
+NameValueMap
+HandUnit::getShapeJointValues(const std::string& shapeName, const Ice::Current&)
{
- EndEffectorPtr efp = robot->getEndEffector(getProperty<std::string>("EndeffectorName").getValue());
+ EndEffectorPtr efp =
+ robot->getEndEffector(getProperty<std::string>("EndeffectorName").getValue());
RobotConfigPtr rc = efp->getPreshape(shapeName);
return rc->getRobotNodeJointValueMap();
}
-NameValueMap HandUnit::getCurrentJointValues(const Ice::Current& c)
+NameValueMap
+HandUnit::getCurrentJointValues(const Ice::Current& c)
{
NameValueMap result;
@@ -179,42 +185,44 @@ NameValueMap HandUnit::getCurrentJointValues(const Ice::Current& c)
return result;
}
-void HandUnit::setObjectGrasped(const std::string& objectName, const Ice::Current&)
+void
+HandUnit::setObjectGrasped(const std::string& objectName, const Ice::Current&)
{
ARMARX_INFO << "Object grasped " << objectName << flush;
graspedObject = objectName;
}
-void HandUnit::setObjectReleased(const std::string& objectName, const Ice::Current&)
+void
+HandUnit::setObjectReleased(const std::string& objectName, const Ice::Current&)
{
ARMARX_INFO << "Object released " << objectName << flush;
graspedObject = "";
}
-
-
-std::string armarx::HandUnit::getHandName(const Ice::Current&)
+std::string
+armarx::HandUnit::getHandName(const Ice::Current&)
{
return eef->getName();
}
-void HandUnit::setJointForces(const NameValueMap& targetJointForces, const Ice::Current&)
+void
+HandUnit::setJointForces(const NameValueMap& targetJointForces, const Ice::Current&)
{
-
}
-void HandUnit::sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&)
+void
+HandUnit::sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&)
{
-
}
-
-void armarx::HandUnit::setJointAngles(const armarx::NameValueMap& targetJointAngles, const Ice::Current& c)
+void
+armarx::HandUnit::setJointAngles(const armarx::NameValueMap& targetJointAngles,
+ const Ice::Current& c)
{
-
}
-std::string HandUnit::describeHandState(const Ice::Current&)
+std::string
+HandUnit::describeHandState(const Ice::Current&)
{
return "not implemented";
}
@@ -224,3 +232,15 @@ armarx::HandUnit::reloadPreshapes(const Ice::Current&)
{
ARMARX_WARNING << "`reloadPreshapes` not implemented for this hand unit!";
}
+
+void
+armarx::HandUnit::tare(const Ice::Current&)
+{
+ ARMARX_WARNING << "Taring is not supported by this hand.";
+}
+
+void
+armarx::HandUnit::resetFirmware(const Ice::Current&)
+{
+ ARMARX_WARNING << "Resetting firmware is not supported by this hand.";
+}
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index c68f915579890d59fb96a1c4d811041477a13ec6..43fb4d00c9799905df3a063a8a51a50f727d6c5f 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -24,16 +24,14 @@
#pragma once
-#include <RobotAPI/components/units/SensorActorUnit.h>
-
-#include <RobotAPI/interface/units/HandUnitInterface.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
-#include <VirtualRobot/VirtualRobot.h>
-
+#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
namespace armarx
{
@@ -41,19 +39,20 @@ namespace armarx
* \class HandUnitPropertyDefinitions
* \brief Defines all necessary properties for armarx::HandUnit
*/
- class HandUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class HandUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- HandUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ HandUnitPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotFileName", "VirtualRobot XML file in which the endeffector is is stored.");
- defineRequiredProperty<std::string>("EndeffectorName", "Name of the endeffector as stored in the XML file (will publish values on EndeffectorName + 'State')");
+ defineRequiredProperty<std::string>(
+ "RobotFileName", "VirtualRobot XML file in which the endeffector is is stored.");
+ defineRequiredProperty<std::string>(
+ "EndeffectorName",
+ "Name of the endeffector as stored in the XML file (will publish values on "
+ "EndeffectorName + 'State')");
}
};
-
/**
* @ingroup Component-HandUnit HandUnit
* \brief Base unit for high-level access to robot hands.
@@ -68,13 +67,12 @@ namespace armarx
* @brief The HandUnit class
* @ingroup Component-HandUnit
*/
- class HandUnit :
- virtual public HandUnitInterface,
- virtual public SensorActorUnit
+ class HandUnit : virtual public HandUnitInterface, virtual public SensorActorUnit
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "HandUnit";
}
@@ -112,7 +110,8 @@ namespace armarx
/**
* Send command to the hand to form a specific shape position. The shapes are defined in the robot.xml file.
*/
- void setShape(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setShape(const std::string& shapeName,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* @brief setShapeWithObjectInstance Send command to the hand to form a specific shape position.
@@ -122,14 +121,18 @@ namespace armarx
* @param shapeName Name of the well known shape that the hand should form
* @param graspedObjectInstanceName name of the object instance which is used to check for collisions while setting the shape
*/
- void setShapeWithObjectInstance(const std::string& shapeName, const std::string& objectInstanceName, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setShapeWithObjectInstance(const std::string& shapeName,
+ const std::string& objectInstanceName,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \return a list of strings for shape positions which can be used together with HandUnit::shape().
*/
- SingleTypeVariantListBasePtr getShapeNames(const Ice::Current& c = Ice::emptyCurrent) override;
+ SingleTypeVariantListBasePtr
+ getShapeNames(const Ice::Current& c = Ice::emptyCurrent) override;
- NameValueMap getShapeJointValues(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;
+ NameValueMap getShapeJointValues(const std::string& shapeName,
+ const Ice::Current& c = Ice::emptyCurrent) override;
NameValueMap getCurrentJointValues(const Ice::Current& c = Ice::emptyCurrent) override;
void setObjectGrasped(const std::string& objectName, const Ice::Current&) override;
@@ -145,6 +148,10 @@ namespace armarx
void reloadPreshapes(const Ice::Current&) override;
+ void tare(const Ice::Current&) override;
+
+ void resetFirmware(const Ice::Current&) override;
+
protected:
/**
* HandUnitListener proxy for publishing state updates
@@ -167,7 +174,7 @@ namespace armarx
std::string robotName;
std::string tcpName;
- std::map <std::string, float> handJoints;
+ std::map<std::string, float> handJoints;
std::string graspedObject;
@@ -176,6 +183,7 @@ namespace armarx
public:
std::string getHandName(const Ice::Current& = Ice::emptyCurrent) override;
void setJointForces(const NameValueMap& targetJointForces, const Ice::Current&) override;
- void sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&) override;
+ void sendJointCommands(const NameCommandMap& targetJointCommands,
+ const Ice::Current&) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index 5bac1a14bec7705c4cd2e03721c0beda91d545c5..c2e0dc1854b402f00beb894231cf44d1377ff75a 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -28,6 +28,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/application/Application.h>
diff --git a/source/RobotAPI/components/units/KinematicUnit.h b/source/RobotAPI/components/units/KinematicUnit.h
index fb29aac629722ad2ae563dd33461544a6da48386..74146b034692247907da52d1cb5dfa4ee604d772 100644
--- a/source/RobotAPI/components/units/KinematicUnit.h
+++ b/source/RobotAPI/components/units/KinematicUnit.h
@@ -32,7 +32,8 @@
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <vector>
namespace armarx
@@ -148,4 +149,3 @@ namespace armarx
std::vector<std::string> armarXPackages;
};
}
-
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
index d18722607d92879c22184fd8afc06a5ed0fa9bb9..5f51021681e02c7162cdc194372f199ac6e6408d 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -28,7 +28,6 @@
#include <algorithm>
#include <cmath>
#include <limits>
-#include <numeric>
// Eigen
#include <Eigen/Core>
@@ -36,6 +35,8 @@
// Simox
#include <SimoxUtility/math.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Safety.h>
// ArmarX
#include <ArmarXCore/observers/variant/Variant.h>
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
index 1bc78b92b4ba4edd0811d7324d2a2f811aa2ee15..7ddf5b4a09a50db16e0c7ee68ab191d6738d9bb7 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
@@ -25,9 +25,7 @@
// STD/STL
-#include <deque>
#include <string>
-#include <tuple>
#include <mutex>
#include <vector>
@@ -38,7 +36,7 @@
#include <IceUtil/Time.h>
// Simox
-#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Safety.h>
// ArmarX
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
index 5ea5de5a0decff5792d66ea890a1e39729c5c4a0..8042b243406d074c7055b33502c6d022fee5deb0 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
@@ -9,6 +9,15 @@
#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+
using namespace armarx;
int
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
index 0c0ed889cebc1671c436eee9226dc79457850a25..46704a4816bcca9a5265143d13ef3eda16fb66e9 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
@@ -1,13 +1,16 @@
#pragma once
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/VirtualRobot.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
namespace armarx
{
+ class SensorValue1DoFActuatorTorque;
+ class SensorValue1DoFActuatorVelocity;
+ class SensorValue1DoFActuatorPosition;
+
class CartesianImpedanceController
{
public:
diff --git a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
index 0ed24cb2fb0459f76a6a373d9c1cd3768c733181..70f06887df16c47032960a8044755c461bb6a696 100644
--- a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
@@ -22,6 +22,8 @@
#include "DefaultWidgetDescriptions.h"
+#include <VirtualRobot/Robot.h>
+
namespace armarx::WidgetDescription
{
StringComboBoxPtr
@@ -86,4 +88,34 @@ namespace armarx::WidgetDescription
}
return rns;
}
+
+ StringComboBoxPtr
+ makeRNSComboBox(const VirtualRobot::RobotPtr& robot,
+ std::string name,
+ const std::set<std::string>& preferredSet,
+ const std::string& mostPreferred)
+ {
+ return makeStringSelectionComboBox(
+ std::move(name), robot->getRobotNodeSetNames(), preferredSet, mostPreferred);
+ }
+
+ StringComboBoxPtr
+ makeRobotNodeComboBox(const VirtualRobot::RobotPtr& robot,
+ std::string name,
+ const std::set<std::string>& preferredSet,
+ const std::string& mostPreferred)
+ {
+ return makeStringSelectionComboBox(
+ std::move(name), robot->getRobotNodeNames(), preferredSet, mostPreferred);
+ }
+
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred,
+ const std::set<std::string>& preferredSet)
+ {
+ return makeStringSelectionComboBox(
+ std::move(name), std::move(options), preferredSet, mostPreferred);
+ }
} // namespace armarx::WidgetDescription
diff --git a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
index e276010f22b2d70b6b6b37fac642991020df968c..3da3c5b5451964d5c83e831489ff6406d048948c 100644
--- a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
+++ b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
@@ -25,7 +25,7 @@
#include <set>
#include <string>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXGui/interface/WidgetDescription.h>
#include <ArmarXGui/libraries/DefaultWidgetDescriptions/DefaultWidgetDescriptions.h>
@@ -57,33 +57,18 @@ namespace armarx::WidgetDescription
const std::set<std::string>& preferredSet,
const std::string& mostPreferred);
- inline StringComboBoxPtr
- makeStringSelectionComboBox(std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred,
- const std::set<std::string>& preferredSet)
- {
- return makeStringSelectionComboBox(
- std::move(name), std::move(options), preferredSet, mostPreferred);
- }
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred,
+ const std::set<std::string>& preferredSet);
- inline StringComboBoxPtr
- makeRNSComboBox(const VirtualRobot::RobotPtr& robot,
- std::string name = "RobotNodeSet",
- const std::set<std::string>& preferredSet = {},
- const std::string& mostPreferred = "")
- {
- return makeStringSelectionComboBox(
- std::move(name), robot->getRobotNodeSetNames(), preferredSet, mostPreferred);
- }
+ StringComboBoxPtr makeRNSComboBox(const VirtualRobot::RobotPtr& robot,
+ std::string name = "RobotNodeSet",
+ const std::set<std::string>& preferredSet = {},
+ const std::string& mostPreferred = "");
- inline StringComboBoxPtr
- makeRobotNodeComboBox(const VirtualRobot::RobotPtr& robot,
- std::string name = "RobotNode",
- const std::set<std::string>& preferredSet = {},
- const std::string& mostPreferred = "")
- {
- return makeStringSelectionComboBox(
- std::move(name), robot->getRobotNodeNames(), preferredSet, mostPreferred);
- }
+ StringComboBoxPtr makeRobotNodeComboBox(const VirtualRobot::RobotPtr& robot,
+ std::string name = "RobotNode",
+ const std::set<std::string>& preferredSet = {},
+ const std::string& mostPreferred = "");
} // namespace armarx::WidgetDescription
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 67e940d30ad6899cc9773c14c950f8207092f905..09338fd57c5de9986647bbd3333f33e7e7925125 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -2,6 +2,7 @@
#include <iomanip>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
index ec93568c8452b7268c5ad2212ddb9df954e8b9ce..26fa0111d4f48e868573fbffc3590da3e7233fd6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
@@ -24,6 +24,8 @@
#include "NJointCartesianTorqueController.h"
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
index 91326e4935517bb6b05457e2843615647a7fa05c..b02c292f635089f09c5553d56a6cf3aa5f3db985 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
@@ -23,9 +23,8 @@
*/
#pragma once
+#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index 486eaa77564de408fcd7c57da71eb1c73e6e93f1..165da2d63db3e5e085eb1c02eb8b229b81a0fd66 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
@@ -24,10 +24,14 @@
#include "NJointCartesianVelocityController.h"
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#define DEFAULT_TCP_STRING "default TCP"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index 35f3a9375629e1a6b2d42c2d9205290b3d2fd4b1..1c0755674bd5ae042858d5138a053109ccbcbedd 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
@@ -24,17 +24,19 @@
#pragma once
-#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/VirtualRobot.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
namespace armarx
{
+ class ControlTarget1DoFActuatorVelocity;
+ class SensorValue1DoFActuatorTorque;
+ class SensorValue1DoFGravityTorque;
+
+
TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityController);
TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerControlData);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 90ef15b6185b7424ef70714af3eec4b16b6a0ba6..ea5ab7c70dbd998b8b0b4e3c549c2712f4649ea1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -24,6 +24,8 @@
#include "NJointCartesianVelocityControllerWithRamp.h"
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
index 40bba081c0c63eaa83edb2454c03dfb73ce2bfe3..34d99731e249308d5a6685c017b9e56031cdadb5 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -23,9 +23,7 @@
*/
#pragma once
-
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
#include <RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 08870cfcc27981ee9e0e97f096563a35a1f51507..943f878897d4eb3e0e08b1fa8a23ae7b686397d3 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -1,6 +1,8 @@
#include "NJointCartesianWaypointController.h"
#include <iomanip>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
index 3239c0e831c96536d442cba4531b2615a1bfe034..76cf0ca051ae2bf2d31b028f074460b6c31d0783 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
@@ -23,7 +23,6 @@
#pragma once
#include <atomic>
-#include <functional>
#include <map>
#include <mutex>
#include <optional>
@@ -35,8 +34,10 @@
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+// #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
namespace IceProxy::armarx
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 2956bbcf81a23ed99c16ac0ee8dc7ab5b697ace0..c02af15ef7fc7888bf2995b4876ea8d92671f313 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -28,6 +28,7 @@
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
+#include <VirtualRobot/Robot.h>
#include "ArmarXCore/core/logging/Logging.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index f59f1b7bef8c52d860490b75f1dc8e2f9eb6462e..f13f66fe7770cb8f3b7f77791e6a73e696edf5ee 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -26,7 +26,8 @@
#include <atomic>
-#include <VirtualRobot/Robot.h>
+
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index cca564afe0ad449f947c3e5be351f9b7ce7952ba..1e827dcc163c5417515ebb006038f025950e9a51 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -27,6 +27,8 @@
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <VirtualRobot/Robot.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformRelativePositionController>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index 62760e241a7b2a4c7b4457fa6669545d040dff63..ba3c5ba50962c864ff1e96ab8f4446aa2664e01b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -26,7 +26,7 @@
#include <atomic>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index 892a1ae7e554387630efa35d048ee6cd44f02584..31870bd96e67c11bbe85aca9352f96d00b679152 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -22,6 +22,8 @@
#include "NJointHolonomicPlatformUnitVelocityPassThroughController.h"
+#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index ef08635a5f8329e80020f47b1eb8b49b3a894b7a..c692e00dea00d7c5d3044054991ba6aaffd9ae39 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -22,7 +22,8 @@
#pragma once
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
index 972f3abd1bb5d9a0ad34582c19033ae635e7ac80..e903a1a5d9e606b62b3f8ffba8f11d1fe9886d42 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
@@ -22,9 +22,14 @@
#include "NJointHolonomicPlatformVelocityControllerWithRamp.h"
+#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h"
+#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
namespace armarx
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
index 8cd957b50d7911524c6e5d68a91ad4359213f145..795b16c8ef2f9d50717ec3ee42ea06d866305f3a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
@@ -22,15 +22,16 @@
#pragma once
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
-#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
-#include "NJointControllerWithTripleBuffer.h"
+// #include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
+ class ControlTargetHolonomicPlatformVelocity;
+ class SensorValueHolonomicPlatformVelocity;
class Cartesian2DimVelocityRamp
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index e51de4dbdb2c7b903d83aeb462d9b9fd8f6a695d..be1af27f6be2ad526b49f178464c4a32bad31ce0 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -22,6 +22,8 @@
#include "NJointKinematicUnitPassThroughController.h"
+#include <VirtualRobot/Robot.h>
+
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index ab280b2245ff23cecbb5062969168dca312abc2f..9f38d25d491b275c944a988f76a36a32880890ab 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -24,7 +24,7 @@
#include <atomic>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index 12ca8b5a1ec6146599034643989c0a668704d888..1c3a68e75ac1131f46372a5bdaa68442b849788a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -24,6 +24,8 @@
#include "NJointTCPController.h"
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index ecf1fc2666dded6b34c9f46850d1b00f7b0c09de..311df4838b1194c954f1ac1e23d4e754ff56434c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -23,9 +23,8 @@
*/
#pragma once
-
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/IK/IKSolver.h>
#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index e4b850fcaea1b866c2b6c6394d77934ffdfd5998..069b296afe87fddbe6493b91007089b66fdac58a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -25,6 +25,10 @@
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
using namespace armarx;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index 42ce54179acab6ab646191155c259a8a05f1e38a..47706d9dcc9ccd835750cad1b51ee0a91a22ac03 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -22,8 +22,7 @@
#pragma once
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index 4e79acb8ff78a77e739ac8b2ee867ca8b64df701..e4a068265734fffb5255660be5302f1e935ada3b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -1,6 +1,8 @@
#include "NJointTrajectoryController.h"
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index eb13d7c90f16f4fe14ce3b70b26928f569aa8343..95b90d9c7ab7219219b2ce944490a2b097cc3755 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -1,6 +1,6 @@
#pragma once
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXGui/interface/StaticPlotterInterface.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 29f4b175adc0de5bbbca50aa92730d8871c31947..2d3a8b5ea8c779fb063a0aa820f9aae418e5a0e4 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -29,4 +29,9 @@ namespace armarx
{
}
+ armarx::PropertyDefinitionsPtr
+ RobotUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
+ }
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 0c97bcd05f4e4d6f4231839b23a99bcec8131c0d..b22844a4ec70de140442e173458c7ae5e01ad287 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -200,10 +200,6 @@ namespace armarx
}
/// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr
- createPropertyDefinitions() override
- {
- return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
- }
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
index 732a0e2c479ed852a1162551f881c67c3fa36489..a2cc1aa25d67e8c84eed521cea12e9a4e55b279f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -24,6 +24,8 @@
#include <sstream>
+#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/Preprocessor.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
index b76f54dc8b0c5de7052552f202f62c9eefcce05a..29732afdeb43944f828b05287439fa856a178397 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
@@ -30,7 +30,7 @@
#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/preprocessor/variadic/to_seq.hpp>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/util/CPPUtility/Pointer.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 6f01acc17ee6c7cf5ede5925d4deffa68ceeca4e..6b65c43badf87a905fb0a60b3ffe87f40fe51998 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -23,6 +23,7 @@
#include "RobotUnitModuleControlThread.h"
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
+#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/time/TimeUtil.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
index 99b4a64e42605d0449e62145b869d8d64972aba7..d9e4084164fb2144b5d4bd2c823c11b6177154ae 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
@@ -24,6 +24,8 @@
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <VirtualRobot/Robot.h>
+
#include "RobotUnitModuleControllerManagement.h"
#include "RobotUnitModuleDevices.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
index 1833042321debe6dbd1487e9d5af4a673c670ac5..d9ed47b7b6775296ec52cefebb77b32aff989bad 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
@@ -22,7 +22,8 @@
#pragma once
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <ArmarXCore/core/util/TripleBuffer.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
index 0c7caccb3cf6fa840928c6a5907f483e21690d5a..4702668c566ab4413b621f66807209bcb38607d6 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
@@ -24,6 +24,10 @@
#include <mutex>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
+
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
@@ -259,6 +263,8 @@ namespace armarx::RobotUnitModule
* @param nodes The VirtualRobot's RobotNodes
* @param sensors The \ref SensorValue "SensorValues"
*/
+
+ // TODO use base type for 'sensors' element
template <class PtrT>
void
updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot,
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index b2ae894908ffc85a0bc906254f93bc4a31f1f5c5..033d869628eb78f86dd723a8671feb26063df5a1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -24,7 +24,9 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index c6e94bd515bea5a6f88597c935eb277c234a33d2..d3e1e68f7ff9da6ba782e7c9454c1faefcbb8c92 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -22,7 +22,7 @@
#pragma once
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 698df2657899773a1a111bd02ce561abd66308ef..1ebf5d476627e4503c355298b96d655ce11a19f0 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -27,8 +27,11 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Robot.h>
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Metronome.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
index fa7e690cd602045f3ed0883e81fb0e3956822b69..36798242c9e365fa26bc9a5db0dc93123da623aa 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
@@ -24,7 +24,7 @@
#include <thread>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
index 776bb731d7ab5115953fb6cb044c0f385ce6e7a2..707800ad73868e83a5b49a0d159121beb8eb20c7 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
@@ -23,7 +23,8 @@
#include <RobotAPI/libraries/core/Pose.h>
-#include "Eigen/Core"
+#include <Eigen/Core>
+
#include "SensorValueBase.h"
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
index debb6a1a7d61f2cc348dd8214dea4f5e512bdf1e..2dc836d8968f0c52974e8b1431a86bc2abea2e2c 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
@@ -25,6 +25,7 @@
#include <Eigen/Geometry>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/interface/core/GeometryBase.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
index c40aa7e4460792dae1ae0e8f5519edb80766a31f..bc679526a8af3080a27de60f98f6ae39b4e0dac4 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
@@ -26,7 +26,6 @@
#include <Eigen/Core>
-#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/core/RobotState.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index c7e7348c57092485ddc70a1e5a18487b091cd358..4186fa390c094c673efe2fcf12509c9efedec2f2 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -25,6 +25,7 @@
#include <Eigen/Geometry>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/interface/core/GeometryBase.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index dd41099f1f79411eb23fcd7e484b9c57b7cd42a8..dc081d781946ac3fe52efb56db4aef88329f9dd6 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -26,8 +26,6 @@
#include <Eigen/Core>
-#include <VirtualRobot/MathTools.h>
-
#include <RobotAPI/components/units/PlatformUnit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
index 79d076f9b2aa7202f4ad649f76090b034da2f6db..bc22d972c5bf6eb3830fc5f3a1b7e8ef12949dfc 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
@@ -28,6 +28,10 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+
+#include <VirtualRobot/Dynamics/Dynamics.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
namespace armarx
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
index edcf445b56cc0354889cce4475147aabf1605c2b..e49736123c26cf94e184d78f0445e4f46cf23754 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
@@ -24,8 +24,8 @@
#pragma once
#include <Eigen/Core>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Dynamics/Dynamics.h>
-#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 4f79878eef58dbe407b744780f8b50203ca26a04..08557d8a0649565caf404908ed2fb979ac095d9f 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -28,10 +28,12 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/MathTools.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <Eigen/Core>
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 8b373d10178d0975bbf78831b08859c15781f9b6..8811a63dd792a7186c12cd8d2cd05d55220655ca 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -27,7 +27,9 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
@@ -280,4 +282,3 @@ namespace armarx
using EDifferentialIKPtr = std::shared_ptr<EDifferentialIK>;
}
-
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 945013a9905234d8eb807745bea34e04c8456b31..fc014c5adab046771815191a4c1e997602dca519 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -22,10 +22,10 @@
#include "ArVizWidgetController.h"
-#include <ArmarXCore/observers/variant/Variant.h>
-
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
#include <QLineEdit>
#include <QMessageBox>
#include <QTimer>
@@ -93,7 +93,6 @@ namespace armarx
connect(this, &This::disconnectGui, this, &This::onDisconnectGui, Qt::QueuedConnection);
// Layer info tree.
-
connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
connect(widget.layerInfoTreeGroupBox, &QGroupBox::toggled, &layerInfoTree, &LayerInfoTree::setEnabled);
@@ -105,7 +104,6 @@ namespace armarx
layerInfoTree.setEnabled(widget.layerInfoTreeGroupBox->isChecked());
layerInfoTree.registerVisualizerCallbacks(visualizer);
-
// We need a callback from the visualizer, when the layers have changed
// So we can update the tree accordingly
visualizer.layersChangedCallback = [this](std::vector<viz::CoinLayerID> const & layers)
@@ -113,6 +111,12 @@ namespace armarx
layersChanged(layers);
};
+ // Arviz Profile tree.
+ arvizeProfileLayerWidget = new ArvizProfileManagerWidget();
+ widget.verticalLayout->addWidget(arvizeProfileLayerWidget);
+ connect(arvizeProfileLayerWidget, &ArvizProfileManagerWidget::publishUpdate, this, &This::updateLayersFromProfile);
+ connect(arvizeProfileLayerWidget, &ArvizProfileManagerWidget::fetchUpdate, this, &This::updateLayers);
+
replayTimer->start(33);
}
@@ -167,6 +171,38 @@ namespace armarx
emit disconnectGui();
}
+ void ArVizWidgetController::updateLayers()
+ {
+ // Update shown/hidden layers
+ layerTreeChanged(nullptr, 0);
+ }
+
+ void ArVizWidgetController::updateLayersFromProfile()
+ {
+ {
+ QSignalBlocker blocker(widget.layerTree);
+
+ // Iterate over all items and activate/deactivate layers accordingly
+ for (int compIndex = 0; compIndex < widget.layerTree->topLevelItemCount(); ++compIndex)
+ {
+ QTreeWidgetItem* componentItem = widget.layerTree->topLevelItem(compIndex);
+ bool parentChecked = true;
+ const std::string component = componentItem->text(0).toStdString();
+ for (int layerIndex = 0; layerIndex < componentItem->childCount(); ++layerIndex)
+ {
+ QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
+ const std::string layer = layerItem->text(0).toStdString();
+ const viz::CoinLayerID layerID(component, layer);
+ const bool visible = not arvizeProfileLayerWidget->isHidden(layerID);
+ layerItem->setCheckState(0, visible ? Qt::CheckState::Checked : Qt::CheckState::Unchecked);
+ parentChecked &= visible;
+ visualizer.showLayer(layerID, visible);
+ }
+ componentItem->setCheckState(0, parentChecked ? Qt::CheckState::Checked : Qt::CheckState::Unchecked);
+ }
+ }
+ }
+
void ArVizWidgetController::onConnectGui()
{
onRefreshRecordings();
@@ -194,6 +230,8 @@ namespace armarx
Qt::CheckState componentCheck = componentItem->checkState(0);
int layerCount = componentItem->childCount();
+ arvizeProfileLayerWidget->update(component, componentCheck == Qt::Checked);
+
if (componentItem == item)
{
// The parent was selected or deselected, so all children should be set accordingly
@@ -208,7 +246,6 @@ namespace armarx
}
return;
}
-
for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
{
QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
@@ -217,6 +254,9 @@ namespace armarx
ARMARX_VERBOSE << "Layer: " << layer << ", Visible: " << layerVisible;
viz::CoinLayerID layerID(component, layer);
+
+ arvizeProfileLayerWidget->update(layerID, layerVisible);
+
visualizer.showLayer(layerID, layerVisible);
}
}
@@ -261,10 +301,10 @@ namespace armarx
if (iter == currentComponents.end())
{
// Create a new item
+ QSignalBlocker blocker(tree); // otherwise added as unchecked and directly updated
currentItem = new QTreeWidgetItem(tree);
currentItem->setText(0, QString::fromStdString(component));
- // A new item is visible by default
- currentItem->setCheckState(0, Qt::Checked);
+ currentItem->setCheckState(0, arvizeProfileLayerWidget->isHidden(component) ? Qt::Unchecked : Qt::Checked);
componentWasNew = true;
}
@@ -286,8 +326,7 @@ namespace armarx
std::string const& layer = entry.second;
QTreeWidgetItem* layerItem = new QTreeWidgetItem;
layerItem->setText(0, QString::fromStdString(layer));
- // A new item is visible by default
- layerItem->setCheckState(0, Qt::Checked);
+ layerItem->setCheckState(0, arvizeProfileLayerWidget->isHidden(entry) ? Qt::Unchecked : Qt::Checked);
if (currentItem)
{
@@ -308,7 +347,11 @@ namespace armarx
}
else
{
- // Item exists already ==> nothing to be done
+ // Item exists already ==> make sure that it is not shown if necessary
+ if (arvizeProfileLayerWidget->isHidden(iter->first) or iter->second->checkState(0) == Qt::Unchecked)
+ {
+ visualizer.showLayer(iter->first, false);
+ }
}
}
}
@@ -398,8 +441,7 @@ namespace armarx
widget.layerTree->blockSignals(false);
- // Update shown/hidden layers
- layerTreeChanged(nullptr, 0);
+ updateLayers();
}
void ArVizWidgetController::showFilteredLayers(bool visible)
@@ -422,8 +464,7 @@ namespace armarx
widget.layerTree->blockSignals(false);
- // Update shown/hidden layers
- layerTreeChanged(nullptr, 0);
+ updateLayers();
}
void ArVizWidgetController::onDeselectElement()
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index cb10e285bee2e0b69497a1032703ca2aa5b5462e..065d04253da98d4c84c9d6f96313b094cfd95237 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -21,6 +21,8 @@
*/
#pragma once
+#include <unordered_set>
+
#include <RobotAPI/gui-plugins/ArViz/ui_ArVizWidget.h>
#include <RobotAPI/components/ArViz/Coin/Visualizer.h>
@@ -34,6 +36,7 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include "LayerInfoTree.h"
+#include "ArvizProfileManagerWidget.h"
namespace armarx
@@ -106,8 +109,10 @@ namespace armarx
void onGetBatchAsync(viz::data::RecordingBatch const& batch);
- public slots:
+ private slots:
/* QT slot declarations */
+ void updateLayers();
+ void updateLayersFromProfile();
signals:
/* QT signal declarations */
@@ -224,6 +229,8 @@ namespace armarx
std::vector<double> timings;
+ ArvizProfileManagerWidget* arvizeProfileLayerWidget;
+
public:
static QIcon GetWidgetIcon()
{
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.cpp b/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4af5a0fd4db95eb36733d5bcb501d1453bdfc5b6
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.cpp
@@ -0,0 +1,362 @@
+#include "ArvizProfileManagerWidget.h"
+
+#include <string>
+
+#include <QVBoxLayout>
+#include <QMessageBox>
+#include <QBoxLayout>
+#include <QLabel>
+#include <QFormLayout>
+#include <QDialogButtonBox>
+#include <QPushButton>
+#include <QBoxLayout>
+#include <QRadioButton>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+namespace armarx
+{
+
+const QString ArvizProfileManagerWidget::SETTINGS_DEFAULT_PROFILE_KEY = "defaultProfileName";
+
+
+ProfileDialog::ProfileDialog(QWidget *parent, const QString &name, bool additive, bool addDialog, bool isDefault)
+ : QDialog(parent)
+{
+ setWindowTitle(addDialog ? "Add ArViz Profile" : "Edit ArViz Profile");
+
+ // Set up UI components
+ nameInput = new QLineEdit(this);
+ nameInput->setText(name);
+
+ additiveRadioButton = new QRadioButton("Save added layers", this);
+ additiveRadioButton->setChecked(additive);
+ substractiveRadioButton = new QRadioButton("Save removed layers", this);
+ substractiveRadioButton->setChecked(not additive);
+
+ defaultCheckBox = new QCheckBox("Mark default", this);
+ defaultCheckBox->setChecked(isDefault);
+
+ // Save/Cancel buttons
+ QDialogButtonBox *buttonBox = new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel, this);
+ connect(buttonBox, &QDialogButtonBox::accepted, this, &ProfileDialog::accept);
+ connect(buttonBox, &QDialogButtonBox::rejected, this, &ProfileDialog::reject);
+ buttonBox->setCenterButtons(true);
+
+ // Delete button
+ deleteButton = new QPushButton("Delete", this);
+ connect(deleteButton, &QPushButton::clicked, this, &ProfileDialog::deleteProfile);
+
+ // Layouts
+ QFormLayout *formLayout = new QFormLayout;
+ formLayout->addRow("Profile Name:", nameInput);
+ formLayout->addRow(additiveRadioButton);
+ formLayout->addRow(substractiveRadioButton);
+ formLayout->addRow(defaultCheckBox);
+ formLayout->addRow(buttonBox);
+ formLayout->addRow(deleteButton);
+
+ setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
+ setLayout(formLayout);
+ adjustSize();
+}
+
+ProfileDialog::~ProfileDialog()
+{
+}
+
+QString ProfileDialog::getName() const
+{
+ return nameInput->text();
+}
+
+bool ProfileDialog::isAdditive() const
+{
+ return additiveRadioButton->isChecked();
+}
+
+bool ProfileDialog::isDefaultProfile() const
+{
+ return defaultCheckBox->isChecked();
+}
+
+void ProfileDialog::deleteProfile()
+{
+ // Handle the profile deletion
+ emit deleteProfileSignal();
+ reject(); // Close the dialog after deletion
+}
+
+
+
+ArvizProfileManagerWidget::ArvizProfileManagerWidget(QWidget* parent) :
+ QWidget(parent),
+ settings(QString((armarx::ArmarXDataPath::GetDefaultUserConfigPath() + "/ArvizProfileManager.conf").c_str()),
+ QSettings::NativeFormat),
+ currentProfile(Profile())
+{
+ QHBoxLayout* layout = new QHBoxLayout(this);
+
+ // Setup UI elements
+ layersComboBox = new QComboBox(this);
+ connect(layersComboBox, QOverload<int>::of(&QComboBox::activated),
+ this, &ArvizProfileManagerWidget::onProfileSelected);
+
+ addButton = new QPushButton("Add", this);
+ editButton = new QPushButton("Edit", this);
+ saveButton = new QPushButton("Save", this);
+
+ layout->addWidget(new QLabel("ArViz Profile: "));
+ layout->addWidget(layersComboBox);
+ layout->addWidget(addButton);
+ layout->addWidget(editButton);
+ layout->addWidget(saveButton);
+
+ setLayout(layout);
+
+ connect(addButton, &QPushButton::clicked, this, &ArvizProfileManagerWidget::addProfile);
+ connect(editButton, &QPushButton::clicked, this, &ArvizProfileManagerWidget::editProfile);
+ connect(saveButton, &QPushButton::clicked, this, &ArvizProfileManagerWidget::saveCurrentProfile);
+
+ loadLayerNames();
+
+ // Load default profile
+ const QString defaultProfileName
+ = settings.value(SETTINGS_DEFAULT_PROFILE_KEY).toString();
+ if (int i = layersComboBox->findText(defaultProfileName); i >= 0)
+ {
+ layersComboBox->setCurrentText(defaultProfileName);
+ onProfileSelected(i);
+ }
+ else
+ {
+ // Not found. Load empty profile
+ layersComboBox->setCurrentIndex(-1);
+ }
+}
+
+void ArvizProfileManagerWidget::onProfileSelected(int index)
+{
+ if (index >= 0 and index < layersComboBox->count())
+ {
+ loadProfile(layersComboBox->itemText(index));
+ emit publishUpdate();
+ }
+}
+
+void ArvizProfileManagerWidget::saveCurrentProfile()
+{
+ const QString name = layersComboBox->currentText();
+ if (name.isEmpty())
+ {
+ QMessageBox::warning(this, "Warning", "Please enter a valid name.");
+ return;
+ }
+
+ if (saveProfile(name, currentProfile))
+ {
+ if (layersComboBox->findText(name) == -1)
+ {
+ layersComboBox->addItem(name);
+ }
+ layersComboBox->setCurrentText(name);
+ }
+}
+
+void ArvizProfileManagerWidget::deleteCurrentProfileSave()
+{
+ const QString name = layersComboBox->currentText();
+ settings.remove(name);
+
+ const int index = layersComboBox->currentIndex();
+ layersComboBox->removeItem(index);
+ settings.remove(name);
+
+ if (layersComboBox->count() == 0)
+ {
+ editButton->setDisabled(true);
+ saveButton->setDisabled(true);
+ }
+
+ emit publishUpdate();
+}
+
+void ArvizProfileManagerWidget::addProfile()
+{
+ if (dialog == nullptr || !dialog->isVisible())
+ {
+ const QString defaultName = "Profile";
+ const bool defaultAdditive = false;
+ QString name = defaultName;
+ unsigned int counter = 2;
+ while (layersComboBox->count() > 0 and layersComboBox->findText(name) >= 0)
+ {
+ name = defaultName + QString::fromStdString(std::to_string(counter++));
+ }
+ dialog = new ProfileDialog(this, name, defaultAdditive, true,
+ settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name);
+ const int result = dialog->exec();
+
+ if (result == QDialog::Accepted)
+ {
+ if (dialog->getName().isEmpty())
+ {
+ ARMARX_WARNING << "Empty name for profile set.";
+ return;
+ }
+ if (layersComboBox->findText(dialog->getName()) >= 0)
+ {
+ ARMARX_WARNING << "Profile with name " << dialog->getName().toStdString() << " already exists!";
+ return;
+ }
+
+ if (dialog->isDefaultProfile())
+ {
+ settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, dialog->getName());
+ }
+
+ currentProfile.layers = {};
+ currentProfile.additive = dialog->isAdditive();
+ emit fetchUpdate();
+
+ saveProfile(dialog->getName(), currentProfile);
+ loadLayerNames();
+ layersComboBox->setCurrentText(dialog->getName());
+ }
+ else
+ {
+ // do nothing
+ }
+ }
+ else
+ {
+ QMessageBox::information(this, "Dialog Already Open", "The dialog is already open.");
+ }
+}
+
+void ArvizProfileManagerWidget::editProfile()
+{
+ if (layersComboBox->count() > 0)
+ {
+ if (dialog == nullptr || !dialog->isVisible())
+ {
+ const auto name = layersComboBox->currentText();
+ dialog = new ProfileDialog(this, name, currentProfile.additive,
+ false, settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name);
+ connect(dialog, &ProfileDialog::deleteProfileSignal, this, &ArvizProfileManagerWidget::deleteCurrentProfileSave);
+ const int result = dialog->exec();
+
+ if (result == QDialog::Accepted)
+ {
+ const auto newName = dialog->getName();
+
+ if (newName.isEmpty())
+ {
+ ARMARX_WARNING << "Empty name for profile set.";
+ return;
+ }
+
+ if (newName != name)
+ {
+ // Rename
+ if (layersComboBox->findText(newName) >= 0)
+ {
+ ARMARX_WARNING << "Profile with name " << newName.toStdString() << " already exists!";
+ return;
+ }
+ settings.remove(layersComboBox->currentText());
+ }
+
+ if (dialog->isAdditive() != currentProfile.additive)
+ {
+ currentProfile.layers.clear();
+ currentProfile.additive = dialog->isAdditive();
+ emit fetchUpdate();
+ }
+
+ if (dialog->isDefaultProfile())
+ {
+ settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, newName);
+ }
+ else if (settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name)
+ {
+ settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, "");
+ }
+
+ saveProfile(dialog->getName(), currentProfile);
+ loadLayerNames();
+ layersComboBox->setCurrentText(newName);
+ }
+ else
+ {
+ // do nothing
+ }
+ }
+ else
+ {
+ QMessageBox::information(this, "Dialog Already Open", "The dialog is already open.");
+ }
+ }
+}
+
+void ArvizProfileManagerWidget::loadLayerNames()
+{
+ layersComboBox->clear();
+ for (const QString& groups : settings.childGroups())
+ {
+ layersComboBox->addItem(groups);
+ }
+ if (layersComboBox->count() == 0)
+ {
+ editButton->setDisabled(true);
+ saveButton->setDisabled(true);
+ }
+ else
+ {
+ editButton->setEnabled(true);
+ saveButton->setEnabled(true);
+ }
+}
+
+void ArvizProfileManagerWidget::loadProfile(const QString& name)
+{
+ ARMARX_INFO << "Loading ArViz profile " << name.toStdString();
+
+ std::scoped_lock lock(mtx);
+ currentProfile.layers.clear();
+
+ settings.beginGroup(name);
+ currentProfile.additive = settings.value("additive").toBool();
+ const int size = settings.beginReadArray("layers");
+ for (int i = 0; i < size; ++i)
+ {
+ settings.setArrayIndex(i);
+ QString first = settings.value("component").toString();
+ QString second = settings.value("layer").toString();
+ currentProfile.layers.insert({first.toStdString(), second.toStdString()});
+ }
+ settings.endArray();
+ settings.endGroup();
+ editButton->setEnabled(true);
+}
+
+bool ArvizProfileManagerWidget::saveProfile(const QString& name, const Profile& profile)
+{
+ settings.beginGroup(name);
+ settings.setValue("additive", profile.additive);
+ settings.remove("layers");
+ settings.beginWriteArray("layers", profile.layers.size());
+ int i = 0;
+ for (const auto& pair : profile.layers)
+ {
+ settings.setArrayIndex(i++);
+ settings.setValue("component", QString::fromStdString(pair.first));
+ settings.setValue("layer", QString::fromStdString(pair.second));
+ }
+ settings.endArray();
+ settings.endGroup();
+ return true;
+}
+
+}
\ No newline at end of file
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.h b/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..1f5a97151f4043c723356e6db76cb491e0479a87
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.h
@@ -0,0 +1,175 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License 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::ArVizProfileManagerWidget
+ * @author Andre Meixner ( andre dot meixner at kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <QWidget>
+#include <QDialog>
+#include <QComboBox>
+#include <QPushButton>
+#include <QLineEdit>
+#include <QSettings>
+#include <QString>
+#include <QCheckBox>
+#include <QRadioButton>
+
+#include <mutex>
+#include <qcheckbox.h>
+#include <unordered_set>
+#include <string>
+#include <utility>
+
+#include <RobotAPI/components/ArViz/Coin/Visualizer.h>
+
+namespace armarx
+{
+ struct pair_hash
+ {
+ template <typename T1, typename T2>
+ std::size_t operator ()(const std::pair<T1, T2>& p) const
+ {
+ auto h1 = std::hash<T1>{}(p.first);
+ auto h2 = std::hash<T2>{}(p.second);
+ return h1 ^ (h2 << 1);
+ }
+ };
+
+ using Layers = std::unordered_set<viz::CoinLayerID, pair_hash>;
+
+ struct Profile
+ {
+ Layers layers;
+ bool additive = false;
+ };
+
+
+ class ProfileDialog : public QDialog
+ {
+ Q_OBJECT
+
+ public:
+ explicit ProfileDialog(QWidget *parent = nullptr, const QString &name = "",
+ bool additive = false, bool addDialog = true, bool isDefault = false);
+ ~ProfileDialog();
+
+ QString getName() const;
+ bool isAdditive() const;
+ bool isDefaultProfile() const;
+
+ signals:
+ void deleteProfileSignal();
+
+ private slots:
+ void deleteProfile();
+
+ private:
+ QLineEdit *nameInput;
+ QRadioButton *additiveRadioButton;
+ QRadioButton *substractiveRadioButton;
+ QPushButton *deleteButton;
+ QCheckBox *defaultCheckBox;
+ };
+
+
+ class ArvizProfileManagerWidget : public QWidget
+ {
+ Q_OBJECT
+
+ public:
+ explicit ArvizProfileManagerWidget(QWidget* parent = nullptr);
+
+ inline void update(const viz::CoinLayerID& layerID, bool visible)
+ {
+ std::scoped_lock lock(mtx);
+
+ if (currentProfile.additive == visible)
+ {
+ currentProfile.layers.insert(layerID);
+ }
+ else if (currentProfile.layers.count(layerID) > 0)
+ {
+ currentProfile.layers.erase(layerID);
+ currentProfile.layers.erase({layerID.first, "*"});
+ }
+
+ }
+
+ inline void update(const std::string& componentName, bool visible)
+ {
+ update({componentName, "*"}, visible);
+ }
+
+ inline bool isHidden(const viz::CoinLayerID& layerID) const
+ {
+ std::scoped_lock lock(mtx);
+
+ const bool result = currentProfile.layers.count(layerID) > 0 or currentProfile.layers.count({layerID.first, "*"}) > 0;
+
+ if (currentProfile.additive)
+ {
+ return not result;
+ }
+ else
+ {
+ return result;
+ }
+ }
+
+ inline bool isHidden(const std::string& componentName) const
+ {
+ std::scoped_lock lock(mtx);
+
+ return currentProfile.additive == (currentProfile.layers.count({componentName, "*"}) == 0);
+ }
+
+ signals:
+ void publishUpdate();
+ void fetchUpdate();
+
+ public slots:
+ void onProfileSelected(int index);
+
+ private slots:
+ void saveCurrentProfile();
+ void deleteCurrentProfileSave();
+
+ void addProfile();
+ void editProfile();
+
+ private:
+ void loadLayerNames();
+ void loadProfile(const QString& name);
+ bool saveProfile(const QString& name, const Profile& profile);
+
+ ProfileDialog* dialog = nullptr;
+ QComboBox* layersComboBox;
+ QPushButton* addButton;
+ QPushButton* editButton;
+ QPushButton* saveButton;
+ QSettings settings;
+ mutable std::mutex mtx;
+
+ Profile currentProfile;
+
+ static const QString SETTINGS_DEFAULT_PROFILE_KEY;
+ };
+
+}
diff --git a/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt b/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
index c9f30ffe5c0d845c057bf9346ca2425e3e146832..94928007fbb4c707002d5f9347b632a32d4e5b12 100644
--- a/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
@@ -10,6 +10,7 @@ armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
set(SOURCES
ArVizGuiPlugin.cpp
ArVizWidgetController.cpp
+ ArvizProfileManagerWidget.cpp
LayerInfoTree.cpp
)
@@ -18,6 +19,7 @@ set(SOURCES
set(HEADERS
ArVizGuiPlugin.h
ArVizWidgetController.h
+ ArvizProfileManagerWidget.h
LayerInfoTree.h
)
diff --git a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
index 288e480893d5f9865a83781fcde00ed799d7ccf7..6cc8d730eec3f7c083e46ff1f5c6b77300cce16f 100644
--- a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
@@ -22,6 +22,7 @@
#include <string>
#include <QRegExp>
+#include <VirtualRobot/RobotNodeSet.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
index 72ff523b09b5e5282d1caff1b5ae92f559cbceee..fec96cb89cc635119afe1d1e260a805c7a850936 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
@@ -22,24 +22,27 @@
* GNU General Public License
*/
#include "HandUnitGuiPlugin.h"
-#include "HandUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
+
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
+#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
+
+#include "HandUnitConfigDialog.h"
+
// Qt headers
-#include <Qt>
-#include <QtGlobal>
-#include <QPushButton>
+#include <cmath>
+
#include <QLabel>
#include <QLineEdit>
#include <QMessageBox>
+#include <QPushButton>
+#include <QString>
#include <QTimer>
+#include <Qt>
+#include <QtGlobal>
#include <QtWidgets/QSlider>
#include <QtWidgets/QTableWidgetItem>
-#include <QString>
-
-#include <cmath>
namespace armarx
{
@@ -63,13 +66,15 @@ namespace armarx
ARMARX_INFO << "Done: Setup UI";
- setLeftHandJointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setLeftHandJointAngles, 50);
- setRightHandJointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setRightHandJointAngles, 50);
+ setLeftHandJointAngleUpdateTask =
+ new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setLeftHandJointAngles, 50);
+ setRightHandJointAngleUpdateTask =
+ new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setRightHandJointAngles, 50);
updateInfoTimer = new QTimer(this);
}
-
- void HandUnitWidget::onInitComponent()
+ void
+ HandUnitWidget::onInitComponent()
{
usingProxy(leftHandUnitProxyName);
usingProxy(rightHandUnitProxyName);
@@ -77,12 +82,12 @@ namespace armarx
//ARMARX_WARNING << "Listening on Topic: " << handName + "State";
}
- void HandUnitWidget::onConnectComponent()
+ void
+ HandUnitWidget::onConnectComponent()
{
updateInfoTimer->start(50);
-
setLeftHandJointAngleUpdateTask->start();
leftHandUnitProxy = getProxy<HandUnitInterfacePrx>(leftHandUnitProxyName);
@@ -97,14 +102,15 @@ namespace armarx
ARMARX_WARNING << "Left hand with name \"" << leftHandName << "\" is not supported.";
}
- SingleTypeVariantListPtr leftHandPreshapeStrings = SingleTypeVariantListPtr::dynamicCast(leftHandUnitProxy->getShapeNames());
+ SingleTypeVariantListPtr leftHandPreshapeStrings =
+ SingleTypeVariantListPtr::dynamicCast(leftHandUnitProxy->getShapeNames());
QStringList leftHandList;
int leftHandPreshapeCount = leftHandPreshapeStrings->getSize();
for (int i = 0; i < leftHandPreshapeCount; ++i)
{
std::string shape = ((leftHandPreshapeStrings->getVariant(i))->get<std::string>());
-// ARMARX_INFO << VAROUT(shape);
+ // ARMARX_INFO << VAROUT(shape);
leftHandList << QString::fromStdString(shape);
}
@@ -112,7 +118,6 @@ namespace armarx
ui.comboLeftHandPreshapes->addItems(leftHandList);
-
setRightHandJointAngleUpdateTask->start();
rightHandUnitProxy = getProxy<HandUnitInterfacePrx>(rightHandUnitProxyName);
@@ -127,14 +132,15 @@ namespace armarx
ARMARX_WARNING << "Right hand with name \"" << rightHandName << "\" is not supported.";
}
- SingleTypeVariantListPtr rightHandPreshapeStrings = SingleTypeVariantListPtr::dynamicCast(rightHandUnitProxy->getShapeNames());
+ SingleTypeVariantListPtr rightHandPreshapeStrings =
+ SingleTypeVariantListPtr::dynamicCast(rightHandUnitProxy->getShapeNames());
QStringList rightHandList;
int rightHandPreshapeCount = rightHandPreshapeStrings->getSize();
for (int i = 0; i < rightHandPreshapeCount; ++i)
{
std::string shape = ((rightHandPreshapeStrings->getVariant(i))->get<std::string>());
-// ARMARX_INFO << VAROUT(shape);
+ // ARMARX_INFO << VAROUT(shape);
rightHandList << QString::fromStdString(shape);
}
@@ -142,14 +148,17 @@ namespace armarx
ui.comboRightHandPreshapes->addItems(rightHandList);
-
-// ARMARX_INFO << "initGUIJointFrames";
+ // ARMARX_INFO << "initGUIJointFrames";
initGUIJointFrames();
- displayJointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::updateJointValueTable, 50);
+ displayJointAngleUpdateTask =
+ new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::updateJointValueTable, 50);
displayJointAngleUpdateTask->start();
+
+ updateInfoLabel();
}
- void HandUnitWidget::onDisconnectComponent()
+ void
+ HandUnitWidget::onDisconnectComponent()
{
setLeftHandJointAngleUpdateTask->stop();
setRightHandJointAngleUpdateTask->stop();
@@ -157,11 +166,13 @@ namespace armarx
updateInfoTimer->stop();
}
- void HandUnitWidget::onExitComponent()
+ void
+ HandUnitWidget::onExitComponent()
{
}
- QPointer<QDialog> HandUnitWidget::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ HandUnitWidget::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
@@ -173,29 +184,33 @@ namespace armarx
return qobject_cast<HandUnitConfigDialog*>(dialog);
}
- void HandUnitWidget::configured()
+ void
+ HandUnitWidget::configured()
{
leftHandUnitProxyName = dialog->proxyFinderLeftHand->getSelectedProxyName().toStdString();
rightHandUnitProxyName = dialog->proxyFinderRightHand->getSelectedProxyName().toStdString();
}
- void HandUnitWidget::preshapeLeftHand()
+ void
+ HandUnitWidget::preshapeLeftHand()
{
setLeftHandPreshape(ui.comboLeftHandPreshapes->currentText().toUtf8().data());
}
- void HandUnitWidget::preshapeRightHand()
+ void
+ HandUnitWidget::preshapeRightHand()
{
setRightHandPreshape(ui.comboRightHandPreshapes->currentText().toUtf8().data());
}
- void HandUnitWidget::reloadPreshapes()
+ void
+ HandUnitWidget::reloadPreshapes()
{
ARMARX_INFO << "Reloading preshapes in robot unit ...";
// left hand unit
{
- if(leftHandUnitProxy)
+ if (leftHandUnitProxy)
{
ARMARX_INFO << "Left hand unit: reloading preshapes.";
leftHandUnitProxy->reloadPreshapes();
@@ -204,7 +219,7 @@ namespace armarx
// right hand unit
{
- if(rightHandUnitProxy)
+ if (rightHandUnitProxy)
{
ARMARX_INFO << "Right hand unit: reloading preshapes.";
rightHandUnitProxy->reloadPreshapes();
@@ -212,9 +227,10 @@ namespace armarx
}
}
- void HandUnitWidget::setLeftHandJointAngles()
- {
-// ARMARX_INFO << "setLeftHandJointAngles";
+ void
+ HandUnitWidget::setLeftHandJointAngles()
+ {
+ // ARMARX_INFO << "setLeftHandJointAngles";
if (!leftHandUnitProxy)
{
ARMARX_WARNING << "invalid proxy";
@@ -245,21 +261,23 @@ namespace armarx
{
if (pair.first == currentLeftHandJoint)
{
- value = static_cast<float>(ui.horizontalSliderLeftHandJointPos->value() * leftHandConversionFactor);
-// ARMARX_INFO << VAROUT(value);
+ value = static_cast<float>(ui.horizontalSliderLeftHandJointPos->value() *
+ leftHandConversionFactor);
+ // ARMARX_INFO << VAROUT(value);
leftHandJa[pair.first] = value;
break;
}
}
-// ARMARX_INFO << VAROUT(value / leftHandConversionFactor);
+ // ARMARX_INFO << VAROUT(value / leftHandConversionFactor);
ui.lcdNumberLeftHandJointValue->display(value / leftHandConversionFactor);
leftHandUnitProxy->setJointAngles(leftHandJa);
}
- void HandUnitWidget::setRightHandJointAngles()
+ void
+ HandUnitWidget::setRightHandJointAngles()
{
-// ARMARX_INFO << "setRightHandJointAngles";
+ // ARMARX_INFO << "setRightHandJointAngles";
if (!rightHandUnitProxy)
{
ARMARX_WARNING << "invalid proxy";
@@ -290,73 +308,88 @@ namespace armarx
{
if (pair.first == currentRightHandJoint)
{
- value = static_cast<float>(ui.horizontalSliderRightHandJointPos->value() * rightHandConversionFactor);
-// ARMARX_INFO << VAROUT(value);
+ value = static_cast<float>(ui.horizontalSliderRightHandJointPos->value() *
+ rightHandConversionFactor);
+ // ARMARX_INFO << VAROUT(value);
rightHandJa[pair.first] = value;
break;
}
}
-// ARMARX_INFO << VAROUT(value / rightHandConversionFactor);
+ // ARMARX_INFO << VAROUT(value / rightHandConversionFactor);
ui.lcdNumberRightHandJointValue->display(value / rightHandConversionFactor);
rightHandUnitProxy->setJointAngles(rightHandJa);
}
- void HandUnitWidget::requestSetLeftHandJointAngles()
+ void
+ HandUnitWidget::requestSetLeftHandJointAngles()
{
setLeftHandJointAnglesFlag = true;
}
- void HandUnitWidget::requestSetRightHandJointAngles()
+ void
+ HandUnitWidget::requestSetRightHandJointAngles()
{
setRightHandJointAnglesFlag = true;
}
- void HandUnitWidget::openLeftHand()
+ void
+ HandUnitWidget::openLeftHand()
{
setLeftHandPreshape("Open");
}
- void HandUnitWidget::openRightHand()
+ void
+ HandUnitWidget::openRightHand()
{
setRightHandPreshape("Open");
}
- void HandUnitWidget::closeLeftHand()
+ void
+ HandUnitWidget::closeLeftHand()
{
setLeftHandPreshape("Close");
}
- void HandUnitWidget::closeRightHand()
+ void
+ HandUnitWidget::closeRightHand()
{
setRightHandPreshape("Close");
}
- void HandUnitWidget::relaxLeftHand()
+ void
+ HandUnitWidget::relaxLeftHand()
{
setLeftHandPreshape("Relax");
}
- void HandUnitWidget::relaxRightHand()
+ void
+ HandUnitWidget::relaxRightHand()
{
setRightHandPreshape("Relax");
}
- void HandUnitWidget::updateInfoLabel()
+ void
+ HandUnitWidget::updateInfoLabel()
{
- ui.labelInfoLeftHand->setText(QString::fromStdString(leftHandUnitProxyName + " :: " + leftHandName + " State: " + leftHandUnitProxy->describeHandState()));
- ui.labelInfoRightHand->setText(QString::fromStdString(rightHandUnitProxyName + " :: " + rightHandName + " State: " + rightHandUnitProxy->describeHandState()));
+ ui.labelInfoLeftHand->setText(
+ QString::fromStdString(leftHandUnitProxyName + " :: " + leftHandName +
+ " State: " + leftHandUnitProxy->describeHandState()));
+ ui.labelInfoRightHand->setText(
+ QString::fromStdString(rightHandUnitProxyName + " :: " + rightHandName +
+ " State: " + rightHandUnitProxy->describeHandState()));
}
- void HandUnitWidget::updateJointValueTable()
+ void
+ HandUnitWidget::updateJointValueTable()
{
NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
-// ARMARX_INFO << VAROUT(leftHandUnitProxy->getCurrentJointValues());
+ // ARMARX_INFO << VAROUT(leftHandUnitProxy->getCurrentJointValues());
int frameLeftHandRowIdx = 0;
for (const auto& pair : currentLeftHandJointValues)
{
-// ARMARX_INFO << VAROUT(pair.first);
+ // ARMARX_INFO << VAROUT(pair.first);
QString name(pair.first.c_str());
QTableWidgetItem* newItem = new QTableWidgetItem(QString::number(pair.second));
ui.tableWidgetLeftHand->setItem(frameLeftHandRowIdx, 1, newItem);
@@ -364,14 +397,13 @@ namespace armarx
}
-
NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
-// ARMARX_INFO << VAROUT(rightHandUnitProxy->getCurrentJointValues());
+ // ARMARX_INFO << VAROUT(rightHandUnitProxy->getCurrentJointValues());
int frameRightHandRowIdx = 0;
for (const auto& pair : currentRightHandJointValues)
{
-// ARMARX_INFO << VAROUT(pair.first);
+ // ARMARX_INFO << VAROUT(pair.first);
QString name(pair.first.c_str());
QTableWidgetItem* newItem = new QTableWidgetItem(QString::number(pair.second));
ui.tableWidgetRightHand->setItem(frameRightHandRowIdx, 1, newItem);
@@ -379,13 +411,73 @@ namespace armarx
}
}
- void HandUnitWidget::selectLeftHandJoint(int i)
+ void
+ HandUnitWidget::setExpertMode(side side, int state)
+ {
+ bool const isExpert = state == Qt::Checked;
+
+ if (side == side::left)
+ {
+ ui.leftTare->setEnabled(isExpert);
+ ui.leftReset->setEnabled(isExpert);
+ }
+ else if (side == side::right)
+ {
+ ui.rightTare->setEnabled(isExpert);
+ ui.rightReset->setEnabled(isExpert);
+ }
+ else
+ {
+ ARMARX_WARNING << "Invalid side to set expert mode.";
+ }
+ }
+
+ void
+ HandUnitWidget::tareHand(side side)
{
-// ARMARX_INFO << "selectLeftHandJoint " << i;
+ if (side == side::left)
+ {
+ ARMARX_INFO << "Trigger taring left hand now...";
+ leftHandUnitProxy->tare();
+ }
+ else if (side == side::right)
+ {
+ ARMARX_INFO << "Trigger taring right hand now...";
+ rightHandUnitProxy->tare();
+ }
+ else
+ {
+ ARMARX_WARNING << "Invalid side to tare hand.";
+ }
+ }
+
+ void
+ HandUnitWidget::resetHand(side side)
+ {
+ if (side == side::left)
+ {
+ ARMARX_INFO << "Trigger resetting left hand firmware now...";
+ leftHandUnitProxy->resetFirmware();
+ }
+ else if (side == side::right)
+ {
+ ARMARX_INFO << "Trigger resetting right hand firmware now...";
+ rightHandUnitProxy->resetFirmware();
+ }
+ else
+ {
+ ARMARX_WARNING << "Invalid side to reset hand firmware.";
+ }
+ }
+
+ void
+ HandUnitWidget::selectLeftHandJoint(int i)
+ {
+ // ARMARX_INFO << "selectLeftHandJoint " << i;
NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
int idx = 0;
- for (const auto& pair: currentLeftHandJointValues)
+ for (const auto& pair : currentLeftHandJointValues)
{
if (idx == i)
{
@@ -393,20 +485,21 @@ namespace armarx
int convertedValue = static_cast<int>(pair.second / leftHandConversionFactor);
ui.horizontalSliderLeftHandJointPos->setSliderPosition(convertedValue);
ui.lcdNumberLeftHandJointValue->display(convertedValue);
-// ARMARX_INFO << "Found joint";
+ // ARMARX_INFO << "Found joint";
break;
}
idx++;
}
}
- void HandUnitWidget::selectRightHandJoint(int i)
+ void
+ HandUnitWidget::selectRightHandJoint(int i)
{
-// ARMARX_INFO << "selectRightHandJoint " << i;
+ // ARMARX_INFO << "selectRightHandJoint " << i;
NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
int idx = 0;
- for (const auto& pair: currentRightHandJointValues)
+ for (const auto& pair : currentRightHandJointValues)
{
if (idx == i)
{
@@ -414,68 +507,113 @@ namespace armarx
int convertedValue = static_cast<int>(pair.second / rightHandConversionFactor);
ui.horizontalSliderRightHandJointPos->setSliderPosition(convertedValue);
ui.lcdNumberRightHandJointValue->display(convertedValue);
-// ARMARX_INFO << "Found joint";
+ // ARMARX_INFO << "Found joint";
break;
}
idx++;
}
}
- void HandUnitWidget::setLeftHandPreshape(std::string preshape)
+ void
+ HandUnitWidget::setLeftHandPreshape(std::string preshape)
{
-// ARMARX_INFO << "Setting new left hand shape: " << preshape;
+ // ARMARX_INFO << "Setting new left hand shape: " << preshape;
leftHandUnitProxy->setShape(preshape);
}
- void HandUnitWidget::setRightHandPreshape(std::string preshape)
+ void
+ HandUnitWidget::setRightHandPreshape(std::string preshape)
{
-// ARMARX_INFO << "Setting new right hand shape: " << preshape;
+ // ARMARX_INFO << "Setting new right hand shape: " << preshape;
rightHandUnitProxy->setShape(preshape);
}
- void HandUnitWidget::loadSettings(QSettings* settings)
+ void
+ HandUnitWidget::loadSettings(QSettings* settings)
{
- leftHandUnitProxyName = settings->value("leftHandUnitProxyName", QString::fromStdString(leftHandUnitProxyName)).toString().toStdString();
- leftHandName = settings->value("leftHandName", QString::fromStdString(leftHandName)).toString().toStdString();
- rightHandUnitProxyName = settings->value("rightHandUnitProxyName", QString::fromStdString(rightHandUnitProxyName)).toString().toStdString();
- rightHandName = settings->value("rightHandName", QString::fromStdString(rightHandName)).toString().toStdString();
+ leftHandUnitProxyName =
+ settings->value("leftHandUnitProxyName", QString::fromStdString(leftHandUnitProxyName))
+ .toString()
+ .toStdString();
+ leftHandName = settings->value("leftHandName", QString::fromStdString(leftHandName))
+ .toString()
+ .toStdString();
+ rightHandUnitProxyName =
+ settings
+ ->value("rightHandUnitProxyName", QString::fromStdString(rightHandUnitProxyName))
+ .toString()
+ .toStdString();
+ rightHandName = settings->value("rightHandName", QString::fromStdString(rightHandName))
+ .toString()
+ .toStdString();
}
- void HandUnitWidget::saveSettings(QSettings* settings)
+ void
+ HandUnitWidget::saveSettings(QSettings* settings)
{
settings->setValue("leftHandUnitProxyName", QString::fromStdString(leftHandUnitProxyName));
settings->setValue("leftHandName", QString::fromStdString(leftHandName));
- settings->setValue("rightHandUnitProxyName", QString::fromStdString(rightHandUnitProxyName));
+ settings->setValue("rightHandUnitProxyName",
+ QString::fromStdString(rightHandUnitProxyName));
settings->setValue("rightHandName", QString::fromStdString(rightHandName));
}
-
- void HandUnitWidget::initGUIJointFrames()
+ void
+ HandUnitWidget::initGUIJointFrames()
{
ARMARX_INFO << "initGUIJointFrames";
-
+
NameValueMap currentLeftHandJointValues = leftHandUnitProxy->getCurrentJointValues();
-// ARMARX_INFO << VAROUT(leftHandUnitProxy->getCurrentJointValues());
+ // ARMARX_INFO << VAROUT(leftHandUnitProxy->getCurrentJointValues());
int frameLeftHandRowIdx = 0;
- connect(ui.buttonPreshapeLeftHand, SIGNAL(clicked()), this, SLOT(preshapeLeftHand()), Qt::UniqueConnection);
- connect(ui.buttonOpenLeftHand, SIGNAL(clicked()), this, SLOT(openLeftHand()), Qt::UniqueConnection);
- connect(ui.buttonCloseLeftHand, SIGNAL(clicked()), this, SLOT(closeLeftHand()), Qt::UniqueConnection);
- connect(ui.buttonRelaxLeftHand, SIGNAL(clicked()), this, SLOT(relaxLeftHand()), Qt::UniqueConnection);
+ connect(ui.buttonPreshapeLeftHand,
+ SIGNAL(clicked()),
+ this,
+ SLOT(preshapeLeftHand()),
+ Qt::UniqueConnection);
+ connect(ui.buttonOpenLeftHand,
+ SIGNAL(clicked()),
+ this,
+ SLOT(openLeftHand()),
+ Qt::UniqueConnection);
+ connect(ui.buttonCloseLeftHand,
+ SIGNAL(clicked()),
+ this,
+ SLOT(closeLeftHand()),
+ Qt::UniqueConnection);
+ connect(ui.buttonRelaxLeftHand,
+ SIGNAL(clicked()),
+ this,
+ SLOT(relaxLeftHand()),
+ Qt::UniqueConnection);
ui.horizontalSliderLeftHandJointPos->setMaximum(100);
ui.horizontalSliderLeftHandJointPos->setMinimum(0);
- connect(ui.horizontalSliderLeftHandJointPos, SIGNAL(sliderMoved(int)), this, SLOT(requestSetLeftHandJointAngles()), Qt::UniqueConnection);
- connect(ui.comboLeftHandJoints, SIGNAL(currentIndexChanged(int)), this, SLOT(selectLeftHandJoint(int)), Qt::UniqueConnection);
+ connect(ui.horizontalSliderLeftHandJointPos,
+ SIGNAL(sliderMoved(int)),
+ this,
+ SLOT(requestSetLeftHandJointAngles()),
+ Qt::UniqueConnection);
+ connect(ui.comboLeftHandJoints,
+ SIGNAL(currentIndexChanged(int)),
+ this,
+ SLOT(selectLeftHandJoint(int)),
+ Qt::UniqueConnection);
+ connect(ui.leftExpertMode,
+ &QCheckBox::stateChanged,
+ [&](int state) { setExpertMode(side::left, state); });
+ connect(ui.leftTare, &QPushButton::clicked, [&] { tareHand(side::left); });
+ connect(ui.leftReset, &QPushButton::clicked, [&] { resetHand(side::left); });
ui.tableWidgetLeftHand->setRowCount(currentLeftHandJointValues.size());
ui.tableWidgetLeftHand->setColumnCount(2);
ui.comboLeftHandJoints->clear();
- qRegisterMetaType<QVector<int> >("QVector<int>");
+ qRegisterMetaType<QVector<int>>("QVector<int>");
for (const auto& pair : currentLeftHandJointValues)
{
-// ARMARX_INFO << VAROUT(pair.first);
+ // ARMARX_INFO << VAROUT(pair.first);
QString name(pair.first.c_str());
QTableWidgetItem* newItem = new QTableWidgetItem(name);
ui.tableWidgetLeftHand->setItem(frameLeftHandRowIdx, 0, newItem);
@@ -486,29 +624,57 @@ namespace armarx
ui.comboLeftHandJoints->setCurrentIndex(-1);
-
NameValueMap currentRightHandJointValues = rightHandUnitProxy->getCurrentJointValues();
-// ARMARX_INFO << VAROUT(rightHandUnitProxy->getCurrentJointValues());
+ // ARMARX_INFO << VAROUT(rightHandUnitProxy->getCurrentJointValues());
int frameRightHandRowIdx = 0;
- connect(ui.buttonPreshapeRightHand, SIGNAL(clicked()), this, SLOT(preshapeRightHand()), Qt::UniqueConnection);
- connect(ui.buttonOpenRightHand, SIGNAL(clicked()), this, SLOT(openRightHand()), Qt::UniqueConnection);
- connect(ui.buttonCloseRightHand, SIGNAL(clicked()), this, SLOT(closeRightHand()), Qt::UniqueConnection);
- connect(ui.buttonRelaxRightHand, SIGNAL(clicked()), this, SLOT(relaxRightHand()), Qt::UniqueConnection);
+ connect(ui.buttonPreshapeRightHand,
+ SIGNAL(clicked()),
+ this,
+ SLOT(preshapeRightHand()),
+ Qt::UniqueConnection);
+ connect(ui.buttonOpenRightHand,
+ SIGNAL(clicked()),
+ this,
+ SLOT(openRightHand()),
+ Qt::UniqueConnection);
+ connect(ui.buttonCloseRightHand,
+ SIGNAL(clicked()),
+ this,
+ SLOT(closeRightHand()),
+ Qt::UniqueConnection);
+ connect(ui.buttonRelaxRightHand,
+ SIGNAL(clicked()),
+ this,
+ SLOT(relaxRightHand()),
+ Qt::UniqueConnection);
ui.horizontalSliderRightHandJointPos->setMaximum(100);
ui.horizontalSliderRightHandJointPos->setMinimum(0);
- connect(ui.horizontalSliderRightHandJointPos, SIGNAL(sliderMoved(int)), this, SLOT(requestSetRightHandJointAngles()), Qt::UniqueConnection);
- connect(ui.comboRightHandJoints, SIGNAL(currentIndexChanged(int)), this, SLOT(selectRightHandJoint(int)), Qt::UniqueConnection);
+ connect(ui.horizontalSliderRightHandJointPos,
+ SIGNAL(sliderMoved(int)),
+ this,
+ SLOT(requestSetRightHandJointAngles()),
+ Qt::UniqueConnection);
+ connect(ui.comboRightHandJoints,
+ SIGNAL(currentIndexChanged(int)),
+ this,
+ SLOT(selectRightHandJoint(int)),
+ Qt::UniqueConnection);
+ connect(ui.rightExpertMode,
+ &QCheckBox::stateChanged,
+ [&](int state) { setExpertMode(side::right, state); });
+ connect(ui.rightTare, &QPushButton::clicked, [&] { tareHand(side::right); });
+ connect(ui.rightReset, &QPushButton::clicked, [&] { resetHand(side::right); });
ui.tableWidgetRightHand->setRowCount(currentRightHandJointValues.size());
ui.tableWidgetRightHand->setColumnCount(2);
ui.comboRightHandJoints->clear();
- qRegisterMetaType<QVector<int> >("QVector<int>");
+ qRegisterMetaType<QVector<int>>("QVector<int>");
for (const auto& pair : currentRightHandJointValues)
{
-// ARMARX_INFO << VAROUT(pair.first);
+ // ARMARX_INFO << VAROUT(pair.first);
QString name(pair.first.c_str());
QTableWidgetItem* newItem = new QTableWidgetItem(name);
ui.tableWidgetRightHand->setItem(frameRightHandRowIdx, 0, newItem);
@@ -520,31 +686,38 @@ namespace armarx
// reloading of preshapes
- connect(ui.reloadPreshapesButton, SIGNAL(clicked()), this, SLOT(reloadPreshapes()), Qt::UniqueConnection);
-
+ connect(ui.reloadPreshapesButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(reloadPreshapes()),
+ Qt::UniqueConnection);
}
-
-
-
-
- void armarx::HandUnitWidget::reportHandShaped(const std::string& handName, const std::string& handShapeName, const Ice::Current&)
+ void
+ armarx::HandUnitWidget::reportHandShaped(const std::string& handName,
+ const std::string& handShapeName,
+ const Ice::Current&)
{
ARMARX_IMPORTANT << handName << ": " << handShapeName;
}
- void armarx::HandUnitWidget::reportNewHandShapeName(const std::string& handName, const std::string& handShapeName, const Ice::Current&)
+ void
+ armarx::HandUnitWidget::reportNewHandShapeName(const std::string& handName,
+ const std::string& handShapeName,
+ const Ice::Current&)
{
ARMARX_IMPORTANT << handName << ": " << handShapeName;
}
- void armarx::HandUnitWidget::reportJointAngles(const armarx::NameValueMap& actualJointAngles, const Ice::Current& c)
+ void
+ armarx::HandUnitWidget::reportJointAngles(const armarx::NameValueMap& actualJointAngles,
+ const Ice::Current& c)
{
-
}
- void armarx::HandUnitWidget::reportJointPressures(const armarx::NameValueMap& actualJointPressures, const Ice::Current& c)
+ void
+ armarx::HandUnitWidget::reportJointPressures(const armarx::NameValueMap& actualJointPressures,
+ const Ice::Current& c)
{
-
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
index dd68d856fad0cfbf0aa215d607826683927ad9f4..a4be90635175282654b604fa7a25c561debe8101 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
@@ -24,20 +24,20 @@
#pragma once
/* ArmarX headers */
-#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitGuiPlugin.h>
#include <ArmarXCore/core/Component.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitGuiPlugin.h>
#include <RobotAPI/interface/units/HandUnitInterface.h>
/* Qt headers */
-#include <QMainWindow>
-
#include <string>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <QMainWindow>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
namespace armarx
{
@@ -48,15 +48,16 @@ namespace armarx
\brief This plugin offers a widget witch which the HandUnit can be controlled.
\see HandUnitWidget
*/
- class HandUnitGuiPlugin :
- public ArmarXGuiPlugin
+ class HandUnitGuiPlugin : public ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
public:
HandUnitGuiPlugin();
- QString getPluginName() override
+
+ QString
+ getPluginName() override
{
return "HandUnitGuiPlugin";
}
@@ -81,8 +82,10 @@ namespace armarx
Q_OBJECT
public:
HandUnitWidget();
+
~HandUnitWidget() override
- {}
+ {
+ }
// inherited from Component
void onInitComponent() override;
@@ -92,15 +95,19 @@ namespace armarx
// HandUnitListener interface
void reportHandShaped(const std::string&, const std::string&, const Ice::Current&) override;
- void reportNewHandShapeName(const std::string&, const std::string&, const Ice::Current&) override;
-
+ void reportNewHandShapeName(const std::string&,
+ const std::string&,
+ const Ice::Current&) override;
// inherited of ArmarXWidget
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.HandUnitGUI";
}
- static QIcon GetWidgetIcon()
+
+ static QIcon
+ GetWidgetIcon()
{
return QIcon("://icons/hand.svg");
}
@@ -110,6 +117,12 @@ namespace armarx
void saveSettings(QSettings* settings) override;
void configured() override;
+ enum class side
+ {
+ left,
+ right
+ };
+
public slots:
void preshapeLeftHand();
@@ -126,6 +139,9 @@ namespace armarx
void relaxRightHand();
void updateInfoLabel();
void updateJointValueTable();
+ void setExpertMode(side side, int state);
+ void tareHand(side side);
+ void resetHand(side side);
void selectLeftHandJoint(int i);
void selectRightHandJoint(int i);
@@ -161,14 +177,17 @@ namespace armarx
bool setRightHandJointAnglesFlag;
std::basic_string<char> currentLeftHandJoint;
std::basic_string<char> currentRightHandJoint;
- double leftHandConversionFactor= 1.0 / 100.0;
+ double leftHandConversionFactor = 1.0 / 100.0;
double rightHandConversionFactor = 1.0 / 100.0;
// HandUnitListener interface
public:
- void reportJointAngles(const::armarx::NameValueMap& actualJointAngles, const Ice::Current&) override;
- void reportJointPressures(const::armarx::NameValueMap& actualJointPressures, const Ice::Current&) override;
+ void reportJointAngles(const ::armarx::NameValueMap& actualJointAngles,
+ const Ice::Current&) override;
+ void reportJointPressures(const ::armarx::NameValueMap& actualJointPressures,
+ const Ice::Current&) override;
};
+
using HandUnitGuiPluginPtr = std::shared_ptr<HandUnitWidget>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.ui b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.ui
index 79ffd00e83b842c062a61e9860f92f92eeda7d5b..031253abc93df81d1c27531bd4e07be27535a0eb 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.ui
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.ui
@@ -57,7 +57,6 @@
</property>
<property name="font">
<font>
- <weight>75</weight>
<bold>true</bold>
</font>
</property>
@@ -154,6 +153,40 @@
</item>
</layout>
</item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QCheckBox" name="leftExpertMode">
+ <property name="text">
+ <string>I know what I'm doing</string>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="leftTare">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>tare</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="leftReset">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>reset</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
<item>
<widget class="QTableWidget" name="tableWidgetLeftHand"/>
</item>
@@ -171,7 +204,6 @@
</property>
<property name="font">
<font>
- <weight>75</weight>
<bold>true</bold>
</font>
</property>
@@ -268,6 +300,40 @@
</item>
</layout>
</item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QCheckBox" name="rightExpertMode">
+ <property name="text">
+ <string>I know what I'm doing</string>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="rightTare">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>tare</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="rightReset">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>reset</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
<item>
<widget class="QTableWidget" name="tableWidgetRightHand"/>
</item>
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 440815d7d495c3a7a0e57acdde433e4a3a1d672f..a5c4e264d190647807344cfdca73a812ac6b3f57 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -44,6 +44,13 @@
#include <RobotAPI/interface/core/NameValueMap.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+
+
#include "KinematicUnitConfigDialog.h"
// Qt headers
@@ -993,7 +1000,7 @@ namespace armarx
if (robot != NULL)
{
ARMARX_VERBOSE << "getting coin visualization" << flush;
- coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
+ coinVisualization = robot->getVisualization();
if (!coinVisualization || !coinVisualization->getCoinVisualization())
{
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 1c94c93057f2a7ff961809c63aab383df6b5623e..f569285ac3ba1ccefd86a88d4e7cfa2db47865eb 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -40,19 +40,15 @@
#include <QToolBar>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Visualization/VisualizationFactory.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
-
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoEventCallback.h>
#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
#include <Inventor/Qt/SoQt.h>
+
#include <QStyledItemDelegate>
+
#include <ArmarXCore/core/util/IceReportSkipper.h>
#include <VirtualRobot/VirtualRobot.h>
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index bbd260410802a01a317945a6893194ccaddfb91e..d13aaf8d1234f45d5fa17e0dbec4d066f3b4e32c 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -33,8 +33,14 @@
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
// Qt headers
#include <Qt>
@@ -389,7 +395,7 @@ void RobotViewerWidgetController::setRobotVisu(bool colModel)
v = VirtualRobot::SceneObject::Collision;
}
- CoinVisualizationPtr robotViewerVisualization = robot->getVisualization<CoinVisualization>(v);
+ CoinVisualizationPtr robotViewerVisualization = robot->getVisualization(v);
if (robotViewerVisualization)
{
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
index 48a161aa9eed86e54d45d7cf58a753b258880005..ebb76786af86a2dff64ea0880547a376a5ae1d06 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
@@ -37,12 +37,7 @@
/* Qt headers */
#include <QMainWindow>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Visualization/VisualizationFactory.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <Inventor/sensors/SoTimerSensor.h>
@@ -186,4 +181,3 @@ namespace armarx
};
using RobotViewerGuiPluginPtr = std::shared_ptr<RobotViewerWidgetController>;
}
-
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
index 80b861c28d248f67d375eb5b4e3860f354d660f5..be74b8d21b39b76d9847876d2b806e9c43c74f75 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
@@ -27,7 +27,10 @@
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
// C++ includes
#include <sstream>
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
index e3a78e725f07cf73ac8fc10d478c3abf914ee7f6..bcbfc2e079c91bd5db1f114bfa4902b3cd99b954 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
@@ -29,11 +29,10 @@
// ArmarX includes
#include <RobotAPI/interface/units/TCPMoverUnitInterface.h>
#include <RobotAPI/interface/units/TCPControlUnit.h>
+
/** VirtualRobot headers **/
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
@@ -151,4 +150,3 @@ namespace armarx
QPointer<TCPMoverConfigDialog> configDialog;
};
}
-
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
index 1fdaaa9630a09ab4a73f83fc05fa065fd0663e4d..7ba61b702daed837495375e5afeeec47b695cda9 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
@@ -6,6 +6,7 @@
#include <QObject>
#include <QTreeWidgetItem>
#include <QVBoxLayout>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
index bfe1be2bf154768ae5ff49eff04d13caf492d133..768d907b7bb7e81576a5a50c9e9bfe5e353383e1 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
@@ -1,6 +1,7 @@
#include "IntEnumWidget.h"
#include <QHBoxLayout>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
#include "RobotAPI/libraries/aron/core/type/variant/All.h"
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
index b1d6fa3b946a704b180109c2598b663e2ba03bad..7ba00d9da9f38fa7975ff67cd2096e30b30cbbbc 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
@@ -3,6 +3,8 @@
#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
+#include <RobotAPI/interface/aron/Aron.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "../../ColorPalettes.h"
#include "NDArrayHelper.h"
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
index d55d2bba35e2712d1d161c83ed86b06b300bcf2d..e1fca223ef52042b8174231d2d0eaf320efcf158 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
@@ -1,11 +1,14 @@
#pragma once
+
+#include <cmath>
#include <vector>
#include <QLineEdit>
#include <QObject>
#include <QVBoxLayout>
-
-#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+#include <SimoxUtility/error/SimoxError.h>
+#include <RobotAPI/interface/aron/Aron.h>
#include "CustomWidget.h"
diff --git a/source/RobotAPI/interface/units/HandUnitInterface.ice b/source/RobotAPI/interface/units/HandUnitInterface.ice
index 76a1000820a4f85b0fd21194aa72b13cb88bcc75..2badb58fdedf6ac7052ff0a4738deab7d5393314 100644
--- a/source/RobotAPI/interface/units/HandUnitInterface.ice
+++ b/source/RobotAPI/interface/units/HandUnitInterface.ice
@@ -24,25 +24,25 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantContainers.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/VariantContainers.ice>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
dictionary<string, string> NameCommandMap;
- /**
+ /**
* Implements an interface to a HandUnit.
*/
interface HandUnitInterface extends SensorActorUnitInterface
{
- /**
+ /**
* setShape sets the name of a known hand shape for the hand unit.
* @param shapeName Name of the hand shape.
*/
@@ -71,7 +71,7 @@ module armarx
* E.g. a state that verifies if a grasp was successful can call this routine.
*/
void setObjectReleased(string objectName);
- /**
+ /**
* setJointAngles controls the joints of the hand unit.
* @param targetJointAngles Map of joint names and corresponding joint angle values.
*/
@@ -82,13 +82,35 @@ module armarx
void sendJointCommands(NameCommandMap targetJointCommands);
void reloadPreshapes();
+
+ /**
+ * @brief Will tare the relative encoders of the hand.
+ *
+ * For hands with relative encoders that support this function. No-op if not supported.
+ *
+ * This will change the relative encoder calibration! Make sure the hand is fully opened
+ * before taring.
+ */
+ void tare();
+
+ /**
+ * @brief Will reset the firmware of the hand, enforcing a reboot.
+ *
+ * For hands that support this function. No-op if not supported.
+ *
+ * This is a very disruptive operation and will result in the firmware rebooting for up to
+ * several seconds, potentially dropping objects in the hand or homing procedures of the
+ * hand motors. This is only meant to be used in cases where the hand is lost due to
+ * connection issues and reconnections.
+ */
+ void resetFirmware();
};
- /**
+ /**
* Implements an interface to a HandUnitListener.
*/
interface HandUnitListener
{
- /**
+ /**
* reportNewHandShapeName reports the current hand shape name.
* @param handName Name of the hand to be controlled.
* @param handName Name of the current hand shape name.
@@ -110,13 +132,9 @@ module armarx
* @param actualJointPressures Map of joint names and corresponding sensed pressure values.
**/
void reportJointPressures(NameValueMap actualJointPressures);
-
};
- /**
+ /**
* Implements an interface to a HandUnitObserver.
*/
- interface HandUnitObserverInterface extends ObserverInterface, HandUnitListener
- {
- };
-
+ interface HandUnitObserverInterface extends ObserverInterface, HandUnitListener{};
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 1167db200b5cacded7c52f265e8c2b9261b85bed..f2c6108a9ace4740e0add45b27ed7aa827347d41 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,7 +1,5 @@
#include <VirtualRobot/XML/ObjectIO.h>
-#include <set>
-
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 82dc134ee4b958981cf1ebe5d74047f49eb62a2a..4bb9a4a1d7078a761b8ac79c99464e1766d8d9fb 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -3,6 +3,7 @@
#include <SimoxUtility/math/pose/invert.h>
#include <SimoxUtility/math/pose/pose.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
index 036e0530c19328397c13f6a1e4b6975047cfd71b..c67e2f98dbdaf9c555890be875b4624075428fa4 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
@@ -1,5 +1,6 @@
#include "RequestedObjects.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -24,8 +25,11 @@ namespace armarx::objpose
void RequestedObjects::requestObjects(
const std::vector<armarx::ObjectID>& objectIDs, IceUtil::Time relativeTimeout)
{
+ ARMARX_INFO << VAROUT(relativeTimeout.toMilliSeconds());
+
if (relativeTimeout.toMilliSeconds() < 0)
{
+ ARMARX_INFO << "Infite localization request for object ids :" << objectIDs;
for (const auto& id : objectIDs)
{
infiniteRequests.push_back(id);
@@ -33,6 +37,8 @@ namespace armarx::objpose
}
else
{
+ ARMARX_INFO << "Localization request for " << relativeTimeout << " for object ids :" << objectIDs;
+
IceUtil::Time absoluteTimeout = TimeUtil::GetTime() + relativeTimeout;
Request req;
req.objectIDs = objectIDs;
@@ -48,8 +54,19 @@ namespace armarx::objpose
RequestedObjects::Update RequestedObjects::updateRequestedObjects(IceUtil::Time now)
{
// Remove requests with timeout.
- while (currentRequests.size() > 0 && currentRequests.begin()->first <= now)
+
+ if(not currentRequests.empty())
{
+ ARMARX_INFO << currentRequests.begin()->first - now;
+ }
+
+ while (not currentRequests.empty() and currentRequests.begin()->first <= now)
+ {
+ ARMARX_INFO << "No longer localizing objects: ";
+ for (const Request& request : currentRequests.begin()->second)
+ {
+ ARMARX_INFO << request.objectIDs;
+ }
currentRequests.erase(currentRequests.begin());
}
@@ -83,4 +100,3 @@ namespace armarx::objpose
}
}
-
diff --git a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.cpp b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.cpp
index ffb42a45b44248c318516b6e74fea570e5db0bd6..af5dec6f77b98de2ea386a39ffa4f5335f118e04 100644
--- a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.cpp
@@ -23,6 +23,7 @@
#include "BimanualGraspCandidateHelper.h"
#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
index 1b2e9b10cf81e17240e09af578468a12c13c4b40..9a861d414a7d35a13e500cc1365a99d8096a40ca 100644
--- a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
+++ b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
@@ -27,7 +27,7 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <memory>
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
index cba726500972e01f8f6ccf34fece40cbad4b2d4a..b8eafe669d406b838852dd10a9ab7a65aca10aec 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
@@ -23,6 +23,7 @@
#include "GraspCandidateHelper.h"
#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
index 0fa9fe1ea3d713e090dda9d3a529050e1b9f82d1..d47e00f3f922ec170d28ceb484b5a1539658e0f6 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
@@ -26,7 +26,7 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <memory>
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index e20c92b7599c0fde9ae42500beba8677a696f4ba..efa9b038a15f3a446af7063f05ccb93cf31b45de 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -30,6 +30,7 @@
#include <SimoxUtility/json/eigen_conversion.h>
#include <SimoxUtility/json/json.hpp>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
#include "ArmarXCore/core/logging/Logging.h"
diff --git a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
index b01b2c6e976d2f3d2f3e882668236cea36943c69..00a84074915aa9a7979ae47e08548c60d145de21 100644
--- a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
@@ -4,6 +4,8 @@
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <VirtualRobot/Robot.h>
+
#include "box_to_grasp_candidates.h"
namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
index 9888dccad9687c74234e5a0e6ee2b59a4511e141..f24952192f6174ece60745d1855f7985fcbc232e 100644
--- a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
@@ -1,4 +1,5 @@
#include <SimoxUtility/math/pose/pose.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index 8072390200cb2696c0a51bde1f7ca6807a76659d..38b5f30cd0d88591fa518fcb11fab3df7357424e 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -25,6 +25,10 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include "RobotStateComponentPlugin.h"
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index af85178be6aaa8e98da1e19e8c76dc0019bc237f..c385948dbea9cb9cf87755874eed4b9c847d3285 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -24,7 +24,7 @@
#include <mutex>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/ComponentPlugin.h>
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
index 0cbf7e730f56186618f92c0a74be2b5dc5ad0f5a..16308fdfe7da160f2691d894c24d5f6a33bc84ce 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
@@ -26,6 +26,7 @@
// Simox
#include <SimoxUtility/math.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
index 85d7543c956369817a0bb4df0c3dbda9fd22a576..d372ee8da0badc3114a8b60971e88a950e970c54 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
@@ -28,7 +28,7 @@
#include <Eigen/Core>
// Simox
-#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/VirtualRobot.h>
// RobotAPI
#include <RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h>
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index b5fe191b9674e89b0105679a22ddc6fdd459b0e5..7fbe6b3f88261e2815f1b8ca11a397c00968424d 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -27,8 +27,11 @@
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Robot.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index 40d1640eab8b5019577e0289cd1c8b19d5f932e8..279242ca5638f3643674bdcc13cabcba593b24d7 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -27,7 +27,8 @@
#include "VelocityControllerHelper.h"
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/IK/JacobiProvider.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/Pose.h>
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index cf5eb9b333563c0850e9e4fa5f7b618041958f63..602cda54231704749972b62390cf0323efe043b2 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -31,6 +31,11 @@
#include <ArmarXCore/util/CPPUtility/trace.h>
// #include <ArmarXCore/core/system/ArmarXDataPath.cpp>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+
#include <Eigen/Dense>
#include <algorithm>
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index c1fa9b4d74194e7fd4b8c2a5748d81ef4ccfdcbd..86e01df9c63b146cf3d37d1b44768b4fa76b3166 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -25,9 +25,7 @@
#include <RobotAPI/interface/core/RobotState.h>
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/Visualization/TriMeshModel.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index 3ab4e0530b54331305cdbc8f8ad05603607b0ac9..fb834d52c07e86f500d05c097d5a5d19616ee2c5 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -185,7 +185,7 @@ namespace armarx
{
if (_last_iteration_id != -1 && _last_iteration_id + 1 != step.iterationId)
{
- ARMARX_INFO << deactivateSpam(10)
+ ARMARX_INFO << deactivateSpam(60)
<< "Missing Iterations or iterations out of order! "
<< "This should not happen. " << VAROUT(_last_iteration_id) << ", "
<< VAROUT(step.iterationId);
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index d476b25ef2003edeeb183fbe4e74bcd70a6176c0..cffe940bcead2f2875ac88c9daa9ba5abb715c52 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -2,7 +2,6 @@
// STD/STL
-#include <mutex>
#include <optional>
#include <vector>
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
index 89e2cf20cb7d5786458ed5453ece73749e4783bd..ecad82410f820bcf6c4fbafa67440e9cb4abbaed 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
@@ -21,6 +21,7 @@
*/
#include "MemoryToDebugObserver.h"
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
#include <RobotAPI/libraries/armem/core/json_conversions.h>
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
index 4950280b340f15c293fdd84d9c9fe25e7b19bf9d..1672554850925c164226123396fd28d681cc9687 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -21,8 +21,6 @@
#pragma once
-#include <mutex>
-
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
index b0302a5cef0178acec1b539500bb591c9eb684f2..ce8932e546b096d2ee30bdc47244fcb59b77f920 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
@@ -2,8 +2,7 @@
#include "EntityInstance.h"
// STD / STL
-#include <fstream>
-#include <iostream>
+#include <SimoxUtility/algorithm/string/string_tools.h>
// ArmarX
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
index 2f974697368c5a375dd9a27715f06cd1cc49dfaa..175840a0c29a0b2ab96b24172c4b92ddf289a234 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
@@ -1,10 +1,6 @@
// Header
#include "EntitySnapshot.h"
-// STD / STL
-#include <fstream>
-#include <iostream>
-
// ArmarX
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
index 03ba8c8241ad20f9301e0df4ad2db2707fc7410c..99f72f1296f4c6c50c5658e173dffded97bcd575 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
@@ -13,6 +13,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/logging/LoggingUtil.h>
#include <ArmarXCore/core/time.h>
+#include <ArmarXCore/interface/core/UserException.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/operations.h>
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.cpp
index e1024a0f6333dd1919f9cf2dd7f66336b293b906..08d4b7a5111d33a6c54bd2afb8ab14c85526a535 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.cpp
@@ -1,6 +1,7 @@
#include "mongodb.h"
// Simox
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/json.h>
#include <RobotAPI/libraries/armem/core/error.h>
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
index 00568c75b2d8e69eabd2d56d27a0b99e1744bcd6..83413578f58480e7d19517980623c88159b5985f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
@@ -1,6 +1,7 @@
#include "Processors.h"
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/interface/core/UserException.h>
#include "converter/data/image/exr/ExrConverter.h"
#include "converter/data/image/png/PngConverter.h"
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.cpp
index b9d4a92d4514075c72d9f70948ab9d39a7428d65..3294e56c181c8811d7471a7fb01a46db48ee14cf 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.cpp
@@ -3,6 +3,7 @@
// ArmarX
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
#include <opencv2/opencv.hpp>
#include <RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h>
diff --git a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
index fcce7ea3a3319aa4b7afeeacae9c02abb7f00482..2f48fb22ba086c0ff82dd096272d2b317d41a086 100644
--- a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
+++ b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
@@ -21,6 +21,10 @@
<ObjectChild key='pose'>
<Pose />
</ObjectChild>
+
+ <ObjectChild key='prepose'>
+ <Pose optional="true" />
+ </ObjectChild>
</Object>
<!-- A Grasp set is a set of grasps that need to be executed to grasp an object. The order is important! -->
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
index ab4750e34435213bd5db7541012fe8e21a7bb2bb..9cf9edee2467c7950002397cd6d5304cc47a462f 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -1,8 +1,9 @@
#include "KnownGraspCandidateReader.h"
-#include <mutex>
#include <optional>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -13,9 +14,6 @@
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
-// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::grasping::known_grasps
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index c805d2ee70a55356bfd46c463cd2f9b315aec583..2ae44a2916d7dceedeb06a6699b3aaa582bc3cef 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -1,6 +1,9 @@
#include "KnownGraspProviderSegment.h"
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Grasping/GraspSet.h>
+#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
@@ -63,9 +66,46 @@ namespace armarx::armem::grasping::segment
arondto::KnownGraspSet retGraspSet;
retGraspSet.name = graspSet->getName();
- retGraspSet.robot = graspSet->getRobotType();
+
+ retGraspSet.robot = simox::alg::split(graspSet->getRobotType(), " ").front();
+
retGraspSet.endeffector = graspSet->getEndEffector();
+ VirtualRobot::GraspSetPtr preGraspSet = [&graspSet]()
+ {
+ std::vector<VirtualRobot::GraspPtr> preGrasps;
+ for (const auto& grasp : graspSet->getGrasps())
+ {
+ // check if grasp is a prepose by checking if grasp name ends with "_Prepose"
+ if (simox::alg::ends_with(grasp->getName(), PREPOSE_SUFFIX))
+ {
+ preGrasps.emplace_back(grasp);
+ }
+
+ // everything else is assumed to be a grasp
+ }
+
+ // create a new grasp set with the preposes
+ VirtualRobot::GraspSetPtr preGraspSet = graspSet->clone();
+ preGraspSet->removeAllGrasps(); // just keep the EEF info etc
+ for (const auto& preGrasp : preGrasps)
+ {
+ preGraspSet->addGrasp(preGrasp);
+ }
+
+ return preGraspSet;
+ }();
+
+
+ ARMARX_DEBUG << VAROUT(preGraspSet->getSize());
+
+ // remove all preposes from the grasp set
+ for (const auto& preGrasp : preGraspSet->getGrasps())
+ {
+ graspSet->removeGrasp(preGrasp);
+ }
+
+
for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
{
ARMARX_CHECK_NOT_NULL(grasp);
@@ -76,8 +116,57 @@ namespace armarx::armem::grasping::segment
retGrasp.quality = grasp->getQuality();
retGrasp.creator = grasp->getCreationMethod();
retGrasp.pose = grasp->getTransformation();
+ retGrasp.prepose.reset();
- ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '"
+ // check if grasp has a prepose by checking if grasp name ends with "_Prepose"
+ {
+ const std::string prePoseName = retGrasp.name + PREPOSE_SUFFIX;
+
+ ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+
+ if (preGraspSet->hasGrasp(prePoseName))
+ {
+ retGrasp.prepose =
+ preGraspSet->getGrasp(prePoseName)->getTransformation();
+
+ // remove the prepose from the set as it found its match
+ preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.prepose.value();
+ }
+ }
+
+ // check if grasp has a prepose for a grasp with a specific name, e.g., "XY_Grasp" (GRASP_OPTIONAL_SUFFIX)
+ if (// not retGrasp.prepose.has_value() and
+ simox::alg::ends_with(retGrasp.name, GRASP_OPTIONAL_SUFFIX))
+ {
+ const std::string prePoseName =
+ retGrasp.name.substr(
+ 0, retGrasp.name.size() - std::strlen(GRASP_OPTIONAL_SUFFIX)) +
+ PREPOSE_SUFFIX;
+
+ ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+
+ if (preGraspSet->hasGrasp(prePoseName))
+ {
+ retGrasp.prepose =
+ preGraspSet->getGrasp(prePoseName)->getTransformation();
+
+ // remove the prepose from the set as it found its match
+ preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.prepose.value();
+ }
+ }
+
+ ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
<< retGraspSet.name << "' for obj '" << objectClassName
<< "' with pose \n"
<< retGrasp.pose;
@@ -85,6 +174,19 @@ namespace armarx::armem::grasping::segment
retGraspSet.grasps.push_back(retGrasp);
}
+ // Now, check if there are any preposes left in the set. This should not have happened.
+ if (preGraspSet->getSize() > 0)
+ {
+ ARMARX_WARNING << "Found " << preGraspSet->getSize()
+ << " preposes in the grasp set '" << retGraspSet.name
+ << "' for obj '" << objectClassName
+ << "' that do not have a corresponding grasp!";
+ for(const auto& preGrasp : preGraspSet->getGrasps())
+ {
+ ARMARX_WARNING << "Prepose '" << preGrasp->getName();
+ }
+ }
+
ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) ==
0)
<< "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name
@@ -132,13 +234,13 @@ namespace armarx::armem::grasping::segment
update.instancesData = {knownGraspCandidate->toAron()};
- ARMARX_VERBOSE << VAROUT(knownGraspCandidate->graspSets.size());
+ ARMARX_DEBUG << VAROUT(knownGraspCandidate->graspSets.size());
for (const auto& gs : knownGraspCandidate->graspSets)
{
- ARMARX_VERBOSE << VAROUT(gs.second.grasps.size());
+ ARMARX_DEBUG << VAROUT(gs.second.grasps.size());
for (const auto& grasp : gs.second.grasps)
{
- ARMARX_VERBOSE << VAROUT(grasp.name);
+ ARMARX_DEBUG << VAROUT(grasp.name);
}
}
}
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
index de92372581076450341fb53e4628c4dd7ae4ba7c..75aadd96aff419a636b802fcc73548b080e4d8d8 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
@@ -23,5 +23,9 @@ namespace armarx::armem::grasping::segment
public:
static const constexpr char* CORE_SEGMENT_NAME = "KnownGraspCandidate";
static const constexpr char* PROVIDER_SEGMENT_NAME = "PriorKnowledgeData";
+
+ private:
+ static const constexpr char* PREPOSE_SUFFIX = "_Prepose";
+ static const constexpr char* GRASP_OPTIONAL_SUFFIX = "_Grasp";
};
}
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index bfa97db1e1afe59d2bd03bc47af21ea60fe3968f..fc6c74049d6768053daa7eb8cec2a8553bb0cd9f 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -15,6 +15,7 @@
#include <Ice/Exception.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index 868c315ad5922061332ef323cc2aa0bb346e9958..0b7b737f03b2fd65f11f498f41c5e47c2e7dc5e9 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -8,6 +8,8 @@
#include <QPushButton>
#include <QSpacerItem>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/armem/server/ltm/Memory.h>
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
index 13cc8ff5a3e515e79e80838a875acad24a6186cd..a9bb3a1b19012fdfefd724d4cf65b0eaad7736bb 100644
--- a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
@@ -1,5 +1,7 @@
#include "Reader.h"
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index a702857316a46fb59fd581c95e92b8b2413d371e..f0ec3136995b80a3a8efedb5132e73df04801849 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -7,6 +7,7 @@
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -32,37 +33,27 @@ namespace armarx::armem::articulated_object
const std::string& instanceName,
VirtualRobot::RobotIO::RobotDescription loadMode)
{
- const auto descriptions = queryDescriptions(timestamp, providerName);
+ ARMARX_VERBOSE << "Getting `" << typeName + "/" + instanceName << "`";
- ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+ const std::optional<ArticulatedObject> articulatedObjectDescription =
+ get(typeName + "/" + instanceName, timestamp, providerName);
+ ARMARX_CHECK_NOT_NULL(articulatedObjectDescription)
+ << "Failed to get articulated object `" << typeName << "/" << instanceName << "`";
- const auto it = std::find_if(
- descriptions.begin(),
- descriptions.end(),
- [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
- { return desc.name == typeName; });
-
- if (it == descriptions.end())
- {
- ARMARX_WARNING << "Description for articulate object with type <" << typeName
- << "> not (yet) available!";
- return nullptr;
- }
-
- ARMARX_DEBUG << "Description for articulate object with type <" << typeName
- << "> available!";
-
- auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(), loadMode);
+ auto obj = VirtualRobot::RobotIO::loadRobot(
+ articulatedObjectDescription->description.xml.toSystemPath(), loadMode);
if (not obj)
{
- ARMARX_WARNING << "Failed to load description for articulated object <" << typeName
- << ">!";
+ ARMARX_WARNING << "Failed to load articulated object `" << typeName << "/"
+ << instanceName << "` from file `"
+ << articulatedObjectDescription->description.xml.toSystemPath() << "`.";
+
return nullptr;
}
obj->setName(instanceName);
- obj->setType(it->name);
+ obj->setType(typeName);
return obj;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index f293741f9ba9baa56853717f1984389ce30ad1d1..9f69b93009bed3d41c34e0d99d2952443e399671 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -3,7 +3,9 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -51,7 +53,8 @@ namespace armarx::armem::articulated_object
bool
ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
- const armem::Time& timestamp)
+ const armem::Time& timestamp,
+ const bool isStatic)
{
ARMARX_CHECK_NOT_NULL(articulatedObject);
@@ -59,6 +62,6 @@ namespace armarx::armem::articulated_object
armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
convert(*articulatedObject, Time::Now());
- return store(armemArticulatedObject);
+ return store(armemArticulatedObject, isStatic);
}
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
index ff39d70f049fa670e41157dbb79608b16d98b66c..edc401b6e0d55487f9446c1b5d0129cd623ea5c1 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
@@ -14,8 +14,8 @@ namespace armarx::armem::articulated_object
public:
using Writer::Writer;
- bool
- storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
- const armem::Time& timestamp);
+ bool storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp,
+ bool isStatic = false);
};
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index e0ce311486d06c5dc859422976f4c96aa9d037dc..55a10158299de3cd3a64883ea44fb957bd0c355a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -1,15 +1,15 @@
#include "Reader.h"
-#include <mutex>
#include <optional>
#include <Eigen/Geometry>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include "RobotAPI/libraries/armem/core/Commit.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
@@ -171,6 +171,7 @@ namespace armarx::armem::articulated_object
if (providerName.has_value()) // query single provider
{
+ ARMARX_VERBOSE << "Single provider query";
ARMARX_CHECK_NOT_EMPTY(providerName.value());
// clang-format off
@@ -183,6 +184,8 @@ namespace armarx::armem::articulated_object
}
else // query all providers
{
+ ARMARX_VERBOSE << "All provider query";
+
// clang-format off
qb
.coreSegments().withName(objects::constants::CoreClassSegmentName)
@@ -192,7 +195,9 @@ namespace armarx::armem::articulated_object
// clang-format on
}
+ ARMARX_VERBOSE << "Before query";
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ ARMARX_VERBOSE << "After query";
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
@@ -201,6 +206,8 @@ namespace armarx::armem::articulated_object
return {};
}
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "getRobotDescriptions";
return getRobotDescriptions(qResult.memory);
}
@@ -380,20 +387,18 @@ namespace armarx::armem::articulated_object
memory.getCoreSegment(objects::constants::CoreClassSegmentName);
std::vector<robot_state::description::RobotDescription> descriptions;
- coreSegment.forEachEntity(
- [&descriptions](const wm::Entity& entity)
+ coreSegment.forEachInstance(
+ [&descriptions](const wm::EntityInstance& instance)
{
- if (not entity.empty())
+ ARMARX_VERBOSE << "Converting ...";
+ if (const auto robotDescription = convertRobotDescription(instance))
{
- const auto robotDescription =
- convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
- if (robotDescription)
- {
- descriptions.push_back(*robotDescription);
- }
+ descriptions.push_back(*robotDescription);
}
});
+ ARMARX_VERBOSE << descriptions.size() << " descriptions";
+
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index d061c692e3399b07279a6cd5420c295396d34e38..f5ae8c13b383662411bc4d33399cc87ec3048a79 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -15,11 +15,11 @@
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include "utils.h"
@@ -191,7 +191,7 @@ namespace armarx::armem::articulated_object
}
bool
- Writer::storeInstance(const ArticulatedObject& obj) const
+ Writer::storeInstance(const ArticulatedObject& obj, const bool isStatic) const
{
std::lock_guard g{memoryWriterMutex};
@@ -218,7 +218,7 @@ namespace armarx::armem::articulated_object
arondto::ObjectInstance objectInstance;
toAron(objectInstance, obj.config);
- const auto classId = storeOrGetClass(obj);
+ const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
if (not classId)
{
@@ -229,10 +229,17 @@ namespace armarx::armem::articulated_object
// install memory link
toAron(objectInstance.classID, *classId);
+ // set object instance id
+ const MemoryID memoryInstanceId = classId->withEntityName(entityName);
+
armem::MemoryID id;
- id.setEntityID(classId->getEntityID());
+ id.setEntityID(memoryInstanceId.getEntityID());
armarx::ObjectID objectId(id.entityName);
+ ARMARX_INFO << "Object ID: " << objectId;
+
+ ARMARX_CHECK_NOT_EMPTY(objectId.instanceName())
+ << "An object instance name must be provided!";
armarx::arondto::ObjectID cs;
cs.className = objectId.className();
@@ -243,6 +250,8 @@ namespace armarx::armem::articulated_object
objectInstance.pose.providerName = properties.providerName;
objectInstance.pose.attachmentValid = false;
+ objectInstance.pose.isStatic = isStatic;
+
update.instancesData = {objectInstance.toAron()};
update.referencedTime = timestamp;
@@ -260,7 +269,7 @@ namespace armarx::armem::articulated_object
}
bool
- Writer::store(const ArticulatedObject& obj) const
+ Writer::store(const ArticulatedObject& obj, const bool isStatic) const
{
const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
@@ -271,7 +280,7 @@ namespace armarx::armem::articulated_object
return false;
}
- return storeInstance(obj);
+ return storeInstance(obj, isStatic);
}
// TODO this is a duplicate
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index dcc49a2b2dc9e9e32a8edd06efa2026e0bb326c6..568bec35dad164f665cd41f9067d92b16693e380 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -45,9 +45,9 @@ namespace armarx::armem::articulated_object
void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- bool store(const ArticulatedObject& obj) const override;
+ bool store(const ArticulatedObject& obj, bool isStatic) const override;
- bool storeInstance(const ArticulatedObject& obj) const;
+ bool storeInstance(const ArticulatedObject& obj, bool isStatic) const;
std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj) const;
// const std::string& getPropertyPrefix() const override;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
index 899cb1e79ac7ada7991e064b2a1ae1c20310749a..19043a9cc3d4c3409012d7111eee172edb4d579b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
@@ -30,7 +30,7 @@ namespace armarx::armem::articulated_object
public:
virtual ~WriterInterface() = default;
- virtual bool store(const ArticulatedObject& obj) const = 0;
+ virtual bool store(const ArticulatedObject& obj, bool isStatic) const = 0;
};
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
index a141b97e4e8b9d4b169ff5b9118c330b71080d5a..fcb2a94487dc7034ce7a02c24478a35890e5ff77 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
@@ -1,6 +1,5 @@
#include "Segment.h"
-#include <filesystem>
#include <sstream>
#include <sys/inotify.h>
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
index 39604fa3dabac64b806d63001bcb2866e2b421ed..84d9e402e1f6816faa9b6a4494d7bf68af9ce2f8 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
@@ -3,6 +3,7 @@
#include <Eigen/Geometry>
#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/cmaps/colormaps.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
@@ -41,10 +42,13 @@ namespace armarx::armem::server::obj::familiar_object_instance
defs->optional(visualizePointCloud, prefix + "visualizePointCloud", "");
defs->optional(visualizePose, prefix + "visualizePose", "");
defs->optional(visualizeBoundingBox, prefix + "visualizeBoundingBox", "");
+ defs->optional(visualizeLabels, prefix + "visualizeLabels", "");
defs->optional(objectFrames, prefix + "objectFrames", "Enable showing object frames.");
defs->optional(
objectFramesScale, prefix + "objectFramesScale", "Scaling of object frames.");
+ defs->optional(
+ visualizationDuration, prefix + "maxAgeSeconds", "Maximum age in seconds for visualization.");
}
void
@@ -70,8 +74,12 @@ namespace armarx::armem::server::obj::familiar_object_instance
{
auto layerPose = arviz.layer("familiar_objects/pose/" + providerName);
auto layerPointCloud = arviz.layer("familiar_objects/points/" + providerName);
+ auto layerLabels = arviz.layer("familiar_objects/labels/" + providerName);
auto layerBox = arviz.layer("familiar_objects/box/" + providerName);
+ auto confidenceCmap = simox::color::cmaps::viridis();
+ confidenceCmap.set_vlimits(0, 1);
+
for (const auto& familiarObject : familiarObjects)
{
if (not isWithinTimeFrame(familiarObject))
@@ -102,6 +110,19 @@ namespace armarx::armem::server::obj::familiar_object_instance
layerPointCloud.add(points);
}
+ if (visualizeLabels)
+ {
+ Eigen::Isometry3f pose{familiarObject.poseGlobal->pose};
+
+ // gently above object
+ // FIXME derive from bounding box
+ pose.translation().z() += 200;
+
+ const std::string name = familiarObject.objectID.className + "/" + familiarObject.objectID.instanceName;
+
+ layerLabels.add(viz::Text(name).pose(pose).scale(20));
+ }
+
if (visualizeBoundingBox)
{
@@ -120,7 +141,11 @@ namespace armarx::armem::server::obj::familiar_object_instance
auto box = viz::Box(objectId.str());
box.pose(global_T_bb.matrix());
box.size(familiarObject.bounding_box.extents);
- box.color(simox::Color::gray(128, alpha));
+
+ auto color = confidenceCmap.at(familiarObject.confidence);
+ color.a = alpha;
+
+ box.color(color);
layerBox.add(box);
}
}
@@ -128,6 +153,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
layers.push_back(layerPose);
layers.push_back(layerPointCloud);
+ layers.push_back(layerLabels);
layers.push_back(layerBox);
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
index c5b08d065c2a3cbb607f3e007973d2925a394882..d92017ae1b729eedc20a58ff63b2481df32cd997 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
@@ -53,6 +53,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
bool visualizePointCloud = true;
bool visualizeBoundingBox = true;
bool visualizePose = true;
+ bool visualizeLabels = true;
bool objectFrames = false;
float objectFramesScale = 1.0;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 130e229fb4f9d988726307a183218f0e7a5b852c..505d2b44b59d52ff9515adeea173b1684e3aaf74 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -6,6 +6,7 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 571838aa3104abbbc89f1834d50f7f8c2a73ca41..83a17cf03a6059ce9effddc40f5777a191a0a37f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -21,7 +21,6 @@
#pragma once
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
index 0bae7b6e9d12bf287307ec5f774bc95389ca216c..b0a9552b82a11d0f44e6d3d7c75c297169bf7bff 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
@@ -21,9 +21,7 @@
#pragma once
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 184fae2481188bbcc6e6f73a72c1dc47fa42d647..884d85255c22a19d84f72e2041b6134fa06573c9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -22,6 +22,8 @@
#pragma once
+#include <mutex>
+
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 532676cb549e96707873ea36b7646cdec2d724c6..2627328d9a0f7efb5270b7b3dbf9a803d6a66c5e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -7,8 +7,10 @@
#include <Eigen/Geometry>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
+#include <VirtualRobot/Nodes/Sensor.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index 1a6b6b7d69eb52cc7857744dfb774564ac984720..1ddd16d87b8cd65665d97f9ea7985c4145cf97d3 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -21,9 +21,9 @@
#pragma once
-#include <vector>
-#include <VirtualRobot/MathTools.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
diff --git a/source/RobotAPI/libraries/aron/common/rw/eigen.h b/source/RobotAPI/libraries/aron/common/rw/eigen.h
index fee5d0f9f2e3e2b3f645b1192024f0bb3e8cc18b..848c5e36c950184161fc6c674a5208cfb3825ee9 100644
--- a/source/RobotAPI/libraries/aron/common/rw/eigen.h
+++ b/source/RobotAPI/libraries/aron/common/rw/eigen.h
@@ -1,12 +1,16 @@
#pragma once
+#include <Eigen/Core>
+#include <Eigen/Geometry> // for Quaternion
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include <RobotAPI/libraries/aron/codegeneration_util/TypeName.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
-#include "../aron_conversions/eigen.h"
namespace armarx
{
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
index 1e9044cb3beea01195c87629e6464169ed392f43..2f4f97e8c8d1a7cea6a0b84f1408da63d23ee9d5 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
@@ -1,28 +1,11 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <numeric>
-#include <string>
-
-// Memory
-#include <RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h>
-#include <RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h>
-#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>
-#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
-#include <RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/aron/core/type/converter/nlohmannJSON/NlohmannJSONConverter.h>
-#include <RobotAPI/libraries/aron/core/type/converter/variant/VariantConverter.h>
-#include <RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h>
-#include <RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
-#include <RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>
-
-// JSON
#include <SimoxUtility/json/json.hpp>
+#include <RobotAPI/libraries/aron/core/Path.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
+
namespace armarx::aron::data::converter
{
class AronNlohmannJSONConverter
diff --git a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.cpp b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.cpp
index 957003a9eb4770c896b14439f171f1dfbaa81c75..5ba8c77e0633349727639816d2afa978342d570c 100644
--- a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.cpp
@@ -21,11 +21,11 @@
* GNU General Public License
*/
-// STD/STL
+#include "OpenCVConverter.h"
+
#include <numeric>
-// Header
-#include "OpenCVConverter.h"
+#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
namespace armarx::aron::data::converter
{
diff --git a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
index 9b3dcc79cb8a9c82c8e588b332a52d8e742e53f0..a43b018193a146e1bba88d1228d20f3eb584ce23 100644
--- a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
@@ -20,18 +20,14 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <string>
-
-// OpenCV
#include <opencv2/core/core.hpp>
-// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
namespace armarx::aron::data::converter
{
diff --git a/source/RobotAPI/libraries/aron/converter/pcl/PCLConverter.h b/source/RobotAPI/libraries/aron/converter/pcl/PCLConverter.h
index 77498aaf815ce7ae3f7736ac4831059bd2adcde2..eae562bf48482e583d9c175a09125a52f9c7225b 100644
--- a/source/RobotAPI/libraries/aron/converter/pcl/PCLConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/pcl/PCLConverter.h
@@ -20,16 +20,12 @@
#pragma once
-// STD/STL
-#include <memory>
#include <numeric>
#include <string>
-// Eigen
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
-// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/aron.h>
diff --git a/source/RobotAPI/libraries/aron/converter/stdstl/StdVectorConverter.h b/source/RobotAPI/libraries/aron/converter/stdstl/StdVectorConverter.h
index 2bc52d046af1186142195bcbc7223a848fa1ecd7..9ed4eb1c30c60ac867a8573b4efd6f1d968a879c 100644
--- a/source/RobotAPI/libraries/aron/converter/stdstl/StdVectorConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/stdstl/StdVectorConverter.h
@@ -21,12 +21,7 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <numeric>
-#include <string>
-// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/aron.h>
diff --git a/source/RobotAPI/libraries/aron/core/Descriptor.h b/source/RobotAPI/libraries/aron/core/Descriptor.h
index 5b386f58376492253e8d7387c0a384def4ffeac6..e2228b3b547c34a8881d8203d7f2db1f7eefb870 100644
--- a/source/RobotAPI/libraries/aron/core/Descriptor.h
+++ b/source/RobotAPI/libraries/aron/core/Descriptor.h
@@ -23,17 +23,10 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <typeindex>
#include <typeinfo>
#include <map>
#include <string>
-// Simox
-#include <SimoxUtility/algorithm/string.h>
-
-// ArmarX
#include <RobotAPI/interface/aron.h>
namespace armarx::aron::conversion::util
diff --git a/source/RobotAPI/libraries/aron/core/Exception.cpp b/source/RobotAPI/libraries/aron/core/Exception.cpp
index c27cbdfd59b8b16efa2f56570e4e98d3a80391a4..7b11ca07ce2bf2df347be03e5bd1a22c722b93ee 100644
--- a/source/RobotAPI/libraries/aron/core/Exception.cpp
+++ b/source/RobotAPI/libraries/aron/core/Exception.cpp
@@ -22,3 +22,107 @@
*/
#include "Exception.h"
+
+armarx::aron::error::AronException::AronException(const std::string& prettymethod,
+ const std::string& reason) :
+ LocalException(prettymethod + ": " + reason + ".")
+{
+}
+
+armarx::aron::error::AronException::AronException(const std::string& prettymethod,
+ const std::string& reason,
+ const Path& path) :
+ LocalException(prettymethod + ": " + reason + ". The path was: " + path.toString())
+{
+}
+
+armarx::aron::error::AronException&
+armarx::aron::error::AronException::operator()(const std::string& additionalMessage)
+{
+ auto currentReason = getReason();
+ if (not additionalMessage.empty())
+ {
+ setReason(currentReason + ". Additional Message: " + additionalMessage);
+ }
+ return *this;
+}
+
+armarx::aron::error::AronEOFException::AronEOFException(const std::string& prettymethod) :
+ AronException(prettymethod,
+ "REACHED THE END OF A NON VOID METHOD. PERHAPS YOU FORGOT TO ADD A VALUE "
+ "TO SOME SWITCH-CASE STATEMEMT?.")
+{
+}
+
+armarx::aron::error::NotImplementedYetException::NotImplementedYetException(
+ const std::string& prettymethod) :
+ AronException(prettymethod, "This method is not yet implemented!")
+{
+}
+
+armarx::aron::error::AronNotValidException::AronNotValidException(
+ const std::string& prettymethod,
+ const std::string& reason,
+ const data::dto::GenericDataPtr& data) :
+ AronException(prettymethod, reason + ". The ice_id of the data was: " + data->ice_id())
+{
+}
+
+armarx::aron::error::AronNotValidException::AronNotValidException(
+ const std::string& prettymethod,
+ const std::string& reason,
+ const data::dto::GenericDataPtr& data,
+ const Path& path) :
+ AronException(prettymethod, reason + ". The ice_id of the data was: " + data->ice_id(), path)
+{
+}
+
+armarx::aron::error::AronNotValidException::AronNotValidException(
+ const std::string& prettymethod,
+ const std::string& reason,
+ const type::dto::GenericTypePtr& type) :
+ AronException(prettymethod, reason + ". The ice_id of the type was: " + type->ice_id())
+{
+}
+
+armarx::aron::error::AronNotValidException::AronNotValidException(
+ const std::string& prettymethod,
+ const std::string& reason,
+ const type::dto::GenericTypePtr& type,
+ const Path& path) :
+ AronException(prettymethod, reason + ". The ice_id of the type was: " + type->ice_id(), path)
+{
+}
+
+armarx::aron::error::ValueNotValidException::ValueNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const std::string& input) :
+ AronException(prettymethod, reason + ". Got: " + input)
+{
+}
+
+armarx::aron::error::ValueNotValidException::ValueNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const std::string& input,
+ const Path& path) :
+ AronException(prettymethod, reason + ". Got: " + input, path)
+{
+}
+
+armarx::aron::error::ValueNotValidException::ValueNotValidException(
+ const std::string& prettymethod,
+ const std::string& reason,
+ const std::string& input,
+ const std::string& expectation) :
+ AronException(prettymethod, reason + ". Got: " + input + ". Expected: " + expectation)
+{
+}
+
+armarx::aron::error::ValueNotValidException::ValueNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const std::string& input,
+ const std::string& expectation,
+ const Path& path) :
+ AronException(prettymethod, reason + ". Got: " + input + ". Expected: " + expectation, path)
+{
+}
diff --git a/source/RobotAPI/libraries/aron/core/Exception.h b/source/RobotAPI/libraries/aron/core/Exception.h
index 49e7ad4444286b115f71f199c553359b97dcdfe4..ef0d2fa2e32567ad3d2fcc56768aa4bdf1c71098 100644
--- a/source/RobotAPI/libraries/aron/core/Exception.h
+++ b/source/RobotAPI/libraries/aron/core/Exception.h
@@ -23,15 +23,8 @@
#pragma once
-// STD/STL
-
-// ArmarX
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/aron/core/Path.h>
namespace armarx::aron::error
@@ -44,29 +37,12 @@ namespace armarx::aron::error
public:
AronException() = delete;
- AronException(const std::string& prettymethod, const std::string& reason) :
- LocalException(prettymethod + ": " + reason + ".")
- {
- }
+ AronException(const std::string& prettymethod, const std::string& reason);
- AronException(const std::string& prettymethod,
- const std::string& reason,
- const Path& path) :
- LocalException(prettymethod + ": " + reason + ". The path was: " + path.toString())
- {
- }
+ AronException(const std::string& prettymethod, const std::string& reason, const Path& path);
/// call operator to append a message to the exception. Used by ARMARX_CHECK_AND_THROW
- AronException&
- operator()(const std::string& additionalMessage = "")
- {
- auto currentReason = getReason();
- if (not additionalMessage.empty())
- {
- setReason(currentReason + ". Additional Message: " + additionalMessage);
- }
- return *this;
- }
+ AronException& operator()(const std::string& additionalMessage = "");
};
class AronEOFException : public AronException
@@ -74,12 +50,7 @@ namespace armarx::aron::error
public:
AronEOFException() = delete;
- AronEOFException(const std::string& prettymethod) :
- AronException(prettymethod,
- "REACHED THE END OF A NON VOID METHOD. PERHAPS YOU FORGOT TO ADD A VALUE "
- "TO SOME SWITCH-CASE STATEMEMT?.")
- {
- }
+ AronEOFException(const std::string& prettymethod);
};
/**
@@ -90,10 +61,7 @@ namespace armarx::aron::error
public:
NotImplementedYetException() = delete;
- NotImplementedYetException(const std::string& prettymethod) :
- AronException(prettymethod, "This method is not yet implemented!")
- {
- }
+ NotImplementedYetException(const std::string& prettymethod);
};
/**
@@ -106,37 +74,21 @@ namespace armarx::aron::error
AronNotValidException(const std::string& prettymethod,
const std::string& reason,
- const data::dto::GenericDataPtr& data) :
- AronException(prettymethod, reason + ". The ice_id of the data was: " + data->ice_id())
- {
- }
+ const data::dto::GenericDataPtr& data);
AronNotValidException(const std::string& prettymethod,
const std::string& reason,
const data::dto::GenericDataPtr& data,
- const Path& path) :
- AronException(prettymethod,
- reason + ". The ice_id of the data was: " + data->ice_id(),
- path)
- {
- }
+ const Path& path);
AronNotValidException(const std::string& prettymethod,
const std::string& reason,
- const type::dto::GenericTypePtr& type) :
- AronException(prettymethod, reason + ". The ice_id of the type was: " + type->ice_id())
- {
- }
+ const type::dto::GenericTypePtr& type);
AronNotValidException(const std::string& prettymethod,
const std::string& reason,
const type::dto::GenericTypePtr& type,
- const Path& path) :
- AronException(prettymethod,
- reason + ". The ice_id of the type was: " + type->ice_id(),
- path)
- {
- }
+ const Path& path);
};
/**
@@ -149,36 +101,22 @@ namespace armarx::aron::error
ValueNotValidException(const std::string& prettymethod,
const std::string& reason,
- const std::string& input) :
- AronException(prettymethod, reason + ". Got: " + input)
- {
- }
+ const std::string& input);
ValueNotValidException(const std::string& prettymethod,
const std::string& reason,
const std::string& input,
- const Path& path) :
- AronException(prettymethod, reason + ". Got: " + input, path)
- {
- }
+ const Path& path);
ValueNotValidException(const std::string& prettymethod,
const std::string& reason,
const std::string& input,
- const std::string& expectation) :
- AronException(prettymethod, reason + ". Got: " + input + ". Expected: " + expectation)
- {
- }
+ const std::string& expectation);
ValueNotValidException(const std::string& prettymethod,
const std::string& reason,
const std::string& input,
const std::string& expectation,
- const Path& path) :
- AronException(prettymethod,
- reason + ". Got: " + input + ". Expected: " + expectation,
- path)
- {
- }
+ const Path& path);
};
} // namespace armarx::aron::error
diff --git a/source/RobotAPI/libraries/aron/core/Path.cpp b/source/RobotAPI/libraries/aron/core/Path.cpp
index c80e00f53c989b8aadd4618757aaa3c6a588dc4a..ac91613ddb4dfc1dcae0f4fd09571783bd72d625 100644
--- a/source/RobotAPI/libraries/aron/core/Path.cpp
+++ b/source/RobotAPI/libraries/aron/core/Path.cpp
@@ -23,6 +23,7 @@
// Header
#include "Path.h"
+#include <SimoxUtility/algorithm/string/string_tools.h>
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
diff --git a/source/RobotAPI/libraries/aron/core/Path.h b/source/RobotAPI/libraries/aron/core/Path.h
index 888327fd929309cde7575133be27649b1a57bcd9..b39ef1c46cb7eca071f694f3506003d228633de9 100644
--- a/source/RobotAPI/libraries/aron/core/Path.h
+++ b/source/RobotAPI/libraries/aron/core/Path.h
@@ -23,7 +23,6 @@
#pragma once
-// STD/STL
#include <string>
#include <vector>
diff --git a/source/RobotAPI/libraries/aron/core/aron_conversions.h b/source/RobotAPI/libraries/aron/core/aron_conversions.h
index 30f2e949b575e22bf631abc06ca1c4ba8172efaf..cbd97326e69b9885b5b0fd31d9dde372dc266ecb 100644
--- a/source/RobotAPI/libraries/aron/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/core/aron_conversions.h
@@ -7,8 +7,6 @@
#include <vector>
#include <algorithm>
-#include "Path.h"
-
namespace armarx::aron
{
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
index 0195adddd9bb2f6cc116158af22b4a4a936e6da4..c5b063f3afed5dbd50cee9dddde6f408c018b3bd 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
@@ -21,12 +21,12 @@
#pragma once
// STD/STL
-#include <memory>
#include <string>
#include <optional>
// ArmarX
#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
// Known types
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
index 2e864ad7a5fc3189d79e769bb5edad4dfd4693de..e6289f74d2bc80cb1057cd329d59ac31bf9e6c95 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
@@ -18,26 +18,21 @@
* GNU General Public License
*/
-// STD/STL
+#include "NlohmannJSONReaderWithoutTypeCheck.h"
+
#include <cmath>
#include <cstdint>
#include <cstring>
-#include <memory>
-#include <numeric>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-// Header
-#include "NlohmannJSONReaderWithoutTypeCheck.h"
-
-// ArmarX
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h>
-#include "../../json/Data.h"
namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
index 0be0b34898308d217d221659d3524d77b2cf8f5c..c2ee596803dde93081f3ff235f2c099a54ced113 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
@@ -20,10 +20,6 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <stack>
-
// BaseClass
#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
index c03b3c37fec376834168d1795dbc6fc390874bea..5619e10ad9fba861b617006dadb2d6083d4e28c2 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
@@ -25,6 +25,7 @@
// Header
#include "Factory.h"
+#include <ArmarXCore/core/logging/Logging.h>
// ArmarX
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
index 38de3e29214ef427687533308c06cdd46aa075f4..8bf1da973e412737f8ede1e1aa52376b8b969acc 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -26,6 +26,7 @@
// Simox
#include <SimoxUtility/algorithm/string.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
// ArmarX
#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 19dd673bd2eeaec02f4be8fb49081d5edeb846c6..a348570cd0ded6b1003eb26bcd07453332ed4d22 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -27,6 +27,7 @@
// ArmarX
#include <SimoxUtility/algorithm/string/string_conversion.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
index 031d58ae16d06a2cdfefcfa180f36d753bb5030c..d2e3965595f94867f39f67cbca69f28a451bc123 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
@@ -25,13 +25,12 @@
// STD/STL
#include <memory>
-#include <string>
-#include <unordered_map>
// Base class
#include "../Variant.h"
// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::aron::data::detail
{
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index ed29f88c343ba218155b7510c28e496882349fe2..618be3850da1674379b30f214eab5a7a7484b198 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -22,6 +22,7 @@
*/
#include "Bool.h"
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx::aron::data
{
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
index 66b23d4e11892841f17fbb520a61b80e54a498e2..c118ea66dc523e5fdfe7cea68e95d38d0acca719 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
@@ -23,6 +23,7 @@
// Header
#include "Double.h"
+#include <ArmarXCore/util/CPPUtility/trace.h>
// ArmarX
#include "Float.h"
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
index 26d08b4b50d88fae2045b9b1bcbcb6ea3fb237de..6ebe3a5a42544573ba348ecee8c1768cff956c9b 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
@@ -22,6 +22,7 @@
*/
#include "Float.h"
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx::aron::data
{
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
index 870cbcd22877a82237e143bc79bf1528e64d35de..18bb4ed52fd1d1bf692c7d95edb7b2da6dc19163 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
@@ -22,6 +22,7 @@
*/
#include "Int.h"
+#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
#include <RobotAPI/libraries/aron/core/type/variant/primitive/Int.h>
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
index 2f62027e1ca65333d48447c4490932ecc032b543..4fc08e880b209a284bd4e4461a4633a6d6ee0fc2 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
@@ -23,6 +23,7 @@
// Header
#include "Long.h"
+#include <ArmarXCore/util/CPPUtility/trace.h>
// ArmarX
#include "Int.h"
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
index 9f765ed209f40a84e6554eebb26e1671d1a04e8b..675a15cbab1c6ac2b5aec75fb742f255c7a1dd26 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
@@ -22,6 +22,7 @@
*/
#include "String.h"
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx::aron::data
{
diff --git a/source/RobotAPI/libraries/aron/core/rw.h b/source/RobotAPI/libraries/aron/core/rw.h
index 7f0f69b5556b0e3c33b998a1e47457789865fef4..02673b3e8be2d01cb75618d8ca6bb66b0d10de38 100644
--- a/source/RobotAPI/libraries/aron/core/rw.h
+++ b/source/RobotAPI/libraries/aron/core/rw.h
@@ -1,14 +1,7 @@
#pragma once
-#include <map>
-#include <memory>
-#include <optional>
-#include <vector>
-
#include "aron_conversions.h"
#include "codegeneration/cpp/AronGeneratedClass.h"
-#include "data/rw/Reader.h"
-#include "data/rw/Writer.h"
namespace armarx::aron
{
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
index 9b9b5cf97723bd1b79f53775ebb7e485627c013f..bde013b6a3f6812ce4671bf161c014a011f1098f 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
@@ -21,12 +21,12 @@
#pragma once
// STD/STL
-#include <memory>
#include <optional>
#include <string>
// ArmarX
#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
index 27bcfcd0da522ce9defa148017ec57f6ea28e9bb..5fef12f117e2c02d61f8dbe68688dbf4094571de 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
@@ -27,6 +27,7 @@
// ArmarX
#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
index 21ba5cd0984b9077f668886327b57e8722b1f553..3c32ade9fdb74f97252250d21acca0f00558acb1 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
@@ -25,6 +25,7 @@
// Header
#include "Factory.h"
+#include <ArmarXCore/core/logging/Logging.h>
// ArmarX
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.h b/source/RobotAPI/libraries/aron/core/type/variant/Factory.h
index 520004578707bf18aea32a34b37ed4b0b5ef8775..678e3ac70734ecc8f3504419f4424ae989f1343f 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Factory.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.h
@@ -23,13 +23,11 @@
#pragma once
-// STD/STL
#include <memory>
-#include <unordered_map>
-// ArmarX
-#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
namespace armarx::aron::type
{
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
index f496b4f3fa380cc60b563b4c7beae47e754a8d15..dc90127ad6fe3664bbcf09ca918238c956c2bee1 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
@@ -40,6 +40,38 @@ namespace armarx::aron::type
{
return FACTORY->create(a, path);
}
-}
+ Variant::Variant(const type::Descriptor& descriptor, const Path& path) :
+ descriptor(descriptor), path(path)
+ {
+ }
+
+ bool
+ Variant::operator==(const VariantPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+
+ return *this == *other;
+ }
+
+ type::Descriptor
+ Variant::getDescriptor() const
+ {
+ return descriptor;
+ }
+ Path
+ Variant::getPath() const
+ {
+ return path;
+ }
+
+ std::string
+ Variant::pathToString() const
+ {
+ return path.toString();
+ }
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Variant.h b/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
index 6a1e2afa366ff9ae61bef7871d1ddaff9f1b070e..37c65e353c97f99554555a009c0f2f265855c741 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
@@ -23,16 +23,10 @@
#pragma once
-// STD/STL
-#include <map>
#include <memory>
#include <string>
#include <vector>
-// Simox
-// #include <SimoxUtility/algorithm/string.h>
-
-// ArmarX
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
@@ -63,49 +57,25 @@ namespace armarx::aron::type
public:
// constructors
- Variant(const type::Descriptor& descriptor, const Path& path = Path()) :
- descriptor(descriptor), path(path)
- {
- }
+ Variant(const type::Descriptor& descriptor, const Path& path = Path());
virtual ~Variant() = default;
// operators
virtual bool operator==(const Variant& other) const = 0;
- bool
- operator==(const VariantPtr& other) const
- {
- if (!other)
- {
- return false;
- }
-
- return *this == *other;
- }
+ bool operator==(const VariantPtr& other) const;
// static methods
/// create a variant object from an dto object
static VariantPtr FromAronDTO(const type::dto::GenericType&, const Path& = Path());
// public methods
- type::Descriptor
- getDescriptor() const
- {
- return descriptor;
- }
-
- Path
- getPath() const
- {
- return path;
- }
-
- std::string
- pathToString() const
- {
- return path.toString();
- }
+ type::Descriptor getDescriptor() const;
+
+ Path getPath() const;
+
+ std::string pathToString() const;
// virtual methods
/// naviate absolute
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
index 9ae860df56869350ba2c6e56127ce8d21d6a3b46..4b8e4dcb674164a0a74c58389580c31d5c782ef5 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
@@ -24,7 +24,6 @@
#pragma once
// STD/STL
-#include <map>
#include <string>
// Base Class
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
index ce88e8d2e9b7cd03127e431c9e2b81e305a04aa4..394cf99777eebb5399c4ae2d37b66731a3cb531b 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
@@ -24,7 +24,6 @@
#pragma once
// STD/STL
-#include <map>
#include <string>
// Base Class
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
index e382cc526eb18e3859c4531b4d7814a17dfd7461..b608dce7d2c7c3ba4d0c0eb4a663e0f6ce2fc7ad 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
@@ -24,7 +24,6 @@
#pragma once
// STD/STL
-#include <map>
#include <string>
// Base Class
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/AnyVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/AnyVariant.h
index 70c9c08968cc838935343e0b7c4ca3ce8711f2e5..eb9be13f27f8691afc7a2cc5168b63a770589e22 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/AnyVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/AnyVariant.h
@@ -23,10 +23,6 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
// Base class
#include "SpecializedVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
index 2673250f8fdda3fd234f2e7174433af45eda5bea..5e61cfac4e1e98943c0a548ff0ca8d684d1700cb 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
@@ -23,10 +23,6 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
// Base class
#include "SpecializedVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/DtoVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/DtoVariant.h
index 2fb240349254475021d75344a35d930c1aea09db..da38cbed4981b64c7ec68b89c42c92fef619693f 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/DtoVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/DtoVariant.h
@@ -23,11 +23,6 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
-
// Base class
#include "SpecializedVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h
index 84b1bf7821f6518498ca40554a26a16fcfaaf019..402565db02609097cfdb7dd1afc454b379a3e352 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h
@@ -24,9 +24,7 @@
#pragma once
// STD/STL
-#include <memory>
#include <string>
-#include <unordered_map>
// Base class
#include "SpecializedVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
index 168db8b1073aee4ea767940b27933109800bd025..0c707c8def23bddc3040e0a99cfb4ac6882ed053 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
@@ -24,9 +24,7 @@
#pragma once
// STD/STL
-#include <memory>
#include <string>
-#include <unordered_map>
// Base class
#include "SpecializedVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
index 24dd9171d771e9957600196cf9b9171d5940f455..f084df0a9202d7da4609853cbffe8f85936618ab 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
@@ -23,15 +23,10 @@
#pragma once
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
+#include <optional>
-// Base class
#include "SpecializedVariant.h"
-// ArmarX
namespace armarx::aron::type::detail
{
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
index a984daed2357434ceae4650c96ad8ff4a10a2356..fdc1bcf17ce912687112c9a9beab080df68d9a59 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
@@ -23,16 +23,12 @@
#pragma once
-// STD/STL
#include <memory>
-#include <optional>
-#include <string>
-#include <unordered_map>
-// Base class
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include "../Variant.h"
-// ArmarX
namespace armarx::aron::type::detail
{
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
index 941687c4861b504a6a57ad7350910af609b61034..e796bab87fd544d5bf660fd476ecd6b0c13250ab 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
@@ -24,7 +24,6 @@
#pragma once
// STD / STL
-#include <memory>
#include <string>
// Base Class
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
index b8df436d15f816c47820bed0e6b58bbe94a68dff..a217a433927d104048483117b59f439ac2f19072 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
@@ -24,7 +24,6 @@
#pragma once
// STD/STL
-#include <map>
#include <string>
// Base class
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h
index 9dfe2057d41256e44c506f9b7f61fc18fa68f7b1..e8368a7dcc988aa38a4ebd0931001c5a52b02dcd 100644
--- a/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h
@@ -23,10 +23,6 @@
#pragma once
-#include <map>
-#include <string>
-#include <vector>
-
#include "../../Descriptor.h"
#include "../../Exception.h"
diff --git a/source/RobotAPI/libraries/aron/filter/data/WhitelistFilter.h b/source/RobotAPI/libraries/aron/filter/data/WhitelistFilter.h
index aa43790e00befde0a269c932e6ebebafc0387d00..88a8c1f3bf1f7fa09bab560b4547838a59c0a13f 100644
--- a/source/RobotAPI/libraries/aron/filter/data/WhitelistFilter.h
+++ b/source/RobotAPI/libraries/aron/filter/data/WhitelistFilter.h
@@ -24,6 +24,7 @@
#include <algorithm>
#include <map>
#include <vector>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
diff --git a/source/RobotAPI/libraries/aron/similarity/cosine.cpp b/source/RobotAPI/libraries/aron/similarity/cosine.cpp
index 396ea66133cf66e6dc1f268c8f3c5bc89c47de05..ac9e22cf073bc56969ae6c37b58fff01f1ec9ef0 100644
--- a/source/RobotAPI/libraries/aron/similarity/cosine.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/cosine.cpp
@@ -1,6 +1,7 @@
#include "cosine.h"
#include <SimoxUtility/algorithm/string.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <cmath>
namespace armarx::aron::similarity
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
index 5fb38a7f8859e804e4ea8a71642402a08ce01e2b..467c9f1da493c54fe0de113cfa1127a6d082f37d 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
@@ -1,6 +1,7 @@
#include "NDArraySimilarity.h"
#include <cmath>
+#include <ArmarXCore/core/logging/Logging.h>
#include "mse.h"
#include "mae.h"
#include "chernoff.h"
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
index 0b4d26bc8dd4adb15a612fcc627eed41ed392cc1..f91feb06aa18d66b7eb31800327d4dedd939938e 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
@@ -3,6 +3,7 @@
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
+#include <ArmarXCore/core/logging/Logging.h>
namespace armarx::aron::similarity::chernoff
{
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
index cb9dcf9d3f7ef6ca4ee31e514e3aa7c122605dee..052cccff38bd7e9dd004a4d75efa578e01836604 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
@@ -1,6 +1,7 @@
#include "mse.h"
#include <SimoxUtility/algorithm/string.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <cmath>
namespace armarx::aron::similarity
diff --git a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
index 9ed6259492303586191d67d5fdcc3aebee525bf8..7541f1ce6ca0a80ccad5482dae7c596cee09b719 100644
--- a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
@@ -22,6 +22,9 @@
#include "PropertyDefinitionVisitors.h"
+#include <SimoxUtility/algorithm/string/string_conversion_eigen.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <ArmarXCore/core/application/properties/PropertyDefinition.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/PropertyUser.h>
@@ -467,4 +470,4 @@ namespace armarx::aron::component_config
}
} // namespace armarx::aron::component_config
-#undef INPUT_GUARD
\ No newline at end of file
+#undef INPUT_GUARD
diff --git a/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.cpp b/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.cpp
index 10e021861bbc16a6464ce29c95946dbbd3170888..797f755ead243754ad14de4c11d04d6af6d71747 100644
--- a/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.cpp
+++ b/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.cpp
@@ -22,8 +22,11 @@
#include "VariantHelperFactory.h"
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+
#include <ArmarXCore/core/application/properties/Property.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/interface/core/UserException.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
@@ -222,4 +225,4 @@ namespace armarx::aron::component_config::products
template struct products::VariantHelper<type::Descriptor::FLOAT>;
template struct products::VariantHelper<type::Descriptor::DOUBLE>;
template struct products::VariantHelper<type::Descriptor::INT_ENUM>;
-} // namespace armarx::aron::component_config::products
\ No newline at end of file
+} // namespace armarx::aron::component_config::products
diff --git a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
index 18f34713915af784e3883b0a78d1112a14eda644..1804cb7326160912cdf214aebb8f887c79f4d4c2 100644
--- a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
@@ -24,6 +24,12 @@
#include "CartesianFeedForwardPositionController.h"
#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+
namespace armarx
{
CartesianFeedForwardPositionController::CartesianFeedForwardPositionController(const VirtualRobot::RobotNodePtr& tcp)
diff --git a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
index 4c2704aca4a0bb8a0edb2779a32dd3d7b135692c..c40a394645c3b8c188315720b9e672e4b4e5ea00 100644
--- a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
@@ -23,11 +23,11 @@
#pragma once
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/math/MathForwardDefinitions.h>
+#include <VirtualRobot/IK/IKSolver.h>
+
#include <Eigen/Dense>
-#include <VirtualRobot/math/AbstractFunctionR1R6.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index b1f6850fbf3cd8067a0903c8a6973561f2418c9d..5c45b8873d933c7d53728603c703af3e4ed0141a 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -29,8 +29,10 @@
#include "CartesianNaturalPositionController.h"
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index c34d94957311c775b378bcda7e0dda17f38fe1c3..a2d75227f2cf8739a62bdd34a7301a2baca5c6e8 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -23,6 +23,12 @@
#include "CartesianPositionController.h"
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index d4f3a688376c1ee470572e6ed1963be41aa2d9c0..df4272087876ef431d2c49ed7800bc1a57d3992d 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -23,9 +23,9 @@
#pragma once
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/IK/IKSolver.h>
+
#include <Eigen/Dense>
#include <RobotAPI/interface/core/CartesianPositionControllerConfig.h>
@@ -128,4 +128,3 @@ namespace armarx
typedef IceInternal::Handle<CartesianPositionControllerConfig> CartesianPositionControllerConfigPtr;
}
-
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 93d3df8e907c5086e00a470d9f4e9bdfe3dfb37f..636104a3993a63b694445d19c1492303280e24bf 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -28,8 +28,11 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <Eigen/Core>
@@ -306,5 +309,3 @@ void CartesianVelocityController::setJointCosts(const std::vector<float>& jointC
_jointCosts(i) = jointCosts.at(i);
}
}
-
-
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 3e37d44f53308858b756f84423cf06cd65995404..0454957fb9cbf3c765c9a794e8d401321251730c 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -23,8 +23,8 @@
#pragma once
-#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/IK/JacobiProvider.h>
#include <Eigen/Core>
@@ -75,4 +75,3 @@ namespace armarx
Eigen::VectorXf _jointCosts;
};
}
-
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index ee06ab837f18e01764757e98e9aa372fc63e785f..6c9d30d58dc25d3a6a58021ad7f763776041cd04 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -23,6 +23,10 @@
#include "CartesianVelocityControllerWithRamp.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+
#include <ArmarXCore/core/logging/Logging.h>
namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
index 1a73b84900971b6cafe810bedd219998362a29a4..3b6b93dc9ba6336acb6e1df39d9c14c6abf59f35 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -28,9 +28,8 @@
#include "JointVelocityRamp.h"
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/IKSolver.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
#include <Eigen/Dense>
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
index e904576b730b5eb9e4d2927cc2819685af7e0b4c..eb5646dac877f1f3632c94b9f4bf64c1d441a765 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
@@ -25,6 +25,9 @@
#include <ArmarXCore/core/logging/Logging.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+
namespace armarx
{
CartesianVelocityRamp::CartesianVelocityRamp()
@@ -88,4 +91,3 @@ namespace armarx
return maxOrientationAcceleration;
}
}
-
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
index b4d5aac333e20e488831c44567f093cafa8dad95..573dfe2b293450fb5dd98be649a982aef81d3842 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
@@ -23,8 +23,9 @@
#pragma once
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/IK/IKSolver.h>
+
#include <Eigen/Dense>
namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index 917877b7a5ce1556ece46a9d8353b274dba87465..34c1902c05f8cbc63435dcb1cd35a1735b4e2c39 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -31,6 +31,7 @@
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Robot.h>
#include <cfloat>
#include <ArmarXCore/util/CPPUtility/trace.h>
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.h b/source/RobotAPI/libraries/core/CartesianWaypointController.h
index b87ae11f3a1bd3341261eb95d638dafd511add6b..d6fb2f7bc2038ac64dfc260346662b1441baaddc 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.h
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.h
@@ -27,7 +27,7 @@
#include <Eigen/Dense>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/Pose.h>
diff --git a/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp b/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
index 67391514fcba1a51f62ab4488aa1d5f4d54a4a1e..9c42823f12c1bb88cc4581d67a5f8d88d50a385c 100644
--- a/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
+++ b/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
@@ -22,7 +22,6 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/LinkedCoordinate.h>
-#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
diff --git a/source/RobotAPI/libraries/core/FramedOrientedPoint.h b/source/RobotAPI/libraries/core/FramedOrientedPoint.h
index 3504cad3a5450c0a8d2e19722f66676995319ac4..22aff2d36db3a88dc92b0237534eb1a867439ebf 100644
--- a/source/RobotAPI/libraries/core/FramedOrientedPoint.h
+++ b/source/RobotAPI/libraries/core/FramedOrientedPoint.h
@@ -23,9 +23,6 @@
#include "OrientedPoint.h"
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/LinkedCoordinate.h>
-#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/core/RobotState.h>
@@ -105,4 +102,3 @@ namespace armarx
};
}
-
diff --git a/source/RobotAPI/libraries/core/RobotPool.cpp b/source/RobotAPI/libraries/core/RobotPool.cpp
index 0b6d58afe038069766bcd96751fcccadfd7056ec..731646d134359076c163f2aa002f2e80946079d0 100644
--- a/source/RobotAPI/libraries/core/RobotPool.cpp
+++ b/source/RobotAPI/libraries/core/RobotPool.cpp
@@ -24,6 +24,7 @@
#include "RobotPool.h"
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/libraries/core/RobotPool.h b/source/RobotAPI/libraries/core/RobotPool.h
index 4d6db6fc1ed2ba14a1f3c9bffe2b7557deaf0508..c65afe3036082809c3f0d36674f40f674d69cc52 100644
--- a/source/RobotAPI/libraries/core/RobotPool.h
+++ b/source/RobotAPI/libraries/core/RobotPool.h
@@ -23,9 +23,10 @@
*/
#pragma once
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <mutex>
+#include <map>
namespace armarx
{
@@ -60,5 +61,3 @@ namespace armarx
};
using RobotPoolPtr = std::shared_ptr<RobotPool>;
}
-
-
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 4f0f563e7960efbb4cc8055f2c198803105b841e..4383ae2e53fafb87f6713bd0a341a380e073cf54 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -31,7 +31,14 @@
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
-
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
+#include <VirtualRobot/Nodes/RobotNodeFixed.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 734db7407354682df6fa45c866acdb0e9c8c0a6f..c976c53411a43af5837a52fb8d58926401db4bee 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -23,15 +23,15 @@
*/
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <RobotAPI/interface/core/RobotState.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <VirtualRobot/Nodes/RobotNodeFixed.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
-#include <VirtualRobot/Nodes/RobotNodeFixed.h>
-#include <VirtualRobot/XML/RobotIO.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
#include <mutex>
@@ -262,4 +262,3 @@ namespace armarx
using RemoteRobotPtr = std::shared_ptr<RemoteRobot>;
}
-
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index 28bb76573e925f3f6135ba0f3d75be2aa060d9f4..6e402e30031d962e4e755e50cd2bd63b0131fe8d 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -29,6 +29,7 @@
#include <ArmarXCore/interface/core/BasicTypes.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index 683ad38f8e45e9d49ffdf0b425bb3569eef4d499..9c2f0fc609d47dfae5993d61d4d887be05e82b5b 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
@@ -23,9 +23,15 @@
#define BOOST_TEST_MODULE RobotAPI::CartesianVelocityController::Test
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
+
#include <ArmarXCore/core/test/IceTestHelper.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/IK/IKSolver.h>
+
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include "../CartesianVelocityController.h"
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
index 5ffd4e0aed928a3e9292cabbb25087cf91758b6f..a4dcccdcec0d71c592dd0198f91e0e7beb2ebac7 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
@@ -20,6 +20,7 @@
* GNU General Public License
*/
+#include <VirtualRobot/IK/DifferentialIK.h>
#define BOOST_TEST_MODULE RobotAPI::CartesianVelocityController::Test
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
index d8d4f0f6d09f30d964146f9428d26efb6de81d52..833e6a399e5e3ca64ad8e7441eb6aa78f5c0a99e 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
@@ -24,9 +24,13 @@
#include "CompositeDiffIK.h"
#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/exceptions/Exception.h>
+
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
#include <cfloat>
using namespace armarx;
@@ -458,3 +462,21 @@ Eigen::VectorXf CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient(Param
}
return r.cwiseProduct(weight);
}
+
+Eigen::Matrix4f
+armarx::CompositeDiffIK::Target::getPoseError() const
+{
+ return tcp->getPoseInRootFrame().inverse() * target;
+}
+
+Eigen::Vector3f
+armarx::CompositeDiffIK::Target::getPosError() const
+{
+ return (tcp->getPoseInRootFrame().inverse() * target).topRightCorner<3, 1>();
+}
+
+auto
+armarx::CompositeDiffIK::Target::getOriError() const
+{
+ return simox::math::delta_angle(tcp->getPoseInRootFrame(), target);
+}
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
index 2736bfff337a33de477c224e34e63a81daf4b048..2963293f63fa644a2769d57d6b3262b77320dc99 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
@@ -22,11 +22,11 @@
*/
#pragma once
+
#include <SimoxUtility/math/distance/delta_angle.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
@@ -150,18 +150,9 @@ namespace armarx
std::vector<TargetStep> ikSteps;
- Eigen::Matrix4f getPoseError() const
- {
- return tcp->getPoseInRootFrame().inverse() * target;
- }
- Eigen::Vector3f getPosError() const
- {
- return (tcp->getPoseInRootFrame().inverse() * target).topRightCorner<3, 1>();
- }
- auto getOriError() const
- {
- return simox::math::delta_angle(tcp->getPoseInRootFrame(), target);
- }
+ Eigen::Matrix4f getPoseError() const;
+ Eigen::Vector3f getPosError() const;
+ auto getOriError() const;
};
typedef std::shared_ptr<Target> TargetPtr;
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
index 3e6cdff5c28a7a27494e222ce78517abe3d8a5f5..ae7aace621b852c4d077aebd52afb0289da09d6e 100644
--- a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
@@ -25,6 +25,9 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+#include <VirtualRobot/math/Helpers.h>
+
using namespace armarx;
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.h b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
index 8cc0a7cad1046733e4e7d52a4fe2d69eff28b660..777ed4066e29dd153284021272039e8cb37d61cc 100644
--- a/source/RobotAPI/libraries/diffik/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
@@ -38,9 +38,6 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/interface/serialization/Eigen.h>
-#include <VirtualRobot/math/AbstractFunctionR1R6.h>
-#include <VirtualRobot/math/Helpers.h>
-
#include <Eigen/Core>
#include <vector>
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
index e7497443b1aded8914af5cc0400f4bf00ee89ffe..802f1e01c139423de6a09eec82eb986e77951247 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
@@ -24,6 +24,11 @@
#include "NaturalDiffIK.h"
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
index 0bdafb506e4cf6a22203b9b88cde2eccacba32d6..dbbd1e1172e271887392f8cfd6433b0578b2d0d6 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
@@ -23,10 +23,8 @@
#pragma once
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/IK/IKSolver.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
index c3f1a32dbaa1001d06762522080ef5367c072b70..ef6a4d91b9d916ea46725f5db6e98b58bd0ef5b9 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
@@ -23,6 +23,9 @@
#include "SimpleDiffIK.h"
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
@@ -171,4 +174,21 @@ namespace armarx
return r;
}
-}
+ void
+ SimpleDiffIK::Reachability::aggregate(const Result& result)
+ {
+ ikResults.emplace_back(result);
+ reachable = reachable && result.reached;
+ minimumJointLimitMargin = std::min(minimumJointLimitMargin, result.minimumJointLimitMargin);
+ if (jointLimitMargins.rows() == 0)
+ {
+ jointLimitMargins = result.jointLimitMargins;
+ }
+ else
+ {
+ jointLimitMargins = jointLimitMargins.cwiseMin(result.jointLimitMargins);
+ }
+ maxPosError = std::max(maxPosError, result.posError);
+ maxOriError = std::max(maxOriError, result.oriError);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index d71ebbee9157cd27e643860fa68287d362d2524a..583fef2a8fec427dbde267d28fb5863ba9da7942 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -25,8 +25,7 @@
#include "DiffIKProvider.h"
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <memory>
@@ -88,22 +87,7 @@ namespace armarx
float maxOriError = 0;
std::vector<Result> ikResults;
- void aggregate(const Result& result)
- {
- ikResults.emplace_back(result);
- reachable = reachable && result.reached;
- minimumJointLimitMargin = std::min(minimumJointLimitMargin, result.minimumJointLimitMargin);
- if (jointLimitMargins.rows() == 0)
- {
- jointLimitMargins = result.jointLimitMargins;
- }
- else
- {
- jointLimitMargins = jointLimitMargins.cwiseMin(result.jointLimitMargins);
- }
- maxPosError = std::max(maxPosError, result.posError);
- maxOriError = std::max(maxOriError, result.oriError);
- }
+ void aggregate(const Result& result);
};
static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index 7a57d550daa7a14141bd3ddfc3e93193df11b394..bb532f39331aeb2ec1c599a918f88dad373b5314 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -27,6 +27,8 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
//#include <RobotAPI/libraries/aron/core/navigator/Navigator.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
@@ -530,5 +532,3 @@ CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::~ScopedJointVa
//ARMARX_IMPORTANT << "restoring joint values";
rns->setJointValues(jointValues);
}
-
-
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
index 76bd6e75e6301cb29a80b608d8fd6fb2b7303b46..6a561959d34228fe6b64a436e74e713891ffcb47 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.cpp
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -23,11 +23,15 @@
#include "NaturalIK.h"
#include <ArmarXCore/core/exceptions/Exception.h>
+
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/convert/rad_to_deg.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
using namespace armarx;
NaturalIK::NaturalIK(std::string side, Eigen::Vector3f shoulderPos, float scale)
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
index 7e1ecb940cc7adb84c0091e4e1f3427923559c74..e71511d9bd4ffef7f64b6fc4c9a86d22d49eb07d 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -26,7 +26,9 @@
#include <memory>
//#include <RobotAPI/libraries/core/SimpleDiffIK.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+
#include <RobotAPI/libraries/diffik/DiffIKProvider.h>
#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
#include <optional>
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
index 0f1f8047f3f47976a7ce52a928bac858d665b053..b8165c4ba086e82aec64611efac92f7da4fea376 100644
--- a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -22,6 +22,7 @@
#include "CollisionModelHelper.h"
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
@@ -62,7 +63,7 @@ namespace armarx::obstacle_avoidance
if (auto obstacle = finder.loadManipulationObject(objectPose))
{
obstacle->setGlobalPose(objectPose.objectPoseGlobal);
- obstacle->setName(objectPose.objectID.instanceName());
+ obstacle->setName(objectPose.objectID.str());
sceneObjects->addSceneObject(obstacle);
}
}
@@ -104,7 +105,6 @@ namespace armarx::obstacle_avoidance
Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
- // ARMARX_INFO << world_T_obj.translation();
auto cube = factory.createBox(boxSize, boxSize, boxSize);
@@ -177,4 +177,4 @@ namespace armarx::obstacle_avoidance
arviz.commitLayerContaining("CollisionModel", mesh);
}
-} // namespace armarx::obstacle_avoidance
\ No newline at end of file
+} // namespace armarx::obstacle_avoidance
diff --git a/source/RobotAPI/libraries/skills/core/CMakeLists.txt b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
index f757e4b5d8d069673c67d39e8b559250202d6c03..7e5a56debc7b98c758991bf6e3d389672f92b483 100644
--- a/source/RobotAPI/libraries/skills/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
@@ -23,8 +23,6 @@ armarx_add_library(
SkillExecutionRequest.cpp
SkillStatusUpdate.cpp
SkillExecutionID.cpp
- SkillPreparationInput.cpp
- SkillParameterization.cpp
Skill.cpp
SkillProxy.cpp
SkillDescription.cpp
@@ -36,8 +34,6 @@ armarx_add_library(
SkillExecutionRequest.h
SkillStatusUpdate.h
SkillExecutionID.h
- SkillPreparationInput.h
- SkillParameterization.h
Skill.h
SkillProxy.h
SkillDescription.h
diff --git a/source/RobotAPI/libraries/skills/core/ProviderID.h b/source/RobotAPI/libraries/skills/core/ProviderID.h
index 5687f649185fa472d0afe1b7a60e6eb2eba46d34..d7710d96e3a6a65de86c0b1c4634f0c84220c8a0 100644
--- a/source/RobotAPI/libraries/skills/core/ProviderID.h
+++ b/source/RobotAPI/libraries/skills/core/ProviderID.h
@@ -1,10 +1,7 @@
#pragma once
-#include <string>
-#include <vector>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/skills/core/ProviderInfo.h b/source/RobotAPI/libraries/skills/core/ProviderInfo.h
index 438a2cf9d894f25f97150dc10ab6c5d9538cdc1e..b4a9c0470f017b1b21d43f70674ca1ab64afc556 100644
--- a/source/RobotAPI/libraries/skills/core/ProviderInfo.h
+++ b/source/RobotAPI/libraries/skills/core/ProviderInfo.h
@@ -1,16 +1,8 @@
#pragma once
-#include <string>
-#include <vector>
-#include <SimoxUtility/algorithm/string.h>
-
-#include <RobotAPI/interface/skills/SkillManagerInterface.h>
-
-#include "ProviderID.h"
-#include "SkillDescription.h"
-#include "SkillID.h"
-#include "error/Exception.h"
+#include <RobotAPI/libraries/skills/core/ProviderID.h>
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
index 62fee239e4bf83fdfe6cb3d9a09428484280d4aa..3a279a8b8e28bb2d7e01eb5ac5b17958c5b5ed51 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -1,4 +1,7 @@
#include "Skill.h"
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
namespace armarx
{
@@ -448,5 +451,46 @@ namespace armarx
<< "'. Please overwrite this method.";
return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
}
+
+ void
+ Skill::setProviderId(const skills::ProviderID& pid)
+ {
+ description.skillId.providerId = pid;
+ }
+
+ void
+ Skill::setCallback(const CallbackT& callback)
+ {
+ this->callback = callback;
+ }
+
+ void
+ Skill::setManager(const manager::dti::SkillManagerInterfacePrx& manager)
+ {
+ this->manager = manager;
+ }
+
+ void
+ Skill::setExecutorName(const std::string& executorName)
+ {
+ this->executorName = executorName;
+ }
+
+ SkillDescription
+ Skill::getSkillDescription() const
+ {
+ return description;
+ }
+
+ SkillID
+ Skill::getSkillId() const
+ {
+ return description.skillId;
+ }
+
+ Skill::~Skill()
+ {
+ //ARMARX_IMPORTANT << "DESTROY SKILL " << getSkillId();
+ }
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/Skill.h b/source/RobotAPI/libraries/skills/core/Skill.h
index 9fffd35a6f258e778200351ef387ba9d3b184480..9be47862810009ec00fb8e9b8138b9ac1e0d5df0 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.h
+++ b/source/RobotAPI/libraries/skills/core/Skill.h
@@ -2,22 +2,14 @@
#include <functional>
#include <mutex>
-#include <queue>
#include <thread>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/time/DateTime.h>
-#include <ArmarXCore/core/time/Metronome.h>
+#include <ArmarXCore/core/time/Frequency.h>
-#include <RobotAPI/interface/skills/SkillManagerInterface.h>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillProxy.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
-#include "SkillDescription.h"
-#include "SkillID.h"
-#include "SkillPreparationInput.h"
-#include "SkillProxy.h"
-#include "SkillStatusUpdate.h"
-#include "error/Exception.h"
namespace armarx
{
@@ -64,50 +56,23 @@ namespace armarx
Skill(const SkillDescription&);
/// Virtual destructor of a skill
- virtual ~Skill()
- {
- //ARMARX_IMPORTANT << "DESTROY SKILL " << getSkillId();
- }
+ virtual ~Skill();
/// Get the id of the skill
- SkillID
- getSkillId() const
- {
- return description.skillId;
- }
+ SkillID getSkillId() const;
/// Get the description of a skill
- SkillDescription
- getSkillDescription() const
- {
- return description;
- }
+ SkillDescription getSkillDescription() const;
/// Set the provider id of the description of the skill.
/// This method is called when creating a skill in a skill provider
- void
- setProviderId(const skills::ProviderID& pid)
- {
- description.skillId.providerId = pid;
- }
+ void setProviderId(const skills::ProviderID& pid);
- void
- setCallback(const CallbackT& callback)
- {
- this->callback = callback;
- }
+ void setCallback(const CallbackT& callback);
- void
- setManager(const manager::dti::SkillManagerInterfacePrx& manager)
- {
- this->manager = manager;
- }
+ void setManager(const manager::dti::SkillManagerInterfacePrx& manager);
- void
- setExecutorName(const std::string& executorName)
- {
- this->executorName = executorName;
- }
+ void setExecutorName(const std::string& executorName);
/// Prepare a skill once. This method is called in a loop as long as it returns RUNNING
/// If the loop does not terminate with SUCCEDED the skill execution is failed.
diff --git a/source/RobotAPI/libraries/skills/core/SkillDescription.cpp b/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
index 2fa37b33ceab97b944e55644c99c530ba3f371c3..c7d5222e45de9215208c4d5547c50c4e367d3768 100644
--- a/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
@@ -65,5 +65,14 @@ namespace armarx
.parametersType = armarx::aron::type::Object::FromAronObjectDTO(i.parametersType),
.resultType = armarx::aron::type::Object::FromAronObjectDTO(i.resultType)};
}
+
+ bool
+ SkillDescription::operator==(const SkillDescription& other) const
+ {
+ return this->skillId == other.skillId && this->description == other.description &&
+ this->rootProfileDefaults == other.rootProfileDefaults &&
+ this->timeout == other.timeout && this->parametersType == other.parametersType &&
+ this->resultType == other.resultType;
+ }
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillDescription.h b/source/RobotAPI/libraries/skills/core/SkillDescription.h
index 784afd9321f4d677109c1f43672f0bdff4846db8..a6f33333fb733c3ccafd6320bd03db5e89153101 100644
--- a/source/RobotAPI/libraries/skills/core/SkillDescription.h
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.h
@@ -1,7 +1,6 @@
#pragma once
#include <string>
-#include <vector>
#include <ArmarXCore/core/time/Duration.h>
@@ -31,15 +30,7 @@ namespace armarx
const std::optional<ProviderID>& = std::nullopt);
static SkillDescription FromIce(const manager::dto::SkillDescription& i);
- bool
- operator==(const SkillDescription& other) const
- {
- return this->skillId == other.skillId && this->description == other.description &&
- this->rootProfileDefaults == other.rootProfileDefaults &&
- this->timeout == other.timeout &&
- this->parametersType == other.parametersType &&
- this->resultType == other.resultType;
- }
+ bool operator==(const SkillDescription& other) const;
};
template <class T>
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
index 2bf5f3235ba19f60f4475e9fe3d126fbcfec1db9..b93ef522c7d604e2fd43870da541d4d59d74e122 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
@@ -1,5 +1,7 @@
#include "SkillExecutionID.h"
+#include <ArmarXCore/core/time/ice_conversions.h>
+
namespace armarx
{
namespace skills
@@ -51,5 +53,22 @@ namespace armarx
executionStartedTime.toDateTimeString() + EXIT_SEPARATOR;
}
+ bool
+ SkillExecutionID::operator<=(const SkillExecutionID& other) const
+ {
+ return this->executionStartedTime <= other.executionStartedTime;
+ }
+
+ bool
+ SkillExecutionID::operator<(const SkillExecutionID& other) const
+ {
+ return this->executionStartedTime < other.executionStartedTime;
+ }
+
+ bool
+ SkillExecutionID::operator==(const SkillExecutionID& other) const
+ {
+ return this->toString() == other.toString();
+ }
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.h b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
index 39f3bc008324d046486d4eb1cd486ef87a1860f8..7a7d5e9e1efccf43f7dfcb3f4dce4d4655c0f395 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
@@ -1,16 +1,12 @@
#pragma once
#include <string>
-#include <vector>
#include <ArmarXCore/core/time/DateTime.h>
-#include <ArmarXCore/core/time/ice_conversions.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-#include "SkillID.h"
-#include "SkillParameterization.h"
+#include <RobotAPI/libraries/skills/core/SkillID.h>
namespace armarx
{
@@ -27,23 +23,11 @@ namespace armarx
static const constexpr char* EXIT_SEPARATOR = "]";
static const constexpr char* SEPARATOR = "@";
- bool
- operator==(const SkillExecutionID& other) const
- {
- return this->toString() == other.toString();
- }
-
- bool
- operator<(const SkillExecutionID& other) const
- {
- return this->executionStartedTime < other.executionStartedTime;
- }
-
- bool
- operator<=(const SkillExecutionID& other) const
- {
- return this->executionStartedTime <= other.executionStartedTime;
- }
+ bool operator==(const SkillExecutionID& other) const;
+
+ bool operator<(const SkillExecutionID& other) const;
+
+ bool operator<=(const SkillExecutionID& other) const;
skills::manager::dto::SkillExecutionID toManagerIce() const;
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
index d1f5bbcdd175f9f3516d45856186d453af2f987e..453291831f7e8e5c53d45855c51c81e04e011c70 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
@@ -1,18 +1,10 @@
#pragma once
-#include <string>
-#include <vector>
-
-#include <SimoxUtility/algorithm/string.h>
-
-#include <ArmarXCore/core/time/DateTime.h>
-#include <ArmarXCore/core/time/ice_conversions.h>
-
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-#include "SkillID.h"
-#include "error/Exception.h"
+#include <RobotAPI/libraries/skills/core/ProviderID.h>
+#include <RobotAPI/libraries/skills/core/SkillID.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.cpp b/source/RobotAPI/libraries/skills/core/SkillID.cpp
index 2bf5cb5596a89a8daab37ef8e957a8c9fcc2b532..87fa6cd75b4faf144e593f7d5702b893aa9dab4c 100644
--- a/source/RobotAPI/libraries/skills/core/SkillID.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillID.cpp
@@ -1,5 +1,9 @@
#include "SkillID.h"
+#include <optional>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
namespace armarx
{
namespace skills
@@ -69,6 +73,28 @@ namespace armarx
}
return NAME_SEPARATOR + skillName;
}
+
+ bool
+ SkillID::isFullySpecified() const
+ {
+ return isSkillSpecified() and isProviderSpecified();
+ }
+
+ bool
+ SkillID::isSkillSpecified() const
+ {
+ return not skillName.empty();
+ }
+
+ bool
+ SkillID::isProviderSpecified() const
+ {
+ if (not providerId.has_value())
+ {
+ return false;
+ }
+ return not providerId->providerName.empty();
+ }
} // namespace skills
std::ostream&
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.h b/source/RobotAPI/libraries/skills/core/SkillID.h
index 91494c78c331272e85babe5052ccd2369e29833e..1de4e5fc764d10bb5af2324f02ce4f71b81b60ef 100644
--- a/source/RobotAPI/libraries/skills/core/SkillID.h
+++ b/source/RobotAPI/libraries/skills/core/SkillID.h
@@ -1,14 +1,11 @@
#pragma once
#include <string>
-#include <vector>
-
-#include <SimoxUtility/algorithm/string.h>
+#include <optional>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include "ProviderID.h"
-#include "error/Exception.h"
namespace armarx
{
@@ -25,27 +22,11 @@ namespace armarx
bool operator<(const SkillID& other) const;
bool operator<=(const SkillID& other) const;
- bool
- isFullySpecified() const
- {
- return isSkillSpecified() and isProviderSpecified();
- }
+ bool isFullySpecified() const;
- bool
- isSkillSpecified() const
- {
- return not skillName.empty();
- }
+ bool isSkillSpecified() const;
- bool
- isProviderSpecified() const
- {
- if (not providerId.has_value())
- {
- return false;
- }
- return not providerId->providerName.empty();
- }
+ bool isProviderSpecified() const;
manager::dto::SkillID toManagerIce() const;
provider::dto::SkillID toProviderIce() const;
diff --git a/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp
deleted file mode 100644
index d0ec681e2b772aa773a6cc890198e24c28714207..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-#include "SkillParameterization.h"
-
-namespace armarx
-{
- namespace skills
- {
- }
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillParameterization.h b/source/RobotAPI/libraries/skills/core/SkillParameterization.h
deleted file mode 100644
index 9b54b3fff3bfe0aaedcb79d9c275165aefe5a37d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/core/SkillParameterization.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-namespace armarx
-{
- namespace skills
- {
- } // namespace skills
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp
deleted file mode 100644
index d0ec681e2b772aa773a6cc890198e24c28714207..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-#include "SkillParameterization.h"
-
-namespace armarx
-{
- namespace skills
- {
- }
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h
deleted file mode 100644
index b5cc2bb20c392864ccb31607e1ef02e9174fb211..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-namespace armarx
-{
- namespace skills
- {
-
- } // namespace skills
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.cpp b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
index ea4fc4d1a0192d5ad4819be4248df3ea52566164..fb46cdc96d1e6581afd938a6e5cbd981e3ea9016 100644
--- a/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
@@ -3,6 +3,8 @@
#include <chrono>
#include <thread>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
+
namespace armarx
{
namespace skills
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.h b/source/RobotAPI/libraries/skills/core/SkillProxy.h
index e20ca01a0bc4b6de877e34a3ad6bc0edcbaa4689..d91dc2ae633cfdbf23c4d7938e455d442892910a 100644
--- a/source/RobotAPI/libraries/skills/core/SkillProxy.h
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.h
@@ -1,5 +1,6 @@
#pragma once
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
index 39220a32bc28269232f506d8368cb5394980b87b..2f57db3940b0d622650565951ccc04e8950d2277 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
@@ -1,5 +1,7 @@
#include "SkillStatusUpdate.h"
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
+
namespace armarx
{
namespace skills
@@ -404,5 +406,103 @@ namespace armarx
return ret;
}
+ bool
+ SkillStatusUpdate::operator<(const SkillStatusUpdate& o) const
+ {
+ if (!hasBeenConstructed() and o.hasBeenConstructed())
+ {
+ return true;
+ }
+ if (!hasBeenInitialized() and o.hasBeenInitialized())
+ {
+ return true;
+ }
+ if (!hasBeenPrepared() and o.hasBeenPrepared())
+ {
+ return true;
+ }
+ if (!hasBeenRunning() and o.hasBeenRunning())
+ {
+ return true;
+ }
+ if (!hasBeenTerminated() and o.hasBeenTerminated())
+ {
+ return true;
+ }
+ return false;
+ }
+
+ bool
+ SkillStatusUpdate::operator<=(const SkillStatusUpdate& o) const
+ {
+ if (status == o.status)
+ {
+ return true;
+ }
+ return *this < o;
+ }
+
+ bool
+ SkillStatusUpdate::hasBeenConstructed() const
+ {
+ return status != SkillStatus::Constructing;
+ }
+
+ bool
+ SkillStatusUpdate::hasBeenInitialized() const
+ {
+ return status != SkillStatus::Initializing && hasBeenConstructed();
+ }
+
+ bool
+ SkillStatusUpdate::hasBeenPrepared() const
+ {
+ return status != SkillStatus::Preparing && hasBeenInitialized();
+ }
+
+ bool
+ SkillStatusUpdate::hasBeenRunning() const
+ {
+ return status != SkillStatus::Running || hasBeenPrepared();
+ }
+
+ bool
+ SkillStatusUpdate::hasBeenTerminated() const
+ {
+ return status == SkillStatus::Succeeded || status == SkillStatus::Failed ||
+ status == SkillStatus::Aborted;
+ }
+
+ bool
+ SkillStatusUpdate::hasBeenSucceeded() const
+ {
+ return status == SkillStatus::Succeeded;
+ }
+
+ bool
+ ActiveOrTerminatedSkillStatusUpdate::hasBeenTerminated() const
+ {
+ return status == ActiveOrTerminatedSkillStatus::Succeeded ||
+ status == ActiveOrTerminatedSkillStatus::Failed ||
+ status == ActiveOrTerminatedSkillStatus::Aborted;
+ }
+
+ bool
+ ActiveOrTerminatedSkillStatusUpdate::hasBeenSucceeded() const
+ {
+ return status == ActiveOrTerminatedSkillStatus::Succeeded;
+ }
+
+ bool
+ TerminatedSkillStatusUpdate::hasBeenTerminated() const
+ {
+ return true;
+ }
+
+ bool
+ TerminatedSkillStatusUpdate::hasBeenSucceeded() const
+ {
+ return status == TerminatedSkillStatus::Succeeded;
+ }
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
index ace31510a8a362ed45289c4facdd51c97ba22fbb..4ce5cba6afad707bac55c6155b64cb44115819b7 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
@@ -1,8 +1,5 @@
#pragma once
-#include <string>
-#include <vector>
-
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
@@ -10,8 +7,6 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include "SkillExecutionID.h"
-#include "SkillID.h"
-#include "SkillParameterization.h"
namespace armarx
{
@@ -80,17 +75,9 @@ namespace armarx
{
TerminatedSkillStatus status = TerminatedSkillStatus::Failed;
- bool
- hasBeenTerminated() const
- {
- return true;
- }
+ bool hasBeenTerminated() const;
- bool
- hasBeenSucceeded() const
- {
- return status == TerminatedSkillStatus::Succeeded;
- }
+ bool hasBeenSucceeded() const;
manager::dto::SkillStatusUpdate toManagerIce() const;
@@ -109,19 +96,9 @@ namespace armarx
{
ActiveOrTerminatedSkillStatus status = ActiveOrTerminatedSkillStatus::Failed;
- bool
- hasBeenTerminated() const
- {
- return status == ActiveOrTerminatedSkillStatus::Succeeded ||
- status == ActiveOrTerminatedSkillStatus::Failed ||
- status == ActiveOrTerminatedSkillStatus::Aborted;
- }
+ bool hasBeenTerminated() const;
- bool
- hasBeenSucceeded() const
- {
- return status == ActiveOrTerminatedSkillStatus::Succeeded;
- }
+ bool hasBeenSucceeded() const;
manager::dto::SkillStatusUpdate toManagerIce() const;
@@ -140,78 +117,21 @@ namespace armarx
{
SkillStatus status = SkillStatus::Constructing;
- bool
- operator<(const SkillStatusUpdate& o) const
- {
- if (!hasBeenConstructed() and o.hasBeenConstructed())
- {
- return true;
- }
- if (!hasBeenInitialized() and o.hasBeenInitialized())
- {
- return true;
- }
- if (!hasBeenPrepared() and o.hasBeenPrepared())
- {
- return true;
- }
- if (!hasBeenRunning() and o.hasBeenRunning())
- {
- return true;
- }
- if (!hasBeenTerminated() and o.hasBeenTerminated())
- {
- return true;
- }
- return false;
- }
-
- bool
- operator<=(const SkillStatusUpdate& o) const
- {
- if (status == o.status)
- {
- return true;
- }
- return *this < o;
- }
-
- bool
- hasBeenConstructed() const
- {
- return status != SkillStatus::Constructing;
- }
-
- bool
- hasBeenInitialized() const
- {
- return status != SkillStatus::Initializing && hasBeenConstructed();
- }
-
- bool
- hasBeenPrepared() const
- {
- return status != SkillStatus::Preparing && hasBeenInitialized();
- }
-
- bool
- hasBeenRunning() const
- {
- return status != SkillStatus::Running || hasBeenPrepared();
- }
-
- bool
- hasBeenTerminated() const
- {
- return status == SkillStatus::Succeeded || status == SkillStatus::Failed ||
- status == SkillStatus::Aborted;
- }
-
- bool
- hasBeenSucceeded() const
- {
- return status == SkillStatus::Succeeded;
- }
+ bool operator<(const SkillStatusUpdate& o) const;
+
+ bool operator<=(const SkillStatusUpdate& o) const;
+
+ bool hasBeenConstructed() const;
+
+ bool hasBeenInitialized() const;
+
+ bool hasBeenPrepared() const;
+
+ bool hasBeenRunning() const;
+
+ bool hasBeenTerminated() const;
+
+ bool hasBeenSucceeded() const;
manager::dto::SkillStatusUpdate toManagerIce() const;
diff --git a/source/RobotAPI/libraries/skills/core/error/Exception.cpp b/source/RobotAPI/libraries/skills/core/error/Exception.cpp
index 32521fedeb0428e384897ce88e2f9f42ebb57a71..c2668867df8b5f5e5ea0f703582f0c92f14c4587 100644
--- a/source/RobotAPI/libraries/skills/core/error/Exception.cpp
+++ b/source/RobotAPI/libraries/skills/core/error/Exception.cpp
@@ -1,2 +1,33 @@
#include "Exception.h"
+
+armarx::skills::error::SkillException::SkillException(const std::string& prettymethod,
+ const std::string& reason) :
+ LocalException(prettymethod + ": " + reason + ".")
+{
+}
+
+armarx::skills::error::SkillNotFoundException::SkillNotFoundException(
+ const std::string& prettymethod,
+ const std::string& reason) :
+ SkillException(prettymethod, reason)
+{
+}
+
+armarx::skills::error::SkillAbortedException::SkillAbortedException(const std::string& prettymethod,
+ const std::string& reason) :
+ SkillException(prettymethod, reason)
+{
+}
+
+armarx::skills::error::SkillFailedException::SkillFailedException(const std::string& prettymethod,
+ const std::string& reason) :
+ SkillException(prettymethod, reason)
+{
+}
+
+armarx::skills::error::NotImplementedYetException::NotImplementedYetException(
+ const std::string& prettymethod) :
+ SkillException(prettymethod, "This method is not yet implemented!")
+{
+}
diff --git a/source/RobotAPI/libraries/skills/core/error/Exception.h b/source/RobotAPI/libraries/skills/core/error/Exception.h
index 9e01205b27e74eddc69fa8ff7c28d2646eafc9e2..66e72e418d51ffb1555498741cf928d1e417e497 100644
--- a/source/RobotAPI/libraries/skills/core/error/Exception.h
+++ b/source/RobotAPI/libraries/skills/core/error/Exception.h
@@ -23,14 +23,10 @@
#pragma once
-// STD/STL
-#include <map>
+
#include <string>
-#include <vector>
-// ArmarX
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
namespace armarx::skills::error
{
@@ -42,10 +38,7 @@ namespace armarx::skills::error
public:
SkillException() = delete;
- SkillException(const std::string& prettymethod, const std::string& reason) :
- LocalException(prettymethod + ": " + reason + ".")
- {
- }
+ SkillException(const std::string& prettymethod, const std::string& reason);
};
/**
@@ -56,10 +49,7 @@ namespace armarx::skills::error
public:
SkillNotFoundException() = delete;
- SkillNotFoundException(const std::string& prettymethod, const std::string& reason) :
- SkillException(prettymethod, reason)
- {
- }
+ SkillNotFoundException(const std::string& prettymethod, const std::string& reason);
};
class SkillAbortedException : public SkillException
@@ -67,10 +57,7 @@ namespace armarx::skills::error
public:
SkillAbortedException() = delete;
- SkillAbortedException(const std::string& prettymethod, const std::string& reason) :
- SkillException(prettymethod, reason)
- {
- }
+ SkillAbortedException(const std::string& prettymethod, const std::string& reason);
};
class SkillFailedException : public SkillException
@@ -78,10 +65,7 @@ namespace armarx::skills::error
public:
SkillFailedException() = delete;
- SkillFailedException(const std::string& prettymethod, const std::string& reason) :
- SkillException(prettymethod, reason)
- {
- }
+ SkillFailedException(const std::string& prettymethod, const std::string& reason);
};
/**
@@ -92,9 +76,6 @@ namespace armarx::skills::error
public:
NotImplementedYetException() = delete;
- NotImplementedYetException(const std::string& prettymethod) :
- SkillException(prettymethod, "This method is not yet implemented!")
- {
- }
+ NotImplementedYetException(const std::string& prettymethod);
};
} // namespace armarx::skills::error
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
index fa354d03d242c9e19b95a4a846309227503ae182..4b8a07d07238f87a2cea835ed33b75c0520d8639 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -27,6 +27,7 @@
#include <ArmarXCore/core/time/Metronome.h>
#include "RobotAPI/libraries/skills/core/Skill.h"
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
namespace armarx::skills
{
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
index 914c6da0221973624309a57666064669e05de14a..b707aee0d5224ab8588c4c95461ec22f5f300fa6 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include "PeriodicSkill.h"
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
index 33a6475b3a9e2e01a30c40f4cee60e304a600b40..68b1fb7e137bbf33045cc320100de697e7ab52e5 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
@@ -1,4 +1,7 @@
#include "SimplePeriodicSkill.h"
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
namespace armarx::skills
{
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
index ce3c968f587a7984fb606e8485297aa56ffff04a..37d1e6084e81f99dd59973e9d22df022df509fb0 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
@@ -1,5 +1,7 @@
#pragma once
+#include <ArmarXCore/core/time/Metronome.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include "PeriodicSkill.h"
#include "SimpleSpecializedSkill.h"
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index 72024a427cfb9e2b0907a36f3bccd6e886e12d4a..ac7ab61a80b89081778f571478dee3ed3fe3910d 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -42,6 +42,10 @@ namespace armarx::plugins
auto id = skills::manager::dto::ProviderID{providerName};
manager->removeProvider(id);
+
+ // remove all skills
+ ARMARX_INFO << "Removing all skills";
+ skillFactories.clear();
}
void
@@ -82,6 +86,7 @@ namespace armarx::plugins
ARMARX_INFO << "Adding skill `" << skillId << "` to component `" << componentName << "` .";
skillFactories.emplace(skillId, std::move(fac));
+
// if (connected)
// {
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 858ee511de6fa042e257b202b6cee3bf94a446a1..21e0d6d97ecb7841237b4ed495355531c38d1972 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -1,11 +1,7 @@
#pragma once
#include <experimental/memory>
-#include <functional>
-#include <queue>
#include <shared_mutex>
-#include <thread>
-#include <type_traits>
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -14,20 +10,13 @@
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-// Include all types of skills
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
-#include <RobotAPI/libraries/skills/core/SkillPreparationInput.h>
-#include "LambdaSkill.h"
-#include "PeriodicSkill.h"
-#include "PeriodicSpecializedSkill.h"
-#include "SkillFactory.h"
-#include "SpecializedSkill.h"
-
-// Helper wrapper for execution
#include "detail/SkillImplementationWrapper.h"
+#include "LambdaSkill.h"
+
namespace armarx
{
class SkillProviderComponentPluginUser; // forward declaration
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index 2fded9a0a50ede4cdc3e8d76861217fbc1127c58..fa3c43ce54eda2feacd0f88895ede30840c08e1f 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -3,47 +3,41 @@
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
-// Debug
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-namespace armarx
+namespace armarx::skills
{
- namespace skills
+ template <class AronT>
+ class SpecializedSkill : public Skill
{
- template <class AronT>
- class SpecializedSkill : public Skill
+ public:
+ using Base = Skill;
+ using ParamType = AronT;
+
+ using Skill::Skill;
+ virtual ~SpecializedSkill() = default;
+
+ void
+ setParameters(const AronT& d)
+ {
+ Base::setParameters(d.toAron());
+ }
+
+ /// Overwrite getter for parameters. Shadow Skill::getParameters()
+ AronT
+ getParameters() const
+ {
+ AronT d;
+ d.fromAron(this->parameters);
+ return d;
+ }
+
+ /// returns the accepted type of the skill
+ static armarx::aron::type::ObjectPtr
+ GetAcceptedType()
{
- public:
- using Base = Skill;
- using ParamType = AronT;
-
- using Skill::Skill;
- virtual ~SpecializedSkill() = default;
-
- void
- setParameters(const AronT& d)
- {
- Base::setParameters(d.toAron());
- }
-
- /// Overwrite getter for parameters. Shadow Skill::getParameters()
- AronT
- getParameters() const
- {
- AronT d;
- d.fromAron(this->parameters);
- return d;
- }
-
- /// returns the accepted type of the skill
- static armarx::aron::type::ObjectPtr
- GetAcceptedType()
- {
- return AronT::ToAronType();
- }
-
-
- protected:
- };
- } // namespace skills
-} // namespace armarx
+ return AronT::ToAronType();
+ }
+
+
+ protected:
+ };
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h b/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h
index db852e076274c6f8b479a25d4b7d65c474912fae..6a6802204822692867f0a9c5545359d4a0fc77bb 100644
--- a/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h
+++ b/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h
@@ -22,6 +22,8 @@
#pragma once
+#include <atomic>
+
#include <RobotAPI/libraries/skills/provider/SkillFactory.h>
namespace armarx
@@ -79,4 +81,4 @@ namespace armarx
ContextT context_;
};
} // namespace skills
-} // namespace armarx
\ No newline at end of file
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index a244ae6593ec82b802dcbd930af07990ce3d4503..35fc69ddd8a073f3b78ae008a67466dddc36ce68 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -1,8 +1,11 @@
#include "SkillImplementationWrapper.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/skills/core/aron/SkillErrorResult.aron.generated.h>
+
+
namespace armarx
{
namespace skills::detail
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index 935fa227bf13831d3bcc789a124b36d12f1ba423..8b3f51277fe351229dbbb4f00eab72655cc724ea 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -1,12 +1,10 @@
#pragma once
#include <experimental/memory>
-#include <shared_mutex>
+#include <mutex>
-#include <RobotAPI/interface/skills/SkillManagerInterface.h>
-#include <RobotAPI/libraries/skills/core/Skill.h>
-#include <RobotAPI/libraries/skills/core/SkillDescription.h>
-#include <RobotAPI/libraries/skills/core/SkillPreparationInput.h>
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SkillFactory.h>
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.h b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.h
index 9d0c8cbfbd9463ab37d8498410381aebd54c6956..e179a85936904abd6ceb96749663252ef939767e 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.h
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.h
@@ -6,6 +6,7 @@
#include <QObject>
#include <QTreeWidgetItem>
#include <QVBoxLayout>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/IntEnumWidget.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/IntEnumWidget.cpp
index bbbee283d5aee3f75a0ce77b07a3e5dbca18eab7..458aa39b9d02ee7cae71bb274a51ba15d3452cae 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/IntEnumWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/IntEnumWidget.cpp
@@ -2,7 +2,8 @@
#include <QHBoxLayout>
-#include "RobotAPI/libraries/aron/core/type/variant/All.h"
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+#include <SimoxUtility/error/SimoxError.h>
#include "../ColorPalettes.h"
#include "../visitors/AronTreeWidgetConverter.h"
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.cpp
index e8a48aa71942e5f86e837a8fc516e7d475615f66..f6d123b7621f051eb21e39dd8d01965e84e66636 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.cpp
@@ -7,6 +7,8 @@
#include "../ColorPalettes.h"
#include "NDArrayHelper.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
namespace armarx::skills::gui
{
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.h b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.h
index f743e3acf3a2eafdfd03f3eb35bddb48b49cfbb4..c256f0a224acd64f4824f6b46cfcb77cc97d1880 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.h
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.h
@@ -1,11 +1,14 @@
#pragma once
+
+#include <cmath>
#include <vector>
#include <QLineEdit>
#include <QObject>
#include <QVBoxLayout>
-
-#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+#include <SimoxUtility/error/SimoxError.h>
+#include <RobotAPI/interface/aron/Aron.h>
#include "CustomWidget.h"
diff --git a/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp b/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
index 780c2bb728874f9865d7147ee2a17be55e925527..87a254728e4479584bc23d6b66959162363c2c24 100644
--- a/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
@@ -4,6 +4,7 @@
#include <QMenu>
#include <QTreeWidgetItem>
+#include <ArmarXCore/core/logging/Logging.h>
#include "SkillExecutionTreeWidgetItem.h"
diff --git a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
index 4ff646277634467ae9cc7d58bd5e29ef40cc9099..cc72d4565872bf8367c67cdc89ddf9af8381b4df 100644
--- a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
+++ b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
@@ -1,6 +1,8 @@
#include "SkillManagerWrapper.h"
#include <mutex>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include "RobotAPI/libraries/skills/core/SkillExecutionRequest.h"
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
index df07b365c25f2f744246fd517d02e1a76e573adb..95ee1853bb68ffe504777330e996b1c5078ace25 100644
--- a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
@@ -21,6 +21,7 @@
*/
#include "TestGetNames.h"
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
@@ -208,4 +209,3 @@ XMLStateFactoryBasePtr TestGetNames::CreateInstance(XMLStateConstructorParams st
{
return XMLStateFactoryBasePtr(new TestGetNames(stateData));
}
-