diff --git a/CMakeModules/FindRBDL.cmake b/CMakeModules/FindRBDL.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..b6462bd30a5619f114488725bf39c1aada585d01
--- /dev/null
+++ b/CMakeModules/FindRBDL.cmake
@@ -0,0 +1,63 @@
+# Searches for RBDL includes and library files
+#
+# Sets the variables
+# RBDL_FOUND
+# RBDL_INCLUDE_DIR
+# RBDL_LIBRARIES
+
+SET (RBDL_FOUND FALSE)
+
+FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h
+ /usr/include
+ /usr/local/include
+ $ENV{HOME}/local/include
+ $ENV{RBDL_PATH}/src
+ $ENV{RBDL_PATH}/include
+ $ENV{RBDL_INCLUDE_PATH}
+ )
+FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl PATHS
+ /usr/lib
+ /usr/local/lib
+ $ENV{HOME}/local/lib
+ $ENV{RBDL_PATH}
+ $ENV{RBDL_LIBRARY_PATH}
+ )
+
+FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY NAMES rbdl_luamodel PATHS
+ /usr/lib
+ /usr/local/lib
+ $ENV{HOME}/local/lib
+ $ENV{RBDL_PATH}
+ $ENV{RBDL_LIBRARY_PATH}
+ )
+
+IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
+ SET (RBDL_FOUND TRUE)
+ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
+
+IF (RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+ SET (RBDL_LIBRARIES ${RBDL_LIBRARY} ${RBDL_LUAMODEL_LIBRARY})
+ELSE (RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+ SET (RBDL_LIBRARIES ${RBDL_LIBRARY})
+ENDIF(RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+
+IF (RBDL_FOUND)
+ IF (NOT RBDL_FIND_QUIETLY)
+ MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}")
+
+ IF (RBDL_LUAMODEL_LIBRARY)
+ MESSAGE(STATUS "Found RBDL Addon Luamodel: ${RBDL_LUAMODEL_LIBRARY}")
+ ENDIF (RBDL_LUAMODEL_LIBRARY)
+ ENDIF (NOT RBDL_FIND_QUIETLY)
+ELSE (RBDL_FOUND)
+ IF (RBDL_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find RBDL")
+ ENDIF (RBDL_FIND_REQUIRED)
+ENDIF (RBDL_FOUND)
+
+MARK_AS_ADVANCED (
+ RBDL_INCLUDE_DIR
+ RBDL_LIBRARIES
+ RBDL_LIBRARY
+ RBDL_LUAMODEL_LIBRARY
+ )
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 1fb4700dec6433dd982910483cf3966c24f777a5..a401a370aa3f10122d2675617ad2322de503c157 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -10,6 +10,8 @@ if(NOT ("${CMAKE_VERSION}" VERSION_LESS 2.8.12))
ENDIF()
+FIND_PACKAGE (RBDL)
+
########################### TESTING #####################################
MACRO(ADD_VR_TEST TEST_NAME)
INCLUDE_DIRECTORIES(${Simox_EXTERNAL_INCLUDE_DIRS})
@@ -103,6 +105,19 @@ Import/RobotImporterFactory.cpp
Import/MeshImport/STLReader.cpp
)
+if (RBDL_FOUND)
+ SET(SOURCES
+ ${SOURCES}
+ Dynamics/dynamics.cpp
+ )
+SET(INCLUDES
+${INCLUDES}
+Dynamics/dynamics.h
+)
+
+endif (RBDL_FOUND)
+
+
SET(INCLUDES
CollisionDetection/CollisionChecker.h
CollisionDetection/CollisionModel.h
diff --git a/VirtualRobot/Dynamics/CMakeLists.txt b/VirtualRobot/Dynamics/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ee36ad81329ddb5fbb47f9ffd905d6dabd55f562
--- /dev/null
+++ b/VirtualRobot/Dynamics/CMakeLists.txt
@@ -0,0 +1,25 @@
+PROJECT ( RBDL )
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4)
+
+CMAKE_POLICY(VERSION 2.6)
+
+ADD_EXECUTABLE(${PROJECT_NAME} rbdl.cpp)
+SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR})
+SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES FOLDER "Examples")
+
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} VirtualRobot)
+
+
+#######################################################################################
+############################ Setup for installation ###################################
+#######################################################################################
+
+install(TARGETS ${PROJECT_NAME}
+ # IMPORTANT: Add the library to the "export-set"
+ EXPORT SimoxTargets
+ RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin
+ COMPONENT dev)
+
+MESSAGE( STATUS " ** Simox application ${PROJECT_NAME} will be placed into " ${Simox_BIN_DIR})
+MESSAGE( STATUS " ** Simox application ${PROJECT_NAME} will be installed into " ${INSTALL_BIN_DIR})
diff --git a/VirtualRobot/Dynamics/dynamics.cpp b/VirtualRobot/Dynamics/dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..56e652e0130c798e9b4966aa7ebbb1c31a1c0f7e
--- /dev/null
+++ b/VirtualRobot/Dynamics/dynamics.cpp
@@ -0,0 +1,133 @@
+#include <rbdl/rbdl.h>
+#include "dynamics.h"
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Nodes/RobotNodeFactory.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+#include <VirtualRobot/Units.h>
+
+#include <Eigen/Dense>
+
+#include <math.h>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/foreach.hpp>
+#include <string>
+#include <iostream>
+
+using std::cout;
+using std::cin;
+using namespace VirtualRobot;
+using namespace RigidBodyDynamics;
+using namespace RigidBodyDynamics::Math;
+
+
+
+
+
+
+VirtualRobot::Dynamics::Dynamics(RobotNodeSetPtr rns) : rns(rns) {
+ if(!rns)
+ THROW_VR_EXCEPTION("Null data");
+
+ gravity = Vector3d(0,0,-9.81);
+ model = boost::shared_ptr<RigidBodyDynamics::Model>(new Model());
+
+ model->gravity = gravity;
+
+ if (!rns->isKinematicChain())
+ THROW_VR_EXCEPTION("RobotNodeSet is no kinematic chain!")
+
+ RobotNodePtr root = rns->getKinematicRoot();
+
+ int rootID = Dynamics::toRBDL(model,root);
+}
+
+Eigen::VectorXd Dynamics::getInverseDynamics(Eigen::VectorXd q, Eigen::VectorXd qdot, Eigen::VectorXd qddot)
+{
+ Eigen::VectorXd tau = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+ InverseDynamics(*model.get(),q,qdot,qddot,tau);
+ return tau;
+}
+
+Eigen::MatrixXd Dynamics::getInertiaMatrix(Eigen::VectorXd q)
+{
+ Eigen::MatrixXd inertia = Eigen::MatrixXd::Zero(model->dof_count,model->dof_count);
+ CompositeRigidBodyAlgorithm(*model.get(),q,inertia);
+ return inertia;
+}
+
+void Dynamics::setGravity(Eigen::Vector3d gravity)
+{
+ model->gravity = gravity;
+}
+
+int Dynamics::getnDoF()
+{
+ return model->dof_count;
+}
+
+
+int Dynamics::toRBDL(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNodePtr node, RobotNodePtr parentNode, int parentID)
+{
+ int nodeID = parentID;
+ // need to define body, joint and spatial transform
+ // body first
+ float mass = node->getMass();
+ Vector3d com = node->getCoMLocal().cast<double>()/1000; // divide by 1000 because Simox defines lengths in mm while the RBDL defines lengths in m
+ Matrix3d inertia = node->getInertiaMatrix().cast<double>();
+ Body body = Body(mass, com, inertia);
+
+
+ // spatial transform next
+ Eigen::Matrix4d trafo = Eigen::Matrix4d::Identity();
+
+ if(parentNode)
+ trafo = node->getTransformationFrom(parentNode).cast<double>();
+
+ Matrix3d spatial_rotation = trafo.block(0,0,3,3);
+ Vector3d spatial_translation = trafo.col(3).head(3)/1000;
+
+ SpatialTransform spatial_transform = SpatialTransform(spatial_rotation,spatial_translation);
+
+ // last, joint
+ Joint joint = Joint(JointTypeFixed);
+ if (node->isRotationalJoint())
+ {
+ JointType joint_type = JointTypeRevolute;
+ boost::shared_ptr<RobotNodeRevolute> rev = boost::dynamic_pointer_cast<RobotNodeRevolute>(node);
+ Vector3d joint_axis = rev->getJointRotationAxisInJointCoordSystem().cast<double>();
+
+ joint = Joint(joint_type, joint_axis);
+ }
+ else if(node->isTranslationalJoint())
+ {
+ JointType joint_type = JointTypePrismatic;
+ boost::shared_ptr<RobotNodePrismatic> prism = boost::dynamic_pointer_cast<RobotNodePrismatic>(node);
+ Vector3d joint_axis = prism->getJointTranslationDirectionJointCoordSystem().cast<double>();
+
+ joint = Joint(joint_type,joint_axis);
+ }
+
+ if (joint.mJointType != JointTypeFixed)
+ {
+ nodeID = model->AddBody(parentID,spatial_transform,joint,body);
+ }
+
+ BOOST_FOREACH(SceneObjectPtr child, node->getChildren())
+ {
+ boost::shared_ptr<RobotNode> childRobotNode = boost::dynamic_pointer_cast<RobotNode>(child);
+ if (childRobotNode != 0 && Dynamics::rns->hasRobotNode(childRobotNode)) // if cast returns 0 pointer, child is a sensor and can be omitted. also, child must be contained in the robotnodeset
+ {
+ if (joint.mJointType == JointTypeFixed) // if current node is fixed, make its parent the parent of the next recursion step and thereby skip it
+ node = parentNode;
+ toRBDL(model, childRobotNode,node, nodeID);
+
+ }
+ }
+
+ return nodeID;
+}
diff --git a/VirtualRobot/Dynamics/dynamics.h b/VirtualRobot/Dynamics/dynamics.h
new file mode 100644
index 0000000000000000000000000000000000000000..830153eb6662e3db250e038ec5c2aff4eea96721
--- /dev/null
+++ b/VirtualRobot/Dynamics/dynamics.h
@@ -0,0 +1,42 @@
+#ifndef _DYNAMICS_H_
+#define _DYNAMICS_H__
+
+#include "../VirtualRobotImportExport.h"
+
+#include "../Nodes/RobotNode.h"
+#include "../RobotNodeSet.h"
+#include <rbdl/rbdl.h>
+
+#include <boost/shared_ptr.hpp>
+
+
+namespace VirtualRobot
+{
+
+/** @brief
+ * Encapsulates dynamics simulations for the virtual robot.
+ */
+class Dynamics{
+public:
+ /// Creates a Dynamics object given a RobotNodeSet
+ Dynamics(RobotNodeSetPtr rns);
+ /// Calculates the Inverse Dynamics for given motion state defined by q, qdot and qddot
+ Eigen::VectorXd getInverseDynamics(Eigen::VectorXd q,Eigen::VectorXd qdot,Eigen::VectorXd qddot);
+ /// Calculates the joint space inertia matrix given a joint position vector q
+ Eigen::MatrixXd getInertiaMatrix(Eigen::VectorXd q);
+ /// Sets the gravity vector of the dynamics system
+ void setGravity(Eigen::Vector3d gravity);
+ /// returns the number of Degrees of Freedom of the dynamics system
+ int getnDoF();
+
+protected:
+ RobotNodeSetPtr rns;
+ boost::shared_ptr<RigidBodyDynamics::Model> model;
+ Eigen::Vector3d gravity;
+
+private:
+ int toRBDL(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNodePtr node, RobotNodePtr parentNode = RobotNodePtr(), int parentID = 0);
+};
+}
+
+#endif
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.cpp b/VirtualRobot/Nodes/RobotNodeRevolute.cpp
index 6c1bbc8ec1f5f4fc749a8c66292f48dd0b58e9f6..e97b9ab286227ae1c05670c3925fecc1a357b0e7 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.cpp
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.cpp
@@ -215,4 +215,38 @@ std::string RobotNodeRevolute::_toXML( const std::string &modelPath )
return ss.str();
}
+float RobotNodeRevolute::getLMTC(RobotNodePtr child, float angle)
+{
+
+ Eigen::Matrix4f fromParent = getTransformationFrom(getParent());
+ Eigen::Matrix4f toChild = child->getTransformationFrom(child->getParent());
+
+ float a = fromParent.col(3).head(3).norm();
+ float c = toChild.col(3).head(3).norm();
+
+ THROW_VR_EXCEPTION_IF (a < 0.0000001 || c < 0.0000001, "Node has no spatial offset to either parent or child");
+
+ float b_squared = a*a + c*c - 2*a*c*cos(angle);
+
+ return sqrt(b_squared);
+}
+
+
+float RobotNodeRevolute::getLMomentArm(RobotNodePtr child, float angle)
+{
+ Eigen::Matrix4f fromParent = getTransformationFrom(getParent());
+ Eigen::Matrix4f toChild = child->getTransformationFrom(child->getParent());
+
+ float a = fromParent.col(3).head(3).norm();
+ float c = toChild.col(3).head(3).norm();
+
+ THROW_VR_EXCEPTION_IF (a < 0.0000001 || c < 0.0000001, "Node has no spatial offset to either parent or child");
+
+ float b = getLMTC(child,angle);
+
+ float lMomentArm = sqrt(2*(a*a*b*b + b*b*c*c + c*c*a*a) - (pow(a,4)+pow(b,4)+pow(c,4)))/(2*b);
+
+ return lMomentArm;
+}
+
} // namespace
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.h b/VirtualRobot/Nodes/RobotNodeRevolute.h
index 7ced414e22475e3bf462e0253a30ead236a64645..560c956fd9b02cf25c3c073ecc18eb5110c25069 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.h
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.h
@@ -96,6 +96,23 @@ public:
This is the original joint axis, without any transformations applied.
*/
Eigen::Vector3f getJointRotationAxisInJointCoordSystem() const;
+
+ /*!
+ * \brief getLMTC Calculates the spatial distance between the parent of a revolute joint and a given child with the joint set to a given angle (e.g. the length of a muscle-tendon complex attached to the parent and the given child)
+ * \param child The child node
+ * \param angle The angle of the revolute joint in radians
+ * \return The spatial distance between parent and given child at given angle
+ */
+ virtual float getLMTC(RobotNodePtr child, float angle);
+
+ /*!
+ * \brief getLMomentArm Calculates the spatial length of a moment arm defined through the triangle given by the node's parent, the specified child and the specified angle at the revolulte joint.
+ * \param child The child node
+ * \param angle The angle of the revolute joint in radians
+ * \return The spatial length of the moment arm
+ */
+ virtual float getLMomentArm(RobotNodePtr child, float angle);
+
protected:
/*!
Can be called by a RobotNodeActuator in order to set the pose of this node.
diff --git a/VirtualRobot/examples/CMakeLists.txt b/VirtualRobot/examples/CMakeLists.txt
index bffc3a8d9e3b32a338e0a75bbfe4c602f19ae358..1abde1698f1c0abadd57d5be269dfc4f429fe5e6 100644
--- a/VirtualRobot/examples/CMakeLists.txt
+++ b/VirtualRobot/examples/CMakeLists.txt
@@ -1,4 +1,10 @@
+FIND_PACKAGE (RBDL)
+
+if (RBDL_FOUND)
+ ADD_SUBDIRECTORY(InverseDynamics)
+endif (RBDL_FOUND)
+
ADD_SUBDIRECTORY(loadRobot)
ADD_SUBDIRECTORY(RobotViewer)
diff --git a/VirtualRobot/examples/InverseDynamics/CMakeLists.txt b/VirtualRobot/examples/InverseDynamics/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..55e0eac5224520cf7c5fc077d2bc1bf218137246
--- /dev/null
+++ b/VirtualRobot/examples/InverseDynamics/CMakeLists.txt
@@ -0,0 +1,25 @@
+PROJECT ( InverseDynamics )
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4)
+
+CMAKE_POLICY(VERSION 2.6)
+
+ADD_EXECUTABLE(${PROJECT_NAME} InverseDynamics.cpp)
+SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR})
+SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES FOLDER "Examples")
+
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} VirtualRobot)
+
+
+#######################################################################################
+############################ Setup for installation ###################################
+#######################################################################################
+
+install(TARGETS ${PROJECT_NAME}
+ # IMPORTANT: Add the library to the "export-set"
+ EXPORT SimoxTargets
+ RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin
+ COMPONENT dev)
+
+MESSAGE( STATUS " ** Simox application ${PROJECT_NAME} will be placed into " ${Simox_BIN_DIR})
+MESSAGE( STATUS " ** Simox application ${PROJECT_NAME} will be installed into " ${INSTALL_BIN_DIR})
diff --git a/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp b/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2090e55cdede5cc31c54efff4bf1c4f55a4376ff
--- /dev/null
+++ b/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp
@@ -0,0 +1,53 @@
+#include <VirtualRobot/Dynamics/dynamics.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+
+using std::cout;
+using namespace VirtualRobot;
+
+
+
+
+int main(int argc, char *argv[])
+{
+ std::string filename("robots/ArmarIII/ArmarIII.xml");
+ VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename);
+ VirtualRobot::RuntimeEnvironment::processCommandLine(argc, argv);
+
+ cout << "Using robot at " << filename << endl;
+ RobotPtr rob;
+
+ try
+ {
+ rob = RobotIO::loadRobot(filename,RobotIO::eStructure);
+ } catch (VirtualRobotException &e)
+ {
+ cout << "Error: " << e.what() << endl;
+ return -1;
+ }
+
+ if (rob)
+ {
+ RobotNodeSetPtr ns = rob->getRobotNodeSet("RightArm");
+
+ VirtualRobot::Dynamics dynamics = VirtualRobot::Dynamics(ns);
+ int nDof = dynamics.getnDoF();
+ Eigen::VectorXd q = Eigen::VectorXd::Random(nDof);
+ Eigen::VectorXd qdot = Eigen::VectorXd::Random(nDof);
+ Eigen::VectorXd qddot = Eigen::VectorXd::Random(nDof);
+
+ cout << "inverse dynamics: " << endl << dynamics.getInverseDynamics(q,qdot,qddot) << endl;
+ cout << "joint space inertia matrix: " << endl << dynamics.getInertiaMatrix(q) << endl;
+
+
+ }
+ else
+ {
+ cout << " ERROR while creating robobt" << endl;
+ }
+}
+
+
+
diff --git a/config.cmake b/config.cmake
index 7bc89abedd00a36ab5f1f9de484a502cdccc0242..fd8d3427eeea868349a144a4ef69b88524b46c4f 100644
--- a/config.cmake
+++ b/config.cmake
@@ -155,6 +155,16 @@ IF (NOT Simox_CONFIGURED)
############################# SETUP MODULES #############################
MESSAGE (STATUS "** Module path: " ${CMAKE_MODULE_PATH})
+ ### RBDL
+ FIND_PACKAGE (RBDL)
+ if (RBDL_FOUND)
+ MESSAGE(STATUS "RBDL found at: ${RBDL_INCLUDE_DIR}")
+ SET (Simox_EXTERNAL_INCLUDE_DIRS ${Simox_EXTERNAL_INCLUDE_DIRS} ${RBDL_INCLUDE_DIR})
+ SET (Simox_EXTERNAL_LIBRARIES ${Simox_EXTERNAL_LIBRARIES} ${RBDL_LIBRARIES})
+ else (RBDL_FOUND)
+ MESSAGE(STATUS "RBDL not found!")
+ endif (RBDL_FOUND)
+
#### Eigen
FIND_PACKAGE (Eigen3 REQUIRED)
if (Eigen3_FOUND)