Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/catkin/dg_control/dg_tools/src/control/control_pd.cpp	Lines:	18	48	37.5 %
Date:	2020-04-15 11:50:02	Branches:	56	152	36.8 %


/*
 * Copyright 2010,
 * Fran��ois Bleibel,
 * Olivier Stasse,
 *
 * 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/>.
 */

/* SOT */
#include "dg_tools/control/control_pd.hpp"

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>

using namespace dynamicgraph::sot;
using namespace dynamicgraph;

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PDController, "PDController");

const double PDController::
TIME_STEP_DEFAULT = .001;

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */


#define __SOT_ControlPD_INIT \

PDController::PDController( const std::string & name )
 :Entity(name)
 ,TimeStep(0)
 ,KpSIN(NULL,"PDController("+name+")::input(vector)::Kp")
 ,KdSIN(NULL,"PDController("+name+")::input(vector)::Kd")
 ,positionSIN(NULL,"PDController("+name+")::input(vector)::position")
 ,desiredpositionSIN(NULL,"PDController("+name+")::input(vector)::desired_position")
 ,velocitySIN(NULL,"PDController("+name+")::input(vector)::velocity")
 ,desiredvelocitySIN(NULL,"PDController("+name+")::input(vector)::desired_velocity")
 ,controlSOUT( boost::bind(&PDController::computeControl,this,_1,_2),
         KpSIN << KdSIN << positionSIN << desiredpositionSIN
         << velocitySIN << desiredvelocitySIN,
        "PDController("+name+")::output(vector)::control" )
  ,positionErrorSOUT(boost::bind(&PDController::computeControl,this,_1,_2),
        controlSOUT,
        "PDController("+name+")::output(vector)::position_error")
  ,velocityErrorSOUT(boost::bind(&PDController::computeControl,this,_1,_2),
        controlSOUT,
        "PDController("+name+")::output(vector)::velocity_error")
{
  Entity::signalRegistration( KpSIN << KdSIN << positionSIN <<
    desiredpositionSIN << velocitySIN << desiredvelocitySIN << controlSOUT
    << positionErrorSOUT << velocityErrorSOUT );
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

void PDController::display( std::ostream& os ) const
{
  os << "PDController "<<getName();
  try{
    os <<"control = "<<controlSOUT;
  }
  catch (ExceptionSignal e) {}
  os <<" ("<<TimeStep<<") ";
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

double& PDController::setsize(int dimension)

{
  _dimension = dimension;
  return _dimension;
}

dynamicgraph::Vector& PDController::
computeControl( dynamicgraph::Vector &tau, int t )
{
  sotDEBUGIN(15);
  const dynamicgraph::Vector& Kp = KpSIN(t);
  const dynamicgraph::Vector& Kd = KdSIN(t);
  const dynamicgraph::Vector& position = positionSIN(t);
  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);

  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);
  return tau;

}



dynamicgraph::Vector& PDController::
getPositionError( dynamicgraph::Vector &position_error, int t)
{
  //sotDEBUGOUT(15) ??
  controlSOUT(t);
  return position_error;
}

