diff --git a/src/algorithm/contact-cholesky.hxx b/src/algorithm/contact-cholesky.hxx
index 4f511d0571e569f6f1794645897b2a7b4090d438..b5ff3988ab363f1be1980ac11b6bd9be537da2e2 100644
--- a/src/algorithm/contact-cholesky.hxx
+++ b/src/algorithm/contact-cholesky.hxx
@@ -86,7 +86,7 @@ namespace pinocchio
for(size_t ee_id = 0; ee_id < extented_parents_fromRow.size(); ++ee_id)
{
BooleanVector & indexes = extented_parents_fromRow[ee_id];
- const JointIndex joint_id = model.frames[contact_infos[ee_id].parent].parent;
+ const JointIndex joint_id = model.frames[contact_infos[ee_id].frame_id].parent;
const typename Model::JointModel & joint = model.joints[joint_id];
Eigen::DenseIndex current_id = joint.idx_v() + joint.nv() - 1 + num_total_constraints;
@@ -123,6 +123,23 @@ namespace pinocchio
const typename Data::MatrixXs & M = data.M;
const size_t num_ee = contact_infos.size();
+
+ // Update frame placements if needed
+ for(size_t f = 0; f < num_ee; ++f)
+ {
+ if(contact_infos[f].reference_frame == WORLD) continue; // skip useless computations
+
+ const typename Model::FrameIndex & parent_frame_id = contact_infos[f].frame_id;
+ const typename Model::Frame & frame = model.frames[parent_frame_id];
+ typename Data::SE3 & oMf = data.oMf[parent_frame_id];
+
+ const typename Model::JointIndex & parent_joint_id = model.frames[parent_frame_id].parent;
+
+ oMf = data.oMi[parent_joint_id] * frame.placement;
+ }
+
+ // Core
+ Motion Jcol_motion;
for(Eigen::DenseIndex j=nv-1;j>=0;--j)
{
// Classic Cholesky decomposition related to the mass matrix
@@ -161,16 +178,62 @@ namespace pinocchio
{
switch(cinfo.type)
{
- case CONTACT_3D:
- for(Eigen::DenseIndex _i = 0; _i < contact_dim<CONTACT_3D>::value; _i++)
+ case WORLD:
+ {
+ typedef typename Data::Matrix6x::ColXpr ColXpr;
+ ColXpr Jcol = data.J.col(j);
+ MotionRef<ColXpr> Jcol_motion(Jcol);
+
+ switch(cinfo.type)
+ {
+ case CONTACT_3D:
+ for(Eigen::DenseIndex _i = 0; _i < contact_dim<CONTACT_3D>::value; _i++)
+ {
+ const Eigen::DenseIndex _ii = current_row - _i;
+ U(_ii,jj) = (Jcol_motion.linear()[contact_dim<CONTACT_3D>::value-_i-1] - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj];
+ }
+ break;
+
+ case CONTACT_6D:
+ for(Eigen::DenseIndex _i = 0; _i < contact_dim<CONTACT_6D>::value; _i++)
+ {
+ const Eigen::DenseIndex _ii = current_row - _i;
+ U(_ii,jj) = (Jcol_motion.toVector()[contact_dim<CONTACT_6D>::value-_i-1] - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj];
+ }
+ break;
+
+ default:
+ assert(false && "Must never happened");
+ }
+ break;
+ } // end case WORLD
+ case LOCAL:
+ {
+ typedef typename Data::Matrix6x::ColXpr ColXpr;
+ ColXpr Jcol = data.J.col(j);
+ MotionRef<ColXpr> Jcol_motion(Jcol);
+
+ const typename Data::SE3 & oMf = data.oMf[cinfo.frame_id];
+ Motion Jcol_local(oMf.actInv(Jcol_motion));
+
+ switch(cinfo.type)
{
const Eigen::DenseIndex _ii = current_row - _i;
U(_ii,jj) = (data.J(contact_dim<CONTACT_3D>::value-_i-1 + LINEAR,j) - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj];
}
break;
-
- case CONTACT_6D:
- for(Eigen::DenseIndex _i = 0; _i < contact_dim<CONTACT_6D>::value; _i++)
+ } // end case LOCAL
+ case LOCAL_WORLD_ALIGNED:
+ {
+ typedef typename Data::Matrix6x::ColXpr ColXpr;
+ ColXpr Jcol = data.J.col(j);
+ MotionRef<ColXpr> Jcol_motion(Jcol);
+
+ const typename Data::SE3 & oMf = data.oMf[cinfo.frame_id];
+ Motion Jcol_local_world_aligned(Jcol_motion);
+ Jcol_local_world_aligned.linear() -= oMf.translation().cross(Jcol_local_world_aligned.angular());
+
+ switch(cinfo.type)
{
const Eigen::DenseIndex _ii = current_row - _i;
U(_ii,jj) = (data.J(contact_dim<CONTACT_6D>::value-_i-1,j) - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj];
diff --git a/src/algorithm/contact-dynamics.hxx b/src/algorithm/contact-dynamics.hxx
index 364f6d57f34c27013309087ba570f0de7ee6a09e..79b8a7c04ab5a2780c78a9765a0f4e5f28d479ab 100644
--- a/src/algorithm/contact-dynamics.hxx
+++ b/src/algorithm/contact-dynamics.hxx
@@ -74,7 +74,7 @@ namespace pinocchio
// Check termination
if(max_it > 1)
- std::cout << "data.diff_lambda_c.template lpNorm<Eigen::Infinity>():\n" << data.diff_lambda_c.template lpNorm<Eigen::Infinity>() << std::endl;
+// std::cout << "data.diff_lambda_c.template lpNorm<Eigen::Infinity>():\n" << data.diff_lambda_c.template lpNorm<Eigen::Infinity>() << std::endl;
if(data.diff_lambda_c.template lpNorm<Eigen::Infinity>() <= prox_settings.threshold)
break;
}
@@ -157,6 +157,183 @@ namespace pinocchio
return forwardDynamics(model,data,tau,J,gamma,mu);
}
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, class Allocator>
+ inline void initContactDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ DataTpl<Scalar,Options,JointCollectionTpl> & data,
+ const std::vector<ContactInfoTpl<Scalar,Options>,Allocator> & contact_infos)
+ {
+ data.contact_chol.allocate(model,contact_infos);
+ data.contact_vector_solution.resize(data.contact_chol.dim());
+ data.contact_forces.resize(contact_infos.size());
+ }
+
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, class Allocator>
+ inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
+ contactDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ DataTpl<Scalar,Options,JointCollectionTpl> & data,
+ const Eigen::MatrixBase<ConfigVectorType> & q,
+ const Eigen::MatrixBase<TangentVectorType1> & v,
+ const Eigen::MatrixBase<TangentVectorType2> & tau,
+ const std::vector<ContactInfoTpl<Scalar,Options>,Allocator> & contact_infos,
+ const Scalar mu)
+ {
+ assert(q.size() == model.nq);
+ assert(v.size() == model.nv);
+ assert(tau.size() == model.nv);
+ assert(model.check(data) && "data is not consistent with model.");
+
+ assert(mu >= (Scalar)0 && "mu must be positive.");
+
+ typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+ typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
+
+ typedef ContactInfoTpl<Scalar,Options> ContactInfo;
+ typedef std::vector<ContactInfo,Allocator> ContactInfoVector;
+
+ typename Data::TangentVectorType & a = data.ddq;
+ typename Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
+ typename Data::VectorXs & contact_vector_solution = data.contact_vector_solution;
+
+ computeAllTerms(model,data,q.derived(),v.derived());
+
+ // Computes the Cholesky decomposition
+ contact_chol.compute(model,data,contact_infos,mu);
+
+ contact_vector_solution.tail(model.nv) = tau - data.nle;
+
+ // Temporary variables
+ typename Data::SE3 oMcontact; // placement of the contact frame in the world frame
+ typename Data::SE3 iMcontact; // placement of the contact frame in the local frame of the joint
+ typename Data::Motion coriolis_centrifugal_acc; // Coriolis/centrifugal acceleration of the contact frame.
+ typename Motion::Vector3 coriolis_centrifugal_acc_local;
+
+ Eigen::DenseIndex current_row_id = 0;
+ for(typename ContactInfoVector::const_iterator it = contact_infos.begin();
+ it != contact_infos.end(); ++it)
+ {
+ const ContactInfo & contact_info = *it;
+ const int contact_dim = contact_info.dim();
+
+ const typename Model::FrameIndex & frame_id = contact_info.frame_id;
+ const typename Model::Frame & frame = model.frames[frame_id];
+ const typename Model::JointIndex & joint_id = frame.parent;
+ const typename Data::SE3 & oMi = data.oMi[joint_id];
+
+ // Update frame placement
+ iMcontact = frame.placement * contact_info.placement;
+// data.oMf[frame_id] = data.oMi[frame.parent] * frame.placement;
+ oMcontact = oMi * iMcontact;
+
+ classicAcceleration(data.v[joint_id],
+ data.a[joint_id],
+ iMcontact,
+ coriolis_centrifugal_acc_local);
+ // LINEAR
+ coriolis_centrifugal_acc.linear().noalias()
+ = oMcontact.rotation() * coriolis_centrifugal_acc_local;
+
+ // ANGULAR
+ coriolis_centrifugal_acc.angular().noalias()
+ = data.oMi[joint_id].rotation() * data.a[joint_id].angular();
+
+ switch(contact_info.reference_frame)
+ {
+ case WORLD:
+ {
+ data.ov[joint_id] = oMi.act(data.v[joint_id]);
+ data.oa[joint_id] = oMi.act(data.a[joint_id]);
+
+ // LINEAR
+ classicAcceleration(data.ov[joint_id],
+ data.oa[joint_id],
+ coriolis_centrifugal_acc.linear());
+ // ANGULAR
+ coriolis_centrifugal_acc.angular() = data.oa[joint_id].angular();
+
+ break;
+ }
+ case LOCAL_WORLD_ALIGNED:
+ {
+ // LINEAR
+ coriolis_centrifugal_acc.linear() = oMcontact.rotation() * coriolis_centrifugal_acc_local;
+ // ANGULAR
+ coriolis_centrifugal_acc.angular().noalias() = oMi.rotation() * data.a[joint_id].angular();
+
+ break;
+ }
+ case LOCAL:
+ {
+ // LINEAR
+ coriolis_centrifugal_acc.linear() = coriolis_centrifugal_acc_local;
+ // ANGULAR
+ coriolis_centrifugal_acc.angular().noalias() = iMcontact.rotation().transpose() * data.a[joint_id].angular();
+
+ break;
+ }
+ default:
+ assert(false && "must never happened");
+ break;
+ }
+
+ switch(contact_info.type)
+ {
+ case CONTACT_3D:
+ contact_vector_solution.segment(current_row_id,contact_dim) = -coriolis_centrifugal_acc.linear();
+ break;
+ case CONTACT_6D:
+ contact_vector_solution.segment(current_row_id,contact_dim) = -coriolis_centrifugal_acc.toVector();
+ break;
+ default:
+ assert(false && "must never happened");
+ break;
+ }
+
+ current_row_id += contact_dim;
+ }
+
+// std::cout << "contact_vector_solution:\n" << contact_vector_solution.head(12).transpose() << std::endl;
+ // Solve the system
+ contact_chol.solveInPlace(contact_vector_solution);
+
+ // Retrieve the joint space acceleration
+ a = contact_vector_solution.tail(model.nv);
+
+ // Retrieve the contact forces
+ size_t current_id = 0;
+ Eigen::DenseIndex current_row_sol_id = 0;
+ for(typename ContactInfoVector::const_iterator it = contact_infos.begin();
+ it != contact_infos.end(); ++it, current_id++)
+ {
+ typename Data::Force & fext = data.contact_forces[current_id];
+ const ContactInfo & contact_info = *it;
+ const int contact_dim = contact_info.dim();
+
+ switch(contact_info.type)
+ {
+ case CONTACT_3D:
+ {
+ fext.linear() = -contact_vector_solution.template segment<3>(current_row_sol_id);
+ fext.angular().setZero();
+ break;
+ }
+ case CONTACT_6D:
+ {
+ typedef typename Data::VectorXs::template FixedSegmentReturnType<6>::Type Segment6d;
+ const ForceRef<Segment6d> f_sol(contact_vector_solution.template segment<6>(current_row_sol_id));
+ fext = -f_sol;
+ break;
+ }
+ default:
+ assert(false && "must never happened");
+ break;
+ }
+
+ current_row_sol_id += contact_dim;
+ }
+
+ return a;
+ }
+
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
typename ConstraintMatrixType, typename KKTMatrixType>
inline void getKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
diff --git a/src/algorithm/contact-info.hpp b/src/algorithm/contact-info.hpp
index bd22145af191f4dcce9f52100430677eb7871060..54d881b6f237dfd6595ec2a3e4cbde40c13b48e4 100644
--- a/src/algorithm/contact-info.hpp
+++ b/src/algorithm/contact-info.hpp
@@ -53,7 +53,7 @@ namespace pinocchio
ContactType type;
/// \brief Index of the parent Frame in the model tree.
- FrameIndex parent;
+ FrameIndex frame_id;
/// \brief Relative placement with respect to the parent frame.
SE3 placement;
@@ -61,22 +61,25 @@ namespace pinocchio
///Â \brief Default constructor.
ContactInfoTpl()
: type(CONTACT_UNDEFINED)
- , parent(std::numeric_limits<FrameIndex>::max())
+ , frame_id(std::numeric_limits<FrameIndex>::max())
+ , reference_frame(WORLD)
{}
///
///Â \brief Contructor with from a given type, parent and placement.
///
/// \param[in] type Type of the contact.
- /// \param[in] parent Index of the parent Frame in the model tree.
- /// \param[in] placement Placement of the contact with respect to the parent Frame".
+ /// \param[in] frame_id Index of the parent Frame in the model tree.
+ /// \param[in] placement Placement of the contact with respect to the parent Frame.
+ /// \param[in] reference_frame Placement of the contact with respect to the parent Frame.
///
template<typename S2, int O2>
ContactInfoTpl(const ContactType type,
- const FrameIndex parent,
- const SE3Tpl<S2,O2> & placement)
+ const FrameIndex frame_id,
+ const SE3Tpl<S2,O2> & placement,
+ const ReferenceFrame & reference_frame = WORLD)
: type(type)
- , parent(parent)
+ , frame_id(frame_id)
, placement(placement)
{}
@@ -84,12 +87,13 @@ namespace pinocchio
///Â \brief Contructor with from a given type, parent and placement.
///
/// \param[in] type Type of the contact.
- /// \param[in] parent Index of the parent Frame in the model tree.
+ /// \param[in] frame_id Index of the parent Frame in the model tree.
///
ContactInfoTpl(const ContactType type,
- const FrameIndex parent)
+ const FrameIndex frame_id,
+ const ReferenceFrame & reference_frame = WORLD)
: type(type)
- , parent(parent)
+ , frame_id(frame_id)
, placement(SE3::Identity())
{}
@@ -105,8 +109,9 @@ namespace pinocchio
{
return
type == other.type
- && parent == other.parent
- && placement == other.placement;
+ && frame_id == other.frame_id
+ && placement == other.placement
+ && reference_frame == other.reference_frame;
}
///
diff --git a/unittest/contact-dynamics.cpp b/unittest/contact-dynamics.cpp
index dd3a54a48e6aa17618b1f84144d8047403731aac..3a33f9ae3c965b9c95d8d3a3b3beb32561e04662 100644
--- a/unittest/contact-dynamics.cpp
+++ b/unittest/contact-dynamics.cpp
@@ -32,14 +32,14 @@ BOOST_AUTO_TEST_CASE(contact_info)
SE3 M(SE3::Random());
ContactInfo ci2(CONTACT_3D,0,M);
BOOST_CHECK(ci2.type == CONTACT_3D);
- BOOST_CHECK(ci2.parent == 0);
+ BOOST_CHECK(ci2.frame_id == 0);
BOOST_CHECK(ci2.placement.isApprox(M));
BOOST_CHECK(ci2.dim() == 3);
// Check contructor with two arguments
ContactInfo ci2prime(CONTACT_3D,0);
BOOST_CHECK(ci2prime.type == CONTACT_3D);
- BOOST_CHECK(ci2prime.parent == 0);
+ BOOST_CHECK(ci2prime.frame_id == 0);
BOOST_CHECK(ci2prime.placement.isIdentity());
BOOST_CHECK(ci2prime.dim() == 3);
@@ -50,7 +50,7 @@ BOOST_AUTO_TEST_CASE(contact_info)
// Check complete constructor 6D
ContactInfo ci4(CONTACT_6D,0,SE3::Identity());
BOOST_CHECK(ci4.type == CONTACT_6D);
- BOOST_CHECK(ci4.parent == 0);
+ BOOST_CHECK(ci4.frame_id == 0);
BOOST_CHECK(ci4.placement.isIdentity());
BOOST_CHECK(ci4.dim() == 6);
}