Skip to content

Commit

Permalink
working SO aq derivs
Browse files Browse the repository at this point in the history
  • Loading branch information
@shubhamsingh91
shubhamsingh91 committed Jan 13, 2025
1 parent e54d0d5 commit 2cd3d73
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 19 deletions.
Binary file modified benchmark/spatial_force
View file
Binary file not shown.
39 changes: 28 additions & 11 deletions benchmark/test_spatial_force.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -317,20 +317,23 @@ int main(int argc, const char ** argv)
//--------------------------------------------------------------------------------
//------------------------- SO partials of cumulative force-----------------------
//--------------------------------------------------------------------------------
std::vector<Eigen::Tensor<double,3>> d2f_dq2_fd, d2f_dv2_fd, d2f_da2_fd;
std::vector<Eigen::Tensor<double,3>> d2f_dq2_fd, d2f_dv2_fd, d2f_da2_fd , d2f_daq_fd;
std::vector<Eigen::Tensor<double,3>> d2f_dq2_ana, d2f_dv2_ana, d2f_da2_ana, d2f_daq_ana;

for (int i = 0; i < model.njoints - 1; i++) {
d2f_dq2_fd.emplace_back(6, model.nv, model.nv);
d2f_dv2_fd.emplace_back(6, model.nv, model.nv);
d2f_da2_fd.emplace_back(6, model.nv, model.nv);
d2f_daq_fd.emplace_back(6, model.nv, model.nv);

d2f_dq2_ana.emplace_back(6, model.nv, model.nv);
d2f_dv2_ana.emplace_back(6, model.nv, model.nv);
d2f_da2_ana.emplace_back(6, model.nv, model.nv);
d2f_daq_ana.emplace_back(6, model.nv, model.nv);
}

for (auto &t : d2f_da2_fd) t.setZero();

double alpha = 1e-6; // performs well

VectorXd v_eps(VectorXd::Zero(model.nv));
Expand All @@ -350,8 +353,9 @@ int main(int argc, const char ** argv)
ten3d df_dv_ana_tensor_plus (6,model.nv, model.nv);
ten3d df_da_ana_tensor_plus (6,model.nv, model.nv);

ten3d t_d2fc_dq_dqk , t_d2fc_dv_dvk;
ten3d t_d2fc_dq_dqk , t_d2fc_dv_dvk, t_d2fc_da_dqk;
Eigen::MatrixXd m_d2fci_dq_dqk(6,model.nv);
Eigen::MatrixXd m_d2fci_da_dqk(6,model.nv);
Eigen::MatrixXd m_d2fci_dv_dvk(6,model.nv);

// Partial wrt q
Expand All @@ -363,11 +367,15 @@ int main(int argc, const char ** argv)
df_dq_ana_tensor_plus, df_dv_ana_tensor_plus, df_da_ana_tensor_plus);

t_d2fc_dq_dqk = (df_dq_ana_tensor_plus - df_dq_ana) / alpha; // 3d tensor d2fc/dq dqk
t_d2fc_da_dqk = (df_da_ana_tensor_plus - df_da_ana) / alpha; // 3d tensor d2fc/da dqk

for (int i = 0; i < model.nv; i++)
{
get_mat_from_tens3_v1_gen(t_d2fc_dq_dqk, m_d2fci_dq_dqk, 6, model.nv, i);
hess_assign_fd_v1_gen(d2f_dq2_fd.at(i), m_d2fci_dq_dqk, 6, model.nv, k); // slicing in the matrix along the kth page for ith tensor

get_mat_from_tens3_v1_gen(t_d2fc_da_dqk, m_d2fci_da_dqk, 6, model.nv, i);
hess_assign_fd_v1_gen(d2f_daq_fd.at(i), m_d2fci_da_dqk, 6, model.nv, k); // slicing in the matrix along the kth page for ith tensor
}

v_eps[k] -= alpha;
Expand All @@ -393,38 +401,47 @@ int main(int argc, const char ** argv)
//------- Analytical algorithm

