Skip to content

Commit

Permalink
feat: implement indirect regressors
Browse files Browse the repository at this point in the history
  • Loading branch information
@lvjonok
lvjonok committed Nov 17, 2024
1 parent fa9f74c commit 24a888e
Show file tree
Hide file tree
Showing 4 changed files with 276 additions and 11 deletions.
16 changes: 16 additions & 0 deletions include/pinocchio/algorithm/regressor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -411,6 +411,22 @@ namespace pinocchio
DataTpl<Scalar, Options, JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v);

template<
typename Scalar,
int Options,
template<typename, int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
std::pair<
typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs,
typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs>
computeIndirectRegressors(
const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
DataTpl<Scalar, Options, JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v);
} // namespace pinocchio

/* --- Details -------------------------------------------------------------------- */
Expand Down
94 changes: 91 additions & 3 deletions include/pinocchio/algorithm/regressor.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -374,11 +374,15 @@ namespace pinocchio
data.liMi[i] = model.jointPlacements[i] * jdata.M();

data.v[i] = jdata.v();
// v[i] = Xup[i] * v[parent[i]] + vJ
if (parent > 0)
data.v[i] += data.liMi[i].actInv(data.v[parent]);

// crm(v{i}) * vJ == v[i] ^ jdata.v()
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
// S{i} * qdd{i}
data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a);
// Xup[i] * a[parent[i]]
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
}
};
Expand Down Expand Up @@ -408,10 +412,12 @@ namespace pinocchio
const JointIndex i = jmodel.id();
const JointIndex parent = model.parents[i];

// Y(jj,param_inds) = S{j}' * Fi;
data.jointTorqueRegressor.block(
jmodel.idx_v(), 10 * (Eigen::DenseIndex(col_idx) - 1), jmodel.nv(), 10) =
jdata.S().transpose() * data.bodyRegressor;

// Fi = Xup{j}' * Fi;
if (parent > 0)
forceSet::se3Action(data.liMi[i], data.bodyRegressor, data.bodyRegressor);
}
Expand Down Expand Up @@ -550,6 +556,79 @@ namespace pinocchio
return data.potentialEnergyRegressor;
}

template<
typename Scalar,
int Options,
template<typename, int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
std::pair<
typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs,
typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs>
computeIndirectRegressors(
const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
DataTpl<Scalar, Options, JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
typedef context::Data::Matrix6x Matrix6x;
typedef context::Data::MatrixXs MatrixXs;
typedef pinocchio::context::BodyRegressorType BodyRegressorType;

MatrixXs CTregressor = MatrixXs::Zero(model.nv, 10 * (model.njoints - 1));
MatrixXs Hregressor = MatrixXs::Zero(model.nv, 10 * (model.njoints - 1));
for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
{
const JointIndex parent_id = model.parents[joint_id];
auto jmodel = model.joints[joint_id];
auto jdata = data.joints[joint_id];
auto i = joint_id;

// update joint model
jmodel.calc(jdata.derived(), q.derived(), v.derived());
// Xup{i} = XJ * model.Xtree{i};
data.liMi[i] = model.jointPlacements[i] * jdata.M();

data.v[i] = jdata.v(); // vJ = S{i} * qd{i};
// if parent>0 then v{i} = Xup{i} * v{parent} + vJ
if (parent_id > 0)
data.v[i] += data.liMi[i].actInv(data.v[parent_id]);

auto Sd = data.v[i].cross(jdata.S());
// compute regressor
// hi = individualRegressor(v[i], v[i] * 0);
// in Wensing's implementation the order is (a, v);
BodyRegressorType hi = bodyRegressor(data.v[i] * 0, data.v[i]);

// reverse substitution
auto j = i;
while (j > 0)
{
auto jdataj = data.joints[j];
auto jmodelj = model.joints[j];

auto Sj = jdataj.S();
auto Sdj = data.v[j].cross(Sj);

// Y_Hqd(jj, param_inds) = S{j}' * hi;
Hregressor.block(model.joints[j].idx_v(), (i - 1) * 10, model.joints[j].nv(), 10) =
Sj.transpose() * hi;
// Y_CTqd(jj, param_inds) = Sd{j}' * hi;
CTregressor.block(model.joints[j].idx_v(), (i - 1) * 10, model.joints[j].nv(), 10) =
Sdj.transpose() * hi;

// hi = Xup[i]' * hi
forceSet::se3Action(data.liMi[j], hi, hi);

// j = model.parent(j);
j = model.parents[j];
}
}

return std::make_pair(Hregressor, CTregressor);
}

template<
typename Scalar,
int Options,
Expand All @@ -573,9 +652,16 @@ namespace pinocchio
auto spatialKineticEnergyJacobian =
[](const Eigen::Vector3d & v, const Eigen::Vector3d & w) -> Eigen::Matrix<double, 6, 10> {
Eigen::Matrix<double, 10, 6> jacobian;
jacobian << v[0], v[1], v[2], 0, 0, 0, 0, w[2], -w[1], 0, -v[2], v[1], -w[2], 0, w[0], v[2],
0, -v[0], w[1], -w[0], 0, -v[1], v[0], 0, 0, 0, 0, w[0], 0, 0, 0, 0, 0, w[1], w[0], 0, 0, 0,
0, 0, w[1], 0, 0, 0, 0, w[2], 0, w[0], 0, 0, 0, 0, w[2], w[1], 0, 0, 0, 0, 0, w[2];
jacobian << v[0], v[1], v[2], 0, 0, 0, \
0, w[2], -w[1], 0, -v[2], v[1], \
-w[2], 0, w[0], v[2], 0, -v[0], \
w[1], -w[0], 0, -v[1], v[0], 0, \
0, 0, 0, w[0], 0, 0, \
0, 0, 0, w[1], w[0], 0, \
0, 0, 0, 0, w[1], 0, \
0, 0, 0, w[2], 0, w[0], \
0, 0, 0, 0, w[2], w[1], \
0, 0, 0, 0, 0, w[2];

return jacobian.transpose();
};
Expand All @@ -595,6 +681,8 @@ namespace pinocchio
MatrixXs gravity_regressor = data.jointTorqueRegressor.eval();

data.momentumRegressor = momentum_with_gravity - gravity_regressor;
forwardKinematics(model, data, q, v);
computeForwardKinematicsDerivatives(model, data, q, v, zero_v);

for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
{
Expand Down
13 changes: 13 additions & 0 deletions src/algorithm/regressor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,19 @@ namespace pinocchio
context::VectorXs>(
const context::Model &, context::Data &, const Eigen::MatrixBase<context::VectorXs> &);

template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI
std::pair<context::Data::MatrixXs, context::Data::MatrixXs>
computeIndirectRegressors<
context::Scalar,
context::Options,
JointCollectionDefaultTpl,
context::VectorXs,
context::VectorXs>(
const context::Model &,
context::Data &,
const Eigen::MatrixBase<context::VectorXs> &,
const Eigen::MatrixBase<context::VectorXs> &);

template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI
std::pair<context::Data::MatrixXs, context::Data::MatrixXs>
computeMomentumRegressor<
Expand Down
164 changes: 156 additions & 8 deletions unittest/regressor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,9 @@
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/multibody/sample-models.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

#include "pinocchio/parsers/urdf.hpp"
#include <iostream>
#include "pinocchio/algorithm/kinematics.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
Expand Down Expand Up @@ -428,39 +429,186 @@ BOOST_AUTO_TEST_CASE(test_potential_energy_regressor)
BOOST_CHECK_CLOSE(potential_energy_regressor, target_energy, 1e-12);
}

