Skip to content

Commit

Permalink
Merge pull request #1 from simeon-ned/feat/log_cholesky_parameters
Browse files Browse the repository at this point in the history 
Feat/log cholesky parameters
  • Loading branch information
@simeon-ned
simeon-ned authored Jun 23, 2024
2 parents be4a836 + 79b35f0 commit 50e174f
Show file tree
Hide file tree
Showing 5 changed files with 316 additions and 3 deletions.
2 changes: 1 addition & 1 deletion cmake
Submodule cmake updated 3 files
+0 −21 package.xml
+0 −13 release.cmake
+4 −4 sdformat.cmake
58 changes: 56 additions & 2 deletions include/pinocchio/bindings/python/spatial/inertia.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,12 @@ namespace pinocchio
typedef typename Inertia::Matrix3 Matrix3;
typedef typename Inertia::Vector6 Vector6;
typedef typename Inertia::Matrix6 Matrix6;
typedef typename Inertia::Matrix4 Matrix4;
typedef typename Inertia::Matrix10 Matrix10;
typedef typename Inertia::Vector10 Vector10;

typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
typedef MotionTpl<Scalar, Options> Motion;
typedef ForceTpl<Scalar, Options> Force;

Expand All @@ -57,7 +61,6 @@ namespace pinocchio
&InertiaPythonVisitor::makeFromMCI, bp::default_call_policies(),
bp::args("mass", "lever", "inertia")),
"Initialize from mass, lever and 3d inertia.")

.def(bp::init<>(bp::arg("self"), "Default constructor."))
.def(bp::init<const Inertia &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))

Expand Down Expand Up @@ -124,6 +127,17 @@ namespace pinocchio
(Matrix6(Inertia::*)(const MotionDense<Motion> &) const)
& Inertia::template variation<Motion>,
bp::args("self", "v"), "Returns the time derivative of the inertia.")
.def(
"LogcholToDynamicParameters", &InertiaPythonVisitor::LogcholToDynamicParameters_proxy,
bp::args("log_cholesky"),
"Converts logarithmic Cholesky parameters directly to theta parameters.")
.staticmethod("LogcholToDynamicParameters")
.def(
"calculateLogCholeskyJacobian",
&InertiaPythonVisitor::calculateLogCholeskyJacobian_proxy, bp::args("log_cholesky"),
"Calculates the Jacobian of the dynamic parameters with respect to the log-Cholesky "
"parameters.")
.staticmethod("calculateLogCholeskyJacobian")

#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
.def(bp::self == bp::self)
Expand Down Expand Up @@ -165,7 +179,32 @@ namespace pinocchio
"I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "
"where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter.")
.staticmethod("FromDynamicParameters")

.def(
"toPseudoInertia", &InertiaPythonVisitor::toPseudoInertia_proxy, bp::arg("self"),
"Converts the inertia to a pseudo inertia matrix."
"\nThe returned 4x4 pseudo inertia matrix has the form:"
"\n[[ -0.5*I_xx + 0.5*I_yy + 0.5*I_zz, -I_xy, -I_xz, mr_x],"
"\n [ -I_xy, 0.5*I_xx - 0.5*I_yy + 0.5*I_zz, -I_yz, mr_y],"
"\n [ -I_xz, -I_yz, 0.5*I_xx + 0.5*I_yy - 0.5*I_zz, mr_z],"
"\n [ mr_x, mr_y, mr_z, m ]].")
.def(
"FromPseudoInertia", &Inertia::FromPseudoInertia, bp::args("pseudo_inertia"),
"Builds an inertia matrix from a 4x4 pseudo inertia matrix."
"\nThe parameters are given as"
"\npseudo_inertia = [[ -0.5*I_xx + 0.5*I_yy + 0.5*I_zz, -I_xy, -I_xz, mr_x],"
"\n [ -I_xy, 0.5*I_xx - 0.5*I_yy + 0.5*I_zz, -I_yz, mr_y],"
"\n [ -I_xz, -I_yz, 0.5*I_xx + 0.5*I_yy - 0.5*I_zz, mr_z],"
"\n [ mr_x, mr_y, mr_z, m ]].")
.staticmethod("FromPseudoInertia")
.def(
"FromLogCholeskyParameters", &Inertia::template FromLogCholeskyParameters<VectorXs>,
bp::args("log_cholesky"),
"Builds an InertiaTpl from log Cholesky parameters."
"\nThe parameters are given as log_cholesky = [alpha, d_1, d_2, d_3, s_{12}, s_{23}, "
"s_{13}, t_1, t_2, t_3] "
"\nThe returned InertiaTpl object is constructed from the provided log Cholesky "
"parameters.")
.staticmethod("FromLogCholeskyParameters")
.def(
"FromSphere", &Inertia::FromSphere, bp::args("mass", "radius"),
"Returns the Inertia of a sphere defined by a given mass and radius.")
Expand Down Expand Up @@ -236,6 +275,21 @@ namespace pinocchio
return self.toDynamicParameters();
}

static Matrix4 toPseudoInertia_proxy(const Inertia & self)
{
return self.toPseudoInertia();
}

static VectorXs LogcholToDynamicParameters_proxy(const VectorXs & log_cholesky)
{
return Inertia::LogcholToDynamicParameters(log_cholesky);
}

static MatrixXs calculateLogCholeskyJacobian_proxy(const VectorXs & log_cholesky)
{
return Inertia::calculateLogCholeskyJacobian(log_cholesky);
}

static Inertia *
makeFromMCI(const Scalar & mass, const Vector3 & lever, const Matrix3 & inertia)
{
Expand Down
216 changes: 216 additions & 0 deletions include/pinocchio/spatial/inertia.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -241,6 +241,7 @@ namespace pinocchio
typedef Eigen::Matrix<T, 3, 3, U> Matrix3;
typedef Eigen::Matrix<T, 4, 4, U> Matrix4;
typedef Eigen::Matrix<T, 6, 6, U> Matrix6;
typedef Eigen::Matrix<T, 10, 10, U> Matrix10;
typedef Matrix6 ActionMatrix_t;
typedef Vector3 Angular_t;
typedef Vector3 Linear_t;
Expand Down Expand Up @@ -271,6 +272,7 @@ namespace pinocchio

typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
typedef typename Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
typedef typename Eigen::Matrix<Scalar, 10, 10, Options> Matrix10;

// Constructors
InertiaTpl()
Expand Down Expand Up @@ -565,6 +567,220 @@ namespace pinocchio
mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass, lever));
}

/**
* Converts the inertia to a pseudo inertia matrix.
*
* @return A 4x4 pseudo inertia matrix.
*/
Matrix4 toPseudoInertia() const
{
Vector10 dynamic_params = toDynamicParameters();
Scalar m = dynamic_params[0];
Vector3 h = dynamic_params.template segment<3>(1);
Matrix3 I_bar;
I_bar << dynamic_params[4], dynamic_params[5], dynamic_params[7], dynamic_params[5],
dynamic_params[6], dynamic_params[8], dynamic_params[7], dynamic_params[8],
dynamic_params[9];

Matrix3 Sigma = 0.5 * I_bar.trace() * Matrix3::Identity() - I_bar;
Matrix4 pseudo_inertia = Matrix4::Zero();
pseudo_inertia.template block<3, 3>(0, 0) = Sigma;
pseudo_inertia.template block<3, 1>(0, 3) = h;
pseudo_inertia.template block<1, 3>(3, 0) = h.transpose();
pseudo_inertia(3, 3) = m;

return pseudo_inertia;
}

/**
* Builds an inertia matrix from a 4x4 pseudo inertia matrix.
*
* @param[in] pseudo_inertia A 4x4 pseudo inertia matrix.
*
* @return An InertiaTpl object constructed from the provided pseudo inertia matrix.
*/
static InertiaTpl FromPseudoInertia(const Matrix4 & pseudo_inertia)
{
Scalar m = pseudo_inertia(3, 3);
Vector3 h = pseudo_inertia.template block<3, 1>(0, 3);
Matrix3 Sigma = pseudo_inertia.template block<3, 3>(0, 0);
Matrix3 I_bar = Sigma.trace() * Matrix3::Identity() - Sigma;

Vector10 dynamic_params;
dynamic_params[0] = m; /* */
dynamic_params.template segment<3>(1) = h;
dynamic_params[4] = I_bar(0, 0);
dynamic_params[5] = I_bar(0, 1);
dynamic_params[6] = I_bar(1, 1);
dynamic_params[7] = I_bar(0, 2);
dynamic_params[8] = I_bar(1, 2);
dynamic_params[9] = I_bar(2, 2);

return FromDynamicParameters(dynamic_params);
}

/**
* Converts logarithmic Cholesky parameters directly to dynamic parameters.
*
* @param[in] log_cholesky A 10-dimensional vector containing logarithmic Cholesky parameters.
* The parameters are given as
* \f$ \eta = [\alpha, d_1, d_2, d_3, s_{12}, s_{23}, s_{13}, t_1, t_2, t_3] \f$
*
* @return A 10-dimensional vector containing mass, first moments, and inertia tensor
* components. The parameters are given as \f$ \theta = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy},
* I_{yy}, I_{xz}, I_{yz}, I_{zz}] \f$
*/
template<typename Vector10Like>
static Vector10 LogcholToDynamicParameters(const Eigen::MatrixBase<Vector10Like> & log_cholesky)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, log_cholesky, 10, 1);
using Scalar = typename Vector10Like::Scalar;
Vector10 dynamic_params;

const Scalar alpha = log_cholesky[0];
const Scalar d1 = log_cholesky[1];
const Scalar d2 = log_cholesky[2];
const Scalar d3 = log_cholesky[3];
const Scalar s12 = log_cholesky[4];
const Scalar s23 = log_cholesky[5];
const Scalar s13 = log_cholesky[6];
const Scalar t1 = log_cholesky[7];
const Scalar t2 = log_cholesky[8];
const Scalar t3 = log_cholesky[9];

const Scalar exp_d1 = math::exp(d1);
const Scalar exp_d2 = math::exp(d2);
const Scalar exp_d3 = math::exp(d3);

dynamic_params[0] = 1;
dynamic_params[1] = t1;
dynamic_params[2] = t2;
dynamic_params[3] = t3;
dynamic_params[4] = s23 * s23 + t2 * t2 + t3 * t3 + exp_d2 * exp_d2 + exp_d3 * exp_d3;
dynamic_params[5] = -s12 * exp_d2 - s13 * s23 - t1 * t2;
dynamic_params[6] =
s12 * s12 + s13 * s13 + t1 * t1 + t3 * t3 + exp_d1 * exp_d1 + exp_d3 * exp_d3;
dynamic_params[7] = -s13 * exp_d3 - t1 * t3;
dynamic_params[8] = -s23 * exp_d3 - t2 * t3;
dynamic_params[9] =
s12 * s12 + s13 * s13 + s23 * s23 + t1 * t1 + t2 * t2 + exp_d1 * exp_d1 + exp_d2 * exp_d2;

const Scalar exp_2_alpha = math::exp(2 * alpha);
dynamic_params *= exp_2_alpha;

return dynamic_params;
}

/**
* Builds an Inertia from log Cholesky parameters.
*
* @param[in] log_cholesky A 10-dimensional vector containing logarithmic Cholesky parameters.
* The parameters are given as
* \f$ \eta = [\alpha, d_1, d_2, d_3, s_{12}, s_{23}, s_{13}, t_1, t_2, t_3] \f$
*
* @return An Inertia object constructed from the provided log Cholesky parameters.
*/
template<typename Vector10Like>
static InertiaTpl
FromLogCholeskyParameters(const Eigen::MatrixBase<Vector10Like> & log_cholesky)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, log_cholesky, 10, 1);
Vector10 dynamic_params = LogcholToDynamicParameters(log_cholesky);
return FromDynamicParameters(dynamic_params);
}