dynamicgraph::Vector& PDController::
getVelocityError( dynamicgraph::Vector &velocity_error, int t)
{
  controlSOUT(t);
  return velocity_error;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010,
3			* Fran��ois Bleibel,
4			* Olivier Stasse,
5			*
6			* CNRS/AIST
7			*
8			* This file is part of sot-core.
9			* sot-core is free software: you can redistribute it and/or
10			* modify it under the terms of the GNU Lesser General Public License
11			* as published by the Free Software Foundation, either version 3 of
12			* the License, or (at your option) any later version.
13			* sot-core is distributed in the hope that it will be
14			* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
15			* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16			* GNU Lesser General Public License for more details. You should
17			* have received a copy of the GNU Lesser General Public License along
18			* with sot-core. If not, see <http://www.gnu.org/licenses/>.
19			*/
20
21			/* SOT */
22			#include "dg_tools/control/control_pd.hpp"
23
24			/* --------------------------------------------------------------------- */
25			/* --------------------------------------------------------------------- */
26			/* --------------------------------------------------------------------- */
27			#include <sot/core/debug.hh>
28			#include <dynamic-graph/factory.h>
29
30			using namespace dynamicgraph::sot;
31			using namespace dynamicgraph;
32
33	✓✗✓✗ ✓✗✗✗	2	DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PDController, "PDController");
34
35			const double PDController::
36			TIME_STEP_DEFAULT = .001;
37
38			/* --------------------------------------------------------------------- */
39			/* --------------------------------------------------------------------- */
40			/* --------------------------------------------------------------------- */
41
42
43			#define __SOT_ControlPD_INIT \
44
45		1	PDController::PDController( const std::string & name )
46			:Entity(name)
47			,TimeStep(0)
48	✓✗✓✗	2	,KpSIN(NULL,"PDController("+name+")::input(vector)::Kp")
49	✓✗✓✗	2	,KdSIN(NULL,"PDController("+name+")::input(vector)::Kd")
50	✓✗✓✗	2	,positionSIN(NULL,"PDController("+name+")::input(vector)::position")
51	✓✗✓✗	2	,desiredpositionSIN(NULL,"PDController("+name+")::input(vector)::desired_position")
52	✓✗✓✗	2	,velocitySIN(NULL,"PDController("+name+")::input(vector)::velocity")
53	✓✗✓✗	2	,desiredvelocitySIN(NULL,"PDController("+name+")::input(vector)::desired_velocity")
54			,controlSOUT( boost::bind(&PDController::computeControl,this,_1,_2),
55	✓✗✓✗ ✓✗	3	KpSIN << KdSIN << positionSIN << desiredpositionSIN
56	✓✗✓✗	1	<< velocitySIN << desiredvelocitySIN,
57	✓✗✓✗	2	"PDController("+name+")::output(vector)::control" )
58			,positionErrorSOUT(boost::bind(&PDController::computeControl,this,_1,_2),
59			controlSOUT,
60	✓✗✓✗	2	"PDController("+name+")::output(vector)::position_error")
61			,velocityErrorSOUT(boost::bind(&PDController::computeControl,this,_1,_2),
62			controlSOUT,
63	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	11	"PDController("+name+")::output(vector)::velocity_error")
64			{
65	✓✗✓✗	3	Entity::signalRegistration( KpSIN << KdSIN << positionSIN <<
66	✓✗✓✗ ✓✗✓✗	1	desiredpositionSIN << velocitySIN << desiredvelocitySIN << controlSOUT
67	✓✗✓✗ ✓✗	2	<< positionErrorSOUT << velocityErrorSOUT );
68		1	}
69
70			/* --------------------------------------------------------------------- */
71			/* --------------------------------------------------------------------- */
72			/* --------------------------------------------------------------------- */
73
74			void PDController::display( std::ostream& os ) const
75			{
76			os << "PDController "<<getName();
77			try{
78			os <<"control = "<<controlSOUT;
79			}
80			catch (ExceptionSignal e) {}
81			os <<" ("<<TimeStep<<") ";
82			}
83
84			/* --------------------------------------------------------------------- */
85			/* --------------------------------------------------------------------- */
86			/* --------------------------------------------------------------------- */
87
88			double& PDController::setsize(int dimension)
89
90			{
91			_dimension = dimension;
92			return _dimension;
93			}
94
95			dynamicgraph::Vector& PDController::
96			computeControl( dynamicgraph::Vector &tau, int t )
97			{
98			sotDEBUGIN(15);
99			const dynamicgraph::Vector& Kp = KpSIN(t);
100			const dynamicgraph::Vector& Kd = KdSIN(t);
101			const dynamicgraph::Vector& position = positionSIN(t);
102			const dynamicgraph::Vector& desired_position = desiredpositionSIN(t);
103			const dynamicgraph::Vector& velocity = velocitySIN(t);
104			const dynamicgraph::Vector& desired_velocity = desiredvelocitySIN(t);
105
106			dynamicgraph::Vector::Index size = Kp.size();
107			tau.resize(size);
108			position_error.resize(size);
109			velocity_error.resize(size);
110
111			position_error.array() = desired_position.array()-position.array();
112			velocity_error.array() = desired_velocity.array()-velocity.array();
113
114			tau.array() = position_error.array()*Kp.array()
115			+ velocity_error.array()*Kd.array();
116
117			sotDEBUGOUT(15);
118			return tau;
119
120			}
121
122
123
124			dynamicgraph::Vector& PDController::
125			getPositionError( dynamicgraph::Vector &position_error, int t)
126			{
127			//sotDEBUGOUT(15) ??
128			controlSOUT(t);
129			return position_error;
130			}
131
132			dynamicgraph::Vector& PDController::
133			getVelocityError( dynamicgraph::Vector &velocity_error, int t)
134			{
135			controlSOUT(t);
136			return velocity_error;
137	✓✗✓✗	6	}