Commit f676d4f4 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub

Merge pull request #876 from jcarpent/topic/constrained-dynamics

Topic/constrained dynamics
parents b9209ba9 45510bf3
...@@ -24,21 +24,27 @@ namespace pinocchio ...@@ -24,21 +24,27 @@ namespace pinocchio
} }
BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 9) BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 9)
// TODO: overloading impulseDynamics directly, as done for forwardDynamics, was apparently not working (it crashed for 5 arguments)
// Therefore, it was necessary to resort to a proxy
static const Eigen::VectorXd impulseDynamics_proxy(const Model & model, static const Eigen::VectorXd impulseDynamics_proxy(const Model & model,
Data & data, Data & data,
const Eigen::VectorXd & q, const Eigen::VectorXd & q,
const Eigen::VectorXd & v_before, const Eigen::VectorXd & v_before,
const Eigen::MatrixXd & J, const Eigen::MatrixXd & J,
const double r_coeff = 0.0, const double r_coeff,
const bool updateKinematics = true) const double inv_damping)
{ {
return impulseDynamics(model, data, q, v_before, J, r_coeff, updateKinematics); return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
}
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,
const double inv_damping)
{
return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
} }
BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads, impulseDynamics_proxy, 5, 7)
static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model, static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model,
Data & data, Data & data,
...@@ -70,15 +76,25 @@ namespace pinocchio ...@@ -70,15 +76,25 @@ namespace pinocchio
bp::def("impulseDynamics", bp::def("impulseDynamics",
&impulseDynamics_proxy, &impulseDynamics_proxy,
impulseDynamics_overloads(
bp::args("Model","Data", bp::args("Model","Data",
"Joint configuration q (size Model::nq)", "Joint configuration q (size Model::nq)",
"Joint velocity before impact v_before (size Model::nv)", "Joint velocity before impact v_before (size Model::nv)",
"Contact Jacobian J (size nb_constraint * Model::nv)", "Contact Jacobian J (size nb_constraint * Model::nv)",
"Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)", "Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)",
"Update kinematics (if true, it updates only the joint space inertia matrix)"), "Damping factor when J is rank deficient."
"Solve the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c" ),
)); "Solves the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c"
);
bp::def("impulseDynamics",
&impulseDynamics_proxy_no_q,
bp::args("Model","Data",
"Joint velocity before impact v_before (size Model::nv)",
"Contact Jacobian J (size nb_constraint * Model::nv)",
"Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)",
"Damping factor when J is rank deficient."),
"Solves the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c"
);
bp::def("getKKTContactDynamicMatrixInverse",getKKTContactDynamicMatrixInverse_proxy, bp::def("getKKTContactDynamicMatrixInverse",getKKTContactDynamicMatrixInverse_proxy,
bp::args("Model","Data", bp::args("Model","Data",
......
# #
# Copyright (c) 2018 CNRS INRIA # Copyright (c) 2018-2019 CNRS INRIA
# #
## In this file, are reported some deprecated functions that are still maintained until the next important future releases ## ## In this file, are reported some deprecated functions that are still maintained until the next important future releases ##
...@@ -23,7 +23,7 @@ StdVect_Force = deprecated("Please use StdVec_Force")(pin.StdVec_Force) ...@@ -23,7 +23,7 @@ StdVect_Force = deprecated("Please use StdVec_Force")(pin.StdVec_Force)
StdVect_Motion = deprecated("Please use StdVec_Motion")(pin.StdVec_Motion) StdVect_Motion = deprecated("Please use StdVec_Motion")(pin.StdVec_Motion)
@deprecated("This function has been renamed to impulseDynamics for consistency with the C++ interface. Please change for impulseDynamics.") @deprecated("This function has been renamed to impulseDynamics for consistency with the C++ interface. Please change for impulseDynamics.")
def impactDynamics(model, data, q, v_before, J, r_coeff=0.0, update_kinematics=True): def impulseDynamics(model, data, q, v_before, J, r_coeff=0.0, update_kinematics=True):
return pin.impulseDynamics(model, data, q, v_before, J, r_coeff, update_kinematics) return pin.impulseDynamics(model, data, q, v_before, J, r_coeff, update_kinematics)
@deprecated("This function has been deprecated. Please use buildSampleModelHumanoid or buildSampleModelHumanoidRandom instead.") @deprecated("This function has been deprecated. Please use buildSampleModelHumanoid or buildSampleModelHumanoidRandom instead.")
...@@ -195,3 +195,49 @@ def log6FromSE3(transform): ...@@ -195,3 +195,49 @@ def log6FromSE3(transform):
@deprecated("This function will be removed in future releases of Pinocchio. You can use log or log6.") @deprecated("This function will be removed in future releases of Pinocchio. You can use log or log6.")
def log6FromMatrix(matrix4): def log6FromMatrix(matrix4):
return pin.log6(matrix4) return pin.log6(matrix4)
@deprecated("This function has been renamed getJointJacobian and will be removed in future releases of Pinocchio. Please change for new getJointJacobian function.")
def getJacobian(model,data,jointId,local):
if local:
return pin.getJointJacobian(model,data,jointId,pin.ReferenceFrame.LOCAL)
else:
return pin.getJointJacobian(model,data,jointId,pin.ReferenceFrame.WORLD)
# This function is only deprecated when using a specific signature. Therefore, it needs special care
# Marked as deprecated on 16 Sept 2019
def impulseDynamics(model, data, q = None, *args):
v_before = args[0]
J = args[1]
if q is None:
r_coeff = args[2]
inv_damping = args[3]
return pin.impulseDynamics(model,data,v_before,J,r_coeff,inv_damping)
elif len(args)==4:
if type(args[3]) is bool:
message = ("This function signature has been deprecated and will be removed in future releases of Pinocchio. "
"Please change for the new signature of impulseDynamics(model,data,[q],v_before,J,r_coeff,inv_damping).")
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
inv_damping = 0.
r_coeff = args[2]
if(args[3]):
return pin.impulseDynamics(model,data,q,v_before,J,r_coeff,inv_damping)
else:
return pin.impulseDynamics(model,data,v_before,J,r_coeff,inv_damping)
else:
r_coeff = args[2]
inv_damping = args[3]
return pin.impulseDynamics(model,data,q,v_before,J,r_coeff,inv_damping)
else:
r_coeff = 0.
inv_damping = 0.
if args:
if len(args) >= 3:
r_coeff = args[2]
return pin.impulseDynamics(model,data,q,v_before,J,r_coeff,inv_damping)
impulseDynamics.__doc__ = (
pin.impulseDynamics.__doc__
+ '\n\nimpulseDynamics( (Model)Model, (Data)Data, (object)Joint configuration q (size Model::nq), (object)Joint velocity before impact v_before (size Model::nv), (object)Contact Jacobian J (size nb_constraint * Model::nv), (float)Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact), (bool)updateKinematics) -> object :'
+ '\n This function signature has been deprecated and will be removed in future releases of Pinocchio.'
)
...@@ -35,7 +35,7 @@ namespace pinocchio ...@@ -35,7 +35,7 @@ namespace pinocchio
/// \param[in] tau The joint torque vector (dim model.nv). /// \param[in] tau The joint torque vector (dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv). /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] gamma The drift of the constraints (dim nb_constraints). /// \param[in] gamma The drift of the constraints (dim nb_constraints).
/// \param[in] inv_damping Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank. /// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \param[in] updateKinematics If true, the algorithm calls first pinocchio::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. \\ /// \param[in] updateKinematics If true, the algorithm calls first pinocchio::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. \\
/// \note A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet. /// \note A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet.
/// ///
...@@ -72,6 +72,70 @@ namespace pinocchio ...@@ -72,6 +72,70 @@ namespace pinocchio
const Eigen::MatrixBase<ConstraintMatrixType> & J, const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv); const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv);
///
/// \brief Compute the impulse dynamics with contact constraints.
/// \note It computes the following problem: <BR>
/// <CENTER> \f$ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\
/// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$ </CENTER> <BR>
/// where \f$ \dot{q}^{-} \f$ is the generalized velocity before impact,
/// \f$ M \f$ is the joint space mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact).
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
/// \tparam TangentVectorType Type of the joint velocity vector.
/// \tparam ConstraintMatrixType Type of the constraint matrix.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration (vector dim model.nq).
/// \param[in] v_before The joint velocity before impact (vector dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] r_coeff The coefficient of restitution. Must be in [0;1].
/// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
///
/// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
inline const typename 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.);
///
/// \brief Compute the impulse dynamics with contact constraints.
/// \note It computes the following problem: <BR>
/// <CENTER> \f$ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\
/// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$ </CENTER> <BR>
/// where \f$ \dot{q}^{-} \f$ is the generalized velocity before impact,
/// \f$ M \f$ is the joint space mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact).
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
/// \tparam TangentVectorType Type of the joint velocity vector.
/// \tparam ConstraintMatrixType Type of the constraint matrix.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] v_before The joint velocity before impact (vector dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] r_coeff The coefficient of restitution. Must be in [0;1].
/// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
///
/// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar r_coeff = 0.,
const Scalar inv_damping = 0.);
/// ///
/// \brief Compute the impulse dynamics with contact constraints. /// \brief Compute the impulse dynamics with contact constraints.
/// \note It computes the following problem: <BR> /// \note It computes the following problem: <BR>
...@@ -96,14 +160,15 @@ namespace pinocchio ...@@ -96,14 +160,15 @@ namespace pinocchio
/// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector. /// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.
/// ///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType> template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
PINOCCHIO_DEPRECATED
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType & inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data, DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q, const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v_before, const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J, const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar r_coeff = 0, const Scalar r_coeff,
const bool updateKinematics = true); const bool updateKinematics);
} // namespace pinocchio } // namespace pinocchio
......
...@@ -110,9 +110,25 @@ namespace pinocchio ...@@ -110,9 +110,25 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v_before, const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J, const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar r_coeff, const Scalar r_coeff,
const bool updateKinematics) const Scalar inv_damping)
{ {
assert(q.size() == model.nq); assert(q.size() == model.nq);
// Compute the mass matrix
crba(model, data, q);
return impulseDynamics(model,data,v_before,J,r_coeff,inv_damping);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType, typename ConstraintMatrixType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar r_coeff,
const Scalar inv_damping)
{
assert(v_before.size() == model.nv); assert(v_before.size() == model.nv);
assert(J.cols() == model.nv); assert(J.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
...@@ -122,10 +138,6 @@ namespace pinocchio ...@@ -122,10 +138,6 @@ namespace pinocchio
typename Data::VectorXs & impulse_c = data.impulse_c; typename Data::VectorXs & impulse_c = data.impulse_c;
typename Data::TangentVectorType & dq_after = data.dq_after; typename Data::TangentVectorType & dq_after = data.dq_after;
// Compute the mass matrix
if (updateKinematics)
crba(model, data, q);
// Compute the UDUt decomposition of data.M // Compute the UDUt decomposition of data.M
cholesky::decompose(model, data); cholesky::decompose(model, data);
...@@ -135,6 +147,8 @@ namespace pinocchio ...@@ -135,6 +147,8 @@ namespace pinocchio
for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]); for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);
data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt; data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
data.JMinvJt.diagonal().array() += inv_damping;
data.llt_JMinvJt.compute(data.JMinvJt); data.llt_JMinvJt.compute(data.JMinvJt);
// Compute the Lagrange Multipliers related to the contact impulses // Compute the Lagrange Multipliers related to the contact impulses
...@@ -148,6 +162,23 @@ namespace pinocchio ...@@ -148,6 +162,23 @@ namespace pinocchio
return dq_after; return dq_after;
} }
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
PINOCCHIO_DEPRECATED
inline const typename 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,
const bool updateKinematics)
{
if(updateKinematics)
return impulseDynamics(model,data,q,v_before,J,r_coeff,Scalar(0));
else
return impulseDynamics(model,data,v_before,J,r_coeff,Scalar(0));
}
} // namespace pinocchio } // namespace pinocchio
#endif // ifndef __pinocchio_contact_dynamics_hxx__ #endif // ifndef __pinocchio_contact_dynamics_hxx__
...@@ -133,7 +133,7 @@ BOOST_AUTO_TEST_CASE (test_KKTMatrix) ...@@ -133,7 +133,7 @@ BOOST_AUTO_TEST_CASE (test_KKTMatrix)
//Check Impulse Dynamics //Check Impulse Dynamics
const double r_coeff = 1.; const double r_coeff = 1.;
VectorXd v_before = VectorXd::Ones(model.nv); VectorXd v_before = VectorXd::Ones(model.nv);
pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, true); pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
MJtJ << data.M, J.transpose(), MJtJ << data.M, J.transpose(),
J, Eigen::MatrixXd::Zero(12, 12); J, Eigen::MatrixXd::Zero(12, 12);
...@@ -234,7 +234,7 @@ BOOST_AUTO_TEST_CASE ( test_ID ) ...@@ -234,7 +234,7 @@ BOOST_AUTO_TEST_CASE ( test_ID )
Eigen::MatrixXd H(J.transpose()); Eigen::MatrixXd H(J.transpose());
pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, true); pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
MatrixXd Minv (data.M.inverse()); MatrixXd Minv (data.M.inverse());
......
...@@ -61,23 +61,26 @@ class TestDynamicsBindings(TestCase): ...@@ -61,23 +61,26 @@ class TestDynamicsBindings(TestCase):
self.assertLess(np.linalg.norm(vnext), self.tolerance) self.assertLess(np.linalg.norm(vnext), self.tolerance)
def test_impulseDynamics6(self): def test_impulseDynamics6(self):
vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,inv_damping) vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,r_coeff)
self.assertLess(np.linalg.norm(vnext), self.tolerance) self.assertLess(np.linalg.norm(vnext), self.tolerance)
def test_impulseDynamics7(self): def test_impulseDynamics7(self):
vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,inv_damping,update_kinematics) vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,r_coeff,inv_damping)
self.assertLess(np.linalg.norm(vnext), self.tolerance) self.assertLess(np.linalg.norm(vnext), self.tolerance)
def test_impulseDynamics567(self): def test_impulseDynamics567(self):
data5 = self.data data5 = self.data
data6 = self.model.createData() data6 = self.model.createData()
data7 = self.model.createData() data7 = self.model.createData()
data8 = self.model.createData()
vnext5 = pin.impulseDynamics(self.model,data5,self.q,self.v,self.J) vnext5 = pin.impulseDynamics(self.model,data5,self.q,self.v,self.J)
vnext6 = pin.impulseDynamics(self.model,data6,self.q,self.v,self.J,inv_damping) vnext6 = pin.impulseDynamics(self.model,data6,self.q,self.v,self.J,r_coeff)
vnext7 = pin.impulseDynamics(self.model,data7,self.q,self.v,self.J,inv_damping,update_kinematics) vnext7 = pin.impulseDynamics(self.model,data7,self.q,self.v,self.J,r_coeff,inv_damping)
vnext7_previous = pin.impulseDynamics(self.model,data8,self.q,self.v,self.J,r_coeff,True)
self.assertTrue((vnext5==vnext6).all()) self.assertTrue((vnext5==vnext6).all())
self.assertTrue((vnext5==vnext7).all()) self.assertTrue((vnext5==vnext7).all())
self.assertTrue((vnext6==vnext7).all()) self.assertTrue((vnext6==vnext7).all())
self.assertTrue((vnext7_previous==vnext7).all())
if __name__ == '__main__': if __name__ == '__main__':
unittest.main() unittest.main()
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment