split_solution.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_SPLIT_SOLUTION_HPP_
#define ROBOTOC_SPLIT_SOLUTION_HPP_
 
#include <vector>
#include <iostream>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/contact_status.hpp"
#include "robotoc/core/split_direction.hpp"
 
 
namespace robotoc {
 
class SplitSolution {
public:
  using Vector6d = Eigen::Matrix<double, 6, 1>;
 
  SplitSolution(const Robot& robot);
 
  SplitSolution();
 
  ~SplitSolution() = default;
 
  SplitSolution(const SplitSolution&) = default;
 
  SplitSolution& operator=(const SplitSolution&) = default;
 
  SplitSolution(SplitSolution&&) noexcept = default;
 
  SplitSolution& operator=(SplitSolution&&) noexcept = default;
 
  void setContactStatus(const ContactStatus& contact_status);
 
  void setContactStatus(const ImpactStatus& contact_status);
 
  void setContactStatus(const SplitSolution& other);
 
  void setSwitchingConstraintDimension(const int dims);
 
  Eigen::VectorXd q;
 
  Eigen::VectorXd v;
 
  Eigen::VectorXd a;
 
  Eigen::VectorXd dv;
 
  Eigen::VectorXd u;
 
  std::vector<Vector6d> f;
 
  Eigen::VectorBlock<Eigen::VectorXd> f_stack();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> f_stack() const;
 
  void set_f_stack();
 
  void set_f_vector();
 
  Eigen::VectorXd lmd;
 
  Eigen::VectorXd gmm;
 
  Eigen::VectorXd beta;
 
  std::vector<Vector6d> mu;
 
  Eigen::VectorXd nu_passive;
 
  Eigen::VectorBlock<Eigen::VectorXd> mu_stack();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> mu_stack() const;
 
  void set_mu_stack();
 
  void set_mu_vector();
 
  Eigen::VectorBlock<Eigen::VectorXd> xi_stack();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> xi_stack() const;
 
  int dimf() const;
 
  int dims() const;
 
  bool isContactActive(const int contact_index) const;
 
  std::vector<bool> isContactActive() const;
 
  void integrate(const Robot& robot, const double step_size, 
                 const SplitDirection& d, const bool impact=false);
 
  void copyPrimal(const SplitSolution& other);
 
  void copyDual(const SplitSolution& other);
 
  bool isApprox(const SplitSolution& other) const;
 
  double lagrangeMultiplierLinfNorm() const;
 
  void setRandom(const Robot& robot);
 
  void setRandom(const Robot& robot, const ContactStatus& contact_status);
 
  void setRandom(const Robot& robot, const ImpactStatus& impact_status);
 
  void setRandom(const Robot& robot, const ContactStatus& contact_status,
                 const ImpactStatus& impact_status);
 
  static SplitSolution Random(const Robot& robot);
 
  static SplitSolution Random(const Robot& robot, 
                              const ContactStatus& contact_status);
 
  static SplitSolution Random(const Robot& robot, 
                              const ImpactStatus& impact_status);
 
  static SplitSolution Random(const Robot& robot, 
                              const ContactStatus& contact_status,
                              const ImpactStatus& impact_status);
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, const SplitSolution& s);
 
private:
  Eigen::VectorXd mu_stack_, f_stack_, xi_stack_;
  bool has_floating_base_;
  std::vector<ContactType> contact_types_;
  std::vector<bool> is_contact_active_;
  int dimf_, dims_, max_num_contacts_;
 
};
 
} // namespace robotoc 
 
#include "robotoc/core/split_solution.hxx"
 
#endif // ROBOTOC_SPLIT_SOLUTION_HPP_