joint-universal.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2023 INRIA
3 //
4 
5 #include "pinocchio/math/fwd.hpp"
6 #include "pinocchio/multibody/joint/joints.hpp"
7 #include "pinocchio/algorithm/rnea.hpp"
8 #include "pinocchio/algorithm/aba.hpp"
9 #include "pinocchio/algorithm/crba.hpp"
10 #include "pinocchio/algorithm/jacobian.hpp"
11 #include "pinocchio/algorithm/compute-all-terms.hpp"
12 
13 #include <boost/test/unit_test.hpp>
14 #include <iostream>
15 
16 using namespace pinocchio;
17 using namespace Eigen;
18 
19 template<typename D>
20 void addJointAndBody(
21  Model & model,
22  const JointModelBase<D> & jmodel,
23  const Model::JointIndex parent_id,
24  const SE3 & joint_placement,
25  const std::string & joint_name,
26  const Inertia & Y)
27 {
28  Model::JointIndex idx;
29 
30  idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
31  model.appendBodyToJoint(idx, Y);
32 }
33 
34 BOOST_AUTO_TEST_SUITE(JointUniversal)
35 
36 BOOST_AUTO_TEST_CASE(vsRXRY)
37 {
38  typedef SE3::Vector3 Vector3;
39  typedef SE3::Matrix3 Matrix3;
40 
41  Vector3 axis1;
42  axis1 << 1.0, 0.0, 0.0;
43  Vector3 axis2;
44  axis2 << 0.0, 1.0, 0.0;
45 
46  Model modelUniversal, modelRXRY;
47  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
48 
49  JointModelUniversal joint_model_U(axis1, axis2);
50  addJointAndBody(modelUniversal, joint_model_U, 0, SE3::Identity(), "universal", inertia);
51 
52  JointModelComposite joint_model_RXRY;
53  joint_model_RXRY.addJoint(JointModelRX());
54  joint_model_RXRY.addJoint(JointModelRY());
55  addJointAndBody(modelRXRY, joint_model_RXRY, 0, SE3::Identity(), "rxry", inertia);
56 
57  Data dataUniversal(modelUniversal);
58  Data dataRXRY(modelRXRY);
59 
60  BOOST_CHECK(modelUniversal.nv == modelRXRY.nv);
61  BOOST_CHECK(modelUniversal.nq == modelRXRY.nq);
62 
63  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelRXRY.nq);
64 
65  forwardKinematics(modelRXRY, dataRXRY, q);
66  forwardKinematics(modelUniversal, dataUniversal, q);
67 
68  BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRXRY.oMi.back()));
69  BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRXRY.liMi.back()));
70  BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRXRY.Ycrb.back().matrix()));
71 
72  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelRXRY.nv);
73  forwardKinematics(modelRXRY, dataRXRY, q, v);
74  forwardKinematics(modelUniversal, dataUniversal, q, v);
75 
76  BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRXRY.oMi.back()));
77  BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRXRY.liMi.back()));
78  BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRXRY.Ycrb.back().matrix()));
79 
80  computeAllTerms(modelRXRY, dataRXRY, q, v);
81  computeAllTerms(modelUniversal, dataUniversal, q, v);
82 
83  BOOST_CHECK(dataUniversal.com.back().isApprox(dataRXRY.com.back()));
84  BOOST_CHECK(dataUniversal.nle.isApprox(dataRXRY.nle));
85  BOOST_CHECK(dataUniversal.f.back().toVector().isApprox(dataRXRY.f.back().toVector()));
86 
87  // InverseDynamics == rnea
88  Eigen::VectorXd a = Eigen::VectorXd::Ones(modelRXRY.nv);
89 
90  Eigen::VectorXd tauRXRY = rnea(modelRXRY, dataRXRY, q, v, a);
91  Eigen::VectorXd tauUniversal = rnea(modelUniversal, dataUniversal, q, v, a);
92 
93  BOOST_CHECK(tauUniversal.isApprox(tauRXRY));
94 
95  // ForwardDynamics == aba
96  Eigen::VectorXd aAbaRXRY = aba(modelRXRY, dataRXRY, q, v, tauRXRY, Convention::WORLD);
97  Eigen::VectorXd aAbaUniversal =
98  aba(modelUniversal, dataUniversal, q, v, tauUniversal, Convention::WORLD);
99 
100  BOOST_CHECK(aAbaUniversal.isApprox(aAbaRXRY));
101 
102  // CRBA
103  crba(modelRXRY, dataRXRY, q, Convention::WORLD);
104  crba(modelUniversal, dataUniversal, q, Convention::WORLD);
105 
106  BOOST_CHECK(dataUniversal.M.isApprox(dataRXRY.M));
107 
108  // Jacobian
109  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRXRY;
110  jacobianRXRY.resize(6, 2);
111  jacobianRXRY.setZero();
112  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianUniversal;
113  jacobianUniversal.resize(6, 2);
114  jacobianUniversal.setZero();
115 
116  computeJointJacobians(modelRXRY, dataRXRY, q);
117  computeJointJacobians(modelUniversal, dataUniversal, q);
118  getJointJacobian(modelRXRY, dataRXRY, 1, LOCAL, jacobianRXRY);
119  getJointJacobian(modelUniversal, dataUniversal, 1, LOCAL, jacobianUniversal);
120 
121  BOOST_CHECK(jacobianUniversal.isApprox(jacobianRXRY));
122 }
123 
124 BOOST_AUTO_TEST_CASE(vsRandomAxis)
125 {
126  typedef SE3::Vector3 Vector3;
127  typedef SE3::Matrix3 Matrix3;
128 
129  Vector3 axis1;
130  axis1 << 0., 0., 1.;
131  Vector3 axis2;
132  axis2 << -1., 0., 0.;
133 
134  Model modelUniversal, modelRandomAxis;
135  Inertia inertia = Inertia::Random();
136 
137  JointModelUniversal joint_model_U(axis1, axis2);
138  addJointAndBody(modelUniversal, joint_model_U, 0, SE3::Identity(), "universal", inertia);
139 
140  JointModelComposite joint_model_RandomAxis;
141  joint_model_RandomAxis.addJoint(JointModelRevoluteUnaligned(axis1));
142  joint_model_RandomAxis.addJoint(JointModelRevoluteUnaligned(axis2));
143  addJointAndBody(
144  modelRandomAxis, joint_model_RandomAxis, 0, SE3::Identity(), "random_axis", inertia);
145 
146  Data dataUniversal(modelUniversal);
147  Data dataRandomAxis(modelRandomAxis);
148 
149  BOOST_CHECK(modelUniversal.nv == modelRandomAxis.nv);
150  BOOST_CHECK(modelUniversal.nq == modelRandomAxis.nq);
151 
152  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelRandomAxis.nq);
153 
154  forwardKinematics(modelRandomAxis, dataRandomAxis, q);
155  forwardKinematics(modelUniversal, dataUniversal, q);
156 
157  BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRandomAxis.oMi.back()));
158  BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRandomAxis.liMi.back()));
159  BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRandomAxis.Ycrb.back().matrix()));
160 
161  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelRandomAxis.nv);
162  forwardKinematics(modelRandomAxis, dataRandomAxis, q, v);
163  forwardKinematics(modelUniversal, dataUniversal, q, v);
164 
165  BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRandomAxis.oMi.back()));
166  BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRandomAxis.liMi.back()));
167  BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRandomAxis.Ycrb.back().matrix()));
168 
169  computeAllTerms(modelRandomAxis, dataRandomAxis, q, v);
170  computeAllTerms(modelUniversal, dataUniversal, q, v);
171 
172  BOOST_CHECK(dataUniversal.com.back().isApprox(dataRandomAxis.com.back()));
173  BOOST_CHECK(dataUniversal.nle.isApprox(dataRandomAxis.nle));
174  BOOST_CHECK(dataUniversal.f.back().toVector().isApprox(dataRandomAxis.f.back().toVector()));
175 
176  // InverseDynamics == rnea
177  Eigen::VectorXd a = Eigen::VectorXd::Ones(modelRandomAxis.nv);
178 
179  Eigen::VectorXd tauRandomAxis = rnea(modelRandomAxis, dataRandomAxis, q, v, a);
180  Eigen::VectorXd tauUniversal = rnea(modelUniversal, dataUniversal, q, v, a);
181 
182  BOOST_CHECK(tauUniversal.isApprox(tauRandomAxis));
183 
184  // ForwardDynamics == aba
185  Eigen::VectorXd aAbaRandomAxis =
186  aba(modelRandomAxis, dataRandomAxis, q, v, tauRandomAxis, Convention::WORLD);
187  Eigen::VectorXd aAbaUniversal =
188  aba(modelUniversal, dataUniversal, q, v, tauUniversal, Convention::WORLD);
189 
190  BOOST_CHECK(aAbaUniversal.isApprox(aAbaRandomAxis));
191 
192  // CRBA
193  crba(modelRandomAxis, dataRandomAxis, q, Convention::WORLD);
194  crba(modelUniversal, dataUniversal, q, Convention::WORLD);
195 
196  BOOST_CHECK(dataUniversal.M.isApprox(dataRandomAxis.M));
197 
198  // Jacobian
199  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRandomAxis;
200  jacobianRandomAxis.resize(6, 2);
201  jacobianRandomAxis.setZero();
202  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianUniversal;
203  jacobianUniversal.resize(6, 2);
204  jacobianUniversal.setZero();
205 
206  computeJointJacobians(modelRandomAxis, dataRandomAxis, q);
207  computeJointJacobians(modelUniversal, dataUniversal, q);
208  getJointJacobian(modelRandomAxis, dataRandomAxis, 1, LOCAL, jacobianRandomAxis);
209  getJointJacobian(modelUniversal, dataUniversal, 1, LOCAL, jacobianUniversal);
210 
211  BOOST_CHECK(jacobianUniversal.isApprox(jacobianRandomAxis));
212 }
213 
214 BOOST_AUTO_TEST_SUITE_END()
pinocchio::InertiaTpl
Definition: spatial/fwd.hpp:58
pinocchio::JointModelUniversalTpl
Definition: multibody/joint/fwd.hpp:102
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:199
pinocchio::addJointAndBody
void addJointAndBody(Model &model, const JointModelBase< D > &jmodel, const Model::JointIndex parent_id, const SE3 &joint_placement, const std::string &name, const Inertia &Y)
Definition: model-generator.hpp:11
Eigen
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::forwardKinematics
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.
compute-all-terms.hpp
pinocchio::JointModelBase
Definition: joint-model-base.hpp:75
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
pinocchio::Convention::WORLD
@ WORLD
pinocchio::JointModelRX
JointModelRevoluteTpl< context::Scalar, context::Options, 0 > JointModelRX
Definition: joint-revolute.hpp:855
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::aba
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(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 Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::computeJointJacobians
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....
pinocchio::JointModelCompositeTpl::addJoint
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
Definition: joint-composite.hpp:279
rnea.hpp
aba.hpp
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:56
pinocchio::JointModelRY
JointModelRevoluteTpl< context::Scalar, context::Options, 1 > JointModelRY
Definition: joint-revolute.hpp:859
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:68
append-urdf-model-with-another-model.parent_id
int parent_id
Definition: append-urdf-model-with-another-model.py:28
pinocchio::getJointJacobian
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...
fwd.hpp
pinocchio::InertiaTpl< context::Scalar, context::Options >::Random
static InertiaTpl Random()
Definition: spatial/inertia.hpp:356
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1118
pinocchio::JointModelCompositeTpl
Definition: multibody/joint/fwd.hpp:141
append-urdf-model-with-another-model.joint_name
string joint_name
Definition: append-urdf-model-with-another-model.py:33
append-urdf-model-with-another-model.joint_placement
joint_placement
Definition: append-urdf-model-with-another-model.py:29
pinocchio::rnea
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...
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: multibody/model.hpp:102
pinocchio::SE3Tpl< context::Scalar, context::Options >::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
Y
Y
joints.hpp
pinocchio::JointModelRevoluteUnaligned
JointModelRevoluteUnalignedTpl< context::Scalar > JointModelRevoluteUnaligned
Definition: multibody/joint/fwd.hpp:38
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
crba.hpp
pinocchio::computeAllTerms
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...
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: multibody/model.hpp:99
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(vsRXRY)
Definition: joint-universal.cpp:36
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::DataTpl::nle
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis,...
Definition: multibody/data.hpp:179
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:48