ComputeSpatialForceSecondOrderDerivatives(model, data, qs[_smooth], qdots[_smooth], qddots[_smooth],
d2f_dq2_ana, d2f_dv2_ana, d2f_dv2_fd, d2f_daq_ana);
d2f_dq2_ana, d2f_dv2_ana, d2f_daq_fd, d2f_daq_ana);


// Comparing the results
for (int i = 0; i < model.nv; ++i)
{

std::cout << "i = " << i << std::endl;
Eigen::Tensor<double,3> concrete_tensor = (d2f_dq2_fd.at(i) - d2f_dq2_ana.at(i)).eval();
auto diff_eq = tensorMax(concrete_tensor);

Eigen::Tensor<double,3> concrete_tensor_SO_v = (d2f_dv2_fd.at(i) - d2f_dv2_ana.at(i)).eval();
auto diff_dv = tensorMax(concrete_tensor_SO_v);

// auto diff_da = (d2f_da2_fd.at(i) - d2f_da2_ana.at(i)).norm();
// std::cout << "i = " << i << std::endl;
// std::cout << "d2f_dq2_fd.at(i) = \n" << d2f_dq2_fd.at(i) << std::endl;
Eigen::Tensor<double,3> concrete_tensor_SO_a = (d2f_da2_fd.at(i) - d2f_da2_ana.at(i)).eval();
auto diff_da = tensorMax(concrete_tensor_SO_a);

Eigen::Tensor<double,3> concrete_tensor_SO_aq = (d2f_daq_fd.at(i) - d2f_daq_ana.at(i)).eval();
auto diff_daq = tensorMax(concrete_tensor_SO_aq);

if (diff_eq > 1e-3)
{
std::cout << "diff SO-q \n" << std::endl;
// std::cout << "diff -------------= \n" << concrete_tensor << std::endl;
}

if (diff_dv > 1e-3)
{
std::cout << "diff SO-v \n" << std::endl;
}

// if (diff_da > 1e-3)
// {
// std::cout << "diff_da = " << diff_da << std::endl;
// }
if (diff_da > 1e-3)
{
std::cout << "diff SO-a = " << diff_da << std::endl;

}

if (diff_daq > 1e-3)
{
std::cout << "diff SO-aq \n" << std::endl;

}
}

}
Expand Down
37 changes: 29 additions & 8 deletions include/pinocchio/algorithm/spatial-force-second-order-derivatives.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,8 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep

std::vector<Tensor1> &d2fc_dqdq_ = const_cast<std::vector<Tensor1> &>(d2fc_dqdq);
std::vector<Tensor2> &d2fc_dvdv_ = const_cast<std::vector<Tensor2> &>(d2fc_dvdv);
std::vector<Tensor2> &d2fc_dada_ = const_cast<std::vector<Tensor2> &>(d2fc_dada);
std::vector<Tensor2> &d2fc_dadq_ = const_cast<std::vector<Tensor2> &>(d2fc_dadq);

Vector6c u1;
Vector6c u2;
Expand Down Expand Up @@ -234,12 +236,8 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep

while (j > 0) {

// std::cout << "j = " << j << std::endl;
// std::cout << "j_idx = " << j_idx << std::endl;

for (int q = 0; q < model.nvs[j]; q++) {
const Eigen::DenseIndex jq = model.idx_vs[j] + q;
// std::cout << "jq = " << jq << std::endl;
const MotionRef<typename Data::Matrix6x::ColXpr> S_j = data.J.col(jq);
const MotionRef<typename Data::Matrix6x::ColXpr> psid_dj = data.psid.col(jq); // psi_dot{j}(:,q)
const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dj = data.psidd.col(jq); // psi_ddot{j}(:,q)
Expand Down Expand Up @@ -307,8 +305,6 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep
k_idx = model.idx_vs[k];

while (k > 0) {
// std::cout << "k = " << k << std::endl;
// std::cout << "k_idx = " << k_idx << std::endl;

for (int r = 0; r < model.nvs[k]; r++) {
const Eigen::DenseIndex kr = model.idx_vs[k] + r;
Expand All @@ -329,7 +325,11 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep
// expr-1 SO-q
tmp_vec = s3*psid_dk.toVector() + ICi_St*psidd_dk.toVector() - crfSk.transpose() * s2;
hess_assign(d2fc_dqdq_.at(i_idx), tmp_vec , 0, jq, kr, 1, 6); // d2fc_dqdq_(i)(1:6, jq, kr)


// expr-1 SO-aq
// d2fc_daq{i}(:, jj(t), kk(r)) = crfSr * s4;
tmp_vec.noalias() = -crfSk.transpose() * s4;
hess_assign(d2fc_dadq_.at(i_idx), tmp_vec, 0, jq, kr, 1, 6); // d2fc_dadq_(i)(1:6, jq, kr)

if (j != i) { // k <= j < i

Expand All @@ -341,7 +341,6 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep
// expr-6 SO-q
hess_assign(d2fc_dqdq_.at(j_idx), tmp_vec, 0, ip, kr, 1, 6); // d2fc_dqdq_(j)(1:6, ip, kr)


// expr-7 SO-v
// d2fc_dv{j}(:, kk(r), ii(p)) = Bic_phii*S_r;
tmp_vec.noalias() = Bicphii * S_k.toVector();
Expand All @@ -351,6 +350,15 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep
// d2fc_dv{j}(:, ii(p), kk(r)) = d2fc_dv{j}(:, kk(r), ii(p));
hess_assign(d2fc_dvdv_.at(j_idx), tmp_vec, 0, ip, kr, 1, 6); // d2fc_dvdv_(j)(1:6, ip, kr)

// % expr-2 SO-aq
// d2fc_daq{j}(:, ii(p), kk(r)) = crfSr * u2;
tmp_vec.noalias() = -crfSk.transpose() * u2;
hess_assign(d2fc_dadq_.at(j_idx), tmp_vec, 0, ip, kr, 1, 6); // d2fc_dadq_(j)(1:6, ip, kr)

// expr-5 SO-aq
// d2fc_daq{j}(:, kk(r), ii(p)) = ICi_Sp * S_r;
tmp_vec.noalias() = ICi_Sp * S_k.toVector();
hess_assign(d2fc_dadq_.at(j_idx), tmp_vec, 0, kr, ip, 1, 6); // d2fc_dadq_(j)(1:6, kr, ip)
}

if (k != j) { // k < j <= i
Expand All @@ -375,6 +383,15 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep
// d2fc_dv{i}(:, kk(r), jj(t)) = d2fc_dv{i}(:, jj(t), kk(r));
hess_assign(d2fc_dvdv_.at(i_idx), tmp_vec, 0, kr, jq, 1, 6); // d2fc_dvdv_(i)(1:6, kr, jq)

// expr-3 SO-aq
// d2fc_daq{i}(:, kk(r), jj(t)) = ICi_St * S_r;
tmp_vec.noalias() = ICi_St * S_k.toVector();
hess_assign(d2fc_dadq_.at(i_idx), tmp_vec, 0, kr, jq, 1, 6); // d2fc_dadq_(i)(1:6, kr, jq)

// % expr-4 SO-aq
// d2fc_daq{k}(:, ii(p), jj(t)) = s8;
hess_assign(d2fc_dadq_.at(k_idx), s8, 0, ip, jq, 1, 6); // d2fc_dadq_(k)(1:6, ip, jq)

if (j != i) { // k < j < i
// expr-4 SO-q d2fc_dq{k}(:, jj(t), ii(p)) = d2fc_dq{k}(:, ii(p), jj(t));
get_vec_from_tens3_v1_gen(d2fc_dqdq_.at(k_idx), tmp_vec, 6 , ip, jq);
Expand All @@ -390,6 +407,10 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep
hess_assign(d2fc_dvdv_.at(k_idx), s7, 0, jq, ip, 1, 6); // d2fc_dvdv_(k)(1:6, jq, ip)


// % expr-6 SO-aq
// d2fc_daq{k}(:, jj(t), ii(p)) = s9;
hess_assign(d2fc_dadq_.at(k_idx), s9, 0, jq, ip, 1, 6); // d2fc_dadq_(k)(1:6, jq, ip)

} else { // k < j = i
// expr-6 SO-v
// d2fc_dv{k}(:, ii(p), jj(t)) = s12;
Expand Down

0 comments on commit 2cd3d73

Please sign in to comment.