Skip to content

Commit

Permalink
wrking FO partials of cumulative spatial-force
Browse files Browse the repository at this point in the history
  • Loading branch information
@shubhamsingh91
shubhamsingh91 committed Dec 26, 2024
1 parent aba6d2f commit 20e1884
Show file tree
Hide file tree
Showing 3 changed files with 226 additions and 112 deletions.
157 changes: 125 additions & 32 deletions benchmark/test_spatial_force.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,30 +32,32 @@
*/


void calc_df_dq(const pinocchio::Model & model, pinocchio::Data & data_fd,
void calc_df(const pinocchio::Model & model, pinocchio::Data & data_fd,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
Eigen::Tensor<double,3> & df_dq)
Eigen::Tensor<double,3> & df_dq,
Eigen::Tensor<double,3> & df_dv,
Eigen::Tensor<double,3> & df_da)

{
using namespace Eigen;
VectorXd v_eps(VectorXd::Zero(model.nv));
VectorXd q_plus(model.nq);
VectorXd tau_plus(model.nv);
const double alpha = 1e-8;

// df/dq
computeRNEADerivatives(model,data_fd,q,v,a);
auto f_vec_orig = data_fd.of; // std::vector

// df/dq

for(int i = 0; i < model.nv; ++i)
{
for (int j = 0; j < model.nv; j++)
for (int j = 0; j < model.nv; ++j)
{
v_eps[j] += alpha;
integrate(model,q,v_eps,q_plus);
computeRNEADerivativesFaster(model,data_fd,q_plus,v,a);
computeRNEADerivatives(model,data_fd,q_plus,v,a);

auto f_vec_plus = data_fd.of;

Expand All @@ -65,9 +67,45 @@ void calc_df_dq(const pinocchio::Model & model, pinocchio::Data & data_fd,
v_eps[j] -= alpha;

}
}

// df/dv
VectorXd v_plus(v);
for(int i = 0; i < model.nv; ++i)
{
for (int j = 0; j < model.nv; ++j)
{
v_plus[j] += alpha;
computeRNEADerivatives(model,data_fd,q,v_plus,a);

auto f_vec_plus = data_fd.of;

auto vec6 = (f_vec_plus.at(i+1).toVector()-f_vec_orig.at(i+1).toVector())/alpha; // dfci_dqj
pinocchio::tens_assign6_col(df_dv, vec6, j, i); // assigning derivative w.r.t qj along the columns

v_plus[j] -= alpha;

}
}

// df/dv
VectorXd a_plus(a);
for(int i = 0; i < model.nv; ++i)
{
for (int j = 0; j < model.nv; ++j)
{
a_plus[j] += alpha;
computeRNEADerivatives(model,data_fd,q,v,a_plus);

auto f_vec_plus = data_fd.of;

auto vec6 = (f_vec_plus.at(i+1).toVector()-f_vec_orig.at(i+1).toVector())/alpha; // dfci_dqj
pinocchio::tens_assign6_col(df_da, vec6, j, i); // assigning derivative w.r.t qj along the columns

a_plus[j] -= alpha;

}
}
}


Expand All @@ -87,6 +125,7 @@ int main(int argc, const char ** argv)
Model model;

//std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
// std::string filename = std::string("../models/double_pendulum.urdf");
std::string filename = std::string("../models/simple_humanoid.urdf");

if(argc>1) filename = argv[1];
Expand All @@ -111,7 +150,7 @@ int main(int argc, const char ** argv)
std::cout << "nv = " << model.nv << std::endl;

Data data(model);
VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
VectorXd qmax = Eigen::VectorXd::Random(model.nq);

PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qs (NBT);
PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qdots (NBT);
Expand All @@ -121,10 +160,10 @@ int main(int argc, const char ** argv)
for(size_t i=0;i<NBT;++i)
{
// qs[i] = randomConfiguration(model,-qmax,qmax);
qs[i] = Eigen::VectorXd::Ones(model.nv);
qdots[i] = Eigen::VectorXd::Ones(model.nv);
qddots[i] = Eigen::VectorXd::Ones(model.nv);
taus[i] = Eigen::VectorXd::Ones(model.nv);
qs[i] = Eigen::VectorXd::Random(model.nv);
qdots[i] = Eigen::VectorXd::Random(model.nv);
qddots[i] = Eigen::VectorXd::Random(model.nv);
taus[i] = Eigen::VectorXd::Random(model.nv);
}

PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dq(MatrixXd::Zero(model.nv,model.nv));
Expand All @@ -144,48 +183,102 @@ int main(int argc, const char ** argv)


// Finite-diff of df_dq
pinocchio::DataTpl<double>::Matrix6x df_dq_fd, df_dq_ana ;
pinocchio::DataTpl<double>::Matrix6x df_dq_fd, df_dq_ana, df_dq_algo ;
Eigen::Tensor<double, 3> df_dq_fd_tensor (6,model.nv, model.nv);
Eigen::Tensor<double, 3> df_dv_fd_tensor (6,model.nv, model.nv);
Eigen::Tensor<double, 3> df_da_fd_tensor (6,model.nv, model.nv);

Eigen::Tensor<double, 3> df_dq_ana_tensor (6,model.nv, model.nv);
Eigen::Tensor<double, 3> df_dv_ana_tensor (6,model.nv, model.nv);
Eigen::Tensor<double, 3> df_da_ana_tensor (6,model.nv, model.nv);

df_dq_fd.resize(6,model.nv);
df_dq_ana.resize(6,model.nv);
df_dq_algo.resize(6,model.nv);

std::cout << "---------- Running finite-diff --------------- " << std::endl;
calc_df_dq(model, data, qs[_smooth], qdots[_smooth], qddots[_smooth], df_dq_fd_tensor);
calc_df(model, data, qs[_smooth], qdots[_smooth], qddots[_smooth],
df_dq_fd_tensor, df_dv_fd_tensor, df_da_fd_tensor);

std::cout << "---------- Running Analytical --------------- " << std::endl;
computeSpatialForceDerivs(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth],
df_dq_ana_tensor);
df_dq_ana_tensor, df_dv_ana_tensor, df_da_ana_tensor);


