Added analytic hessian SQP example, removed binary
							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