BOOST_AUTO_TEST_CASE(test_coriolis_regressor)
{
using namespace Eigen;
using namespace pinocchio;

pinocchio::Model model;
buildModels::manipulator(model);

pinocchio::Data data(model);

VectorXd q = randomConfiguration(model);
VectorXd v = Eigen::VectorXd::Random(model.nv);

Eigen::VectorXd params(10 * (model.njoints - 1));
for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
params.segment<10>(Eigen::DenseIndex((i - 1) * 10)) = model.inertias[i].toDynamicParameters();

computeAllTerms(model, data, q, v);
forwardKinematics(model, data, q, v);
framesForwardKinematics(model, data, q);
auto regressors = computeIndirectRegressors(model, data, q, v);
auto Hregressor = regressors.first;
auto CTregressor = regressors.second;
auto CTv_regressor = CTregressor * params;

auto CTv = computeCoriolisMatrix(model, data, q, v).transpose() * v;

// they should match
BOOST_CHECK(CTv_regressor.isApprox(CTv));

// fill in the mass inertia matrix
data.M.triangularView<Eigen::StrictlyLower>() =
data.M.transpose().triangularView<Eigen::StrictlyLower>();

const auto momentum = data.M * v;

auto momentum_regressor = Hregressor * params;

BOOST_CHECK(momentum_regressor.isApprox(momentum));
}

