Commit 3034ebf9 authored by Justin Carpentier's avatar Justin Carpentier

algo/contact-dyn: adjust signatures of impulseDynamics

parent fa40d7b1
......@@ -30,20 +30,20 @@ namespace pinocchio
const Eigen::VectorXd & q,
const Eigen::VectorXd & v_before,
const Eigen::MatrixXd & J,
const double inv_damping,
const double r_coeff)
const double r_coeff,
const double inv_damping)
{
return impulseDynamics(model, data, q, v_before, J, inv_damping, r_coeff);
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 inv_damping,
const double r_coeff)
const double r_coeff,
const double inv_damping)
{
return impulseDynamics(model, data, v_before, J, inv_damping, r_coeff);
return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
}
static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model,
......@@ -80,8 +80,9 @@ namespace pinocchio
"Joint configuration q (size Model::nq)",
"Joint velocity before impact v_before (size Model::nv)",
"Contact Jacobian J (size nb_constraint * Model::nv)",
"Damping factor when J is rank deficient.",
"Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)"),
"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"
);
......@@ -90,8 +91,8 @@ namespace pinocchio
bp::args("Model","Data",
"Joint velocity before impact v_before (size Model::nv)",
"Contact Jacobian J (size nb_constraint * Model::nv)",
"Damping factor when J is rank deficient.",
"Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)"),
"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"
);
......
......@@ -23,7 +23,7 @@ StdVect_Force = deprecated("Please use StdVec_Force")(pin.StdVec_Force)
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.")
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)
@deprecated("This function has been deprecated. Please use buildSampleModelHumanoid or buildSampleModelHumanoidRandom instead.")
......@@ -205,32 +205,32 @@ def getJacobian(model,data,jointId,local):
# This function is only deprecated when using a specific signature. Therefore, it needs special care
# Marked as deprecated on 16 Sept 2019
def impactDynamics(model, data, q = None, *args):
def impulseDynamics(model, data, q = None, *args):
v_before = args[0]
J = args[1]
if q is None:
inv_damping = args[2]
r_coeff = args[3]
return pin.impactDynamics(model,data,v_before,J,inv_damping,r_coeff)
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 impactDynamics(model,data,[q],v_before,J,inv_damping,r_coeff).")
"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.impactDynamics(model,data,q,v_before,J,inv_damping,r_coeff)
return pin.impulseDynamics(model,data,q,v_before,J,r_coeff,inv_damping)
else:
return pin.impactDynamics(model,data,v_before,J,inv_damping,r_coeff)
return pin.impulseDynamics(model,data,v_before,J,r_coeff,inv_damping)
else:
inv_damping = args[2]
r_coeff = args[3]
return pin.impactDynamics(model,data,q,v_before,J,inv_damping,r_coeff)
r_coeff = args[2]
inv_damping = args[3]
return pin.impulseDynamics(model,data,q,v_before,J,r_coeff,inv_damping)
impactDynamics.__doc__ = (
pin.impactDynamics.__doc__
+ '\n\nimpactDynamics( (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 :'
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.'
)
......@@ -90,8 +90,8 @@ namespace pinocchio
/// \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] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \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.
///
......@@ -102,9 +102,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar inv_damping,
const Scalar r_coeff);
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>
......@@ -122,8 +122,8 @@ namespace pinocchio
/// \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] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \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.
///
......@@ -133,8 +133,8 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar inv_damping,
const Scalar r_coeff);
const Scalar r_coeff = 0.,
const Scalar inv_damping = 0.);
///
/// \brief Compute the impulse dynamics with contact constraints.
......@@ -167,8 +167,8 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar r_coeff = 0,
const bool updateKinematics = true);
const Scalar r_coeff,
const bool updateKinematics);
} // namespace pinocchio
......
......@@ -109,15 +109,15 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar inv_damping,
const Scalar r_coeff)
const Scalar r_coeff,
const Scalar inv_damping)
{
assert(q.size() == model.nq);
// Compute the mass matrix
crba(model, data, q);
return impulseDynamics(model,data,v_before,J,inv_damping,r_coeff);
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>
......@@ -126,8 +126,8 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar inv_damping,
const Scalar r_coeff)
const Scalar r_coeff,
const Scalar inv_damping)
{
assert(v_before.size() == model.nv);
assert(J.cols() == model.nv);
......@@ -175,9 +175,9 @@ namespace pinocchio
const bool updateKinematics)
{
if(updateKinematics)
return impulseDynamics(model,data,q,v_before,J,Scalar(0),r_coeff);
return impulseDynamics(model,data,q,v_before,J,r_coeff,Scalar(0));
else
return impulseDynamics(model,data,v_before,J,Scalar(0),r_coeff);
return impulseDynamics(model,data,v_before,J,r_coeff,Scalar(0));
}
} // namespace pinocchio
......
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