integrated rbdl forward backward dynamics

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

VirtualRobot/Dynamics/dynamics.cpp

@@ -55,6 +55,40 @@ Eigen::VectorXd Dynamics::getInverseDynamics(Eigen::VectorXd q, Eigen::VectorXd
     return tau;
 }
 
+Eigen::VectorXd Dynamics::getForwardDynamics(Eigen::VectorXd q, Eigen::VectorXd qdot, Eigen::VectorXd tau)
+{
+    Eigen::VectorXd qddot = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+    InverseDynamics(*model.get(), q, qdot, tau, qddot);
+    return qddot;
+}
+
+
+Eigen::VectorXd Dynamics::getGravityMatrix(Eigen::VectorXd q, int nDof)
+{
+
+    Eigen::VectorXd qdot = Eigen::VectorXd::Zero(nDof);
+    Eigen::VectorXd qddot = Eigen::VectorXd::Zero(nDof);
+
+    Eigen::VectorXd tauGravity = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+    InverseDynamics(*model.get(), q, qdot, qddot, tauGravity);
+    return tauGravity;
+}
+
+Eigen::VectorXd Dynamics::getCoriolisMatrix(Eigen::VectorXd q, Eigen::VectorXd qdot, int nDof)
+{
+
+    Eigen::VectorXd qddot = Eigen::VectorXd::Zero(nDof);
+    Eigen::VectorXd tauGravity = getGravityMatrix(q, nDof);
+    Eigen::VectorXd tau= Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+
+    Eigen::MatrixXd tauCoriolis = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+    InverseDynamics(*model.get(), q, qdot, qddot, tau);
+    tauCoriolis=tau-tauGravity;
+    return tauCoriolis;
+}
+
+
+
 Eigen::MatrixXd Dynamics::getInertiaMatrix(Eigen::VectorXd q)
 {
     Eigen::MatrixXd inertia = Eigen::MatrixXd::Zero(model->dof_count, model->dof_count);

VirtualRobot/Dynamics/dynamics.h

@@ -24,6 +24,12 @@ namespace VirtualRobot
         /// 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::VectorXd getGravityMatrix(Eigen::VectorXd q, int nDof);
+        /// Calculates the joint space Gravity Matrix given a joint position vector q and Number of DOF
+        Eigen::VectorXd getCoriolisMatrix(Eigen::VectorXd q, Eigen::VectorXd qdot, int nDof);
+        /// Calculates the coriolis matrix given position vector q, velocity vector qdot and Number of DOF
+        Eigen::VectorXd getForwardDynamics(Eigen::VectorXd q, Eigen::VectorXd qdot, Eigen::VectorXd tau);
+        /// Calculates forward dynamics given position vector q velocity vector qdot and joint torques tau
         Eigen::MatrixXd getInertiaMatrix(Eigen::VectorXd q);
         /// Sets the gravity vector of the dynamics system
         void setGravity(Eigen::Vector3d gravity);

VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp

@@ -39,9 +39,14 @@ int main(int argc, char* argv[])
         Eigen::VectorXd q = Eigen::VectorXd::Random(nDof);
         Eigen::VectorXd qdot = Eigen::VectorXd::Random(nDof);
         Eigen::VectorXd qddot = Eigen::VectorXd::Random(nDof);
+        Eigen::VectorXd tau = Eigen::VectorXd::Random(nDof);
 
-        cout << "inverse dynamics: " << endl << dynamics.getInverseDynamics(q, qdot, qddot) << endl;
-        cout << "joint space inertia matrix: " << endl << dynamics.getInertiaMatrix(q) << endl;
+
+        cout << "joint torques from inverse dynamics: " << endl << dynamics.getInverseDynamics(q, qdot, qddot) << endl;
+        cout << "joint asdasdspace inertia matrix: " << endl << dynamics.getInertiaMatrix(q) << endl;
+        cout << "joint space gravitational matrix:" << endl << dynamics.getGravityMatrix(q, nDof) << endl;
+        cout << "joint space coriolis matrix:" << endl << dynamics.getCoriolisMatrix(q, qdot, nDof) << endl;
+        cout << "joint space accelerations from forward dynamics:" << endl << dynamics.getForwardDynamics(q, qdot, tau) << endl;
     }
     else
     {