Changed argument ordering for constraints
Alex Cunningham
parent
7dfc67860c
commit
7d0de77fc6
|
|
@ -48,9 +48,9 @@ bool NonlinearConstraint<Config>::active(const Config& config) const {
|
|||
|
||||
template <class Config>
|
||||
NonlinearConstraint1<Config>::NonlinearConstraint1(
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
const std::string& key,
|
||||
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
|
|
@ -67,9 +67,9 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
|
|||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint1<Config>::NonlinearConstraint1(
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
const std::string& key,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
|
|
@ -138,11 +138,11 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
|
|||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint2<Config>::NonlinearConstraint2(
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
const std::string& key1,
|
||||
Matrix (*gradG1)(const Config& config, const std::list<std::string>& keys),
|
||||
const std::string& key2,
|
||||
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
|
|
@ -161,11 +161,11 @@ NonlinearConstraint2<Config>::NonlinearConstraint2(
|
|||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint2<Config>::NonlinearConstraint2(
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
const std::string& key1,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1,
|
||||
const std::string& key2,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG2,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
|
|
|
|||
|
|
@ -148,9 +148,9 @@ public:
|
|||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint1(
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
const std::string& key,
|
||||
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
@ -165,9 +165,9 @@ public:
|
|||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint1(
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
const std::string& key,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
@ -224,11 +224,11 @@ public:
|
|||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint2(
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
const std::string& key1,
|
||||
Matrix (*gradG1)(const Config& config, const std::list<std::string>& keys),
|
||||
const std::string& key2,
|
||||
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
@ -244,11 +244,11 @@ public:
|
|||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint2(
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
const std::string& key1,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1,
|
||||
const std::string& key2,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG2,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
|
|||
// the lagrange multipliers will be expected on L_x1
|
||||
// and there is only one multiplier
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *test1::G, *test1::g, p, "L_x1");
|
||||
NonlinearConstraint1<VectorConfig> c1(*test1::g, "x", *test1::G, p, "L_x1");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig config;
|
||||
|
|
@ -53,7 +53,7 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_linearize ) {
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *test1::G, *test1::g, p, "L_x1");
|
||||
NonlinearConstraint1<VectorConfig> c1(*test1::g, "x", *test1::G, p, "L_x1");
|
||||
|
||||
// get a configuration to use for linearization
|
||||
VectorConfig realconfig;
|
||||
|
|
@ -77,10 +77,10 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_equal ) {
|
||||
NonlinearConstraint1<VectorConfig>
|
||||
c1("x", *test1::G, *test1::g, 1, "L_x1", true),
|
||||
c2("x", *test1::G, *test1::g, 1, "L_x1"),
|
||||
c3("x", *test1::G, *test1::g, 2, "L_x1"),
|
||||
c4("y", *test1::G, *test1::g, 1, "L_x1");
|
||||
c1(*test1::g, "x", *test1::G, 1, "L_x1", true),
|
||||
c2(*test1::g, "x", *test1::G, 1, "L_x1"),
|
||||
c3(*test1::g, "x", *test1::G, 2, "L_x1"),
|
||||
c4(*test1::g, "y", *test1::G, 1, "L_x1");
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
|
|
@ -121,9 +121,10 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
|
|||
// and there is only one multiplier
|
||||
size_t p = 1;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
*test2::g,
|
||||
"x", *test2::G1,
|
||||
"y", *test2::G2,
|
||||
*test2::g, p, "L_xy");
|
||||
p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig config;
|
||||
|
|
@ -141,9 +142,10 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
|
|||
// create a constraint
|
||||
size_t p = 1;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
*test2::g,
|
||||
"x", *test2::G1,
|
||||
"y", *test2::G2,
|
||||
*test2::g, p, "L_xy");
|
||||
p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig realconfig;
|
||||
|
|
@ -172,10 +174,10 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_equal ) {
|
||||
NonlinearConstraint2<VectorConfig>
|
||||
c1("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"),
|
||||
c2("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"),
|
||||
c3("y", *test2::G1, "x", *test2::G2,*test2::g, 1, "L_xy"),
|
||||
c4("x", *test2::G1, "z", *test2::G2,*test2::g, 3, "L_xy");
|
||||
c1(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"),
|
||||
c2(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"),
|
||||
c3(*test2::g, "y", *test2::G1, "x", *test2::G2, 1, "L_xy"),
|
||||
c4(*test2::g, "x", *test2::G1, "z", *test2::G2, 3, "L_xy");
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
|
|
@ -206,8 +208,9 @@ namespace inequality1 {
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_inequality ) {
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *inequality1::G,
|
||||
*inequality1::g, p, "L_x1",
|
||||
NonlinearConstraint1<VectorConfig> c1(*inequality1::g,
|
||||
"x", *inequality1::G,
|
||||
p, "L_x1",
|
||||
false); // inequality constraint
|
||||
|
||||
// get configurations to use for evaluation
|
||||
|
|
@ -225,8 +228,9 @@ TEST( NonlinearConstraint1, unary_inequality ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_inequality_linearize ) {
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *inequality1::G,
|
||||
*inequality1::g, p, "L_x",
|
||||
NonlinearConstraint1<VectorConfig> c1(*inequality1::g,
|
||||
"x", *inequality1::G,
|
||||
p, "L_x",
|
||||
false); // inequality constraint
|
||||
|
||||
// get configurations to use for linearization
|
||||
|
|
@ -283,9 +287,9 @@ TEST( NonlinearConstraint1, unary_binding ) {
|
|||
size_t p = 1;
|
||||
double coeff = 2;
|
||||
double radius = 5;
|
||||
NonlinearConstraint1<VectorConfig> c1("x",
|
||||
boost::bind(binding1::G, coeff, _1, _2),
|
||||
NonlinearConstraint1<VectorConfig> c1(
|
||||
boost::bind(binding1::g, radius, _1, _2),
|
||||
"x", boost::bind(binding1::G, coeff, _1, _2),
|
||||
p, "L_x1",
|
||||
false); // inequality constraint
|
||||
|
||||
|
|
@ -335,9 +339,10 @@ TEST( NonlinearConstraint2, binary_binding ) {
|
|||
double b = 1.0;
|
||||
double r = 5.0;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
boost::bind(binding2::g, r, _1, _2),
|
||||
"x", boost::bind(binding2::G1, a, _1, _2),
|
||||
"y", boost::bind(binding2::G2, b, _1, _2),
|
||||
boost::bind(binding2::g, r, _1, _2), p, "L_xy");
|
||||
p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig config;
|
||||
|
|
|
|||
|
|
@ -367,8 +367,9 @@ TEST (SQP, two_pose ) {
|
|||
// constant constraint on x1
|
||||
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
|
||||
new NonlinearConstraint1<VectorConfig>(
|
||||
*sqp_test2::g,
|
||||
"x1", *sqp_test2::G,
|
||||
*sqp_test2::g, 2, "L_x1"));
|
||||
2, "L_x1"));
|
||||
|
||||
// measurement from x1 to l1
|
||||
Vector z1 = Vector_(2, 0.0, 5.0);
|
||||
|
|
@ -383,9 +384,10 @@ TEST (SQP, two_pose ) {
|
|||
// equality constraint between l1 and l2
|
||||
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
|
||||
new NonlinearConstraint2<VectorConfig>(
|
||||
*sqp_test1::g,
|
||||
"l1", *sqp_test1::G1,
|
||||
"l2", *sqp_test1::G2,
|
||||
*sqp_test1::g, 2, "L_l1l2"));
|
||||
2, "L_l1l2"));
|
||||
|
||||
// construct the graph
|
||||
NLGraph graph;
|
||||
|
|
@ -675,9 +677,10 @@ VSLAMGraph stereoExampleGraph() {
|
|||
// as the previous examples
|
||||
boost::shared_ptr<NonlinearConstraint2<VSLAMConfig> > c2(
|
||||
new NonlinearConstraint2<VSLAMConfig>(
|
||||
*sqp_stereo::g,
|
||||
"l1", *sqp_stereo::G1,
|
||||
"l2", *sqp_stereo::G2,
|
||||
*sqp_stereo::g, 3, "L_l1l2"));
|
||||
3, "L_l1l2"));
|
||||
graph.push_back(c2);
|
||||
|
||||
return graph;
|
||||
|
|
|
|||
|
|
@ -94,8 +94,9 @@ NLGraph linearMapWarpGraph() {
|
|||
// constant constraint on x1
|
||||
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
|
||||
new NonlinearConstraint1<VectorConfig>(
|
||||
*sqp_LinearMapWarp1::g_func,
|
||||
"x1", *sqp_LinearMapWarp1::grad_g,
|
||||
*sqp_LinearMapWarp1::g_func, 2, "L_x1"));
|
||||
2, "L_x1"));
|
||||
|
||||
// measurement from x1 to l1
|
||||
Vector z1 = Vector_(2, 0.0, 5.0);
|
||||
|
|
@ -110,9 +111,10 @@ NLGraph linearMapWarpGraph() {
|
|||
// equality constraint between l1 and l2
|
||||
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
|
||||
new NonlinearConstraint2<VectorConfig>(
|
||||
*sqp_LinearMapWarp2::g_func,
|
||||
"l1", *sqp_LinearMapWarp2::grad_g1,
|
||||
"l2", *sqp_LinearMapWarp2::grad_g2,
|
||||
*sqp_LinearMapWarp2::g_func, 2, "L_l1l2"));
|
||||
2, "L_l1l2"));
|
||||
|
||||
// construct the graph
|
||||
NLGraph graph;
|
||||
|
|
@ -270,9 +272,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
|
|||
|
||||
// create a binary inequality constraint that forces the middle point away from
|
||||
// the obstacle
|
||||
shared_NLC2 c1(new NLC2("x2", *sqp_avoid1::grad_g1,
|
||||
"obs", *sqp_avoid1::grad_g2,
|
||||
*sqp_avoid1::g_func, 1, "L_x2obs", false));
|
||||
shared_NLC2 c1(new NLC2(*sqp_avoid1::g_func,
|
||||
"x2", *sqp_avoid1::grad_g1,
|
||||
"obs", *sqp_avoid1::grad_g2,
|
||||
1, "L_x2obs", false));
|
||||
|
||||
// construct the graph
|
||||
NLGraph graph;
|
||||
|
|
@ -400,9 +403,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
|
|||
|
||||
// create a binary inequality constraint that forces the middle point away from
|
||||
// the obstacle
|
||||
shared_NLC2 c1(new NLC2("x2", boost::bind(sqp_avoid2::grad_g1, _1, _2),
|
||||
"obs", boost::bind(sqp_avoid2::grad_g2, _1, _2),
|
||||
boost::bind(sqp_avoid2::g_func, radius, _1, _2), 1, "L_x2obs", false));
|
||||
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, _2),
|
||||
"x2", boost::bind(sqp_avoid2::grad_g1, _1, _2),
|
||||
"obs", boost::bind(sqp_avoid2::grad_g2, _1, _2),
|
||||
1, "L_x2obs", false));
|
||||
|
||||
// construct the graph
|
||||
NLGraph graph;
|
||||
|
|
|
|||
Loading…
Reference in New Issue