casadi/explog.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2021 INRIA
3 //
4 
5 #include "pinocchio/autodiff/casadi.hpp"
6 #include "pinocchio/autodiff/casadi-algo.hpp"
7 
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 
10 #include "pinocchio/multibody/sample-models.hpp"
11 
12 #include <casadi/casadi.hpp>
13 
14 #include <iostream>
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17 
18 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19 
20 BOOST_AUTO_TEST_CASE(test_squaredDistance)
21 {
22  typedef double Scalar;
23  typedef casadi::SX ADScalar;
24  typedef pinocchio::ModelTpl<Scalar> Model;
25  typedef pinocchio::ModelTpl<ADScalar> ADModel;
26  using casadi::SXVector;
27 
28  Model model;
29  pinocchio::buildModels::humanoidRandom(model, true);
30  model.lowerPositionLimit.head<3>().fill(-1.);
31  model.upperPositionLimit.head<3>().fill(1.);
32 
33  typedef Model::ConfigVectorType ConfigVector;
34  typedef Eigen::Map<ConfigVector> ConfigVectorMap;
35 
36  ConfigVector q0(pinocchio::neutral(model));
37  ConfigVector q1(pinocchio::randomConfiguration(model));
38  ConfigVector q2(pinocchio::randomConfiguration(model));
39 
40  ADModel ad_model = model.cast<ADScalar>();
41  typedef ADModel::ConfigVectorType cConfig_t;
42  typedef ADModel::TangentVectorType cTangent_t;
43 
44  casadi::SX cs_q0 = casadi::SX::sym("q0", model.nq, 1);
45  casadi::SX cs_q1 = casadi::SX::sym("q1", model.nq, 1);
46  casadi::SX cv0 = casadi::SX::sym("v0", model.nv, 1);
47  casadi::SX cv1 = casadi::SX::sym("v1", model.nv, 1);
48  casadi::SX v_all = vertcat(cv0, cv1);
49  cConfig_t q0_ad(model.nq), q1_ad(model.nq);
50  cTangent_t v0_ad(model.nq), v1_ad(model.nv);
51 
52  q0_ad = Eigen::Map<cConfig_t>(static_cast<SXVector>(cs_q0).data(), model.nq, 1);
53  q1_ad = Eigen::Map<cConfig_t>(static_cast<SXVector>(cs_q1).data(), model.nq, 1);
54  v0_ad = Eigen::Map<cTangent_t>(static_cast<SXVector>(cv0).data(), model.nv, 1);
55  v1_ad = Eigen::Map<cTangent_t>(static_cast<SXVector>(cv1).data(), model.nv, 1);
56 
57  auto cq0_i = pinocchio::integrate(ad_model, q0_ad, v0_ad);
58  auto cq1_i = pinocchio::integrate(ad_model, q1_ad, v1_ad);
59 
60  cTangent_t d = pinocchio::difference(ad_model, cq0_i, cq1_i);
61  casadi::SX d_vec(model.nv);
62  pinocchio::casadi::copy(d, d_vec);
63  ADScalar dist_expr = d.dot(d);
64 
65  size_t nq = (size_t)model.nq;
66 
67  casadi::SX zero_vec = casadi::SX::zeros(2 * model.nv);
68  casadi::SX Jdist_expr = substitute(gradient(dist_expr, v_all), v_all, zero_vec);
69  casadi::Function eval_Jdist("Jdistance", SXVector{cs_q0, cs_q1}, SXVector{Jdist_expr});
70 
71  std::cout << "Jdistance func: " << eval_Jdist << '\n';
72 
73  std::vector<Scalar> q0_vec(nq);
74  ConfigVectorMap(q0_vec.data(), model.nq, 1) = q0;
75  std::vector<Scalar> q1_vec(nq);
76  ConfigVectorMap(q1_vec.data(), model.nq, 1) = q1;
77  std::vector<Scalar> q2_vec(nq);
78  ConfigVectorMap(q2_vec.data(), model.nq, 1) = q2;
79 
80  auto J0 = eval_Jdist(casadi::DMVector{q0_vec, q0_vec})[0];
81  auto J1 = eval_Jdist(casadi::DMVector{q0_vec, q1_vec})[0];
82  auto J2 = eval_Jdist(casadi::DMVector{q0_vec, q2_vec})[0];
83 
84  ConfigVector q(model.nq);
85  std::vector<Scalar> q_vec(nq);
86  for (size_t it = 0; it < 10; it++)
87  {
88  q.noalias() = pinocchio::randomConfiguration(model);
89  ConfigVectorMap(q_vec.data(), model.nq, 1) = q;
90  auto J_vec = static_cast<std::vector<Scalar>>(eval_Jdist(casadi::DMVector{q_vec, q_vec})[0]);
91  Eigen::Map<Eigen::MatrixXd> J_as_mat(J_vec.data(), 2 * model.nv, 1);
92  BOOST_CHECK(J_as_mat.isApprox(Eigen::MatrixXd::Zero(2 * model.nv, 1)));
93  }
94 }
95 
96 template<typename Derived>
97 casadi::DM SE3toCasadiDM(const pinocchio::SE3Base<Derived> & M)
98 {
99  typedef typename Derived::Scalar Scalar;
100  auto M_mat = M.toHomogeneousMatrix();
101  std::vector<Scalar> flat_M_vec(M_mat.data(), M_mat.data() + M_mat.size());
102  casadi::DM out{flat_M_vec};
103  return reshape(out, 4, 4);
104 }
105 
106 BOOST_AUTO_TEST_CASE(test_Jlog6)
107 {
108  namespace pin = pinocchio;
109  using casadi::DMVector;
110  using casadi::SXVector;
111  using casadi::SXVectorVector;
112  typedef pin::SE3Tpl<double> SE3;
113  typedef casadi::SX ADScalar;
114  typedef pin::SE3Tpl<ADScalar> cSE3;
115  typedef pin::MotionTpl<ADScalar> cMotion;
116 
117  casadi::SX csM0 = casadi::SX::sym("cM0", 4, 4);
118  casadi::SX csM1 = casadi::SX::sym("cM1", 4, 4);
119  casadi::SX csm0 = casadi::SX::sym("cm0", 6);
120  casadi::SX csm1 = casadi::SX::sym("cm1", 6);
121  casadi::SX csm_all = vertcat(csm0, csm1);
122 
123  cMotion::Vector6 cm0_v, cm1_v;
124 
125  cSE3 cM0_i, cM1_i;
126  {
127 
128  cm0_v = Eigen::Map<cMotion::Vector6>(static_cast<SXVector>(csm0).data(), 6, 1);
129  cm1_v = Eigen::Map<cMotion::Vector6>(static_cast<SXVector>(csm1).data(), 6, 1);
130 
131  cSE3::Matrix4 cM0_mat, cM1_mat;
132  cM0_mat = Eigen::Map<cSE3::Matrix4>(&static_cast<SXVector>(csM0)[0], 4, 4);
133  cM1_mat = Eigen::Map<cSE3::Matrix4>(&static_cast<SXVector>(csM1)[0], 4, 4);
134 
135  auto rot0 = cM0_mat.template block<3, 3>(0, 0);
136  auto rot1 = cM1_mat.template block<3, 3>(0, 0);
137  auto trans0 = cM0_mat.template block<3, 1>(0, 3);
138 
139  cSE3 cM0(rot0, trans0);
140  cSE3 cM1(rot1, cM1_mat.template block<3, 1>(0, 3));
141 
142  cM0_i = cM0 * pin::exp6(cm0_v);
143  cM1_i = cM1 * pin::exp6(cm1_v);
144  }
145 
146  auto cdM(pin::log6(cM0_i.actInv(cM1_i)).toVector());
147  casadi::SX cdM_s(6);
148  pinocchio::casadi::copy(cdM, cdM_s);
149  casadi::SX zeros = casadi::SX::zeros(12);
150 
151  auto dist = cdM.squaredNorm();
152  auto grad_cdM_expr = gradient(dist, csm_all);
153  auto hess_cdM_expr = jacobian(grad_cdM_expr, csm_all);
154 
155  casadi::Function grad_cdM_eval(
156  "gdM", SXVector{csM0, csM1}, SXVector{substitute(grad_cdM_expr, csm_all, zeros)});
157  casadi::Function hess_cdM_eval(
158  "gdM", SXVector{csM0, csM1}, SXVector{substitute(hess_cdM_expr, csm_all, zeros)});
159 
160  std::cout << "dM_eval: " << grad_cdM_eval << '\n';
161 
162  SE3 M_neutral(SE3::Identity());
163  SE3 M0(SE3::Random()), M1(SE3::Random());
164 
165  casadi::DM M_n_dm = SE3toCasadiDM(M_neutral);
166  casadi::DM M0_dm = SE3toCasadiDM(M0);
167  casadi::DM M1_dm = SE3toCasadiDM(M1);
168  casadi::DM M2_dm = SE3toCasadiDM(M1.actInv(M1));
169  std::cout << M0_dm << "\n\n";
170  std::cout << M2_dm << '\n';
171 
172  auto J0 = grad_cdM_eval(DMVector{M0_dm, M1_dm})[0];
173  std::cout << J0 << '\n';
174 
175  auto J1 = grad_cdM_eval(DMVector{M0_dm, M2_dm})[0];
176  std::cout << J1 << '\n';
177 
178  auto J2 = grad_cdM_eval(DMVector{M2_dm, M2_dm})[0];
179  std::cout << J2 << '\n';
180 
181  std::cout << hess_cdM_eval(DMVector{M_n_dm, M_n_dm})[0];
182  std::cout << hess_cdM_eval(DMVector{M0_dm, M0_dm})[0];
183  std::cout << hess_cdM_eval(DMVector{M0_dm, M1_dm})[0];
184  std::cout << hess_cdM_eval(DMVector{M2_dm, M2_dm})[0];
185 }
186 
187 BOOST_AUTO_TEST_SUITE_END()
pinocchio.casadi::sym
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
Definition: autodiff/casadi.hpp:230
pinocchio::SE3Tpl
Definition: context/casadi.hpp:24
setup.data
data
Definition: cmake/cython/setup.in.py:48
q2
q2
pinocchio::difference
void difference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &dvout)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.
Definition: joint-configuration.hpp:193
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true, bool mimic=false)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
meshcat-viewer.q1
q1
Definition: meshcat-viewer.py:84
pinocchio::randomConfiguration
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.
Definition: joint-configuration.hpp:315
casadi-algo.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
codegen-rnea.nq
nq
Definition: codegen-rnea.py:18
joint-configuration.hpp
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_squaredDistance)
Definition: casadi/explog.cpp:20
pinocchio::integrate
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.
Definition: joint-configuration.hpp:70
pinocchio::ModelTpl< ADScalar >::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: multibody/model.hpp:95
dist
FCL_REAL & dist(short i)
M
M
q
q
inverse-kinematics-3d.it
int it
Definition: inverse-kinematics-3d.py:17
pinocchio::jacobian
void jacobian(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel, ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > &cdata, const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< JacobianMatrix > &jacobian_matrix)
Definition: constraint-model-visitor.hpp:199
ADScalar
CppAD::AD< Scalar > ADScalar
Definition: timings-cppad-jit.cpp:38
ADModel
pinocchio::ModelTpl< ADScalar > ADModel
Definition: timings-cppad-jit.cpp:51
pinocchio::SE3Base
Base class for rigid transformation.
Definition: se3-base.hpp:30
fill
fill
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: spatial/explog.hpp:435
pinocchio.casadi::copy
void copy(::casadi::Matrix< Scalar > const &src, Eigen::MatrixBase< MT > &dst)
Definition: autodiff/casadi.hpp:154
pinocchio::quaternion::exp6
void exp6(const MotionDense< MotionDerived > &motion, Eigen::MatrixBase< Config_t > &qout)
The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
Definition: explog-quaternion.hpp:92
casadi.hpp
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:363
pinocchio::python::context::SE3
SE3Tpl< Scalar, Options > SE3
Definition: bindings/python/context/generic.hpp:53
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: multibody/model.hpp:87
sample-models.hpp
pinocchio::MotionTpl
Definition: context/casadi.hpp:22
SE3toCasadiDM
casadi::DM SE3toCasadiDM(const pinocchio::SE3Base< Derived > &M)
Definition: casadi/explog.cpp:97
Model
pinocchio::ModelTpl< Scalar > Model
Definition: timings-cppad-jit.cpp:48
Scalar
double Scalar
Definition: timings-cppad-jit.cpp:37
pinocchio::ModelTpl< Scalar >
ur5x4.q0
q0
Definition: ur5x4.py:42
d
FCL_REAL d
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:33

pinocchio
Author(s):
autogenerated on Wed May 28 2025 02:41:17