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

Merge improvment on Control PD using Eigen.

parent 3fa9b6b0
......@@ -97,24 +97,9 @@ computeControl( dynamicgraph::Vector &tau, int t )
dynamicgraph::Vector::Index size = Kp.size();
tau.resize(size);
for(unsigned i = 0u; i < size; ++i)
{
tau(i) = desiredposition(i);
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;
}
tau.array() = (desiredposition.array()-position.array())*Kp.array();
tau.array() = (desiredvelocity.array()-velocity.array())*Kd.array();
sotDEBUGOUT(15);
// 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