surface_contact.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_SURFACE_CONTACT_HPP_
#define ROBOTOC_SURFACE_CONTACT_HPP_
 
#include "Eigen/Core"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/spatial/force.hpp"
 
#include "robotoc/robot/contact_model_info.hpp"
#include "robotoc/robot/se3.hpp"
 
#include <iostream>
 
 
namespace robotoc {
 
class SurfaceContact {
public:
  using Vector6d = Eigen::Matrix<double, 6, 1>;
  using Matrix66d = Eigen::Matrix<double, 6, 6>;
  using Matrix6xd = Eigen::Matrix<double, 6, Eigen::Dynamic>;
 
  SurfaceContact(const pinocchio::Model& model, 
                 const ContactModelInfo& contact_model_info); 
 
  SurfaceContact();
 
  ~SurfaceContact() = default;
 
  SurfaceContact(const SurfaceContact&) = default;
 
  SurfaceContact& operator=(const SurfaceContact&) = default;
 
  SurfaceContact(SurfaceContact&&) noexcept = default;
 
  SurfaceContact& operator=(SurfaceContact&&) noexcept = default;
 
  void computeJointForceFromContactWrench(
      const Vector6d& contact_wrench, 
      pinocchio::container::aligned_vector<pinocchio::Force>& joint_forces) const;
 
  template <typename VectorType>
  void computeBaumgarteResidual(
      const pinocchio::Model& model, const pinocchio::Data& data, 
      const SE3& desired_contact_placement,
      const Eigen::MatrixBase<VectorType>& baumgarte_residual);
 
  template <typename MatrixType1, typename MatrixType2, typename MatrixType3>
  void computeBaumgarteDerivatives(
      const pinocchio::Model& model, pinocchio::Data& data, 
      const Eigen::MatrixBase<MatrixType1>& baumgarte_partial_dq, 
      const Eigen::MatrixBase<MatrixType2>& baumgarte_partial_dv, 
      const Eigen::MatrixBase<MatrixType3>& baumgarte_partial_da);
 
  template <typename VectorType>
  void computeContactVelocityResidual(
      const pinocchio::Model& model, const pinocchio::Data& data, 
      const Eigen::MatrixBase<VectorType>& velocity_residual) const;
 
  template <typename MatrixType1, typename MatrixType2>
  void computeContactVelocityDerivatives(
      const pinocchio::Model& model, pinocchio::Data& data,
      const Eigen::MatrixBase<MatrixType1>& velocity_partial_dq, 
      const Eigen::MatrixBase<MatrixType2>& velocity_partial_dv);
 
  template <typename VectorType>
  void computeContactPositionResidual(
      const pinocchio::Model& model, const pinocchio::Data& data, 
      const SE3& desired_contact_placement,
      const Eigen::MatrixBase<VectorType>& position_residual);
 
  template <typename MatrixType>
  void computeContactPositionDerivative(
      const pinocchio::Model& model, pinocchio::Data& data,
      const Eigen::MatrixBase<MatrixType>& position_partial_dq);
 
  const SE3& contactPlacement(const pinocchio::Data& data) const;
 
  void setBaumgarteGains(const double baumgarte_position_gain, 
                         const double baumgarte_velocity_gain);
 
  int contactFrameId() const;
 
  int parentJointId() const;
 
  const ContactModelInfo& contactModelInfo() const;
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const SurfaceContact& surface_contact);
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
private:
  ContactModelInfo info_;
  int contact_frame_id_, parent_joint_id_, dimv_;
  pinocchio::SE3 jXf_, X_diff_;
  Matrix6xd J_frame_, frame_v_partial_dq_, frame_a_partial_dq_, 
            frame_a_partial_dv_, frame_a_partial_da_;
  Matrix66d Jlog6_;
};
 
} // namespace robotoc
 
#include "robotoc/robot/surface_contact.hxx"
 
#endif // ROBOTOC_SURFACE_CONTACT_HPP_