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/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_ */