Commit 65cbb2fe authored by Olivier Stasse's avatar Olivier Stasse

Merge remote-tracking branch 'remotes/origin/devel' into cmake-export

parents 4e870bd6 47ae37a3
......@@ -10,7 +10,6 @@ SET(NEWHEADERS
sot/core/clamp-workspace.hh
sot/core/com-freezer.hh
sot/core/contiifstream.hh
sot/core/constraint.hh
sot/core/debug.hh
sot/core/derivator.hh
sot/core/device.hh
......
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_CONSTRAINT_H__
#define __SOT_CONSTRAINT_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* STD */
#include <string>
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <sot/core/exception-signal.hh>
#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(constraint_EXPORTS)
#define SOTCONSTRAINT_EXPORT __declspec(dllexport)
#else
#define SOTCONSTRAINT_EXPORT __declspec(dllimport)
#endif
#else
#define SOTCONSTRAINT_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTCONSTRAINT_EXPORT Constraint : public TaskAbstract {
protected:
typedef std::list<Signal<dg::Matrix, int> *> JacobianList;
JacobianList jacobianList;
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public:
Constraint(const std::string &n);
void addJacobian(Signal<dg::Matrix, int> &sig);
void clearJacobianList(void);
void setControlSelection(const Flags &act);
void addControlSelection(const Flags &act);
void clearControlSelection(void);
/* --- COMPUTATION --- */
dg::Matrix &computeJacobian(dg::Matrix &J, int time);
/* --- DISPLAY ------------------------------------------------------------ */
SOTCONSTRAINT_EXPORT friend std::ostream &operator<<(std::ostream &os,
const Constraint &t);
};
} // namespace sot
} // namespace dynamicgraph
#endif /* #ifndef __SOT_CONSTRAINT_H__ */
......@@ -18,20 +18,20 @@ namespace sot {
class DoubleConstant : public Entity {
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public:
DoubleConstant( const std::string& name );
DoubleConstant(const std::string &name);
virtual ~DoubleConstant( void ){}
virtual ~DoubleConstant(void) {}
SignalTimeDependent<double,int> SOUT;
SignalTimeDependent<double, int> SOUT;
/// \brief Set value of vector (and therefore of output signal)
void setValue(const double& inValue);
void setValue(const double &inValue);
};
} // namespace sot
} // namespace dynamicgraph
#endif //DYNAMICGRAPH_SOT_DOUBLE_CONSTANT_H
#endif // DYNAMICGRAPH_SOT_DOUBLE_CONSTANT_H
......@@ -46,6 +46,7 @@ namespace dg = dynamicgraph;
\class FeaturePoint6dRelative
\brief Class that defines the motion of a point of the body wrt. another
point.
\deprecated This class was replaced by FeaturePose.
*/
class SOTFEATUREPOINT6DRELATIVE_EXPORT FeaturePoint6dRelative
: public FeaturePoint6d {
......
......@@ -44,7 +44,8 @@ namespace dg = dynamicgraph;
/*!
\class FeaturePoint6d
\brief Class that defines point-6d control feature
\brief Class that defines point-6d control feature.
\deprecated This class was replaced by FeaturePose.
*/
class SOTFEATUREPOINT6D_EXPORT FeaturePoint6d
: public FeatureAbstract,
......
......@@ -44,6 +44,14 @@ namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/** Exponentially decreasing gain.
* It follows the law \f[ g(e) = a \exp (-b ||e||) + c \f].
*
* The default values for
* - \f$ a = 0 \f$,
* - \f$ b = 0 \f$,
* - \f$ c = 0.1 \f$.
*/
class SOTGAINADAPTATIVE_EXPORT GainAdaptive : public dg::Entity {
public: /* --- CONSTANTS --- */
......@@ -75,9 +83,36 @@ public: /* --- INIT --- */
inline void init(const double &lambda) { init(lambda, lambda, 1.); }
void init(const double &valueAt0, const double &valueAtInfty,
const double &tanAt0);
/** \brief Set the gain
* by providing the value at 0, at \f$ \infty \f$ and the percentage of
* accomplishment between both to be reached when the error is
* \c errorReference.
*
* To visualize the curve of the gain versus the error, use
* \code{.py}
* from dynamic_graph.sot.core import GainAdaptive
* import numpy, matplotlib.pyplot as plt
* g = GainAdaptive('g')
* g.setByPoint(4.9, 0.001, 0.01, 0.1)
*
* errors = numpy.linspace(0, 0.1, 1000)
* def compute(e):
* t = g.error.time + 1
* g.error.value = (e,)
* g.error.time = t
* g.gain.recompute(t)
* return g.gain.value
*
* gains = [ compute(e) for e in errors ]
*
* lg = plt.plot(errors, gains, 'r', label="Gain")
* ld = plt.twinx().plot(errors, [ g*e for e,g in zip(errors,gains) ], 'b',
* label="Derivative") lines = lg + ld plt.legend(lines, [l.get_label() for l
* in lines]) plt.show() \endcode
*/
void initFromPassingPoint(const double &valueAt0, const double &valueAtInfty,
const double &errorReference,
const double &valueAtReference);
const double &percentage);
void forceConstant(void);
public: /* --- SIGNALS --- */
......
......@@ -44,6 +44,14 @@ namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/** \brief Hyperbolic gain.
* It follows the law \f[ g(e) = a \frac{\tanh(-b(||e|| - d)) + 1}{2} + c \f]
* The default coefficients are:
* - \f$ a = 0 \f$,
* - \f$ b = 0 \f$,
* - \f$ c = 0.1 \f$,
* - \f$ d = 0 \f$.
*/
class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic : public dg::Entity {
public: /* --- CONSTANTS --- */
......@@ -75,6 +83,12 @@ public: /* --- CONSTRUCTORS ---- */
public: /* --- INIT --- */
inline void init(void) { init(ZERO_DEFAULT, INFTY_DEFAULT, TAN_DEFAULT, 0); }
inline void init(const double &lambda) { init(lambda, lambda, 1., 0); }
/** Set the coefficients.
* - \f$ a = valueAt0 - valueAtInfty \f$,
* - \f$ b = \frac{tanAt0}{2*a} \f$, or \f$ b = 0 \f$ if \f$ a == 0 \f$,
* - \f$ c = valueAtInfty \f$,
* - \f$ d = decal0 \f$.
*/
void init(const double &valueAt0, const double &valueAtInfty,
const double &tanAt0, const double &decal0);
void forceConstant(void);
......
......@@ -27,15 +27,15 @@ namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace internal
{
template<class coefT>
bool integratorEulerCoeffIsIdentity(const coefT c) { return c == 1; }
namespace internal {
template <class coefT> bool integratorEulerCoeffIsIdentity(const coefT c) {
return c == 1;
}
bool integratorEulerCoeffIsIdentity(const Vector c) { return c.isOnes(); }
bool integratorEulerCoeffIsIdentity(const Vector c) { return c.isOnes(); }
bool integratorEulerCoeffIsIdentity(const Matrix c) { return c.isIdentity(); }
}
bool integratorEulerCoeffIsIdentity(const Matrix c) { return c.isIdentity(); }
} // namespace internal
/*!
* \class IntegratorEuler
......@@ -184,8 +184,10 @@ public:
// Check that denominator.back is the identity
if (!internal::integratorEulerCoeffIsIdentity(denominator.back()))
throw dg::ExceptionSignal (dg::ExceptionSignal::GENERIC,
"The coefficient of the highest order derivative of denominator should be 1 (the last pushDenomCoef should be the identity).");
throw dg::ExceptionSignal(
dg::ExceptionSignal::GENERIC,
"The coefficient of the highest order derivative of denominator "
"should be 1 (the last pushDenomCoef should be the identity).");
}
};
......
......@@ -20,7 +20,6 @@
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/constraint.hh>
/* -------------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------------ */
......
......@@ -30,9 +30,7 @@ public: // protected:
dg::Matrix Jp;
dg::Matrix PJp;
dg::Matrix Jff; //( nJ,FF_SIZE ); // Free-floating part
dg::Matrix Jact; //( nJ,mJ ); // Activated part
dg::Matrix JK; //(nJ,mJ);
dg::Matrix JK; //(nJ,mJ);
dg::Matrix Proj;
......@@ -40,13 +38,14 @@ public: // protected:
SVD_t svd;
public:
/* mJ is the number of actuated joints, nJ the number of feature in the task,
* and ffsize the number of unactuated DOF. */
/**
* \param mJ is the number of joints
* \param nJ the number of feature in the task
**/
MemoryTaskSOT(const std::string &name, const Matrix::Index nJ = 0,
const Matrix::Index mJ = 0, const Matrix::Index ffsize = 0);
const Matrix::Index mJ = 0);
virtual void initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
const Matrix::Index ffsize,
bool atConstruction = false);
public: /* --- ENTITY INHERITANCE --- */
......
......@@ -23,7 +23,6 @@ namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/entity.h>
#include <sot/core/constraint.hh>
#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>
......@@ -53,10 +52,7 @@ namespace sot {
/*! @ingroup stackoftasks
\brief This class implements the Stack of Task.
It allows to deal with the priority of the controllers
through the shell. The controllers can be either constraints
either tasks.
through the shell.
*/
class SOTSOT_CORE_EXPORT Sot : public Entity {
public:
......@@ -75,21 +71,6 @@ protected:
managed by the stack of tasks. */
StackType stack;
/*! \brief Defines a type for a list of constraints */
typedef std::list<Constraint *> ConstraintListType;
/*! \brief This field is a list of constraints
managed by the stack of tasks. */
ConstraintListType constraintList;
/*! \brief Defines an interval in the state vector of the robot
which is the free flyer. */
unsigned int ffJointIdFirst, ffJointIdLast;
/*! \brief Defines a default joint. */
static const unsigned int FF_JOINT_ID_DEFAULT = 0;
/* double directionalThreshold; */
/* bool useContiInverse; */
/*! \brief Store the number of joints to be used in the
command computed by the stack of tasks. */
unsigned int nbJoints;
......@@ -110,11 +91,6 @@ public:
/* static const bool USE_CONTI_INVERSE_DEFAULT = false; */
/*! \brief Number of joints by default. */
static dg::Matrix &computeJacobianConstrained(const dg::Matrix &Jac,
const dg::Matrix &K,
dg::Matrix &JK);
static dg::Matrix &computeJacobianConstrained(const TaskAbstract &task,
const dg::Matrix &K);
static void taskVectorToMlVector(const VectorMultiBound &taskVector,
Vector &err);
......@@ -164,23 +140,10 @@ public:
virtual void clear(void);
/*! @} */
/*! \name Methods to handle the constraints.
@{
*/
/*! \brief Add a constraint to the stack with the current level
of priority. */
virtual void addConstraint(Constraint &constraint);
/*! \brief Remove a constraint from the stack. */
virtual void removeConstraint(const Constraint &constraint);
/*! \brief Remove all the constraints from the stack. */
virtual void clearConstraint(void);
/*! @} */
/*! \brief This method defines the part of the state vector
which correspond to the free flyer of the robot. */
virtual void defineFreeFloatingJoints(const unsigned int &jointIdFirst,
const unsigned int &jointIdLast = -1);
virtual void defineNbDof(const unsigned int &nbDof);
virtual const unsigned int &getNbDof() const { return nbJoints; }
......@@ -194,10 +157,6 @@ public: /* --- CONTROL --- */
/*! \brief Compute the control law. */
virtual dg::Vector &computeControlLaw(dg::Vector &control, const int &time);
/*! \brief Compute the projector of the constraint. */
virtual dg::Matrix &computeConstraintProjector(dg::Matrix &Proj,
const int &time);
/*! @} */
public: /* --- DISPLAY --- */
......@@ -227,8 +186,6 @@ public: /* --- SIGNALS --- */
/*! \brief This signal allow to change the threshold for the
damped pseudo-inverse on-line */
SignalPtr<double, int> inversionThresholdSIN;
/*! \brief Allow to get the result of the Constraint projector. */
SignalTimeDependent<dg::Matrix, int> constraintSOUT;
/*! \brief Allow to get the result of the computed control law. */
SignalTimeDependent<dg::Vector, int> controlSOUT;
/*! @} */
......
......@@ -38,7 +38,6 @@ SET(plugins
task/gain-adaptive
task/task-pd
task/constraint
task/gain-hyperbolic
task/task
task/task-conti
......@@ -105,7 +104,7 @@ ENDIF(WIN32)
set(ADDITIONAL_feature-task_LIBS feature-generic task)
set(ADDITIONAL_feature-point6d-relative_LIBS feature-point6d)
set(ADDITIONAL_sot_LIBS constraint task)
set(ADDITIONAL_sot_LIBS task)
set(ADDITIONAL_sequencer_LIBS sot)
......
# flake8: noqa
from .constraint import *
from .exp_moving_avg import *
from .feature_generic import *
from .feature_joint_limits import *
......
......@@ -14,32 +14,27 @@
namespace dynamicgraph {
namespace sot {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DoubleConstant,"DoubleConstant");
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DoubleConstant, "DoubleConstant");
DoubleConstant::DoubleConstant(const std::string& name)
: Entity(name),
SOUT("DoubleConstant(" + name + ")::output(double)::sout")
{
DoubleConstant::DoubleConstant(const std::string &name)
: Entity(name), SOUT("DoubleConstant(" + name + ")::output(double)::sout") {
SOUT.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
signalRegistration(SOUT);
//
// Commands
// set
std::string docstring =
" \n"
" Set value of output signal\n"
" \n"
" input:\n"
" - a double\n"
" \n";
addCommand("set",
new ::dynamicgraph::command::Setter<DoubleConstant, double>
(*this, &DoubleConstant::setValue, docstring));
std::string docstring = " \n"
" Set value of output signal\n"
" \n"
" input:\n"
" - a double\n"
" \n";
addCommand("set", new ::dynamicgraph::command::Setter<DoubleConstant, double>(
*this, &DoubleConstant::setValue, docstring));
}
void DoubleConstant::setValue(const double& inValue)
{
void DoubleConstant::setValue(const double &inValue) {
SOUT.setConstant(inValue);
}
......
......@@ -38,7 +38,7 @@ public:
std::vector<Value> values = getParameterValues();
unsigned rows = values[0].value();
unsigned cols = values[1].value();
Matrix m (Matrix::Zero(rows, cols));
Matrix m(Matrix::Zero(rows, cols));
mc.SOUT.setConstant(m);
// return void
......
......@@ -32,13 +32,12 @@ MatrixConstant::MatrixConstant(const std::string &name)
// Resize
std::string docstring;
docstring =
" \n"
" Resize the matrix and set it to zero.\n"
" Input\n"
" - unsigned int: number of lines.\n"
" - unsigned int: number of columns.\n"
"\n";
docstring = " \n"
" Resize the matrix and set it to zero.\n"
" Input\n"
" - unsigned int: number of lines.\n"
" - unsigned int: number of columns.\n"
"\n";
addCommand("resize", new command::matrixConstant::Resize(*this, docstring));
// set
docstring = " \n"
......
......@@ -35,7 +35,7 @@ public:
VectorConstant &vc = static_cast<VectorConstant &>(owner());
std::vector<Value> values = getParameterValues();
unsigned size = values[0].value();
Vector m (Vector::Zero(size));
Vector m(Vector::Zero(size));
vc.SOUT.setConstant(m);
// return void
......
......@@ -32,12 +32,11 @@ VectorConstant::VectorConstant(const std::string &name)
//
// Resize
std::string docstring;
docstring =
" \n"
" Resize the vector and set it to zero.\n"
" Input\n"
" unsigned size.\n"
"\n";
docstring = " \n"
" Resize the vector and set it to zero.\n"
" Input\n"
" unsigned size.\n"
"\n";
addCommand("resize", new command::vectorConstant::Resize(*this, docstring));
// set
docstring = " \n"
......
......@@ -16,7 +16,7 @@ using namespace dynamicgraph;
const std::string MemoryTaskSOT::CLASS_NAME = "MemoryTaskSOT";
MemoryTaskSOT::MemoryTaskSOT(const std::string &name, const Matrix::Index nJ,
const Matrix::Index mJ, const Matrix::Index ffsize)
const Matrix::Index mJ)
: Entity(name),
jacobianInvSINOUT("sotTaskAbstract(" + name + ")::inout(matrix)::Jinv"),
jacobianConstrainedSINOUT("sotTaskAbstract(" + name +
......@@ -29,11 +29,10 @@ MemoryTaskSOT::MemoryTaskSOT(const std::string &name, const Matrix::Index nJ,
signalRegistration(jacobianInvSINOUT << singularBaseImageSINOUT << rankSINOUT
<< jacobianConstrainedSINOUT
<< jacobianProjectedSINOUT);
initMemory(nJ, mJ, ffsize, true);
initMemory(nJ, mJ, true);
}
void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
const Matrix::Index ffsize,
bool atConstruction) {
sotDEBUG(15) << "Task-mermory " << getName() << ": resize " << nJ << "x" << mJ
<< std::endl;
......@@ -42,9 +41,6 @@ void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
Jp.resize(mJ, nJ);
PJp.resize(mJ, nJ);
Jff.resize(nJ, ffsize);
Jact.resize(nJ, mJ);
JK.resize(nJ, mJ);
svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
......@@ -54,8 +50,6 @@ void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
Jt.setZero();
Jp.setZero();
PJp.setZero();
Jff.setZero();
Jact.setZero();
JK.setZero();
} else {
Eigen::pseudoInverse(Jt, Jp);
......
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#include <sot/core/rotation-simple.hh>
bool MATLAB::fullPrec = false;
MATLAB::MATLAB(const RotationSimple &Qh, const unsigned int nJ) {
Matrix eye = Eigen::MatrixXd::Identity(nJ, nJ);
Qh.multiplyRight(eye);
initFromBubMatrix(eye);
}
This diff is collapsed.
......@@ -22,29 +22,6 @@ namespace classSot {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
// Command AddConstraint
class AddConstraint : public Command {
public:
virtual ~AddConstraint() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
AddConstraint(Sot &entity, const std::string &docstring)
: Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
virtual Value doExecute() {
Sot &sot = static_cast<Sot &>(owner());
std::vector<Value> values = getParameterValues();
std::string constraintName = values[0].value();
Constraint &constraint = dynamic_cast<Constraint &>(
PoolStorage::getInstance()->getTask(constraintName));
sot.addConstraint(constraint);
sot.constraintSOUT.setReady();
// return void
return Value();
}
}; // class AddConstraint
// Command Push
class Push : public Command {
public:
......
This diff is collapsed.
This diff is collapsed.
......@@ -50,39 +50,22 @@ const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
Sot::Sot(const std::string &name)
: Entity(name), stack(), constraintList(),
ffJointIdFirst(FF_JOINT_ID_DEFAULT),
ffJointIdLast(FF_JOINT_ID_DEFAULT + 6)
,
nbJoints(0), taskGradient(0), recomputeEachTime(true),
: Entity(name), stack(), nbJoints(0), taskGradient(0),
recomputeEachTime(true),
q0SIN(NULL, "sotSOT(" + name + ")::input(double)::q0"),
proj0SIN(NULL, "sotSOT(" + name + ")::input(double)::proj0"),
inversionThresholdSIN(NULL,
"sotSOT(" + name + ")::input(double)::damping"),
constraintSOUT(
boost::bind(&Sot::computeConstraintProjector, this, _1, _2),
sotNOSIGNAL, "sotSOT(" + name + ")::output(matrix)::constraint"),
controlSOUT(boost::bind(&Sot::computeControlLaw, this, _1, _2),
constraintSOUT << inversionThresholdSIN << q0SIN << proj0SIN,
inversionThresholdSIN << q0SIN << proj0SIN,
"sotSOT(" + name + ")::output(vector)::control") {
inversionThresholdSIN = INVERSION_THRESHOLD_DEFAULT;
signalRegistration(inversionThresholdSIN << controlSOUT << constraintSOUT
<< q0SIN << proj0SIN);
signalRegistration(inversionThresholdSIN << controlSOUT << q0SIN << proj0SIN);
// Commands
//
std::string docstring;
// addConstraint
docstring = " \n"
" AddConstraint\n"
" \n"
" Input:\n"
" - a string: name of the constraint object\n"
" \n";
addCommand("addConstraint",
new command::classSot::AddConstraint(*this, docstring));
docstring = " \n"
" setNumberDofs.\n"
......@@ -269,52 +252,8 @@ void Sot::clear(void) {
controlSOUT.setReady();
}
/* --------------------------------------------------------------------- */
/* --- CONSTRAINTS ----------------------------------------------------- */
/* --------------------------------------------------------------------- */
void Sot::addConstraint(Constraint &constraint) {
constraintList.push_back(&constraint);
constraintSOUT.addDependency(constraint.jacobianSOUT);
}
void Sot::removeConstraint(const Constraint &key) {
bool find = false;
ConstraintListType::iterator it;
for (it = constraintList.begin(); constraintList.end() != it; ++it) {
if (*it == &key) {
find = true;
break;
}
}
if (!find) {
return;
}
constraintList.erase(it);
constraintSOUT.removeDependency(key.jacobianSOUT);
}
void Sot::clearConstraint(void) {
for (ConstraintListType::iterator it = constraintList.begin();
constraintList.end() != it; ++it) {
constraintSOUT.removeDependency((*it)->jacobianSOUT);
}
constraintList.clear();
}
void Sot::defineFreeFloatingJoints(const unsigned int &first,
const unsigned int &last) {
ffJointIdFirst = first;
if (last > 0)
ffJointIdLast = last;
else
ffJointIdLast = ffJointIdFirst + 6;
}
void Sot::defineNbDof(const unsigned int &nbDof) {
nbJoints = nbDof;
constraintSOUT.setReady();
controlSOUT.setReady();
}
......@@ -322,36 +261,6 @@ void Sot::defineNbDof(const unsigned int &nbDof) {
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
dynamicgraph::Matrix &
Sot::computeJacobianConstrained(const dynamicgraph::Matrix &Jac,
const dynamicgraph::Matrix &K,
dynamicgraph::Matrix &JK) {
const Matrix::Index nJ = Jac.rows();
const Matrix::Index mJ = K.cols();
const Matrix::Index nbConstraints = Jac.cols() - mJ;
if (nbConstraints == 0) {
JK = Jac;
return JK;
}
JK.resize(nJ, mJ);
JK.noalias() = Jac.leftCols(nbConstraints) * K;