DynamicFunction.h

Go to the documentation of this file.

 
#pragma once
 
#include <tvm/api.h>
 
#include <tvm/Robot.h>
 
#include <tvm/function/abstract/LinearFunction.h>
 
#include <tvm/robot/enums.h>
#include <tvm/robot/internal/FrictionCone.h>
#include <tvm/robot/internal/GeometricContactFunction.h>
 
#include <RBDyn/FD.h>
 
namespace tvm
{
 
class ControlProblem;
 
namespace robot
{
 
namespace internal
{
 
class TVM_DLLAPI DynamicFunction : public function::abstract::LinearFunction
{
public:
  using Output = function::abstract::LinearFunction::Output;
  DISABLE_OUTPUTS(Output::JDot)
  SET_UPDATES(DynamicFunction, Jacobian, B)
 
  
  DynamicFunction(RobotPtr robot);
 
  bool addContact(ContactPtr contact, bool linearize, double mu, unsigned int nrGen);
 
  void removeContact(const Contact::Id & id);
 
  sva::ForceVecd contactForce(const Contact::Id & id) const;
 
  void addPositiveLambdaToProblem(ControlProblem & problem);
 
protected:
  void updateb();
 
  RobotPtr robot_;
 
  struct ForceContact
  {
    Contact::Id id_;
 
    bool linearized_;
 
    std::vector<tvm::VariablePtr> forces_;
 
    std::function<sva::ForceVecd(const ForceContact &)> force_;
 
    Eigen::MatrixXd force_jac_;
    Eigen::MatrixXd full_jac_;
 
    std::function<void(ForceContact &, DynamicFunction &)> updateJacobians_;
  };
  std::vector<ForceContact> contacts_;
 
  void updateJacobian();
 
  void addContact_(const Contact::View & contact, bool linearize, double mu, unsigned int nrGen, double dir);
 
  std::vector<ForceContact>::iterator getContact(const Contact::Id & id);
  std::vector<ForceContact>::const_iterator getContact(const Contact::Id & id) const;
};
 
} // namespace internal
 
} // namespace robot
 
} // namespace tvm