std::cout << "------------ Comparing df_dq----------------------" << std::endl;

for (int i = 0; i< model.nv; i++)
{
for (int j = 0; j < model.nv; j++)
{
// std::cout << " j = " << j << std::endl;

pinocchio::DataTpl<double>::Vector6c temp, temp_ana;
pinocchio::hess_get(df_dq_fd_tensor,temp, 0,j,i,1,6);
pinocchio::hess_get(df_dq_ana_tensor,temp_ana,0,j,i,1,6); // dfic_dqi

auto diff = (temp - temp_ana).norm();

if (diff > 1e-3)
{
std::cout << " i = " << i << std::endl;
std::cout << " j = " << j << std::endl;
std::cout << "diff between fd and analytical = " << diff << std::endl;
}
}
}

std::cout << "------------ Comparing df_dv----------------------" << std::endl;
for (int i = 0; i< model.nv; i++)
{
std::cout << "---------------------------------------------" << std::endl;
std::cout << " i = " << i << std::endl;
pinocchio::DataTpl<double>::Vector6c temp, temp_ana;

pinocchio::hess_get(df_dq_fd_tensor,temp,0,i,i,1,6);
pinocchio::hess_get(df_dq_ana_tensor,temp_ana,0,i,i,1,6); // dfic_dqi

df_dq_fd.col(i) = temp;
df_dq_ana.col(i) = temp_ana;
auto diff = (temp - temp_ana).norm();


std::cout << "diff between fd and analytical = " << (temp - temp_ana).norm() << std::endl;
std::cout << "df = " << temp.transpose() << std::endl;
std::cout << "df_ana = " << temp_ana.transpose() << std::endl;

// std::cout << " i = " << i << std::endl;
for (int j = 0; j < model.nv; j++)
{

pinocchio::DataTpl<double>::Vector6c temp, temp_ana;
pinocchio::hess_get(df_dv_fd_tensor,temp, 0,j,i,1,6);
pinocchio::hess_get(df_dv_ana_tensor,temp_ana,0,j,i,1,6); // dfic_dqi

auto diff = (temp - temp_ana).norm();

if (diff > 1e-3)
{
std::cout << " i = " << i << std::endl;
std::cout << " j = " << j << std::endl;
std::cout << "diff between fd and analytical = " << diff << std::endl;
}
}
}

std::cout << "------------ Comparing df_da----------------------" << std::endl;
for (int i = 0; i< model.nv; i++)
{
// std::cout << " i = " << i << std::endl;
for (int j = 0; j < model.nv; j++)
{

pinocchio::DataTpl<double>::Vector6c temp, temp_ana;
pinocchio::hess_get(df_da_fd_tensor,temp, 0,j,i,1,6);
pinocchio::hess_get(df_da_ana_tensor,temp_ana,0,j,i,1,6); // dfic_dqi

auto diff = (temp - temp_ana).norm();

if (diff > 1e-3)
{
std::cout << " i = " << i << std::endl;
std::cout << " j = " << j << std::endl;
std::cout << "diff between fd and analytical = " << diff << std::endl;
}
}
}

std::cout << "diff mat = " << (df_dq_fd - data.dFdq).norm() << std::endl;
// std::cout << "diff mat = " << (df_dq_fd - data.dFdq).norm() << std::endl;




}


std::cout << "--" << std::endl;
return 0;
}
14 changes: 7 additions & 7 deletions include/pinocchio/algorithm/spatial-force-derivatives.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,17 +20,15 @@ namespace pinocchio
/// \tparam ConfigVectorType Type of the joint configuration vector.
/// \tparam TangentVectorType1 Type of the joint velocity vector.
/// \tparam TangentVectorType2 Type of the joint acceleration vector.
/// \tparam MatrixType1 Type of the matrix containing the partial derivative with respect to the joint configuration vector.
/// \tparam MatrixType2 Type of the matrix containing the partial derivative with respect to the joint velocity vector.
/// \tparam MatrixType3 Type of the matrix containing the partial derivative with respect to the joint acceleration vector.
///

/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] v The joint velocity vector (dim model.nv).
/// \param[in] a The joint acceleration vector (dim model.nv).
/// \param[out] df_dq Partial derivative of the generalized torque vector with respect to the joint configuration.
///
/// \param[out] df_dq Partial derivative of the cumulative-spatial-force with respect to the joint configuration.
/// \param[out] df_dv Partial derivative of the cumulative-spatial-force with respect to the joint velocity.
/// \param[out] df_da Partial derivative of the cumulative-spatial-force with respect to the joint acceleration.

/// \sa pinocchio::rnea
///
Expand All @@ -41,7 +39,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a,
Eigen::Tensor<double,3> & df_dq);
Eigen::Tensor<double,3> & df_dq,
Eigen::Tensor<double,3> & df_dv,
Eigen::Tensor<double,3> & df_da);

///
} // namespace pinocchio
Expand Down
Loading

0 comments on commit 20e1884

Please sign in to comment.