Klamp't  0.8.1
ContactTimeScaling.h
1 #ifndef CONTACT_TIME_SCALING_H
2 #define CONTACT_TIME_SCALING_H
3 
4 #include "TimeScaling.h"
5 #include "Modeling/MultiPath.h"
6 #include "Modeling/Robot.h"
7 #include "RobotCSpace.h"
8 #include <KrisLibrary/math3d/Polygon2D.h>
9 
17 {
18  public:
19  CustomTimeScaling(Robot& robot);
21  void SetPath(const GeneralizedCubicBezierSpline& path,
22  const vector<Real>& colocationParams);
24  void SetPath(const MultiPath& path,
25  const vector<Real>& colocationParams);
27  void SetStartStop();
29  void SetDefaultBounds();
31  bool Optimize();
33  bool IsFeasible(const vector<Real>& ds) const;
35  void PrintActiveConstraints(ostream& out);
36 
37  RobotCSpace cspace;
38 
42  vector<Real> paramDivs;
44  vector<int> paramSections;
46  vector<Vector> xs,dxs,ddxs;
48  vector<Real> dsmax;
51  vector<vector<Vector2> > ds2ddsConstraintNormals;
52  vector<vector<Real> > ds2ddsConstraintOffsets;
55  vector<vector<string> > ds2ddsConstraintNames;
56 
66  vector<vector<Real> > constraintLagrangeMultipliers;
67 };
68 
80 {
81  public:
82  TorqueTimeScaling(Robot& robot);
83  void SetParams(const MultiPath& path,const vector<Real>& colocationParams);
84 
87 };
88 
104 {
105  public:
106  ZMPTimeScaling(Robot& robot);
108  void SetParams(const MultiPath& path,const vector<Real>& colocationParams,
109  const vector<ConvexPolygon2D>& supportPolys,const vector<Real>& groundHeights);
111  void SetParams(const MultiPath& path,const vector<Real>& colocationParams,
112  const vector<Vector2>& supportPoly,Real groundHeight=0);
113 
115  bool Check(const MultiPath& path);
116 
117  vector<ConvexPolygon2D> supportPolys;
118  vector<Real> groundHeights;
119 };
120 
134 {
135  public:
136  ContactTimeScaling(Robot& robot);
139  bool SetParams(const MultiPath& path,const vector<Real>& colocationParams,
140  int numFCEdges = 4);
141 
143  bool Check(const MultiPath& path);
144 
149 };
150 
151 #endif
bool IsFeasible(const vector< Real > &ds) const
Returns true if the time scaling derivatives are feasible under the current constraints.
A very general multi-section path container.
Definition: MultiPath.h:29
The main robot type used in RobotSim.
Definition: Robot.h:79
void SetStartStop()
Sets up the path to start and stop at zero velocity.
Real torqueLimitShift
offsets the torque limits by a fixed amount (default 0)
Definition: ContactTimeScaling.h:145
vector< Real > variableLagrangeMultipliers
Definition: ContactTimeScaling.h:62
vector< Real > dsmax
time scaling maxima. Defaults to Inf
Definition: ContactTimeScaling.h:48
bool saveConstraintNames
Names of each constraint plane.
Definition: ContactTimeScaling.h:54
A time scaling with torque constraints |t| <= tmax. Assuming fixed base manipulator.
Definition: ContactTimeScaling.h:79
void SetPath(const GeneralizedCubicBezierSpline &path, const vector< Real > &colocationParams)
Sets up the path.
Real frictionRobustness
from 0 to 1, indicates the amount of increased robustness in friction cones (default 0) ...
Definition: ContactTimeScaling.h:147
Real torqueLimitScale
from 0 to 1, scales the torque limits (default 1)
Definition: ContactTimeScaling.h:86
vector< Real > paramDivs
colocation grid
Definition: ContactTimeScaling.h:42
vector< int > paramSections
index from grid to section of multipath, if multipath is given
Definition: ContactTimeScaling.h:44
void PrintActiveConstraints(ostream &out)
After running Optimize, prints out all the active constraints.
A base class for a time scaling with colocation point constraints. Subclasses should fill in dsmax...
Definition: ContactTimeScaling.h:16
void SetDefaultBounds()
Sets up velocity and acceleration bounds.
Real forceRobustness
>= 0, indicates the absolute margin for forces to be contained within the friction cone ...
Definition: ContactTimeScaling.h:148
A time scaling with torque/contact constraints.
Definition: ContactTimeScaling.h:133
vector< Vector > xs
colocation points
Definition: ContactTimeScaling.h:46
Real torqueLimitShift
offsets the torque limits by a fixed amount (default 0)
Definition: ContactTimeScaling.h:85
TimeScaledBezierCurve traj
Output trajectory.
Definition: ContactTimeScaling.h:40
vector< vector< Real > > constraintLagrangeMultipliers
Definition: ContactTimeScaling.h:66
Real torqueLimitScale
from 0 to 1, scales the torque limits (default 1)
Definition: ContactTimeScaling.h:146
Implements a basic robot configuration space with only joint limit constraint testing.
Definition: RobotCSpace.h:30
bool Optimize()
Runs the optimizer with the custom constraints.
bool computeLagrangeMultipliers
Whether the lagrange multipliers of a solution are requested.
Definition: ContactTimeScaling.h:58
A time scaling with Zero Moment Point constraints. Each section of the multipath corresponds with one...
Definition: ContactTimeScaling.h:103
A convenience class that stores a Bezier curve and its time scaling. Useful for evaluating the scaled...
Definition: TimeScaling.h:128
vector< vector< Vector2 > > ds2ddsConstraintNormals
Definition: ContactTimeScaling.h:51