Commit 4fa17b4c authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[eigen] Replacing jrl-mal with eigen. Removed solvers sot-h, weighted-sot,...

[eigen] Replacing jrl-mal with eigen. Removed solvers sot-h, weighted-sot, solver-hierarchical-inequalities.

>Todo: computederivation template specialization for vectorquaternion in derivator.h
>Removed solvers
>does not build
parent 0e77ee03
......@@ -17,6 +17,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/cpack.cmake)
......@@ -43,8 +44,8 @@ PKG_CONFIG_APPEND_LIBS("sot-core")
# Boost
SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework system regex )
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.8.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 1.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
......
......@@ -18,10 +18,8 @@ SET(NEWHEADERS
sot/core/abstract-sot-external-interface.hh
sot/core/device.hh
sot/core/robot-simu.hh
sot/core/matrix-homogeneous.hh
sot/core/matrix-rotation.hh
sot/core/vector-utheta.hh
sot/core/vector-rotation.hh
sot/core/matrix-geometry.hh
sot/core/matrix-svd.hh
sot/core/contiifstream.hh
sot/core/debug.hh
sot/core/exception-abstract.hh
......@@ -44,11 +42,7 @@ SET(NEWHEADERS
sot/core/factory.hh
sot/core/macros-signal.hh
sot/core/pool.hh
sot/core/matrix-force.hh
sot/core/matrix-twist.hh
sot/core/op-point-modifier.hh
sot/core/vector-quaternion.hh
sot/core/vector-roll-pitch-yaw.hh
sot/core/feature-point6d.hh
sot/core/feature-vector3.hh
sot/core/feature-abstract.hh
......@@ -71,13 +65,8 @@ SET(NEWHEADERS
sot/core/task.hh
sot/core/gain-hyperbolic.hh
sot/core/flags.hh
sot/core/sot-qr.hh
sot/core/memory-task-sot.hh
sot/core/sot-h.hh
sot/core/sot.hh
sot/core/rotation-simple.hh
sot/core/weighted-sot.hh
sot/core/solver-hierarchical-inequalities.hh
sot/core/reader.hh
sot/core/utils-windows.hh
sot/core/time-stamp.hh
......
......@@ -26,14 +26,14 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <jrl/mal/boost.hh>
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <sot/core/flags.hh>
#include <dynamic-graph/entity.h>
#include <sot/core/pool.hh>
#include <dynamic-graph/all-signals.h>
#include <sot/core/vector-quaternion.hh>
#include <sot/core/matrix-geometry.hh>
/* STD */
#include <string>
......
......@@ -25,17 +25,14 @@
#include <utility>
/* Matrix */
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/entity.h>
#include <sot/core/exception-task.hh>
#include <dynamic-graph/all-signals.h>
#include <sot/core/vector-roll-pitch-yaw.hh>
#include <sot/core/matrix-rotation.hh>
#include <sot/core/matrix-homogeneous.hh>
#include <sot/core/matrix-twist.hh>
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
......@@ -71,8 +68,8 @@ class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace
dg::SignalPtr< MatrixHomogeneous,int > positionrefSIN;
dg::SignalPtr< MatrixHomogeneous,int > positionSIN;
dg::SignalTimeDependent< ml::Matrix,int > alphaSOUT;
dg::SignalTimeDependent< ml::Matrix,int > alphabarSOUT;
dg::SignalTimeDependent< dg::Matrix,int > alphaSOUT;
dg::SignalTimeDependent< dg::Matrix,int > alphabarSOUT;
dg::SignalTimeDependent< MatrixHomogeneous,int > handrefSOUT;
public:
......@@ -82,8 +79,8 @@ class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace
void update( int time );
virtual ml::Matrix& computeOutput( ml::Matrix& res, int time );
virtual ml::Matrix& computeOutputBar( ml::Matrix& res, int time );
virtual dg::Matrix& computeOutput( dg::Matrix& res, int time );
virtual dg::Matrix& computeOutputBar( dg::Matrix& res, int time );
virtual MatrixHomogeneous& computeRef( MatrixHomogeneous& res, int time );
virtual void display( std::ostream& ) const;
......@@ -95,10 +92,10 @@ class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace
int timeUpdate;
ml::Matrix alpha;
ml::Matrix alphabar;
dg::Matrix alpha;
dg::Matrix alphabar;
MatrixHomogeneous prefMp;
ml::Vector pd;
dg::Vector pd;
MatrixRotation Rd;
MatrixHomogeneous handref;
......
......@@ -26,8 +26,8 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/entity.h>
......@@ -64,7 +64,7 @@ class SOTCOMFREEZER_EXPORT CoMFreezer
virtual const std::string & getClassName() const { return CLASS_NAME; }
private:
ml::Vector m_lastCoM;
dg::Vector m_lastCoM;
bool m_previousPGInProcess;
int m_lastStopTime;
......@@ -73,12 +73,12 @@ class SOTCOMFREEZER_EXPORT CoMFreezer
virtual ~CoMFreezer(void);
public: /* --- SIGNAL --- */
dg::SignalPtr<ml::Vector, int> CoMRefSIN;
dg::SignalPtr<dg::Vector, int> CoMRefSIN;
dg::SignalPtr<unsigned, int> PGInProcessSIN;
dg::SignalTimeDependent<ml::Vector, int> freezedCoMSOUT;
dg::SignalTimeDependent<dg::Vector, int> freezedCoMSOUT;
public: /* --- FUNCTION --- */
ml::Vector& computeFreezedCoM(ml::Vector & freezedCoM, const int& time);
dg::Vector& computeFreezedCoM(dg::Vector & freezedCoM, const int& time);
public: /* --- PARAMS --- */
virtual void display(std::ostream & os) const;
......
......@@ -26,8 +26,8 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* STD */
#include <string>
......@@ -66,7 +66,7 @@ namespace dynamicgraph {
: public TaskAbstract
{
protected:
typedef std::list< Signal<ml::Matrix,int>* > JacobianList;
typedef std::list< Signal<dg::Matrix,int>* > JacobianList;
JacobianList jacobianList;
public:
......@@ -76,7 +76,7 @@ namespace dynamicgraph {
public:
Constraint( const std::string& n );
void addJacobian( Signal<ml::Matrix,int>& sig );
void addJacobian( Signal<dg::Matrix,int>& sig );
void clearJacobianList( void );
void setControlSelection( const Flags& act );
......@@ -84,7 +84,7 @@ namespace dynamicgraph {
void clearControlSelection( void );
/* --- COMPUTATION --- */
ml::Matrix& computeJacobian( ml::Matrix& J,int time );
dg::Matrix& computeJacobian( dg::Matrix& J,int time );
/* --- DISPLAY ------------------------------------------------------------ */
SOTCONSTRAINT_EXPORT friend std::ostream& operator<< ( std::ostream& os,const Constraint& t );
......
......@@ -26,8 +26,8 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/signal-time-dependent.h>
......@@ -90,15 +90,15 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
SignalPtr<ml::Matrix,int> matrixASIN;
SignalPtr<ml::Vector,int> accelerationSIN;
SignalPtr<ml::Vector,int> gravitySIN;
SignalTimeDependent<ml::Vector,int> controlSOUT;
SignalPtr<dg::Matrix,int> matrixASIN;
SignalPtr<dg::Vector,int> accelerationSIN;
SignalPtr<dg::Vector,int> gravitySIN;
SignalTimeDependent<dg::Vector,int> controlSOUT;
protected:
double& setsize(int dimension);
ml::Vector& computeControl( ml::Vector& tau,int t );
dg::Vector& computeControl( dg::Vector& tau,int t );
};
......
......@@ -26,8 +26,8 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/signal-time-dependent.h>
......@@ -88,18 +88,18 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
SignalPtr<ml::Vector,int> KpSIN;
SignalPtr<ml::Vector,int> KdSIN;
SignalPtr<ml::Vector,int> positionSIN;
SignalPtr<ml::Vector,int> desiredpositionSIN;
SignalPtr<ml::Vector,int> velocitySIN;
SignalPtr<ml::Vector,int> desiredvelocitySIN;
SignalTimeDependent<ml::Vector,int> controlSOUT;
SignalPtr<dg::Vector,int> KpSIN;
SignalPtr<dg::Vector,int> KdSIN;
SignalPtr<dg::Vector,int> positionSIN;
SignalPtr<dg::Vector,int> desiredpositionSIN;
SignalPtr<dg::Vector,int> velocitySIN;
SignalPtr<dg::Vector,int> desiredvelocitySIN;
SignalTimeDependent<dg::Vector,int> controlSOUT;
protected:
double& setsize(int dimension);
ml::Vector& computeControl( ml::Vector& tau,int t );
dg::Vector& computeControl( dg::Vector& tau,int t );
};
......
......@@ -22,7 +22,7 @@
#define __SOT_DERIVATOR_IMPL_H__
#include <sot/core/derivator.hh>
#include <sot/core/vector-quaternion.hh>
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
......@@ -58,8 +58,8 @@ namespace dg = dynamicgraph;
#endif
DECLARE_SPECIFICATION(DerivatorDouble,double)
DECLARE_SPECIFICATION(DerivatorVector,ml::Vector)
DECLARE_SPECIFICATION(DerivatorMatrix,ml::Matrix)
DECLARE_SPECIFICATION(DerivatorVector,dg::Vector)
DECLARE_SPECIFICATION(DerivatorMatrix,dg::Matrix)
DECLARE_SPECIFICATION(DerivatorVectorQuaternion,VectorQuaternion)
} /* namespace sot */} /* namespace dynamicgraph */
......
......@@ -26,15 +26,14 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <sot/core/flags.hh>
#include <sot/core/pool.hh>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <sot/core/vector-quaternion.hh>
#include <sot/core/matrix-geometry.hh>
/* STD */
#include <string>
......
......@@ -25,14 +25,13 @@
/* --------------------------------------------------------------------- */
/* -- MaaL --- */
#include <jrl/mal/boost.hh>
namespace ml= maal::boost;
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include "sot/core/vector-roll-pitch-yaw.hh"
#include "sot/core/periodic-call.hh"
#include "sot/core/matrix-homogeneous.hh"
#include <sot/core/matrix-geometry.hh>
#include "sot/core/api.hh"
namespace dynamicgraph {
......@@ -74,10 +73,10 @@ namespace dynamicgraph {
};
protected:
ml::Vector state_;
ml::Vector velocity_;
dg::Vector state_;
dg::Vector velocity_;
bool vel_controlInit_;
ml::Vector vel_control_;
dg::Vector vel_control_;
ControlInput controlInputType_;
bool withForceSignals[4];
PeriodicCall periodicCallBefore_;
......@@ -90,9 +89,9 @@ namespace dynamicgraph {
virtual ~Device();
virtual void setStateSize(const unsigned int& size);
virtual void setState(const ml::Vector& st);
virtual void setState(const dg::Vector& st);
void setVelocitySize(const unsigned int& size);
virtual void setVelocity(const ml::Vector & vel);
virtual void setVelocity(const dg::Vector & vel);
virtual void setSecondOrderIntegration();
virtual void setControlInputType(const std::string& cit);
virtual void increment(const double & dt = 5e-2);
......@@ -106,29 +105,29 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
dynamicgraph::SignalPtr<ml::Vector,int> controlSIN;
dynamicgraph::SignalPtr<ml::Vector,int> attitudeSIN;
dynamicgraph::SignalPtr<ml::Vector,int> zmpSIN;
dynamicgraph::SignalPtr<dg::Vector,int> controlSIN;
dynamicgraph::SignalPtr<dg::Vector,int> attitudeSIN;
dynamicgraph::SignalPtr<dg::Vector,int> zmpSIN;
dynamicgraph::Signal<ml::Vector,int> stateSOUT;
dynamicgraph::Signal<ml::Vector,int> velocitySOUT;
dynamicgraph::Signal<dg::Vector,int> stateSOUT;
dynamicgraph::Signal<dg::Vector,int> velocitySOUT;
dynamicgraph::Signal<MatrixRotation,int> attitudeSOUT;
dynamicgraph::Signal<ml::Vector,int>* forcesSOUT[4];
dynamicgraph::Signal<dg::Vector,int>* forcesSOUT[4];
dynamicgraph::Signal<ml::Vector,int> pseudoTorqueSOUT;
dynamicgraph::Signal<ml::Vector,int> previousControlSOUT;
dynamicgraph::Signal<dg::Vector,int> pseudoTorqueSOUT;
dynamicgraph::Signal<dg::Vector,int> previousControlSOUT;
/*! \brief The current state of the robot from the command viewpoint. */
dynamicgraph::Signal<ml::Vector,int> motorcontrolSOUT;
dynamicgraph::Signal<dg::Vector,int> motorcontrolSOUT;
/*! \brief The ZMP reference send by the previous controller. */
dynamicgraph::Signal<ml::Vector,int> ZMPPreviousControllerSOUT;
dynamicgraph::Signal<dg::Vector,int> ZMPPreviousControllerSOUT;
public: /* --- COMMANDS --- */
void commandLine(const std::string&, std::istringstream&,
std::ostream&){}
protected:
/// Compute roll pitch yaw angles of freeflyer joint.
void integrateRollPitchYaw(ml::Vector& state, const ml::Vector& control,
void integrateRollPitchYaw(dg::Vector& state, const dg::Vector& control,
double dt);
/// Store Position of free flyer joint
MatrixHomogeneous ffPose_;
......@@ -138,11 +137,13 @@ namespace dynamicgraph {
/// Get freeflyer pose
const MatrixHomogeneous& freeFlyerPose() const;
public:
virtual void setRoot( const ml::Matrix & root );
virtual void setRoot( const dg::Matrix & root );
virtual void setRoot( const MatrixHomogeneous & worldMwaist );
private:
// Intermediate variable to avoid dynamic allocation
ml::Vector forceZero6;
dg::Vector forceZero6;
};
} // namespace sot
} // namespace dynamicgraph
......
......@@ -77,10 +77,10 @@ class SOTFEATURE1D_EXPORT Feature1D
@{
*/
/*! \brief Input for the error. */
dg::SignalPtr< ml::Vector,int > errorSIN;
dg::SignalPtr< dg::Vector,int > errorSIN;
/*! \brief Input for the Jacobian. */
dg::SignalPtr< ml::Matrix,int > jacobianSIN;
dg::SignalPtr< dg::Matrix,int > jacobianSIN;
/*! @} */
......@@ -109,10 +109,10 @@ class SOTFEATURE1D_EXPORT Feature1D
*/
/*! \brief Compute the error between the desired value and the value itself. */
virtual ml::Vector& computeError( ml::Vector& res,int time );
virtual dg::Vector& computeError( dg::Vector& res,int time );
/*! \brief Compute the Jacobian of the value according to the robot state.. */
virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time );
virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
/*! @} */
......
......@@ -26,8 +26,8 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
#include <sot/core/flags.hh>
......@@ -131,20 +131,20 @@ namespace dynamicgraph {
\par[in] time: The time at which the error is computed.
\return The vector res with the appropriate value.
*/
virtual ml::Vector& computeError( ml::Vector& res,int time ) = 0;
virtual dg::Vector& computeError( dg::Vector& res,int time ) = 0;
/*! \brief Compute the Jacobian of the error according the robot state.
\par[out] res: The matrix in which the error will be written.
\return The matrix res with the appropriate values.
*/
virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time ) = 0;
virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time ) = 0;
/// Callback for signal errordotSOUT
///
/// Copy components of the input signal errordotSIN defined by selection
/// flag selectionSIN.
virtual ml::Vector& computeErrorDot (ml::Vector& res,int time);
virtual dg::Vector& computeErrorDot (dg::Vector& res,int time);
/*! @} */
......@@ -164,7 +164,7 @@ namespace dynamicgraph {
SignalPtr< Flags,int > selectionSIN;
/// Derivative of the reference value.
SignalPtr< ml::Vector,int > errordotSIN;
SignalPtr< dg::Vector,int > errordotSIN;
/*! @} */
......@@ -173,14 +173,14 @@ namespace dynamicgraph {
/*! \brief This signal returns the error between the desired value and
the current value : \f$ {\bf s}^*(t) - {\bf s}(t)\f$ */
SignalTimeDependent<ml::Vector,int> errorSOUT;
SignalTimeDependent<dg::Vector,int> errorSOUT;
/// Derivative of the reference value.
SignalTimeDependent< ml::Vector,int > errordotSOUT;
SignalTimeDependent< dg::Vector,int > errordotSOUT;
/*! \brief This signal returns the Jacobian of the current value
according to the robot state: \f$ J(t) = \frac{\delta{\bf s}^*(t)}{\delta {\bf q}(t)}\f$ */
SignalTimeDependent<ml::Matrix,int> jacobianSOUT;
SignalTimeDependent<dg::Matrix,int> jacobianSOUT;
/*! \brief Returns the dimension of the feature as an output signal. */
SignalTimeDependent<unsigned int,int> dimensionSOUT;
......@@ -193,7 +193,7 @@ namespace dynamicgraph {
{
return true;
}
virtual SignalTimeDependent<ml::Vector,int>& getErrorDot()
virtual SignalTimeDependent<dg::Vector,int>& getErrorDot()
{
return errordotSOUT;
}
......
......@@ -85,10 +85,10 @@ class SOTFEATUREGENERIC_EXPORT FeatureGeneric
@{
*/
/*! \brief Input for the error. */
dg::SignalPtr< ml::Vector,int > errorSIN;
dg::SignalPtr< dg::Vector,int > errorSIN;
/*! \brief Input for the Jacobian. */
dg::SignalPtr< ml::Matrix,int > jacobianSIN;
dg::SignalPtr< dg::Matrix,int > jacobianSIN;
/*! @} */
......@@ -118,10 +118,10 @@ class SOTFEATUREGENERIC_EXPORT FeatureGeneric
*/
/*! \brief Compute the error between the desired value and the value itself. */
virtual ml::Vector& computeError( ml::Vector& res,int time );
virtual dg::Vector& computeError( dg::Vector& res,int time );
/*! \brief Compute the Jacobian of the value according to the robot state.. */
virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time );
virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
/*! @} */
......
......@@ -74,10 +74,10 @@ class SOTFEATUREJOINTLIMITS_EXPORT FeatureJointLimits
/* --- SIGNALS ------------------------------------------------------------ */
public:
dg::SignalPtr< ml::Vector,int > jointSIN;
dg::SignalPtr< ml::Vector,int > upperJlSIN;
dg::SignalPtr< ml::Vector,int > lowerJlSIN;
dg::SignalTimeDependent< ml::Vector,int > widthJlSINTERN;
dg::SignalPtr< dg::Vector,int > jointSIN;
dg::SignalPtr< dg::Vector,int > upperJlSIN;
dg::SignalPtr< dg::Vector,int > lowerJlSIN;
dg::SignalTimeDependent< dg::Vector,int > widthJlSINTERN;
using FeatureAbstract::selectionSIN;
......@@ -96,9 +96,9 @@ class SOTFEATUREJOINTLIMITS_EXPORT FeatureJointLimits
virtual unsigned int& getDimension( unsigned int & dim, int time );
virtual ml::Vector& computeError( ml::Vector& res,int time );
virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time );
ml::Vector& computeWidthJl( ml::Vector& res,const int& time );
virtual dg::Vector& computeError( dg::Vector& res,int time );
virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
dg::Vector& computeWidthJl( dg::Vector& res,const int& time );
/** Static Feature selection. */
inline static Flags selectActuated( void );
......
......@@ -28,8 +28,7 @@
/* SOT */
#include <sot/core/feature-abstract.hh>
#include <sot/core/exception-task.hh>
#include <sot/core/matrix-homogeneous.hh>
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -68,10 +67,10 @@ class SOTFEATURELINEDISTANCE_EXPORT FeatureLineDistance
/* --- SIGNALS ------------------------------------------------------------ */
public:
dg::SignalPtr< MatrixHomogeneous,int > positionSIN;
dg::SignalPtr< ml::Matrix,int > articularJacobianSIN;
dg::SignalPtr< ml::Vector,int > positionRefSIN;
dg::SignalPtr< ml::Vector,int > vectorSIN;
dg::SignalTimeDependent<ml::Vector,int> lineSOUT;
dg::SignalPtr< dg::Matrix,int > articularJacobianSIN;
dg::SignalPtr< dg::Vector,int > positionRefSIN;
dg::SignalPtr< dg::Vector,int > vectorSIN;
dg::SignalTimeDependent<dg::Vector,int> lineSOUT;
using FeatureAbstract::selectionSIN;
using FeatureAbstract::jacobianSOUT;
......@@ -90,9 +89,9 @@ class SOTFEATURELINEDISTANCE_EXPORT FeatureLineDistance
virtual unsigned int& getDimension( unsigned int & dim, int time );
virtual ml::Vector& computeError( ml::Vector& res,int time );
virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time );
ml::Vector& computeLineCoordinates( ml::Vector& cood,int time );
virtual dg::Vector& computeError( dg::Vector& res,int time );
virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
dg::Vector& computeLineCoordinates( dg::Vector& cood,int time );
virtual void display( std::ostream& os ) const;
......
......@@ -29,7 +29,7 @@
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-point6d.hh>