contact_wrench_cone.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_CONTACT_WRENCH_CONE_HPP_
#define ROBOTOC_CONTACT_WRENCH_CONE_HPP_
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/contact_status.hpp"
#include "robotoc/core/split_solution.hpp"
#include "robotoc/core/split_direction.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/constraints/constraint_component_base.hpp"
#include "robotoc/constraints/constraint_component_data.hpp"
 
 
namespace robotoc {
 
class ContactWrenchCone final : public ConstraintComponentBase {
public:
  ContactWrenchCone(const Robot& robot, const double X, const double Y);
 
  ContactWrenchCone();
 
  ~ContactWrenchCone();
 
  ContactWrenchCone(const ContactWrenchCone&) = default;
 
  ContactWrenchCone& operator=(const ContactWrenchCone&) = default;
 
  ContactWrenchCone(ContactWrenchCone&&) noexcept = default;
 
  ContactWrenchCone& operator=(ContactWrenchCone&&) noexcept = default;
 
  void setRectangular(const double X, const double Y);
 
  KinematicsLevel kinematicsLevel() const override;
 
  void allocateExtraData(ConstraintComponentData& data) const override;
 
  bool isFeasible(Robot& robot, const ContactStatus& contact_status, 
                  ConstraintComponentData& data, 
                  const SplitSolution& s) const override;
 
  void setSlack(Robot& robot, const ContactStatus& contact_status, 
                ConstraintComponentData& data, 
                const SplitSolution& s) const override;
 
  void evalConstraint(Robot& robot, const ContactStatus& contact_status, 
                      ConstraintComponentData& data, 
                      const SplitSolution& s) const override;
 
  void evalDerivatives(Robot& robot, const ContactStatus& contact_status, 
                       ConstraintComponentData& data, const SplitSolution& s,
                       SplitKKTResidual& kkt_residual) const override;
 
  void condenseSlackAndDual(const ContactStatus& contact_status, 
                            ConstraintComponentData& data, 
                            SplitKKTMatrix& kkt_matrix,
                            SplitKKTResidual& kkt_residual) const override;
 
  void expandSlackAndDual(const ContactStatus& contact_status, 
                          ConstraintComponentData& data, 
                          const SplitDirection& d) const override; 
 
  int dimc() const override;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW 
 
private:
  int dimv_, dimc_, max_num_contacts_;
  std::vector<int> contact_frame_;
  std::vector<ContactType> contact_types_;
  double X_, Y_;
 
  void computeCone(const double mu, Eigen::MatrixXd& cone) const;
  void updateCone(const double mu, Eigen::MatrixXd& cone) const;
 
};
 
} // namespace robotoc
 
#endif // ROBOTOC_CONTACT_WRENCH_CONE_HPP_