multiple-contacts.hpp

Go to the documentation of this file.

// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef SOBEC_MULTIPLE_CONTACTS_HPP_
#define SOBEC_MULTIPLE_CONTACTS_HPP_
 
#include <crocoddyl/core/utils/exception.hpp>
#include <crocoddyl/multibody/contact-base.hpp>
#include <crocoddyl/multibody/contacts/multiple-contacts.hpp>
#include <crocoddyl/multibody/fwd.hpp>
#include <map>
#include <set>
#include <string>
#include <utility>
 
#include "sobec/crocomplements/contact/contact1d.hpp"
#include "sobec/crocomplements/contact/contact3d.hpp"
#include "sobec/crocomplements/contact/contact6d.hpp"
#include "sobec/fwd.hpp"
 
namespace sobec {
namespace newcontacts {
 
template <typename _Scalar>
class ContactModelMultipleTpl
    : public crocoddyl::ContactModelMultipleTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef crocoddyl::ContactModelMultipleTpl<Scalar> Base;
  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
  typedef crocoddyl::StateMultibodyTpl<Scalar> StateMultibody;
  typedef crocoddyl::ContactDataAbstractTpl<Scalar> ContactDataAbstract;
  typedef crocoddyl::ContactDataMultipleTpl<Scalar> ContactDataMultiple;
  typedef crocoddyl::ContactModelAbstractTpl<Scalar> ContactModelAbstract;
 
  typedef crocoddyl::ContactItemTpl<Scalar> ContactItem;
 
  typedef typename MathBase::Vector2s Vector2s;
  typedef typename MathBase::Vector3s Vector3s;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
  typedef typename MathBase::Matrix3s Matrix3s;
 
  typedef std::map<std::string, boost::shared_ptr<ContactItem> >
      ContactModelContainer;
  typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> >
      ContactDataContainer;
  typedef typename pinocchio::container::aligned_vector<
      pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
 
  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state,
                          const std::size_t nu);
 
  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
  ~ContactModelMultipleTpl();
 
  void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data,
                       const boost::shared_ptr<MatrixXs> df_dx,
                       const boost::shared_ptr<MatrixXs> df_du) const;
 
  void updateRneaDerivatives(const boost::shared_ptr<ContactDataMultiple>& data,
                             pinocchio::DataTpl<Scalar>& pinocchio) const;
 
  //   MatrixXs rotateJacobians(const boost::shared_ptr<MatrixXs> Jin);
};
 
}  // namespace newcontacts
}  // namespace sobec
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "sobec/crocomplements/contact/multiple-contacts.hxx"
 
#endif  // SOBEC_MULTIPLE_CONTACTS_HPP_