Keeps the 6D base motion in a specified range. More...
#include <base_motion_constraint.h>
Public Member Functions | |
BaseMotionConstraint (double T, double dt, const SplineHolder &spline_holder) | |
Links the base variables and sets hardcoded bounds on the state. | |
void | UpdateBoundsAtInstance (double t, int k, VecBound &) const override |
Sets upper/lower bound a specific time t, corresponding to node k. | |
void | UpdateConstraintAtInstance (double t, int k, VectorXd &g) const override |
Sets the constraint value a specific time t, corresponding to node k. | |
void | UpdateJacobianAtInstance (double t, int k, std::string, Jacobian &) const override |
Sets Jacobian rows at a specific time t, corresponding to node k. | |
virtual | ~BaseMotionConstraint () |
Private Member Functions | |
int | GetRow (int node, int dim) const |
Private Attributes | |
NodeSpline::Ptr | base_angular_ |
NodeSpline::Ptr | base_linear_ |
VecBound | node_bounds_ |
same bounds for each discretized node |
Keeps the 6D base motion in a specified range.
In general this constraint should be avoided, since a similar affect can be achieved with RangeOfMotionConstraint.
Definition at line 48 of file base_motion_constraint.h.
towr::BaseMotionConstraint::BaseMotionConstraint | ( | double | T, |
double | dt, | ||
const SplineHolder & | spline_holder | ||
) |
Links the base variables and sets hardcoded bounds on the state.
T | The total time of the optimization horizon. |
dt | The discretization interval of the constraints. |
spline_holder | Holds pointers to the base variables. |
Definition at line 38 of file base_motion_constraint.cc.
virtual towr::BaseMotionConstraint::~BaseMotionConstraint | ( | ) | [virtual] |
int towr::BaseMotionConstraint::GetRow | ( | int | node, |
int | dim | ||
) | const [private] |
Definition at line 88 of file base_motion_constraint.cc.
void towr::BaseMotionConstraint::UpdateBoundsAtInstance | ( | double | t, |
int | k, | ||
VecBound & | b | ||
) | const [override, virtual] |
Sets upper/lower bound a specific time t, corresponding to node k.
t | The time along the trajectory to set the bounds. |
k | The index of the time t, so t=k*dt |
in/out] | b The complete vector of bounds, for which the corresponding row must be set. |
Implements towr::TimeDiscretizationConstraint.
Definition at line 69 of file base_motion_constraint.cc.
void towr::BaseMotionConstraint::UpdateConstraintAtInstance | ( | double | t, |
int | k, | ||
VectorXd & | g | ||
) | const [override, virtual] |
Sets the constraint value a specific time t, corresponding to node k.
t | The time along the trajectory to set the constraint. |
k | The index of the time t, so t=k*dt |
in/out] | g The complete vector of constraint values, for which the corresponding row must be filled. |
Implements towr::TimeDiscretizationConstraint.
Definition at line 61 of file base_motion_constraint.cc.
void towr::BaseMotionConstraint::UpdateJacobianAtInstance | ( | double | t, |
int | k, | ||
std::string | var_set, | ||
Jacobian & | jac | ||
) | const [override, virtual] |
Sets Jacobian rows at a specific time t, corresponding to node k.
t | The time along the trajcetory to set the bounds. |
k | The index of the time t, so t=k*dt |
var_set | The name of the ifopt variables currently being queried for. |
in/out] | jac The complete Jacobian, for which the corresponding row and columns must be set. |
Implements towr::TimeDiscretizationConstraint.
Definition at line 76 of file base_motion_constraint.cc.
NodeSpline::Ptr towr::BaseMotionConstraint::base_angular_ [private] |
Definition at line 65 of file base_motion_constraint.h.
NodeSpline::Ptr towr::BaseMotionConstraint::base_linear_ [private] |
Definition at line 64 of file base_motion_constraint.h.
VecBound towr::BaseMotionConstraint::node_bounds_ [private] |
same bounds for each discretized node
Definition at line 67 of file base_motion_constraint.h.