diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 54e555329705bb870e0c963cf1fba2d167bf8a13..312cb8623bd754777eb8de264dc8371abd1714c8 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -200,329 +200,412 @@ ENDMACRO()
#################################### FILES ##################################
SET(SOURCES
-CollisionDetection/CollisionChecker.cpp
-CollisionDetection/CollisionModel.cpp
-CollisionDetection/CDManager.cpp
-EndEffector/EndEffector.cpp
-EndEffector/EndEffectorActor.cpp
-Nodes/RobotNode.cpp
-Nodes/RobotNodePrismatic.cpp
-Nodes/RobotNodePrismaticFactory.cpp
-Nodes/RobotNodeRevolute.cpp
-Nodes/RobotNodeRevoluteFactory.cpp
-Nodes/RobotNodeFixed.cpp
-Nodes/RobotNodeFixedFactory.cpp
-Nodes/RobotNodeActuator.cpp
-Nodes/Sensor.cpp
-Nodes/PositionSensor.cpp
-Nodes/ForceTorqueSensor.cpp
-Nodes/ContactSensor.cpp
-Nodes/CameraSensor.cpp
-Nodes/PositionSensorFactory.cpp
-Nodes/ForceTorqueSensorFactory.cpp
-Nodes/ContactSensorFactory.cpp
-Nodes/CameraSensorFactory.cpp
-Visualization/Visualization.cpp
-Visualization/VisualizationNode.cpp
-Visualization/ColorMap.cpp
-Visualization/TriMeshModel.cpp
-Visualization/TriMeshUtils.cpp
-XML/BaseIO.cpp
-XML/RobotIO.cpp
-XML/SceneIO.cpp
-XML/ObjectIO.cpp
-XML/FileIO.cpp
-XML/mjcf/MasslessBodySanitizer.cpp
-XML/mjcf/MjcfDocument.cpp
-XML/mjcf/MujocoIO.cpp
-XML/mjcf/utils.cpp
-IK/IKSolver.cpp
-IK/AdvancedIKSolver.cpp
-IK/DifferentialIK.cpp
-IK/GenericIKSolver.cpp
-IK/CoMIK.cpp
-IK/JacobiProvider.cpp
-IK/PoseQualityMeasurement.cpp
-IK/PoseQualityManipulability.cpp
-IK/PoseQualityExtendedManipulability.cpp
-IK/HierarchicalIK.cpp
-IK/HierarchicalIKSolver.cpp
-IK/JointLimitAvoidanceJacobi.cpp
-IK/GazeIK.cpp
-IK/StackedIK.cpp
-IK/SupportPolygon.cpp
-IK/FeetPosture.cpp
-IK/ConstrainedIK.cpp
-IK/ConstrainedHierarchicalIK.cpp
-IK/ConstrainedStackedIK.cpp
-IK/Constraint.cpp
-IK/constraints/BalanceConstraint.cpp
-IK/constraints/PoseConstraint.cpp
-IK/constraints/PositionConstraint.cpp
-IK/constraints/OrientationConstraint.cpp
-IK/constraints/TSRConstraint.cpp
-IK/constraints/ReferenceConfigurationConstraint.cpp
-IK/constraints/JointLimitAvoidanceConstraint.cpp
-IK/constraints/CoMConstraint.cpp
-IK/constraints/CollisionCheckConstraint.cpp
-Workspace/WorkspaceDataArray.cpp
-Workspace/WorkspaceRepresentation.cpp
-Workspace/Reachability.cpp
-Workspace/Manipulability.cpp
-Workspace/WorkspaceGrid.cpp
-#Workspace/ReachabilityInversion/InverseReachability.cpp
-#Workspace/ReachabilityInversion/OrientedWorkspaceGrid.cpp
-#Workspace/ReachabilityInversion/ReachabilityProcessor.cpp
-#Workspace/ReachabilityInversion/RobotPlacementIK.cpp
-#Workspace/ReachabilityInversion/RobotPlacementTrajectoryIK.cpp
-TimeOptimalTrajectory/Path.cpp
-TimeOptimalTrajectory/TimeOptimalTrajectory.cpp
-Grasping/Grasp.cpp
-Grasping/GraspSet.cpp
-Grasping/BasicGraspQualityMeasure.cpp
-MathTools.cpp
-Robot.cpp
-RobotConfig.cpp
-RobotNodeSet.cpp
-Trajectory.cpp
-KinematicChain.cpp
-RobotFactory.cpp
-SceneObject.cpp
-SceneObjectSet.cpp
-Scene.cpp
-Obstacle.cpp
-Primitive.cpp
-VirtualRobotException.cpp
-ManipulationObject.cpp
-BoundingBox.cpp
-RuntimeEnvironment.cpp
-Random.cpp
-LinkedCoordinate.cpp
-SphereApproximator.cpp
-VirtualRobot.cpp
-Compression/CompressionRLE.cpp
-Compression/CompressionBZip2.cpp
-Import/SimoxXMLFactory.cpp
-Import/RobotImporterFactory.cpp
-Import/MeshImport/STLReader.cpp
-Tools/Gravity.cpp
-math/AbstractFunctionR1R2.cpp
-math/AbstractFunctionR1R3.cpp
-math/AbstractFunctionR1R6.cpp
-math/AbstractFunctionR2R3.cpp
-math/Bezier.cpp
-math/CompositeFunctionR1R6.cpp
-math/Contact.cpp
-math/ContactList.cpp
-math/DataR3R1.cpp
-math/DataR3R2.cpp
-math/FitPlane.cpp
-math/GaussianImplicitSurface3D.cpp
-math/GaussianImplicitSurface3DNormals.cpp
-math/GaussianImplicitSurface3DCombined.cpp
-math/GaussianObjectModel.cpp
-math/GaussianObjectModelNormals.cpp
-math/Grid3D.cpp
-math/GridCacheFloat3.cpp
-math/Helpers.cpp
-math/ImplicitPlane.cpp
-math/ImplicitObjectModel.cpp
-math/Index3.cpp
-math/Kernels.cpp
-math/Line.cpp
-math/LinearContinuedBezier.cpp
-math/LinearInterpolatedOrientation.cpp
-math/LineR2.cpp
-math/LineStrip.cpp
-math/MarchingCubes.cpp
-math/TransformedFunctionR1R3.cpp
-math/TransformedFunctionR2R3.cpp
-math/Plane.cpp
-math/Primitive.cpp
-math/Triangle.cpp
-math/WeightedAverage.cpp
-math/statistics/measures.cpp
-math/statistics/BoxPlot.cpp
-math/statistics/Histogram.cpp
-Util/xml/tinyxml2.cpp
-Util/json/eigen_conversion.cpp
+
+ BoundingBox.cpp
+ KinematicChain.cpp
+ LinkedCoordinate.cpp
+ ManipulationObject.cpp
+ MathTools.cpp
+ Obstacle.cpp
+ Primitive.cpp
+ Random.cpp
+ Robot.cpp
+ RobotConfig.cpp
+ RobotFactory.cpp
+ RobotNodeSet.cpp
+ RuntimeEnvironment.cpp
+ Scene.cpp
+ SceneObject.cpp
+ SceneObjectSet.cpp
+ SphereApproximator.cpp
+ Trajectory.cpp
+ VirtualRobot.cpp
+ VirtualRobotException.cpp
+
+ CollisionDetection/CDManager.cpp
+ CollisionDetection/CollisionChecker.cpp
+ CollisionDetection/CollisionModel.cpp
+
+ Compression/CompressionBZip2.cpp
+ Compression/CompressionRLE.cpp
+
+ EndEffector/EndEffector.cpp
+ EndEffector/EndEffectorActor.cpp
+
+ Grasping/BasicGraspQualityMeasure.cpp
+ Grasping/Grasp.cpp
+ Grasping/GraspSet.cpp
+
+ IK/AdvancedIKSolver.cpp
+ IK/CoMIK.cpp
+ IK/ConstrainedHierarchicalIK.cpp
+ IK/ConstrainedIK.cpp
+ IK/ConstrainedStackedIK.cpp
+ IK/Constraint.cpp
+ IK/DifferentialIK.cpp
+ IK/FeetPosture.cpp
+ IK/GazeIK.cpp
+ IK/GenericIKSolver.cpp
+ IK/HierarchicalIK.cpp
+ IK/HierarchicalIKSolver.cpp
+ IK/IKSolver.cpp
+ IK/JacobiProvider.cpp
+ IK/JointLimitAvoidanceJacobi.cpp
+ IK/PoseQualityExtendedManipulability.cpp
+ IK/PoseQualityManipulability.cpp
+ IK/PoseQualityMeasurement.cpp
+ IK/StackedIK.cpp
+ IK/SupportPolygon.cpp
+ IK/constraints/BalanceConstraint.cpp
+ IK/constraints/CoMConstraint.cpp
+ IK/constraints/CollisionCheckConstraint.cpp
+ IK/constraints/JointLimitAvoidanceConstraint.cpp
+ IK/constraints/OrientationConstraint.cpp
+ IK/constraints/PoseConstraint.cpp
+ IK/constraints/PositionConstraint.cpp
+ IK/constraints/ReferenceConfigurationConstraint.cpp
+ IK/constraints/TSRConstraint.cpp
+
+ Import/RobotImporterFactory.cpp
+ Import/SimoxXMLFactory.cpp
+ Import/MeshImport/STLReader.cpp
+
+ math/AbstractFunctionR1R2.cpp
+ math/AbstractFunctionR1R3.cpp
+ math/AbstractFunctionR1R6.cpp
+ math/AbstractFunctionR2R3.cpp
+ math/Bezier.cpp
+ math/CompositeFunctionR1R6.cpp
+ math/Contact.cpp
+ math/ContactList.cpp
+ math/DataR3R1.cpp
+ math/DataR3R2.cpp
+ math/FitPlane.cpp
+ math/GaussianImplicitSurface3D.cpp
+ math/GaussianImplicitSurface3DNormals.cpp
+ math/GaussianImplicitSurface3DCombined.cpp
+ math/GaussianObjectModel.cpp
+ math/GaussianObjectModelNormals.cpp
+ math/Grid3D.cpp
+ math/GridCacheFloat3.cpp
+ math/Helpers.cpp
+ math/ImplicitPlane.cpp
+ math/ImplicitObjectModel.cpp
+ math/Index3.cpp
+ math/Kernels.cpp
+ math/Line.cpp
+ math/LinearContinuedBezier.cpp
+ math/LinearInterpolatedOrientation.cpp
+ math/LineR2.cpp
+ math/LineStrip.cpp
+ math/MarchingCubes.cpp
+ math/TransformedFunctionR1R3.cpp
+ math/TransformedFunctionR2R3.cpp
+ math/Plane.cpp
+ math/Primitive.cpp
+ math/Triangle.cpp
+ math/WeightedAverage.cpp
+ math/statistics/measures.cpp
+ math/statistics/BoxPlot.cpp
+ math/statistics/Histogram.cpp
+
+ MJCF/Document.cpp
+ MJCF/elements/core/AnyElement.cpp
+ MJCF/elements/core/Attribute.cpp
+ MJCF/elements/core/Element.cpp
+ MJCF/elements/core/exceptions.cpp
+ MJCF/elements/core/mjcf_utils.cpp
+ MJCF/elements/core/Visitor.cpp
+ MJCF/elements/types/actuator.cpp
+ MJCF/elements/types/asset.cpp
+ MJCF/elements/types/body.cpp
+ MJCF/elements/types/contact.cpp
+ MJCF/elements/types/compiler.cpp
+ MJCF/elements/types/custom.cpp
+ MJCF/elements/types/default.cpp
+ MJCF/elements/types/equality.cpp
+ MJCF/elements/types/keyframe.cpp
+ MJCF/elements/types/meta.cpp
+ MJCF/elements/types/option.cpp
+ MJCF/elements/types/sensor.cpp
+ MJCF/elements/types/size.cpp
+ MJCF/elements/types/statistic.cpp
+ MJCF/elements/types/tendon.cpp
+ MJCF/elements/types/visual.cpp
+ MJCF/visitors/Collector.cpp
+
+ Nodes/CameraSensor.cpp
+ Nodes/CameraSensorFactory.cpp
+ Nodes/ContactSensor.cpp
+ Nodes/ContactSensorFactory.cpp
+ Nodes/ForceTorqueSensor.cpp
+ Nodes/ForceTorqueSensorFactory.cpp
+ Nodes/PositionSensor.cpp
+ Nodes/PositionSensorFactory.cpp
+ Nodes/RobotNode.cpp
+ Nodes/RobotNodeActuator.cpp
+ Nodes/RobotNodeFixed.cpp
+ Nodes/RobotNodeFixedFactory.cpp
+ Nodes/RobotNodePrismatic.cpp
+ Nodes/RobotNodePrismaticFactory.cpp
+ Nodes/RobotNodeRevolute.cpp
+ Nodes/RobotNodeRevoluteFactory.cpp
+ Nodes/Sensor.cpp
+
+ TimeOptimalTrajectory/Path.cpp
+ TimeOptimalTrajectory/TimeOptimalTrajectory.cpp
+
+ Tools/Gravity.cpp
+
+ Util/json/eigen_conversion.cpp
+ Util/xml/tinyxml2.cpp
+
+ Visualization/ColorMap.cpp
+ Visualization/TriMeshModel.cpp
+ Visualization/TriMeshUtils.cpp
+ Visualization/Visualization.cpp
+ Visualization/VisualizationNode.cpp
+
+ Workspace/Manipulability.cpp
+ Workspace/Reachability.cpp
+ Workspace/WorkspaceDataArray.cpp
+ Workspace/WorkspaceGrid.cpp
+ Workspace/WorkspaceRepresentation.cpp
+ #Workspace/ReachabilityInversion/InverseReachability.cpp
+ #Workspace/ReachabilityInversion/OrientedWorkspaceGrid.cpp
+ #Workspace/ReachabilityInversion/ReachabilityProcessor.cpp
+ #Workspace/ReachabilityInversion/RobotPlacementIK.cpp
+ #Workspace/ReachabilityInversion/RobotPlacementTrajectoryIK.cpp
+
+ XML/BaseIO.cpp
+ XML/FileIO.cpp
+ XML/ObjectIO.cpp
+ XML/RobotIO.cpp
+ XML/SceneIO.cpp
+ XML/mujoco/MasslessBodySanitizer.cpp
+ XML/mujoco/MujocoIO.cpp
)
SET(INCLUDES
-CollisionDetection/CollisionChecker.h
-CollisionDetection/CollisionModel.h
-CollisionDetection/CDManager.h
-CollisionDetection/CollisionModelImplementation.h
-CollisionDetection/CollisionCheckerImplementation.h
-EndEffector/EndEffector.h
-EndEffector/EndEffectorActor.h
-Nodes/RobotNode.h
-Nodes/RobotNodeFactory.h
-Nodes/RobotNodePrismatic.h
-Nodes/RobotNodePrismaticFactory.h
-Nodes/RobotNodeRevolute.h
-Nodes/RobotNodeRevoluteFactory.h
-Nodes/RobotNodeFixed.h
-Nodes/RobotNodeFixedFactory.h
-Nodes/RobotNodeActuator.h
-Nodes/ConditionedLock.h
-Nodes/Sensor.h
-Nodes/PositionSensor.h
-Nodes/ForceTorqueSensor.h
-Nodes/CameraSensor.h
-Nodes/SensorFactory.h
-Nodes/PositionSensorFactory.h
-Nodes/ForceTorqueSensorFactory.h
-Nodes/CameraSensorFactory.h
-Transformation/DHParameter.h
-Visualization/VisualizationFactory.h
-Visualization/Visualization.h
-Visualization/VisualizationNode.h
-Visualization/ColorMap.h
-Visualization/TriMeshModel.h
-Visualization/TriMeshUtils.h
-XML/BaseIO.h
-XML/RobotIO.h
-XML/SceneIO.h
-XML/ObjectIO.h
-XML/FileIO.h
-XML/mjcf/MasslessBodySanitizer.h
-XML/mjcf/MjcfDocument.h
-XML/mjcf/MujocoIO.h
-XML/mjcf/utils.h
-IK/IKSolver.h
-IK/AdvancedIKSolver.h
-IK/DifferentialIK.h
-IK/GenericIKSolver.h
-IK/CoMIK.h
-IK/JacobiProvider.h
-IK/PoseQualityMeasurement.h
-IK/PoseQualityManipulability.h
-IK/PoseQualityExtendedManipulability.h
-IK/HierarchicalIK.h
-IK/HierarchicalIKSolver.h
-IK/JointLimitAvoidanceJacobi.h
-IK/GazeIK.h
-IK/StackedIK.h
-IK/SupportPolygon.h
-IK/FeetPosture.h
-IK/ConstrainedIK.h
-IK/ConstrainedHierarchicalIK.h
-IK/ConstrainedStackedIK.h
-IK/Constraint.h
-IK/constraints/BalanceConstraint.h
-IK/constraints/PoseConstraint.h
-IK/constraints/PositionConstraint.h
-IK/constraints/OrientationConstraint.h
-IK/constraints/TSRConstraint.h
-IK/constraints/ReferenceConfigurationConstraint.h
-IK/constraints/JointLimitAvoidanceConstraint.h
-IK/constraints/CoMConstraint.h
-IK/constraints/CollisionCheckConstraint.h
-Workspace/WorkspaceData.h
-Workspace/WorkspaceDataArray.h
-Workspace/WorkspaceRepresentation.h
-Workspace/Reachability.h
-Workspace/Manipulability.h
-Workspace/VoxelTree6D.hpp
-Workspace/VoxelTree6DElement.hpp
-Workspace/VoxelTreeND.hpp
-Workspace/VoxelTreeNDElement.hpp
-Workspace/WorkspaceGrid.h
-#Workspace/ReachabilityInversion/InverseReachability.h
-#Workspace/ReachabilityInversion/OrientedWorkspaceGrid.h
-#Workspace/ReachabilityInversion/ReachabilityProcessor.h
-#Workspace/ReachabilityInversion/RobotPlacementIK.h
-#Workspace/ReachabilityInversion/RobotPlacementTrajectoryIK.h
-TimeOptimalTrajectory/Path.h
-TimeOptimalTrajectory/TimeOptimalTrajectory.h
-Grasping/Grasp.h
-Grasping/GraspSet.h
-Grasping/BasicGraspQualityMeasure.h
-AbstractFactoryMethod.h
-VirtualRobot.h
-MathTools.h
-Robot.h
-RobotConfig.h
-RobotNodeSet.h
-Trajectory.h
-KinematicChain.h
-RobotFactory.h
-SceneObject.h
-SceneObjectSet.h
-Scene.h
-Obstacle.h
-Primitive.h
-VirtualRobotException.h
-VirtualRobotImportExport.h
-VirtualRobotTest.h
-ManipulationObject.h
-BoundingBox.h
-RuntimeEnvironment.h
-Random.h
-DataStructures/nanoflann.hpp
-DataStructures/KdTreePointCloud.h
-Compression/CompressionRLE.h
-Compression/CompressionBZip2.h
-SphereApproximator.h
-Import/SimoxXMLFactory.h
-Import/RobotImporterFactory.h
-Import/MeshImport/STLReader.h
-Tools/Gravity.h
-math/AbstractFunctionR1Ori.h
-math/AbstractFunctionR1R2.h
-math/AbstractFunctionR1R3.h
-math/AbstractFunctionR1R6.h
-math/AbstractFunctionR2R3.h
-math/AbstractFunctionR3R1.h
-math/Array3D.h
-math/Bezier.h
-math/ClampedNormalDistribution.hpp
-math/CompositeFunctionR1R6.h
-math/Contact.h
-math/ContactList.h
-math/DataR3R1.h
-math/DataR3R2.h
-math/FitPlane.h
-math/GaussianImplicitSurface3D.h
-math/GaussianImplicitSurface3DCombined.h
-math/GaussianImplicitSurface3DNormals.h
-math/GaussianObjectModel.h
-math/GaussianObjectModelNormals.h
-math/Grid3D.h
-math/GridCacheFloat3.h
-math/Helpers.h
-math/ImplicitObjectModel.cpp
-math/ImplicitPlane.h
-math/Index3.h
-math/Kernels.h
-math/Line.h
-math/LineR2.h
-math/LineStrip.h
-math/LineStripR1RM.h
-math/LinearContinuedBezier.h
-math/LinearInterpolatedOrientation.h
-math/MarchingCubes.h
-math/MathForwardDefinitions.h
-math/Plane.h
-math/Primitive.h
-math/SimpleAbstractFunctionR1Ori.h
-math/SimpleAbstractFunctionR1R3.h
-math/SimpleAbstractFunctionR1R6.h
-math/SimpleAbstractFunctionR2R3.h
-math/SimpleAbstractFunctionR3R1.h
-math/SimpleAbstractFunctionRNRM.h
-math/TransformedFunctionR1R3.h
-math/TransformedFunctionR2R3.h
-math/Triangle.h
-math/WeightedAverage.h
-math/statistics/measures.h
-math/statistics/BoxPlot.h
-math/statistics/Histogram.h
-Util/xml/tinyxml2.h
-Util/json/json.hpp
-Util/json/eigen_conversion.h
-Util/json/eigen_conversion.hpp
+
+ AbstractFactoryMethod.h
+ BoundingBox.h
+ KinematicChain.h
+ ManipulationObject.h
+ MathTools.h
+ Obstacle.h
+ Primitive.h
+ Random.h
+ Robot.h
+ RobotConfig.h
+ RobotFactory.h
+ RobotNodeSet.h
+ RuntimeEnvironment.h
+ Scene.h
+ SceneObject.h
+ SceneObjectSet.h
+ SphereApproximator.h
+ Trajectory.h
+ VirtualRobot.h
+ VirtualRobotException.h
+ VirtualRobotImportExport.h
+ VirtualRobotTest.h
+
+ CollisionDetection/CollisionChecker.h
+ CollisionDetection/CollisionModel.h
+ CollisionDetection/CDManager.h
+ CollisionDetection/CollisionModelImplementation.h
+ CollisionDetection/CollisionCheckerImplementation.h
+
+ Compression/CompressionBZip2.h
+ Compression/CompressionRLE.h
+
+ DataStructures/nanoflann.hpp
+ DataStructures/KdTreePointCloud.h
+
+ EndEffector/EndEffector.h
+ EndEffector/EndEffectorActor.h
+
+ Grasping/Grasp.h
+ Grasping/GraspSet.h
+ Grasping/BasicGraspQualityMeasure.h
+
+ IK/AdvancedIKSolver.h
+ IK/CoMIK.h
+ IK/ConstrainedHierarchicalIK.h
+ IK/ConstrainedIK.h
+ IK/ConstrainedStackedIK.h
+ IK/Constraint.h
+ IK/DifferentialIK.h
+ IK/FeetPosture.h
+ IK/GazeIK.h
+ IK/GenericIKSolver.h
+ IK/HierarchicalIK.h
+ IK/HierarchicalIKSolver.h
+ IK/IKSolver.h
+ IK/JacobiProvider.h
+ IK/JointLimitAvoidanceJacobi.h
+ IK/PoseQualityExtendedManipulability.h
+ IK/PoseQualityManipulability.h
+ IK/PoseQualityMeasurement.h
+ IK/StackedIK.h
+ IK/SupportPolygon.h
+ IK/constraints/BalanceConstraint.h
+ IK/constraints/CoMConstraint.h
+ IK/constraints/CollisionCheckConstraint.h
+ IK/constraints/JointLimitAvoidanceConstraint.h
+ IK/constraints/OrientationConstraint.h
+ IK/constraints/PoseConstraint.h
+ IK/constraints/PositionConstraint.h
+ IK/constraints/ReferenceConfigurationConstraint.h
+ IK/constraints/TSRConstraint.h
+
+ Import/RobotImporterFactory.h
+ Import/SimoxXMLFactory.h
+ Import/MeshImport/STLReader.h
+
+ math/AbstractFunctionR1Ori.h
+ math/AbstractFunctionR1R2.h
+ math/AbstractFunctionR1R3.h
+ math/AbstractFunctionR1R6.h
+ math/AbstractFunctionR2R3.h
+ math/AbstractFunctionR3R1.h
+ math/Array3D.h
+ math/Bezier.h
+ math/ClampedNormalDistribution.hpp
+ math/CompositeFunctionR1R6.h
+ math/Contact.h
+ math/ContactList.h
+ math/DataR3R1.h
+ math/DataR3R2.h
+ math/FitPlane.h
+ math/GaussianImplicitSurface3D.h
+ math/GaussianImplicitSurface3DCombined.h
+ math/GaussianImplicitSurface3DNormals.h
+ math/GaussianObjectModel.h
+ math/GaussianObjectModelNormals.h
+ math/Grid3D.h
+ math/GridCacheFloat3.h
+ math/Helpers.h
+ math/ImplicitObjectModel.cpp
+ math/ImplicitPlane.h
+ math/Index3.h
+ math/Kernels.h
+ math/Line.h
+ math/LineR2.h
+ math/LineStrip.h
+ math/LineStripR1RM.h
+ math/LinearContinuedBezier.h
+ math/LinearInterpolatedOrientation.h
+ math/MarchingCubes.h
+ math/MathForwardDefinitions.h
+ math/Plane.h
+ math/Primitive.h
+ math/SimpleAbstractFunctionR1Ori.h
+ math/SimpleAbstractFunctionR1R3.h
+ math/SimpleAbstractFunctionR1R6.h
+ math/SimpleAbstractFunctionR2R3.h
+ math/SimpleAbstractFunctionR3R1.h
+ math/SimpleAbstractFunctionRNRM.h
+ math/TransformedFunctionR1R3.h
+ math/TransformedFunctionR2R3.h
+ math/Triangle.h
+ math/WeightedAverage.h
+ math/statistics/measures.h
+ math/statistics/BoxPlot.h
+ math/statistics/Histogram.h
+
+ MJCF/Document.h
+ MJCF/elements.h
+ MJCF/elements/has_member.hpp
+ MJCF/elements/core/AnyElement.h
+ MJCF/elements/core/Attribute.h
+ MJCF/elements/core/const_aware_ptr.hpp
+ MJCF/elements/core/Element.h
+ MJCF/elements/core/exceptions.h
+ MJCF/elements/core/mjcf_utils.h
+ MJCF/elements/core/Visitor.h
+ MJCF/elements/types/actuator.h
+ MJCF/elements/types/asset.h
+ MJCF/elements/types/body.h
+ MJCF/elements/types/compiler.h
+ MJCF/elements/types/contact.h
+ MJCF/elements/types/custom.h
+ MJCF/elements/types/default.h
+ MJCF/elements/types/equality.h
+ MJCF/elements/types/keyframe.h
+ MJCF/elements/types/meta.h
+ MJCF/elements/types/option.h
+ MJCF/elements/types/sensor.h
+ MJCF/elements/types/size.h
+ MJCF/elements/types/statistic.h
+ MJCF/elements/types/tendon.h
+ MJCF/elements/types/visual.h
+ MJCF/visitors.h
+ MJCF/visitors/Collector.h
+
+ Nodes/CameraSensor.h
+ Nodes/CameraSensorFactory.h
+ Nodes/ConditionedLock.h
+ Nodes/ForceTorqueSensor.h
+ Nodes/ForceTorqueSensorFactory.h
+ Nodes/PositionSensor.h
+ Nodes/PositionSensorFactory.h
+ Nodes/RobotNode.h
+ Nodes/RobotNodeActuator.h
+ Nodes/RobotNodeFactory.h
+ Nodes/RobotNodeFixed.h
+ Nodes/RobotNodeFixedFactory.h
+ Nodes/RobotNodePrismatic.h
+ Nodes/RobotNodePrismaticFactory.h
+ Nodes/RobotNodeRevolute.h
+ Nodes/RobotNodeRevoluteFactory.h
+ Nodes/Sensor.h
+ Nodes/SensorFactory.h
+
+ TimeOptimalTrajectory/Path.h
+ TimeOptimalTrajectory/TimeOptimalTrajectory.h
+
+ Tools/Gravity.h
+
+ Transformation/DHParameter.h
+
+ Util/json/eigen_conversion.h
+ Util/json/eigen_conversion.hpp
+ Util/json/json.hpp
+ Util/xml/tinyxml2.h
+
+ Visualization/ColorMap.h
+ Visualization/TriMeshModel.h
+ Visualization/TriMeshUtils.h
+ Visualization/Visualization.h
+ Visualization/VisualizationFactory.h
+ Visualization/VisualizationNode.h
+
+ Workspace/Manipulability.h
+ Workspace/Reachability.h
+ Workspace/VoxelTree6D.hpp
+ Workspace/VoxelTree6DElement.hpp
+ Workspace/VoxelTreeND.hpp
+ Workspace/VoxelTreeNDElement.hpp
+ Workspace/WorkspaceData.h
+ Workspace/WorkspaceDataArray.h
+ Workspace/WorkspaceGrid.h
+ Workspace/WorkspaceRepresentation.h
+ #Workspace/ReachabilityInversion/InverseReachability.h
+ #Workspace/ReachabilityInversion/OrientedWorkspaceGrid.h
+ #Workspace/ReachabilityInversion/ReachabilityProcessor.h
+ #Workspace/ReachabilityInversion/RobotPlacementIK.h
+ #Workspace/ReachabilityInversion/RobotPlacementTrajectoryIK.h
+
+ XML/BaseIO.h
+ XML/FileIO.h
+ XML/ObjectIO.h
+ XML/RobotIO.h
+ XML/SceneIO.h
+ XML/mujoco/MasslessBodySanitizer.h
+ XML/mujoco/MujocoIO.h
+
)
if (Simox_USE_RBDL AND RBDL_FOUND)
diff --git a/VirtualRobot/MJCF/Document.cpp b/VirtualRobot/MJCF/Document.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a69b1066ac20c1b2d68f0f4902fa1f48b48fd5d4
--- /dev/null
+++ b/VirtualRobot/MJCF/Document.cpp
@@ -0,0 +1,111 @@
+#include "Document.h"
+
+#include "elements/core/exceptions.h"
+
+
+namespace mjcf
+{
+
+
+Document::Document()
+{
+ // create root element
+ tinyxml2::XMLElement* xml = doc.NewElement(MujocoRoot::tag.c_str());
+ doc.InsertEndChild(xml);
+ root.reset(new MujocoRoot(this, xml));
+}
+
+void Document::loadFile(const std::string& fileName)
+{
+ tinyxml2::XMLError error = doc.LoadFile(fileName.c_str());
+ if (error != tinyxml2::XML_SUCCESS)
+ {
+ throw MjcfIOError(doc.ErrorStr());
+ }
+}
+
+void Document::saveFile(const std::string& fileName)
+{
+ tinyxml2::XMLError error = doc.SaveFile(fileName.c_str());
+ if (error != tinyxml2::XML_SUCCESS)
+ {
+ throw MjcfIOError(doc.ErrorStr());
+ }
+}
+
+void Document::deepCopyFrom(const Document& source)
+{
+ source.doc.DeepCopy(&this->doc);
+}
+
+void Document::print(std::ostream& os) const
+{
+ tinyxml2::XMLPrinter printer;
+ doc.Print(&printer);
+ os << printer.CStr();
+}
+
+
+
+std::string Document::getModelName() const
+{
+ if (root->isAttributeSet("model"))
+ {
+ return root->getAttribute("model");
+ }
+ else
+ {
+ return "";
+ }
+}
+void Document::setModelName(const std::string& name)
+{
+ root->setAttribute("model", name);
+}
+
+Include Document::addInclude(const std::string& filename)
+{
+ Include include = root->addChild<Include>();
+ include.file = filename;
+ return include;
+}
+
+void Document::setNewElementClass(const std::string& className, bool excludeBody)
+{
+ this->newElementClass = className;
+ this->newElementClassExcludeBody = excludeBody;
+
+ if (!className.empty() && !default_().hasChild<DefaultClass>("class", className))
+ {
+ default_().addClass(className);
+ }
+}
+
+
+float Document::getFloatCompPrecision() const
+{
+ return floatCompPrecision;
+}
+
+void Document::setFloatCompPrecision(float value)
+{
+ floatCompPrecision = value;
+}
+
+float Document::getDummyMass() const
+{
+ return dummyMass;
+}
+
+void Document::setDummyMass(float value)
+{
+ dummyMass = value;
+}
+
+std::ostream& operator<<(std::ostream& os, const Document& rhs)
+{
+ rhs.print(os);
+ return os;
+}
+
+}
diff --git a/VirtualRobot/MJCF/Document.h b/VirtualRobot/MJCF/Document.h
new file mode 100644
index 0000000000000000000000000000000000000000..1be06a178fec53bb9484ab1096cd91924e097428
--- /dev/null
+++ b/VirtualRobot/MJCF/Document.h
@@ -0,0 +1,231 @@
+#pragma once
+
+#include <iostream>
+#include <memory>
+#include <set>
+
+#include <Eigen/Eigen>
+
+#include <VirtualRobot/Util/xml/tinyxml2.h>
+
+#include "elements.h"
+
+
+namespace mjcf
+{
+
+ /**
+ * @brief A MJCF (Mujoco XML) document.
+ */
+ class Document
+ {
+
+ public:
+
+ /// Constructor.
+ Document();
+
+ /// Set the precision for float comparison.
+ float getFloatCompPrecision() const;
+ /// Set the precision for float comparison (used e.g. when comparing
+ /// to zero or identity).
+ void setFloatCompPrecision(float value);
+ /// Get the mass of dummy inertial elements.
+ float getDummyMass() const;
+ /// Set the mass of dummy inertial elements.
+ void setDummyMass(float value);
+
+ /// Load from an MJCF file.
+ void loadFile(const std::string& fileName);
+ /// Save to an MJCF file.
+ void saveFile(const std::string& fileName);
+
+ /// Make a deep copy of source to this.
+ void deepCopyFrom(const Document& source);
+
+ void print(std::ostream& os = std::cout) const;
+
+ std::string getModelName() const;
+ /// Set the name of the Mujoco model.
+ void setModelName(const std::string& name);
+
+
+ // Section elements (children of top-level 'mujoco' element).
+ CompilerSection compiler() { return section<CompilerSection>(); }
+ OptionSection option() { return section<OptionSection>(); }
+ SizeSection size() { return section<SizeSection>(); }
+ VisualSection visual() { return section<VisualSection>(); }
+ StatisticSection statistic() { return section<StatisticSection>(); }
+ DefaultSection default_() { return section<DefaultSection>(); }
+ AssetSection asset() { return section<AssetSection>(); }
+ Worldbody worldbody() { return section<Worldbody>(); }
+ ContactSection contact() { return section<ContactSection>(); }
+ EqualitySection equality() { return section<EqualitySection>(); }
+ TendonSection tendon() { return section<TendonSection>(); }
+ ActuatorSection actuator() { return section<ActuatorSection>(); }
+ SensorSection sensor() { return section<SensorSection>(); }
+ KeyframeSection keyframe() { return section<KeyframeSection>(); }
+
+ Include addInclude(const std::string& filename);
+
+ /**
+ * @brief Set the class attribute of all new applicable elements
+ * (after calling this method). Pass an empty string to disable class attributes.
+ * @param excludeBody If true (default), the class will not be set on
+ * new body elements and its children (inertial, joint, ...).
+ * They should be the childclass attribute of the robot root body.
+ */
+ void setNewElementClass(const std::string& className, bool excludeBody = true);
+
+
+ template <class Derived>
+ friend class Element;
+
+
+ private:
+
+ // METHODS
+
+ /**
+ * @brief Add a new element to a parent.
+ * @param className If not empty, set the class attribute of the new element.
+ * @param first If true, will be inserted as first, otherweise at end (default)
+ */
+ template <class ElementD, class ParentD>
+ ElementD createElement(Element<ParentD> parent, const std::string& className = "", bool front = false);
+
+ /// Return the first child element of parent with the given element name.
+ /// If it does not exist, create it.
+ template <class ElementD, class ParentD>
+ ElementD getOrCreateElement(Element<ParentD>& parent);
+
+ /// Gets the section element (child of root element). If it does not exist, it is created.
+ template <class SectionD>
+ SectionD section();
+
+ /// Prints the document to os.
+ friend std::ostream& operator<<(std::ostream& os, const Document& rhs);
+
+
+ private:
+
+ // ATTRIBUTES
+
+ /// The document.
+ tinyxml2::XMLDocument doc;
+
+ /// The "mujoco" root element.
+ std::unique_ptr<MujocoRoot> root;
+
+
+ /// Precision when comparing floats (e.g. with zero).
+ float floatCompPrecision = 1e-6f;
+ /// Mass used for dummy inertial elements.
+ float dummyMass = 0.0001f;
+
+ /// The class added to new elements, if applicable.
+ std::string newElementClass = "";
+ /// Indicate whether body elements and their children shall be
+ /// exluded from setting the class attribute.
+ bool newElementClassExcludeBody = true;
+
+
+ };
+
+ using DocumentPtr = std::unique_ptr<Document>;
+
+
+
+#include "elements/has_member.hpp"
+
+ define_has_member(class_);
+
+ template <class ElementD, class ParentD>
+ ElementD Document::createElement(Element<ParentD> parent, const std::string& className, bool front)
+ {
+ ElementD element(this, doc.NewElement(ElementD::Derived::tag.c_str()));
+
+ auto applyNewElementClass = [this]()
+ {
+ bool isSet = !newElementClass.empty();
+ bool hasClass = has_member(ElementD, class_);
+ bool excludeBecauseBody = std::is_same<ParentD, Body>() && newElementClassExcludeBody;
+ // body itself does not have a class attribute
+
+ bool inDefaultClass = std::is_same<ParentD, DefaultClass>();
+
+ return isSet // must not be empty
+ && hasClass // must have class attribute
+ && !excludeBecauseBody // not excluded because of body exclusion
+ && !inDefaultClass; // not part of default class
+ };
+
+
+ if (!className.empty())
+ {
+ element.setAttribute("class", className);
+ }
+ else if (applyNewElementClass())
+ {
+ element.setAttribute("class", newElementClass);
+ }
+
+ if (front)
+ {
+ parent.insertFirstChild(element);
+ }
+ else
+ {
+ parent.insertEndChild(element);
+ }
+ return element;
+ }
+
+// cleanup macros by has_member.hpp
+#undef define_has_member
+#undef has_member
+
+
+ template <class ElementD, class ParentD>
+ ElementD Document::getOrCreateElement(Element<ParentD>& parent)
+ {
+ ElementD element = parent.template firstChild<ElementD>();
+
+ if (!element)
+ {
+ element = createElement<ElementD>(parent);
+ }
+
+ return element;
+ }
+
+ template<class SectionT>
+ SectionT Document::section()
+ {
+ return getOrCreateElement<SectionT>(*root);
+ }
+
+ // Implementation of Element::createNewElement, which depends on the
+ // definition of Document.
+ template <class D>
+ template <class ParentD, class ElementD>
+ ElementD Element<D>::createElement(Element<ParentD> parent, const std::string& className)
+ {
+ return _document->createElement<ElementD, ParentD>(parent, className);
+ }
+
+ template <class D>
+ void Element<D>::insertComment(const std::string& text, bool front)
+ {
+ tinyxml2::XMLComment* comment = _document->doc.NewComment(text.c_str());
+ if (front)
+ {
+ _element->InsertFirstChild(comment);
+ }
+ else
+ {
+ _element->InsertEndChild(comment);
+ }
+ }
+
+}
diff --git a/VirtualRobot/MJCF/elements.h b/VirtualRobot/MJCF/elements.h
new file mode 100644
index 0000000000000000000000000000000000000000..4f2ec6f949cb8dc5b0775033b5967be91abc2a21
--- /dev/null
+++ b/VirtualRobot/MJCF/elements.h
@@ -0,0 +1,66 @@
+#pragma once
+
+#include "elements/core/AnyElement.h"
+#include "elements/types/actuator.h"
+#include "elements/types/asset.h"
+#include "elements/types/body.h"
+#include "elements/types/compiler.h"
+#include "elements/types/contact.h"
+#include "elements/types/default.h"
+#include "elements/types/equality.h"
+#include "elements/types/keyframe.h"
+#include "elements/types/meta.h"
+#include "elements/types/option.h"
+#include "elements/types/sensor.h"
+#include "elements/types/size.h"
+#include "elements/types/statistic.h"
+#include "elements/types/tendon.h"
+#include "elements/types/visual.h"
+
+
+#undef mjcf_AttributeFullNoDef
+#undef mjcf_AttributeFullDef
+
+#undef mjcf_AttributeReq
+#undef mjcf_AttributeOpt
+#undef mjcf_AttributeDef
+
+#undef mjcf_AttributeKwReq
+#undef mjcf_AttributeKwOpt
+#undef mjcf_AttributeKwDef
+
+#undef mjcf_BoolAttributeReq
+#undef mjcf_BoolAttributeOpt
+#undef mjcf_BoolAttributeDef
+
+#undef mjcf_IntAttributeReq
+#undef mjcf_IntAttributeOpt
+#undef mjcf_IntAttributeDef
+
+#undef mjcf_FloatAttributeReq
+#undef mjcf_FloatAttributeOpt
+#undef mjcf_FloatAttributeDef
+
+#undef mjcf_StringAttributeReq
+#undef mjcf_StringAttributeOpt
+#undef mjcf_StringAttributeDef
+#undef mjcf_StringAttributeDefEmpty
+
+#undef mjcf_Vector2fAttributeDef
+#undef mjcf_Vector3fAttributeReq
+#undef mjcf_Vector3fAttributeOpt
+#undef mjcf_Vector3fAttributeDef
+
+#undef mjcf_NameAttribute
+#undef mjcf_ClassAttribute
+#undef mjcf_GroupAttribute
+
+#undef mjcf_RgbAttributeDef
+#undef mjcf_RgbaAttributeDef
+
+#undef mjcf_PosAttribute
+#undef mjcf_QuatAttribute
+#undef mjcf_PoseAttributes
+
+#undef mjcf_SolimpAttribute
+#undef mjcf_SolrefAttribute
diff --git a/VirtualRobot/MJCF/elements/core/AnyElement.cpp b/VirtualRobot/MJCF/elements/core/AnyElement.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..22ee462c4585e9da9abf85cee6aafef28a58d437
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/AnyElement.cpp
@@ -0,0 +1,7 @@
+#include "AnyElement.h"
+
+namespace mjcf
+{
+ const std::string AnyElement::tag = "";
+}
+
diff --git a/VirtualRobot/MJCF/elements/core/AnyElement.h b/VirtualRobot/MJCF/elements/core/AnyElement.h
new file mode 100644
index 0000000000000000000000000000000000000000..a16af2cc7f50597116d4fb515dec7282186f6761
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/AnyElement.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include "Element.h"
+
+
+namespace mjcf
+{
+
+ /**
+ * @brief An element that can hold any type.
+ */
+ class AnyElement : public Element<AnyElement>
+ {
+ public:
+
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(AnyElement)
+
+ /// Construct from any other element type.
+ template <class OtherDerived>
+ AnyElement(const Element<OtherDerived>& other) :
+ AnyElement(other.template reinterpret<AnyElement>())
+ {}
+
+
+ /// Indicate whether *this is an element of type OtherDerived.
+ template <typename OtherDerived>
+ bool is() const { return isElement<OtherDerived>(); }
+
+ /// Get *this as element of type OtherDerived.
+ /// Should only be called if `is()` is true.
+ template <typename OtherDerived>
+ OtherDerived as() { return { document(), element() }; }
+
+ template <typename OtherDerived>
+ const OtherDerived as() const { return reinterpret<OtherDerived>(); }
+
+
+ /// Return the actual tag.
+ std::string actualTag() const { return element()->Value(); }
+
+ };
+
+}
diff --git a/VirtualRobot/MJCF/elements/core/Attribute.cpp b/VirtualRobot/MJCF/elements/core/Attribute.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cdab83817271e1f57cb0d0f54b79d33a1c3c5cd6
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/Attribute.cpp
@@ -0,0 +1,14 @@
+#include "Attribute.h"
+
+
+namespace mjcf
+{
+
+Eigen::Vector5f mjcf_solimpDefault()
+{
+ Eigen::Vector5f v;
+ v << 0.9f, 0.95f, 0.001f, 0.5, 2;
+ return v;
+}
+
+}
diff --git a/VirtualRobot/MJCF/elements/core/Attribute.h b/VirtualRobot/MJCF/elements/core/Attribute.h
new file mode 100644
index 0000000000000000000000000000000000000000..df07ca70541f15945f6f9fea737480e57087f73a
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/Attribute.h
@@ -0,0 +1,346 @@
+#pragma once
+
+#include <VirtualRobot/math/Helpers.h>
+
+#include "Element.h"
+
+
+namespace mjcf
+{
+
+ /**
+ * @class Base class for an attribute of an Element<Derived>.
+ * Stores a pointer to the owning element.
+ */
+ template <class Derived>
+ class AttributeBase
+ {
+ public:
+
+ AttributeBase() : _owner(nullptr)
+ {}
+
+ /// Constructor.
+ /// @param owner the owning element (Element instance of which this is a member).
+ AttributeBase(Element<Derived>& owner) : _owner(&owner)
+ {}
+
+
+ /* _owner must always point to the element this was constructed with.
+ * We therefore delete copy and move constructors (otherwise, the owner
+ * will be unavailable. In addition, _owner is constant, so it is not
+ * changed by copy or move operators. */
+
+ /// Deleted copy constructor.
+ AttributeBase(const AttributeBase<Derived>& other) = delete;
+ /// Deleted move constructor.
+ AttributeBase(AttributeBase<Derived>&& other) = delete;
+
+
+ protected:
+ /// Get the owning element.
+ Element<Derived>& owner() { return *_owner; }
+ const Element<Derived>& owner() const { return *_owner; }
+
+ private:
+ /// The owning element (Element instance of which this is a member).
+ /// It must not change after construction, and thus is const-qualified.
+ Element<Derived>* const _owner;
+
+ };
+
+
+
+ /**
+ * @class An attribute of an Element<Derived> of type AttrT.
+ */
+ template <class Derived, typename AttrT>
+ class Attribute : public AttributeBase<Derived>
+ {
+ public:
+
+ /// Constructor.
+ Attribute(Element<Derived>& element, const std::string& name)
+ : AttributeBase<Derived>(element), name(name)
+ {}
+
+ /// Get the attribute value (or the default value if the attribute does not exist).
+ virtual AttrT get() const = 0;
+ /// Set the attribute value.
+ void set(const AttrT& value)
+ {
+ this->owner().template setAttribute<AttrT>(name, value);
+ }
+
+ /// Conversion operator to AttrT for reading.
+ operator AttrT() const
+ {
+ return this->get();
+ }
+
+ /// Copy assignment operator for writing an attribute value.
+ Attribute& operator=(const AttrT& value)
+ {
+ this->set(value);
+ return *this;
+ }
+
+ /// Move assignment operator for writing an attribute value.
+ Attribute& operator=(AttrT&& value)
+ {
+ this->set(value);
+ return *this;
+ }
+
+ /// Copy assignment operator for writing an attribute value.
+ template <class OtherDerived, class OtherAttrT>
+ Attribute& operator=(const Attribute<OtherDerived, OtherAttrT>& attribute)
+ {
+ this->set(attribute.get());
+ return *this;
+ }
+
+ /// Copy assignment operator for writing an attribute value.
+ template <class OtherDerived, class OtherAttrT>
+ Attribute& operator=(Attribute<OtherDerived, OtherAttrT>&& attribute)
+ {
+ this->set(attribute.get());
+ return *this;
+ }
+
+ /* Equivalent operators should be called implicitly due to conversion operators.
+ bool operator==(const AttrT& rhs) { return get() == rhs; }
+ bool operator!=(const AttrT& rhs) { return get() != rhs; }
+ */
+ template <class OtherDerived, class OtherAttrT>
+ bool operator==(const Attribute<OtherDerived, OtherAttrT>& rhs) { return get() == rhs.get(); }
+ template <class OtherDerived, class OtherAttrT>
+ bool operator!=(const Attribute<OtherDerived, OtherAttrT>& rhs) { return get() != rhs.get(); }
+
+ /// Indicate whether the attribute is required (implies no default).
+ virtual bool isRequired() const = 0;
+ /// Indicate whether the attribute is optional or has a default.
+ bool isOptional() const { return !isRequired(); }
+ /// Indicate whether the attribute has a default (implies optional).
+ virtual bool hasDefault() const = 0;
+
+ /// Indicate whether the attribute is set.
+ bool isSet() const { return this->owner().isAttributeSet(this->name); }
+
+
+ protected:
+
+ std::string name; ///< The attribute name.
+
+ };
+
+ template <class Derived, typename AttrT>
+ std::ostream& operator<<(std::ostream& os, const Attribute<Derived, AttrT>& rhs)
+ {
+ os << rhs.get();
+ return os;
+ }
+
+
+ /**
+ * @class An attribute of an Element<Derived> of type AttrT
+ * that is required or optional and has no default value.
+ */
+ template <class Derived, typename AttrT>
+ class AttributeNoDef : public Attribute<Derived, AttrT>
+ {
+ using Base = Attribute<Derived, AttrT>;
+ public:
+
+ /// Construct an required or optional attribute without a default.
+ AttributeNoDef(Element<Derived>& element, const std::string& name, bool required)
+ : Base(element, name), required(required)
+ {}
+
+ AttrT get() const override
+ {
+ return this->owner().template getAttribute<AttrT>(this->name);
+ }
+
+ bool isRequired() const override { return required; }
+ bool hasDefault() const override { return false; }
+
+ using Base::operator=;
+
+
+ private:
+
+ bool required; ///< Whether the attribute is optional.
+ };
+
+
+
+ /**
+ * @class An attribute of an Element<Derived> of type AttrT
+ * that is optional and has a default value.
+ */
+ template <class Derived, typename AttrT>
+ class AttributeDef : public Attribute<Derived, AttrT>
+ {
+ using Base = Attribute<Derived, AttrT>;
+
+ public:
+
+ /// Construct an optional attribute with a default.
+ AttributeDef(Element<Derived>& element, const std::string& name, const AttrT& defaultValue)
+ : Base(element, name), defaultValue(defaultValue)
+ {}
+
+ AttrT get() const override
+ {
+ return this->owner().template getAttribute<AttrT>(this->name, this->defaultValue);
+ }
+
+ bool isRequired() const override { return false; }
+ bool hasDefault() const override { return true; }
+
+ using Base::operator=;
+
+
+ private:
+
+ AttrT defaultValue; ///< The default value.
+
+ };
+
+
+// Macro for a requried or optional Attribute in an Element<Derived> of type AttrT
+// with potentially different memberName and attrName
+#define mjcf_AttributeFullNoDef(Derived, AttrT, memberName, attrName, required) \
+ ::mjcf::AttributeNoDef<Derived, AttrT> memberName {*this, attrName, required}
+
+// Macro for an optional Attribute in an Element<Derived> of type AttrT
+// with potentially different memberName and attrName.
+#define mjcf_AttributeFullDef(Derived, AttrT, memberName, attrName, defaultValue) \
+ ::mjcf::AttributeDef<Derived, AttrT> memberName {*this, attrName, defaultValue}
+
+
+// A requried Attribute in an Element<Derived> of type AttrT.
+#define mjcf_AttributeReq(Derived, AttrT, name) \
+ mjcf_AttributeFullNoDef(Derived, AttrT, name, #name, true)
+
+#define mjcf_AttributeOpt(Derived, AttrT, name) \
+ mjcf_AttributeFullNoDef(Derived, AttrT, name, #name, false)
+
+// An optional Attribute in an Element<Derived> of type AttrT.
+#define mjcf_AttributeDef(Derived, AttrT, name, defaultValue) \
+ mjcf_AttributeFullDef(Derived, AttrT, name, #name, defaultValue)
+
+
+// Attributes with a name that is a keyword (e.g. "class").
+#define mjcf_AttributeKwReq(Derived, AttrT, name) \
+ mjcf_AttributeFullNoDef(Derived, AttrT, name ## _, #name, true)
+
+#define mjcf_AttributeKwOpt(Derived, AttrT, name) \
+ mjcf_AttributeFullNoDef(Derived, AttrT, name ## _, #name, false)
+
+#define mjcf_AttributeKwDef(Derived, AttrT, name, defaultValue) \
+ mjcf_AttributeFullDef(Derived, AttrT, name ## _, #name, defaultValue)
+
+
+// bool shorthands
+#define mjcf_BoolAttributeReq(Derived, name) \
+ mjcf_AttributeReq(Derived, bool, name)
+#define mjcf_BoolAttributeOpt(Derived, name) \
+ mjcf_AttributeOpt(Derived, bool, name)
+#define mjcf_BoolAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, bool, name, defaultValue)
+
+// int shorthands
+#define mjcf_IntAttributeReq(Derived, name) \
+ mjcf_AttributeReq(Derived, int, name)
+#define mjcf_IntAttributeOpt(Derived, name) \
+ mjcf_AttributeOpt(Derived, int, name)
+#define mjcf_IntAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, int, name, defaultValue)
+
+
+// float shorthands
+#define mjcf_FloatAttributeReq(Derived, name) \
+ mjcf_AttributeReq(Derived, float, name)
+#define mjcf_FloatAttributeOpt(Derived, name) \
+ mjcf_AttributeOpt(Derived, float, name)
+#define mjcf_FloatAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, float, name, defaultValue)
+
+// string shorthands
+#define mjcf_StringAttributeReq(Derived, memberName) \
+ mjcf_AttributeReq(Derived, std::string, memberName)
+#define mjcf_StringAttributeOpt(Derived, memberName) \
+ mjcf_AttributeOpt(Derived, std::string, memberName)
+#define mjcf_StringAttributeDef(Derived, memberName, defaultValue) \
+ mjcf_AttributeDef(Derived, std::string, memberName, defaultValue)
+
+#define mjcf_StringAttributeDefEmpty(Derived, memberName, defaultValue) \
+ mjcf_StringAttributeDef(Derived, std::string, memberName, "")
+
+
+// vector shorthands
+
+#define mjcf_Vector2fAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, Eigen::Vector2f, name, defaultValue)
+
+#define mjcf_Vector3fAttributeReq(Derived, name) \
+ mjcf_AttributeReq(Derived, Eigen::Vector3f, name)
+#define mjcf_Vector3fAttributeOpt(Derived, name) \
+ mjcf_AttributeOpt(Derived, Eigen::Vector3f, name)
+#define mjcf_Vector3fAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, Eigen::Vector3f, name, defaultValue)
+
+
+// name
+#define mjcf_NameAttribute(Derived) \
+ mjcf_StringAttributeOpt(Derived, name)
+
+// class
+#define mjcf_ClassAttribute(Derived) \
+ mjcf_AttributeKwReq(Derived, std::string, class)
+
+// group
+#define mjcf_GroupAttribute(Derived) \
+ mjcf_IntAttributeDef(Derived, group, 0)
+
+
+// RGB
+#define mjcf_RgbAttributeDef(Derived, name, defaultValue) \
+ mjcf_Vector3fAttributeDef(Derived, name, defaultValue)
+
+#define mjcf_RgbaAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, Eigen::Vector4f, name, defaultValue)
+
+
+// pos
+#define mjcf_PosAttribute(Derived) \
+ mjcf_Vector3fAttributeDef(Derived, pos, Eigen::Vector3f::Zero())
+
+// quat
+#define mjcf_QuatAttribute(Derived) \
+ mjcf_AttributeDef(Derived, Eigen::Quaternionf, quat, Eigen::Quaternionf::Identity())
+
+// pose = pos + quat
+#define mjcf_PoseAttributes(Derived) \
+ Eigen::Matrix4f getPose() const \
+ { return math::Helpers::Pose(pos.get(), quat.get()); } \
+ void setPose(const Eigen::Matrix4f& p) \
+ { pos = math::Helpers::Position(p); \
+ quat = Eigen::Quaternionf(math::Helpers::Orientation(p)); } \
+ mjcf_PosAttribute(Derived); \
+ mjcf_QuatAttribute(Derived)
+
+
+Eigen::Vector5f mjcf_solimpDefault();
+
+// solref
+#define mjcf_SolrefAttribute(Derived) \
+ mjcf_AttributeDef(Derived, Eigen::Vector2f, solref, Eigen::Vector2f(0.02f, 1.f))
+// solimp
+#define mjcf_SolimpAttribute(Derived) \
+ mjcf_AttributeDef(Derived, Eigen::Vector5f, solimp, ::mjcf::mjcf_solimpDefault())
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/core/Element.cpp b/VirtualRobot/MJCF/elements/core/Element.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..114513eaf07a443165d2a50e46b9f8c62dd79040
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/Element.cpp
@@ -0,0 +1,2 @@
+#include "Element.h"
+
diff --git a/VirtualRobot/MJCF/elements/core/Element.h b/VirtualRobot/MJCF/elements/core/Element.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc7f94a7afa8648235e90c2e5686712835f814d0
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/Element.h
@@ -0,0 +1,514 @@
+#pragma once
+
+#include <VirtualRobot/Util/xml/tinyxml2.h>
+
+#include "const_aware_ptr.hpp"
+#include "exceptions.h"
+#include "mjcf_utils.h"
+#include "Visitor.h"
+
+
+namespace mjcf
+{
+
+ // Forward declaration.
+ class Document;
+
+ template <class _Derived>
+ class Element
+ {
+ public:
+
+ using Derived = _Derived;
+
+ // Make all Element<*> friends of each other.
+ template <class OtherDerived> friend class Element;
+
+
+ public:
+
+ // CONSTRUCTORS
+
+ /// Empty constructor.
+ Element() {}
+
+ /**
+ * @brief Construct and element with given document and underlying (tinyxml2) element.
+ * @throws error::InvalidElementTag if `element`'s tag does not match `Derived::tag`.
+ */
+ Element(Document* document, tinyxml2::XMLElement* element);
+
+ protected:
+
+ /**
+ * Only allow inheriting classes to construct with a const pointers,
+ * since const-ness is effectively cast away on construction, and offering
+ * it publicly would create a false sense of security at this point.
+ */
+ Element(const Document* document, const tinyxml2::XMLElement* element);
+
+
+ public:
+
+ // TYPE CHECKING
+
+ /// Indicate whether this is an element of the given type.
+ template <class OtherT>
+ static bool isSame() { return std::is_same<Derived, typename OtherT::Derived>(); }
+
+
+ /// Indicate whether the underlying element's tag is OtherT's tag.
+ /// This should be true at virtually all times.
+ template <class OtherT>
+ bool isElement() const { return OtherT::tag.empty() || std::string(_element->Value()) == OtherT::tag; }
+
+
+ // ATTRIBUTES
+
+ /// Indicate whether this element has an attribute with the given name.
+ bool isAttributeSet(const std::string& attrName) const;
+
+ /// Get attribute value.
+ template <typename AttrT = std::string>
+ AttrT getAttribute(const std::string& name) const;
+ /// Get the attribute value, or the default value if it does not exist.
+ template <typename AttrT = std::string>
+ AttrT getAttribute(const std::string& name, const AttrT& defaultValue) const;
+ /// Set the attribute.
+ template <typename AttrT>
+ void setAttribute(const std::string& name, const AttrT& value);
+
+ std::vector<std::string> getSetAttributeNames() const;
+
+
+ // CHILDREN
+
+ /// Indicate whether this element has any children.
+ bool hasChildren() const;
+
+ /// Indicate whether there is a child of the given type.
+ template <class OtherDerived>
+ bool hasChild() const;
+ /// Indicate whether there is a child for which the given predicate is true.
+ template <class OtherDerived>
+ bool hasChild(std::function<bool(OtherDerived)> predicate) const;
+ /// Indicate whether there is a child with the given attribute value.
+ template <class OtherDerived>
+ bool hasChild(const std::string& attrName, const std::string& attrValue) const;
+
+ /// Get the first child of the given type.
+ template <class OtherDerived>
+ OtherDerived firstChild() const;
+ /// Get the first child of the given type for which the given predicate is true.
+ template <class OtherDerived>
+ OtherDerived firstChild(std::function<bool(OtherDerived)> predicate) const;
+ /// Get the first child of the given type with the given attribute value.
+ template <class OtherDerived>
+ OtherDerived firstChild(const std::string& attrName, const std::string& attrValue) const;
+
+
+ // SIBLINGS
+
+ /// Get the next sibling element of the given type.
+ template <class OtherDerived>
+ OtherDerived nextSiblingElement() const;
+ /// Get the next sibling element of the given type for which the predicate is true.
+ template <class OtherDerived>
+ OtherDerived nextSiblingElement(std::function<bool(OtherDerived)> predicate) const;
+ /// Get the next sibling element of the given type with the given attribute value.
+ template <class OtherDerived>
+ OtherDerived nextSiblingElement(const std::string& attrName, const std::string& attrValue) const;
+
+
+ // PARENT
+
+ /// Get the parent element.
+ template <class ParentDerived>
+ ParentDerived parent();
+ template <class ParentDerived>
+ const ParentDerived parent() const;
+
+
+ // INSERTION & DELETION
+
+ /// Add a new child of this with of given type.
+ template <class OtherDerived>
+ OtherDerived addChild(const std::string& className = "");
+
+ /// Get the first child of type OtherDerived. If there is none, add one.
+ template <class OtherDerived>
+ OtherDerived getOrCreateChild();
+
+
+ template <class OtherDerived>
+ void insertChild(Element<OtherDerived>& element, bool front = false);
+
+ /// Insert an element at the front.
+ template <class OtherDerived>
+ void insertFirstChild(Element<OtherDerived>& element);
+ /// Insert an element at the end.
+ template <class OtherDerived>
+ void insertEndChild(Element<OtherDerived>& element);
+
+ /// Delete given child from this.
+ template <class OtherDerived>
+ void deleteChild(Element<OtherDerived>& element);
+
+
+ /// Insert a comment as child of *this.
+ void insertComment(const std::string& text, bool front = false);
+
+
+ // VISITING
+
+ /// Accept a hierarchical visit of the elements in *this.
+ /// @see tinyxml2::XMLNode::Accept()
+ bool accept(Visitor& visitor);
+
+
+ // CLONING AND TRANSFORMATION
+
+ /// Create a deep clone of this element.
+ Derived deepClone() const;
+
+ /**
+ * @brief Transform this element to another type.
+ *
+ * Change the underlying element's tag and return the element as the
+ * new type. Using `*this` afterwards can result in unexpected
+ * behaviour (and should in general be avoided), so use this method
+ * with caution!
+ */
+ template <class OtherDerived>
+ OtherDerived transform();
+
+
+ /**
+ * @brief Reinterpret this element as the given element type.
+ * This is only safe from and to `AnyElement`.
+ */
+ // (Ideally, this functionality would only be accessible from AnyElement.
+ // However, giving AnyElement access to other types' document() and element() seems not
+ // to work without making document() and element() public (friend declaration does not
+ // seem to work). This seems to only effect the const methods.
+ template <class OtherDerived>
+ OtherDerived reinterpret() const { return Element<OtherDerived>(_document, _element); }
+
+
+ // OPERATORS
+
+ /// Indicate whether this contains a valid element.
+ operator bool() const { return bool(_element); }
+
+ template <typename Derived>
+ friend std::ostream& operator<<(std::ostream& os, const Element<Derived>& element);
+
+
+ protected:
+
+ Document* document() { return _document; }
+ const Document* document() const { return _document; }
+
+ tinyxml2::XMLElement* element() { return _element; }
+ const tinyxml2::XMLElement* element() const { return _element; }
+
+
+ private:
+
+ /// Get Derived::tag as c-string, or nullptr if Derived::tag is empty (AnyElement).
+ /// The result can be passed to tinyxml2 methods taking element values (tags) as filters.
+ static const char* tag_c_str() { return Derived::tag.empty() ? nullptr : Derived::tag.c_str(); }
+
+ /// Use document to create a new element of type ElementD with given parent.
+ template <class ParentD, class ElementD>
+ ElementD createElement(Element<ParentD> parent, const std::string& className = "");
+
+ void assertElemValueEqualsTag();
+
+ template <class OtherDerived>
+ std::function<bool(OtherDerived)> predicateWithAttrib(
+ const std::string& attrName, const std::string& attrValue) const;
+
+ Document* _document = nullptr;
+ detail::const_aware_ptr<tinyxml2::XMLElement> _element = nullptr;
+
+ };
+
+
+ template <class D>
+ Element<D>::Element(Document* document, tinyxml2::XMLElement* element) :
+ _document(document), _element(element)
+ {
+ assertElemValueEqualsTag();
+ }
+
+ template <class D>
+ Element<D>::Element(const Document* document, const tinyxml2::XMLElement* element) :
+ Element(const_cast<Document*>(document), const_cast<tinyxml2::XMLElement*>(element))
+ {}
+
+ template <class D>
+ bool Element<D>::isAttributeSet(const std::string& attrName) const
+ {
+ return _element->Attribute(attrName.c_str()) != nullptr;
+ }
+
+ template <class D>
+ template <typename AttrT>
+ AttrT Element<D>::getAttribute(const std::string& name) const
+ {
+ const char* attr = _element->Attribute(name.c_str());
+ if (!attr)
+ {
+ throw AttribNotSetError(name);
+ }
+ AttrT value;
+ fromAttr(attr, value);
+ return value;
+ }
+
+ template <class D>
+ template <typename AttrT>
+ AttrT Element<D>::getAttribute(const std::string& name, const AttrT& defaultValue) const
+ {
+ return isAttributeSet(name) ? getAttribute<AttrT>(name) : defaultValue;
+ }
+
+ template <class D>
+ template<typename AttrT>
+ void Element<D>::setAttribute(const std::string& name, const AttrT& value)
+ {
+ std::string attrStr = toAttr(value);
+ _element->SetAttribute(name.c_str(), attrStr.c_str());
+ }
+
+ template <class D>
+ std::vector<std::string> Element<D>::getSetAttributeNames() const
+ {
+ std::vector<std::string> names;
+ for (auto* attr = _element->FirstAttribute(); attr; attr = attr->Next())
+ {
+ names.emplace_back(attr->Name());
+ }
+ return names;
+ }
+
+ template <class D>
+ bool Element<D>::hasChildren() const
+ {
+ return !_element->NoChildren();
+ }
+
+ template <class D>
+ template <class OtherD>
+ bool Element<D>::hasChild() const
+ {
+ return firstChild<OtherD>();
+ }
+
+ template <class D>
+ template <class OtherD>
+ bool Element<D>::hasChild(std::function<bool(OtherD)> predicate) const
+ {
+ return firstChild<OtherD>(predicate);
+ }
+
+ template <class D>
+ template <class OtherD>
+ bool Element<D>::hasChild(const std::string& attrName, const std::string& attrValue) const
+ {
+ return firstChild<OtherD>(attrName, attrValue);
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::firstChild() const
+ {
+ return Element<OtherD>(_document, /*may be null*/
+ _element->FirstChildElement(OtherD::tag_c_str()));
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::firstChild(std::function<bool(OtherD)> predicate) const
+ {
+ for (OtherD child = firstChild<OtherD>(); child;
+ child = child.template nextSiblingElement<OtherD>())
+ {
+ if (predicate(child))
+ {
+ return child;
+ }
+ }
+ return {};
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::firstChild(const std::string& attrName, const std::string& attrValue) const
+ {
+ return firstChild<OtherD>(predicateWithAttrib<OtherD>(attrName, attrValue));
+ }
+
+
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::nextSiblingElement() const
+ {
+ return Element<OtherD>(_document, _element->NextSiblingElement(OtherD::tag_c_str()));
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::nextSiblingElement(std::function<bool (OtherD)> predicate) const
+ {
+ for (OtherD sibling = nextSiblingElement<OtherD>(); sibling;
+ sibling = sibling.template nextSiblingElement<OtherD>())
+ {
+ if (predicate(sibling))
+ {
+ return sibling;
+ }
+ }
+ return {};
+ }
+
+ template <class D>
+ template<class OtherD>
+ OtherD Element<D>::nextSiblingElement(const std::string& attrName, const std::string& attrValue) const
+ {
+ return nextSiblingElement<OtherD>(predicateWithAttrib<OtherD>(attrName, attrValue));
+ }
+
+
+ template <class D>
+ template <class ParentDerived>
+ ParentDerived Element<D>::parent()
+ {
+ return { _document, _element->Parent()->ToElement() };
+ }
+
+ template <class D>
+ template <class ParentDerived>
+ const ParentDerived Element<D>::parent() const
+ {
+ return Element<ParentDerived>(_document, _element->Parent()->ToElement());
+ }
+
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::addChild(const std::string& className)
+ {
+ return createElement<D, OtherD>(*this, className);
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::getOrCreateChild()
+ {
+ OtherD child = firstChild<OtherD>();
+ if (!child)
+ {
+ child = addChild<OtherD>();
+ }
+ return child;
+ }
+
+
+ template <class D>
+ template <class OtherD>
+ void Element<D>::insertChild(Element<OtherD>& element, bool front)
+ {
+ front ? insertFirstChild(element) : insertEndChild(element());
+ }
+
+ template <class D>
+ template <class OtherD>
+ void Element<D>::insertFirstChild(Element<OtherD>& element)
+ {
+ _element->InsertFirstChild(element._element);
+ }
+
+ template <class D>
+ template <class OtherD>
+ void Element<D>::insertEndChild(Element<OtherD>& element)
+ {
+ _element->InsertEndChild(element._element);
+ }
+
+ template <class D>
+ template <class OtherD>
+ void Element<D>::deleteChild(Element<OtherD>& element)
+ {
+ _element->DeleteChild(element._element);
+ }
+
+ template <class D>
+ bool Element<D>::accept(Visitor& visitor)
+ {
+ return _element->Accept(visitor.adapter());
+ }
+
+ template <class D>
+ auto Element<D>::deepClone() const -> Derived
+ {
+ return { _document, _element->DeepClone(nullptr)->ToElement() };
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::transform()
+ {
+ _element->SetValue(OtherD::tag.c_str());
+ return { _document, _element };
+ }
+
+ template <class D>
+ void Element<D>::assertElemValueEqualsTag()
+ {
+ if (_element && !isElement<D>())
+ {
+ throw error::InvalidElementTag(Derived::tag, _element->Value());
+ }
+ }
+
+ template <class D>
+ template <class OtherDerived>
+ std::function<bool(OtherDerived)> Element<D>::predicateWithAttrib(
+ const std::string& attrName, const std::string& attrValue) const
+ {
+ return [&attrName, &attrValue](OtherDerived e)
+ {
+ return e.isAttributeSet(attrName) && e.getAttribute(attrName) == attrValue;
+ };
+ }
+
+ template <class D>
+ std::ostream& operator<<(std::ostream& os, const Element<D>& element)
+ {
+ os << "MJCF Element '" << D::tag << "' (-> " << element._element << ")";
+ return os;
+ }
+
+
+#define mjcf_ElementDerivedConstructorsBase(Base, Derived) \
+ Derived() : Base<Derived>() {} \
+ Derived(Document* document, ::tinyxml2::XMLElement* elem) : Base<Derived>(document, elem) {} \
+ Derived(const Base<Derived>& base) : Base<Derived>(base) {} \
+ Derived(const Derived& other) : Base<Derived>(other) {} \
+ Derived(Derived&& other) : Base<Derived>(other) {} \
+ Derived& operator=(const Base<Derived>& base) \
+ { Base<Derived>::operator=(base); return *this; } \
+ Derived& operator=(const Derived& other) \
+ { Base<Derived>::operator=(other); return *this; } \
+ Derived& operator=(Derived&& other) \
+ { Base<Derived>::operator=(other); return *this; }
+
+
+#define mjcf_ElementDerivedConstructors(Derived) \
+ mjcf_ElementDerivedConstructorsBase(Element, Derived)
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/core/Visitor.cpp b/VirtualRobot/MJCF/elements/core/Visitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..668b442902ef62964c409dbae3dad9c4ce28d847
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/Visitor.cpp
@@ -0,0 +1,43 @@
+#include "Visitor.h"
+
+#include "AnyElement.h"
+
+namespace mjcf
+{
+
+Visitor::Adapter::Adapter(Visitor& owner) :
+ owner(owner)
+{}
+
+bool Visitor::Adapter::VisitEnter(const tinyxml2::XMLElement& element, const tinyxml2::XMLAttribute*)
+{
+ return owner.visitEnter(owner.cast(element));
+}
+
+bool Visitor::Adapter::VisitExit(const tinyxml2::XMLElement& element)
+{
+ return owner.visitExit(owner.cast(element));
+}
+
+Visitor::Visitor(Document& document) : document(document)
+{}
+
+Visitor::Adapter* Visitor::adapter()
+{
+ return &_adapter;
+}
+
+const Visitor::Adapter* Visitor::adapter() const
+{
+ return &_adapter;
+}
+
+const AnyElement Visitor::cast(const tinyxml2::XMLElement& element)
+{
+ // The constructor of Element requires a non-const element pointer.
+ // As AnyElement is passed as const& to the user-provided visit method,
+ // this const cast should be sufficiently safe.
+ return { &document, const_cast<tinyxml2::XMLElement*>(&element) };
+}
+
+}
diff --git a/VirtualRobot/MJCF/elements/core/Visitor.h b/VirtualRobot/MJCF/elements/core/Visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..fa50051feb03ad7fa75a6e0a7588cd59c6936809
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/Visitor.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <VirtualRobot/Util/xml/tinyxml2.h>
+
+
+namespace mjcf
+{
+ class AnyElement;
+ class Document;
+ class Visitor;
+
+
+ class Visitor
+ {
+ public:
+ class Adapter : public tinyxml2::XMLVisitor
+ {
+ public:
+ Adapter(Visitor& owner);
+
+ virtual bool VisitEnter(const tinyxml2::XMLElement& element,
+ const tinyxml2::XMLAttribute*) override;
+ virtual bool VisitExit(const tinyxml2::XMLElement& element) override;
+
+ private:
+ Visitor& owner;
+ };
+
+ friend class Adapter;
+
+
+ public:
+
+ Visitor(Document& document);
+ virtual ~Visitor() = default;
+
+ virtual bool visitEnter(const AnyElement&) { return true; }
+ virtual bool visitExit(const AnyElement&) { return true; }
+
+
+ Adapter* adapter();
+ const Adapter* adapter() const;
+
+
+ private:
+
+ const AnyElement cast(const tinyxml2::XMLElement& element);
+
+ Document& document;
+ Adapter _adapter { *this };
+
+ };
+
+}
diff --git a/VirtualRobot/MJCF/elements/core/const_aware_ptr.hpp b/VirtualRobot/MJCF/elements/core/const_aware_ptr.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..de0088d4b64f955890107d46428d7f010c8149d6
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/const_aware_ptr.hpp
@@ -0,0 +1,27 @@
+#pragma once
+
+namespace mjcf { namespace detail
+{
+
+ template <typename T>
+ class const_aware_ptr
+ {
+ public:
+ const_aware_ptr(T* p = nullptr) : _p(p) {}
+
+ //void operator=(T* p) { this->_p = p; }
+
+ operator T*() { return _p; }
+ operator const T*() const { return _p; }
+
+ operator bool() const { return _p; }
+
+ T* operator->() { return _p; }
+ const T* operator->() const { return _p; }
+
+ private:
+ T* _p;
+
+ };
+
+}}
diff --git a/VirtualRobot/MJCF/elements/core/exceptions.cpp b/VirtualRobot/MJCF/elements/core/exceptions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d0784c38c959b1f8b603938cb5860834c70dc85
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/exceptions.cpp
@@ -0,0 +1,37 @@
+#include "exceptions.h"
+
+#include <typeinfo>
+
+
+namespace mjcf
+{
+
+MjcfError::MjcfError(const std::string& message) : std::runtime_error(message)
+{}
+
+MjcfIOError::MjcfIOError(const std::string& message) : MjcfError (message)
+{}
+
+AttribNotSetError::AttribNotSetError(const std::string& name) :
+ MjcfError("Attrib '" + name + "' (without default) not set.")
+{}
+
+ParseAttributeError::ParseAttributeError(
+ const std::string& source,
+ const std::type_info& targetType,
+ const std::string& reason) :
+ MjcfError("Could not parse attribute string '" + source + "' to " + targetType.name() + ".\n"
+ "Reason: " + reason)
+{}
+
+namespace error
+{
+
+InvalidElementTag::InvalidElementTag(const std::string& expected, const std::string& actual) :
+ MjcfError ("Expected tag '" + expected + "', but received '" + actual + "'.")
+{}
+
+}
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/core/exceptions.h b/VirtualRobot/MJCF/elements/core/exceptions.h
new file mode 100644
index 0000000000000000000000000000000000000000..34fa96bb1ab8ff8200fa75428f78560c5b2de4a1
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/exceptions.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include <stdexcept>
+
+namespace mjcf
+{
+
+ class MjcfError : public std::runtime_error
+ {
+ public:
+ MjcfError(const std::string& message);
+ };
+
+
+ /// Indicates an error in load/saveFile.
+ class MjcfIOError : public MjcfError
+ {
+ public:
+ MjcfIOError(const std::string& message);
+ };
+
+
+ /// Indicates that an attribute without default was not set when it was
+ /// attempted to read.
+ class AttribNotSetError : public MjcfError
+ {
+ public:
+ AttribNotSetError(const std::string& name);
+ };
+
+ /// Indicates that an attribute without default was not set when it was
+ /// attempted to read.
+ class ParseAttributeError : public MjcfError
+ {
+ public:
+ ParseAttributeError(const std::string& source, const std::type_info& targetType,
+ const std::string& reason);
+ };
+
+namespace error
+{
+
+ class InvalidElementTag : public MjcfError
+ {
+ public:
+ InvalidElementTag(const std::string& expected, const std::string& actual);
+ };
+
+}
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/core/mjcf_utils.cpp b/VirtualRobot/MJCF/elements/core/mjcf_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..993ab7e1e8f98c55846a6690898a57ad38c4fb40
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/mjcf_utils.cpp
@@ -0,0 +1,80 @@
+#include "mjcf_utils.h"
+
+#include <algorithm>
+#include <map>
+
+
+namespace mjcf
+{
+
+ std::size_t commonPrefixLength(const std::string& a, const std::string& b)
+ {
+ const std::string* smaller = &a;
+ const std::string* bigger = &b;
+ if (b.size() < a.size())
+ {
+ std::swap(smaller, bigger);
+ }
+
+ auto mismatch = std::mismatch(smaller->begin(), smaller->end(),
+ bigger->begin()).first;
+ return std::size_t(std::distance(smaller->begin(), mismatch));
+ }
+
+ template <>
+ std::string toAttr<bool>(const bool& b)
+ {
+ static const std::string strings[] = { "false", "true" };
+ return strings[int(b)];
+ }
+
+ void fromAttr(const std::string& valueStr, bool& value)
+ {
+ static const std::map<std::string, bool> map = {
+ { "true", true },
+ { "false", false },
+ { "1", true },
+ { "0", false },
+ };
+ value = map.at(valueStr);
+ }
+
+ Eigen::Vector2f strToVec2(const char* string)
+ {
+ Eigen::Vector2f v;
+ sscanf(string, "%f %f", &v(0), &v(1));
+ return v;
+ }
+
+ Eigen::Vector3f strToVec3(const char* string)
+ {
+ Eigen::Vector3f v;
+ sscanf(string, "%f %f %f", &v(0), &v(1), &v(2));
+ return v;
+ }
+
+ Eigen::Quaternionf strToQuat(const char* string)
+ {
+ Eigen::Quaternionf q;
+ sscanf(string, "%f %f %f %f", &q.w(), &q.x(), &q.y(), &q.z());
+ return q;
+ }
+
+
+
+ bool equals(const char* lhs, const char* rhs)
+ {
+ return strcmp(lhs, rhs) == 0;
+ }
+
+}
+
+bool std::operator==(const char* lhs, const std::string& rhs)
+{
+ return mjcf::equals(lhs, rhs.c_str());
+}
+
+bool std::operator==(const std::string& lhs, const char* rhs)
+{
+ return mjcf::equals(lhs.c_str(), rhs);
+}
diff --git a/VirtualRobot/MJCF/elements/core/mjcf_utils.h b/VirtualRobot/MJCF/elements/core/mjcf_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..fdc534e6742f9216e371ee7e92f586e426bff00d
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/core/mjcf_utils.h
@@ -0,0 +1,210 @@
+#pragma once
+
+#include <string>
+#include <type_traits>
+
+#include <Eigen/Eigen>
+#include <boost/lexical_cast.hpp>
+
+#include "exceptions.h"
+
+
+namespace std
+{
+ bool operator==(const char* lhs, const string& rhs);
+ bool operator==(const string& lhs, const char* rhs);
+}
+
+namespace Eigen
+{
+ using Vector5f = Eigen::Matrix<float, 5, 1>;
+ using Vector6f = Eigen::Matrix<float, 6, 1>;
+ using Vector7f = Eigen::Matrix<float, 7, 1>;
+
+ template<typename Derived>
+ struct is_matrix_expression :
+ std::is_base_of<Eigen::MatrixBase<typename std::decay<Derived>::type>, typename std::decay<Derived>::type>
+ {};
+
+ template<typename Derived>
+ struct is_quaternion_expression :
+ std::is_base_of<Eigen::QuaternionBase<typename std::decay<Derived>::type>, typename std::decay<Derived>::type>
+ {};
+
+ template<typename Derived>
+ struct is_eigen_expression :
+ is_matrix_expression<Derived>, is_quaternion_expression<Derived>
+ {};
+}
+
+
+namespace mjcf
+{
+
+ // Get lenght of common prefix of two strings (was used for mergin body names).
+ std::size_t commonPrefixLength(const std::string& a, const std::string& b);
+
+ bool equals(const char* lhs, const char* rhs);
+
+
+ // VALUE -> ATTRIBUTE
+
+ // Convert to MJCF XML attribute format.
+
+ template <typename AttrT,
+ typename std::enable_if<!Eigen::is_eigen_expression<AttrT>::value, AttrT>::type* = nullptr>
+ std::string toAttr(const AttrT& b);
+ template <>
+ std::string toAttr<bool>(const bool& b);
+ template <typename Derived>
+ std::string toAttr(const Eigen::MatrixBase<Derived>& mat);
+ template <typename Derived>
+ std::string toAttr(const Eigen::QuaternionBase<Derived>& quat);
+
+
+ // ATTRIBUTE -> VALUE
+
+ // Convert from MJCF XML attribute.
+ Eigen::Vector2f strToVec2(const char* string);
+ Eigen::Vector3f strToVec3(const char* string);
+ Eigen::Quaternionf strToQuat(const char* string);
+
+
+
+ /// Single values via boost::lexical cast. Only enabled for non-Eigen types.
+ template <typename AttrT,
+ typename std::enable_if<!Eigen::is_eigen_expression<AttrT>::value, AttrT>::type* = nullptr>
+ void fromAttr(const std::string& valueStr, AttrT& value);
+
+ /// Bool
+ void fromAttr(const std::string& valueStr, bool& value);
+
+ /// Eigen Matrix and vectors
+ template <typename Derived>
+ void fromAttr(const std::string& valueStr, Eigen::MatrixBase<Derived>& value);
+ /// Eigen Quaternions
+ template <typename Derived>
+ void fromAttr(const std::string& valueStr, Eigen::QuaternionBase<Derived>& value);
+
+
+
+ template <typename Scalar>
+ std::vector<Scalar> parseCoeffs(const std::string& string, char delim = ' ');
+
+
+ // DEFINITIONS of templated methods
+
+ template <typename AttrT,
+ typename std::enable_if<!Eigen::is_eigen_expression<AttrT>::value, AttrT>::type*>
+ std::string toAttr(const AttrT& b)
+ {
+ static_assert (!Eigen::is_eigen_expression<AttrT>::value, "Resolved an Eigen type.");
+ static_assert (!std::is_same<bool, AttrT>::value, "Resolved bool.");
+ return boost::lexical_cast<std::string>(b);
+ }
+
+ template<typename Derived>
+ std::string toAttr(const Eigen::MatrixBase<Derived>& mat)
+ {
+ static const Eigen::IOFormat iof
+ {
+ Eigen::FullPrecision, Eigen::DontAlignCols, " ", " ", "", "", "", ""
+ };
+
+ std::stringstream ss;
+ ss << mat.format(iof);
+ return ss.str();
+ }
+
+ template<typename Derived>
+ std::string toAttr(const Eigen::QuaternionBase<Derived>& quat)
+ {
+ std::stringstream ss;
+ ss << quat.w() << " " << quat.x() << " " << quat.y() << " " << quat.z();
+ return ss.str();
+ }
+
+
+ template <typename AttrT,
+ typename std::enable_if<!Eigen::is_eigen_expression<AttrT>::value, AttrT>::type*>
+ void fromAttr(const std::string& valueStr, AttrT& value)
+ {
+ static_assert (!Eigen::is_eigen_expression<AttrT>::value, "Resolved an Eigen type.");
+ value = boost::lexical_cast<AttrT>(valueStr);
+ }
+
+ template <typename Derived>
+ void fromAttr(const std::string& valueStr, Eigen::MatrixBase<Derived>& value)
+ {
+ using Matrix = Eigen::MatrixBase<Derived>;
+ using Scalar = typename Matrix::Scalar;
+
+ std::vector<Scalar> coeffs;
+ try
+ {
+ coeffs = parseCoeffs<Scalar>(valueStr);
+ }
+ catch (const std::bad_cast& e)
+ {
+ throw mjcf::ParseAttributeError(valueStr, typeid(Matrix), e.what());
+ }
+
+ if (value.size() >= 0 && static_cast<long>(coeffs.size()) != value.size())
+ {
+ throw mjcf::ParseAttributeError(valueStr, typeid(Matrix), "Number of coefficients does not match.");
+ }
+
+ long i = 0;
+ for (const auto& coeff : coeffs)
+ {
+ value(i++) = coeff;
+ }
+ }
+
+ template <typename Derived>
+ void fromAttr(const std::string& valueStr, Eigen::QuaternionBase<Derived>& value)
+ {
+ using Quaternion = Eigen::QuaternionBase<Derived>;
+ using Scalar = typename Quaternion::Scalar;
+ std::vector<Scalar> coeffs;
+
+ try
+ {
+ coeffs = parseCoeffs<Scalar>(valueStr);
+ }
+ catch (const std::bad_cast& e)
+ {
+ throw mjcf::ParseAttributeError(valueStr, typeid(Quaternion), e.what());
+ }
+
+
+ if (coeffs.size() != 4)
+ {
+ throw mjcf::ParseAttributeError(valueStr, typeid(Quaternion),
+ "Number of coefficients does not match.");
+ }
+
+ value.w() = coeffs[0];
+ value.x() = coeffs[1];
+ value.y() = coeffs[2];
+ value.z() = coeffs[3];
+ }
+
+
+ template <typename Scalar>
+ std::vector<Scalar> parseCoeffs(const std::string& string, char delim)
+ {
+ std::vector<Scalar> coeffs;
+
+ std::stringstream ss(string);
+ for (std::string valueStr; std::getline(ss, valueStr, delim);)
+ {
+ // may throw boost::bad_lexical_cast (derives from std::bad_cast)
+ Scalar value = boost::lexical_cast<Scalar>(valueStr);
+ coeffs.push_back(value);
+ }
+
+ return coeffs;
+ }
+}
+
diff --git a/VirtualRobot/MJCF/elements/has_member.hpp b/VirtualRobot/MJCF/elements/has_member.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1f4f2239ad6e6b5a7f682308421152e43048f229
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/has_member.hpp
@@ -0,0 +1,34 @@
+// from: https://gist.github.com/maddouri/0da889b331d910f35e05ba3b7b9d869b
+
+// A compile-time method for checking the existence of a class member
+// @see https://general-purpose.io/2017/03/10/checking-the-existence-of-a-cpp-class-member-at-compile-time/
+
+// This code uses "decltype" which, according to http://en.cppreference.com/w/cpp/compiler_support
+// should be supported by Clang 2.9+, GCC 4.3+ and MSVC 2010+ (if you have an older compiler, please upgrade :)
+// As of "constexpr", if not supported by your compiler, you could try "const"
+// or use the value as an inner enum value e.g. enum { value = ... }
+
+// check "test_has_member.cpp" for a usage example
+
+/// Defines a "has_member_member_name" class template
+///
+/// This template can be used to check if its "T" argument
+/// has a data or function member called "member_name"
+#define define_has_member(member_name) \
+ template <typename T> \
+ class has_member_##member_name \
+ { \
+ typedef char yes_type; \
+ typedef long no_type; \
+ template <typename U> static yes_type test(decltype(&U::member_name)); \
+ template <typename U> static no_type test(...); \
+ public: \
+ static constexpr bool value = sizeof(test<T>(0)) == sizeof(yes_type); \
+ }
+
+/// Shorthand for testing if "class_" has a member called "member_name"
+///
+/// @note "define_has_member(member_name)" must be used
+/// before calling "has_member(class_, member_name)"
+#define has_member(class_, member_name) has_member_##member_name<class_>::value
+
diff --git a/VirtualRobot/MJCF/elements/types/actuator.cpp b/VirtualRobot/MJCF/elements/types/actuator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31df438f4e08cac13909cd52a95f6ecbc4b5bf01
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/actuator.cpp
@@ -0,0 +1,84 @@
+#include "actuator.h"
+
+#include "../../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string ActuatorGeneral::tag = "general";
+const std::string ActuatorMotor::tag = "motor";
+const std::string ActuatorPosition::tag = "position";
+const std::string ActuatorVelocity::tag = "velocity";
+const std::string ActuatorCylinder::tag = "cylinder";
+const std::string ActuatorMuscle::tag = "muscle";
+
+const std::string ActuatorSection::tag = "actuator";
+
+
+Eigen::VectorXf ActuatorGeneral::dynprmDefault()
+{
+ Eigen::VectorXf v(10);
+ v.setZero();
+ v(0) = 1;
+ return v;
+}
+
+Eigen::VectorXf ActuatorGeneral::gainprmDefault()
+{
+ Eigen::VectorXf v(10);
+ v.setZero();
+ v(0) = 1;
+ return v;
+}
+
+Eigen::VectorXf ActuatorGeneral::biasprmDefault()
+{
+ Eigen::VectorXf v(10);
+ v.setZero();
+ return v;
+}
+
+Eigen::Vector2f ActuatorMuscle::timeconstDefault()
+{
+ return Eigen::Vector2f(0.01, 0.04);
+}
+
+Eigen::Vector2f ActuatorMuscle::rangeDefault()
+{
+ return Eigen::Vector2f(0.75, 1.05);
+}
+
+
+template <class ElementD>
+ElementD addActuator(ActuatorSection& as, const std::string& joint)
+{
+ ElementD motor = as.addChild<ElementD>();
+ motor.joint = joint;
+ return motor;
+}
+
+ActuatorMotor ActuatorSection::addMotor(const std::string& joint)
+{
+ return addActuator<ActuatorMotor>(*this, joint);
+}
+
+ActuatorPosition ActuatorSection::addPosition(const std::string& joint, float kp)
+{
+ ActuatorPosition position = addActuator<ActuatorPosition>(*this, joint);
+ if (kp >= 0)
+ {
+ position.kp = kp;
+ }
+ return position;
+}
+
+ActuatorVelocity ActuatorSection::addVelocity(const std::string& joint, float kv)
+{
+ ActuatorVelocity velocity = addActuator<ActuatorVelocity>(*this, joint);
+ if (kv >= 0)
+ {
+ velocity.kv = kv;
+ }
+ return velocity;
+}
diff --git a/VirtualRobot/MJCF/elements/types/actuator.h b/VirtualRobot/MJCF/elements/types/actuator.h
new file mode 100644
index 0000000000000000000000000000000000000000..4f7399d1c9465b41b2706b0ce011e9b14955fef7
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/actuator.h
@@ -0,0 +1,143 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+
+namespace mjcf
+{
+
+static Eigen::Vector6f gearDefault()
+{
+ Eigen::Vector6f v;
+ v << 1, 0, 0, 0, 0, 0;
+ return v;
+}
+
+
+#define mjcf_ActuatorAttributes(Derived) \
+ mjcf_NameAttribute(Derived); \
+ mjcf_ClassAttribute(Derived); \
+ \
+ mjcf_GroupAttribute(Derived); \
+ \
+ mjcf_BoolAttributeDef(Derived, ctrllimited, false); \
+ mjcf_BoolAttributeDef(Derived, forcelimited, false); \
+ \
+ mjcf_Vector2fAttributeDef(Derived, ctrlrange, Eigen::Vector2f::Zero()); \
+ mjcf_Vector2fAttributeDef(Derived, forcerange, Eigen::Vector2f::Zero()); \
+ mjcf_Vector2fAttributeDef(Derived, lengthrange, Eigen::Vector2f::Zero()); \
+ \
+ mjcf_AttributeDef(Derived, Eigen::Vector6f, gear, gearDefault()); \
+ mjcf_FloatAttributeDef(Derived, cranklength, 0); \
+ \
+ mjcf_StringAttributeOpt(Derived, joint); \
+ mjcf_StringAttributeOpt(Derived, jointinparent); \
+ mjcf_StringAttributeOpt(Derived, site); \
+ mjcf_StringAttributeOpt(Derived, tendon); \
+ mjcf_StringAttributeOpt(Derived, cranksite); \
+ mjcf_StringAttributeOpt(Derived, slidersite)
+
+
+struct ActuatorGeneral : public Element<ActuatorGeneral>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorGeneral)
+
+ mjcf_ActuatorAttributes(ActuatorGeneral);
+
+ // dyntype : [none, integrator, filter, muscle, user],
+ mjcf_StringAttributeDef(ActuatorGeneral, dyntype, "none");
+ // gaintype : [fixed, muscle, user], "fixed"
+ mjcf_StringAttributeDef(ActuatorGeneral, gaintype, "fixed");
+ // biastype : [none, affine, muscle, user], "none"
+ mjcf_StringAttributeDef(ActuatorGeneral, biastype, "none");
+
+ // dynprm : real(10), "1 0 ... 0"
+ mjcf_AttributeDef(ActuatorGeneral, Eigen::VectorXf, dynprm, dynprmDefault());
+ // gainprm : real(10), "1 0 ... 0"
+ mjcf_AttributeDef(ActuatorGeneral, Eigen::VectorXf, gainprm, gainprmDefault());
+ // biasprm : real(10), "1 0 ... 0"
+ mjcf_AttributeDef(ActuatorGeneral, Eigen::VectorXf, biasprm, biasprmDefault());
+
+
+private:
+ static Eigen::VectorXf dynprmDefault();
+ static Eigen::VectorXf gainprmDefault();
+ static Eigen::VectorXf biasprmDefault();
+
+};
+
+struct ActuatorMotor : public Element<ActuatorMotor>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorMotor)
+
+ mjcf_ActuatorAttributes(ActuatorMotor);
+};
+
+struct ActuatorPosition : public Element<ActuatorPosition>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorPosition)
+
+ mjcf_ActuatorAttributes(ActuatorPosition);
+ mjcf_FloatAttributeDef(ActuatorPosition, kp, 1);
+};
+
+struct ActuatorVelocity : public Element<ActuatorVelocity>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorVelocity)
+
+ mjcf_ActuatorAttributes(ActuatorVelocity);
+ mjcf_FloatAttributeDef(ActuatorVelocity, kv, 1);
+};
+
+struct ActuatorCylinder : public Element<ActuatorCylinder>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorCylinder)
+
+ mjcf_ActuatorAttributes(ActuatorCylinder);
+ mjcf_FloatAttributeDef(ActuatorCylinder, timeconst, 1);
+ mjcf_FloatAttributeDef(ActuatorCylinder, area, 1);
+ mjcf_FloatAttributeOpt(ActuatorCylinder, diameter);
+ mjcf_Vector3fAttributeDef(ActuatorCylinder, bias, Eigen::Vector3f::Zero());
+};
+
+struct ActuatorMuscle : public Element<ActuatorMuscle>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorMuscle)
+
+ mjcf_ActuatorAttributes(ActuatorMuscle);
+
+ mjcf_Vector2fAttributeDef(ActuatorMuscle, timeconst, timeconstDefault());
+ mjcf_Vector2fAttributeDef(ActuatorMuscle, range, rangeDefault());
+
+ mjcf_FloatAttributeDef(ActuatorMuscle, force, -1);
+ mjcf_FloatAttributeDef(ActuatorMuscle, scale, 200);
+ mjcf_FloatAttributeDef(ActuatorMuscle, lmin, 0.5);
+ mjcf_FloatAttributeDef(ActuatorMuscle, lmax, 1.6f);
+ mjcf_FloatAttributeDef(ActuatorMuscle, vmax, 1.5);
+ mjcf_FloatAttributeDef(ActuatorMuscle, fpmax, 1.3f);
+ mjcf_FloatAttributeDef(ActuatorMuscle, vvmax, 1.2f);
+
+private:
+ static Eigen::Vector2f timeconstDefault();
+ static Eigen::Vector2f rangeDefault();
+};
+
+
+struct ActuatorSection : public Element<ActuatorSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorSection)
+
+ ActuatorMotor addMotor(const std::string& jointName);
+ ActuatorPosition addPosition(const std::string& joint, float kp = -1);
+ ActuatorVelocity addVelocity(const std::string& joint, float kv = -1);
+
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/asset.cpp b/VirtualRobot/MJCF/elements/types/asset.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..916feb621c9d0a99ed7950e1e718065741d5b5ee
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/asset.cpp
@@ -0,0 +1,55 @@
+#include "asset.h"
+
+#include "../../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string Texture::tag = "texture";
+const std::string Material::tag = "material";
+const std::string Mesh::tag = "mesh";
+const std::string AssetSection::tag = "asset";
+
+
+Texture AssetSection::addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2)
+{
+ Texture texSkybox = addChild<Texture>();
+
+ texSkybox.type = "skybox";
+ texSkybox.builtin = "gradient";
+ texSkybox.width = 128;
+ texSkybox.height = 128;
+ texSkybox.rgb1 = rgb1;
+ texSkybox.rgb2 = rgb2;
+
+ return texSkybox;
+}
+
+Texture AssetSection::addTextureFile(const std::string& name, const std::string& file,
+ bool vflip, bool hflip)
+{
+ Texture texture = addChild<Texture>();
+ texture.name = name;
+ texture.file = file;
+ texture.type = "2d";
+ texture.vflip = vflip;
+ texture.hflip = hflip;
+ return texture;
+}
+
+Mesh AssetSection::addMesh(const std::string& name, const std::string& file)
+{
+ Mesh mesh = addChild<Mesh>();
+ mesh.name = name;
+ mesh.file = file;
+ return mesh;
+}
+
+Material AssetSection::addMaterial(const std::string& name, const std::string& textureName)
+{
+ Material material = addChild<Material>();
+ material.name = name;
+ material.texture = textureName;
+ return material;
+}
diff --git a/VirtualRobot/MJCF/elements/types/asset.h b/VirtualRobot/MJCF/elements/types/asset.h
new file mode 100644
index 0000000000000000000000000000000000000000..33f718384cfe92b48211d906307f5d564362f273
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/asset.h
@@ -0,0 +1,93 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+
+namespace mjcf
+{
+
+struct Texture : public Element<Texture>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Texture)
+
+ mjcf_NameAttribute(Texture);
+ mjcf_StringAttributeDef(Texture, type, "cube");
+ mjcf_StringAttributeOpt(Texture, file);
+
+ mjcf_AttributeDef(Texture, Eigen::Vector2i, gridsize, Eigen::Vector2i::Ones());
+ mjcf_StringAttributeDef(Texture, gridlayout, "............");
+
+ mjcf_StringAttributeDef(Texture, builtin, "none");
+ mjcf_StringAttributeDef(Texture, mark, "none");
+ mjcf_RgbAttributeDef(Texture, markrgb, Eigen::Vector3f::Zero());
+ mjcf_FloatAttributeDef(Texture, random, 0.01f);
+
+ mjcf_IntAttributeDef(Texture, width, 0);
+ mjcf_IntAttributeDef(Texture, height, 0);
+
+ mjcf_RgbAttributeDef(Texture, rgb1, 0.8f * Eigen::Vector3f::Ones());
+ mjcf_RgbAttributeDef(Texture, rgb2, 0.5f * Eigen::Vector3f::Ones());
+
+ mjcf_BoolAttributeDef(Texture, hflip, false);
+ mjcf_BoolAttributeDef(Texture, vflip, true);
+};
+
+
+struct Mesh : public Element<Mesh>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Mesh)
+
+ mjcf_NameAttribute(Mesh);
+ mjcf_ClassAttribute(Mesh);
+
+ mjcf_StringAttributeOpt(Mesh, file);
+
+ mjcf_Vector3fAttributeDef(Mesh, scale, Eigen::Vector3f::Ones());
+ mjcf_BoolAttributeDef(Mesh, smoothnormal, false);
+
+ mjcf_Vector3fAttributeDef(Mesh, refpos, Eigen::Vector3f::Ones());
+ mjcf_AttributeDef(Mesh, Eigen::Quaternionf, refquat, Eigen::Quaternionf::Identity());
+};
+
+
+struct Material : public Element<Material>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Material)
+
+ mjcf_NameAttribute(Material);
+ mjcf_ClassAttribute(Material);
+
+ mjcf_StringAttributeOpt(Material, texture);
+
+ mjcf_AttributeDef(Material, Eigen::Vector2f, texrepeat, Eigen::Vector2f::Ones());
+ mjcf_BoolAttributeDef(Material, texuniform, false);
+
+ mjcf_FloatAttributeDef(Material, emission, 0);
+ mjcf_FloatAttributeDef(Material, specular, 0);
+ mjcf_FloatAttributeDef(Material, shininess, 0);
+ mjcf_FloatAttributeDef(Material, reflectance, 0);
+ mjcf_RgbaAttributeDef(Material, rgba, Eigen::Vector4f::Ones());
+};
+
+
+
+struct AssetSection : public Element<AssetSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(AssetSection)
+
+ Texture addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2);
+
+ Texture addTextureFile(const std::string& name, const std::string& file,
+ bool vflip = false, bool hflip = false);
+
+ /// Add a mesh asset with a name and a file.
+ Mesh addMesh(const std::string& name, const std::string& file);
+
+ Material addMaterial(const std::string& name, const std::string& textureName);
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/body.cpp b/VirtualRobot/MJCF/elements/types/body.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de08ed5983437056f93a9761a561fdab8fb844f0
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/body.cpp
@@ -0,0 +1,162 @@
+#include "body.h"
+
+#include "../../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string Inertial::tag = "inertial";
+const std::string Joint::tag = "joint";
+const std::string FreeJoint::tag = "freejoint";
+const std::string Geom::tag = "geom";
+const std::string Site::tag = "site";
+const std::string Camera::tag = "camera";
+const std::string Light::tag = "light";
+const std::string Body::tag = "body";
+const std::string Worldbody::tag = "worldbody";
+
+
+void Inertial::inertiaFromMatrix(const Eigen::Matrix3f& matrix)
+{
+ if (matrix.isDiagonal(1e-9f))
+ {
+ this->diaginertia = matrix.diagonal();
+ }
+ else
+ {
+ /* Full inertia matrix M. Since M is 3-by-3 and symmetric,
+ * it is specified using only 6 numbers in the following order:
+ * M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3).
+ * -- http://www.mujoco.org/book/XMLreference.html#inertial
+ */
+
+ Eigen::Vector6f inertia;
+ inertia << matrix(0, 0), matrix(1, 1), matrix(2, 2),
+ matrix(0, 1), matrix(0, 2), matrix(1, 2);
+ this->fullinertia = inertia;
+ }
+}
+
+bool Body::hasMass() const
+{
+ return hasChild<Geom>() || hasChild<Inertial>();
+}
+
+Body Body::addBody(const std::string& name)
+{
+ Body body = addChild<Body>();
+
+ if (!name.empty())
+ {
+ body.name = name;
+ }
+
+ return body;
+}
+
+Inertial Body::addInertial()
+{
+ return addChild<Inertial>();
+}
+
+Inertial Body::addInertial(const Eigen::Vector3f& pos, float mass, const Eigen::Matrix3f& matrix)
+{
+ Inertial inertial = addInertial();
+
+ inertial.pos = pos;
+ inertial.mass = mass;
+
+ if (matrix.isDiagonal(document()->getFloatCompPrecision()))
+ {
+ inertial.diaginertia = matrix.diagonal();
+ }
+ else
+ {
+ /* from mujoco xml reference:
+ * "Full inertia matrix M. Since M is 3-by-3 and symmetric, it is
+ * specified using only 6 numbers in the following order:
+ * M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3)."*/
+ Eigen::Vector6f inertia;
+ inertia << matrix(0, 0), matrix(1, 1), matrix(2, 2),
+ matrix(0, 1), matrix(0, 2), matrix(1, 2);
+ inertial.fullinertia = inertia;
+ }
+
+ return inertial;
+}
+
+Inertial Body::addDummyInertial()
+{
+ Inertial inertial = addInertial();
+
+ inertial.pos = Eigen::Vector3f(0, 0, 0);
+ inertial.mass = document()->getDummyMass();
+ inertial.diaginertia = Eigen::Vector3f::Ones();
+
+ return inertial;
+}
+
+Joint Body::addJoint()
+{
+ return addChild<Joint>();
+}
+
+FreeJoint Body::addFreeJoint()
+{
+ return addChild<FreeJoint>();
+
+}
+
+Geom Body::addGeom(const std::string& type)
+{
+ Geom geom = addChild<Geom>();
+
+ geom.type = type;
+
+ return geom;
+}
+
+Geom Body::addGeomMesh(const std::string& meshName, const std::string& materialName)
+{
+ Geom geom = addGeom("mesh");
+
+ geom.mesh = meshName;
+ if (!materialName.empty())
+ {
+ geom.material = materialName;
+ }
+
+ return geom;
+}
+
+
+Body Worldbody::addMocapBody(const std::string& name, float geomSize)
+{
+ Body mocap = addChild<Body>();
+ mocap.name = name;
+ mocap.mocap = true;
+ // add geom for visualization
+
+ Geom geom = mocap.addChild<Geom>();
+ geom.type = "box";
+ geom.size = geomSize * Eigen::Vector3f::Ones();
+
+ return mocap;
+}
+
+Body Worldbody::addBody(const std::string& name, const std::string& childClass)
+{
+ Body body = addChild<Body>();
+
+ if (!name.empty())
+ {
+ body.name = name;
+ }
+ if (!childClass.empty())
+ {
+ body.childclass = childClass;
+ }
+
+ return body;
+}
diff --git a/VirtualRobot/MJCF/elements/types/body.h b/VirtualRobot/MJCF/elements/types/body.h
new file mode 100644
index 0000000000000000000000000000000000000000..9bacb6b8ef9f49e4b7c4b7927c10700eebaa94bb
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/body.h
@@ -0,0 +1,232 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+
+namespace mjcf
+{
+
+
+struct Inertial : public Element<Inertial>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Inertial)
+
+ mjcf_PoseAttributes(Inertial);
+
+ mjcf_AttributeReq(Inertial, float, mass);
+
+ mjcf_AttributeOpt(Inertial, Eigen::Vector3f, diaginertia);
+ mjcf_AttributeOpt(Inertial, Eigen::Vector6f, fullinertia);
+
+ /// Sets diaginertia or fullinertia (as adequate) from the given matrix.
+ void inertiaFromMatrix(const Eigen::Matrix3f& matrix);
+};
+
+
+struct Joint : public Element<Joint>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Joint)
+
+ void setAxis(const Eigen::Vector3f& axis);
+
+ mjcf_NameAttribute(Joint);
+ mjcf_ClassAttribute(Joint);
+
+ // free, ball, slide, hinge
+ mjcf_StringAttributeDef(Joint, type, "hinge");
+ mjcf_IntAttributeDef(Joint, group, 0);
+
+ mjcf_PosAttribute(Joint);
+ mjcf_Vector3fAttributeDef(Joint, axis, Eigen::Vector3f(0, 0, 1));
+
+ mjcf_AttributeDef(Joint, Eigen::Vector2f, springdamper, Eigen::Vector2f::Zero());
+ mjcf_FloatAttributeDef(Joint, stiffness, 0);
+
+ mjcf_BoolAttributeDef(Joint, limited, false);
+ mjcf_AttributeDef(Joint, Eigen::Vector2f, range, Eigen::Vector2f::Zero());
+
+ mjcf_FloatAttributeDef(Joint, margin, 0);
+
+ mjcf_FloatAttributeDef(Joint, ref, 0);
+ mjcf_FloatAttributeDef(Joint, springref, 0);
+
+ mjcf_FloatAttributeDef(Joint, armature, 0);
+ mjcf_FloatAttributeDef(Joint, damping, 0);
+ mjcf_FloatAttributeDef(Joint, frictionloss, 0);
+};
+
+
+struct FreeJoint : public Element<FreeJoint>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(FreeJoint)
+
+ mjcf_NameAttribute(FreeJoint);
+ mjcf_IntAttributeDef(FreeJoint, group, 0);
+};
+
+
+struct Geom : public Element<Geom>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Geom)
+
+ mjcf_NameAttribute(Geom);
+ mjcf_ClassAttribute(Geom);
+
+ // plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh
+ mjcf_StringAttributeDef(Geom, type, "sphere");
+
+ mjcf_IntAttributeDef(Geom, contype, 1);
+ mjcf_IntAttributeDef(Geom, conaffinity, 1);
+ mjcf_IntAttributeDef(Geom, condim, 3);
+
+ mjcf_IntAttributeDef(Geom, group, 0);
+
+ mjcf_Vector3fAttributeDef(Geom, size, Eigen::Vector3f::Zero());
+ mjcf_StringAttributeOpt(Geom, material);
+ mjcf_RgbaAttributeDef(Geom, rgba, Eigen::Vector4f(.5, .5, .5, 1));
+
+ mjcf_Vector3fAttributeDef(Geom, friction, Eigen::Vector3f(1, 0.005f, 0.0001f));
+
+ mjcf_FloatAttributeOpt(Geom, mass);
+ mjcf_FloatAttributeDef(Geom, density, 1000);
+
+ mjcf_FloatAttributeDef(Geom, solmix, 1);
+
+ mjcf_FloatAttributeDef(Geom, marin, 0);
+ mjcf_FloatAttributeDef(Geom, gap, 0);
+
+ mjcf_AttributeOpt(Geom, Eigen::Vector6f, fromto);
+
+ mjcf_PoseAttributes(Geom);
+
+ mjcf_StringAttributeOpt(Geom, hfield);
+ mjcf_StringAttributeOpt(Geom, mesh);
+
+ mjcf_FloatAttributeDef(Geom, fitscale, 1);
+};
+
+struct Site : public Element<Site>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Site)
+
+ mjcf_NameAttribute(Site);
+ mjcf_ClassAttribute(Site);
+
+ // sphere, capsule, ellipsoid, cylinder, box
+ mjcf_StringAttributeDef(Site, type, "sphere");
+ mjcf_IntAttributeDef(Site, group, 0);
+
+ mjcf_StringAttributeOpt(Site, material);
+ mjcf_RgbaAttributeDef(Site, rgba, Eigen::Vector4f(.5, .5, .5, 1));
+
+ mjcf_Vector3fAttributeDef(Site, size, Eigen::Vector3f::Constant(0.005f));
+ mjcf_AttributeOpt(Site, Eigen::Vector6f, fromto);
+
+ mjcf_PoseAttributes(Site);
+};
+
+struct Camera : public Element<Camera>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Camera)
+
+ mjcf_NameAttribute(Camera);
+ mjcf_ClassAttribute(Camera);
+
+ // fixed, track, trackcom, targetbody, targetbodycom
+ mjcf_StringAttributeDef(Camera, mode, "fixed");
+ mjcf_StringAttributeOpt(Camera, target); // required if targetbody or targetbodycom
+
+ /// Vertical field of view of the camera, expressed in degrees regardless
+ /// of the global angle setting.
+ mjcf_FloatAttributeDef(Camera, fovy, 45);
+ /// Inter-pupilary distance.
+ mjcf_FloatAttributeDef(Camera, ipd, 0.068f);
+
+ mjcf_PoseAttributes(Camera);
+};
+
+struct Light : public Element<Light>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Light)
+
+ mjcf_NameAttribute(Light);
+ mjcf_ClassAttribute(Light);
+
+ // fixed, track, trackcom, targetbody, targetbodycom
+ mjcf_StringAttributeDef(Light, mode, "fixed");
+ mjcf_StringAttributeOpt(Light, target); // required if targetbody or targetbodycom
+
+ mjcf_BoolAttributeDef(Light, directional, false);
+ mjcf_BoolAttributeDef(Light, castshadow, false);
+ mjcf_BoolAttributeDef(Light, active, false);
+
+ mjcf_PosAttribute(Light);
+ mjcf_Vector3fAttributeDef(Light, dir, - Eigen::Vector3f::UnitZ());
+
+ mjcf_Vector3fAttributeDef(Light, attenuation, Eigen::Vector3f::UnitX());
+ mjcf_FloatAttributeDef(Light, cutoff, 45);
+ mjcf_FloatAttributeDef(Light, exponent, 10);
+
+ mjcf_Vector3fAttributeDef(Light, ambient, Eigen::Vector3f::Zero());
+ mjcf_Vector3fAttributeDef(Light, diffuse, Eigen::Vector3f::Constant(0.7f));
+ mjcf_Vector3fAttributeDef(Light, specular, Eigen::Vector3f::Constant(0.3f));
+};
+
+struct Body : public Element<Body>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Body)
+
+ bool hasMass() const;
+
+ /// Add a child body element.
+ Body addBody(const std::string& name = "");
+
+ /// Add an inertial element.
+ Inertial addInertial();
+ /// Add an inertial element with given settings.
+ Inertial addInertial(const Eigen::Vector3f& pos, float mass, const Eigen::Matrix3f& matrix);
+ /// Add a dummy inertial element with small mass and identity inertia matrix.
+ Inertial addDummyInertial();
+
+
+ /// Add a joint element to a body.
+ Joint addJoint();
+ /// Add a free joint element to a body.
+ FreeJoint addFreeJoint();
+
+ /// Add a geom to a body, referencing a mesh.
+ Geom addGeom(const std::string& type);
+ /// Add a mesh geom with optional material (for texture).
+ Geom addGeomMesh(const std::string& meshName, const std::string& materialName = "");
+
+
+ mjcf_NameAttribute(Body);
+ mjcf_StringAttributeOpt(Body, childclass);
+
+ mjcf_BoolAttributeDef(Body, mocap, false);
+
+ mjcf_PoseAttributes(Body);
+};
+
+struct Worldbody : public Element<Worldbody>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Worldbody)
+
+ /// Add a body element.
+ Body addBody(const std::string& name = "", const std::string& childClass = "");
+
+ /// Add a mocap body with the given name to the worldbody.
+ Body addMocapBody(const std::string& name, float geomSize);
+
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/compiler.cpp b/VirtualRobot/MJCF/elements/types/compiler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5619b7e67503ec6ce6fffdc12c0ef52865127e44
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/compiler.cpp
@@ -0,0 +1,11 @@
+#include "compiler.h"
+
+//#include "../Document.h"
+
+
+namespace mjcf
+{
+ const std::string CompilerSection::tag = "compiler";
+}
+
+
diff --git a/VirtualRobot/MJCF/elements/types/compiler.h b/VirtualRobot/MJCF/elements/types/compiler.h
new file mode 100644
index 0000000000000000000000000000000000000000..f63347b56ded633051f4095e1ec9838cecbaa6eb
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/compiler.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+namespace mjcf
+{
+
+struct CompilerSection : public Element<CompilerSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(CompilerSection)
+
+ mjcf_FloatAttributeDef(CompilerSection, boundmass, 0);
+ mjcf_FloatAttributeDef(CompilerSection, boundinertia, 0);
+ mjcf_FloatAttributeDef(CompilerSection, settotalmass, -1);
+
+ mjcf_BoolAttributeDef(CompilerSection, balanceinertia, false);
+ mjcf_BoolAttributeDef(CompilerSection, strippath, false);
+
+ // local, global
+ mjcf_StringAttributeDef(CompilerSection, coordinate, "local");
+ // degree, radian (default "degree" for MJCF, always "radian" for URDF)
+ mjcf_StringAttributeDef(CompilerSection, angle, "degree");
+ mjcf_BoolAttributeDef(CompilerSection, fitaabb, false);
+ mjcf_StringAttributeDef(CompilerSection, eulerseq, "xyz");
+
+ mjcf_StringAttributeOpt(CompilerSection, meshdir);
+ mjcf_StringAttributeOpt(CompilerSection, texturedir);
+ // Default: false for MJCF, true for URDF
+ mjcf_BoolAttributeDef(CompilerSection, discardvisual, false);
+
+ mjcf_BoolAttributeDef(CompilerSection, convexhull, true);
+ mjcf_BoolAttributeDef(CompilerSection, userthread, true);
+
+ // Default: false for MJCF, true for URDF
+ mjcf_BoolAttributeDef(CompilerSection, fusestatic, false);
+
+ // false, true, auto
+ mjcf_StringAttributeDef(CompilerSection, inertiafromgeom, "auto");
+ mjcf_AttributeDef(CompilerSection, Eigen::Vector2i, inertiagrouprange, Eigen::Vector2i(0, 5));
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/contact.cpp b/VirtualRobot/MJCF/elements/types/contact.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..27b412e864709535247272b36fd2947ad8946a6b
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/contact.cpp
@@ -0,0 +1,49 @@
+#include "contact.h"
+
+#include "../../Document.h"
+
+using namespace mjcf;
+
+
+const std::string ContactPair::tag = "pair";
+const std::string ContactExclude::tag = "exclude";
+const std::string ContactSection::tag = "contact";
+
+
+Eigen::Vector5f ContactPair::condimDefault()
+{
+ Eigen::Vector5f v;
+ v << 1, 1, 0.005f, 0.0001f, 0.0001f;
+ return v;
+}
+
+ContactPair ContactSection::addPair(const Geom& geom1, const Geom& geom2)
+{
+ return addPair(geom1.name.get(), geom2.name.get());
+}
+
+ContactPair ContactSection::addPair(const std::string& geom1Name, const std::string& geom2Name)
+{
+ ContactPair pair = addChild<ContactPair>();
+
+ pair.geom1 = geom1Name;
+ pair.geom2 = geom2Name;
+
+ return pair;
+}
+
+
+ContactExclude ContactSection::addExclude(const Body& body1, const Body& body2)
+{
+ return addExclude(body1.name.get(), body2.name.get());
+}
+
+ContactExclude ContactSection::addExclude(const std::string& body1Name, const std::string& body2Name)
+{
+ ContactExclude exclude = addChild<ContactExclude>();
+
+ exclude.body1 = body1Name;
+ exclude.body2 = body2Name;
+
+ return exclude;
+}
diff --git a/VirtualRobot/MJCF/elements/types/contact.h b/VirtualRobot/MJCF/elements/types/contact.h
new file mode 100644
index 0000000000000000000000000000000000000000..dff53f638c5e0513baed734a42747383a554f57f
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/contact.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include "../core/Attribute.h"
+#include "body.h"
+
+
+namespace mjcf
+{
+
+
+struct ContactPair : public Element<ContactPair>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ContactPair)
+
+ mjcf_NameAttribute(ContactPair);
+ mjcf_ClassAttribute(ContactPair);
+
+ mjcf_StringAttributeReq(ContactPair, geom1);
+ mjcf_StringAttributeReq(ContactPair, geom2);
+
+ mjcf_IntAttributeDef(ContactPair, condim, 3);
+
+ mjcf_AttributeDef(ContactPair, Eigen::Vector5f, friction, condimDefault());
+
+ mjcf_FloatAttributeDef(ContactPair, margin, 0);
+ mjcf_FloatAttributeDef(ContactPair, gap, 0);
+
+private:
+ static Eigen::Vector5f condimDefault();
+};
+
+
+struct ContactExclude : public Element<ContactExclude>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ContactExclude)
+
+ mjcf_NameAttribute(ContactExclude);
+ mjcf_StringAttributeReq(ContactExclude, body1);
+ mjcf_StringAttributeReq(ContactExclude, body2);
+};
+
+
+struct ContactSection : public Element<ContactSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ContactSection)
+
+ /// Add a conact/pair element between the given geoms.
+ ContactPair addPair(const Geom& geom1, const Geom& geom2);
+ /// Add a conact/pair element between the given geoms.
+ ContactPair addPair(const std::string& geom1Name, const std::string& geom2Name);
+
+ /// Add a conact/exclude element between the given bodies.
+ ContactExclude addExclude(const Body& body1, const Body& body2);
+ /// Add a conact/exclude element between the given bodies.
+ ContactExclude addExclude(const std::string& body1Name, const std::string& body2Name);
+};
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/custom.cpp b/VirtualRobot/MJCF/elements/types/custom.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f2655738d04500a6d053d35b1aee4538c999db7
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/custom.cpp
@@ -0,0 +1,6 @@
+#include "compiler.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
diff --git a/VirtualRobot/MJCF/elements/types/custom.h b/VirtualRobot/MJCF/elements/types/custom.h
new file mode 100644
index 0000000000000000000000000000000000000000..64fef95b4d0f24e68d5071817f3d0285a9e223dc
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/custom.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+namespace mjcf
+{
+
+
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/default.cpp b/VirtualRobot/MJCF/elements/types/default.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa50b0cdf42aba96c63803bf4a804f06dc9c4480
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/default.cpp
@@ -0,0 +1,31 @@
+#include "default.h"
+
+#include "../../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string DefaultClass::tag = "default";
+const std::string DefaultSection::tag = "default";
+
+
+bool DefaultSection::hasClass(const std::string& className)
+{
+ return !hasChild<mjcf::DefaultClass>("class", className);
+}
+
+DefaultClass DefaultSection::getClass(const std::string& className)
+{
+ DefaultClass def = firstChild<DefaultClass>("class", className);
+ if (!def)
+ {
+ def = addClass(className);
+ }
+ return def ;
+}
+
+DefaultClass DefaultSection::addClass(const std::string& className)
+{
+ return addChild<DefaultClass>(className);
+}
diff --git a/VirtualRobot/MJCF/elements/types/default.h b/VirtualRobot/MJCF/elements/types/default.h
new file mode 100644
index 0000000000000000000000000000000000000000..9c22f0cf2aceb8b79b8261a81e5a6197f2da451d
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/default.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+
+namespace mjcf
+{
+
+
+struct DefaultClass : public Element<DefaultClass>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(DefaultClass)
+
+ mjcf_ClassAttribute(DefaultClass);
+
+ /// Get the element of the specified type (create it if it does not exist).
+ template <class ElementD>
+ ElementD getElement();
+
+ template <class ElementD, typename AttrT>
+ ElementD setElementAttr(const std::string& attrName, const AttrT& attrValue);
+};
+
+
+struct DefaultSection : public Element<DefaultSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(DefaultSection)
+
+ /// Indicate whether there is a default class with the given name.
+ bool hasClass(const std::string& className);
+
+ /// Get the default class for given class name. If it does not exist, add it.
+ DefaultClass getClass(const std::string& className);
+
+ /// Add a new default class with given class name.
+ DefaultClass addClass(const std::string& className);
+};
+
+
+
+template<class ElementD>
+ElementD DefaultClass::getElement()
+{
+ return getOrCreateChild<ElementD>();
+}
+
+template<class ElementD, typename AttrT>
+ElementD DefaultClass::setElementAttr(const std::string& attrName, const AttrT& attrValue)
+{
+ ElementD element = getElement<ElementD>();
+ element.setAttribute(attrName, attrValue);
+ return element;
+}
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/equality.cpp b/VirtualRobot/MJCF/elements/types/equality.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2732d939bbecff717d11fe448750c5d92097f014
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/equality.cpp
@@ -0,0 +1,49 @@
+#include "equality.h"
+
+#include "../../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string EqualityDefaults::tag = "equality";
+const std::string EqualityConnect::tag = "connect";
+const std::string EqualityWeld::tag = "weld";
+const std::string EqualityJoint::tag = "joint";
+const std::string EqualityTendon::tag = "tendon";
+const std::string EqualityDistance::tag = "distance";
+const std::string EqualitySection::tag = "equality";
+
+
+Eigen::Vector7f EqualityWeld::relposeDefault()
+{
+ Eigen::Vector7f v;
+ v << 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f;
+ return v;
+}
+
+
+Eigen::Vector5f EqualityJoint::polycoefDefault()
+{
+ Eigen::Vector5f v;
+ v << 0, 1, 0, 0, 0;
+ return v;
+}
+
+Eigen::Vector5f EqualityTendon::polycoefDefault()
+{
+ Eigen::Vector5f v;
+ v << 0, 1, 0, 0, 0;
+ return v;
+}
+
+EqualityWeld EqualitySection::addWeld(const std::string& name, const std::string& body1, const std::string& body2)
+{
+ EqualityWeld weld = addChild<EqualityWeld>();
+
+ weld.name = name;
+ weld.body1 = body1;
+ weld.body2 = body2;
+
+ return weld;
+}
diff --git a/VirtualRobot/MJCF/elements/types/equality.h b/VirtualRobot/MJCF/elements/types/equality.h
new file mode 100644
index 0000000000000000000000000000000000000000..d31253e85e27cedd8534b572056e56c41d2d8d63
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/equality.h
@@ -0,0 +1,109 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+
+namespace mjcf
+{
+
+#define mjcf_EqualityAttribs(Derived, entityName) \
+ mjcf_NameAttribute(Derived); \
+ mjcf_ClassAttribute(Derived); \
+ mjcf_BoolAttributeDef(Derived, active, true); \
+ mjcf_StringAttributeReq(Derived, entityName ## 1); \
+ mjcf_StringAttributeOpt(Derived, entityName ## 2)
+
+/**
+ * @brief Element of a defaults class for equality constraints.
+ * Should only be used as a child of mjcf::DefaultsClass.
+ */
+struct EqualityDefaults : public Element<EqualityDefaults>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityDefaults)
+
+ mjcf_BoolAttributeOpt(EqualityDefaults, active);
+ mjcf_SolrefAttribute(EqualityDefaults);
+ mjcf_SolimpAttribute(EqualityDefaults);
+};
+
+
+struct EqualityConnect : public Element<EqualityConnect>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityConnect)
+
+ mjcf_EqualityAttribs(EqualityConnect, body);
+
+ mjcf_Vector3fAttributeReq(EqualityConnect, anchor);
+};
+
+
+struct EqualityWeld : public Element<EqualityWeld>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityWeld)
+
+ mjcf_EqualityAttribs(EqualityWeld, body);
+
+ mjcf_AttributeDef(EqualityWeld, Eigen::Vector7f, relpose, relposeDefault());
+
+private:
+ static Eigen::Vector7f relposeDefault();
+
+};
+
+
+struct EqualityJoint : public Element<EqualityJoint>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityJoint)
+
+ mjcf_EqualityAttribs(EqualityJoint, joint);
+
+ mjcf_AttributeDef(EqualityJoint, Eigen::Vector5f, polycoef, polycoefDefault());
+
+private:
+ static Eigen::Vector5f polycoefDefault();
+
+};
+
+
+struct EqualityTendon : public Element<EqualityTendon>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityTendon)
+
+ mjcf_EqualityAttribs(EqualityTendon, tendon);
+
+ mjcf_AttributeDef(EqualityTendon, Eigen::Vector5f, polycoef, polycoefDefault());
+
+private:
+ static Eigen::Vector5f polycoefDefault();
+
+};
+
+struct EqualityDistance : public Element<EqualityDistance>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityDistance)
+
+ mjcf_EqualityAttribs(EqualityDistance, geom);
+
+ mjcf_FloatAttributeDef(EqualityDistance, distance, 0);
+};
+
+
+struct EqualitySection : public Element<EqualitySection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualitySection)
+
+ EqualityWeld addWeld(const std::string& name, const std::string& body1,
+ const std::string& body2);
+
+};
+
+
+#undef mjcf_EqualityAttribs
+}
diff --git a/VirtualRobot/MJCF/elements/types/keyframe.cpp b/VirtualRobot/MJCF/elements/types/keyframe.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5ba1e0678b6860d084f656133d99f7e9bd340da1
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/keyframe.cpp
@@ -0,0 +1,9 @@
+#include "keyframe.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string KeyframeSection::tag = "keyframe";
diff --git a/VirtualRobot/MJCF/elements/types/keyframe.h b/VirtualRobot/MJCF/elements/types/keyframe.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ca8c42315ddb31645e3ded2d90c6d74a46ff7bf
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/keyframe.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+namespace mjcf
+{
+
+struct KeyframeSection : public Element<KeyframeSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(KeyframeSection)
+};
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/meta.cpp b/VirtualRobot/MJCF/elements/types/meta.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e53f9b94da36c61b74044643c92290a3c00bcd17
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/meta.cpp
@@ -0,0 +1,10 @@
+#include "meta.h"
+
+namespace mjcf
+{
+
+const std::string MujocoRoot::tag = "mujoco";
+const std::string Include::tag = "include";
+
+}
+
diff --git a/VirtualRobot/MJCF/elements/types/meta.h b/VirtualRobot/MJCF/elements/types/meta.h
new file mode 100644
index 0000000000000000000000000000000000000000..082b272db5ed21e849151a328e06c31c2fd41273
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/meta.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+
+namespace mjcf
+{
+
+/// @see http://www.mujoco.org/book/XMLreference.html#mujoco
+struct MujocoRoot : public Element<MujocoRoot>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(MujocoRoot)
+
+ mjcf_StringAttributeDef(MujocoRoot, model, "MuJoCo Model");
+};
+
+
+/// @see http://www.mujoco.org/book/XMLreference.html#include
+struct Include : public Element<Include>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Include)
+
+ mjcf_StringAttributeReq(Include, file);
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/option.cpp b/VirtualRobot/MJCF/elements/types/option.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53d4730a1631fe13b32062ab5762e62f80088117
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/option.cpp
@@ -0,0 +1,16 @@
+#include "option.h"
+
+#include "../../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string OptionFlag::tag = "flag";
+const std::string OptionSection::tag = "option";
+
+
+OptionFlag OptionSection::flag()
+{
+ return getOrCreateChild<OptionFlag>();
+}
diff --git a/VirtualRobot/MJCF/elements/types/option.h b/VirtualRobot/MJCF/elements/types/option.h
new file mode 100644
index 0000000000000000000000000000000000000000..2dc4e1a2a33f8e626f2f60595c3fa4d50a43a60c
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/option.h
@@ -0,0 +1,71 @@
+#pragma once
+
+#include "../core/Attribute.h"
+#include "../core/mjcf_utils.h"
+
+namespace mjcf
+{
+
+
+struct OptionFlag : public Element<OptionFlag>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(OptionFlag)
+};
+
+struct OptionSection : public Element<OptionSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(OptionSection)
+
+
+ /// Get (or create) the option/flag child.
+ OptionFlag flag();
+
+
+ mjcf_FloatAttributeDef(OptionSection, timestep, 0.002f);
+ mjcf_FloatAttributeDef(OptionSection, impratio, 1);
+
+ mjcf_Vector3fAttributeDef(OptionSection, gravity, Eigen::Vector3f(0, 0, -9.81f));
+
+ mjcf_Vector3fAttributeDef(OptionSection, wind, Eigen::Vector3f::Zero());
+ mjcf_Vector3fAttributeDef(OptionSection, magnetic, Eigen::Vector3f(0, -0.5, 0));
+
+ mjcf_FloatAttributeDef(OptionSection, density, 0);
+ mjcf_FloatAttributeDef(OptionSection, viscosity, 0);
+
+ mjcf_FloatAttributeDef(OptionSection, o_margin, 0);
+ mjcf_Vector2fAttributeDef(OptionSection, o_solref, Eigen::Vector2f(0.02, 1));
+ mjcf_AttributeDef(OptionSection, Eigen::Vector5f, o_solimp, o_solimp_default());
+
+ // [Euler, RK4]
+ mjcf_StringAttributeDef(OptionSection, integrator, "Euler");
+ // [all, predefined, dynamic]
+ mjcf_StringAttributeDef(OptionSection, collision, "all");
+ // [pyramidal, elliptic]
+ mjcf_StringAttributeDef(OptionSection, cone, "pyramidal");
+ // [dense, sparse, auto]
+ mjcf_StringAttributeDef(OptionSection, jacobian, "auto");
+ // [PGS, CG, Newton]
+ mjcf_StringAttributeDef(OptionSection, solver, "Newton");
+
+ mjcf_IntAttributeDef(OptionSection, iterations, 100);
+ mjcf_FloatAttributeDef(OptionSection, tolerance, 1e-8f);
+ mjcf_IntAttributeDef(OptionSection, noslip_iterations, 0);
+ mjcf_FloatAttributeDef(OptionSection, noslip_tolerance, 1e-6f);
+ mjcf_IntAttributeDef(OptionSection, mpr_iterations, 50);
+ mjcf_FloatAttributeDef(OptionSection, mpr_tolerance, 1e-6f);
+
+private:
+
+ static Eigen::Vector5f o_solimp_default()
+ {
+ Eigen::Vector5f v;
+ v << 0.9f, 0.95f, 0.001f, 0.5, 2;
+ return v;
+ }
+};
+
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/sensor.cpp b/VirtualRobot/MJCF/elements/types/sensor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4d9a02cef8baf45ace6846bb891cb723c0cb0094
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/sensor.cpp
@@ -0,0 +1,9 @@
+#include "sensor.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string SensorSection::tag = "sensor";
diff --git a/VirtualRobot/MJCF/elements/types/sensor.h b/VirtualRobot/MJCF/elements/types/sensor.h
new file mode 100644
index 0000000000000000000000000000000000000000..fbeda6e4ec75e2f04212457e5732096a42685bd8
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/sensor.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+namespace mjcf
+{
+
+struct SensorSection : public Element<SensorSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(SensorSection)
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/size.cpp b/VirtualRobot/MJCF/elements/types/size.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..79e6a61c5a7fbef1606ca7afb2d2629e3de2ba0a
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/size.cpp
@@ -0,0 +1,8 @@
+#include "size.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+const std::string SizeSection::tag = "size";
diff --git a/VirtualRobot/MJCF/elements/types/size.h b/VirtualRobot/MJCF/elements/types/size.h
new file mode 100644
index 0000000000000000000000000000000000000000..4800f4e007100cb13a0d9b00ac1a5a130b3afb00
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/size.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+namespace mjcf
+{
+
+struct SizeSection : public Element<SizeSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(SizeSection)
+};
+
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/statistic.cpp b/VirtualRobot/MJCF/elements/types/statistic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5dfd99ff8f6431fd2fea32a76b7df71d01091250
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/statistic.cpp
@@ -0,0 +1,9 @@
+#include "statistic.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string StatisticSection::tag = "statistic";
diff --git a/VirtualRobot/MJCF/elements/types/statistic.h b/VirtualRobot/MJCF/elements/types/statistic.h
new file mode 100644
index 0000000000000000000000000000000000000000..be439e89f62f7bdc9a5d86265ec267fa700beafe
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/statistic.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+namespace mjcf
+{
+
+struct StatisticSection : public Element<StatisticSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(StatisticSection)
+};
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/tendon.cpp b/VirtualRobot/MJCF/elements/types/tendon.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9148a1bb58f3546ebd03d0f2cfe89763e59f9a4
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/tendon.cpp
@@ -0,0 +1,10 @@
+#include "tendon.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string TendonSection::tag = "tendon";
+
diff --git a/VirtualRobot/MJCF/elements/types/tendon.h b/VirtualRobot/MJCF/elements/types/tendon.h
new file mode 100644
index 0000000000000000000000000000000000000000..7fc81e5c2975767c8e87cd36f84c4e078403cb40
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/tendon.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+namespace mjcf
+{
+
+
+struct TendonSection : public Element<TendonSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(TendonSection)
+};
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/types/visual.cpp b/VirtualRobot/MJCF/elements/types/visual.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..72dd1bb81cb2b1728400f5e8329cef0106ff4986
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/visual.cpp
@@ -0,0 +1,9 @@
+#include "visual.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string VisualSection::tag = "visual";
diff --git a/VirtualRobot/MJCF/elements/types/visual.h b/VirtualRobot/MJCF/elements/types/visual.h
new file mode 100644
index 0000000000000000000000000000000000000000..4c9233cbb44594ff3f4775397d756e2a6d9172fa
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/types/visual.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "../core/Attribute.h"
+
+namespace mjcf
+{
+
+struct VisualSection : public Element<VisualSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(VisualSection)
+};
+
+}
diff --git a/VirtualRobot/MJCF/visitors.h b/VirtualRobot/MJCF/visitors.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2cec55304656772d1fcdfddcef23870ac4deb99
--- /dev/null
+++ b/VirtualRobot/MJCF/visitors.h
@@ -0,0 +1,3 @@
+#pragma once
+
+#include "visitors/Collector.h"
diff --git a/VirtualRobot/MJCF/visitors/Collector.cpp b/VirtualRobot/MJCF/visitors/Collector.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3f838bce55a8fe3bdd79dc2948abc2cdb042c234
--- /dev/null
+++ b/VirtualRobot/MJCF/visitors/Collector.cpp
@@ -0,0 +1,7 @@
+#include "Collector.h"
+
+
+namespace mjcf
+{
+
+}
diff --git a/VirtualRobot/MJCF/visitors/Collector.h b/VirtualRobot/MJCF/visitors/Collector.h
new file mode 100644
index 0000000000000000000000000000000000000000..bb8ed7e45f3c50c5ea7177b91103c7371d14f2a5
--- /dev/null
+++ b/VirtualRobot/MJCF/visitors/Collector.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "../elements.h"
+
+
+namespace mjcf
+{
+
+ /**
+ * @brief A visitor that collects all elements of the specified type.
+ */
+ template <class ElementT>
+ class Collector : public Visitor
+ {
+ public:
+
+ static std::vector<ElementT> collect(Document& document, AnyElement root);
+
+ Collector(Document& document) : Visitor(document) {}
+
+ // Visitor interface
+ virtual bool visitEnter(const AnyElement& element) override
+ {
+ if (element.is<ElementT>())
+ {
+ collected.push_back(element.as<ElementT>());
+ }
+ return true;
+ }
+
+ std::vector<ElementT>& getCollected() { return collected; }
+ const std::vector<ElementT>& getCollected() const { return collected; }
+
+
+ private:
+
+ std::vector<ElementT> collected;
+
+ };
+
+ template <class ElementT>
+ std::vector<ElementT> Collector<ElementT>::collect(Document& document, AnyElement root)
+ {
+ mjcf::Collector<ElementT> collector(document);
+ root.accept(collector);
+ return collector.getCollected();
+ }
+}
+
diff --git a/VirtualRobot/RuntimeEnvironment.cpp b/VirtualRobot/RuntimeEnvironment.cpp
index f732247d4186fbd41ec41f58f4d66f6c282fd46f..ddf09253616b702d2d2ab715f2bea58bd291a561 100644
--- a/VirtualRobot/RuntimeEnvironment.cpp
+++ b/VirtualRobot/RuntimeEnvironment.cpp
@@ -11,11 +11,18 @@
namespace VirtualRobot
{
- std::vector< std::string > RuntimeEnvironment::processKeys;
- std::vector< std::string > RuntimeEnvironment::unrecognizedOptions;
+ bool RuntimeEnvironment::pathInitialized = false;
+
+ std::string RuntimeEnvironment::caption = "Simox runtime options";
+
+ std::vector< std::pair<std::string, std::string> > RuntimeEnvironment::processKeys;
+ std::vector< std::pair<std::string, std::string> > RuntimeEnvironment::processFlags;
+
std::vector< std::string > RuntimeEnvironment::dataPaths;
+ std::vector< std::string > RuntimeEnvironment::unrecognizedOptions;
std::map< std::string, std::string > RuntimeEnvironment::keyValues;
- bool RuntimeEnvironment::pathInitialized = false;
+ std::set< std::string > RuntimeEnvironment::flags;
+ bool RuntimeEnvironment::helpFlag = false;
void RuntimeEnvironment::init()
{
@@ -100,7 +107,7 @@ namespace VirtualRobot
}
}
}
-
+
bool RuntimeEnvironment::getDataFileAbsolute(std::string& fileName)
{
if (!pathInitialized)
@@ -153,39 +160,60 @@ namespace VirtualRobot
return false;
}
-
void RuntimeEnvironment::processCommandLine(int argc, char* argv[])
{
if (!pathInitialized)
{
init();
}
+
+ const boost::program_options::options_description description = makeOptionsDescription();
+
+ const boost::program_options::parsed_options parsed =
+ boost::program_options::command_line_parser(argc, argv).options(description).allow_unregistered().run();
+ processParsedOptions(parsed);
+ }
+
+ boost::program_options::options_description RuntimeEnvironment::makeOptionsDescription()
+ {
// Declare the supported options.
- boost::program_options::options_description desc("Simox runtime options");
+
+ boost::program_options::options_description desc(caption);
desc.add_options()
- ("help", "Simox command line parser: Set options with '--key value'\n")
- ("data-path", boost::program_options::value< std::vector< std::string > >()->composing(), "Set data path. Multiple data paths are allowed.")
+ ("help", "Simox command line parser: Set options with '--key value'\n")
+ ("data-path", boost::program_options::value<std::vector<std::string>>()->composing(),
+ "Set data path. Multiple data paths are allowed.")
;
- for (auto& processKey : processKeys)
+ for (const auto& item : processKeys)
+ {
desc.add_options()
- (processKey.c_str(), boost::program_options::value< std::vector< std::string > >(), processKey.c_str())
+ (item.first.c_str(), boost::program_options::value<std::vector<std::string>>(), item.second.c_str())
;
+ }
- boost::program_options::parsed_options parsed =
- boost::program_options::command_line_parser(argc, argv).options(desc).allow_unregistered().run();
-
+ for (const auto& item : processFlags)
+ {
+ desc.add_options()
+ (item.first.c_str(), item.second.c_str())
+ ;
+ }
+ return desc;
+ }
+
+ void RuntimeEnvironment::processParsedOptions(const boost::program_options::parsed_options& parsed)
+ {
boost::program_options::variables_map vm;
//boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
boost::program_options::store(parsed, vm);
boost::program_options::notify(vm);
-
+
// process data-path entries
if (vm.count("data-path"))
{
//VR_INFO << "Data paths are: " << endl;
- std::vector< std::string > dp = vm["data-path"].as< std::vector< std::string > >();
+ std::vector<std::string> dp = vm["data-path"].as< std::vector< std::string > >();
for (const auto& i : dp)
{
@@ -195,36 +223,45 @@ namespace VirtualRobot
}
// process generic keys
- for (auto& processKey : processKeys)
+ for (const auto& processKey : processKeys)
{
- if (vm.count(processKey.c_str()))
+ const std::string& key = processKey.first;
+ if (vm.count(key.c_str()) > 0)
{
- std::vector< std::string > dp = vm[processKey.c_str()].as< std::vector< std::string > >();
+ std::vector<std::string> dp = vm[key.c_str()].as<std::vector<std::string>>();
if (dp.size() > 1)
{
- VR_WARNING << "More than one parameter for key " << processKey << ". taking the first one..." << endl;
+ VR_WARNING << "More than one parameter for key '" << key << "'. Using only first one..." << endl;
}
if (dp.size() > 0)
{
- addKeyValuePair(processKey, dp[0]); // take the first one...
+ addKeyValuePair(key, dp[0]); // take the first one...
}
}
}
+
+ for (const auto& flag : processFlags)
+ {
+ if (vm.count(flag.first.c_str()) > 0)
+ {
+ flags.insert(flag.first);
+ }
+ }
+ helpFlag = vm.count("help") > 0;
// collect unrecognized arguments
- std::vector<std::string> options = boost::program_options::collect_unrecognized(parsed.options, boost::program_options::include_positional);
+ std::vector<std::string> options = boost::program_options::collect_unrecognized(
+ parsed.options, boost::program_options::include_positional);
for (const auto& option : options)
{
unrecognizedOptions.push_back(option);
}
-
-
}
-
+
void RuntimeEnvironment::addKeyValuePair(const std::string& key, const std::string& value)
{
keyValues[key] = value;
@@ -260,6 +297,11 @@ namespace VirtualRobot
return dataPaths;
}
+
+ void RuntimeEnvironment::setCaption(const std::string& caption)
+ {
+ RuntimeEnvironment::caption = caption;
+ }
bool RuntimeEnvironment::addDataPath(const std::string& path, bool quiet)
{
@@ -285,6 +327,26 @@ namespace VirtualRobot
return false;
}
+ static std::size_t getMaxLength(const std::vector<std::pair<std::string, std::string>>& items)
+ {
+ std::size_t max = 0;
+ for (const auto& key : items)
+ {
+ max = std::max(max, key.first.size());
+ }
+ return max;
+ }
+
+ static std::string padding(std::size_t current, std::size_t target, char c = ' ')
+ {
+ std::stringstream ss;
+ while (target --> current)
+ {
+ ss << c;
+ }
+ return ss.str();
+ }
+
void RuntimeEnvironment::print()
{
if (!pathInitialized)
@@ -300,25 +362,45 @@ namespace VirtualRobot
cout << " * " << dataPath << endl;
}
- if (processKeys.size() > 0)
+ const std::size_t descriptionOffset = std::max(
+ getMaxLength(processKeys), getMaxLength(processFlags)) + 4;
+
+ auto printDescriptions = [&descriptionOffset](
+ const std::vector<std::pair<std::string, std::string>>& items,
+ const std::string& name)
{
- cout << "Known keys:" << endl;
-
- for (const auto& processKey : processKeys)
+ if (items.size() > 0)
{
- cout << " * " << processKey << endl;
+ cout << "Known " << name << ":" << endl;
+
+ for (const auto& item : items)
+ {
+ cout << " * " << item.first
+ << padding(item.first.size(), descriptionOffset)
+ << item.second << endl;
+ }
}
- }
+ };
+
+ printDescriptions(processKeys, "keys");
+ printDescriptions(processFlags, "flags");
+
if (keyValues.size() > 0)
{
cout << "Parsed options:" << endl;
- std::map< std::string, std::string >::iterator it = keyValues.begin();
-
- while (it != keyValues.end())
+ for (const auto& item : keyValues)
{
- cout << " * " << it->first << ": " << it->second << endl;
- it++;
+ cout << " * " << item.first << ": " << item.second << endl;
+ }
+ }
+
+ if (flags.size() > 0)
+ {
+ cout << "Parsed flags:" << endl;
+ for (const auto& flag : flags)
+ {
+ cout << " * " << flag << endl;
}
}
@@ -331,84 +413,98 @@ namespace VirtualRobot
cout << " * <" << unrecognizedOption << ">" << endl;
}
}
-
+ }
+
+ void RuntimeEnvironment::printOptions(std::ostream& os)
+ {
+ os << makeOptionsDescription() << std::endl;
}
- void RuntimeEnvironment::considerKey(const std::string& key)
+ void RuntimeEnvironment::considerKey(const std::string& key, const std::string& description)
{
- processKeys.push_back(key);
+ processKeys.emplace_back(key, description);
+ }
+
+ void RuntimeEnvironment::considerFlag(const std::string& flag, const std::string& description)
+ {
+ processFlags.emplace_back(flag, description);
}
bool RuntimeEnvironment::hasValue(const std::string& key)
{
- return (keyValues.find(key) != keyValues.end());
+ return keyValues.find(key) != keyValues.end();
+ }
+
+ bool RuntimeEnvironment::hasFlag(const std::string& flag)
+ {
+ return flags.find(flag) != flags.end();
+ }
+
+ bool RuntimeEnvironment::hasHelpFlag()
+ {
+ return helpFlag;
}
float RuntimeEnvironment::toFloat(const std::string& s)
{
- float a = (float)atof(s.c_str());
- return a;
+ return std::stof(s);
}
int RuntimeEnvironment::toInt(const std::string& s)
{
- int a = (int)atoi(s.c_str());
- return a;
+ return std::stoi(s);
}
- bool RuntimeEnvironment::toVector3f(const std::string& s, Eigen::Vector3f& storeResult)
+ bool RuntimeEnvironment::toVector3f(const std::string& string, Eigen::Vector3f& storeResult)
{
- if (s.length() < 3)
+ if (string.length() < 3)
{
return false;
}
- if (s[0] != '(' || s[s.length() - 1] != ')')
+ if (string[0] != '(' || string[string.length() - 1] != ')')
{
- VR_WARNING << "Expecting string to start and end with brackets (): " << s << endl;
+ VR_WARNING << "Expecting string to start and end with brackets (): " << string << endl;
return false;
}
- std::string s2 = s;
- s2.erase(s2.begin(), s2.begin() + 1);
- s2.erase(s2.end() - 1, s2.end());
- std::vector<std::string> strs;
- std::string del(",");
+ const std::string stringTrimmed = string.substr(1, string.size() - 1);
+ const std::string delimiter = ",";
- boost::split(strs, s2, boost::is_any_of(del));
+ std::vector<std::string> stringSplit;
+ boost::split(stringSplit, stringTrimmed, boost::is_any_of(delimiter));
- if (strs.size() != 3)
+ if (stringSplit.size() != 3)
{
- VR_WARNING << "Expecting values of string to be separated with a ',': " << s << endl;
+ VR_WARNING << "Expecting values of string to be separated with a ',': " << string << endl;
return false;
}
- float a = (float)atof(strs[0].c_str());
- float b = (float)atof(strs[1].c_str());
- float c = (float)atof(strs[2].c_str());
-
- if (boost::math::isinf(a) || boost::math::isinf(-a) || boost::math::isnan(a))
+ Eigen::Vector3f result;
+
+ for (int i = 0; i < result.SizeAtCompileTime; ++i)
{
- VR_WARNING << "Could not convert " << strs[0] << " to a number" << endl;
- return false;
- }
-
- if (boost::math::isinf(b) || boost::math::isinf(-b) || boost::math::isnan(b))
- {
- VR_WARNING << "Could not convert " << strs[1] << " to a number" << endl;
- return false;
- }
-
- if (boost::math::isinf(c) || boost::math::isinf(-c) || boost::math::isnan(c))
- {
- VR_WARNING << "Could not convert " << strs[2] << " to a number" << endl;
- return false;
+ const std::string& string = stringSplit[static_cast<std::size_t>(i)];
+ bool error = false;
+ float a;
+ try
+ {
+ a = std::stof(string);
+ }
+ catch (const std::invalid_argument&)
+ {
+ error = true;
+ }
+ if (error || boost::math::isinf(a) || boost::math::isinf(-a) || boost::math::isnan(a))
+ {
+ VR_WARNING << "Could not convert '" << string << "' to a number." << endl;
+ return false;
+ }
+ result(i) = a;
}
-
- storeResult(0) = a;
- storeResult(1) = b;
- storeResult(2) = c;
+
+ storeResult = result;
return true;
}
@@ -453,7 +549,7 @@ namespace VirtualRobot
void RuntimeEnvironment::cleanup()
{
- VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(nullptr);
if (visualizationFactory)
{
diff --git a/VirtualRobot/RuntimeEnvironment.h b/VirtualRobot/RuntimeEnvironment.h
index d333e6aef7e81ef416ae2f7925afd8e6c97224e5..a38c2f1392ccb6d3ec6f9b958e09604f066b428c 100644
--- a/VirtualRobot/RuntimeEnvironment.h
+++ b/VirtualRobot/RuntimeEnvironment.h
@@ -24,66 +24,73 @@
#include "VirtualRobot.h"
+#include <map>
+#include <set>
#include <string>
#include <vector>
-#include <map>
+
#include <Eigen/Core>
+
namespace VirtualRobot
{
/*!
- The runtime environment holds data paths and program arguments.
- Data files can be located by the getDataFileAbsolute method.
- Here, the environment variable SIMOX_DATA_PATH and VIRTUAL_ROBOT_DATA_PATH are considered.
- In addition, the install data directory is added at the end of the list of data paths for convenient access.
+ * The runtime environment holds data paths and program arguments.
+ * Data files can be located by the getDataFileAbsolute method.
+ * Here, the environment variable SIMOX_DATA_PATH and VIRTUAL_ROBOT_DATA_PATH are considered.
+ * In addition, the install data directory is added at the end of the list of data paths for convenient access.
*/
class VIRTUAL_ROBOT_IMPORT_EXPORT RuntimeEnvironment
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- /*!
- Return vector of all data paths.
- */
+ //! Return vector of all data paths.
static std::vector< std::string > getDataPaths();
+ /// Set the runtime environment caption.
+ static void setCaption(const std::string& caption);
+
/*!
- Add a path to global data path vector. These paths are searched within the getDataFileAbsolute() method.
- Only valid paths are processed.
- \param path The path to add.
- \param quiet If set, invalid paths are quietly ignored. Otherwise an error is printed.
- \return true on success (path has to be a directory or a symlink).
- */
+ * Add a path to global data path vector.
+ * These paths are searched within the getDataFileAbsolute() method.
+ * Only valid paths are processed.
+ * \param path The path to add.
+ * \param quiet If set, invalid paths are quietly ignored. Otherwise an error is printed.
+ * \return true on success (path has to be a directory or a symlink).
+ */
static bool addDataPath(const std::string& path, bool quiet = false);
/*!
- Enable the command line search for given key. Only keys that are enabled can later be accessed with the getValue() method.
- */
- static void considerKey(const std::string& key);
+ * \brief Enable the command line search for given key.
+ * Only keys that are enabled can later be accessed with the getValue() method.
+ */
+ static void considerKey(const std::string& key, const std::string& description = "");
+ /*!
+ * \brief Enable the command line search for given flag.
+ * A flag does not take an argument but is either present or not.
+ */
+ static void considerFlag(const std::string& flag, const std::string& description = "");
/*!
- Tries to find a file with name fileName. Therefore the working directory followed by all data paths are checked if the file can be found.
- \param fileName Could be a filename or a relative path. In case the file could be located, the absolute path is stored in place.
- \return True when the file could be located (the result will be stored in fileName).
- */
+ * Tries to find a file with name fileName. Therefore the working directory followed by all data paths are checked if the file can be found.
+ * \param fileName Could be a filename or a relative path. In case the file could be located, the absolute path is stored in place.
+ * \return True when the file could be located (the result will be stored in fileName).
+ */
static bool getDataFileAbsolute(std::string& fileName);
/*!
- The command line parameters can be passed in order to generate a map of key/value pairs.
- All "--key value" pairs for which the key was enabled with the allowCommandLineOption() method are processed and stored as std::strings in a std::map.
- In addition, all "--data-path <path>" entries are extracted and the according data paths are stored.
- All unrecognized options are also stored.
- */
+ * The command line parameters can be passed in order to generate a map of key/value pairs.
+ * All "--key value" pairs for which the key was enabled with the allowCommandLineOption() method are processed and stored as std::strings in a std::map.
+ * In addition, all "--data-path <path>" entries are extracted and the according data paths are stored.
+ * All unrecognized options are also stored.
+ */
static void processCommandLine(int argc, char* argv[]);
- /*!
- Manually add a key/value pair.
- */
+ //! Manually add a key/value pair.
static void addKeyValuePair(const std::string& key, const std::string& value);
- /*!
- Return the corresponding vale to key. If key cannot be found, defaultValue is returned.
- */
+ //! Return the corresponding vale to key. If key cannot be found, defaultValue is returned.
static std::string getValue(const std::string& key, const std::string& defaultValue = "");
template<class T>
static T getValue(const std::string& key, const T& defaultValue)
@@ -92,66 +99,85 @@ namespace VirtualRobot
}
static bool hasValue(const std::string& key);
- //! return all key value pairs
- static std::map< std::string, std::string > getKeyValuePairs();
+ /// Indicate whether the given flag was specified.
+ static bool hasFlag(const std::string& flag);
+ /// Indicate whether the 'help' flag was specified.
+ static bool hasHelpFlag();
+
+ //! Return all key value pairs
+ static std::map<std::string, std::string> getKeyValuePairs();
- //! return all unrecognized options
- static std::vector< std::string > getUnrecognizedOptions();
+ //! Return all unrecognized options
+ static std::vector<std::string> getUnrecognizedOptions();
- /*!
- Converts strings as '(a,b,c)' to 3dim Vectors.
- */
+ //! Convert strings as '(a,b,c)' to a 3 dimensional vector.
static bool toVector3f(const std::string& s, Eigen::Vector3f& storeResult);
- /*!
- * \brief toFloat Get command line parameter as float
- * \param s
- * \return
- */
+ //! Get the given string as float.
static float toFloat(const std::string& s);
+ //! Get the given string as int.
static int toInt(const std::string& s);
/*!
- Check if command line parameters specify a valid filename and
- in case the key is not present in the command line arguments or the file was not found, the standardFilename is used.
- Additionally the absolute filenames are considered by calling getDataFileAbsolute().
- \param key The key which is checked for a filename. It is checked if the key is present and if so, it is tried to
- construct a valid filename with getDataFileAbsolute().
- \param standardFilename In case a valid file could not be determined, this file will be returned
- (additionally it is made absolute by calling getDataFileAbsolute().
- Hence, a filename with a relative path can be passed
- and all datapaths are searched for it.
- \return A valid filename if it can be found, otherwise the standardFilename is returned.
+ * Check if command line parameters specify a valid filename and
+ * in case the key is not present in the command line arguments or the file was not found, the standardFilename is used.
+ * Additionally the absolute filenames are considered by calling getDataFileAbsolute().
+ * \param key The key which is checked for a filename. It is checked if the key is present and if so, it is tried to
+ * construct a valid filename with getDataFileAbsolute().
+ * \param standardFilename In case a valid file could not be determined, this file will be returned
+ * (additionally it is made absolute by calling getDataFileAbsolute().
+ * Hence, a filename with a relative path can be passed
+ * and all datapaths are searched for it.
+ * \return A valid filename if it can be found, otherwise the standardFilename is returned.
*/
static std::string checkValidFileParameter(const std::string& key, const std::string& standardFilename);
/*!
- Checks command line arguments for parameter key. iF present the corresponding value is returned,
- otherwise standardValue will be returned.
- */
+ * Checks command line arguments for parameter key.
+ * If present the corresponding value is returned, otherwise standardValue will be returned.
+ */
static std::string checkParameter(const std::string& key, const std::string& standardValue = "");
- //! Print status
+ //! Print status.
static void print();
+
+ //! Print the command line options (as printed by boost::program_options) to os.
+ static void printOptions(std::ostream& os = std::cout);
+
/*!
- Free all resources. Usually not not needed, since on application exit all resources are freed automatically.
- */
+ * \brief Free all resources.
+ * Usually not not needed, since on application exit all resources are freed automatically.
+ */
static void cleanup();
+
+
protected:
RuntimeEnvironment() {}
virtual ~RuntimeEnvironment() {}
+
static bool pathInitialized;
+
static void init();
-
-
- static std::vector< std::string > processKeys;
+ /// Make the options description based on the added keys and flags.
+ static boost::program_options::options_description makeOptionsDescription();
+ static void processParsedOptions(const boost::program_options::parsed_options& parsed);
+
+ static std::string caption;
+
+ /// Pairs of (key, description). If not given, description is empty.
+ static std::vector< std::pair<std::string, std::string> > processKeys;
+ /// Pairs of (flag, description). If not given, description is empty.
+ static std::vector< std::pair<std::string, std::string> > processFlags;
+
static std::vector< std::string > dataPaths;
static std::vector< std::string > unrecognizedOptions;
static std::map< std::string, std::string > keyValues;
+ static std::set< std::string > flags;
+ static bool helpFlag;
};
} // namespace
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 4ee8782c5a8f9bb5c5af5fe06fed34dd25cc9e52..23e3ee8c67e9e00f05d6ca2806eaab6b97b6ce87 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -15,7 +15,7 @@
#include "../RobotConfig.h"
#include "../RuntimeEnvironment.h"
#include "rapidxml.hpp"
-#include "mjcf/MujocoIO.h"
+#include "mujoco/MujocoIO.h"
#include <vector>
@@ -291,7 +291,7 @@ namespace VirtualRobot
float maxVelocity = -1.0f; // m/s
float maxAcceleration = -1.0f; // m/s^2
float maxTorque = -1.0f; // Nm
- float scaleVisu = false;
+ bool scaleVisu = false;
Eigen::Vector3f scaleVisuFactor = Eigen::Vector3f::Zero();
while (node)
@@ -1526,8 +1526,8 @@ namespace VirtualRobot
void RobotIO::saveMJCF(RobotPtr robot, const std::string& filename,
const std::string& basePath, const std::string& meshDir)
{
- mjcf::MujocoIO mujocoIO(robot);
- mujocoIO.setActuatorType(mjcf::ActuatorType::MOTOR);
+ mujoco::MujocoIO mujocoIO(robot);
+ mujocoIO.setActuatorType(mujoco::ActuatorType::MOTOR);
mujocoIO.saveMJCF(filename, basePath, meshDir);
}
diff --git a/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp b/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp
deleted file mode 100644
index e04389f540d4d08a5e3019ba8b96a57d8e5e0926..0000000000000000000000000000000000000000
--- a/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp
+++ /dev/null
@@ -1,263 +0,0 @@
-#include "MasslessBodySanitizer.h"
-#include "utils.h"
-
-#include <boost/algorithm/string/join.hpp>
-
-
-using namespace VirtualRobot;
-using namespace mjcf;
-namespace tx = tinyxml2;
-
-
-MasslessBodySanitizer::MasslessBodySanitizer(DocumentPtr& document, RobotPtr& robot) :
- document(document), robot(robot)
-{
-
-}
-
-void MasslessBodySanitizer::sanitize()
-{
- // merge body leaf nodes with parent if they do not have a mass (inertial or geom)
-
- XMLElement* root = document->robotRootBody();
-
- for (XMLElement* body = root->FirstChildElement("body");
- body;
- body = body->NextSiblingElement("body"))
- {
- sanitizeRecursion(body);
- }
-}
-
-
-void MasslessBodySanitizer::sanitizeRecursion(XMLElement* body)
-{
- assertElementIsBody(body);
-
- std::string bodyName = body->Attribute("name");
- RobotNodePtr bodyNode = robot->getRobotNode(bodyName);
- Eigen::Matrix4f accChildPose = Eigen::Matrix4f::Identity();
-
- while (!hasMass(body))
- {
- std::cout << t << bodyName << ": \t";
-
- if (!hasElementChild(body, "body"))
- {
- // leaf => end of recursion
- sanitizeLeafBody(body);
- return;
- }
-
- // non-leaf body
- // check whether there is only one child body
- XMLElement* childBody = body->FirstChildElement("body");
- if (!childBody->NextSiblingElement("body"))
- {
- mergeBodies(body, childBody, accChildPose);
- }
- else
- {
- std::cout << "Adding dummy inertial to massless body with >1 child bodies." << std::endl;
- // add a small mass
- document->addDummyInertial(body, true);
- break;
- }
- }
-
-
- // recursive descend
-
- for (XMLElement* child = body->FirstChildElement("body");
- child;
- child = child->NextSiblingElement("body"))
- {
- sanitizeRecursion(child);
- }
-
-}
-
-void MasslessBodySanitizer::mergeBodies(XMLElement* body, XMLElement* childBody,
- Eigen::Matrix4f& accChildPose)
-{
- std::string childBodyName = childBody->Attribute("name");
-
- std::cout << "Merging with '" << childBodyName << "' ";
-
- RobotNodePtr childNode = robot->getRobotNode(childBodyName);
- Eigen::Matrix4f childPose = childNode->getTransformationFrom(childNode->getParent());
-
- // update accumulated child pose
- // merged child's frame w.r.t. body's frame
- accChildPose = childPose * accChildPose;
- Eigen::Matrix3f accChildOri = accChildPose.block<3,3>(0, 0);
-
- // merge childBody into body => move all its elements here
- // while doing this, apply accChildPose to elements
- for (tx::XMLNode* grandChild = childBody->FirstChild(); grandChild;
- grandChild = grandChild->NextSibling())
- {
- // clone grandchild
- tx::XMLNode* grandChildClone = grandChild->DeepClone(nullptr);
-
- XMLElement* elem = grandChildClone->ToElement();
- if (elem)
- {
- /* Adapt pose/axis elements in child. Their poses/axes will be
- * relative to body's frame, so the transformation from body
- * to child will be lost. Therefore, apply accChildPose to
- * their poses/axes. */
-
- if (isElement(elem, "joint"))
- {
- // update pos and axis
- updateChildPos(elem, accChildPose);
- updateChildAxis(elem, accChildOri);
- }
- else if (isElement(elem, "body")
- || isElement(elem, "inertial")
- || isElement(elem, "geom")
- || isElement(elem, "site")
- || isElement(elem, "camera"))
- {
- updateChildPos(elem, accChildPose);
- updateChildQuat(elem, accChildOri);
- }
- else if (isElement(elem, "light"))
- {
- updateChildPos(elem, accChildPose);
- updateChildAxis(elem, accChildOri, "dir");
- }
- }
-
- // insert to body
- body->InsertEndChild(grandChildClone);
- }
-
- // update body name
- MergedBodySet& bodySet = getMergedBodySetWith(body->Attribute("name"));
- bodySet.addBody(childBodyName);
- body->SetAttribute("name", bodySet.getMergedBodyName().c_str());
-
- std::cout << "\t(new name: " << bodySet.getMergedBodyName() << ")" << std::endl;
-
- // delete child
- body->DeleteChild(childBody);
-}
-
-void MasslessBodySanitizer::updateChildPos(XMLElement* elem, const Eigen::Matrix4f& accChildPose)
-{
- const char* posStr = elem->Attribute("pos");
- Eigen::Vector3f pos = posStr ? strToVec(posStr) :
- Eigen::Vector3f::Zero();
-
- Eigen::Vector4f posHom;
- posHom << pos, 1;
- posHom = accChildPose * posHom;
- pos = posHom.head<3>();
-
- document->setElementPos(elem, pos);
-}
-
-void MasslessBodySanitizer::updateChildQuat(XMLElement* elem, const Eigen::Matrix3f& accChildOri)
-{
- const char* quatStr = elem->Attribute("quat");
- Eigen::Quaternionf quat = quatStr ? strToQuat(quatStr) :
- Eigen::Quaternionf::Identity();
-
- quat = accChildOri * quat;
- document->setElementQuat(elem, quat);
-}
-
-void MasslessBodySanitizer::updateChildAxis(XMLElement* elem, const Eigen::Matrix3f& accChildOri,
- const char* attrName)
-{
- Eigen::Vector3f axis = strToVec(elem->Attribute(attrName));
- axis = accChildOri * axis;
- elem->SetAttribute(attrName, toAttr(axis).c_str());
-
- if (strcmp(attrName, "axis") == 0)
- {
- document->setJointAxis(elem, axis);
- }
- else
- {
- elem->SetAttribute(attrName, toAttr(axis).c_str());
- }
-}
-
-void MasslessBodySanitizer::sanitizeLeafBody(XMLElement* body)
-{
- assert(!hasElementChild(body, "body"));
- assert(!hasMass(body));
-
- if (body->NoChildren()) // is completely empty?
- {
- // leaf without geom: make it a site
- std::cout << "Changing to site." << std::endl;
- body->SetValue("site");
- }
- else
- {
- // add a small mass
- std::cout << "Adding dummy inertial to massless leaf body with children." << std::endl;
- document->addDummyInertial(body);
- }
-}
-
-const std::vector<MergedBodySet>& MasslessBodySanitizer::getMergedBodySets() const
-{
- return mergedBodySets;
-}
-
-const std::string& MasslessBodySanitizer::getMergedBodyName(const std::string& originalBodyName)
-{
- return getMergedBodySetWith(originalBodyName).getMergedBodyName();
-}
-
-MergedBodySet&MasslessBodySanitizer::getMergedBodySetWith(const std::string& bodyName)
-{
- for (auto& set : mergedBodySets)
- {
- if (set.containsBody(bodyName))
- {
- return set;
- }
- }
-
- // not found => add
- mergedBodySets.push_back(MergedBodySet(bodyName));
-
- return mergedBodySets.back();
-}
-
-
-MergedBodySet::MergedBodySet()
-= default;
-
-MergedBodySet::MergedBodySet(const std::string& bodyName)
-{
- addBody(bodyName);
-}
-
-void MergedBodySet::addBody(const std::string& bodyName)
-{
- originalBodyNames.push_back(bodyName);
- updateMergedBodyName();
-}
-
-bool MergedBodySet::containsBody(const std::string& bodyName) const
-{
- return std::find(originalBodyNames.begin(), originalBodyNames.end(),
- bodyName) != originalBodyNames.end();
-}
-
-const std::string& MergedBodySet::getMergedBodyName() const
-{
- return mergedBodyName;
-}
-
-void MergedBodySet::updateMergedBodyName()
-{
- mergedBodyName = boost::algorithm::join(originalBodyNames, "~");
-}
diff --git a/VirtualRobot/XML/mjcf/MjcfDocument.cpp b/VirtualRobot/XML/mjcf/MjcfDocument.cpp
deleted file mode 100644
index 0224841e54cda8a1c3c464a1d0d36c0dc1027a54..0000000000000000000000000000000000000000
--- a/VirtualRobot/XML/mjcf/MjcfDocument.cpp
+++ /dev/null
@@ -1,425 +0,0 @@
-#include "MjcfDocument.h"
-
-#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
-#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
-
-
-using namespace VirtualRobot;
-using namespace mjcf;
-
-
-Document::Document()
-{
- // create root element
- root = NewElement("mujoco");
- this->InsertEndChild(root);
-}
-
-void Document::setModelName(const std::string& name)
-{
- root->SetAttribute("model", name.c_str());
-}
-
-void Document::setNewElementClass(const std::string& className, bool excludeBody)
-{
- this->newElementClass = className;
- this->newElementClassExcludeBody = excludeBody;
-}
-
-XMLElement* Document::addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2)
-{
- XMLElement* texSkybox = addNewElement(asset(), "texture");
-
- texSkybox->SetAttribute("type", "skybox");
- texSkybox->SetAttribute("builtin", "gradient");
- texSkybox->SetAttribute("width", 128);
- texSkybox->SetAttribute("height", 128);
- texSkybox->SetAttribute("rgb1", toAttr(rgb1).c_str());
- texSkybox->SetAttribute("rgb2", toAttr(rgb2).c_str());
-
- return texSkybox;
-}
-
-XMLElement* Document::addMocapBody(const std::string& name, float geomSize)
-{
- /*
- <body name="hand-ctrl" mocap="true" >
- <geom type="box" size="0.01 0.01 0.01" contype="0" conaffinity="0" rgba="0.9 0.5 0.5 0.5" />
- </body>
- */
- XMLElement* mocap = addNewElement(worldbody(), "body");
- mocap->SetAttribute("name", name.c_str());
- mocap->SetAttribute("mocap", "true");
-
- // add geom for visualization
-
- XMLElement* geom = addNewElement(mocap, "geom");
- geom->SetAttribute("type", "box");
- setAttr(geom, "size", Eigen::Vector3f{geomSize, geomSize, geomSize});
-
- return mocap;
-}
-
-
-
-XMLElement* Document::addDefaultsClass(const std::string& className)
-{
- XMLElement* def = addNewElement(default_(), "default", className);
-
- return def;
-}
-
-XMLElement* Document::addRobotRootBodyElement(const std::string& robotName)
-{
- this->robotRootBody_ = addNewElement(worldbody(), "body");
-
- robotRootBody_->SetAttribute("name", robotName.c_str());
- robotRootBody_->SetAttribute("childclass", robotName.c_str());
-
- return robotRootBody_;
-}
-
-XMLElement* Document::addBodyElement(XMLElement* parent, RobotNodePtr node)
-{
- XMLElement* body = addNewElement(parent, "body");
- body->SetAttribute("name", node->getName().c_str());
-
- if (node->hasParent())
- {
- Eigen::Matrix4f tf = node->getTransformationFrom(node->getParent());
- setElementPose(body, tf);
- }
-
- if (node->isRotationalJoint() || node->isTranslationalJoint())
- {
- addJointElement(body, node);
- }
-
- addInertialElement(body, node);
-
- return body;
-}
-
-
-XMLElement* Document::addGeomElement(XMLElement* body, const std::string& meshName)
-{
- assertElementIsBody(body);
-
- XMLElement* geom = addNewElement(body, "geom", "", true);
-
- geom->SetAttribute("type", "mesh");
- geom->SetAttribute("mesh", meshName.c_str());
-
- return geom;
-}
-
-XMLElement* Document::addInertialElement(XMLElement* body, RobotNodePtr node)
-{
- assertElementIsBody(body);
-
- Eigen::Matrix3f matrix = node->getInertiaMatrix();
- if (matrix.isIdentity(floatCompPrecision) && node->getMass() < floatCompPrecision)
- {
- // dont set an inertial element and let it be derived automatically
- return nullptr;
- }
-
- XMLElement* inertial = addNewElement(body, "inertial");
-
- Eigen::Vector3f pos = node->getCoMLocal();
- inertial->SetAttribute("pos", toAttr(pos).c_str());
- inertial->SetAttribute("mass", double(node->getMass()));
-
- if (matrix.isDiagonal(floatCompPrecision))
- {
- Eigen::Vector3f diag = matrix.diagonal();
- inertial->SetAttribute("diaginertia", toAttr(diag).c_str());
- }
- else
- {
- /* from mujoco xml reference:
- * "Full inertia matrix M. Since M is 3-by-3 and symmetric, it is
- * specified using only 6 numbers in the following order:
- * M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3)." */
- Eigen::Matrix<float, 6, 1> inertia;
- inertia << matrix(0, 0), matrix(1, 1), matrix(2, 2),
- matrix(0, 1), matrix(0, 2), matrix(1, 2);
- inertial->SetAttribute("fullinertia", toAttr(inertia).c_str());
- }
-
- return inertial;
-}
-
-XMLElement*Document::addDummyInertial(XMLElement* body, bool first)
-{
- assertElementIsBody(body);
-
- XMLElement* inertial = addNewElement(body, "inertial", "", first);
-
- inertial->SetAttribute("pos", toAttr(Eigen::Vector3f(0, 0, 0)).c_str());
- inertial->SetAttribute("mass", dummyMass);
- inertial->SetAttribute("diaginertia", toAttr(Eigen::Vector3f(1, 1, 1)).c_str());
-
- return inertial;
-}
-
-
-XMLElement* Document::addJointElement(XMLElement* body, RobotNodePtr node)
-{
- assert(node->isRotationalJoint() xor node->isTranslationalJoint());
-
- XMLElement* joint = addNewElement(body, "joint");
-
- joint->SetAttribute("name", node->getName().c_str());
-
- // get the axis
- Eigen::Vector3f axis;
- if (node->isRotationalJoint())
- {
- RobotNodeRevolutePtr revolute = boost::dynamic_pointer_cast<RobotNodeRevolute>(node);
- assert(revolute);
- axis = revolute->getJointRotationAxisInJointCoordSystem();
- }
- else if (node->isTranslationalJoint())
- {
- RobotNodePrismaticPtr prismatic = boost::dynamic_pointer_cast<RobotNodePrismatic>(node);
- assert(prismatic);
- axis = prismatic->getJointTranslationDirectionJointCoordSystem();
- }
-
- joint->SetAttribute("type", node->isRotationalJoint() ? "hinge" : "slide");
- setJointAxis(joint, axis);
- joint->SetAttribute("limited", toAttr(!node->isLimitless()).c_str());
-
- if (!node->isLimitless())
- {
- Eigen::Vector2f range(node->getJointLimitLow(), node->getJointLimitHigh());
- joint->SetAttribute("range", toAttr(range).c_str());
- }
-
- return joint;
-}
-
-XMLElement* Document::addFreeJointElement(XMLElement* body)
-{
- assertElementIsBody(body);
- return addNewElement(body, "freejoint");
-}
-
-XMLElement* Document::addMeshElement(const std::string& name, const std::string& file, const std::string& className)
-{
- XMLElement* mesh = addNewElement(asset(), "mesh", className);
-
- mesh->SetAttribute("name", name.c_str());
- mesh->SetAttribute("file", file.c_str());
-
- return mesh;
-}
-
-XMLElement* Document::addActuatorMotorElement(const std::string& jointName)
-{
- return addActuatorShortcut("motor", jointName, jointName);
-}
-
-XMLElement*Document::addActuatorPositionElement(const std::string& jointName, float kp)
-{
- return addActuatorShortcut("position", jointName, jointName, "kp", kp);
-}
-
-XMLElement*Document::addActuatorPositionElement(const std::string& jointName, bool ctrlLimited,
- const Eigen::Vector2f& ctrlRange, float kp)
-{
- XMLElement* act = addActuatorPositionElement(jointName, kp);
- act->SetAttribute("ctrllimited", toAttr(ctrlLimited).c_str());
- act->SetAttribute("ctrlrange", toAttr(ctrlRange).c_str());
- return act;
-}
-
-XMLElement*Document::addActuatorVelocityElement(const std::string& jointName, float kv)
-{
- return addActuatorShortcut("velocity", jointName, jointName, "kv", kv);
-}
-
-XMLElement* Document::addEqualityWeld(const std::string& name, const std::string& body1, const std::string& body2, const std::string& className)
-{
- XMLElement* weld = addNewElement(equality(), "weld", className);
-
- weld->SetAttribute("name", name.c_str());
- weld->SetAttribute("body1", body1.c_str());
- weld->SetAttribute("body2", body2.c_str());
-
- return weld;
-}
-
-
-XMLElement* Document::addActuatorShortcut(
- const std::string& type, const std::string& name, const std::string& jointName,
- const std::string& paramName, float paramValue)
-{
- XMLElement* motor = addNewElement(actuator(), type.c_str());
-
- motor->SetAttribute("name", name.c_str());
- motor->SetAttribute("joint", jointName.c_str());
-
- if (!paramName.empty() && paramValue > floatCompPrecision)
- {
- motor->SetAttribute(paramName.c_str(), paramValue);
- }
-
- return motor;
-}
-
-
-void Document::setElementPose(XMLElement* elem, const Eigen::Matrix4f& pose)
-{
- Eigen::Vector3f pos = pose.block<3,1>(0, 3);
- Eigen::Matrix3f oriMat = pose.block<3,3>(0, 0);
-
- Eigen::Quaternionf quat(oriMat);
-
- setElementPos(elem, pos);
- setElementQuat(elem, quat);
-}
-
-void Document::setElementPos(XMLElement* elem, Eigen::Vector3f pos)
-{
- if (!pos.isZero(floatCompPrecision))
- {
- elem->SetAttribute("pos", toAttr(pos).c_str());
- }
- else
- {
- elem->DeleteAttribute("pos");
- }
-}
-
-void Document::setElementQuat(XMLElement* elem, const Eigen::Quaternionf& quat)
-{
- if (!quat.isApprox(Eigen::Quaternionf::Identity(), floatCompPrecision))
- {
- elem->SetAttribute("quat", toAttr(quat).c_str());
- }
- else
- {
- elem->DeleteAttribute("quat");
- }
-}
-
-void Document::setJointAxis(XMLElement* joint, const Eigen::Vector3f& axis)
-{
- assertElementIs(joint, "joint");
- joint->SetAttribute("axis", toAttr(axis).c_str());
-}
-
-
-XMLElement* Document::addContactExclude(const XMLElement& body1, const XMLElement& body2)
-{
- return addContactExclude(body1.Attribute("name"), body2.Attribute("name"));
-}
-
-XMLElement* Document::addContactExclude(const std::string& body1Name, const std::string& body2Name)
-{
- XMLElement* exclude = addNewElement(contact(), "exclude");
-
- exclude->SetAttribute("body1", body1Name.c_str());
- exclude->SetAttribute("body2", body2Name.c_str());
-
- return exclude;
-}
-
-
-XMLElement* Document::addNewElement(XMLElement* parent, const std::string& elemName,
- const std::string& className, bool first)
-{
- XMLElement* elem = NewElement(elemName.c_str());
-
- if (!className.empty())
- {
- elem->SetAttribute("class", className.c_str());
- }
- else if (!newElementClass.empty()
- && !(newElementClassExcludeBody && (isElement(parent, "body") || isElement(elem, "body")))
- && allowsClassAttr({parent->Value(), elemName}))
- {
- elem->SetAttribute("class", newElementClass.c_str());
- }
-
- if (first)
- {
- parent->InsertFirstChild(elem);
- }
- else
- {
- parent->InsertEndChild(elem);
- }
- return elem;
-}
-
-XMLElement* Document::getOrCreateElement(XMLElement* parent, const std::string& elemName)
-{
- XMLElement* elem = parent->FirstChildElement(elemName.c_str());
- if (!elem)
- {
- elem = addNewElement(parent, elemName);
- }
- return elem;
-}
-
-XMLElement* Document::section(const std::string& name)
-{
- return getOrCreateElement(root, name);
-}
-
-void Document::setDummyMass(float value)
-{
- dummyMass = value;
-}
-
-void Document::setFloatCompPrecision(float value)
-{
- floatCompPrecision = value;
-}
-
-
-XMLElement* Document::robotRootBody() const
-{
- return robotRootBody_;
-}
-
-
-const std::set<Document::ElementType> Document::ELEM_NAMES_WITH_ATTR_CLASS =
-{
- {"asset", "mesh" },
- {"asset", "material"},
-
- {"body", "joint" },
- {"body", "geom" },
- {"body", "site" },
- {"body", "camera" },
- {"body", "light" },
-
- {"contact", "pair" },
-
- {"equality", "connect" },
- {"equality", "weld" },
- {"equality", "joint" },
- {"equality", "tendon" },
- {"equality", "distance" },
-
- {"tendon", "spatial" },
- {"tendon", "fixed" },
-
- {"actuator", "general" },
- {"actuator", "motor" },
- {"actuator", "position" },
- {"actuator", "velocity" },
- {"actuator", "cylinder" },
- {"actuator", "muscle" },
-};
-
-bool Document::allowsClassAttr(const Document::ElementType& type)
-{
- return ELEM_NAMES_WITH_ATTR_CLASS.find(type) != ELEM_NAMES_WITH_ATTR_CLASS.end();
-}
-
-
diff --git a/VirtualRobot/XML/mjcf/MjcfDocument.h b/VirtualRobot/XML/mjcf/MjcfDocument.h
deleted file mode 100644
index 6f89d5c7c98c7737c2b6b64d27d0dc015612e86f..0000000000000000000000000000000000000000
--- a/VirtualRobot/XML/mjcf/MjcfDocument.h
+++ /dev/null
@@ -1,204 +0,0 @@
-#pragma once
-
-#include <memory>
-
-#include <Eigen/Eigen>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/Util/xml/tinyxml2.h>
-
-#include "utils.h"
-
-
-namespace VirtualRobot
-{
-namespace mjcf
-{
-
- using XMLElement = tinyxml2::XMLElement;
-
- /**
- * @brief A MJCF (Mujoco XML) document.
- */
- class Document : public tinyxml2::XMLDocument
- {
-
- public:
-
- /// Constructor.
- Document();
-
-
- /// Set the precision for float comparison (used e.g. when comparing
- /// to zero or identity).
- void setFloatCompPrecision(float value);
- /// Set the mass set in dummy inertial elements.
- void setDummyMass(float value);
-
-
- /// Set the name of the Mujoco model.
- void setModelName(const std::string& name);
-
- /**
- * @brief Set the class attribute of all new applicable elements
- * (after calling this method). Pass an empty string to disable class attributes.
- * @param excludeBody If true (default), the class will not be set on
- * new body elements and its children (inertial, joint, ...).
- * They should be the childclass attribute of the robot root body.
- */
- void setNewElementClass(const std::string& className, bool excludeBody = true);
-
-
- // Section elements (children of top-level 'mujoco' element).
- XMLElement* compiler() { return section("compiler"); }
- XMLElement* option() { return section("option"); }
- XMLElement* size() { return section("size"); }
- XMLElement* visual() { return section("visual"); }
- XMLElement* statistic() { return section("statistic"); }
- XMLElement* default_() { return section("default"); }
- XMLElement* asset() { return section("asset"); }
- XMLElement* worldbody() { return section("worldbody"); }
- XMLElement* contact() { return section("contact"); }
- XMLElement* equality() { return section("equality"); }
- XMLElement* tendon() { return section("tendon"); }
- XMLElement* actuator() { return section("actuator"); }
- XMLElement* sensor() { return section("sensor"); }
- XMLElement* keyframe() { return section("keyframe"); }
-
-
- /// Adds a body element to the worldbody with the robot's name as name and childclass.
- XMLElement* addRobotRootBodyElement(const std::string& robotName);
- /// Get the (least recently added) robot root body.
- XMLElement* robotRootBody() const;
-
- /// Add a new defaults class with a class name.
- XMLElement* addDefaultsClass(const std::string& className);
-
- template <typename AttrT>
- XMLElement* addDefaultAttr(XMLElement* defaultsClass, const std::string& elementName,
- const std::string& attrName, const AttrT& attrValue);
-
-
- /// Add a skybox texture asset (only one allowed).
- XMLElement* addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2);
-
- /// Add a mocap body with the given name to the worldbody.
- XMLElement* addMocapBody(const std::string& name, float geomSize);
-
- /// Add a body element to a parent from a RobotNode.
- /// Adds inertial and joint element, if necessary.
- XMLElement* addBodyElement(XMLElement* parent, RobotNodePtr node);
-
- /// Add an inertial element to a body from a RobotNode.
- XMLElement* addInertialElement(XMLElement* body, RobotNodePtr node);
- /// Add a dummy inertial element with small mass and identity inertia matrix.
- XMLElement* addDummyInertial(XMLElement* body, bool first=false);
- /// Add a joint element to a body from a RobotNode.
- XMLElement* addJointElement(XMLElement* body, RobotNodePtr node);
- /// Add a free joint element to a body.
- XMLElement* addFreeJointElement(XMLElement* body);
-
- /// Add a geom to a body, referencing a mesh.
- XMLElement* addGeomElement(XMLElement* body, const std::string& meshName);
- /// Add a mesh asset with a name and a file.
- XMLElement* addMeshElement(const std::string& name, const std::string& file,
- const std::string& className = "");
-
- /// Add a conact/exclude element between the given bodies.
- XMLElement* addContactExclude(const XMLElement& body1, const XMLElement& body2);
- /// Add a conact/exclude element between the given bodies.
- XMLElement* addContactExclude(const std::string& body1Name, const std::string& body2Name);
-
- XMLElement* addActuatorMotorElement(const std::string& jointName);
- XMLElement* addActuatorPositionElement(const std::string& jointName, float kp = 0);
- XMLElement* addActuatorPositionElement(
- const std::string& jointName, bool ctrlLimited, const Eigen::Vector2f& ctrlRange, float kp = 0);
- XMLElement* addActuatorVelocityElement(const std::string& jointName, float kv = 0);
-
- XMLElement* addEqualityWeld(const std::string& name, const std::string& body1, const std::string& body2,
- const std::string& className="");
-
- /// Set the pos and quat attribute of an element.
- void setElementPose(XMLElement* elem, const Eigen::Matrix4f& pose);
- void setElementPos(XMLElement* elem, Eigen::Vector3f pos);
- void setElementQuat(XMLElement* elem, const Eigen::Quaternionf& quat);
-
- /// Set the axis attribute of a joint.
- void setJointAxis(XMLElement* joint, const Eigen::Vector3f& axis);
-
-
-
- private:
-
- /// (ParentTag, ElementTag)
- using ElementType = std::pair<std::string, std::string>;
-
- /// Element types allowing a class attribute.
- static const std::set<ElementType> ELEM_NAMES_WITH_ATTR_CLASS;
-
- /// Return true if the given ElementType allows for a class attribute.
- static bool allowsClassAttr(const ElementType& type);
-
-
- private:
-
-
-
- /// Add a new element with a name (tag) to a parent.
- /// @param className If not empty, set the class attribute of the new element.
- /// @param first If true, will be inserted as first, otherweise at end (default)
- XMLElement* addNewElement(XMLElement* parent, const std::string& elemName,
- const std::string& className = "", bool first = false);
-
- /// Return the first child element of parent with the given element name.
- /// If it does not exist, create it.
- XMLElement* getOrCreateElement(XMLElement* parent, const std::string& elemName);
-
- /// Gets the section element (child of root element) with a name.
- /// If it does not exist, it is created.
- XMLElement* section(const std::string& name);
-
- XMLElement* addActuatorShortcut(
- const std::string& type, const std::string& name, const std::string& jointName,
- const std::string& paramName = "", float paramValue = 0);
-
-
- /// Precision when comparing floats (e.g. with zero).
- float floatCompPrecision = 1e-6f;
- /// Mass used for dummy inertial elements.
- float dummyMass = 0.0001f;
-
-
- /// The "mujoco" root element.
- XMLElement* root;
-
- /// The wrapper element of the robot.
- XMLElement* robotRootBody_;
-
-
- /// The class added to new elements, if applicable.
- std::string newElementClass = "";
- /// Indicate whether body elements and their children shall be
- /// exluded from setting the class attribute.
- bool newElementClassExcludeBody = true;
-
-
- };
-
- template<typename AttrT>
- XMLElement* Document::addDefaultAttr(
- XMLElement* defaultsClass, const std::string& elementName,
- const std::string& attrName, const AttrT& attrValue)
- {
- assertElementIs(defaultsClass, "default");
-
- XMLElement* element = getOrCreateElement(defaultsClass, elementName);
- element->SetAttribute(attrName.c_str(), attrValue);
-
- return element;
- }
-
- using DocumentPtr = std::unique_ptr<Document>;
-
-}
-}
diff --git a/VirtualRobot/XML/mjcf/MujocoIO.cpp b/VirtualRobot/XML/mjcf/MujocoIO.cpp
deleted file mode 100644
index 36655017333a07dba7546e84eff09ff9acfd16de..0000000000000000000000000000000000000000
--- a/VirtualRobot/XML/mjcf/MujocoIO.cpp
+++ /dev/null
@@ -1,520 +0,0 @@
-#include "MujocoIO.h"
-#include "utils.h"
-
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/VirtualRobotException.h>
-#include <VirtualRobot/Visualization/TriMeshModel.h>
-
-
-using namespace VirtualRobot;
-using namespace mjcf;
-namespace tx = tinyxml2;
-namespace fs = std::filesystem;
-
-namespace
-{
- namespace fs = std::filesystem;
- inline fs::path remove_trailing_separator(fs::path p)
- {
- p /= "dummy";
- return p.parent_path();
- }
-}
-
-
-MujocoIO::MujocoIO(RobotPtr robot) : robot(robot)
-{
- THROW_VR_EXCEPTION_IF(!robot, "Given RobotPtr robot is null.");
-}
-
-void MujocoIO::saveMJCF(const std::string& filename, const std::string& basePath,
- const std::string& meshRelDir)
-{
- THROW_VR_EXCEPTION_IF(filename.empty(), "Given filename is empty.");
-
- setPaths(filename, basePath, meshRelDir);
-
- document.reset(new mjcf::Document());
- document->setModelName(robot->getName());
-
- makeCompiler();
-
- makeDefaultsGroup();
- document->setNewElementClass(robot->getName(), true);
-
- addSkybox();
-
- if (withMocapBody)
- {
- std::cout << "Adding mocap body ..." << std::endl;
- addMocapBody();
- }
-
- std::cout << "Creating bodies structure ..." << std::endl;
- makeNodeBodies();
-
- std::cout << "Adding meshes and geoms ..." << std::endl;
- addNodeBodyMeshes();
-
- bool print = false;
- if (print)
- {
- std::cout << "===========================" << std::endl
- << "Current model: " << std::endl
- << "--------------" << std::endl;
- document->Print();
- std::cout << "===========================" << std::endl;
- }
-
- std::cout << "Merging massless bodies ..." << std::endl;
- masslessBodySanitizer.sanitize();
-
- std::cout << "Adding contact excludes ..." << std::endl;
- addContactExcludes();
-
- std::cout << "Adding actuators ..." << std::endl;
- addActuators();
-
- std::cout << "Scaling lengths by " << lengthScaling << " ..." << std::endl;
- scaleLengths(document->robotRootBody());
-
- std::cout << "Done." << std::endl;
-
- if (print)
- {
- std::cout << std::endl;
- std::cout << "===========================" << std::endl
- << "Output file: " << std::endl
- << "------------" << std::endl;
- document->Print();
- std::cout << "===========================" << std::endl;
- }
-
- assert(!outputFileName.empty());
- std::cout << "Writing to " << (outputDirectory / outputFileName) << std::endl;
- document->SaveFile((outputDirectory / outputFileName).c_str());
-}
-
-void MujocoIO::setPaths(const std::string& filename, const std::string& basePath, const std::string& meshRelDir)
-{
- outputDirectory = basePath;
- outputFileName = filename;
- outputMeshRelDirectory = meshRelDir;
-
- ensureDirectoriesExist();
-}
-
-void MujocoIO::ensureDirectoriesExist()
-{
- auto ensureDirExists = [](const fs::path & dir, const std::string & errMsgName)
- {
- if (!fs::is_directory(dir))
- {
- std::cout << "Creating directory: " << dir << std::endl;
- bool success = fs::create_directories(remove_trailing_separator(dir));
- THROW_VR_EXCEPTION_IF(!success, "Could not create " << errMsgName << ": " << dir);
- }
- };
-
- ensureDirExists(outputDirectory, "output directory");
- ensureDirExists(outputMeshDirectory(), "output mesh directory");
-}
-
-void MujocoIO::makeCompiler()
-{
- document->compiler()->SetAttribute("angle", "radian");
- document->compiler()->SetAttribute("balanceinertia", "true");
-}
-
-void MujocoIO::makeDefaultsGroup()
-{
- XMLElement* defaultsClass = document->addDefaultsClass(robot->getName());
-
- std::stringstream comment;
- comment << "Add default values for " << robot->getName() << " here.";
- defaultsClass->InsertFirstChild(document->NewComment(comment.str().c_str()));
-
- document->addDefaultAttr(defaultsClass, "joint", "frictionloss", 1);
- document->addDefaultAttr(defaultsClass, "joint", "damping", 0);
- document->addDefaultAttr(defaultsClass, "geom", "condim", 4);
- document->addDefaultAttr(defaultsClass, "position", "kp", 1);
- document->addDefaultAttr(defaultsClass, "velocity", "kv", 1);
-}
-
-
-void MujocoIO::addSkybox()
-{
- document->addSkyboxTexture(Eigen::Vector3f(.8f, .9f, .95f),
- Eigen::Vector3f(.4f, .6f, .8f));
-}
-
-
-void MujocoIO::addMocapBody()
-{
- std::string className = "mocap";
- float geomSize = 0.01f;
-
- std::string bodyName;
- {
- std::stringstream ss;
- ss << robot->getName() << "_Mocap";
- bodyName = ss.str();
- }
-
- // add defaults class
- XMLElement* defClass = document->addDefaultsClass(className);
-
- document->addDefaultAttr(defClass, "geom", "rgba",
- toAttr(Eigen::Vector4f(.9f, .5f, .5f, .5f)).c_str());
-
- document->addDefaultAttr(defClass, "equality", "solimp",
- toAttr(Eigen::Vector3f{.95f, .99f, .001f}).c_str());
- document->addDefaultAttr(defClass, "equality", "solref",
- toAttr(Eigen::Vector2f{ .02f, 1.f}).c_str());
-
- // add body
- XMLElement* mocap = document->addMocapBody(bodyName, geomSize);
- mocap->SetAttribute("childclass", className.c_str());
-
- // add equality weld constraint
- document->addEqualityWeld(bodyName, robot->getName(), bodyName, className);
-}
-
-
-void MujocoIO::makeNodeBodies()
-{
- nodeBodies.clear();
-
- RobotNodePtr rootNode = robot->getRootNode();
- assert(rootNode);
-
- // add root
- XMLElement* robotRootBody = document->addRobotRootBodyElement(robot->getName());
-
- if (withMocapBody)
- {
- document->addDummyInertial(robotRootBody);
- XMLElement* joint = document->addFreeJointElement(robotRootBody);
- joint->SetAttribute("name", robot->getName().c_str());
- }
-
- XMLElement* root = document->addBodyElement(robotRootBody, rootNode);
- nodeBodies[rootNode->getName()] = root;
- assert(root);
-
- for (RobotNodePtr node : robot->getRobotNodes())
- {
- addNodeBody(node);
- }
-}
-
-void MujocoIO::addNodeBodyMeshes()
-{
- bool meshlabserverAviable = system("which meshlabserver > /dev/null 2>&1") == 0;
- bool notAvailableReported = false;
-
- for (RobotNodePtr node : robot->getRobotNodes())
- {
- VisualizationNodePtr visualization = node->getVisualization(SceneObject::VisualizationType::Full);
-
- if (!visualization)
- {
- continue;
- }
-
- std::cout << t << "Node " << node->getName() << ":\t";
-
- fs::path srcMeshPath = visualization->getFilename();
-
- fs::path dstMeshFileName = srcMeshPath.filename();
- dstMeshFileName.replace_extension("stl");
- fs::path dstMeshPath = outputMeshDirectory() / dstMeshFileName;
-
- if (!fs::exists(dstMeshPath))
- {
- if (srcMeshPath.extension().string() != "stl")
- {
- std::cout << "Converting to .stl: " << srcMeshPath;
-
- if (!meshlabserverAviable)
- {
- if (!notAvailableReported)
- {
- std::cout << std::endl
- << "Command 'meshlabserver' not available, "
- "cannot convert meshes." << std::endl;
- notAvailableReported = true;
- }
- continue;
- }
-
- // meshlabserver available
- std::stringstream convertCommand;
- convertCommand << "meshlabserver"
- << " -i " << srcMeshPath
- << " -o " << dstMeshPath;
-
- // run command
- std::cout << "----------------------------------------------------------" << std::endl;
- int r = system(convertCommand.str().c_str());
- std::cout << "----------------------------------------------------------" << std::endl;
- if (r != 0)
- {
- std::cout << "Command returned with error: " << r << "\n"
- << "Command was: " << convertCommand.str() << std::endl;
- }
- }
- else
- {
- std::cout << "Copying: " << srcMeshPath << "\n"
- << " to: " << dstMeshPath;
- fs::copy_file(srcMeshPath, dstMeshPath);
- }
- }
- else
- {
- std::cout << "skipping (" << outputMeshRelDirectory / dstMeshFileName
- << " already exists)";
- }
- std::cout << std::endl;
-
-
-
- // add asset
- std::string meshName = node->getName();
- document->addMeshElement(meshName, fs::absolute(dstMeshPath).string());
-
- // add geom to body
- XMLElement* body = nodeBodies[node->getName()];
- document->addGeomElement(body, meshName);
- }
-}
-
-
-
-XMLElement* MujocoIO::addNodeBody(RobotNodePtr node)
-{
- XMLElement* element = nodeBodies[node->getName()];
- if (element)
- {
- // break recursion
- return element;
- }
-
- XMLElement* parent = nodeBodies[node->getParent()->getName()];
- if (!parent)
- {
- parent = addNodeBody(robot->getRobotNode(node->getParent()->getName()));
- }
-
- element = document->addBodyElement(parent, node);
- nodeBodies[node->getName()] = element;
-
- return element;
-}
-
-struct ParentChildContactExcludeVisitor : public tinyxml2::XMLVisitor
-{
-
- ParentChildContactExcludeVisitor(Document& document) : document(document) {}
- ~ParentChildContactExcludeVisitor() override = default;
-
- bool VisitEnter(const tinyxml2::XMLElement&, const tinyxml2::XMLAttribute*) override;
-
- Document& document; ///< The document.
- bool firstSkipped = false; ///< Used to skip the root element.
-};
-
-bool ParentChildContactExcludeVisitor::VisitEnter(const tinyxml2::XMLElement& body, const tinyxml2::XMLAttribute*)
-{
- if (!isElement(body, "body"))
- {
- return true;
- }
-
- if (!firstSkipped)
- {
- firstSkipped = true;
- return true;
- }
-
- const XMLElement* parent = body.Parent()->ToElement();
- assert(parent);
-
- document.addContactExclude(*parent, body);
-
- return true;
-}
-
-
-void MujocoIO::addContactExcludes()
-{
- RobotNodePtr rootNode = robot->getRootNode();
-
- std::vector<std::pair<std::string, std::string>> excludePairs;
-
- for (RobotNodePtr node : robot->getRobotNodes())
- {
- for (std::string& ignore : node->getPhysics().ignoreCollisions)
- {
- // I found an <IgnoreCollision> element referring to a non-existing node.
- // => check node existence here
- if (robot->hasRobotNode(ignore))
- {
- excludePairs.push_back({node->getName(), ignore});
- }
- }
- }
-
- // resolve body names and add exludes
- for (auto& excludePair : excludePairs)
- {
- std::string body1 = masslessBodySanitizer.getMergedBodyName(excludePair.first);
- std::string body2 = masslessBodySanitizer.getMergedBodyName(excludePair.second);
- document->addContactExclude(body1, body2);
- }
-
- ParentChildContactExcludeVisitor visitor(*document);
- document->robotRootBody()->Accept(&visitor);
-}
-
-void MujocoIO::addActuators()
-{
- std::vector<const mjcf::XMLElement*> jointElements = getAllElements("joint");
-
- for (auto joint : jointElements)
- {
- std::string name = joint->Attribute("name");
- switch (actuatorType)
- {
- case ActuatorType::MOTOR:
- document->addActuatorMotorElement(name);
- break;
-
- case ActuatorType::POSITION:
- {
- XMLElement* act = document->addActuatorPositionElement(name);
-
- const char* limited = joint->Attribute("limited");
- if (limited)
- {
- act->SetAttribute("ctrllimited", limited);
-
- const char* range = joint->Attribute("range");
- if (range)
- {
- act->SetAttribute("ctrlrange", range);
- }
- }
- }
- break;
-
- case ActuatorType::VELOCITY:
- document->addActuatorVelocityElement(name);
- break;
- }
- }
-}
-
-void MujocoIO::scaleLengths(XMLElement* elem)
-{
- assert(elem);
-
- if (isElement(elem, "joint"))
- {
- if (isAttr(elem, "type", "slide") && hasAttr(elem, "range"))
- {
- std::cout << t << "Scaling range of slide joint "
- << elem->Attribute("name") << std::endl;
-
- Eigen::Vector2f range = strToVec2(elem->Attribute("range"));
- range *= lengthScaling;
- setAttr(elem, "range", range);
- }
- }
- else if (isElement(elem, "position")
- //&& isElement(elem.Parent()->ToElement(), "actuator")
- && hasAttr(elem, "ctrlrange"))
- {
- std::cout << t << "Scaling ctrlrange of position actuator "
- << elem->Attribute("name") << std::endl;
-
- Eigen::Vector2f ctrlrange = strToVec2(elem->Attribute("ctrlrange"));
- ctrlrange *= lengthScaling;
- setAttr(elem, "ctrlrange", ctrlrange);
- }
- else if (hasAttr(elem, "pos"))
- {
- std::cout << t << "Scaling pos of " << elem->Value() << " ";
- if (hasAttr(elem, "name"))
- {
- std::cout << "'" << elem->Attribute("name") << "'";
- }
- else
- {
- std::cout << "element";
- }
- std::cout << std::endl;
-
- Eigen::Vector3f pos = strToVec(elem->Attribute("pos"));
- pos *= lengthScaling;
- setAttr(elem, "pos", pos);
- }
-
-
- for (XMLElement* child = elem->FirstChildElement();
- child;
- child = child->NextSiblingElement())
- {
- scaleLengths(child);
- }
-}
-
-
-struct ListElementsVisitor : public tinyxml2::XMLVisitor
-{
-
- ListElementsVisitor(const std::string& elementName) : elementName(elementName) {}
- ~ListElementsVisitor() override = default;
-
- // XMLVisitor interface
- bool VisitEnter(const tinyxml2::XMLElement&, const tinyxml2::XMLAttribute*) override;
-
- const std::vector<const tinyxml2::XMLElement*>& getFoundElements() const;
-
- std::string elementName;
- std::vector<const tinyxml2::XMLElement*> foundElements;
-};
-
-bool ListElementsVisitor::VisitEnter(const tinyxml2::XMLElement& elem, const tinyxml2::XMLAttribute*)
-{
- if (isElement(elem, elementName))
- {
- foundElements.push_back(&elem);
- }
- return true;
-}
-
-std::vector<const mjcf::XMLElement*> MujocoIO::getAllElements(const std::string& elemName)
-{
- ListElementsVisitor visitor(elemName);
- document->worldbody()->Accept(&visitor);
- return visitor.foundElements;
-}
-
-void MujocoIO::setLengthScaling(float value)
-{
- lengthScaling = value;
-}
-
-void MujocoIO::setActuatorType(ActuatorType value)
-{
- actuatorType = value;
-}
-
-void MujocoIO::setWithMocapBody(bool enabled)
-{
- withMocapBody = enabled;
-}
-
diff --git a/VirtualRobot/XML/mjcf/MujocoIO.h b/VirtualRobot/XML/mjcf/MujocoIO.h
deleted file mode 100644
index 91a83d701c4655204b6aa3eba2b335a40ac44651..0000000000000000000000000000000000000000
--- a/VirtualRobot/XML/mjcf/MujocoIO.h
+++ /dev/null
@@ -1,139 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include <VirtualRobot/Robot.h>
-
-#include "MjcfDocument.h"
-#include "MasslessBodySanitizer.h"
-#include "utils.h"
-
-
-namespace VirtualRobot
-{
- namespace mjcf
- {
-
- class MujocoIO
- {
- public:
-
- /// Constructor.
- /// @throws VirtualRobotException if robot is null
- MujocoIO(RobotPtr robot);
-
- /**
- * @brief Create a Mujoco XML (MJCF) document for the given robot.
- * @param filename the output filename (without directory)
- * @param basePath the output directory
- * @param meshRelDir the directory relative to basePath where meshes shall be placed
- */
- void saveMJCF(const std::string& filename, const std::string& basePath,
- const std::string& meshRelDir);
-
-
- /// Set the scaling for lenghts.
- void setLengthScaling(float value);
-
- /// Set the actuator type.
- void setActuatorType(ActuatorType value);
-
- /**
- * @brief Enable or disable adding of a mocap body controlling the robot pose.
- *
- * @param enabled If true:
- * - Add a free joint to the robot root body mocap body
- * - Add a mocap body in the world body (called <RobotName>_Mocap)
- * - Add a weld constraint welding them together.
- */
- void setWithMocapBody(bool enabled);
-
-
- private:
-
- /// Set the output path members.
- void setPaths(const std::string& filename, const std::string& basePath,
- const std::string& meshRelDir);
- /// Create output directories if the do not exist.
- void ensureDirectoriesExist();
-
- /// Make the compiler section.
- void makeCompiler();
- /// Add a defaults group for the robot.
- void makeDefaultsGroup();
- /// Add a skybox texture asset.
- void addSkybox();
-
- /// Add a mocap body with defaults group.
- void addMocapBody();
-
- /// Construct the body structure corresponding to the robot nodes.
- void makeNodeBodies();
- /// Add a body element for a robot node. If its parent does not exist,
- /// create the parent body first.
- mjcf::XMLElement* addNodeBody(RobotNodePtr node);
-
- /// Convert meshes and add mesh assets for robot nodes with visualization.
- void addNodeBodyMeshes();
-
- /// Add contact exclude elements for IgnoreCollision elements.
- void addContactExcludes();
-
- /// Add actuators for all joints.
- void addActuators();
-
- /// Scale all lengths by lengthScaling.
- void scaleLengths(XMLElement* elem);
-
- std::vector<const mjcf::XMLElement*> getAllElements(const std::string& elemName);
-
-
- // Paremeters
-
- /// Scaling for lengths, such as positions and translations.
- float lengthScaling = 0.001f;
- /// The actuator type.
- ActuatorType actuatorType = ActuatorType::MOTOR;
-
- /// Add a mocap
- bool withMocapBody = false;
-
-
- // Paths
-
- std::filesystem::path outputDirectory;
- std::filesystem::path outputFileName;
- std::filesystem::path outputMeshRelDirectory;
- std::filesystem::path outputMeshDirectory()
- {
- return outputDirectory / outputMeshRelDirectory;
- }
-
-
- // Input
-
- /// The input robot.
- RobotPtr robot;
-
-
- // Output
-
- /// The built MJCF document.
- DocumentPtr document = nullptr;
-
-
- // Processing
-
- /// Sanitizes massless bodies.
- mjcf::MasslessBodySanitizer masslessBodySanitizer {document, robot};
-
- /// Map of robot node names to XML elements.
- std::map<std::string, mjcf::XMLElement*> nodeBodies;
-
-
- const std::string t = "| ";
-
- };
-
- }
-}
diff --git a/VirtualRobot/XML/mjcf/utils.cpp b/VirtualRobot/XML/mjcf/utils.cpp
deleted file mode 100644
index be83738e4487d531078f6a3f2fc881b4e7ed99f2..0000000000000000000000000000000000000000
--- a/VirtualRobot/XML/mjcf/utils.cpp
+++ /dev/null
@@ -1,139 +0,0 @@
-#include "utils.h"
-
-#include <algorithm>
-#include <map>
-
-
-namespace VirtualRobot
-{
-namespace mjcf
-{
-
-ActuatorType toActuatorType(std::string str)
-{
- static const std::map<std::string, ActuatorType> map {
- {"motor", ActuatorType::MOTOR},
- {"position", ActuatorType::POSITION},
- {"velocity", ActuatorType::VELOCITY}
- };
- std::transform(str.begin(), str.end(), str.begin(), ::tolower);
- return map.at(str);
-}
-
-
-
-bool isElement(const XMLElement* elem, const char* tag)
-{
- return isElement(*elem, tag);
-}
-
-bool isElement(const XMLElement* elem, const std::string& tag)
-{
- return isElement(*elem, tag);
-}
-
-bool isElement(const XMLElement& elem, const char* tag)
-{
- return std::strcmp(elem.Value(), tag) == 0;
-}
-
-bool isElement(const XMLElement& elem, const std::string& tag)
-{
- return std::strcmp(elem.Value(), tag.c_str()) == 0;
-}
-
-
-bool hasElementChild(const XMLElement* elem, const std::string& elemName)
-{
- return elem->FirstChildElement(elemName.c_str()) != nullptr;
-}
-
-bool hasMass(const XMLElement* body)
-{
- assertElementIsBody(body);
- return hasElementChild(body, "geom") || hasElementChild(body, "inertial");
-}
-
-
-
-std::string toAttr(bool b)
-{
- static const std::string strings[] = { "false", "true" };
- return strings[int(b)];
-}
-
-std::string toAttr(const Eigen::Quaternionf& quat)
-{
- std::stringstream ss;
- ss << quat.w() << " " << quat.x() << " " << quat.y() << " " << quat.z();
- return ss.str();
-}
-
-
-Eigen::Vector2f strToVec2(const char* string)
-{
- Eigen::Vector2f v;
- sscanf(string, "%f %f", &v(0), &v(1));
- return v;
-}
-
-Eigen::Vector3f strToVec(const char* string)
-{
- Eigen::Vector3f v;
- sscanf(string, "%f %f %f", &v(0), &v(1), &v(2));
- return v;
-}
-
-Eigen::Quaternionf strToQuat(const char* string)
-{
- Eigen::Quaternionf q;
- sscanf(string, "%f %f %f %f", &q.w(), &q.x(), &q.y(), &q.z());
- return q;
-}
-
-
-std::size_t commonPrefixLength(const std::string& a, const std::string& b)
-{
- const std::string* smaller = &a;
- const std::string* bigger = &b;
- if (b.size() < a.size())
- {
- std::swap(smaller, bigger);
- }
-
- auto mismatch = std::mismatch(smaller->begin(), smaller->end(),
- bigger->begin()).first;
- return std::size_t(std::distance(smaller->begin(), mismatch));
-}
-
-
-void assertElementIsBody(const XMLElement* elem)
-{
- assertElementIs(elem, "body");
-}
-
-bool hasAttr(const XMLElement* elem, const std::string& attrName)
-{
- return hasAttr(*elem, attrName);
-}
-
-bool hasAttr(const XMLElement& elem, const std::string& attrName)
-{
- return elem.Attribute(attrName.c_str());
-}
-
-bool isAttr(const XMLElement* elem, const std::string& attrName, const std::string& attrValue)
-{
- return isAttr(*elem, attrName, attrValue);
-}
-
-bool isAttr(const XMLElement& elem, const std::string& attrName, const std::string& attrValue)
-{
- return hasAttr(elem, attrName) && elem.Attribute(attrName.c_str()) == attrValue;
-}
-
-
-
-
-}
-}
diff --git a/VirtualRobot/XML/mjcf/utils.h b/VirtualRobot/XML/mjcf/utils.h
deleted file mode 100644
index 08e57af0f252f046a4b8c2bcc19a12f7340bac68..0000000000000000000000000000000000000000
--- a/VirtualRobot/XML/mjcf/utils.h
+++ /dev/null
@@ -1,111 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Util/xml/tinyxml2.h>
-#include <string>
-#include <Eigen/Eigen>
-
-
-namespace VirtualRobot
-{
-namespace mjcf
-{
-
-enum class ActuatorType
-{
- MOTOR, POSITION, VELOCITY
-};
-
-ActuatorType toActuatorType(std::string str);
-
-
-using XMLElement = tinyxml2::XMLElement;
-
-
-bool isElement(const XMLElement* elem, const char* tag);
-bool isElement(const XMLElement& elem, const char* tag);
-bool isElement(const XMLElement* elem, const std::string& tag);
-bool isElement(const XMLElement& elem, const std::string& tag);
-
-bool hasElementChild(const XMLElement* elem, const std::string& elemName);
-
-bool hasMass(const XMLElement* body);
-
-
-bool hasAttr(const XMLElement* elem, const std::string& attrName);
-bool hasAttr(const XMLElement& elem, const std::string& attrName);
-bool isAttr(const XMLElement* elem, const std::string& attrName, const std::string& attrValue);
-bool isAttr(const XMLElement& elem, const std::string& attrName, const std::string& attrValue);
-
-template <typename AttrT>
-void setAttr(XMLElement* elem, const std::string& name, const AttrT& value);
-template <typename AttrT>
-void setAttr(XMLElement& elem, const std::string& name, const AttrT& value);
-
-// Convert to MJCF XML attribute format.
-std::string toAttr(bool b);
-template <int dim>
-std::string toAttr(const Eigen::Matrix<float, dim, 1>& v);
-std::string toAttr(const Eigen::Quaternionf& v);
-
-// Convert from MJCF XML attribute.
-Eigen::Vector2f strToVec2(const char* string);
-Eigen::Vector3f strToVec(const char* string);
-Eigen::Quaternionf strToQuat(const char* string);
-
-// Get lenght of common prefix of two strings (was used for mergin body names).
-std::size_t commonPrefixLength(const std::string& a, const std::string& b);
-
-
-// Assert that isElement(elem, tag).
-template <class StringT>
-void assertElementIs(const XMLElement* elem, const StringT tag);
-
-
-// Assert that isElement(elem, "body").
-void assertElementIsBody(const XMLElement* elem);
-
-
-
-
-// DEFINITIONS of templated methods
-
-
-template <int dim>
-std::string toAttr(const Eigen::Matrix<float, dim, 1>& v)
-{
- static const Eigen::IOFormat iofVector {7, 0, "", " ", "", "", "", ""};
-
- std::stringstream ss;
- ss << v.format(iofVector);
- return ss.str();
-}
-
-template <typename AttrT>
-void setAttr(XMLElement* elem, const std::string& name, const AttrT& value)
-{
- setAttr(*elem, name, value);
-}
-
-template <typename AttrT>
-void setAttr(XMLElement& elem, const std::string& name, const AttrT& value)
-{
- elem.SetAttribute(name.c_str(), toAttr(value).c_str());
-}
-
-template <class StringT>
-void assertElementIs(const XMLElement*
- #ifndef NDEBUG
- elem
- #endif
- , const StringT
- #ifndef NDEBUG
- tag
- #endif
- )
-{
- assert(isElement(elem, tag));
-}
-
-}
-}
-
diff --git a/VirtualRobot/XML/mujoco/MasslessBodySanitizer.cpp b/VirtualRobot/XML/mujoco/MasslessBodySanitizer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf13eda4799ec49b3aaf783a5cce6ff6d3c0fc80
--- /dev/null
+++ b/VirtualRobot/XML/mujoco/MasslessBodySanitizer.cpp
@@ -0,0 +1,227 @@
+#include "MasslessBodySanitizer.h"
+
+#include <boost/algorithm/string/join.hpp>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/math/Helpers.h>
+
+
+namespace VirtualRobot::mujoco
+{
+
+using namespace mjcf;
+
+MasslessBodySanitizer::MasslessBodySanitizer(RobotPtr& robot) :
+ robot(robot)
+{}
+
+void MasslessBodySanitizer::sanitize(Body root)
+{
+ // merge body leaf nodes with parent if they do not have a mass (inertial or geom)
+
+ for (Body body = root.firstChild<Body>();
+ body; body = body.nextSiblingElement<Body>())
+ {
+ sanitizeRecursion(body);
+ }
+}
+
+
+void MasslessBodySanitizer::sanitizeRecursion(mjcf::Body body)
+{
+ RobotNodePtr bodyNode = robot->getRobotNode(body.name);
+ Eigen::Matrix4f accChildPose = Eigen::Matrix4f::Identity();
+
+ while (!body.hasMass())
+ {
+ std::cout << t << body.name << ": \t";
+
+ if (!body.hasChild<Body>())
+ {
+ // leaf => end of recursion
+ sanitizeLeafBody(body);
+ return;
+ }
+
+ // non-leaf body
+ // check whether there is only one child body
+ Body childBody = body.firstChild<Body>();
+ if (!childBody.nextSiblingElement<Body>())
+ {
+ mergeBodies(body, childBody, accChildPose);
+ }
+ else
+ {
+ std::cout << "Adding dummy inertial to massless body with >1 child bodies." << std::endl;
+ // add a small mass
+ body.addDummyInertial();
+ break;
+ }
+ }
+
+ // recursive descend
+ for (Body child = body.firstChild<Body>();
+ child; child = child.nextSiblingElement<Body>())
+ {
+ sanitizeRecursion(child);
+ }
+}
+
+static void updatePos(AnyElement element, const Eigen::Matrix4f& accChildPose)
+{
+ const Eigen::Vector3f pos = element.getAttribute<Eigen::Vector3f>("pos", Eigen::Vector3f::Zero());
+ element.setAttribute("pos", math::Helpers::TransformPosition(accChildPose, pos));
+}
+
+static void updateOri(AnyElement element, const Eigen::Matrix3f& accChildOri)
+{
+ if (element.is<Joint>())
+ {
+ Joint joint = element.as<Joint>();
+ joint.axis = accChildOri * joint.axis.get();
+ }
+ else if (element.is<Light>())
+ {
+ Light light = element.as<Light>();
+ light.dir = accChildOri * light.dir.get();
+ }
+ else
+ {
+ const Eigen::Quaternionf quat =
+ element.getAttribute<Eigen::Quaternionf>("quat", Eigen::Quaternionf::Identity());
+ element.setAttribute("quat", Eigen::Quaternionf(accChildOri * quat));
+ }
+}
+
+void copyChildren(Body body, Body child, const Eigen::Matrix4f& childPose)
+{
+ // merge childBody into body => move all its elements here
+ // while doing this, apply accChildPose to elements
+ for (AnyElement grandChild = child.firstChild<AnyElement>();
+ grandChild; grandChild = grandChild.template nextSiblingElement<AnyElement>())
+ {
+ // clone grandchild
+ AnyElement elem = grandChild.deepClone();
+
+ if (elem)
+ {
+ /* Adapt pose/axis elements in child. Their poses/axes will be
+ * relative to body's frame, so the transformation from body
+ * to child will be lost. Therefore, apply accChildPose to
+ * their poses/axes. */
+
+ updatePos(elem, childPose);
+ updateOri(elem, math::Helpers::Orientation(childPose));
+ }
+
+ // insert to body
+ body.insertEndChild(elem);
+ }
+}
+
+
+
+void MasslessBodySanitizer::mergeBodies(Body body, Body childBody, Eigen::Matrix4f& accChildPose)
+{
+ std::cout << "Merging with '" << childBody.name << "' " << std::endl;
+
+ RobotNodePtr childNode = robot->getRobotNode(childBody.name);
+ Eigen::Matrix4f childPose = childNode->getTransformationFrom(childNode->getParent());
+
+ // update accumulated child pose
+ // merged child's frame w.r.t. body's frame
+ accChildPose = childPose * accChildPose;
+
+ // merge childBody into body => move all its elements here
+ // while doing this, apply accChildPose to elements
+ copyChildren(body, childBody, accChildPose);
+
+ // update body name
+ MergedBodySet& bodySet = getMergedBodySetWith(body.name);
+ bodySet.addBody(childBody.name);
+ body.name = bodySet.getMergedBodyName();
+
+ std::cout << t << "\t(new name: '" << bodySet.getMergedBodyName() << "')" << std::endl;
+
+ // delete child
+ body.deleteChild(childBody);
+}
+
+
+void MasslessBodySanitizer::sanitizeLeafBody(Body body)
+{
+ VR_ASSERT_MESSAGE(!body.hasChild<Body>(), "Leaf body must not have a child body.");
+ VR_ASSERT_MESSAGE(!body.hasMass(), "Leaf body must not have mass.");
+
+ if (!body.hasChildren()) // is completely empty?
+ {
+ // leaf without geom: make it a site
+ std::cout << "Changing to site." << std::endl;
+ body.transform<Site>();
+ }
+ else
+ {
+ // add a small mass
+ std::cout << "Adding dummy inertial to massless leaf body with children." << std::endl;
+ body.addDummyInertial();
+ }
+}
+
+const std::vector<MergedBodySet>& MasslessBodySanitizer::getMergedBodySets() const
+{
+ return mergedBodySets;
+}
+
+const std::string& MasslessBodySanitizer::getMergedBodyName(const std::string& originalBodyName)
+{
+ return getMergedBodySetWith(originalBodyName).getMergedBodyName();
+}
+
+MergedBodySet&MasslessBodySanitizer::getMergedBodySetWith(const std::string& bodyName)
+{
+ for (auto& set : mergedBodySets)
+ {
+ if (set.containsBody(bodyName))
+ {
+ return set;
+ }
+ }
+
+ // not found => add
+ mergedBodySets.push_back(MergedBodySet(bodyName));
+
+ return mergedBodySets.back();
+}
+
+
+MergedBodySet::MergedBodySet()
+= default;
+
+MergedBodySet::MergedBodySet(const std::string& bodyName)
+{
+ addBody(bodyName);
+}
+
+void MergedBodySet::addBody(const std::string& bodyName)
+{
+ originalBodyNames.push_back(bodyName);
+ updateMergedBodyName();
+}
+
+bool MergedBodySet::containsBody(const std::string& bodyName) const
+{
+ return std::find(originalBodyNames.begin(), originalBodyNames.end(),
+ bodyName) != originalBodyNames.end();
+}
+
+const std::string& MergedBodySet::getMergedBodyName() const
+{
+ return mergedBodyName;
+}
+
+void MergedBodySet::updateMergedBodyName()
+{
+ mergedBodyName = boost::algorithm::join(originalBodyNames, "~");
+}
+
+}
diff --git a/VirtualRobot/XML/mjcf/MasslessBodySanitizer.h b/VirtualRobot/XML/mujoco/MasslessBodySanitizer.h
similarity index 60%
rename from VirtualRobot/XML/mjcf/MasslessBodySanitizer.h
rename to VirtualRobot/XML/mujoco/MasslessBodySanitizer.h
index d3bef857d759621187c48c273d88a313078514a0..25f1f327d2ddfa93e7322115021f65bbf73faabd 100644
--- a/VirtualRobot/XML/mjcf/MasslessBodySanitizer.h
+++ b/VirtualRobot/XML/mujoco/MasslessBodySanitizer.h
@@ -1,11 +1,12 @@
#pragma once
-#include "MjcfDocument.h"
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/MJCF/Document.h>
namespace VirtualRobot
{
-namespace mjcf
+namespace mujoco
{
class MergedBodySet
@@ -35,9 +36,9 @@ namespace mjcf
{
public:
- MasslessBodySanitizer(DocumentPtr& document, RobotPtr& robot);
+ MasslessBodySanitizer(RobotPtr& robot);
- void sanitize();
+ void sanitize(mjcf::Body root);
const std::vector<MergedBodySet>& getMergedBodySets() const;
@@ -48,22 +49,14 @@ namespace mjcf
private:
- void sanitizeRecursion(mjcf::XMLElement* body);
- void sanitizeLeafBody(mjcf::XMLElement* body);
-
- void mergeBodies(XMLElement* body, XMLElement* childBody, Eigen::Matrix4f& accChildPose);
-
- void updateChildPos(XMLElement* elem, const Eigen::Matrix4f& accChildPose);
- void updateChildQuat(XMLElement* elem, const Eigen::Matrix3f& accChildOri);
- void updateChildAxis(XMLElement* elem, const Eigen::Matrix3f& accChildOri,
- const char* attrName = "axis");
-
+ void sanitizeRecursion(mjcf::Body body);
+ void sanitizeLeafBody(mjcf::Body body);
+ void mergeBodies(mjcf::Body body, mjcf::Body childBody, Eigen::Matrix4f& accChildPose);
const std::string t = "| ";
- DocumentPtr& document;
RobotPtr& robot;
std::vector<MergedBodySet> mergedBodySets;
diff --git a/VirtualRobot/XML/mujoco/MujocoIO.cpp b/VirtualRobot/XML/mujoco/MujocoIO.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..83aeb56977c0d99a27a0b031392460e7e02f4e88
--- /dev/null
+++ b/VirtualRobot/XML/mujoco/MujocoIO.cpp
@@ -0,0 +1,684 @@
+#include "MujocoIO.h"
+
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobotException.h>
+
+#include <VirtualRobot/MJCF/visitors/Collector.h>
+#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+
+namespace fs = std::filesystem;
+
+
+namespace
+{
+ namespace fs = std::filesystem;
+ inline fs::path removeTrailingSeparator(fs::path p)
+ {
+ p /= "dummy";
+ return p.parent_path();
+ }
+}
+
+
+
+namespace VirtualRobot::mujoco
+{
+
+ActuatorType toActuatorType(const std::string& string)
+{
+ static const std::map<std::string, ActuatorType> map
+ {
+ {"motor", ActuatorType::MOTOR},
+ {"position", ActuatorType::POSITION},
+ {"velocity", ActuatorType::VELOCITY}
+ };
+ std::string lower = string;
+ std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
+ return map.at(lower);
+}
+
+
+MujocoIO::MujocoIO(RobotPtr robot) : robot(robot)
+{
+ THROW_VR_EXCEPTION_IF(!robot, "Given RobotPtr robot is null.");
+}
+
+void MujocoIO::saveMJCF(const std::string& filename, const std::string& basePath,
+ const std::string& meshRelDir)
+{
+ THROW_VR_EXCEPTION_IF(filename.empty(), "Given filename is empty.");
+
+ setPaths(filename, basePath, meshRelDir);
+
+ document.reset(new mjcf::Document());
+ document->setModelName(robot->getName());
+
+ makeCompiler();
+
+ makeDefaultsClass();
+ document->setNewElementClass(robot->getName(), true);
+
+ addSkybox();
+
+ mjcf::Body mocapBody;
+ if (withMocapBody)
+ {
+ std::cout << "Adding mocap body ..." << std::endl;
+ mocapBody = addMocapBody();
+ }
+
+ std::cout << "Creating bodies structure ..." << std::endl;
+ makeNodeBodies();
+
+ std::cout << "Adding meshes and geoms ..." << std::endl;
+ addNodeBodyMeshes();
+
+ if (verbose)
+ {
+ std::cout << "===========================" << std::endl
+ << "Current model: " << std::endl
+ << "--------------" << std::endl;
+ std::cout << *document;
+ std::cout << "===========================" << std::endl;
+ }
+
+ std::cout << "Merging massless bodies ..." << std::endl;
+ masslessBodySanitizer.sanitize(robotRoot);
+
+ std::cout << "Adding contact excludes ..." << std::endl;
+ addContactExcludes();
+
+ if (withMocapBody)
+ {
+ std::cout << "Adding mocap body contact excludes ..." << std::endl;
+ addMocapContactExcludes(mocapBody);
+ }
+
+ std::cout << "Adding actuators ..." << std::endl;
+ addActuators();
+
+ // this is now done directly when constructing the respective elements
+ //std::cout << "Scaling lengths by " << lengthScale << " ..." << std::endl;
+ //scaleLengths(robotRoot);
+
+ std::cout << "Done." << std::endl;
+
+ if (verbose)
+ {
+ std::cout << std::endl;
+ std::cout << "===========================" << std::endl
+ << "Output file: " << std::endl
+ << "------------" << std::endl;
+ std::cout << *document;
+ std::cout << "===========================" << std::endl;
+ }
+
+ assert(!outputFileName.empty());
+ std::cout << "Writing to " << (outputDirectory / outputFileName) << std::endl;
+ document->saveFile((outputDirectory / outputFileName).string());
+}
+
+void MujocoIO::setUseRelativePaths(bool useRelative)
+{
+ this->useRelativePaths = useRelative;
+}
+
+void MujocoIO::setPaths(const std::string& filename, const std::string& basePath, const std::string& meshRelDir)
+{
+ outputDirectory = basePath;
+ outputFileName = filename;
+ outputMeshRelDirectory = meshRelDir;
+
+ ensureDirectoriesExist();
+}
+
+void MujocoIO::ensureDirectoriesExist()
+{
+ auto ensureDirExists = [](const fs::path& dir, const std::string& errMsgName)
+ {
+ if (!fs::is_directory(dir))
+ {
+ std::cout << "Creating directory: " << dir << std::endl;
+ bool success = fs::create_directories(removeTrailingSeparator(dir));
+ THROW_VR_EXCEPTION_IF(!success, "Could not create " << errMsgName << ": " << dir);
+ }
+ };
+
+ ensureDirExists(outputDirectory, "output directory");
+ ensureDirExists(outputMeshDirectory(), "output mesh directory");
+}
+
+void MujocoIO::makeCompiler()
+{
+ document->compiler().angle = "radian";
+ document->compiler().balanceinertia = true;
+}
+
+void MujocoIO::makeDefaultsClass()
+{
+ mjcf::DefaultClass defaultsClass = document->default_().addClass(robot->getName());
+
+ defaultsClass.insertComment("Add default values for " + robot->getName() + " here.", true);
+
+ mjcf::Joint joint = defaultsClass.getElement<mjcf::Joint>();
+ joint.frictionloss = 1;
+ joint.damping = 0;
+
+ mjcf::Mesh mesh = defaultsClass.getElement<mjcf::Mesh>();
+ mesh.scale = Eigen::Vector3f::Constant(meshScale);
+
+ mjcf::Geom geom = defaultsClass.getElement<mjcf::Geom>();
+ geom.condim = 4;
+
+ mjcf::ActuatorPosition actPos = defaultsClass.getElement<mjcf::ActuatorPosition>();
+ actPos.kp = 1;
+
+ mjcf::ActuatorVelocity actVel = defaultsClass.getElement<mjcf::ActuatorVelocity >();
+ actVel.kv = 1;
+}
+
+
+void MujocoIO::addSkybox()
+{
+ document->asset().addSkyboxTexture(Eigen::Vector3f(.8f, .9f, .95f),
+ Eigen::Vector3f(.4f, .6f, .8f));
+}
+
+
+mjcf::Body MujocoIO::addMocapBody()
+{
+ const std::string className = "mocap";
+ const float geomSize = 0.01f;
+
+ std::string bodyName;
+ {
+ std::stringstream ss;
+ ss << robot->getName() << "_Mocap";
+ bodyName = ss.str();
+ }
+
+ // add defaults class
+ mjcf::DefaultClass defaultClass = document->default_().getClass(className);
+
+ mjcf::Geom geom = defaultClass.getElement<mjcf::Geom>();
+ geom.rgba = Eigen::Vector4f(.9f, .5f, .5f, .5f);
+
+ {
+ mjcf::EqualityDefaults equality = defaultClass.getElement<mjcf::EqualityDefaults>();
+ Eigen::Vector5f solimp = equality.solimp;
+ solimp(1) = 0.99f;
+ equality.solimp = solimp;
+ //equality.solref = Eigen::Vector2f(.02f, 1.f);
+ }
+
+ // add body
+ mjcf::Body mocap = document->worldbody().addMocapBody(bodyName, geomSize);
+ mocap.childclass = className;
+
+ // add equality weld constraint
+ mjcf::EqualityWeld weld = document->equality().addWeld(bodyName, robot->getName(), bodyName);
+ weld.class_ = className;
+
+ document->contact().addExclude(mocap.name, robot->getName());
+
+ return mocap;
+}
+
+
+void MujocoIO::makeNodeBodies()
+{
+ nodeBodies.clear();
+
+ RobotNodePtr rootNode = robot->getRootNode();
+ assert(rootNode);
+
+ // add root
+ robotRoot = document->worldbody().addBody(robot->getName(), robot->getName());
+
+ if (withMocapBody)
+ {
+ robotRoot.addDummyInertial();
+ mjcf::FreeJoint joint = robotRoot.addFreeJoint();
+ joint.name = robot->getName();
+ }
+
+ mjcf::Body root = addNodeBody(robotRoot, rootNode);
+ nodeBodies[rootNode->getName()] = root;
+ assert(root);
+
+ for (RobotNodePtr node : robot->getRobotNodes())
+ {
+ addNodeBody(node);
+ }
+}
+
+mjcf::Body MujocoIO::addNodeBody(mjcf::Body parent, RobotNodePtr node)
+{
+ mjcf::Body body = parent.addBody(node->getName());
+
+ if (node->hasParent())
+ {
+ Eigen::Matrix4f pose = node->getTransformationFrom(node->getParent());
+ math::Helpers::ScaleTranslation(pose, lengthScale);
+ body.setPose(pose);
+ }
+
+ if (node->isRotationalJoint() || node->isTranslationalJoint())
+ {
+ addNodeJoint(body, node);
+ }
+
+ addNodeInertial(body, node);
+
+ return body;
+}
+
+mjcf::Joint MujocoIO::addNodeJoint(mjcf::Body body, RobotNodePtr node)
+{
+ assert(node->isRotationalJoint() xor node->isTranslationalJoint());
+
+ mjcf::Joint joint = body.addJoint();
+ joint.name = node->getName();
+
+ // get the axis
+ Eigen::Vector3f axis;
+ if (node->isRotationalJoint())
+ {
+ RobotNodeRevolutePtr revolute = boost::dynamic_pointer_cast<RobotNodeRevolute>(node);
+ assert(revolute);
+ axis = revolute->getJointRotationAxisInJointCoordSystem();
+ }
+ else if (node->isTranslationalJoint())
+ {
+ RobotNodePrismaticPtr prismatic = boost::dynamic_pointer_cast<RobotNodePrismatic>(node);
+ assert(prismatic);
+ axis = prismatic->getJointTranslationDirectionJointCoordSystem();
+ }
+
+ joint.type = node->isRotationalJoint() ? "hinge" : "slide";
+ joint.axis = axis;
+ joint.limited = !node->isLimitless();
+
+ if (!node->isLimitless())
+ {
+ Eigen::Vector2f range = { node->getJointLimitLow(), node->getJointLimitHigh() };
+ if (node->isTranslationalJoint())
+ {
+ range *= lengthScale;
+ }
+ joint.range = range;
+ }
+
+ return joint;
+}
+
+mjcf::Inertial MujocoIO::addNodeInertial(mjcf::Body body, RobotNodePtr node)
+{
+ const Eigen::Matrix3f matrix = node->getInertiaMatrix();
+ if (matrix.isIdentity(document->getFloatCompPrecision())
+ && node->getMass() < document->getFloatCompPrecision())
+ {
+ // dont set an inertial element and let it be derived automatically
+ return { };
+ }
+
+ mjcf::Inertial inertial = body.addInertial();
+ inertial.pos = node->getCoMLocal() * lengthScale;
+ inertial.mass = node->getMass() * massScale;
+ inertial.inertiaFromMatrix(matrix);
+
+ return inertial;
+}
+
+void MujocoIO::addNodeBodyMeshes()
+{
+ bool meshlabserverAviable = system("which meshlabserver > /dev/null 2>&1") == 0;
+ bool notAvailableReported = false;
+
+ for (RobotNodePtr node : robot->getRobotNodes())
+ {
+ VisualizationNodePtr visualization = node->getVisualization(SceneObject::VisualizationType::Full);
+
+ if (!visualization)
+ {
+ continue;
+ }
+
+ std::cout << t << "Node " << node->getName() << ":\t";
+
+ const fs::path srcMeshPath = visualization->getFilename();
+
+ fs::path dstMeshFileName = srcMeshPath.filename();
+ dstMeshFileName.replace_extension("stl");
+ const fs::path dstMeshPath = outputMeshDirectory() / dstMeshFileName;
+
+ if (!fs::exists(dstMeshPath))
+ {
+ if (srcMeshPath.extension().string() != ".stl")
+ {
+ std::cout << "Converting to .stl: " << srcMeshPath << std::endl;
+
+ if (!meshlabserverAviable)
+ {
+ if (!notAvailableReported)
+ {
+ std::cout << std::endl
+ << "Command 'meshlabserver' not available, "
+ "cannot convert meshes." << std::endl;
+ notAvailableReported = true;
+ }
+ continue;
+ }
+
+ // meshlabserver available
+ std::stringstream convertCommand;
+ convertCommand << "meshlabserver"
+ << " -i " << srcMeshPath
+ << " -o " << dstMeshPath;
+
+ // run command
+ std::cout << "----------------------------------------------------------" << std::endl;
+ std::cout << "Running command: " << convertCommand.str() << std::endl;
+ int r = system(convertCommand.str().c_str());
+ std::cout << "----------------------------------------------------------" << std::endl;
+ if (r != 0)
+ {
+ std::cout << "Command returned with error: " << r << "\n"
+ << "Command was: " << convertCommand.str() << std::endl;
+ }
+ }
+ else
+ {
+ std::cout << "Copying: " << srcMeshPath << "\n"
+ << " to: " << dstMeshPath;
+ fs::copy_file(srcMeshPath, dstMeshPath);
+ }
+ }
+ else
+ {
+ std::cout << "skipping (" << outputMeshRelDirectory / dstMeshFileName
+ << " already exists)";
+ }
+ std::cout << std::endl;
+
+
+
+ // add asset
+ const std::string meshName = node->getName();
+ const fs::path meshPath = useRelativePaths
+ ? outputMeshRelDirectory / dstMeshFileName
+ : fs::absolute(dstMeshPath);
+
+ document->asset().addMesh(meshName, meshPath.string());
+
+ // add geom to body
+ mjcf::Body& body = nodeBodies.at(node->getName());
+ mjcf::Geom geom = body.addGeomMesh(meshName);
+ geom.name = node->getName();
+ }
+}
+
+
+
+mjcf::Body MujocoIO::addNodeBody(RobotNodePtr node)
+{
+ // see whether body for the node already exists
+ auto find = nodeBodies.find(node->getName());
+ if (find != nodeBodies.end())
+ {
+ // exists => break recursion
+ return find->second;
+ }
+
+ // check whether body exists for parent node
+ mjcf::Body parent;
+ find = nodeBodies.find(node->getParent()->getName());
+ if (find == nodeBodies.end())
+ {
+ // parent does not exist => create it first
+ parent = addNodeBody(robot->getRobotNode(node->getParent()->getName()));
+ }
+ else
+ {
+ // parent exists
+ parent = find->second;
+ }
+
+ // add body as child of parent
+ mjcf::Body body = addNodeBody(parent, node);
+ nodeBodies[node->getName()] = body;
+
+ return body;
+}
+
+struct ParentChildContactExcludeVisitor : public mjcf::Visitor
+{
+ ParentChildContactExcludeVisitor(mjcf::Document& document) : mjcf::Visitor (document)
+ {}
+ virtual ~ParentChildContactExcludeVisitor() override = default;
+
+ //bool VisitEnter(const tinyxml2::XMLElement&, const tinyxml2::XMLAttribute*) override;
+ bool visitEnter(const mjcf::AnyElement& element) override;
+
+ std::vector<std::pair<std::string, std::string>> excludePairs;
+ bool firstSkipped = false; ///< Used to skip the root element.
+};
+
+bool ParentChildContactExcludeVisitor::visitEnter(const mjcf::AnyElement& element)
+{
+ if (!element.is<mjcf::Body>())
+ {
+ return true;
+ }
+
+ const mjcf::Body body = element.as<mjcf::Body>();
+
+ if (!firstSkipped)
+ {
+ firstSkipped = true;
+ return true;
+ }
+
+ const mjcf::Body parent = body.parent<mjcf::Body>();
+ assert(parent);
+ excludePairs.emplace_back(parent.name.get(), body.name.get());
+
+ return true;
+}
+
+
+void MujocoIO::addContactExcludes()
+{
+ RobotNodePtr rootNode = robot->getRootNode();
+
+ std::vector<std::pair<std::string, std::string>> excludePairs;
+
+ for (RobotNodePtr node : robot->getRobotNodes())
+ {
+ for (std::string& ignore : node->getPhysics().ignoreCollisions)
+ {
+ // I found an <IgnoreCollision> element referring to a non-existing node.
+ // => check node existence here
+ if (robot->hasRobotNode(ignore))
+ {
+ excludePairs.push_back({node->getName(), ignore});
+ }
+ }
+ }
+
+ // resolve body names and add exludes
+ for (const auto& excludePair : excludePairs)
+ {
+ const std::string body1 = masslessBodySanitizer.getMergedBodyName(excludePair.first);
+ const std::string body2 = masslessBodySanitizer.getMergedBodyName(excludePair.second);
+ document->contact().addExclude(body1, body2);
+ }
+
+ // Add excludes between parent and child elemenets.
+ // This should actually not be necessary?
+ ParentChildContactExcludeVisitor visitor(*document);
+ robotRoot.accept(visitor);
+ for (const auto& excludePair : visitor.excludePairs)
+ {
+ document->contact().addExclude(excludePair.first, excludePair.second);
+ }
+}
+
+void MujocoIO::addMocapContactExcludes(mjcf::Body mocap)
+{
+ if (!mocap)
+ {
+ throw std::invalid_argument("Passed uninitialized mocap body to " + std::string(__FUNCTION__) + "()");
+ }
+ for (const auto& nodeBody : nodeBodies)
+ {
+ document->contact().addExclude(mocap, nodeBody.second);
+ }
+}
+
+void MujocoIO::addActuators()
+{
+ const std::vector<mjcf::Joint> jointElements = mjcf::Collector<mjcf::Joint>::collect(
+ *document, document->worldbody());
+
+ for (const auto& joint : jointElements)
+ {
+ mjcf::AnyElement actuator;
+
+ const std::string jointName = joint.name;
+ switch (actuatorType)
+ {
+ case ActuatorType::MOTOR:
+ {
+ mjcf::ActuatorMotor act = document->actuator().addMotor(jointName);
+ actuator = act;
+ break;
+ }
+
+ case ActuatorType::POSITION:
+ {
+ mjcf::ActuatorPosition act = document->actuator().addPosition(jointName);
+ actuator = act;
+
+ if (joint.limited)
+ {
+ act.ctrllimited = joint.limited;
+ act.ctrlrange = joint.range;
+ }
+ }
+ break;
+
+ case ActuatorType::VELOCITY:
+ actuator = document->actuator().addVelocity(jointName);
+ break;
+ }
+
+ std::stringstream actuatorName;
+ actuatorName << joint.name;
+ if (addActuatorTypeSuffix)
+ {
+ actuatorName << actuatorTypeSuffixes.at(actuatorType);
+ }
+ actuator.setAttribute("name", actuatorName.str());
+ }
+}
+
+void MujocoIO::scaleLengths(mjcf::AnyElement element)
+{
+ assert(elem);
+ if (verbose)
+ {
+ std::cout << "Traversing element " << element.actualTag() << std::endl;
+ }
+
+ if (element.is<mjcf::Joint>())
+ {
+ mjcf::Joint joint = element.as<mjcf::Joint>();
+
+ if (joint.type == "slide" && joint.range.isSet())
+ {
+ std::cout << t << "Scaling range of slide joint '" << joint.name << "'" << std::endl;
+ joint.range = joint.range.get() * lengthScale;
+ }
+ }
+ else if (element.is<mjcf::ActuatorPosition>())
+ {
+ mjcf::ActuatorPosition act = element.as<mjcf::ActuatorPosition>();
+ if (act.ctrlrange.isSet())
+ {
+ std::cout << t << "Scaling ctrlrange of position actuator '" << act.name << "'" << std::endl;
+ act.ctrlrange = act.ctrlrange.get() * lengthScale;
+ }
+ }
+ else if (element.isAttributeSet("pos"))
+ {
+ std::cout << t << "Scaling pos of " << element.actualTag() << " ";
+ if (element.isAttributeSet("name"))
+ {
+ std::cout << "'" << element.getAttribute("name") << "'";
+ }
+ else
+ {
+ std::cout << "element";
+ }
+ std::cout << std::endl;
+
+ Eigen::Vector3f pos = element.getAttribute<Eigen::Vector3f>("pos");
+ pos *= lengthScale;
+ element.setAttribute("pos", pos);
+ }
+
+ for (mjcf::AnyElement child = element.firstChild<mjcf::AnyElement>();
+ child; child = child.nextSiblingElement<mjcf::AnyElement>())
+ {
+ scaleLengths(child);
+ }
+}
+
+void MujocoIO::setLengthScale(float value)
+{
+ this->lengthScale = value;
+}
+
+void MujocoIO::setMeshScale(float value)
+{
+ this->meshScale = value;
+}
+
+void MujocoIO::setMassScale(float value)
+{
+ this->massScale = value;
+}
+
+void MujocoIO::setActuatorType(ActuatorType value)
+{
+ this->actuatorType = value;
+}
+
+void MujocoIO::setAddActuatorTypeSuffix(bool enable)
+{
+ this->addActuatorTypeSuffix = enable;
+}
+
+void MujocoIO::setActuatorTypeSuffixes(const std::map<ActuatorType, std::string>& suffixes)
+{
+ this->actuatorTypeSuffixes = suffixes;
+}
+
+void MujocoIO::setWithMocapBody(bool enabled)
+{
+ this->withMocapBody = enabled;
+}
+
+void MujocoIO::setVerbose(bool value)
+{
+ this->verbose = value;
+}
+
+
+
+}
diff --git a/VirtualRobot/XML/mujoco/MujocoIO.h b/VirtualRobot/XML/mujoco/MujocoIO.h
new file mode 100644
index 0000000000000000000000000000000000000000..98a040421cd9a0966613e97d4fd08122416434d8
--- /dev/null
+++ b/VirtualRobot/XML/mujoco/MujocoIO.h
@@ -0,0 +1,186 @@
+#pragma once
+
+#include <filesystem>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/MJCF/Document.h>
+
+#include "MasslessBodySanitizer.h"
+
+
+namespace VirtualRobot
+{
+
+namespace mujoco
+{
+
+ enum class ActuatorType
+ {
+ MOTOR, POSITION, VELOCITY
+ };
+ ActuatorType toActuatorType(const std::string& string);
+
+
+ class MujocoIO
+ {
+ public:
+
+ /// Constructor.
+ /// @throws VirtualRobotException if robot is null
+ MujocoIO(RobotPtr robot);
+
+ /**
+ * @brief Create a Mujoco XML (MJCF) document for the given robot.
+ * @param filename the output filename (without directory)
+ * @param basePath the output directory
+ * @param meshRelDir the directory relative to basePath where meshes shall be placed
+ */
+ void saveMJCF(const std::string& filename, const std::string& basePath,
+ const std::string& meshRelDir);
+
+
+ /// Set whether paths (e.g. meshes) shall be relative instead of absolute.
+ /// This can cause problems when including the generated model from another directory.
+ void setUseRelativePaths(bool useRelative);
+
+ /// Set the scaling for lengths.
+ void setLengthScale(float value);
+ /// Set the scaling for meshes.
+ void setMeshScale(float value);
+ /// Set the scaling for mass (to kg).
+ void setMassScale(float value);
+
+ /// Set the actuator type.
+ void setActuatorType(ActuatorType value);
+ /// Set whether a type suffix shall be added to actuator names.
+ void setAddActuatorTypeSuffix(bool enable);
+ /// Set suffixes appended to actuator names if adding actuator type suffixes is enabled.
+ void setActuatorTypeSuffixes(const std::map<ActuatorType, std::string>& suffixes);
+
+
+ /**
+ * @brief Enable or disable adding of a mocap body controlling the robot pose.
+ *
+ * @param enabled If true:
+ * - Add a free joint to the robot root body mocap body
+ * - Add a mocap body in the world body (called <RobotName>_Mocap)
+ * - Add a weld constraint welding them together.
+ */
+ void setWithMocapBody(bool enabled);
+
+ void setVerbose(bool value);
+
+
+ private:
+
+ /// Set the output path members.
+ void setPaths(const std::string& filename, const std::string& basePath,
+ const std::string& meshRelDir);
+ /// Create output directories if the do not exist.
+ void ensureDirectoriesExist();
+
+ /// Make the compiler section.
+ void makeCompiler();
+ /// Add a defaults group for the robot.
+ void makeDefaultsClass();
+ /// Add a skybox texture asset.
+ void addSkybox();
+
+ /// Add a mocap body with defaults group.
+ mjcf::Body addMocapBody();
+
+ /// Construct the body structure corresponding to the robot nodes.
+ void makeNodeBodies();
+
+ /// Add a body element for a robot node.
+ /// If its parent does not exist, create the parent body first.
+ mjcf::Body addNodeBody(RobotNodePtr node);
+
+ /// Add a body for the given node as child of parent and return it.
+ mjcf::Body addNodeBody(mjcf::Body parent, RobotNodePtr node);
+ /// Add a joint for the given node in body and return it.
+ mjcf::Joint addNodeJoint(mjcf::Body body, RobotNodePtr node);
+ /// Add an inertial for the given node in body and return it.
+ mjcf::Inertial addNodeInertial(mjcf::Body body, RobotNodePtr node);
+
+ /// Convert meshes and add mesh assets for robot nodes with visualization.
+ void addNodeBodyMeshes();
+
+ /// Add contact exclude elements for IgnoreCollision elements.
+ void addContactExcludes();
+ /// Add contact exclude elements for IgnoreCollision elements.
+ void addMocapContactExcludes(mjcf::Body mocap);
+
+ /// Add actuators for all joints.
+ void addActuators();
+
+ /// Scale all lengths by lengthScaling.
+ void scaleLengths(mjcf::AnyElement elem);
+
+
+ // Paremeters
+
+ bool useRelativePaths = false;
+
+ /// Scaling for lengths, such as positions and translations (to m).
+ float lengthScale = 0.001f;
+ /// Scaling for lengths, such as positions and translations (to m).
+ float meshScale = 1.f;
+ /// Scaling for mass (to kg).
+ float massScale = 1.f;
+
+ /// The actuator type.
+ ActuatorType actuatorType = ActuatorType::MOTOR;
+
+ /// Whether to add type suffix to actuator names
+ bool addActuatorTypeSuffix = false;
+ /// The suffixes added if addActuatorTypeSuffix is true.
+ std::map<ActuatorType, std::string> actuatorTypeSuffixes = {
+ { ActuatorType::MOTOR, "_motor" },
+ { ActuatorType::POSITION, "_position" },
+ { ActuatorType::VELOCITY, "_velocity" },
+ };
+
+
+ /// Add a mocap
+ bool withMocapBody = false;
+
+ /// Verbose printing.
+ bool verbose = false;
+
+
+ // Paths
+
+ std::filesystem::path outputDirectory;
+ std::filesystem::path outputFileName;
+ std::filesystem::path outputMeshRelDirectory;
+ std::filesystem::path outputMeshDirectory() { return outputDirectory / outputMeshRelDirectory; }
+
+
+ // Input
+
+ /// The input robot.
+ RobotPtr robot;
+
+
+ // Output
+
+ /// The built MJCF document.
+ mjcf::DocumentPtr document = nullptr;
+ mjcf::Body robotRoot;
+
+
+ // Processing
+
+ /// Sanitizes massless bodies.
+ mujoco::MasslessBodySanitizer masslessBodySanitizer { robot };
+
+ /// Map of robot node names to XML elements.
+ std::map<std::string, mjcf::Body> nodeBodies;
+
+ const std::string t = "| ";
+
+ };
+
+
+}}
diff --git a/VirtualRobot/examples/Simox2Mjcf/main.cpp b/VirtualRobot/examples/Simox2Mjcf/main.cpp
index 28d62a8de95bd46499a40a1f1053930a5d498972..e65ab1cd2ac9186661d90d03d18e9b124ee17f13 100644
--- a/VirtualRobot/examples/Simox2Mjcf/main.cpp
+++ b/VirtualRobot/examples/Simox2Mjcf/main.cpp
@@ -3,7 +3,7 @@
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/XML/mjcf/MujocoIO.h>
+#include <VirtualRobot/XML/mujoco/MujocoIO.h>
using namespace VirtualRobot;
@@ -12,17 +12,6 @@ using VirtualRobot::RuntimeEnvironment;
namespace fs = std::filesystem;
-void printUsage(const char* argv0)
-{
- std::cout << "Usage: " << argv0
- << " --robot <simox robot file> "
- << "[--outdir <output directory>] "
- << "[--meshRelDir <relative mesh directory>] "
- << "[--actuator {motor|position|velocity}] "
- << "[--mocap {y|n}]"
- << std::endl;
-}
-
/**
* Loads a Simox robot and converts it to Mujoco's XML format (MJCF).
* The converted file is stored in a directory mjcf/ placed in the directory
@@ -30,20 +19,46 @@ void printUsage(const char* argv0)
*/
int main(int argc, char* argv[])
{
- RuntimeEnvironment::considerKey("robot");
- RuntimeEnvironment::considerKey("outdir");
- RuntimeEnvironment::considerKey("meshRelDir");
- RuntimeEnvironment::considerKey("actuator");
- RuntimeEnvironment::considerKey("mocap");
+ RuntimeEnvironment::setCaption("Convert Simox XML to MJCF (Mujoco XML)");
+
+ RuntimeEnvironment::considerKey(
+ "robot", "The Simox robot model to convert. (required)");
+ RuntimeEnvironment::considerKey(
+ "outdir", "The output directory. (default: 'mjcf')");
+ RuntimeEnvironment::considerKey(
+ "mesh_rel_dir", "The mesh directory relative to outdir. (default: 'mesh')");
+ RuntimeEnvironment::considerKey(
+ "actuator", "The actuator type to add (motor, position, velocity). (default: motor)");
+ RuntimeEnvironment::considerFlag(
+ "mocap", "Add a mocap body to which the robot root body is welded.");
+
+ RuntimeEnvironment::considerFlag(
+ "rel_paths", "Store relative mesh paths instead of absolute ones. (This can "
+ "cause problems when including the generated model from another directory.)");
+
+ RuntimeEnvironment::considerKey(
+ "scale_length", "Scaling of lengths and distances (to m). For meshes, see 'scale_mesh'. (default: 1.0)");
+ RuntimeEnvironment::considerKey(
+ "scale_mesh", "Scaling of meshes (to m). (default: 1.0)");
+ RuntimeEnvironment::considerKey(
+ "scale_mass", "Scaling of masses (to kg). (default: 1.0)");
+
+ RuntimeEnvironment::considerFlag(
+ "actuator_suffix", "Add a type suffix to actuator names.");
+
+ RuntimeEnvironment::considerFlag(
+ "verbose", "Enable verbose output.");
RuntimeEnvironment::processCommandLine(argc, argv);
- //RuntimeEnvironment::print();
-
- fs::path inputFilename;
+ if (RuntimeEnvironment::hasHelpFlag() || !RuntimeEnvironment::hasValue("robot"))
+ {
+ RuntimeEnvironment::printOptions();
+ return 0;
+ }
- if (RuntimeEnvironment::hasValue("robot"))
+ fs::path inputFilename;
{
std::string robFile = RuntimeEnvironment::getValue("robot");
@@ -56,44 +71,79 @@ int main(int argc, char* argv[])
std::cout << "Something is wrong with " << robFile;
}
}
- else
- {
- printUsage(argv[0]);
- return 0;
- }
- fs::path outputDir = RuntimeEnvironment::checkParameter(
+ const fs::path outputDir = RuntimeEnvironment::checkParameter(
"outdir", (inputFilename.parent_path() / "mjcf").string());
- std::string meshRelDir = RuntimeEnvironment::checkParameter("meshRelDir", "mesh");
+ const std::string meshRelDir = RuntimeEnvironment::checkParameter("mesh_rel_dir", "mesh");
- std::string actuatorTypeStr = RuntimeEnvironment::checkParameter("actuator", "motor");
- mjcf::ActuatorType actuatorType;
+ const std::string actuatorTypeStr = RuntimeEnvironment::checkParameter("actuator", "motor");
+ mujoco::ActuatorType actuatorType;
try
{
- actuatorType = mjcf::toActuatorType(actuatorTypeStr);
+ actuatorType = mujoco::toActuatorType(actuatorTypeStr);
}
catch (const std::out_of_range&)
{
- std::cout << "No actuator '" << actuatorTypeStr << "'" << std::endl;
+ std::cout << "No actuator type '" << actuatorTypeStr << "'" << std::endl;
std::cout << "Avaliable: motor|position|velocity" << std::endl;
return -1;
}
- bool mocap = RuntimeEnvironment::checkParameter("mocap", "n") == "y";
+ const bool mocap = RuntimeEnvironment::hasFlag("mocap");
+ const bool verbose = RuntimeEnvironment::hasFlag("verbose");
+ const bool addActuatorSuffix = RuntimeEnvironment::hasFlag("actuator_suffix");
+
+ const bool useRelativePaths = RuntimeEnvironment::hasFlag("rel_paths");
+
+
+ auto parseFloatParameter = [](const std::string& key, const std::string& default_)
+ {
+ const std::string string = RuntimeEnvironment::checkParameter(key, default_);
+ try
+ {
+ return std::stof(string);
+ }
+ catch (const std::invalid_argument& e)
+ {
+ std::cerr << "Could not parse value of argument " << key << ": '" << string << "' \n"
+ << "Reason: " << e.what() << std::endl;
+ throw e;
+ }
+ };
+
+ float scaleLength;
+ float scaleMesh;
+ float scaleMass;
+ try
+ {
+ scaleLength = parseFloatParameter("scale_length", "1");
+ scaleMesh = parseFloatParameter("mesh_length", "1");
+ scaleMass = parseFloatParameter("scale_mass", "1");
+ }
+ catch (const std::invalid_argument&)
+ {
+ return -1;
+ }
std::cout << "Input file: " << inputFilename << std::endl;
std::cout << "Output dir: " << outputDir << std::endl;
std::cout << "Output mesh dir: " << outputDir / meshRelDir << std::endl;
- //std::cout << "Actuator type: " << actuatorType << std::endl;
- std::cout << "With mocap body: " << (mocap ? "yes" : "no ");
+ std::cout << "Actuator type: " << actuatorTypeStr << std::endl;
+ std::cout << "Mocap body: " << (mocap ? "yes" : "no ") << std::endl;
+
+ std::cout << "Scaling: " << std::endl
+ << " - length: " << scaleLength << std::endl
+ << " - mesh: " << scaleMesh << std::endl
+ << " - mass: " << scaleMass << std::endl;
std::cout << "Loading robot ..." << std::endl;
RobotPtr robot;
try
{
+ std::cout << "Loading robot from " << inputFilename << " ..." << std::endl;
robot = RobotIO::loadRobot(inputFilename.string(), RobotIO::eFull);
assert(robot);
}
@@ -102,7 +152,7 @@ int main(int argc, char* argv[])
throw; // rethrow
}
- if (false)
+ if (/* DISABLES CODE */ (false))
{
// using RobotIO
RobotIO::saveMJCF(robot, inputFilename.filename().string(), outputDir.string(), meshRelDir);
@@ -110,9 +160,20 @@ int main(int argc, char* argv[])
else
{
// direct API
- mjcf::MujocoIO mujocoIO(robot);
+ mujoco::MujocoIO mujocoIO(robot);
+
mujocoIO.setActuatorType(actuatorType);
+ mujocoIO.setAddActuatorTypeSuffix(addActuatorSuffix);
+
+ mujocoIO.setUseRelativePaths(useRelativePaths);
mujocoIO.setWithMocapBody(mocap);
+
+ mujocoIO.setLengthScale(scaleLength);
+ mujocoIO.setMeshScale(scaleMesh);
+ mujocoIO.setMassScale(scaleMass);
+
+ mujocoIO.setVerbose(verbose);
+
mujocoIO.saveMJCF(inputFilename.filename().string(), outputDir.string(), meshRelDir);
}
}
diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index 6ee4ca92209380f12b71f7235d5bea130cbe8122..643734184e81a0d4808af13bcf58ea2a0559bfce 100644
--- a/VirtualRobot/math/Helpers.cpp
+++ b/VirtualRobot/math/Helpers.cpp
@@ -229,6 +229,18 @@ void Helpers::InvertPose(Eigen::Matrix4f& pose)
Position(pose) = - Orientation(pose) * Position(pose);
}
+void Helpers::ScaleTranslation(Eigen::Matrix4f& pose, float scale)
+{
+ Position(pose) *= scale;
+}
+
+Eigen::Matrix4f Helpers::ScaledTranslation(const Eigen::Matrix4f& pose, float scale)
+{
+ Eigen::Matrix4f scaled = pose;
+ ScaleTranslation(scaled, scale);
+ return scaled;
+}
+
Eigen::Matrix3f Helpers::GetRotationMatrix(const Eigen::Vector3f& source, const Eigen::Vector3f& target)
{
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index 33703de24faf0dd13d399ca45884211fae43694d..9b3d079537ff1dd6e08bccd94b66f3fa4577e7f5 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -138,6 +138,11 @@ namespace math
template <typename Derived>
static Eigen::Matrix4f InvertedPose(const Eigen::MatrixBase<Derived>& pose);
+ /// Scale the translation/position of the given pose.
+ static void ScaleTranslation(Eigen::Matrix4f& pose, float scale);
+ /// Get the pose with its translation/position scaled.
+ static Eigen::Matrix4f ScaledTranslation(const Eigen::Matrix4f& pose, float scale);
+
/// Get a cartesian vector from cylinder coordinates.
static Eigen::Vector3f CreateVectorFromCylinderCoords(float r, float angle, float z);
/// Get a cartesian vector from cylinder coordinates.
diff --git a/VirtualRobot/tests/CMakeLists.txt b/VirtualRobot/tests/CMakeLists.txt
index 112052c6fb14f0f5ec8e3b8b5339d37f2507e058..d6399d134a6bcbd7e6190042d242209f176c785f 100644
--- a/VirtualRobot/tests/CMakeLists.txt
+++ b/VirtualRobot/tests/CMakeLists.txt
@@ -30,7 +30,10 @@ ADD_VR_TEST( VirtualRobotMeshImportTest )
ADD_VR_TEST( VirtualRobotTimeOptimalTrajectoryTest )
+ADD_VR_TEST( VirtualRobotRuntimeEnvironmentTest )
+
ADD_VR_TEST( VirtualRobotJsonEigenConversionTest )
+ADD_VR_TEST( VirtualRobotMjcfTest )
ADD_VR_TEST( MathFitPlaneTest )
ADD_VR_TEST( MathGaussianImplicitSurface3DNormalsTest )
diff --git a/VirtualRobot/tests/VirtualRobotMjcfTest.cpp b/VirtualRobot/tests/VirtualRobotMjcfTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..863addcdf96ca9b551fc4d873742df140380b25c
--- /dev/null
+++ b/VirtualRobot/tests/VirtualRobotMjcfTest.cpp
@@ -0,0 +1,134 @@
+/**
+* @package VirtualRobot
+* @author Rainer Kartmann
+* @copyright 2018 Rainer Kartmann
+*/
+
+#define BOOST_TEST_MODULE VirtualRobot_VirtualRobotMjcfTest
+
+#include <VirtualRobot/VirtualRobotTest.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <VirtualRobot/MJCF/Document.h>
+
+
+namespace Eigen
+{
+ std::ostream& operator<<(std::ostream& os, const Vector3f& rhs)
+ {
+ static const IOFormat iof(4, 0, " ", " ", "", "", "[", "]");
+ os << rhs.format(iof);
+ return os;
+ }
+
+ bool operator==(const Quaternionf& lhs, const Quaternionf& rhs)
+ {
+ return lhs.isApprox(rhs);
+ }
+
+ std::ostream& operator<<(std::ostream& os, const Quaternionf& rhs)
+ {
+ os << "[ " << rhs.w() << " | " << rhs.x() << " " << rhs.y() << " " << rhs.z() << " ]";
+ return os;
+ }
+}
+
+#define MSG_CONVERSION(in, string, out) \
+ BOOST_TEST_MESSAGE(in << " -> '" << string << "' -> " << out)
+
+BOOST_AUTO_TEST_SUITE(VirtualRobotMjcfTest)
+
+BOOST_AUTO_TEST_CASE(test_boost_lexical_cast)
+{
+ for (bool in : { true, false })
+ {
+ const std::string string = boost::lexical_cast<std::string>(in);
+ bool out = boost::lexical_cast<bool>(string);
+ MSG_CONVERSION(in, string, out);
+ BOOST_CHECK_EQUAL(in, out);
+ }
+
+ // this cannot be handled by boost
+ BOOST_CHECK_THROW(boost::lexical_cast<bool>("true"), boost::bad_lexical_cast);
+ BOOST_CHECK_THROW(boost::lexical_cast<bool>("false"), boost::bad_lexical_cast);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
+
+using namespace Eigen;
+
+
+BOOST_AUTO_TEST_CASE(test_parseCoeffs)
+{
+ const std::string string = "1 -3 2.4";
+ Eigen::Vector3f vector(1, -3, 2.4f);
+
+ std::vector<float> coeffs = mjcf::parseCoeffs<float>(string, ' ');
+
+ BOOST_CHECK_EQUAL(coeffs[0], vector.x());
+ BOOST_CHECK_EQUAL(coeffs[1], vector.y());
+ BOOST_CHECK_EQUAL(coeffs[2], vector.z());
+}
+
+
+BOOST_AUTO_TEST_CASE(test_attrib_conversion_vector3f)
+{
+ Vector3f in(1, -3, 2.4f), out;
+
+ const std::string string = mjcf::toAttr(in);
+ mjcf::fromAttr(string, out);
+ MSG_CONVERSION(in, string, out);
+
+ BOOST_CHECK_EQUAL(in, out);
+}
+
+BOOST_AUTO_TEST_CASE(test_attrib_conversion_quaternionf)
+{
+ Quaternionf in(AngleAxisf(1.4f, Vector3f(.5, -1, .3f).normalized())), out;
+
+ const std::string string = mjcf::toAttr(in);
+ mjcf::fromAttr(string, out);
+ MSG_CONVERSION(in, string, out);
+
+ BOOST_CHECK_EQUAL(in, out);
+}
+
+
+
+
+struct Fixture
+{
+ mjcf::Document document;
+
+ mjcf::Body body;
+ mjcf::Joint joint;
+
+ Fixture()
+ {
+ body = document.worldbody().addBody("thebody");
+ joint = body.addJoint();
+ joint.name = "thejoint";
+ }
+};
+
+
+BOOST_FIXTURE_TEST_SUITE(VirtualRobotMjcfTest, Fixture)
+
+BOOST_AUTO_TEST_CASE(test_attribute_pos)
+{
+ Eigen::Vector3f in(1, -3, 2.4f), out;
+ body.pos = in;
+ out = body.pos;
+ BOOST_CHECK_EQUAL(in, out);
+}
+
+BOOST_AUTO_TEST_CASE(test_attribute_quat)
+{
+ Eigen::Quaternionf out, in(Eigen::AngleAxisf(1.2, Eigen::Vector3f(1, -3, 2.4f).normalized()));
+ body.quat = in;
+ out = body.quat;
+ BOOST_CHECK_EQUAL(in, out);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/VirtualRobot/tests/VirtualRobotRuntimeEnvironmentTest.cpp b/VirtualRobot/tests/VirtualRobotRuntimeEnvironmentTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1935b9d0be8a277e16e797a773b7a95c0a43cec
--- /dev/null
+++ b/VirtualRobot/tests/VirtualRobotRuntimeEnvironmentTest.cpp
@@ -0,0 +1,55 @@
+/**
+* @package VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2011 Nikolaus Vahrenkamp
+*/
+
+#define BOOST_TEST_MODULE VirtualRobot_VirtualRobotRuntimeEnvironmentTest
+
+#include <VirtualRobot/VirtualRobotTest.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+
+
+BOOST_AUTO_TEST_SUITE(RuntimeEnvironment)
+
+
+BOOST_AUTO_TEST_CASE(test_toVector3f_valid_input)
+{
+ Eigen::Vector3f vector;
+
+ BOOST_CHECK(VirtualRobot::RuntimeEnvironment::toVector3f("(0, 0, 0)", vector));
+ BOOST_CHECK_EQUAL(vector, Eigen::Vector3f::Zero());
+
+ BOOST_CHECK(VirtualRobot::RuntimeEnvironment::toVector3f("(1, 2, 3)", vector));
+ BOOST_CHECK_EQUAL(vector, Eigen::Vector3f(1, 2, 3));
+
+ BOOST_CHECK(VirtualRobot::RuntimeEnvironment::toVector3f("(-1, 2, 5)", vector));
+ BOOST_CHECK_EQUAL(vector, Eigen::Vector3f(-1, 2, 5));
+
+ BOOST_CHECK(VirtualRobot::RuntimeEnvironment::toVector3f("(-3.14, 0, 9.99)", vector));
+ BOOST_CHECK_EQUAL(vector, Eigen::Vector3f(-3.14f, 0, 9.99f));
+}
+
+
+BOOST_AUTO_TEST_CASE(test_toVector3f_invalid_input)
+{
+ Eigen::Vector3f vector;
+
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("", vector));
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("()", vector));
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("nonsense", vector));
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("(0)", vector));
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("(0, 0)", vector));
+
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("1, 2, 3", vector));
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("1, 2, 3)", vector));
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("(1, 2, 3", vector));
+
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("(0. 0, 0)", vector));
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("(0; 0; 0)", vector));
+
+ BOOST_CHECK(!VirtualRobot::RuntimeEnvironment::toVector3f("(0, 0, a)", vector));
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()