6 #include "pinocchio/algorithm/contact-dynamics.hpp" 18 const Eigen::MatrixXd &
J,
47 return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
70 Eigen::MatrixXd KKTMatrix_inv(
model.nv+
J.rows(),
model.nv+
J.rows());
78 static const
Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const
Model &
model,
82 Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
89 using namespace Eigen;
91 bp::def(
"forwardDynamics",
93 forwardDynamics_overloads(
94 bp::args(
"Model",
"Data",
"q",
"v",
"tau",
"J",
"gamma",
"damping"),
95 "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c." 96 " Internally, pinocchio.computeAllTerms is called." 99 bp::def(
"forwardDynamics",
101 forwardDynamics_overloads_no_q(
102 bp::args(
"Model",
"Data",
"tau",
"J",
"gamma",
"damping"),
103 "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c." 104 " Assumes pinocchio.computeAllTerms has been called." 107 bp::def(
"impulseDynamics",
109 impulseDynamics_overloads(
110 bp::args(
"Model",
"Data",
"q",
"v_before",
"J",
"r_coeff",
"damping"),
111 "Solves the impact dynamics problem with contacts, store the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c." 112 " Internally, pinocchio.crba is called." 115 bp::def(
"impulseDynamics",
117 impulseDynamics_overloads_no_q(
118 bp::args(
"Model",
"Data",
"v_before",
"J",
"r_coeff",
"damping"),
119 "Solves the impact dynamics problem with contacts, store the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c." 120 " Assumes pinocchio.crba has been called." 123 bp::def(
"computeKKTContactDynamicMatrixInverse",
125 computeKKTContactDynamicMatrixInverse_overload(bp::args(
"model",
"data",
"q",
"J",
"damping"),
126 "Computes the inverse of the constraint matrix [[M J^T], [J 0]]."));
128 bp::def(
"getKKTContactDynamicMatrixInverse",
129 getKKTContactDynamicMatrixInverse_proxy,
130 bp::args(
"Model",
"Data",
"J"),
131 "Computes the inverse of the constraint matrix [[M JT], [J 0]]. forward/impulseDynamics must be called first. The jacobian should be the same that was provided to forward/impulseDynamics.");
static const Eigen::VectorXd impulseDynamics_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v_before, const Eigen::MatrixXd &J, const double r_coeff=0., const double inv_damping=0.)
static const Eigen::VectorXd forwardDynamics_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau, const Eigen::MatrixXd &J, const Eigen::VectorXd &gamma, const double inv_damping=0.0)
void getKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv)
Computes the inverse of the KKT matrix for dynamics with contact constraints. It computes the followi...
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & impulseDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v_before, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Scalar r_coeff=0., const Scalar inv_damping=0.)
Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called...
void computeKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv, const Scalar &inv_damping=0.)
Computes the inverse of the KKT matrix for dynamics with contact constraints. It computes the followi...
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, computeKKTContactDynamicMatrixInverse_proxy, 4, 5) static const Eigen
static Eigen::MatrixXd computeKKTContactDynamicMatrixInverse_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::MatrixXd &J, const double mu=0)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
static const Eigen::VectorXd impulseDynamics_proxy_no_q(const Model &model, Data &data, const Eigen::VectorXd &v_before, const Eigen::MatrixXd &J, const double r_coeff=0., const double inv_damping=0.)
static const Eigen::VectorXd forwardDynamics_proxy_no_q(const Model &model, Data &data, const Eigen::VectorXd &tau, const Eigen::MatrixXd &J, const Eigen::VectorXd &gamma, const double inv_damping=0.0)
Main pinocchio namespace.
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & forwardDynamics(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, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< DriftVectorType > &gamma, const Scalar inv_damping=0.)
Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is call...
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
JointCollectionTpl & model