Added analytic hessian SQP example, removed binary
Alex Cunningham
parent
16e2f8430b
commit
e35e7a98d1
|
|
@ -1,148 +0,0 @@
|
||||||
#! /bin/sh
|
|
||||||
|
|
||||||
# testConstraintOptimizer - temporary wrapper script for .libs/testConstraintOptimizer
|
|
||||||
# Generated by ltmain.sh (GNU libtool) 2.2.6 Debian-2.2.6a-4
|
|
||||||
#
|
|
||||||
# The testConstraintOptimizer program cannot be directly executed until all the libtool
|
|
||||||
# libraries that it depends on are installed.
|
|
||||||
#
|
|
||||||
# This wrapper script should never be moved out of the build directory.
|
|
||||||
# If it is, it will not operate correctly.
|
|
||||||
|
|
||||||
# Sed substitution that helps us do robust quoting. It backslashifies
|
|
||||||
# metacharacters that are still active within double-quoted strings.
|
|
||||||
Xsed='/bin/sed -e 1s/^X//'
|
|
||||||
sed_quote_subst='s/\([`"$\\]\)/\\\1/g'
|
|
||||||
|
|
||||||
# Be Bourne compatible
|
|
||||||
if test -n "${ZSH_VERSION+set}" && (emulate sh) >/dev/null 2>&1; then
|
|
||||||
emulate sh
|
|
||||||
NULLCMD=:
|
|
||||||
# Zsh 3.x and 4.x performs word splitting on ${1+"$@"}, which
|
|
||||||
# is contrary to our usage. Disable this feature.
|
|
||||||
alias -g '${1+"$@"}'='"$@"'
|
|
||||||
setopt NO_GLOB_SUBST
|
|
||||||
else
|
|
||||||
case `(set -o) 2>/dev/null` in *posix*) set -o posix;; esac
|
|
||||||
fi
|
|
||||||
BIN_SH=xpg4; export BIN_SH # for Tru64
|
|
||||||
DUALCASE=1; export DUALCASE # for MKS sh
|
|
||||||
|
|
||||||
# The HP-UX ksh and POSIX shell print the target directory to stdout
|
|
||||||
# if CDPATH is set.
|
|
||||||
(unset CDPATH) >/dev/null 2>&1 && unset CDPATH
|
|
||||||
|
|
||||||
relink_command="(cd /home/alexgc/borg/gtsam/cpp; { test -z \"\${LIBRARY_PATH+set}\" || unset LIBRARY_PATH || { LIBRARY_PATH=; export LIBRARY_PATH; }; }; { test -z \"\${COMPILER_PATH+set}\" || unset COMPILER_PATH || { COMPILER_PATH=; export COMPILER_PATH; }; }; { test -z \"\${GCC_EXEC_PREFIX+set}\" || unset GCC_EXEC_PREFIX || { GCC_EXEC_PREFIX=; export GCC_EXEC_PREFIX; }; }; { test -z \"\${LD_RUN_PATH+set}\" || unset LD_RUN_PATH || { LD_RUN_PATH=; export LD_RUN_PATH; }; }; LD_LIBRARY_PATH=/usr/lib/jvm/java-6-openjdk/jre/lib/amd64/server:/usr/lib/jvm/java-6-openjdk/jre/lib/amd64:/usr/lib/jvm/java-6-openjdk/jre/../lib/amd64:/usr/lib64/xulrunner-addons; export LD_LIBRARY_PATH; PATH=/home/alexgc/ros/ros/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games; export PATH; g++ -g -I/usr/include/ -fPIC -I.. -g -O2 -march=core2 -DNDEBUG -o \$progdir/\$file testConstraintOptimizer.o -L/home/alexgc/borg/gtsam/CppUnitLite -lCppUnitLite ../ldl/libldl.a ./.libs/libgtsam.so -Wl,-rpath -Wl,/home/alexgc/borg/gtsam/cpp/.libs)"
|
|
||||||
|
|
||||||
# This environment variable determines our operation mode.
|
|
||||||
if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
|
|
||||||
# install mode needs the following variables:
|
|
||||||
generated_by_libtool_version='2.2.6'
|
|
||||||
notinst_deplibs=' libgtsam.la'
|
|
||||||
else
|
|
||||||
# When we are sourced in execute mode, $file and $ECHO are already set.
|
|
||||||
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
|
|
||||||
ECHO="echo"
|
|
||||||
file="$0"
|
|
||||||
# Make sure echo works.
|
|
||||||
if test "X$1" = X--no-reexec; then
|
|
||||||
# Discard the --no-reexec flag, and continue.
|
|
||||||
shift
|
|
||||||
elif test "X`{ $ECHO '\t'; } 2>/dev/null`" = 'X\t'; then
|
|
||||||
# Yippee, $ECHO works!
|
|
||||||
:
|
|
||||||
else
|
|
||||||
# Restart under the correct shell, and then maybe $ECHO will work.
|
|
||||||
exec /bin/sh "$0" --no-reexec ${1+"$@"}
|
|
||||||
fi
|
|
||||||
fi
|
|
||||||
|
|
||||||
# Find the directory that this script lives in.
|
|
||||||
thisdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
|
|
||||||
test "x$thisdir" = "x$file" && thisdir=.
|
|
||||||
|
|
||||||
# Follow symbolic links until we get to the real thisdir.
|
|
||||||
file=`ls -ld "$file" | /bin/sed -n 's/.*-> //p'`
|
|
||||||
while test -n "$file"; do
|
|
||||||
destdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
|
|
||||||
|
|
||||||
# If there was a directory component, then change thisdir.
|
|
||||||
if test "x$destdir" != "x$file"; then
|
|
||||||
case "$destdir" in
|
|
||||||
[\\/]* | [A-Za-z]:[\\/]*) thisdir="$destdir" ;;
|
|
||||||
*) thisdir="$thisdir/$destdir" ;;
|
|
||||||
esac
|
|
||||||
fi
|
|
||||||
|
|
||||||
file=`$ECHO "X$file" | $Xsed -e 's%^.*/%%'`
|
|
||||||
file=`ls -ld "$thisdir/$file" | /bin/sed -n 's/.*-> //p'`
|
|
||||||
done
|
|
||||||
|
|
||||||
|
|
||||||
# Usually 'no', except on cygwin/mingw when embedded into
|
|
||||||
# the cwrapper.
|
|
||||||
WRAPPER_SCRIPT_BELONGS_IN_OBJDIR=no
|
|
||||||
if test "$WRAPPER_SCRIPT_BELONGS_IN_OBJDIR" = "yes"; then
|
|
||||||
# special case for '.'
|
|
||||||
if test "$thisdir" = "."; then
|
|
||||||
thisdir=`pwd`
|
|
||||||
fi
|
|
||||||
# remove .libs from thisdir
|
|
||||||
case "$thisdir" in
|
|
||||||
*[\\/].libs ) thisdir=`$ECHO "X$thisdir" | $Xsed -e 's%[\\/][^\\/]*$%%'` ;;
|
|
||||||
.libs ) thisdir=. ;;
|
|
||||||
esac
|
|
||||||
fi
|
|
||||||
|
|
||||||
# Try to get the absolute directory name.
|
|
||||||
absdir=`cd "$thisdir" && pwd`
|
|
||||||
test -n "$absdir" && thisdir="$absdir"
|
|
||||||
|
|
||||||
program=lt-'testConstraintOptimizer'
|
|
||||||
progdir="$thisdir/.libs"
|
|
||||||
|
|
||||||
if test ! -f "$progdir/$program" ||
|
|
||||||
{ file=`ls -1dt "$progdir/$program" "$progdir/../$program" 2>/dev/null | /bin/sed 1q`; \
|
|
||||||
test "X$file" != "X$progdir/$program"; }; then
|
|
||||||
|
|
||||||
file="$$-$program"
|
|
||||||
|
|
||||||
if test ! -d "$progdir"; then
|
|
||||||
mkdir "$progdir"
|
|
||||||
else
|
|
||||||
rm -f "$progdir/$file"
|
|
||||||
fi
|
|
||||||
|
|
||||||
# relink executable if necessary
|
|
||||||
if test -n "$relink_command"; then
|
|
||||||
if relink_command_output=`eval $relink_command 2>&1`; then :
|
|
||||||
else
|
|
||||||
echo "$relink_command_output" >&2
|
|
||||||
rm -f "$progdir/$file"
|
|
||||||
exit 1
|
|
||||||
fi
|
|
||||||
fi
|
|
||||||
|
|
||||||
mv -f "$progdir/$file" "$progdir/$program" 2>/dev/null ||
|
|
||||||
{ rm -f "$progdir/$program";
|
|
||||||
mv -f "$progdir/$file" "$progdir/$program"; }
|
|
||||||
rm -f "$progdir/$file"
|
|
||||||
fi
|
|
||||||
|
|
||||||
if test -f "$progdir/$program"; then
|
|
||||||
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
|
|
||||||
# Run the actual program with our arguments.
|
|
||||||
|
|
||||||
exec "$progdir/$program" ${1+"$@"}
|
|
||||||
|
|
||||||
$ECHO "$0: cannot exec $program $*" 1>&2
|
|
||||||
exit 1
|
|
||||||
fi
|
|
||||||
else
|
|
||||||
# The program doesn't exist.
|
|
||||||
$ECHO "$0: error: \`$progdir/$program' does not exist" 1>&2
|
|
||||||
$ECHO "This script is just a wrapper for $program." 1>&2
|
|
||||||
echo "See the libtool documentation for more information." 1>&2
|
|
||||||
exit 1
|
|
||||||
fi
|
|
||||||
fi
|
|
||||||
|
|
@ -6,6 +6,7 @@
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
#include <boost/tuple/tuple.hpp>
|
#include <boost/tuple/tuple.hpp>
|
||||||
|
#include <boost/optional.hpp>
|
||||||
|
|
||||||
#include <ConstraintOptimizer.h>
|
#include <ConstraintOptimizer.h>
|
||||||
|
|
||||||
|
|
@ -65,6 +66,90 @@ TEST( matrix, CQP_example_automatic ) {
|
||||||
CHECK(assert_equal(expected, actState));
|
CHECK(assert_equal(expected, actState));
|
||||||
CHECK(actLam.size() == 2);
|
CHECK(actLam.size() == 2);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
/** SQP example from SQP tutorial */
|
||||||
|
namespace sqp_example1 {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* objective function with gradient and hessian
|
||||||
|
* fx = (x2-2)^2 + x1^2;
|
||||||
|
*/
|
||||||
|
Vector objective(const Vector& x, boost::optional<Vector&> g = boost::none,
|
||||||
|
boost::optional<Matrix&> B = boost::none) {
|
||||||
|
double x1 = x(0), x2 = x(1);
|
||||||
|
if (g) *g = Vector_(2, 2.0*x1, 2.0*(x2-2.0));
|
||||||
|
if (B) *B = 2.0 * eye(2,2);
|
||||||
|
return Vector_(1, (x2-2)*(x2-2) + x1*x1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* constraint function with gradient and hessian
|
||||||
|
* cx = 4*x1^2 + x2^2 - 1;
|
||||||
|
*/
|
||||||
|
Vector constraint(const Vector& x, boost::optional<Matrix&> A = boost::none,
|
||||||
|
boost::optional<Matrix&> B = boost::none) {
|
||||||
|
double x1 = x(0), x2 = x(1);
|
||||||
|
if (A) *A = Matrix_(2,1, 8.0*x1, 2.0*x2);
|
||||||
|
if (B) *B = Matrix_(2,2,
|
||||||
|
8.0, 0.0,
|
||||||
|
0.0, 2.0);
|
||||||
|
return Vector_(1, 4.0*x1*x1 + x2*x2 - 1.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( matrix, SQP_simple_analytic ) {
|
||||||
|
using namespace sqp_example1;
|
||||||
|
|
||||||
|
// Problem:
|
||||||
|
// min(x) f(x) = (x2-2)^2 + x1^2
|
||||||
|
// s.t. c(x) = 4x1^2 + x2^2 - 1 =0
|
||||||
|
// state is x = [x1 x2]' , with dim(state) = 2
|
||||||
|
// constraint has dim p = 1
|
||||||
|
|
||||||
|
// parameters
|
||||||
|
double stepsize = 0.25;
|
||||||
|
size_t maxIt = 50;
|
||||||
|
|
||||||
|
// initial conditions
|
||||||
|
Vector x0 = Vector_(2, 2.0, 4.0),
|
||||||
|
lam0 = Vector_(1, -0.5);
|
||||||
|
|
||||||
|
// current state
|
||||||
|
Vector x = x0, lam = lam0;
|
||||||
|
|
||||||
|
for (size_t i =0; i<maxIt; ++i) {
|
||||||
|
|
||||||
|
// evaluate functions
|
||||||
|
Vector dfx;
|
||||||
|
Matrix dcx, ddfx, ddcx;
|
||||||
|
Vector fx = objective(x, dfx, ddfx);
|
||||||
|
Vector cx = constraint(x, dcx, ddcx);
|
||||||
|
|
||||||
|
// use analytic hessian
|
||||||
|
Matrix B = ddfx - lam(0)*ddcx;
|
||||||
|
|
||||||
|
// solve subproblem
|
||||||
|
Vector delta, lambda;
|
||||||
|
boost::tie(delta, lambda) = solveCQP(B, -dcx, dfx, -cx);
|
||||||
|
|
||||||
|
// update
|
||||||
|
Vector step = stepsize * delta;
|
||||||
|
x = x + step;
|
||||||
|
lam = lambda;
|
||||||
|
}
|
||||||
|
|
||||||
|
// verify
|
||||||
|
Vector expX = Vector_(2, 0.0, 1.0),
|
||||||
|
expLambda = Vector_(1, -1.0);
|
||||||
|
|
||||||
|
CHECK(assert_equal(expX, x, 1e-4));
|
||||||
|
CHECK(assert_equal(expLambda, lam, 1e-4));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue