Go to the documentation of this file.
17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
20 template<
typename Derived>
21 inline bool isFinite(
const Eigen::MatrixBase<Derived> & x)
23 return ((
x -
x).array() == (
x -
x).array()).all();
26 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
30 using namespace Eigen;
37 VectorXd
q = VectorXd::Zero(
model.nq);
47 VectorXd qdot = VectorXd::Random(
model.nv);
48 VectorXd qddot = VectorXd::Zero(
model.nv);
51 BOOST_CHECK(
v.toVector().isApprox(Jrh * qdot, 1e-12));
59 BOOST_CHECK(XJrh.isApprox(rhJrh, 1e-12));
64 BOOST_CHECK(XJrh.isApprox(rhJrh, 1e-12));
71 BOOST_CHECK(data_fk.
J.isApprox(
data.J));
76 using namespace Eigen;
85 model, -1 * Eigen::VectorXd::Ones(
model.nq), Eigen::VectorXd::Ones(
model.nq));
86 VectorXd
v = VectorXd::Random(
model.nv);
87 VectorXd
a = VectorXd::Random(
model.nv);
108 BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx])));
111 const Motion & a_ref = data_ref.oMi[idx].act(data_ref.a[idx]);
112 BOOST_CHECK(a_idx.isApprox(a_ref));
119 v_idx = (Motion::Vector6)(
J *
v);
120 BOOST_CHECK(v_idx.isApprox(data_ref.v[idx]));
122 a_idx = (Motion::Vector6)(
J *
a + dJ *
v);
123 BOOST_CHECK(a_idx.isApprox(data_ref.a[idx]));
133 world_v_local.linear().setZero();
135 v_idx = (Motion::Vector6)(
J *
v);
136 BOOST_CHECK(v_idx.isApprox(worldMlocal.act(data_ref.v[idx])));
138 a_idx = (Motion::Vector6)(
J *
a + dJ *
v);
139 BOOST_CHECK(a_idx.isApprox(
140 world_v_local.cross(worldMlocal.act(data_ref.v[idx])) + worldMlocal.act(data_ref.a[idx])));
146 const double alpha = 1e-8;
147 Eigen::VectorXd q_plus(
model.nq);
162 dJ_ref = (J_ref_plus - J_ref) /
alpha;
169 BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(
alpha)));
176 const double alpha = 1e-8;
177 Eigen::VectorXd q_plus(
model.nq);
191 dJ_ref = (J_ref_plus - J_ref) /
alpha;
198 BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(
alpha)));
205 const double alpha = 1e-8;
206 Eigen::VectorXd q_plus(
model.nq);
220 dJ_ref = (J_ref_plus - J_ref) /
alpha;
227 BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(
alpha)));
233 using namespace Eigen;
240 long flag = BOOST_BINARY(1111);
243 #ifdef _INTENSE_TESTING_
244 const size_t NBT = 1000 * 1000;
246 const size_t NBT = 10;
249 const size_t NBT = 1;
250 std::cout <<
"(the time score in debug mode is not relevant) ";
253 bool verbose = flag & (flag - 1);
255 std::cout <<
"--" << std::endl;
256 Eigen::VectorXd
q = Eigen::VectorXd::Zero(
model.nq);
266 std::cout <<
"Compute =\t";
267 timer.
toc(std::cout, NBT);
284 std::cout <<
"Copy =\t";
285 timer.
toc(std::cout, NBT);
302 std::cout <<
"Change frame =\t";
303 timer.
toc(std::cout, NBT);
320 std::cout <<
"Single jacobian =\t";
321 timer.
toc(std::cout, NBT);
325 BOOST_AUTO_TEST_SUITE_END()
void getJointJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &dJ)
Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (r...
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 humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobiansTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depen...
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
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.
PINOCCHIO_COMPILER_DIAGNOSTIC_POP typedef std::size_t Index
ConstLinearRef translation() const
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.
bool isFinite(const Eigen::MatrixBase< Derived > &x)
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
static void se3Action(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
SE3 action on a set of forces, represented by a 6xN matrix whose each column represent a spatial forc...
BOOST_AUTO_TEST_CASE(test_jacobian)
Matrix6x J
Jacobian of joint placements.
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(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 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
void computeJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex joint_id, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store t...
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Main pinocchio namespace.
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:10