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 <boost/tuple/tuple.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <ConstraintOptimizer.h>
|
||||
|
||||
|
|
@ -65,6 +66,90 @@ TEST( matrix, CQP_example_automatic ) {
|
|||
CHECK(assert_equal(expected, actState));
|
||||
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