Commit 461d1226 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Format

parent 90876e4c
......@@ -62,8 +62,10 @@ public:
void update(int time);
virtual dynamicgraph::Matrix &computeOutput(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Matrix &computeOutputBar(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Matrix &computeOutput(dynamicgraph::Matrix &res,
int time);
virtual dynamicgraph::Matrix &computeOutputBar(dynamicgraph::Matrix &res,
int time);
virtual MatrixHomogeneous &computeRef(MatrixHomogeneous &res, int time);
virtual void display(std::ostream &) const;
......
......@@ -62,7 +62,8 @@ public: /* --- SIGNAL --- */
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> freezedCoMSOUT;
public: /* --- FUNCTION --- */
dynamicgraph::Vector &computeFreezedCoM(dynamicgraph::Vector &freezedCoM, const int &time);
dynamicgraph::Vector &computeFreezedCoM(dynamicgraph::Vector &freezedCoM,
const int &time);
public: /* --- PARAMS --- */
virtual void display(std::ostream &os) const;
......
......@@ -39,7 +39,6 @@
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......
......@@ -80,8 +80,10 @@ protected:
dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t);
dynamicgraph::Vector position_error_;
dynamicgraph::Vector velocity_error_;
dynamicgraph::Vector &getPositionError(dynamicgraph::Vector &position_error, int t);
dynamicgraph::Vector &getVelocityError(dynamicgraph::Vector &velocity_error, int t);
dynamicgraph::Vector &getPositionError(dynamicgraph::Vector &position_error,
int t);
dynamicgraph::Vector &getVelocityError(dynamicgraph::Vector &velocity_error,
int t);
};
} // namespace sot
......
......@@ -141,8 +141,8 @@ public: /* --- SIGNALS --- */
protected:
/// Compute roll pitch yaw angles of freeflyer joint.
void integrateRollPitchYaw(dynamicgraph::Vector &state, const dynamicgraph::Vector &control,
double dt);
void integrateRollPitchYaw(dynamicgraph::Vector &state,
const dynamicgraph::Vector &control, double dt);
/// Store Position of free flyer joint
MatrixHomogeneous ffPose_;
/// Compute the new position, from the current control.
......
......@@ -18,7 +18,6 @@
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/config.hh>
namespace dynamicgraph {
namespace sot {
......
......@@ -98,10 +98,12 @@ public:
/*! \brief Compute the error between the desired value and the value itself.
*/
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
/*! \brief Compute the Jacobian of the value according to the robot state.. */
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
/*! @} */
......
......@@ -136,20 +136,23 @@ public:
\par[in] time: The time at which the error is computed.
\return The vector res with the appropriate value.
*/
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time) = 0;
virtual dynamicgraph::Vector &computeError(dynamicgraph::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 dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time) = 0;
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time) = 0;
/// Callback for signal errordotSOUT
///
/// Copy components of the input signal errordotSIN defined by selection
/// flag selectionSIN.
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
int time);
/*! @} */
......
......@@ -109,10 +109,12 @@ public:
/*! \brief Compute the error between the desired value and the value itself.
*/
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
/*! \brief Compute the Jacobian of the value according to the robot state.. */
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
/*! @} */
......
......@@ -83,9 +83,12 @@ public:
virtual unsigned int &getDimension(unsigned int &dim, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
dynamicgraph::Vector &computeWidthJl(dynamicgraph::Vector &res, const int &time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
dynamicgraph::Vector &computeWidthJl(dynamicgraph::Vector &res,
const int &time);
/** Static Feature selection. */
inline static Flags selectActuated(void);
......
......@@ -75,9 +75,12 @@ public:
virtual unsigned int &getDimension(unsigned int &dim, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
dynamicgraph::Vector &computeLineCoordinates(dynamicgraph::Vector &cood, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
dynamicgraph::Vector &computeLineCoordinates(dynamicgraph::Vector &cood,
int time);
virtual void display(std::ostream &os) const;
};
......
......@@ -60,7 +60,8 @@ protected:
/* --- SIGNALS ------------------------------------------------------------ */
public:
dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionReferenceSIN;
dynamicgraph::SignalPtr<dynamicgraph::Matrix, int> articularJacobianReferenceSIN;
dynamicgraph::SignalPtr<dynamicgraph::Matrix, int>
articularJacobianReferenceSIN;
/*! dynamicgraph::Signals related to the computation of the derivative of
the error
......@@ -80,9 +81,12 @@ public:
FeaturePoint6dRelative(const std::string &name);
virtual ~FeaturePoint6dRelative(void) {}
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
virtual void display(std::ostream &os) const;
......
......@@ -89,9 +89,12 @@ public:
virtual unsigned int &getDimension(unsigned int &dim, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector &res,
int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
/** Static Feature selection. */
inline static Flags selectX(void) { return FLAG_LINE_1; }
......
......@@ -120,21 +120,24 @@ public:
virtual unsigned int &getDimension(unsigned int &dim, int time);
/// Computes \f$ {}^oM^{-1}_{fa} {}^oM_{fb} \ominus {}^{fa}M^*_{fb} \f$
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
/// Computes \f$ \frac{\partial\ominus}{\partial b} X {}^{fa}\nu^*_{fafb} \f$.
/// There are two different cases, depending on the representation:
/// - R3xSO3Representation: \f$ X = \left( \begin{array}{cc} I_3 & [
/// {}^{fa}t_{fb} ] \\ 0_3 & {{}^{fa}R^*_{fb}}^T \end{array} \right) \f$
/// - SE3Representation: \f$ X = {{}^{fa}X^*_{fb}}^{-1} \f$ (see
/// pinocchio::SE3Base<Scalar,Options>::toActionMatrix)
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
int time);
/// Computes \f$ \frac{\partial\ominus}{\partial b} Y \left( {{}^{fb}X_{jb}}
/// {}^{jb}J_{jb} - {{}^{fb}X_{ja}} {}^{ja}J_{ja} \right) \f$. There are two
/// different cases, depending on the representation:
/// - R3xSO3Representation: \f$ Y = \left( \begin{array}{cc} {{}^{fa}R_{fb}} &
/// 0_3 \\ 0_3 & I_3 \end{array} \right) \f$
/// - SE3Representation: \f$ Y = I_6 \f$
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
/** Static Feature selection. */
inline static Flags selectX(void) { return FLAG_LINE_1; }
......@@ -160,10 +163,10 @@ private:
/// \todo Intermediate variables for internal computations
};
template < typename T >
template <typename T>
Vector6d convertVelocity(const MatrixHomogeneous &M,
const MatrixHomogeneous &Mdes,
const Vector &faNufafbDes);
const MatrixHomogeneous &Mdes,
const Vector &faNufafbDes);
#if __cplusplus >= 201103L
extern template class SOT_CORE_DLLAPI FeaturePose<SE3Representation>;
extern template class SOT_CORE_DLLAPI FeaturePose<R3xSO3Representation>;
......
......@@ -41,7 +41,7 @@ template <Representation_t representation> struct LG_t {
typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
R3xSO3_t>::type type;
};
}
} // namespace internal
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
......@@ -104,11 +104,7 @@ FeaturePose<representation>::FeaturePose(const std::string &pointName)
}
template <Representation_t representation>
FeaturePose<representation>::~FeaturePose()
{
}
FeaturePose<representation>::~FeaturePose() {}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -261,8 +257,8 @@ Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
// feature (R^3xSO(3) or SE(3)), in the right frame.
template <>
Vector6d convertVelocity<SE3_t>(const MatrixHomogeneous &M,
const MatrixHomogeneous &Mdes,
const Vector &faNufafbDes) {
const MatrixHomogeneous &Mdes,
const Vector &faNufafbDes) {
(void)M;
MatrixTwist X;
buildFrom(Mdes.inverse(Eigen::Affine), X);
......@@ -351,6 +347,5 @@ void FeaturePose<representation>::display(std::ostream &os) const {
}
}
}
}
} // namespace sot
} // namespace dynamicgraph
......@@ -53,7 +53,8 @@ class SOTFEATUREPOSTURE_EXPORT FeaturePosture : public FeatureAbstract {
public:
typedef dynamicgraph::SignalPtr<dynamicgraph::Vector, int> signalIn_t;
typedef dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> signalOut_t;
typedef dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
signalOut_t;
DECLARE_NO_REFERENCE;
......@@ -65,7 +66,8 @@ public:
protected:
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int);
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
int time);
signalIn_t state_;
signalIn_t posture_;
......
......@@ -40,7 +40,6 @@
namespace dynamicgraph {
namespace sot {
class SOTFEATURETASK_EXPORT FeatureTask : public FeatureGeneric {
public:
......
......@@ -70,8 +70,10 @@ public:
virtual unsigned int &getDimension(unsigned int &dim, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
virtual void display(std::ostream &os) const;
};
......
......@@ -74,8 +74,10 @@ public:
virtual unsigned int &getDimension(unsigned int &dim, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time);
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
int time);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
/** Static Feature selection. */
inline static Flags selectX(void) { return FLAG_LINE_1; }
......
......@@ -21,7 +21,6 @@
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/entity.h>
namespace dynamicgraph {
namespace sot {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment