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

Add position-controller.

parent 2b12ae94
......@@ -43,15 +43,6 @@
#include <initializer_list>
#include "boost/assign.hpp"
/* Metapod */
#include <metapod/models/hrp2_14/hrp2_14.hh>
#include <metapod/algos/rnea.hh>
#include <metapod/algos/jac.hh>
#include <metapod/tools/jcalc.hh>
#include <metapod/tools/bcalc.hh>
#include <metapod/tools/print.hh>
#include <metapod/tools/initconf.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
......@@ -72,7 +63,7 @@ namespace dynamicgraph {
/* --- CONSTRUCTOR ---- */
PositionController( const std::string & name );
void init(const double& dt);
void init(const double& dt,const double &nJoints);
void resetIntegral();
......@@ -103,6 +94,7 @@ namespace dynamicgraph {
}
protected:
unsigned int m_nJoints;
Eigen::VectorXd m_pwmDes;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
......
......@@ -32,7 +32,6 @@ namespace dynamicgraph
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;
using namespace metapod;
//Size to be aligned "-------------------------------------------------------"
#define PROFILE_PWM_DES_COMPUTATION "PositionController: desired pwm computation "
......@@ -76,15 +75,17 @@ namespace dynamicgraph
/* Commands. */
addCommand("init",
makeCommandVoid1(*this, &PositionController::init,
docCommandVoid1("Initialize the entity.",
"Time period in seconds (double)")));
makeCommandVoid2(*this, &PositionController::init,
docCommandVoid2("Initialize the entity.",
"Time period in seconds (double)",
"Size of the state vector (double)")));
addCommand("resetIntegral",
makeCommandVoid0(*this, &PositionController::resetIntegral,
docCommandVoid0("Reset the integral.")));
}
void PositionController::init(const double& dt)
void PositionController::init(const double& dt,
const double& nJoints)
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
......@@ -103,8 +104,9 @@ namespace dynamicgraph
if(!m_KiSIN.isPlugged())
return SEND_MSG("Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
m_nJoints = nJoints;
m_dt = dt;
m_pwmDes.setZero(N_JOINTS);
m_pwmDes.setZero(m_nJoints);
m_q.setZero();
m_dq.setZero();
......@@ -115,7 +117,7 @@ namespace dynamicgraph
void PositionController::resetIntegral()
{
m_e_integral.setZero(N_JOINTS);
m_e_integral.setZero(m_nJoints);
}
/* ------------------------------------------------------------------- */
......@@ -139,14 +141,14 @@ namespace dynamicgraph
const VectorN& qRef = m_qRefSIN(iter); // n
const VectorN& dqRef = m_dqRefSIN(iter); // n
assert(q.size()==N_JOINTS+6 && "Unexpected size of signal base6d_encoder");
assert(dq.size()==N_JOINTS && "Unexpected size of signal dq");
assert(qRef.size()==N_JOINTS && "Unexpected size of signal qRef");
assert(dqRef.size()==N_JOINTS && "Unexpected size of signal dqRef");
assert(Kp.size()==N_JOINTS && "Unexpected size of signal Kd");
assert(Kd.size()==N_JOINTS && "Unexpected size of signal Kd");
assert(q.size()==m_nJoints+6 && "Unexpected size of signal base6d_encoder");
assert(dq.size()==m_nJoints && "Unexpected size of signal dq");
assert(qRef.size()==m_nJoints && "Unexpected size of signal qRef");
assert(dqRef.size()==m_nJoints && "Unexpected size of signal dqRef");
assert(Kp.size()==m_nJoints && "Unexpected size of signal Kd");
assert(Kd.size()==m_nJoints && "Unexpected size of signal Kd");
m_pwmDes = Kp.cwiseProduct(qRef-q.tail<N_JOINTS>()) + Kd.cwiseProduct(dqRef-dq);
m_pwmDes = Kp.cwiseProduct(qRef-q.tail(m_nJoints)) + Kd.cwiseProduct(dqRef-dq);
if(s.size()!=N_JOINTS)
s.resize(N_JOINTS);
......@@ -165,14 +167,15 @@ namespace dynamicgraph
return s;
}
const VectorN6& q = m_base6d_encodersSIN(iter); //n+6
const VectorN& qRef = m_qRefSIN(iter); // n
assert(q.size()==N_JOINTS+6 && "Unexpected size of signal base6d_encoder");
assert(qRef.size()==N_JOINTS && "Unexpected size of signal qRef");
EIGEN_CONST_VECTOR_FROM_SIGNAL(q, m_base6d_encodersSIN(iter)); //n+6
EIGEN_CONST_VECTOR_FROM_SIGNAL(qRef, m_qRefSIN(iter)); // n
assert(q.size()==m_nJoints+6 && "Unexpected size of signal base6d_encoder");
assert(qRef.size()==m_nJoints && "Unexpected size of signal qRef");
if(s.size()!=N_JOINTS)
s.resize(N_JOINTS);
s = qRef - q.tail<N_JOINTS>();
if(s.size()!=m_nJoints)
s.resize(m_nJoints);
for(unsigned int i=0; i<m_nJoints; i++)
s(i)= qRef[i]-q(6+i);
return s;
}
......
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.torque_control.control_manager import *
from dynamic_graph.sot.torque_control.free_flyer_locator import *
from numpy import matrix, identity, zeros, eye, array, pi, ndarray
# Instanciate the free flyer
ffl = FreeFlyerLocator("ffl_test")
# Mapping with urdf to sot
ffl.setJointsUrdfToSot((12,13,14,15,23,24,25,26,27,28,29,16,17,18,19,20,21,22,6,7,8,9,10,11,0,1,2,3,4,5))
# Check if the map from URDF to SOT is done properly.
v=ffl.getJointsUrdfToSot()
v
# Initialize FFL
ffl.init("/opt/openrobots/share/hrp2_14_description/urdf/hrp2_14_reduced.urdf","RLEG_JOINT5","LLEG_JOINT5")
# TODO : Set the value of the encoders.
q=( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0, 0.0, 0.0, 0.0)
ffl.base6d_encoders.value = q
#ffl.freeflyer_aa.recompute(100)
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.torque_control.position_controller import *
from numpy import matrix, identity, zeros, eye, array, pi, ndarray
# Instanciate the free flyer
pc = PositionController("pc_test")
q=zeros(36)
pc.base6d_encoders.value=[36](0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
# Mapping with urdf to sot
pc.init(0.005,36)
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