Commit 42088d5f authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Merge improvment on Control PD using Eigen.

parent 3fa9b6b0
...@@ -42,9 +42,9 @@ ControlPD( const std::string & name ) ...@@ -42,9 +42,9 @@ ControlPD( const std::string & name )
,velocitySIN(NULL,"ControlPD("+name+")::input(vector)::velocity") ,velocitySIN(NULL,"ControlPD("+name+")::input(vector)::velocity")
,desiredvelocitySIN(NULL,"ControlPD("+name+")::input(vector)::desiredvelocity") ,desiredvelocitySIN(NULL,"ControlPD("+name+")::input(vector)::desiredvelocity")
,controlSOUT( boost::bind(&ControlPD::computeControl,this,_1,_2), ,controlSOUT( boost::bind(&ControlPD::computeControl,this,_1,_2),
KpSIN << KdSIN << positionSIN << desiredpositionSIN KpSIN << KdSIN << positionSIN << desiredpositionSIN
<< velocitySIN << desiredvelocitySIN, << velocitySIN << desiredvelocitySIN,
"ControlPD("+name+")::output(vector)::control" ) "ControlPD("+name+")::output(vector)::control" )
{ {
init(TimeStep); init(TimeStep);
Entity::signalRegistration( KpSIN << KdSIN << positionSIN << desiredpositionSIN << velocitySIN << desiredvelocitySIN << controlSOUT ); Entity::signalRegistration( KpSIN << KdSIN << positionSIN << desiredpositionSIN << velocitySIN << desiredvelocitySIN << controlSOUT );
...@@ -80,7 +80,7 @@ display( std::ostream& os ) const ...@@ -80,7 +80,7 @@ display( std::ostream& os ) const
double& ControlPD::setsize(int dimension) double& ControlPD::setsize(int dimension)
{ {
_dimension = dimension; _dimension = dimension;
return _dimension; return _dimension;
} }
...@@ -97,24 +97,9 @@ computeControl( dynamicgraph::Vector &tau, int t ) ...@@ -97,24 +97,9 @@ computeControl( dynamicgraph::Vector &tau, int t )
dynamicgraph::Vector::Index size = Kp.size(); dynamicgraph::Vector::Index size = Kp.size();
tau.resize(size); tau.resize(size);
for(unsigned i = 0u; i < size; ++i)
{ tau.array() = (desiredposition.array()-position.array())*Kp.array();
tau(i) = desiredposition(i); tau.array() = (desiredvelocity.array()-velocity.array())*Kd.array();
tau(i) -= position(i);
tau(i) *= Kp(i);
// std::cout << " position " << position << std::endl;
// std::cout << " tau1 " << tau << std::endl;
}
size = Kd.size();
double de = 0.0;
for(unsigned i = 0u; i < size; ++i)
{
de = desiredvelocity(i);
de -= velocity(i);
de *= Kd(i);
tau(i) += de;
}
sotDEBUGOUT(15); sotDEBUGOUT(15);
// std::cout << " tau " << tau << std::endl; // std::cout << " tau " << tau << std::endl;
......
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