Commit c206f480 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[control] Merge sheim full pull request.

parent cc841787
Pipeline #3564 passed with stage
in 28 minutes and 22 seconds
......@@ -29,22 +29,22 @@ TIME_STEP_DEFAULT = .001;
/* --------------------------------------------------------------------- */
#define __SOT_ControlPD_INIT \
#define __SOT_ControlPD_INIT \
ControlPD::
ControlPD( const std::string & name )
:Entity(name)
,TimeStep(0)
,KpSIN(NULL,"ControlPD("+name+")::input(vector)::Kp")
,KdSIN(NULL,"ControlPD("+name+")::input(vector)::Kd")
,positionSIN(NULL,"ControlPD("+name+")::input(vector)::position")
,desiredpositionSIN(NULL,"ControlPD("+name+")::input(vector)::desiredposition")
,velocitySIN(NULL,"ControlPD("+name+")::input(vector)::velocity")
,desiredvelocitySIN(NULL,"ControlPD("+name+")::input(vector)::desiredvelocity")
,controlSOUT( boost::bind(&ControlPD::computeControl,this,_1,_2),
KpSIN << KdSIN << positionSIN << desiredpositionSIN
<< velocitySIN << desiredvelocitySIN,
"ControlPD("+name+")::output(vector)::control" )
:Entity(name)
,TimeStep(0)
,KpSIN(NULL,"ControlPD("+name+")::input(vector)::Kp")
,KdSIN(NULL,"ControlPD("+name+")::input(vector)::Kd")
,positionSIN(NULL,"ControlPD("+name+")::input(vector)::position")
,desiredpositionSIN(NULL,"ControlPD("+name+")::input(vector)::desiredposition")
,velocitySIN(NULL,"ControlPD("+name+")::input(vector)::velocity")
,desiredvelocitySIN(NULL,"ControlPD("+name+")::input(vector)::desiredvelocity")
,controlSOUT( boost::bind(&ControlPD::computeControl,this,_1,_2),
KpSIN << KdSIN << positionSIN << desiredpositionSIN
<< velocitySIN << desiredvelocitySIN,
"ControlPD("+name+")::output(vector)::control" )
{
init(TimeStep);
Entity::signalRegistration( KpSIN << KdSIN << positionSIN << desiredpositionSIN << velocitySIN << desiredvelocitySIN << controlSOUT );
......@@ -81,30 +81,29 @@ double& ControlPD::setsize(int dimension)
{
_dimension = dimension;
return _dimension;
return _dimension;
}
dynamicgraph::Vector& ControlPD::
computeControl( dynamicgraph::Vector &tau, int t )
{
sotDEBUGIN(15);
sotDEBUGIN(15);
const dynamicgraph::Vector& Kp = KpSIN(t);
const dynamicgraph::Vector& Kd = KdSIN(t);
const dynamicgraph::Vector& Kd = KdSIN(t);
const dynamicgraph::Vector& position = positionSIN(t);
const dynamicgraph::Vector& desiredposition = desiredpositionSIN(t);
const dynamicgraph::Vector& velocity = velocitySIN(t);
const dynamicgraph::Vector& velocity = velocitySIN(t);
const dynamicgraph::Vector& desiredvelocity = desiredvelocitySIN(t);
dynamicgraph::Vector::Index size = Kp.size();
dynamicgraph::Vector::Index size = Kp.size();
tau.resize(size);
tau.array() = (desiredposition.array()-position.array())*Kp.array();
tau.array() = (desiredvelocity.array()-velocity.array())*Kd.array();
sotDEBUGOUT(15);
// std::cout << " tau " << tau << std::endl;
// std::cout << "velocity " << velocity << std::endl;
// std::cout << " tau " << tau << std::endl;
// std::cout << "velocity " << velocity << std::endl;
return tau;
}
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