Unverified Commit d7806483 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1474 from jcarpent/topic/centroidal

Fix bug in Centroidal derivatives
parents 4c834f20 73eb1e36
Pipeline #15365 passed with stage
in 128 minutes and 7 seconds
......@@ -271,7 +271,7 @@ class MeshcatVisualizer(BaseVisualizer):
T = np.array(M.homogeneous).dot(S)
else:
T = M.homogeneous
# Update viewer configuration.
self.viewer[visual_name].set_transform(T)
......
......@@ -285,8 +285,16 @@ namespace pinocchio
data.Ig.inertia() = Ytot.inertia();
// Compute the partial derivatives
translateForceSet(data.dHdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike0,dh_dq));
translateForceSet(data.dFdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dHdq,com,dh_dq.const_cast_derived());
Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived();
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
dh_dq_.col(k).template segment<3>(Force::ANGULAR) += data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass();
translateForceSet(data.dFdq,com,dhdot_dq.const_cast_derived());
Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived();
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass();
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da));
}
......@@ -403,8 +411,16 @@ namespace pinocchio
data.Ig.inertia() = Ytot.inertia();
// Retrieve the partial derivatives from RNEA derivatives
translateForceSet(data.dHdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike0,dh_dq));
translateForceSet(Ftmp,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dHdq,com,dh_dq.const_cast_derived());
Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived();
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
dh_dq_.col(k).template segment<3>(Force::ANGULAR) += data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass();
translateForceSet(Ftmp,com,dhdot_dq.const_cast_derived());
Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived();
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass();
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da));
}
......
......@@ -64,11 +64,11 @@ namespace pinocchio
*
*/
PINOCCHIO_DLLAPI void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
const std::istream& xmlStream,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs,
::hpp::fcl::MeshLoaderPtr meshLoader);
const std::istream& xmlStream,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs,
::hpp::fcl::MeshLoaderPtr meshLoader);
} // namespace details
......
......@@ -125,13 +125,9 @@ BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
const pinocchio::Force & dhg_plus
= pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q_plus,v,a);
const pinocchio::Force hg_plus = data_fd.hg;
const pinocchio::Force::Vector3 com_plus = data_fd.com[0];
pinocchio::SE3 transform(pinocchio::SE3::Identity());
transform.translation() = com_plus - com;
dhdot_dq_fd.col(k) = (transform.act(dhg_plus) - dhg).toVector()/eps;
dh_dq_fd.col(k) = (transform.act(hg_plus) - hg).toVector()/eps;
dhdot_dq_fd.col(k) = (dhg_plus - dhg).toVector()/eps;
dh_dq_fd.col(k) = (hg_plus - hg).toVector()/eps;
v_eps[k] = 0.;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment