Go to the documentation of this file.
18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
21 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
24 using namespace Eigen;
32 model.lowerPositionLimit.head<3>().
fill(-1.);
33 model.upperPositionLimit.head<3>().
fill(1.);
36 const VectorXd
v = VectorXd::Random(
model.nv);
37 const VectorXd
tau = VectorXd::Random(
model.nv);
39 const std::string RF =
"rleg6_joint";
40 const std::string LF =
"lleg6_joint";
48 ci_RF_LF.joint1_placement.setRandom();
49 ci_RF_LF.joint2_placement.setRandom();
57 const double mu0 = 0.;
64 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
65 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
71 model, data_ref,
model.getJointId(RF), ci_RF_LF.joint1_placement,
LOCAL, J_RF_local);
73 model, data_ref,
model.getJointId(LF), ci_RF_LF.joint2_placement,
LOCAL, J_LF_local);
75 const SE3 oMc1_ref = data_ref.oMi[ci_RF_LF.joint1_id] * ci_RF_LF.joint1_placement;
76 const SE3 oMc2_ref = data_ref.oMi[ci_RF_LF.joint2_id] * ci_RF_LF.joint2_placement;
77 const SE3 c1Mc2_ref = oMc1_ref.actInv(oMc2_ref);
79 J_ref = J_RF_local - c1Mc2_ref.toActionMatrix() * J_LF_local;
83 const Motion vc1_ref = ci_RF_LF.joint1_placement.actInv(data_ref.v[ci_RF_LF.joint1_id]);
84 const Motion vc2_ref = ci_RF_LF.joint2_placement.actInv(data_ref.v[ci_RF_LF.joint2_id]);
85 const Motion constraint_velocity_error_ref = vc1_ref - c1Mc2_ref.act(vc2_ref);
86 BOOST_CHECK(constraint_velocity_error_ref.isApprox(
Motion(J_ref *
v)));
88 const Motion ac1_ref = ci_RF_LF.joint1_placement.actInv(data_ref.a[ci_RF_LF.joint1_id]);
89 const Motion ac2_ref = ci_RF_LF.joint2_placement.actInv(data_ref.a[ci_RF_LF.joint2_id]);
90 const Motion constraint_acceleration_error_ref =
91 ac1_ref - c1Mc2_ref.act(ac2_ref) + constraint_velocity_error_ref.cross(c1Mc2_ref.act(vc2_ref));
92 rhs_ref.segment<6>(0) = constraint_acceleration_error_ref.toVector();
94 Eigen::MatrixXd KKT_matrix_ref =
96 KKT_matrix_ref.bottomRightCorner(
model.nv,
model.nv) = data_ref.
M;
107 BOOST_CHECK((J_ref * data_ref.
ddq + rhs_ref).isZero());
112 BOOST_CHECK((J_ref *
data.ddq + rhs_ref).isZero());
114 BOOST_CHECK(data_ref.
ddq.isApprox(
data.ddq));
116 const Eigen::MatrixXd KKT_matrix =
data.contact_chol.matrix();
117 BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
121 const double dt = 1e-8;
126 const SE3 oMc1_plus_ref = data_plus.oMi[ci_RF_LF.joint1_id] * ci_RF_LF.joint1_placement;
127 const SE3 oMc2_plus_ref = data_plus.oMi[ci_RF_LF.joint2_id] * ci_RF_LF.joint2_placement;
128 const SE3 c1Mc2_plus_ref = oMc1_plus_ref.actInv(oMc2_plus_ref);
134 BOOST_CHECK(
contact_datas[0].contact1_velocity.isApprox(vc1_ref));
135 BOOST_CHECK(
contact_datas[0].contact2_velocity.isApprox(vc2_ref));
137 const Motion constraint_velocity_error_fd =
138 -c1Mc2_ref.act(
log6(c1Mc2_ref.actInv(c1Mc2_plus_ref))) /
dt;
139 BOOST_CHECK(constraint_velocity_error_ref.isApprox(constraint_velocity_error_fd, math::sqrt(
dt)));
140 BOOST_CHECK(
contact_datas[0].contact_velocity_error.isApprox(constraint_velocity_error_ref));
143 const Motion vc1_plus_ref = ci_RF_LF.joint1_placement.actInv(data_plus.v[ci_RF_LF.joint1_id]);
144 const Motion vc2_plus_ref = ci_RF_LF.joint2_placement.actInv(data_plus.v[ci_RF_LF.joint2_id]);
145 const Motion constraint_velocity_error_plus_ref = vc1_plus_ref - c1Mc2_plus_ref.act(vc2_plus_ref);
146 const Motion constraint_acceleration_error_fd =
147 (constraint_velocity_error_plus_ref - constraint_velocity_error_ref) /
dt;
149 constraint_acceleration_error_ref.isApprox(constraint_acceleration_error_fd, math::sqrt(
dt)));
152 BOOST_AUTO_TEST_SUITE_END()
RigidConstraintDataTpl< context::Scalar, context::Options > RigidConstraintData
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
TangentVectorType ddq
The joint accelerations computed from ABA.
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
void getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame,...
#define PINOCCHIO_COMPILER_DIAGNOSTIC_POP
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
@ CONTACT_6D
Point contact model.
Structure containing all the settings parameters for the proximal algorithms.
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
BOOST_AUTO_TEST_CASE(closed_loop_constraint_6D_LOCAL)
const PINOCCHIO_DEPRECATED 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...
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
#define PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & constraintDynamics(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 std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
Computes the forward dynamics with contact constraints according to a given list of contact informati...
JointCollectionTpl & model
Main pinocchio namespace.
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:06