From 4df2e609c266f80d932ef278acde57a24f2c8c0b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mirko=20W=C3=A4chter?= <mirko.waechter@kit.edu>
Date: Thu, 16 May 2019 15:17:51 +0200
Subject: [PATCH] extended inverse dynamics example
---
.../InverseDynamics/InverseDynamics.cpp | 74 +++++++++++++++++--
1 file changed, 66 insertions(+), 8 deletions(-)
diff --git a/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp b/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp
index 5a671b4b2..f8208dcec 100644
--- a/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp
+++ b/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp
@@ -17,7 +17,6 @@ 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;
@@ -35,21 +34,80 @@ int main(int argc, char* argv[])
{
RobotNodeSetPtr ns = rob->getRobotNodeSet("RightArm");
+ RobotNodeSetPtr bodyNs = rob->getRobotNodeSet("RightArmHandColModel");
+// RobotNodeSetPtr bodyNs = rob->getRobotNodeSet("RightArmCol");
- VirtualRobot::Dynamics dynamics = VirtualRobot::Dynamics(ns);
+ Gravity g(rob, ns, bodyNs);
+ for(auto& pair:g.getMasses())
+ {
+ cout << pair.first <<": " << pair.second << endl;
+ }
+ VirtualRobot::Dynamics dynamics = VirtualRobot::Dynamics(ns, bodyNs, true);
+ dynamics.print();
int nDof = dynamics.getnDoF();
- Eigen::VectorXd q = Eigen::VectorXd::Random(nDof);
+ Eigen::VectorXd q = Eigen::VectorXd::Zero(nDof);
+ ns->setJointValues(q.cast<float>());
+ q = ns->getJointValuesEigen().cast<double>(); // get joint values with joint limits applied
Eigen::VectorXd qdot = Eigen::VectorXd::Random(nDof);
Eigen::VectorXd qddot = Eigen::VectorXd::Random(nDof);
Eigen::VectorXd tau = Eigen::VectorXd::Random(nDof);
+ auto start = std::chrono::system_clock::now();
+ int c = 1000;
+ for (int i = 0; i < c; ++i) {
+ 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);
+ Eigen::VectorXd invDyn = dynamics.getInverseDynamics(q, qdot, qddot);
+ }
+ auto end = std::chrono::system_clock::now();
+ auto elapsed =
+ std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+ std::cout << "duration:" << (elapsed.count()/c) << '\n';
+
+ for (int i = 0; i < c; ++i) {
+// break;
+ 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);
+ ns->setJointValues(q.cast<float>());
+ q = ns->getJointValuesEigen().cast<double>(); // get joint values with joint limits applied
+ Eigen::VectorXd gravityRBDL = dynamics.getGravityMatrix(q);
+ int d=0;
+ std::vector<float> gravityVR;
+ g.computeGravityTorque(gravityVR);
+ for(auto & val: gravityVR)
+ {
+ auto diff = val- gravityRBDL(d);
+ if(std::abs(diff)> 0.01)
+ throw std::runtime_error((std::to_string(i) + " dim: " + std::to_string(d) + " diff: " + std::to_string(diff).c_str()));
+ d++;
+ }
+
+ }
+
+ Eigen::VectorXd invDyn = dynamics.getInverseDynamics(q, qdot, qddot);
+ cout << "Joint values:\n" << q << endl;
+ ns->setJointValues(q.cast<float>());
+ cout << "Joint values in VR:\n" << q << endl;
+ std::vector<float> gravityVR;
+ g.computeGravityTorque(gravityVR);
- 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 torques from inverse dynamics: " << endl << invDyn << endl;
+ cout << "joint space inertia matrix: " << endl << dynamics.getInertiaMatrix(q) << endl;
+ cout << "joint space gravitational matrix:" << endl << dynamics.getGravityMatrix(q) << endl;
+ cout << "joint space VR gravity :" << endl;
+ int i=0;
+ for(auto & val: gravityVR)
+ {
+ cout << ns->getNode(i)->getName() << ": " << val << endl;
+ i++;
+ }
+ cout << "joint space coriolis matrix:" << endl << dynamics.getCoriolisMatrix(q, qdot) << endl;
cout << "joint space accelerations from forward dynamics:" << endl << dynamics.getForwardDynamics(q, qdot, tau) << endl;
- cout << "Identifier for Elbow R:" << endl << dynamics.getIdentifier("Elbow R") << endl;
+// cout << "Identifier for Elbow R:" << endl << dynamics.getIdentifier("Elbow R") << endl;
}
else
--
GitLab