Skip to content

Commit

Permalink
added SO v/q, q/v partials
Browse files Browse the repository at this point in the history
  • Loading branch information
@shubhamsingh91
shubhamsingh91 committed Jan 21, 2025
1 parent 3c237f1 commit 1766123
Show file tree
Hide file tree
Showing 4 changed files with 31 additions and 27 deletions.
2 changes: 1 addition & 1 deletion benchmark/model.txt
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
../models/ur3_robot.urdf
../models/atlas.urdf
Binary file modified benchmark/spatial_force_body
View file
Binary file not shown.
25 changes: 13 additions & 12 deletions benchmark/test_spatial_force_body.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -451,10 +451,12 @@ int main(int argc, const char ** argv)
for (int i = 0; i < model.nv; ++i)
{

// std::cout << "i = " << i << std::endl;
// 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);

Expand All @@ -470,16 +472,15 @@ int main(int argc, const char ** argv)
Eigen::Tensor<double,3> concrete_tensor_SO_qa = (d2f_dqa_fd.at(i) - d2f_dqa_ana.at(i)).eval();
auto diff_dqa = tensorMax(concrete_tensor_SO_qa);

// if (diff_eq > 1e-3)
// {
// std::cout << "diff SO-q \n" << std::endl;
// // throw std::runtime_error("Error in SO-q");

if (diff_eq > 1e-3)
{
std::cout << "diff SO-q \n" << std::endl;
throw std::runtime_error("Error in SO-q");

} else
{
std::cout << "SO-q is correct max diff_eq = " << diff_eq << std::endl;
}
// } else
// {
// std::cout << "SO-q is correct max diff_eq = " << diff_eq << std::endl;
// }

if (diff_dv > 1e-3)
{
Expand All @@ -503,7 +504,7 @@ int main(int argc, const char ** argv)
if (diff_dvq > 1e-3)
{
std::cout << "diff SO-vq \n" << std::endl;
throw std::runtime_error("Error in SO-vq");
// throw std::runtime_error("Error in SO-vq");
} else
{
std::cout << "SO-vq is correct max diff_dvq = " << diff_dvq << std::endl;
Expand Down
31 changes: 17 additions & 14 deletions include/pinocchio/algorithm/spatial-force-second-order-derivatives.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,9 +140,6 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep
const JointIndex i_idx = jmodel.idx_v();
JointIndex j_idx, k_idx;
const JointIndex parent = model.parents[i];
// std::cout << "------------------------------------" << std::endl;
// std::cout << "i = " << i << std::endl;
// std::cout << "i_idx = " << i_idx << std::endl;

const Inertia &oYcrb = data.oYcrb[i]; // IC{i}
const Matrix6 &oBcrb = data.doYcrb[i]; // BC{i}
Expand Down Expand Up @@ -331,9 +328,9 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep
// expr-1 SO-vq
//d2fc_dvq{i}(:, jj(t), kk(r)) =(Bic_psikr_dot + crfSr*BCi + 2*ICi*crmPsidr)*S_t + crfSr*s5;
r1 = Bic_psikt_dot + crfSk * oBcrb + 2.0 * oYcrb.matrix() * crmpsidk;
tmp_vec.noalias() = r1 * Sj_vec+ crfSk * s5;

slice_in_v6(d2fc_dvdq_.at(i_idx), tmp_vec, jq, kr); // d2fc_dvdq_(i)(1:6, jq, kr)
tmp_vec.noalias() = r1 * Sj_vec+ crfSk * s5 - crfSk * (oYcrb.matrix() * (psid_dj + phid_dj).toVector() + oBcrb * Sj_vec);
ftmp1.toVector() = tmp_vec;
slice_in_v6(d2fc_dvdq_.at(i_idx), data.oMi[i].actInv(ftmp1).toVector(), jq, kr); // d2fc_dvdq_(i)(1:6, jq, kr)

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

Expand Down Expand Up @@ -369,14 +366,16 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep

// expr-2 SO-vq
// d2fc_dvq{j}(:, ii(p), kk(r)) = (Bic_psikr_dot + crfSr*BCi + 2*ICi*crmPsidr)*S_p + crfSr*u1;
tmp_vec.noalias() = r1 * Si_vec + crfSk * u1;
slice_in_v6(d2fc_dvdq_.at(j_idx), tmp_vec, ip, kr); // d2fc_dvdq_(j)(1:6, ip, kr)
tmp_vec.noalias() = r1 * Si_vec + crfSk * u1 -
crfSk * (oYcrb.matrix() * (psid_dm + phid_dm).toVector() + oBcrb * S_i.toVector());
ftmp1.toVector() = tmp_vec;
slice_in_v6(d2fc_dvdq_.at(j_idx), data.oMi[j].actInv(ftmp1).toVector(), ip, kr); // d2fc_dvdq_(j)(1:6, ip, kr)

// expr-5 SO-vq
// d2fc_dvq{j}(:, kk(r), ii(p)) = u3*S_r + ICi_Sp*(psid_r + Sd_r);
tmp_vec.noalias() = u3 * S_k.toVector() + ICi_Sp * (psid_dk + phid_dk).toVector();
slice_in_v6(d2fc_dvdq_.at(j_idx), tmp_vec, kr, ip); // d2fc_dvdq_(j)(1:6, kr, ip)

ftmp1.toVector() = tmp_vec;
slice_in_v6(d2fc_dvdq_.at(j_idx), data.oMi[j].actInv(ftmp1).toVector(), kr, ip); // d2fc_dvdq_(j)(1:6, kr, ip)
}

if (k != j) { // k < j <= i
Expand Down Expand Up @@ -414,12 +413,15 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep

// expr-3 SO-vq
// d2fc_dvq{i}(:, kk(r), jj(t)) = s3*S_r + ICi_St*(psid_r + Sd_r);
tmp_vec.noalias() = s3 * S_k.toVector() + ICi_St * (psid_dk + phid_dk).toVector();
slice_in_v6(d2fc_dvdq_.at(i_idx), tmp_vec, kr, jq); // d2fc_dvdq_(i)(1:6, kr, jq)
tmp_vec.noalias() = s3 * S_k.toVector() + ICi_St * (psid_dk + phid_dk).toVector() -
crfSt * (oYcrb.matrix() * (psid_dk + phid_dk).toVector() + oBcrb * S_k.toVector());
ftmp1.toVector() = tmp_vec;
slice_in_v6(d2fc_dvdq_.at(i_idx), data.oMi[i].actInv(ftmp1).toVector(), kr, jq); // d2fc_dvdq_(i)(1:6, kr, jq)

// expr-4 SO-vq
// d2fc_dvq{k}(:, ii(p), jj(t)) = s10;
slice_in_v6(d2fc_dvdq_.at(k_idx), s10, ip, jq); // d2fc_dvdq_(k)(1:6, ip, jq)
ftmp1.toVector() = s10;
slice_in_v6(d2fc_dvdq_.at(k_idx), data.oMi[k].actInv(ftmp1).toVector(), ip, jq); // d2fc_dvdq_(k)(1:6, ip, jq)


if (j != i) { // k < j < i
Expand All @@ -445,7 +447,8 @@ struct ComputeSpatialForceSecondOrderDerivativesBackwardStep

// expr-6 SO-vq
// d2fc_dvq{k}(:, jj(t), ii(p)) = s11;
slice_in_v6(d2fc_dvdq_.at(k_idx), s11, jq, ip); // d2fc_dvdq_(k)(1:6, jq, ip)
ftmp1.toVector() = s11;
slice_in_v6(d2fc_dvdq_.at(k_idx), data.oMi[k].actInv(ftmp1).toVector(), jq, ip); // d2fc_dvdq_(k)(1:6, jq, ip)

} else { // k < j = i
// expr-6 SO-v
Expand Down

0 comments on commit 1766123

Please sign in to comment.