Squashed 'third_party/ct/' content from commit 0048d02
Change-Id: Ia7e5360cbb414f92ce4f118bd9613ea23597db52
git-subtree-dir: third_party/ct
git-subtree-split: 0048d027531b6cf1ea730da17b68a0b7ef9070b1
diff --git a/ct_models/src/HyQ/codegen/HyQLinearizationCodgen.cpp b/ct_models/src/HyQ/codegen/HyQLinearizationCodgen.cpp
new file mode 100644
index 0000000..3e27019
--- /dev/null
+++ b/ct_models/src/HyQ/codegen/HyQLinearizationCodgen.cpp
@@ -0,0 +1,154 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+#include <ct/rbd/rbd.h>
+#include <ct/models/HyQ/HyQ.h>
+#include <ct/models/CodegenOutputDirs.h>
+
+#include "helperFunctions.h"
+
+// shortcut for the auto-diff codegen scalar
+typedef CppAD::AD<CppAD::cg::CG<double>> SCALAR;
+typedef typename SCALAR::value_type AD_ValueType;
+
+// a floating base forward-dynamic system templated on scalar type
+typedef ct::rbd::FloatingBaseFDSystem<ct::rbd::HyQ::tpl::Dynamics<SCALAR>, false> HyQSystemAD;
+
+// extract dimensions
+const size_t state_dim = HyQSystemAD::STATE_DIM;
+const size_t control_dim = HyQSystemAD::CONTROL_DIM;
+
+// shortcut for number of joints
+const size_t njoints = HyQSystemAD::Kinematics::NJOINTS;
+
+
+void generateInverseDynamics()
+{
+ typedef ct::core::DerivativesCppadCG<state_dim + 18, control_dim + 6> JacCG;
+ typename JacCG::FUN_TYPE_CG f = ct::models::HyQ::hyqInverseDynamics<SCALAR>;
+ JacCG jacCG(f);
+
+ try
+ {
+ std::cout << "Generating Jacobian of Inverse Dynamics wrt state using forward mode... " << std::endl;
+ jacCG.generateJacobianSource("HyQInverseDynJacForward", ct::models::HYQ_CODEGEN_OUTPUT_DIR,
+ ct::core::CODEGEN_TEMPLATE_DIR, "models", "HyQ", JacCG::Sparsity::Ones(), false);
+
+ std::cout << "Generating Jacobian of Inverse Dynamics wrt state using reverse mode... " << std::endl;
+ jacCG.generateJacobianSource("HyQInverseDynJacReverse", ct::models::HYQ_CODEGEN_OUTPUT_DIR,
+ ct::core::CODEGEN_TEMPLATE_DIR, "models", "HyQ", JacCG::Sparsity::Ones(), true);
+ } catch (const std::runtime_error& e)
+ {
+ std::cout << "inverse dynamics code generation failed: " << e.what() << std::endl;
+ }
+}
+
+void generateForwardKinematics()
+{
+ typedef ct::core::DerivativesCppadCG<state_dim, 4 * 6> JacCG;
+ typename JacCG::FUN_TYPE_CG f = ct::models::HyQ::hyqForwardKinematics<SCALAR>;
+ JacCG jacCG(f);
+
+ try
+ {
+ std::cout << "Generating Jacobian of Forward Kinematics wrt state using forward mode... " << std::endl;
+ jacCG.generateJacobianSource("HyQForwardKinJacForward", ct::models::HYQ_CODEGEN_OUTPUT_DIR,
+ ct::core::CODEGEN_TEMPLATE_DIR, "models", "HyQ", JacCG::Sparsity::Ones(), false);
+
+ std::cout << "Generating Jacobian of Forward Kinematics wrt state using reverse mode... " << std::endl;
+ jacCG.generateJacobianSource("HyQForwardKinJacReverse", ct::models::HYQ_CODEGEN_OUTPUT_DIR,
+ ct::core::CODEGEN_TEMPLATE_DIR, "models", "HyQ", JacCG::Sparsity::Ones(), true);
+ } catch (const std::runtime_error& e)
+ {
+ std::cout << "forward kinematics code generation failed: " << e.what() << std::endl;
+ }
+}
+
+void generateForwardZeroForwardDynamics()
+{
+ typedef ct::core::DerivativesCppadCG<state_dim + control_dim + 1, state_dim> JacCG;
+ // Eigen::Matrix<SCALAR, state_dim + control_dim + 1, 1> testInput = Eigen::Matrix<SCALAR, state_dim + control_dim + 1, 1>::Random();
+ // auto asd = ct::models::HyQ::hyqContactModelForwardDynamics<SCALAR>(testInput);
+ typename JacCG::FUN_TYPE_CG f = ct::models::HyQ::hyqContactModelForwardDynamics<SCALAR>;
+ JacCG jacCG(f);
+
+ try
+ {
+ std::cout << "Generating Forward Zero Code... " << std::endl;
+ jacCG.generateForwardZeroSource("HyQForwardZero", ct::models::HYQ_CODEGEN_OUTPUT_DIR,
+ ct::core::CODEGEN_TEMPLATE_DIR, "models", "HyQ", false);
+ } catch (const std::runtime_error& e)
+ {
+ std::cout << "forward zero code generation failed: " << e.what() << std::endl;
+ }
+}
+
+
+void generateFDLinearization(int argc, char* argv[])
+{
+ std::cout << "Generating Jacobian of Forward Dynamics... " << std::endl;
+
+ // a contact model (auto-diff'able)
+ typedef ct::rbd::EEContactModel<typename HyQSystemAD::Kinematics> ContactModel;
+
+ std::shared_ptr<HyQSystemAD> adSystem = std::shared_ptr<HyQSystemAD>(new HyQSystemAD);
+
+ // explicitely pass kinematics from the system such that both, contact model and system use
+ // the same instance to give the AD codegen the opportunity to fully optimize the code
+ std::shared_ptr<ContactModel> contactModel =
+ std::shared_ptr<ContactModel>(new ContactModel(SCALAR(5000.0), SCALAR(1000.0), SCALAR(100.0), SCALAR(100.0),
+ SCALAR(-0.02), ContactModel::VELOCITY_SMOOTHING::SIGMOID, adSystem->dynamics().kinematicsPtr()));
+
+ bool useContactModel = (argc <= 2 || !std::string(argv[2]).compare("nocontact") == 0);
+ std::cout << std::boolalpha << "using contact model: " << useContactModel << std::endl;
+
+ // asign the contact model
+ if (useContactModel)
+ adSystem->setContactModel(contactModel);
+
+ // create the codegen linearizer
+ ct::core::ADCodegenLinearizer<state_dim, control_dim> adLinearizer(adSystem);
+
+ std::string baseName;
+ if (useContactModel)
+ baseName = "HyQWithContactModelLinearized";
+ else
+ baseName = "HyQBareModelLinearized";
+
+ try
+ {
+ std::cout << "generating code..." << std::endl;
+ if (argc > 1 && std::string(argv[1]).compare("reverse") == 0)
+ {
+ std::cout << "generating using reverse mode" << std::endl;
+
+ adLinearizer.generateCode(baseName + "Reverse", ct::models::HYQ_CODEGEN_OUTPUT_DIR,
+ ct::core::CODEGEN_TEMPLATE_DIR, "models", "HyQ", true);
+ }
+ else
+ {
+ std::cout << "generating using forward mode" << std::endl;
+
+ adLinearizer.generateCode(baseName + "Forward", ct::models::HYQ_CODEGEN_OUTPUT_DIR,
+ ct::core::CODEGEN_TEMPLATE_DIR, "models", "HyQ", false);
+ }
+
+ std::cout << "... done!" << std::endl;
+ } catch (const std::runtime_error& e)
+ {
+ std::cout << "forward dynamics code generation failed: " << e.what() << std::endl;
+ }
+}
+
+
+int main(int argc, char* argv[])
+{
+ generateFDLinearization(argc, argv);
+ generateInverseDynamics();
+ generateForwardKinematics();
+ generateForwardZeroForwardDynamics();
+}
diff --git a/ct_models/src/HyQ/codegen/compareForwardReverseFD.cpp b/ct_models/src/HyQ/codegen/compareForwardReverseFD.cpp
new file mode 100644
index 0000000..cc1452d
--- /dev/null
+++ b/ct_models/src/HyQ/codegen/compareForwardReverseFD.cpp
@@ -0,0 +1,232 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <ct/models/HyQ/HyQ.h>
+#include <ct/models/HyQ/codegen/HyQWithContactModelLinearizedForward.h>
+#include <ct/models/HyQ/codegen/HyQWithContactModelLinearizedReverse.h>
+#include <ct/models/HyQ/codegen/HyQBareModelLinearizedForward.h>
+#include <ct/models/HyQ/codegen/HyQBareModelLinearizedReverse.h>
+
+using namespace ct::models::HyQ;
+
+void timing(bool useContactModel)
+{
+ std::cout << std::boolalpha << "Using contact model: " << useContactModel << std::endl;
+
+ typedef ct::rbd::FloatingBaseFDSystem<ct::rbd::HyQ::Dynamics, false> HyQSystem;
+ std::shared_ptr<HyQSystem> hyqSys = std::shared_ptr<HyQSystem>(new HyQSystem);
+
+ typedef ct::rbd::EEContactModel<typename HyQSystem::Kinematics> ContactModel;
+ std::shared_ptr<ContactModel> contactModel = std::shared_ptr<ContactModel>(new ContactModel(5000.0, 1000.0, 100.0,
+ 100.0, -0.02, ContactModel::VELOCITY_SMOOTHING::SIGMOID, hyqSys->dynamics().kinematicsPtr()));
+ if (useContactModel)
+ hyqSys->setContactModel(contactModel);
+
+ ct::rbd::RbdLinearizer<HyQSystem> linearizer(hyqSys);
+ ct::core::SystemLinearizer<HyQSystem::STATE_DIM, HyQSystem::CONTROL_DIM> sysLinearizer(hyqSys, false);
+
+ typedef std::shared_ptr<ct::core::LinearSystem<HyQSystem::STATE_DIM, HyQSystem::CONTROL_DIM>> LinModelPtr;
+ LinModelPtr linModelForward;
+ LinModelPtr linModelReverse;
+
+ if (useContactModel)
+ {
+ linModelForward = LinModelPtr(new HyQWithContactModelLinearizedForward);
+ linModelReverse = LinModelPtr(new HyQWithContactModelLinearizedReverse);
+ }
+ else
+ {
+ linModelForward = LinModelPtr(new HyQBareModelLinearizedForward);
+ linModelReverse = LinModelPtr(new HyQBareModelLinearizedReverse);
+ }
+
+ static const size_t nTests = 10000;
+
+ typedef typename HyQSystem::StateVector X;
+ typedef typename HyQSystem::ControlVector U;
+
+ typedef Eigen::Matrix<double, HyQSystem::STATE_DIM, HyQSystem::STATE_DIM> JacA;
+ typedef Eigen::Matrix<double, HyQSystem::STATE_DIM, HyQSystem::CONTROL_DIM> JacB;
+
+
+ std::vector<X, Eigen::aligned_allocator<X>> x(nTests);
+ std::vector<U, Eigen::aligned_allocator<U>> u(nTests);
+
+ std::vector<JacA, Eigen::aligned_allocator<JacA>> forwardA(nTests);
+ std::vector<JacA, Eigen::aligned_allocator<JacA>> reverseA(nTests);
+ std::vector<JacA, Eigen::aligned_allocator<JacA>> rbdA(nTests);
+ std::vector<JacA, Eigen::aligned_allocator<JacA>> numDiffA(nTests);
+
+ std::vector<JacB, Eigen::aligned_allocator<JacB>> forwardB(nTests);
+ std::vector<JacB, Eigen::aligned_allocator<JacB>> reverseB(nTests);
+ std::vector<JacB, Eigen::aligned_allocator<JacB>> rbdB(nTests);
+ std::vector<JacB, Eigen::aligned_allocator<JacB>> numDiffB(nTests);
+
+ std::cout << "input dim: " << HyQSystem::STATE_DIM + HyQSystem::CONTROL_DIM
+ << ", output dim A: " << HyQSystem::STATE_DIM << ", output dim B: " << HyQSystem::CONTROL_DIM
+ << std::endl;
+
+ std::cout << "running " << nTests << " tests" << std::endl;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ x[i].setRandom();
+ u[i].setRandom();
+ }
+
+ auto start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ forwardA[i] = linModelForward->getDerivativeState(x[i], u[i]);
+ }
+ auto end = std::chrono::high_resolution_clock::now();
+ auto diff = end - start;
+ double msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "forwardA: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ reverseA[i] = linModelReverse->getDerivativeState(x[i], u[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "reverseA: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ rbdA[i] = linearizer.getDerivativeState(x[i], u[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "rbdA: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ numDiffA[i] = sysLinearizer.getDerivativeState(x[i], u[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "numDiffA: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ forwardB[i] = linModelForward->getDerivativeControl(x[i], u[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "forwardB: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ reverseB[i] = linModelReverse->getDerivativeControl(x[i], u[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "reverseB: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ rbdB[i] = linearizer.getDerivativeControl(x[i], u[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "rbdB: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ numDiffB[i] = sysLinearizer.getDerivativeControl(x[i], u[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "numDiffB: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+
+ bool failed = false;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ if (!forwardA[i].isApprox(rbdA[i], 1e-5))
+ {
+ std::cout << "Forward A and RbdLinearizer A not similar" << std::endl;
+ std::cout << "forward A: " << std::endl << forwardA[i] << std::endl;
+ std::cout << "rbd A: " << std::endl << rbdA[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (!reverseA[i].isApprox(rbdA[i], 1e-5))
+ {
+ std::cout << "Reverse A and RbdLinearizer A not similar" << std::endl;
+ std::cout << "reverse A: " << std::endl << reverseA[i] << std::endl;
+ std::cout << "rbd A: " << std::endl << rbdA[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (!forwardA[i].isApprox(reverseA[i], 1e-12))
+ {
+ std::cout << "Forward A and reverse A not similar" << std::endl;
+ std::cout << "forward A: " << std::endl << forwardA[i] << std::endl;
+ std::cout << "reverse A: " << std::endl << reverseA[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (failed)
+ {
+ std::cout << "test failed, aborting" << std::endl;
+ break;
+ }
+ }
+
+ failed = false;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ if (!forwardB[i].isApprox(rbdB[i], 1e-5))
+ {
+ std::cout << "Forward B and RbdLinearizer B not similar" << std::endl;
+ std::cout << "forward B: " << std::endl << forwardB[i] << std::endl;
+ std::cout << "rbd B: " << std::endl << rbdB[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (!reverseB[i].isApprox(rbdB[i], 1e-5))
+ {
+ std::cout << "Forward B and RbdLinearizer B not similar" << std::endl;
+ std::cout << "reverse B: " << std::endl << reverseB[i] << std::endl;
+ std::cout << "rbd B: " << std::endl << rbdB[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (!forwardB[i].isApprox(reverseB[i], 1e-12))
+ {
+ std::cout << "Forward B and reverse B not similar" << std::endl;
+ std::cout << "forward B: " << std::endl << forwardB[i] << std::endl;
+ std::cout << "reverse B: " << std::endl << reverseB[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (failed)
+ {
+ std::cout << "test failed, aborting" << std::endl;
+ break;
+ }
+ }
+}
+
+
+int main(int argc, char* argv[])
+{
+ timing(true);
+ std::cout << std::endl;
+ timing(false);
+ return 0;
+}
diff --git a/ct_models/src/HyQ/codegen/compareForwardReverseID.cpp b/ct_models/src/HyQ/codegen/compareForwardReverseID.cpp
new file mode 100644
index 0000000..f98a842
--- /dev/null
+++ b/ct_models/src/HyQ/codegen/compareForwardReverseID.cpp
@@ -0,0 +1,111 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <ct/models/HyQ/HyQ.h>
+#include <ct/models/HyQ/codegen/HyQInverseDynJacForward.h>
+#include <ct/models/HyQ/codegen/HyQInverseDynJacReverse.h>
+
+#include "helperFunctions.h"
+
+using namespace ct::models::HyQ;
+
+void timing()
+{
+ std::cout << "Timing inverse dynamics " << std::endl;
+ static const size_t nTests = 10000;
+
+ const size_t IN_DIM = state_dim + 18;
+ const size_t OUT_DIM = control_dim + 6;
+ typedef Eigen::Matrix<double, IN_DIM, 1> X;
+ typedef Eigen::Matrix<double, OUT_DIM, IN_DIM> Jac;
+
+ std::vector<X, Eigen::aligned_allocator<X>> x(nTests);
+
+ std::vector<Jac, Eigen::aligned_allocator<Jac>> forward(nTests);
+ std::vector<Jac, Eigen::aligned_allocator<Jac>> reverse(nTests);
+ std::vector<Jac, Eigen::aligned_allocator<Jac>> numDiff(nTests);
+
+ HyQInverseDynJacForward forwardJac;
+ HyQInverseDynJacReverse reverseJac;
+ ct::core::DerivativesNumDiff<IN_DIM, OUT_DIM>::Function idFun = ct::models::HyQ::hyqInverseDynamics<double>;
+ ct::core::DerivativesNumDiff<IN_DIM, OUT_DIM> numDiffJac(idFun);
+
+
+ std::cout << "input dim: " << IN_DIM << ", output dim: " << OUT_DIM << std::endl;
+
+ std::cout << "running " << nTests << " tests" << std::endl;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ x[i].setRandom();
+ }
+
+ auto start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ forward[i] = forwardJac.jacobian(x[i]);
+ }
+ auto end = std::chrono::high_resolution_clock::now();
+ auto diff = end - start;
+ size_t msTotal = std::chrono::duration<double, std::micro>(diff).count();
+ std::cout << "forwardA: " << msTotal / 1000.0 << " ms. Average: " << msTotal / double(nTests) / 1000.0 << " ms"
+ << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ reverse[i] = reverseJac.jacobian(x[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count();
+ std::cout << "reverseA: " << msTotal / 1000.0 << " ms. Average: " << msTotal / double(nTests) / 1000.0 << " ms"
+ << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ numDiff[i] = numDiffJac.jacobian(x[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count();
+ std::cout << "numDiffA: " << msTotal / 1000.0 << " ms. Average: " << msTotal / double(nTests) / 1000.0 << " ms"
+ << std::endl;
+
+
+ bool failed = false;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ if (!forward[i].isApprox(numDiff[i], 1e-5))
+ {
+ std::cout << "Forward and NumDiff not similar" << std::endl;
+ std::cout << "forward: " << std::endl << forward[i] << std::endl;
+ std::cout << "numDiff: " << std::endl << numDiff[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (!forward[i].isApprox(reverse[i], 1e-12))
+ {
+ std::cout << "Forward and reverse not similar" << std::endl;
+ std::cout << "forward: " << std::endl << forward[i] << std::endl;
+ std::cout << "reverse: " << std::endl << reverse[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (failed)
+ {
+ std::cout << "test failed, aborting" << std::endl;
+ break;
+ }
+ }
+}
+
+
+int main(int argc, char* argv[])
+{
+ timing();
+ return 0;
+}
diff --git a/ct_models/src/HyQ/codegen/compareForwardReverseKin.cpp b/ct_models/src/HyQ/codegen/compareForwardReverseKin.cpp
new file mode 100644
index 0000000..d6786b0
--- /dev/null
+++ b/ct_models/src/HyQ/codegen/compareForwardReverseKin.cpp
@@ -0,0 +1,110 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <ct/models/HyQ/HyQ.h>
+#include <ct/models/HyQ/codegen/HyQForwardKinJacForward.h>
+#include <ct/models/HyQ/codegen/HyQForwardKinJacReverse.h>
+
+#include "helperFunctions.h"
+
+using namespace ct::models::HyQ;
+
+void timing()
+{
+ std::cout << "Timing forward kinematics " << std::endl;
+ static const size_t nTests = 10000;
+
+ const size_t IN_DIM = state_dim;
+ const size_t OUT_DIM = nEE * 6;
+ typedef Eigen::Matrix<double, IN_DIM, 1> X;
+ typedef Eigen::Matrix<double, OUT_DIM, IN_DIM> Jac;
+
+ std::vector<X, Eigen::aligned_allocator<X>> x(nTests);
+
+ std::vector<Jac, Eigen::aligned_allocator<Jac>> forward(nTests);
+ std::vector<Jac, Eigen::aligned_allocator<Jac>> reverse(nTests);
+ std::vector<Jac, Eigen::aligned_allocator<Jac>> numDiff(nTests);
+
+ HyQForwardKinJacForward forwardJac;
+ HyQForwardKinJacReverse reverseJac;
+ ct::core::DerivativesNumDiff<IN_DIM, OUT_DIM>::Function idFun = ct::models::HyQ::hyqForwardKinematics<double>;
+ ct::core::DerivativesNumDiff<IN_DIM, OUT_DIM> numDiffJac(idFun);
+
+ std::cout << "input dim: " << IN_DIM << ", output dim: " << OUT_DIM << std::endl;
+
+ std::cout << "running " << nTests << " tests" << std::endl;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ x[i].setRandom();
+ }
+
+ auto start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ forward[i] = forwardJac.jacobian(x[i]);
+ }
+ auto end = std::chrono::high_resolution_clock::now();
+ auto diff = end - start;
+ size_t msTotal = std::chrono::duration<double, std::micro>(diff).count();
+ std::cout << "forwardA: " << msTotal / 1000.0 << " ms. Average: " << msTotal / double(nTests) / 1000.0 << " ms"
+ << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ reverse[i] = reverseJac.jacobian(x[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count();
+ std::cout << "reverseA: " << msTotal / 1000.0 << " ms. Average: " << msTotal / double(nTests) / 1000.0 << " ms"
+ << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ numDiff[i] = numDiffJac.jacobian(x[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count();
+ std::cout << "numDiffA: " << msTotal / 1000.0 << " ms. Average: " << msTotal / double(nTests) / 1000.0 << " ms"
+ << std::endl;
+
+
+ bool failed = false;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ if (!forward[i].isApprox(numDiff[i], 1e-5))
+ {
+ std::cout << "Forward and NumDiff not similar" << std::endl;
+ std::cout << "forward: " << std::endl << forward[i] << std::endl;
+ std::cout << "numDiff: " << std::endl << numDiff[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (!forward[i].isApprox(reverse[i], 1e-12))
+ {
+ std::cout << "Forward and reverse not similar" << std::endl;
+ std::cout << "forward: " << std::endl << forward[i] << std::endl;
+ std::cout << "reverse: " << std::endl << reverse[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+ if (failed)
+ {
+ std::cout << "test failed, aborting" << std::endl;
+ break;
+ }
+ }
+}
+
+
+int main(int argc, char* argv[])
+{
+ timing();
+ return 0;
+}
diff --git a/ct_models/src/HyQ/codegen/compareForwardZero.cpp b/ct_models/src/HyQ/codegen/compareForwardZero.cpp
new file mode 100644
index 0000000..bfc7fa4
--- /dev/null
+++ b/ct_models/src/HyQ/codegen/compareForwardZero.cpp
@@ -0,0 +1,102 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <ct/models/HyQ/HyQ.h>
+#include <ct/models/HyQ/codegen/HyQForwardZero.h>
+
+using namespace ct::models::HyQ;
+
+void timing(bool useContactModel)
+{
+ std::cout << std::boolalpha << "Using contact model: " << useContactModel << std::endl;
+
+ typedef ct::rbd::FloatingBaseFDSystem<ct::rbd::HyQ::Dynamics, false> HyQSystem;
+ std::shared_ptr<HyQSystem> hyqSys = std::shared_ptr<HyQSystem>(new HyQSystem);
+
+ typedef ct::rbd::EEContactModel<typename HyQSystem::Kinematics> ContactModel;
+ std::shared_ptr<ContactModel> contactModel = std::shared_ptr<ContactModel>(new ContactModel(5000.0, 1000.0, 100.0,
+ 100.0, -0.02, ContactModel::VELOCITY_SMOOTHING::SIGMOID, hyqSys->dynamics().kinematicsPtr()));
+ if (useContactModel)
+ hyqSys->setContactModel(contactModel);
+
+ ct::models::HyQ::HyQForwardZero hyqForwardZero;
+
+ static const size_t nTests = 100000;
+
+ typedef typename HyQSystem::StateVector X;
+ typedef typename HyQSystem::ControlVector U;
+ typedef Eigen::Matrix<double, HyQSystem::STATE_DIM + HyQSystem::CONTROL_DIM + 1, 1> XUT;
+
+ typedef HyQSystem::StateVector JacA;
+
+ std::vector<X, Eigen::aligned_allocator<X>> x(nTests);
+ std::vector<U, Eigen::aligned_allocator<U>> u(nTests);
+ std::vector<double, Eigen::aligned_allocator<double>> t(nTests);
+ std::vector<XUT, Eigen::aligned_allocator<XUT>> xut(nTests);
+
+ std::vector<JacA, Eigen::aligned_allocator<JacA>> rawFd(nTests);
+ std::vector<JacA, Eigen::aligned_allocator<JacA>> forwardZeroFd(nTests);
+
+ std::cout << "running " << nTests << " tests" << std::endl;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ x[i].setRandom();
+ u[i].setRandom();
+ t[i] = i;
+
+ xut[i] << x[i], u[i], t[i];
+ }
+
+ auto start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ hyqSys->computeControlledDynamics(x[i], t[i], u[i], rawFd[i]);
+ }
+ auto end = std::chrono::high_resolution_clock::now();
+ auto diff = end - start;
+ double msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "rawFd: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ forwardZeroFd[i] = hyqForwardZero.forwardZero(xut[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::micro>(diff).count() / 1000.0;
+ std::cout << "ForwardZero: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+
+ bool failed = false;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ if (!rawFd[i].isApprox(forwardZeroFd[i], 1e-10))
+ {
+ std::cout << "Raw FD and forwardZero not identical" << std::endl;
+ std::cout << "raw FD: " << std::endl << rawFd[i] << std::endl;
+ std::cout << "forwardZeroFd A: " << std::endl << forwardZeroFd[i] << std::endl << std::endl << std::endl;
+ failed = true;
+ }
+
+ if (failed)
+ {
+ std::cout << "test failed, aborting" << std::endl;
+ break;
+ }
+ }
+}
+
+
+int main(int argc, char* argv[])
+{
+ timing(true);
+ std::cout << std::endl;
+ // timing(false);
+ return 0;
+}
diff --git a/ct_models/src/HyQ/codegen/helperFunctions.h b/ct_models/src/HyQ/codegen/helperFunctions.h
new file mode 100644
index 0000000..c4a2351
--- /dev/null
+++ b/ct_models/src/HyQ/codegen/helperFunctions.h
@@ -0,0 +1,105 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#ifndef SRC_HYQ_CODEGEN_HELPERFUNCTIONS_H_
+#define SRC_HYQ_CODEGEN_HELPERFUNCTIONS_H_
+
+#include <ct/core/core.h>
+#include <ct/rbd/rbd.h>
+#include <ct/models/HyQ/HyQ.h>
+
+namespace ct {
+namespace models {
+namespace HyQ {
+
+// a floating base forward-dynamic system templated on scalar type
+typedef ct::rbd::FloatingBaseFDSystem<ct::rbd::HyQ::tpl::Dynamics<double>, false> HyQSystem;
+
+template <typename SCALAR>
+using ContactModel = ct::rbd::EEContactModel<ct::rbd::HyQ::tpl::Kinematics<SCALAR>>;
+
+// extract dimensions
+const size_t state_dim = HyQSystem::STATE_DIM;
+const size_t control_dim = HyQSystem::CONTROL_DIM;
+
+// shortcut for number of joints
+const size_t njoints = HyQSystem::Kinematics::NJOINTS;
+const size_t nEE = HyQSystem::Kinematics::NUM_EE;
+
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, control_dim + 6, 1> hyqInverseDynamics(const Eigen::Matrix<SCALAR, state_dim + 18, 1>& x)
+{
+ ct::rbd::HyQ::tpl::Dynamics<SCALAR> hyqDynamics;
+ ct::rbd::tpl::RBDState<njoints, SCALAR> hyqState;
+
+ // we assume x contains: q, q_dot, q_ddot
+ hyqState.fromStateVectorEulerXyz(x.template topRows<state_dim>());
+
+ ct::rbd::tpl::RigidBodyAcceleration<SCALAR> base_a(x.template segment<6>(state_dim));
+ ct::rbd::tpl::JointAcceleration<njoints, SCALAR> qdd(x.template bottomRows<njoints>());
+
+ typename ct::rbd::HyQ::tpl::Dynamics<SCALAR>::ExtLinkForces_t fext(Eigen::Matrix<SCALAR, 6, 1>::Zero()); //zero
+
+ // output
+ ct::core::ControlVector<control_dim, SCALAR> u;
+ typename ct::rbd::HyQ::tpl::Dynamics<SCALAR>::ForceVector_t base_w;
+
+ hyqDynamics.FloatingBaseFullyActuatedID(hyqState, base_a, qdd, fext, base_w, u);
+
+ Eigen::Matrix<SCALAR, control_dim + 6, 1> y;
+ y << base_w, u;
+ return y;
+}
+
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, nEE * 6, 1> hyqForwardKinematics(const Eigen::Matrix<SCALAR, state_dim, 1>& x)
+{
+ ct::rbd::HyQ::tpl::Kinematics<SCALAR> hyqKinematics;
+ ct::rbd::tpl::RBDState<njoints, SCALAR> hyqState;
+ hyqState.fromStateVectorEulerXyz(x.template topRows<state_dim>());
+
+ // output vector: positions and velocities for all endeffectors
+ Eigen::Matrix<SCALAR, nEE * 6, 1> y;
+
+ for (size_t i = 0; i < nEE; i++)
+ {
+ y.template segment<3>(i * 6) =
+ hyqKinematics.getEEPositionInWorld(i, hyqState.basePose(), hyqState.jointPositions()).toImplementation();
+ y.template segment<3>(i * 6 + 3) = hyqKinematics.getEEVelocityInWorld(i, hyqState).toImplementation();
+ }
+
+ return y;
+}
+
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, state_dim, 1> hyqContactModelForwardDynamics(
+ const Eigen::Matrix<SCALAR, state_dim + control_dim + 1, 1>& x) // state, input, time
+{
+ ct::rbd::FloatingBaseFDSystem<ct::rbd::HyQ::tpl::Dynamics<SCALAR>, false> system;
+ std::shared_ptr<ContactModel<SCALAR>> contactModel =
+ std::shared_ptr<ContactModel<SCALAR>>(new ContactModel<SCALAR>());
+ contactModel->k() = SCALAR(5000);
+ contactModel->d() = SCALAR(1000);
+ contactModel->alpha() = SCALAR(100);
+ contactModel->alpha_n() = SCALAR(100);
+ contactModel->zOffset() = SCALAR(-0.02);
+ contactModel->smoothing() = static_cast<typename ContactModel<SCALAR>::VELOCITY_SMOOTHING>(
+ ContactModel<SCALAR>::VELOCITY_SMOOTHING::SIGMOID);
+
+ system.setContactModel(contactModel);
+ ct::core::StateVector<state_dim, SCALAR> y;
+
+ system.computeControlledDynamics(x.segment(0, state_dim), SCALAR(0.0), x.segment(state_dim, control_dim), y);
+
+ return y;
+}
+}
+}
+}
+
+
+#endif /* SRC_HYQ_CODEGEN_HELPERFUNCTIONS_H_ */