Commit 789f7464 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Merge branch 'master' of https://github.com/sheim/sot-core into devel

parents c2d73ecf 92971ccb
......@@ -5,6 +5,17 @@
*
* CNRS/AIST
*
* This file is part of sot-core.
* sot-core is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-core is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __SOT_Control_PD_HH__
......@@ -28,12 +39,12 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (control_pd_EXPORTS)
# define ControlPD_EXPORT __declspec(dllexport)
# else
# else
# define ControlPD_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define ControlPD_EXPORT
#endif
......@@ -64,13 +75,13 @@ namespace dynamicgraph {
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const {return CLASS_NAME;}
protected:
/* Parameters of the torque-control function:
protected:
/* Parameters of the torque-control function:
* tau = kp * (qd-q) + kd* (dqd-dq) */
double TimeStep;
double _dimension;
......@@ -84,11 +95,17 @@ namespace dynamicgraph {
SignalPtr<dg::Vector,int> velocitySIN;
SignalPtr<dg::Vector,int> desiredvelocitySIN;
SignalTimeDependent<dg::Vector,int> controlSOUT;
SignalTimeDependent<dg::Vector,int> positionErrorSOUT;
SignalTimeDependent<dg::Vector,int> velocityErrorSOUT;
protected:
double& setsize(int dimension);
dg::Vector& computeControl( dg::Vector& tau,int t );
dg::Vector position_error;
dg::Vector velocity_error;
dg::Vector& getPositionError( dg::Vector& position_error,int t );
dg::Vector& getVelocityError( dg::Vector& velocity_error,int t );
};
......
......@@ -19,7 +19,7 @@
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlPD,"ControlPD");
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlPD, "ControlPD");
const double ControlPD::
TIME_STEP_DEFAULT = .001;
......@@ -31,25 +31,30 @@ TIME_STEP_DEFAULT = .001;
#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" )
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)::desired_position")
,velocitySIN(NULL,"ControlPD("+name+")::input(vector)::velocity")
,desiredvelocitySIN(NULL,"ControlPD("+name+")::input(vector)::desired_velocity")
,controlSOUT( boost::bind(&ControlPD::computeControl,this,_1,_2),
KpSIN << KdSIN << positionSIN << desiredpositionSIN
<< velocitySIN << desiredvelocitySIN,
"ControlPD("+name+")::output(vector)::control" )
,positionErrorSOUT(boost::bind(&ControlPD::computeControl,this,_1,_2),
controlSOUT,
"ControlPD("+name+")::output(vector)::position_error")
,velocityErrorSOUT(boost::bind(&ControlPD::computeControl,this,_1,_2),
controlSOUT,
"ControlPD("+name+")::output(vector)::velocity_error")
{
init(TimeStep);
Entity::signalRegistration( KpSIN << KdSIN << positionSIN
<< desiredpositionSIN << velocitySIN
<< desiredvelocitySIN << controlSOUT );
Entity::signalRegistration( KpSIN << KdSIN << positionSIN <<
desiredpositionSIN << velocitySIN << desiredvelocitySIN << controlSOUT
<< positionErrorSOUT << velocityErrorSOUT );
}
void ControlPD::
......@@ -64,12 +69,11 @@ init(const double& Stept)
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void ControlPD::
display( std::ostream& os ) const
void ControlPD::display( std::ostream& os ) const
{
os << "ControlPD "<<getName();
try{
os <<"control = "<<controlSOUT;
os <<"control = "<<controlSOUT;
}
catch (ExceptionSignal e) {}
os <<" ("<<TimeStep<<") ";
......@@ -87,25 +91,43 @@ double& ControlPD::setsize(int dimension)
}
dynamicgraph::Vector& ControlPD::
computeControl( dynamicgraph::Vector &tau, int t )
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& desiredvelocity = desiredvelocitySIN(t);
dynamicgraph::Vector::Index size = Kp.size();
const dynamicgraph::Vector& desired_position = desiredpositionSIN(t);
const dynamicgraph::Vector& velocity = velocitySIN(t);
const dynamicgraph::Vector& desired_velocity = desiredvelocitySIN(t);
dynamicgraph::Vector::Index size = Kp.size();
tau.resize(size);
position_error.resize(size);
velocity_error.resize(size);
tau.array() = (desiredposition.array()-position.array())*Kp.array();
tau.array() = (desiredvelocity.array()-velocity.array())*Kd.array();
position_error.array() = desired_position.array()-position.array();
velocity_error.array() = desired_velocity.array()-velocity.array();
tau.array() = position_error.array()*Kp.array()
+ velocity_error.array()*Kd.array();
sotDEBUGOUT(15);
// std::cout << " tau " << tau << std::endl;
// std::cout << "velocity " << velocity << std::endl;
return tau;
}
dynamicgraph::Vector& ControlPD::
getPositionError( dynamicgraph::Vector &position_error, int t)
{
//sotDEBUGOUT(15) ??
controlSOUT(t);
return position_error;
}
dynamicgraph::Vector& ControlPD::
getVelocityError( dynamicgraph::Vector &velocity_error, int t)
{
controlSOUT(t);
return velocity_error;
}
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