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