Commit d529bffa authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Use homogeneous matrix as input in HolonomicConstraint

parent ca1a10e9
......@@ -39,8 +39,8 @@ namespace dynamicgraph {
g1SIN(NULL, "HolonomicConstraint("+name+")::input(double)::g1"),
g2SIN(NULL, "HolonomicConstraint("+name+")::input(double)::g2"),
g3SIN(NULL, "HolonomicConstraint("+name+")::input(double)::g3"),
positionSIN (NULL, "HolonomicConstraint("+name+")::input(vector)::position"),
positionRefSIN(NULL, "HolonomicConstraint("+name+")::input(vector)::positionRef"),
positionSIN (NULL, "HolonomicConstraint("+name+")::input(matrixHomo)::position"),
positionRefSIN(NULL, "HolonomicConstraint("+name+")::input(matrixHomo)::positionRef"),
velocityRefSIN(NULL, "HolonomicConstraint("+name+")::input(vector)::velocityRef"),
errorSOUT ("HolonomicConstraint("+name+")::output(vector)::error"),
controlSOUT ("HolonomicConstraint("+name+")::output(vector)::control"),
......@@ -71,26 +71,33 @@ namespace dynamicgraph {
Vector& HolonomicConstraint::computeError (Vector& error, const int& time)
{
const Vector& p = positionSIN .access (time);
const Vector& p0 = positionRefSIN.access (time);
const MatrixHomogeneous& oMp = positionSIN .access (time);
const MatrixHomogeneous& oMpr = positionRefSIN.access (time);
MatrixHomogeneous pMpr = oMp.inverse() * oMpr;
Eigen::AngleAxisd aa (pMpr.linear());
error.resize(6);
error.head<3>() = pMpr.translation();
error.tail<3>() = aa.angle() * aa.axis();
error = p0.head<6>() - p.head<6>();
return error;
}
Vector& HolonomicConstraint::computeControl (Vector& control , const int& time)
{
const Vector& v0 = velocityRefSIN.access (time);
const Vector& vr = velocityRefSIN.access (time);
const MatrixHomogeneous& oMp = positionSIN .access (time);
const Vector& error = errorSOUT. access (time);
const double& g1 = g1SIN.access(time);
const double& g2 = g2SIN.access(time);
const double& g3 = g3SIN.access(time);
control.resize (6);
control.segment<4> (1).setZero();
control[0] = v0[0] * cos (error[5]) + g1 * error[0];
control[5] = v0[5] + g3 * error[5] + g2 * v0[0] * sinc(error[5]) * error[1];
control.head<3>() = oMp.linear().row(0) * (vr[0] * cos (error[5]) + g1 * error[0]);
control.tail<3>() = oMp.linear().row(2) * (vr[5] + g3 * error[5] + g2 * vr[0] * sinc(error[5]) * error[1]);
assert (!control.hasNaN());
return control;
}
......
......@@ -23,6 +23,8 @@
#include <dynamic-graph/pool.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/hpp/config.hh>
namespace dynamicgraph {
......@@ -59,8 +61,8 @@ namespace dynamicgraph {
SignalPtr <double, int> g2SIN;
SignalPtr <double, int> g3SIN;
SignalPtr <Vector, int> positionSIN;
SignalPtr <Vector, int> positionRefSIN;
SignalPtr <MatrixHomogeneous, int> positionSIN;
SignalPtr <MatrixHomogeneous, int> positionRefSIN;
SignalPtr <Vector, int> velocityRefSIN;
Signal <Vector, int> errorSOUT;
......
Markdown is supported
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