Fixed calculations of muslce lengths and moment arms. Fixed Dynamics...

Fixed calculations of muslce lengths and moment arms. Fixed Dynamics calculations with RBDL. Added Simox_USE_RBDL to CMake

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@737 042f3d55-54a8-47e9-b7fb-15903f145c44

CMakeModules/SimoxConfig.cmake.in

@@ -46,6 +46,7 @@ SET(Simox_VISUALIZATION "@Simox_VISUALIZATION@")
 SET(Simox_USE_COIN_VISUALIZATION "@Simox_USE_COIN_VISUALIZATION@")
 SET(Simox_USE_OPENSCENEGRAPH_VISUALIZATION "@Simox_USE_OPENSCENEGRAPH_VISUALIZATION@")
 SET(Simox_USE_COLLADA "@Simox_USE_COLLADA@")
+SET(Simox_USE_RBDL "@Simox_USE_RBDL@")
 
 
 SET(Simox_EXTERNAL_INCLUDE_DIRS "@Simox_EXTERNAL_INCLUDE_DIRS@")

VirtualRobot/Dynamics/dynamics.cpp

@@ -29,12 +29,11 @@ using namespace RigidBodyDynamics::Math;
 
 
 
-VirtualRobot::Dynamics::Dynamics(RobotNodeSetPtr rns) : rns(rns)
-{
-    if (!rns)
-    {
-        THROW_VR_EXCEPTION("Null data");
-    }
+VirtualRobot::Dynamics::Dynamics(RobotNodeSetPtr rns) : rns(rns) {
+   if(!rns)
+   {
+       THROW_VR_EXCEPTION("RobotNodeSetPtr is zero pointer");
+   }
 
     gravity = Vector3d(0, 0, -9.81);
     model = boost::shared_ptr<RigidBodyDynamics::Model>(new Model());
@@ -76,12 +75,37 @@ int Dynamics::getnDoF()
 
 int Dynamics::toRBDL(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNodePtr node, RobotNodePtr parentNode, int parentID)
 {
+    RobotNodePtr physicsFromChild;
     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>();
+
+
+    // if mass is 0, check children for translational joint. In case there is one, take its mass, com and inertia. Probably a bit hacky right now!
+    if (fabs(mass) < 0.000001)
+    {
+        BOOST_FOREACH(SceneObjectPtr child, node->getChildren())
+        {
+            boost::shared_ptr<RobotNode> childPtr = boost::dynamic_pointer_cast<RobotNode>(child);
+            if(childPtr != 0 && childPtr->isTranslationalJoint())
+            {
+                mass = childPtr->getMass();
+                com = child->getCoMLocal().cast<double>()/1000;
+                inertia = child->getInertiaMatrix().cast<double>();
+                physicsFromChild = childPtr;
+                break;
+            }
+        }
+    }
+    /*
+    cout << "Mass: " << mass << endl;
+    cout << "CoM:" << com << endl;
+    cout << "Inertia: " << inertia << endl;
+    */
+
     Body body = Body(mass, com, inertia);
 
 
@@ -123,6 +147,9 @@ int Dynamics::toRBDL(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNod
         nodeID = model->AddBody(parentID, spatial_transform, joint, body);
     }
 
+    if(physicsFromChild != 0)
+        node = physicsFromChild;
+
     BOOST_FOREACH(SceneObjectPtr child, node->getChildren())
     {
         boost::shared_ptr<RobotNode> childRobotNode = boost::dynamic_pointer_cast<RobotNode>(child);
@@ -133,7 +160,6 @@ int Dynamics::toRBDL(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNod
             {
                 node = parentNode;
             }
-
             toRBDL(model, childRobotNode, node, nodeID);
 
         }

VirtualRobot/Nodes/RobotNodeRevolute.cpp

@@ -203,8 +203,8 @@ namespace VirtualRobot
     }
 
 
-    std::string RobotNodeRevolute::_toXML(const std::string& modelPath)
-    {
+   std::string RobotNodeRevolute::_toXML(const std::string& modelPath)
+   {
         std::stringstream ss;
         ss << "\t\t<Joint type='revolute'>" << endl;
         ss << "\t\t\t<axis x='" << jointRotationAxis[0] << "' y='" << jointRotationAxis[1] << "' z='" << jointRotationAxis[2] << "'/>" << endl;
@@ -212,8 +212,23 @@ namespace VirtualRobot
         ss << "\t\t\t<MaxAcceleration value='" << maxAcceleration << "'/>" << endl;
         ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << endl;
         ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << endl;
-
         std::map< std::string, float >::iterator propIt = propagatedJointValues.begin();
+        while (propIt != propagatedJointValues.end())
+        {
+            ss << "\t\t\t<PropagateJointValue name='" << propIt->first << "' factor='" << propIt->second << "'/>" << endl;
+            propIt++;
+        }
+        ss << "\t\t</Joint>" << endl;
+        return ss.str();
+}
+
+float RobotNodeRevolute::getLMTC(float angle)
+{
+
+    return sqrt(2 - 2 * cos(angle));
+    /*
+    Eigen::Matrix4f fromParent = getTransformationFrom(getParent());
+    Eigen::Matrix4f toChild = child->getTransformationFrom(child->getParent());
 
         while (propIt != propagatedJointValues.end())
         {
@@ -226,14 +241,21 @@ namespace VirtualRobot
         return ss.str();
     }
 
-    float RobotNodeRevolute::getLMTC(RobotNodePtr child, float angle)
-    {
+    return sqrt(b_squared);
+    */
+}
 
-        Eigen::Matrix4f fromParent = getTransformationFrom(getParent());
-        Eigen::Matrix4f toChild = child->getTransformationFrom(child->getParent());
+        //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();
+float RobotNodeRevolute::getLMomentArm(float angle)
+{
+    float b = getLMTC(angle);
+
+    return sqrt(4*pow(b,2)-pow(b,4))/(2*b);
+    /*
+    Eigen::Matrix4f fromParent = getTransformationFrom(getParent());
+    Eigen::Matrix4f toChild = child->getTransformationFrom(child->getParent());
 
         THROW_VR_EXCEPTION_IF(a < 0.0000001 || c < 0.0000001, "Node has no spatial offset to either parent or child");
 
@@ -243,11 +265,10 @@ namespace VirtualRobot
     }
 
 
-    float RobotNodeRevolute::getLMomentArm(RobotNodePtr child, float angle)
-    {
-        Eigen::Matrix4f fromParent = getTransformationFrom(getParent());
-        Eigen::Matrix4f toChild = child->getTransformationFrom(child->getParent());
-
+    return lMomentArm;
+    */
+}
+        /*
         float a = fromParent.col(3).head(3).norm();
         float c = toChild.col(3).head(3).norm();
 
@@ -258,6 +279,7 @@ namespace VirtualRobot
         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

VirtualRobot/Nodes/RobotNodeRevolute.h

@@ -97,21 +97,21 @@ namespace VirtualRobot
         */
         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);
+    /*!
+     * \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(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(float angle);
 
     protected:
         /*!

VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp

@@ -31,6 +31,7 @@ int main(int argc, char* argv[])
 
     if (rob)
     {
+
         RobotNodeSetPtr ns = rob->getRobotNodeSet("RightArm");
 
         VirtualRobot::Dynamics dynamics = VirtualRobot::Dynamics(ns);
@@ -41,8 +42,6 @@ int main(int argc, char* argv[])
 
         cout << "inverse dynamics: " << endl << dynamics.getInverseDynamics(q, qdot, qddot) << endl;
         cout << "joint space inertia matrix: " << endl << dynamics.getInertiaMatrix(q) << endl;
-
-
     }
     else
     {

config.cmake

@@ -162,8 +162,10 @@ IF (NOT Simox_CONFIGURED)
 	    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})
+            OPTION (Simox_USE_RBDL "Use RBDL" ON)
 	else (RBDL_FOUND)
 	    MESSAGE(STATUS "RBDL not found!")
+            OPTION (Simox_USE_RBDL "Use RBDL" OFF)
 	endif (RBDL_FOUND)
 
 	#### Eigen