KrisLibrary  1.0.0
TorqueSolver.h
1 #ifndef TORQUE_SOLVER_H
2 #define TORQUE_SOLVER_H
3 
4 #include <KrisLibrary/Logger.h>
5 #include "RobotDynamics3D.h"
6 #include "Contact.h"
7 #include <KrisLibrary/optimization/MinNormProblem.h>
8 
40 class TorqueSolver
41 {
42  public:
43  TorqueSolver(RobotDynamics3D& robot, const ContactFormation& contacts,int nFrictionConeEdges=4);
44  TorqueSolver(RobotDynamics3D& robot, const CustomContactFormation& contacts);
45  void SetGravity(const Vector3& gravity) { gravityVector=gravity; }
46  void SetNorm(Real norm) { problem.norm = norm; }
47  void SetDynamics(const Vector& ddq);
48 
50  void Init(bool weighted=true);
51  void Clear();
53  void LimitContactForce(int i,Real maximum,const Vector3& dir);
54  void LimitContactForceSum(const std::vector<int>& indices,Real maximum,const Vector3& dir);
57  bool Solve();
59  bool InTorqueBounds();
60 
62  void FillProblem();
63 
65  RobotDynamics3D& robot;
66  CustomContactFormation contacts;
67  Vector3 gravityVector;
68  Vector internalForces;
69 
71  Vector t,f;
72 
73  //temp
74  std::vector<int> active,passive;
76  Real tmaxMin;
77  Vector lhs;
78 };
79 
80 #endif
A robot-environment contact formation. The list of contacts contacts[i] is associated with links[i] (...
Definition: Contact.h:61
Solves a sparse, linearly constrained minimum-norm problem.
Definition: MinNormProblem.h:52
A 3D vector class.
Definition: math3d/primitives.h:136
A more advanced ContactFormation that accepts custom contact points and custom constraints.
Definition: Contact.h:249
The logging system used in KrisLibrary.
Class defining kinematic and dynamic state of a robot, with commonly used functions. Inherits from RobotKinematics3D.
Definition: RobotDynamics3D.h:32