point_contact.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_POINT_CONTACT_HPP_
#define ROBOTOC_POINT_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 PointContact {
public:
  using Matrix6xd = Eigen::Matrix<double, 6, Eigen::Dynamic>;
 
  PointContact(const pinocchio::Model& model, 
               const ContactModelInfo& contact_model_info); 
 
  PointContact();
 
  ~PointContact() = default;
 
  PointContact(const PointContact&) = default;
 
  PointContact& operator=(const PointContact&) = default;
 
  PointContact(PointContact&&) noexcept = default;
 
  PointContact& operator=(PointContact&&) noexcept = default;
 
  void computeJointForceFromContactForce(
      const Eigen::Vector3d& contact_force, 
      pinocchio::container::aligned_vector<pinocchio::Force>& joint_forces) const;
 
  template <typename VectorType1, typename VectorType2>
  void computeBaumgarteResidual(
      const pinocchio::Model& model, const pinocchio::Data& data, 
      const Eigen::MatrixBase<VectorType1>& desired_contact_position,
      const Eigen::MatrixBase<VectorType2>& baumgarte_residual) const;
 
  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 VectorType1, typename VectorType2>
  void computeContactPositionResidual(
      const pinocchio::Model& model, const pinocchio::Data& data, 
      const Eigen::MatrixBase<VectorType1>& desired_contact_position,
      const Eigen::MatrixBase<VectorType2>& position_residual) const;
 
  template <typename MatrixType>
  void computeContactPositionDerivative(
      const pinocchio::Model& model, pinocchio::Data& data,
      const Eigen::MatrixBase<MatrixType>& position_partial_dq);
 
  const Eigen::Vector3d& contactPosition(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 PointContact& point_contact);
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
private:
  ContactModelInfo info_;
  int contact_frame_id_, parent_joint_id_, dimv_;
  SE3 jXf_;
  pinocchio::Motion v_frame_;
  Eigen::Matrix3d v_linear_skew_, v_angular_skew_;
  Matrix6xd J_frame_, frame_v_partial_dq_, frame_a_partial_dq_, 
            frame_a_partial_dv_, frame_a_partial_da_;
};
 
} // namespace robotoc
 
#include "robotoc/robot/point_contact.hxx"
 
#endif // ROBOTOC_POINT_CONTACT_HPP_