/**
* Calculates the Jacobian of the dynamic parameters with respect to the log-Cholesky
* parameters.
*
* @param[in] log_cholesky A 10-dimensional vector containing the log-Cholesky parameters.
*
* @return A 10x10 matrix containing the Jacobian of dynamic parameters with respect to
* log-Cholesky parameters.
*/
static Matrix10 calculateLogCholeskyJacobian(const Vector10 & log_cholesky)
{
Matrix10 jacobian = Matrix10::Zero();
const Scalar alpha = log_cholesky[0];
const Scalar d1 = log_cholesky[1];
const Scalar d2 = log_cholesky[2];
const Scalar d3 = log_cholesky[3];
const Scalar s12 = log_cholesky[4];
const Scalar s23 = log_cholesky[5];
const Scalar s13 = log_cholesky[6];
const Scalar t1 = log_cholesky[7];
const Scalar t2 = log_cholesky[8];
const Scalar t3 = log_cholesky[9];

const Scalar exp_2alpha = math::exp(2 * alpha);
const Scalar exp_2d1 = math::exp(2 * d1);
const Scalar exp_2d2 = math::exp(2 * d2);
const Scalar exp_2d3 = math::exp(2 * d3);
const Scalar exp_d1 = math::exp(d1);
const Scalar exp_d2 = math::exp(d2);
const Scalar exp_d3 = math::exp(d3);

jacobian(0, 0) = 2 * exp_2alpha;

jacobian(1, 0) = 2 * t1 * exp_2alpha;
jacobian(1, 7) = exp_2alpha;

jacobian(2, 0) = 2 * t2 * exp_2alpha;
jacobian(2, 8) = exp_2alpha;

jacobian(3, 0) = 2 * t3 * exp_2alpha;
jacobian(3, 9) = exp_2alpha;

jacobian(4, 0) = 2 * (s23 * s23 + t2 * t2 + t3 * t3 + exp_2d2 + exp_2d3) * exp_2alpha;
jacobian(4, 2) = 2 * exp_2alpha * exp_2d2;
jacobian(4, 3) = 2 * exp_2alpha * exp_2d3;
jacobian(4, 5) = 2 * s23 * exp_2alpha;
jacobian(4, 8) = 2 * t2 * exp_2alpha;
jacobian(4, 9) = 2 * t3 * exp_2alpha;

jacobian(5, 0) = -2 * (s12 * exp_d2 + s13 * s23 + t1 * t2) * exp_2alpha;
jacobian(5, 2) = -s12 * exp_2alpha * exp_d2;
jacobian(5, 4) = -exp_2alpha * exp_d2;
jacobian(5, 5) = -s13 * exp_2alpha;
jacobian(5, 6) = -s23 * exp_2alpha;
jacobian(5, 7) = -t2 * exp_2alpha;
jacobian(5, 8) = -t1 * exp_2alpha;

jacobian(6, 0) =
2 * (s12 * s12 + s13 * s13 + t1 * t1 + t3 * t3 + exp_2d1 + exp_2d3) * exp_2alpha;
jacobian(6, 1) = 2 * exp_2alpha * exp_2d1;
jacobian(6, 3) = 2 * exp_2alpha * exp_2d3;
jacobian(6, 4) = 2 * s12 * exp_2alpha;
jacobian(6, 6) = 2 * s13 * exp_2alpha;
jacobian(6, 7) = 2 * t1 * exp_2alpha;
jacobian(6, 9) = 2 * t3 * exp_2alpha;

jacobian(7, 0) = -2 * (s13 * exp_d3 + t1 * t3) * exp_2alpha;
jacobian(7, 3) = -s13 * exp_2alpha * exp_d3;
jacobian(7, 6) = -exp_2alpha * exp_d3;
jacobian(7, 7) = -t3 * exp_2alpha;
jacobian(7, 9) = -t1 * exp_2alpha;

jacobian(8, 0) = -2 * (s23 * exp_d3 + t2 * t3) * exp_2alpha;
jacobian(8, 3) = -s23 * exp_2alpha * exp_d3;
jacobian(8, 5) = -exp_2alpha * exp_d3;
jacobian(8, 8) = -t3 * exp_2alpha;
jacobian(8, 9) = -t2 * exp_2alpha;

jacobian(9, 0) = 2
* (s12 * s12 + s13 * s13 + s23 * s23 + t1 * t1 + t2 * t2 + exp_2d1 + exp_2d2)
* exp_2alpha;
jacobian(9, 1) = 2 * exp_2alpha * exp_2d1;
jacobian(9, 2) = 2 * exp_2alpha * exp_2d2;
jacobian(9, 4) = 2 * s12 * exp_2alpha;
jacobian(9, 5) = 2 * s23 * exp_2alpha;
jacobian(9, 6) = 2 * s13 * exp_2alpha;
jacobian(9, 7) = 2 * t1 * exp_2alpha;
jacobian(9, 8) = 2 * t2 * exp_2alpha;

return jacobian;
}

// Arithmetic operators
InertiaTpl & __equl__(const InertiaTpl & clone)
{
Expand Down
34 changes: 34 additions & 0 deletions unittest/python/bindings_inertia.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,40 @@ def test_dynamic_parameters(self):
I2 = pin.Inertia.FromDynamicParameters(v)
self.assertApprox(I2, I)

def test_pseudo_inertia(self):
I = pin.Inertia.Random()
pseudo = I.toPseudoInertia()
self.assertTrue(np.all(np.linalg.eigvals(pseudo) > 0))
I_back = pin.Inertia.FromPseudoInertia(pseudo)
self.assertApprox(I_back.mass, I.mass)
self.assertApprox(I_back.lever, I.lever)
self.assertApprox(I_back.inertia, I.inertia)

def test_log_cholesky(self):
eta = np.random.randn(10)
alpha, d1, d2, d3, s12, s23, s13, t1, t2, t3 = eta
# Compute the exponential terms
exp_alpha = np.exp(alpha)
exp_d1 = np.exp(d1)
exp_d2 = np.exp(d2)
exp_d3 = np.exp(d3)

# Create the matrix U
U = exp_alpha * np.array(
[
[exp_d1, s12, s13, t1],
[0, exp_d2, s23, t2],
[0, 0, exp_d3, t3],
[0, 0, 0, 1],
]
)
pseudo_chol = U @ U.T

inertia = pin.Inertia.FromLogCholeskyParameters(eta)
pseudo = inertia.toPseudoInertia()
self.assertTrue(np.all(np.linalg.eigvals(pseudo) > 0))
self.assertApprox(pseudo, pseudo_chol)

def test_array(self):
I = pin.Inertia.Random()
I_array = np.array(I)
Expand Down
9 changes: 9 additions & 0 deletions unittest/spatial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -631,6 +631,15 @@ BOOST_AUTO_TEST_CASE(test_Inertia)
Symmetric3(3.79705882, 0., 3.79705882, 0., 0., 1.81176471).matrix(),
1e-5)); // Computed with hppfcl::Capsule

// Test Inertia From Pseudo Inertia and backward
I1 = Inertia::FromCapsule(1., 2., 3);
BOOST_CHECK(I1.isApprox(Inertia::FromPseudoInertia(I1.toPseudoInertia())));

// Test Inertia From LogCholesky parameters and backward
Inertia::Vector10 logChol = Inertia::Vector10::Random();
I1 = Inertia::FromLogCholeskyParameters(logChol);
BOOST_CHECK(I1.isApprox(Inertia::FromDynamicParameters(I1.toDynamicParameters())));

// Copy operator
Inertia aI_copy(aI);
BOOST_CHECK(aI_copy == aI);
Expand Down

0 comments on commit 50e174f

Please sign in to comment.