BOOST_AUTO_TEST_CASE(test_momentum_regressor)
{
using namespace Eigen;
using namespace pinocchio;

pinocchio::Model model;
buildModels::humanoidRandom(model);
return;

model.lowerPositionLimit.head<7>().fill(-1.);
model.upperPositionLimit.head<7>().fill(1.);
// buildModels::humanoid(model);
// model.lowerPositionLimit.head<7>().fill(-1.);
// model.upperPositionLimit.head<7>().fill(1.);

buildModels::manipulator(model);

pinocchio::Data data(model);
pinocchio::Data data_ref(model);

const VectorXd q = randomConfiguration(model);
const VectorXd v = Eigen::VectorXd::Random(model.nv);
const VectorXd dv = Eigen::VectorXd::Random(model.nv);
VectorXd q = randomConfiguration(model);
VectorXd v = Eigen::VectorXd::Random(model.nv);
VectorXd dv = Eigen::VectorXd::Random(model.nv);

computeAllTerms(model, data_ref, q, v);
forwardKinematics(model, data_ref, q, v);

// fill in the mass inertia matrix
data_ref.M.triangularView<Eigen::StrictlyLower>() =
data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
const VectorXd target_momentum = data_ref.M * v;

computeMomentumRegressor(model, data, q, v);
std::cout << "executed momentum regressor" << std::endl;
Eigen::VectorXd params(10 * (model.njoints - 1));
for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
params.segment<10>(Eigen::DenseIndex((i - 1) * 10)) = model.inertias[i].toDynamicParameters();

const VectorXd momentum_regressor = data.momentumRegressor * params;
BOOST_CHECK(momentum_regressor.isApprox(target_momentum));

// perform simple simulation for one timestep to validate dpartial_lagrangian_q
q = randomConfiguration(model);
v = Eigen::VectorXd::Random(model.nv);
dv = Eigen::VectorXd::Random(model.nv);
VectorXd tau = rnea(model, data, q, v, dv);

forwardKinematics(model, data_ref, q, v);
computeMomentumRegressor(model, data, q, v);

const Eigen::VectorXd momentum_test = data.momentumRegressor * params;
const Eigen::MatrixXd dpartial_lagrangian_q_test = data.dpartial_lagrangian_q;

const auto coriolis = computeCoriolisMatrix(model, data_ref, q, v);
const auto gravity = computeGeneralizedGravity(model, data_ref, q);

const auto p_dot = coriolis.transpose() * v - gravity + tau;

std::cout << "p_dot: " << p_dot.transpose() << std::endl;

const double alpha = 1e-8;
VectorXd v_plus = v + alpha * dv;
VectorXd q_plus = integrate(model, q, alpha * v);

forwardKinematics(model, data_ref, q_plus, v_plus);
computeMomentumRegressor(model, data, q_plus, v_plus);

const Eigen::VectorXd momentum_plus = data.momentumRegressor * params;
// const Eigen::VectorXd dpartial_lagrangian_q_plus = data.dpartial_lagrangian_q;

// compare the time derivative of momentum
const Eigen::VectorXd momentum_time_derivative_analytical =
dpartial_lagrangian_q_test * params + tau;
// const Eigen::VectorXd momentum_time_derivative_analytical2 =
// dpartial_lagrangian_q_plus * params + tau;
const Eigen::VectorXd momentum_time_derivative_numerical =
(momentum_plus - momentum_test) / alpha;

// theoretically, the two should be approximately equal
std::cout << "analytical: " << momentum_time_derivative_analytical.transpose() << std::endl;
// std::cout << "analytical: " << momentum_time_derivative_analytical2.transpose() << std::endl;
std::cout << "numerical: " << momentum_time_derivative_numerical.transpose() << std::endl;

// // define a random trajectory for all joints
// const Eigen::VectorXd amplitude = Eigen::VectorXd::Ones(model.nv);
// auto positions = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
// return amplitude * std::cos(time);
// };
// auto velocities = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
// return -amplitude * std::sin(time);
// };
// auto accelerations = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
// return -amplitude * std::cos(time);
// };

// const double time_step = 1e-2;
// double t = std::rand() / (double)RAND_MAX;
// double t_plus = t + time_step;
// double t_minus = t - time_step;

// const Eigen::VectorXd q_test = positions(amplitude, t);
// const Eigen::VectorXd v_test = velocities(amplitude, t);
// const Eigen::VectorXd a_test = accelerations(amplitude, t);
// const Eigen::VectorXd q_plus = positions(amplitude, t_plus);
// const Eigen::VectorXd v_plus = velocities(amplitude, t_plus);
// const Eigen::VectorXd q_minus = positions(amplitude, t_minus);
// const Eigen::VectorXd v_minus = velocities(amplitude, t_minus);

// // compute generalized torque according to the trajectories
// rnea(model, data, q_test, v_test, a_test);
// const Eigen::VectorXd tau = data.tau;
// std::cout << "executed tau: " << tau.transpose() << std::endl;

// // compute momemtum at each time instance
// // computeAllTerms(model, data, q_test, v_test);
// forwardKinematics(model, data_ref, q_test, v_test);

// auto answer = computeMomentumRegressor(model, data, q_test, v_test);
// const Eigen::VectorXd momentum_test = data.momentumRegressor * params;
// const Eigen::MatrixXd dpartial_lagrangian_q_test = data.dpartial_lagrangian_q;

// // std::cout << "dpartial langrangian q: " << std::endl;
// // std::cout << dpartial_lagrangian_q_test << std::endl;

// // computeAllTerms(model, data, q_plus, v_plus);
// forwardKinematics(model, data_ref, q_plus, v_plus);
// computeMomentumRegressor(model, data, q_plus, v_plus);
// const Eigen::VectorXd momentum_plus = data.momentumRegressor * params;

// // computeAllTerms(model, data, q_minus, v_minus);
// forwardKinematics(model, data_ref, q_minus, v_minus);
// computeMomentumRegressor(model, data, q_minus, v_minus);
// const Eigen::VectorXd momentum_minus = data.momentumRegressor * params;

// // compute the time derivative of momentum analytically and numerically
// const Eigen::VectorXd momentum_time_derivative_analytical =
// dpartial_lagrangian_q_test * params + tau;
// const Eigen::VectorXd momentum_time_derivative_numerical =
// (momentum_plus - momentum_minus) / (2 * time_step);

// // theoretically, the two should be approximately equal
// std::cout << "analytical: " << momentum_time_derivative_analytical.transpose() << std::endl;
// std::cout << "numerical: " << momentum_time_derivative_numerical.transpose() << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()

0 comments on commit 24a888e

Please sign in to comment.