Commit cc841787 authored by Steve Heim's avatar Steve Heim Committed by Olivier Stasse
Browse files

compute PD control using eigen operations instead of for loop

parent 42088d5f
......@@ -87,23 +87,24 @@ double& ControlPD::setsize(int 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;
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