Preliminary urdf model support

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "plugins/MotionMarkerPlugin/c3dfile"]
 	path = plugins/MotionMarkerPlugin/c3dfile
 	url = https://github.com/ORB-HD/c3dfile
+[submodule "vendor/urdfparser"]
+	path = vendor/urdfparser
+	url = https://github.com/ORB-HD/URDF_Parser

CMakeLists.txt

@@ -1,6 +1,6 @@
 CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 
-SET ( TOOLKIT_VERSION_MAJOR 0 )
+SET ( TOOLKIT_VERSION_MAJOR 1 )
 SET ( TOOLKIT_VERSION_MINOR 1 )
 SET ( TOOLKIT_VERSION_PATCH 0 )
 SET ( TOOLKIT_VERSION 
@@ -15,6 +15,8 @@ IF(APPLE)
 OPTION(BUILD_OSX_BUNDLE "Build Application as osx app bundle" OFF)
 ENDIF(APPLE)
 
+SET(CMAKE_CXX_STANDARD 17)
+
 # The QT Interface
 SET( QT_USE_QTXML TRUE )
 SET( QT_WRAP_CPP TRUE )
@@ -45,6 +47,7 @@ INCLUDE_DIRECTORIES (${CMAKE_CURRENT_BINARY_DIR})
 INCLUDE_DIRECTORIES (${CMAKE_CURRENT_SOURCE_DIR})
 INCLUDE_DIRECTORIES (${CMAKE_CURRENT_SOURCE_DIR}/include)
 INCLUDE_DIRECTORIES (${CMAKE_CURRENT_BINARY_DIR}/include)
+INCLUDE_DIRECTORIES (${CMAKE_SOURCE_DIR}/vendor/urdfparser/include)
 INCLUDE_DIRECTORIES (${CMAKE_CURRENT_BINARY_DIR}/ui)
 INCLUDE_DIRECTORIES (${RBDL_INCLUDE_DIR})
 INCLUDE_DIRECTORIES (${EIGEN3_INCLUDE_DIR})
@@ -96,14 +99,20 @@ SET ( ToolkitLib_SRCS
 	src/rbdl_wrapper.cc
 	src/variantdelegate.cc
 	src/toolkit_errors.cc
+	src/luamodel_wrapper.cc
+	src/urdfmodel_wrapper.cc
 )
 
 SET ( Toolkit_SRCS
 	src/main.cc
 )
 
 ADD_SUBDIRECTORY (
-	plugins
+  plugins
+)
+
+ADD_SUBDIRECTORY (
+  vendor
 )
 
 ADD_LIBRARY ( toolkitlib SHARED
@@ -123,6 +132,8 @@ TARGET_LINK_LIBRARIES ( toolkitlib
 	Qt5::3DInput
 	${RBDL_LIBRARY}
 	${RBDL_LUAMODEL_LIBRARY}
+	${RBDL_URDFREADER_LIBRARY}
+	urdfparser
 )
 
 IF(BUILD_OSX_BUNDLE)

include/luamodel_wrapper.h

@@ -0,0 +1,25 @@
+#ifndef LUAMODEL_WRAPPER_H_INCLUDED
+#define LUAMODEL_WRAPPER_H_INCLUDED
+
+#include <rbdl/rbdl.h>
+#include <rbdl/rbdl_math.h>
+#include <rbdl/addons/luamodel/luamodel.h>
+#include <rbdl/addons/luamodel/luatables.h>
+
+#include "rbdl_wrapper.h"
+
+#define MODELTYPE_LUA "lua_model"
+
+class LuaModelWrapper : public RBDLModelWrapper {
+	public:
+		LuaModelWrapper();
+		LuaTable model_luatable;
+
+		void load(QString model_file);
+		ModelInfo loadModelInfo();
+		std::vector<SegmentVisualInfo> loadSegmentInfo();
+};
+
+#endif
+
+

include/rbdl_wrapper.h

@@ -6,6 +6,7 @@
 #include <rbdl/addons/luamodel/luamodel.h>
 #include <rbdl/addons/luamodel/luatables.h>
 
+#include <QColor>
 #include <QString>
 #include <QFileInfo>
 #include <Qt3DCore/QEntity>
@@ -33,50 +34,73 @@ class WrapperExtension {
 		virtual Qt3DCore::QEntity* getVisual();
 };
 
+struct ModelInfo {
+	QMatrix4x4 model_world_transform;
+	float unit_scaling;
+};
+
+struct SegmentVisualInfo {
+	std::string segment_name;
+
+	QString mesh_file;
+	QVector3D mesh_translation;
+	QQuaternion mesh_rotation;
+
+	QColor visual_color;
+	QVector3D visual_center;
+	QVector3D visual_dimensions;
+};
+
 
 /* This class provides a wrapper around rbdl models, in order to visualize them with Qt3D
  */
 class RBDLModelWrapper : public QObject {
 	Q_OBJECT
-	private:
+	protected:
 		QString model_file;
+		std::string model_type;
 
-		Qt3DCore::QEntity* model_render_obj;
-		std::map<std::string, Qt3DCore::QEntity*> body_mesh_map;
+		Qt3DCore::QEntity* model_root;
+		std::map<std::string, Qt3DCore::QEntity*> body_segment_map;
 		std::map<std::string, Qt3DCore::QTransform*> body_transform_map;
 		float mesh_unit_scaling;
 
 		//all loaded extra data is supposed to be loaded as an extension to the model
 		std::vector<std::string> extension_names;
 		std::map<std::string, WrapperExtension*> extensions;
+
+		RigidBodyDynamics::Math::VectorNd current_Q;
+
+		void clear();
+		virtual void load(QString model_file) = 0;
 	public:
-		LuaTable model_luatable;
+		static RBDLModelWrapper* loadFromFile(QString model_file);
+
 
 		RigidBodyDynamics::Model* rbdl_model;
 
 		RBDLModelWrapper();
+		void build3DEntity(ModelInfo&, std::vector<SegmentVisualInfo>&);
+
+		QString getFileName() { return QFileInfo(model_file).baseName(); }
+		std::string getModelType() { return model_type; }
+		QString getModelFile();
+		int getModelDof();
+		Qt3DCore::QEntity* getRenderObj() { return model_root; }
+		Qt3DCore::QEntity* getSegmentEntity(std::string segment_name, bool create=false);
 
-		Qt3DCore::QEntity* getRenderObj() { return model_render_obj; }
+		void addStaticVisual(std::string segment_name, Qt3DCore::QEntity *visual);
+		void updateKinematics(RigidBodyDynamics::Math::VectorNd Q);
 
-		Qt3DCore::QEntity* loadFromFile(QString model_file);
+		void reload();
 
 		//takes ownership of extension -> only delete via model not where it was created
 		void addExtension(WrapperExtension* extension);
-		void addStaticVisual(std::string segment_name, Qt3DCore::QEntity *visual);
 		//void deleteExtension(std::string name);
 		bool hasExtension(std::string name);
 		const std::vector<std::string>& loadedExtensions() { return extension_names; }
 		WrapperExtension* getExtension(std::string name);
 
-		void updateKinematics(RigidBodyDynamics::Math::VectorNd Q);
-		QString getFileName() { return QFileInfo(model_file).baseName(); }
-
-		int getModelDof();
-
-		void reload();
-
-		QString getModelFile();
-
 	public slots:
 		void model_update(float current_time);
 
@@ -86,5 +110,6 @@ class RBDLModelWrapper : public QObject {
 
 };
 
+
 #endif 
 

include/render_util.h

@@ -7,7 +7,12 @@
 #include <rbdl/addons/luamodel/luatables.h>
 
 Qt3DCore::QEntity* createGridFloor(float lxy_border, float r_xyborder, int count, QColor line_color=QColor(QRgb(0xffffff)));
-//Qt3DCore::QEntity* loadFromLuaTable(LuaTableNode &table);
+
+Qt3DCore::QEntity* createMeshEntity(const QString& mesh_file, 
+                                    const QColor& mesh_color, 
+                                    const QVector3D& mesh_translation, 
+                                    const QQuaternion& mesh_rotation, 
+                                    Qt3DCore::QEntity* parent=nullptr);
 
 #endif 
 

include/urdfmodel_wrapper.h

@@ -0,0 +1,25 @@
+#ifndef URDFMODEL_WRAPPER_H_INCLUDED
+#define URDFMODEL_WRAPPER_H_INCLUDED
+
+#include <rbdl/rbdl.h>
+#include <rbdl/rbdl_math.h>
+#include <rbdl/addons/urdfreader/urdfreader.h>
+
+#include "rbdl_wrapper.h"
+
+#define MODELTYPE_URDF "urdf_model"
+
+class UrdfModelWrapper : public RBDLModelWrapper {
+	public:
+		UrdfModelWrapper();
+
+		std::string model_xml_string;
+
+		void load(QString model_file);
+		ModelInfo loadModelInfo();
+		std::vector<SegmentVisualInfo> loadSegmentInfo();
+};
+
+
+#endif 
+

include/util.h

@@ -5,13 +5,18 @@
 
 #include <QString>
 #include <QStringList>
+#include <QVector3D>
 #include <string>
 
+#include <rbdl/rbdl_math.h>
+
 // returs an empty file on failure
 QString findFile(QString file, bool absolute=false);
 QString findFile(std::string file, bool absolute=false); 
 
 QString findPlugin(QString plugin);
 QStringList findAllPlugins();
 
+QVector3D to_qt_vector(RigidBodyDynamics::Math::Vector3d&);
+
 #endif 

plugins/AnimationPlugin/AnimationModelExtension.cc

@@ -16,6 +16,7 @@ std::string AnimationModelExtension::getExtensionName() {
 
 void AnimationModelExtension::update(float current_time) {
 	unsigned int time_index = 0;
+
 	for (; time_index < animation_times.size(); time_index++) {
 		if (time_index == animation_times.size()-1) break;
 		if (animation_times[time_index] <= current_time && animation_times[time_index+1] > current_time) {

plugins/InverseKinematicsPlugin/CMakeLists.txt

@@ -1,5 +1,5 @@
 PROJECT(InverseKinematicsPlugin)
-	
+
 SET(InverseKinematicsPlugin_SRCS
 	InverseKinematicsPlugin.cc
 )
@@ -10,7 +10,7 @@ SET(InverseKinematicsPlugin_MOC_HDRS
 )
 
 SET(CMAKE_AUTOMOC ON)
-	
+
 INCLUDE_DIRECTORIES (${QT_INCLUDE_DIR})
 INCLUDE_DIRECTORIES (${CMAKE_SOURCE_DIR}/include)
 INCLUDE_DIRECTORIES (${CMAKE_CURRENT_SOURCE_DIR})
@@ -30,9 +30,9 @@ SET(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 ADD_LIBRARY (IKplugin SHARED ${InverseKinematicsPlugin_SRCS})
 
-TARGET_LINK_LIBRARIES (IKplugin 
-	${QT_LIBRARIES} 
-	${QT_QTXML_LIBRARY} 
+TARGET_LINK_LIBRARIES (IKplugin
+	${QT_LIBRARIES}
+	${QT_QTXML_LIBRARY}
 	Qt5::Core
 	Qt5::Gui
 	Qt5::Widgets

plugins/MotionMarkerPlugin/MotionMarkerPlugin.cc

@@ -112,25 +112,30 @@ void MotionMarkerPlugin::loadMarkerSettings() {
 	parentApp->toolkit_settings.endGroup();
 }
 
-void MotionMarkerPlugin::addModelMarkersToModel(RBDLModelWrapper *model) {
-	ModelMarkerExtension* ext = new ModelMarkerExtension(marker_color_model, marker_size);
-	unsigned int segment_cnt = model->model_luatable["frames"].length();
-	for(int i=1; i<=segment_cnt; i++) {
-		std::string segment_name = model->model_luatable["frames"][i]["name"].get<std::string>();
-		std::vector<LuaKey> keys= model->model_luatable["frames"][i]["markers"].keys();
-
-		for (auto marker_name : keys) {
-			Vector3d marker_position = model->model_luatable["frames"][i]["markers"][marker_name.string_value.c_str()].getDefault(Vector3d(0.,0.,0.));
-			//marker_position = model->axis_transform * marker_position;
-
-			ext->addModelMarker(marker_name.string_value, segment_name, marker_position);
+void MotionMarkerPlugin::addModelMarkersToModel(RBDLModelWrapper *rbdl_model) {
+	if (rbdl_model->getModelType() == MODELTYPE_LUA) {
+		LuaModelWrapper *model = (LuaModelWrapper*)rbdl_model;
+		ModelMarkerExtension* ext = new ModelMarkerExtension(marker_color_model, marker_size);
+		unsigned int segment_cnt = model->model_luatable["frames"].length();
+		for(int i=1; i<=segment_cnt; i++) {
+			std::string segment_name = model->model_luatable["frames"][i]["name"].get<std::string>();
+			std::vector<LuaKey> keys= model->model_luatable["frames"][i]["markers"].keys();
+
+			for (auto marker_name : keys) {
+				Vector3d marker_position = model->model_luatable["frames"][i]["markers"][marker_name.string_value.c_str()].getDefault(Vector3d(0.,0.,0.));
+				//marker_position = model->axis_transform * marker_position;
+
+				ext->addModelMarker(marker_name.string_value, segment_name, marker_position);
+			}
 		}
+		if (ext->getMarkerLabels().size() == 0) {
+			delete ext;
+			return;
+		}
+		model->addExtension(ext);
+	} else {
+		parentApp->showWarningDialog("Loading model markes from this model type is not supported.");
 	}
-	if (ext->getMarkerLabels().size() == 0) {
-		delete ext;
-		return;
-	}
-	model->addExtension(ext);
 }
 
 void MotionMarkerPlugin::action_load_data() {

plugins/MotionMarkerPlugin/MotionMarkerPlugin.h

@@ -8,6 +8,7 @@
 
 #include "toolkit_interfaces.h"
 #include "util.h"
+#include "luamodel_wrapper.h"
 
 #include "MotionMarkerExtension.h"
 #include "ModelMarkerExtension.h"