32 #include <pinocchio/algorithm/aba.hpp> 33 #include <pinocchio/algorithm/joint-configuration.hpp> 34 #include <pinocchio/algorithm/rnea.hpp> 72 if (first_or_second != ArgumentPosition::ARG0 && first_or_second != ArgumentPosition::ARG1)
74 ThrowPretty(
"Can only take derivative w.r.t. x_1 or x_2, i.e., ARG0 or ARG1. Provided: " << first_or_second);
79 if (first_or_second == ArgumentPosition::ARG0)
96 const Eigen::VectorBlock<const Eigen::VectorXd>
q = x.head(
num_positions_);
98 const Eigen::VectorBlock<const Eigen::VectorXd>
a = dx.tail(
num_velocities_);
103 case Integrator::RK1:
105 Eigen::VectorXd dx_times_dt = dt * dx;
112 case Integrator::SymplecticEuler:
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
Eigen::VectorXd u_command_
void integrate(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
StateVector StateDelta(const StateVector &x_1, const StateVector &x_2) override
Eigen::MatrixXd dStateDelta(const StateVector &x_1, const StateVector &x_2, const ArgumentPosition first_or_second) override
Eigen::VectorXd xdot_analytic_
void difference(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &v)
void Integrate(const StateVector &x, const StateVector &dx, const double dt, StateVector &xout) override
Eigen::Matrix< T, NU, 1 > ControlVector
StateVector f(const StateVector &x, const ControlVector &u) override
std::unique_ptr< pinocchio::Data > pinocchio_data_
void dDifference(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J, const ArgumentPosition arg)
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & nonLinearEffects(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
int get_num_state_derivative() const
Eigen::Matrix< T, NX, 1 > StateVector