[roboptim-commit] [SCM] roboptim-trajectory branch, master, updated. v0.2-14-g4e4d008
Status: Beta
Brought to you by:
flamiraux
Menu
▾
▴
[roboptim-commit] [SCM] roboptim-trajectory branch, master, updated. v0.2-14-g4e4d008
|
From: Thomas M. <tho...@us...> - 2009-11-03 16:50:29
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "roboptim-trajectory".
The branch, master has been updated
via 4e4d008559a1549e566a2842019dc8d2e724db21 (commit)
from 826959a7d6d886a5eb6fe7ff64540ae447b635f9 (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 4e4d008559a1549e566a2842019dc8d2e724db21
Author: Thomas Moulard <tho...@gm...>
Date: Tue Nov 3 17:49:47 2009 +0100
Implement FixedPointStateFunction.
* include/Makefile.am: Distribute new files.
* include/roboptim/trajectory/fixed-point-state-function.hh: New.
* include/roboptim/trajectory/fixed-point-state-function.hxx: New.
* include/roboptim/trajectory/free-time-trajectory.hh: Add
getter to allow access to interal fixed time trajectory.
Signed-off-by: Thomas Moulard <tho...@gm...>
diff --git a/ChangeLog b/ChangeLog
index d41ce68..32cd048 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,5 +1,14 @@
2009-11-03 Thomas Moulard <tho...@gm...>
+ Implement FixedPointStateFunction.
+ * include/Makefile.am: Distribute new files.
+ * include/roboptim/trajectory/fixed-point-state-function.hh: New.
+ * include/roboptim/trajectory/fixed-point-state-function.hxx: New.
+ * include/roboptim/trajectory/free-time-trajectory.hh: Add
+ getter to allow access to interal fixed time trajectory.
+
+2009-11-03 Thomas Moulard <tho...@gm...>
+
Make StateFunction only usable with fixed point trajectory.
* include/roboptim/trajectory/fwd.hh: Fix forward declaration.
* include/roboptim/trajectory/state-function.hh,
diff --git a/include/Makefile.am b/include/Makefile.am
index 71a5d08..c471d02 100644
--- a/include/Makefile.am
+++ b/include/Makefile.am
@@ -4,6 +4,8 @@ include $(top_srcdir)/build-aux/init.mk
nobase_include_HEADERS = \
roboptim/trajectory/anthropomorphic-cost-function.hh \
roboptim/trajectory/anthropomorphic-cost-function.hxx \
+ roboptim/trajectory/fixed-point-state-function.hh \
+ roboptim/trajectory/fixed-point-state-function.hxx \
roboptim/trajectory/free-time-trajectory.hh \
roboptim/trajectory/free-time-trajectory.hxx \
roboptim/trajectory/freeze.hh \
diff --git a/include/roboptim/trajectory/fixed-point-state-function.hh b/include/roboptim/trajectory/fixed-point-state-function.hh
new file mode 100644
index 0000000..762e4e9
--- /dev/null
+++ b/include/roboptim/trajectory/fixed-point-state-function.hh
@@ -0,0 +1,111 @@
+// Copyright (C) 2009 by Florent Lamiraux, Thomas Moulard, AIST, CNRS, INRIA.
+//
+// This file is part of the roboptim.
+//
+// roboptim is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// roboptim is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with roboptim. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef ROBOPTIM_TRAJECTORY_FIXED_POINT_STATE_FUNCTION_HH
+# define ROBOPTIM_TRAJECTORY_FIXED_POINT_STATE_FUNCTION_HH
+# include <roboptim/trajectory/sys.hh>
+
+# include <boost/shared_ptr.hpp>
+
+# include <roboptim/trajectory/fwd.hh>
+# include <roboptim/core/derivable-function.hh>
+# include <roboptim/trajectory/stable-time-point.hh>
+
+namespace roboptim
+{
+ /// \addtogroup roboptim_meta_function
+ /// @{
+
+ /// \brief Trajectory cost function defined by state evaluation at parameter.
+ ///
+ /// The state along a trajectory is defined as the vector containing the
+ /// configuration and derivatives up to order \f$r\f$ of the
+ /// configuration.
+ /**\f[
+\textbf{Cost}(\Gamma) = cost
+\left({\Gamma(t)}, {\dot{\Gamma}(t)},\cdots,\frac{d^{r}\Gamma}{dt^{r}}(t)
+\right)
+ \f]*/
+ /// where
+ /// - \f$\textbf{Cost}\f$ is the trajectory cost,
+ /// - \f$cost\f$ is the state cost,
+ /// - \f$t\f$ is the parameter along the trajectory where the cost is
+ /// evaluated (fixed at construction),
+ /// - \f$r\f$ is called the order of the state.
+ ///
+ /// \tparam T trajectory type
+
+ template <typename T>
+ class FixedPointStateFunction : public DerivableFunction
+ {
+ public:
+ /// \brief Trajectory type.
+ typedef FreeTimeTrajectory<T> trajectory_t;
+
+ /// \brief Constructor.
+ ///
+ /// \param gamma Trajectory \f$\Gamma\f$ along which the state is evaluated.
+ /// \param cost state cost: \f$cost\f$.
+ /// \param tpt parameter \f$t\f$ where the state is evaluated.
+ /// \param order order \f$r\f$ of derivation.
+ FixedPointStateFunction (const trajectory_t& gamma,
+ boost::shared_ptr<DerivableFunction> cost,
+ const StableTimePoint tpt,
+ size_type order = 1) throw ();
+
+ virtual ~FixedPointStateFunction () throw ();
+
+ size_type order () const throw ();
+
+ template <typename F, typename CLIST>
+ static void addToProblem (const trajectory_t& trajectory,
+ boost::shared_ptr<DerivableFunction> function,
+ unsigned order,
+ Problem<F, CLIST>& problem,
+ typename Function::interval_t bounds,
+ unsigned nConstraints)
+ {
+ using namespace boost;
+
+ for (unsigned i = 0; i < nConstraints; ++i)
+ {
+ const value_type t = (i + 1.) / (nConstraints + 1.);
+ assert (t > 0. && t < 1.);
+ shared_ptr<DerivableFunction> constraint
+ (new StateFunction (trajectory, function, t * tMax, order));
+ problem.addConstraint (constraint, bounds);
+ }
+ }
+
+ protected:
+ void impl_compute (result_t&, const argument_t&) const throw ();
+ void impl_gradient (gradient_t&, const argument_t&, size_type)
+ const throw ();
+
+ private:
+ const trajectory_t& trajectory_;
+ boost::shared_ptr<DerivableFunction> function_;
+ StableTimePoint tpt_;
+ size_type order_;
+ };
+
+ /// @}
+
+} // end of namespace roboptim.
+
+# include <roboptim/trajectory/fixed-point-state-function.hxx>
+#endif //! ROBOPTIM_TRAJECTORY_FIXED_POINT_STATE_FUNCTION_HH
diff --git a/include/roboptim/trajectory/fixed-point-state-function.hxx b/include/roboptim/trajectory/fixed-point-state-function.hxx
new file mode 100644
index 0000000..eceb978
--- /dev/null
+++ b/include/roboptim/trajectory/fixed-point-state-function.hxx
@@ -0,0 +1,94 @@
+// Copyright (C) 2009 by Florent Lamiraux, Thomas Moulard, AIST, CNRS, INRIA.
+//
+// This file is part of the roboptim.
+//
+// roboptim is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// roboptim is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with roboptim. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef ROBOPTIM_TRAJECTORY_FIXED_POINT_STATE_COST_HXX
+# define ROBOPTIM_TRAJECTORY_FIXED_POINT_STATE_COST_HXX
+# include <boost/format.hpp>
+
+namespace roboptim
+{
+ template <typename T>
+ FixedPointStateFunction<T>::FixedPointStateFunction
+ (const trajectory_t& trajectory,
+ boost::shared_ptr<DerivableFunction> function,
+ const StableTimePoint tpt,
+ size_type order) throw ()
+ : DerivableFunction (trajectory.parameters ().size (),
+ function->outputSize (),
+ (boost::format
+ ("fixed point state cost using function ``%1%''")
+ % function->getName ()).str ()),
+ trajectory_ (trajectory),
+ function_ (function),
+ tpt_ (tpt),
+ order_ (order)
+ {
+ assert (function_->inputSize () == trajectory_.outputSize () * (order + 1));
+ }
+
+ template <typename T>
+ FixedPointStateFunction<T>::~FixedPointStateFunction() throw ()
+ {
+ }
+
+ template <typename T>
+ typename FixedPointStateFunction<T>::size_type
+ FixedPointStateFunction<T>::order () const throw ()
+ {
+ return order_;
+ }
+
+ template <typename T>
+ void
+ FixedPointStateFunction<T>::impl_compute (result_t& res,
+ const argument_t& p) const throw ()
+ {
+ static boost::shared_ptr<T> updatedTrajectory =
+ boost::shared_ptr<trajectory_t>
+ (trajectory_->getFixedPointTrajectory ().clone ());
+ updatedTrajectory->setParameters (removeScaleFromParameters (p));
+ (*function_) (res, updatedTrajectory->state
+ (tpt_.getTime (updatedTrajectory->timeRange ()),
+ this->order_));
+ }
+
+ template <typename T>
+ void
+ FixedPointStateFunction<T>::impl_gradient (gradient_t& grad,
+ const argument_t& p,
+ size_type i) const throw ()
+ {
+ using namespace boost::numeric::ublas;
+
+ static boost::shared_ptr<T> updatedTrajectory =
+ boost::shared_ptr<trajectory_t>
+ (trajectory_->getFixedPointTrajectory ().clone ());
+ updatedTrajectory->setParameters (removeScaleFromParameters (p));
+
+ const value_type t = tpt_.getTime (updatedTrajectory->timeRange ());
+
+ grad[0] = 0.;
+ subrange (grad, 1, grad.size ()) =
+ prod (function_->gradient
+ (updatedTrajectory->state (t, this->order_), i),
+ updatedTrajectory->variationStateWrtParam (t, this->order_));
+ }
+
+} // end of namespace roboptim.
+
+#endif //! ROBOPTIM_TRAJECTORY_FIXED_POINT_STATE_COST_HXX
+
diff --git a/include/roboptim/trajectory/free-time-trajectory.hh b/include/roboptim/trajectory/free-time-trajectory.hh
index 3e315a1..08b537a 100644
--- a/include/roboptim/trajectory/free-time-trajectory.hh
+++ b/include/roboptim/trajectory/free-time-trajectory.hh
@@ -104,6 +104,12 @@ namespace roboptim
/// \param index Angles index in parameter array.
virtual void normalizeAngles (size_type index) throw ();
+ const Trajectory<DerivabilityOrder>& getFixedTimeTrajectory ()
+ const throw ()
+ {
+ assert (trajectory_);
+ return *trajectory_;
+ }
protected:
void impl_compute (result_t&, double) const throw ();
void impl_derivative (gradient_t& g, double x, size_type order)
-----------------------------------------------------------------------
Summary of changes:
ChangeLog | 9 +++
include/Makefile.am | 2 +
...e-function.hh => fixed-point-state-function.hh} | 18 +++---
...function.hxx => fixed-point-state-function.hxx} | 63 +++++++++++--------
.../roboptim/trajectory/free-time-trajectory.hh | 6 ++
5 files changed, 62 insertions(+), 36 deletions(-)
copy include/roboptim/trajectory/{state-function.hh => fixed-point-state-function.hh} (87%)
copy include/roboptim/trajectory/{state-function.hxx => fixed-point-state-function.hxx} (51%)
hooks/post-receive
--
roboptim-trajectory
|
