Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/position-controller.cpp	Lines:	0	88	0.0 %
Date:	2023-06-05 17:45:50	Branches:	0	330	0.0 %


/*
 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
 *
 */

#include <dynamic-graph/factory.h>

#include <sot/core/debug.hh>
#include <sot/core/stop-watch.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/position-controller.hh>

namespace dynamicgraph {
namespace sot {
namespace torque_control {
namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;
// Size to be aligned "-------------------------------------------------------"
#define PROFILE_PWM_DES_COMPUTATION \
  "PositionController: desired pwm computation            "

#define GAIN_SIGNALS m_KpSIN << m_KdSIN << m_KiSIN
#define REF_JOINT_SIGNALS m_qRefSIN << m_dqRefSIN
#define STATE_SIGNALS m_base6d_encodersSIN << m_jointsVelocitiesSIN

#define INPUT_SIGNALS STATE_SIGNALS << REF_JOINT_SIGNALS << GAIN_SIGNALS

#define OUTPUT_SIGNALS m_pwmDesSOUT << m_qErrorSOUT

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef PositionController EntityClassName;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6;

/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PositionController, "PositionController");

/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
PositionController::PositionController(const std::string& name)
    : Entity(name),
      CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(qRef, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(dqRef, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(Ki, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_OUT(pwmDes, dynamicgraph::Vector, INPUT_SIGNALS),
      CONSTRUCT_SIGNAL_OUT(qError, dynamicgraph::Vector,
                           m_base6d_encodersSIN << m_qRefSIN),
      m_robot_util(RefVoidRobotUtil()),
      m_initSucceeded(false) {
  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);

  /* Commands. */
  addCommand("init", makeCommandVoid2(
                         *this, &PositionController::init,
                         docCommandVoid2("Initialize the entity.",
                                         "Time period in seconds (double)",
                                         "Reference to the robot (string)")));
  addCommand("resetIntegral",
             makeCommandVoid0(*this, &PositionController::resetIntegral,
                              docCommandVoid0("Reset the integral.")));
}

void PositionController::init(const double& dt, const std::string& robotRef) {
  if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
  if (!m_base6d_encodersSIN.isPlugged())
    return SEND_MSG("Init failed: signal base6d_encoders is not plugged",
                    MSG_TYPE_ERROR);
  if (!m_jointsVelocitiesSIN.isPlugged())
    return SEND_MSG("Init failed: signal jointsVelocities is not plugged",
                    MSG_TYPE_ERROR);
  if (!m_qRefSIN.isPlugged())
    return SEND_MSG("Init failed: signal qRef is not plugged", MSG_TYPE_ERROR);
  if (!m_dqRefSIN.isPlugged())
    return SEND_MSG("Init failed: signal dqRef is not plugged", MSG_TYPE_ERROR);
  if (!m_KpSIN.isPlugged())
    return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
  if (!m_KdSIN.isPlugged())
    return SEND_MSG("Init failed: signal Kd is not plugged", MSG_TYPE_ERROR);
  if (!m_KiSIN.isPlugged())
    return SEND_MSG("Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);

  /* Retrieve m_robot_util  informations */
  std::string localName(robotRef);
  if (isNameInRobotUtil(localName))
    m_robot_util = getRobotUtil(localName);
  else {
    SEND_MSG("You should have an entity controller manager initialized before",
             MSG_TYPE_ERROR);
    return;
  }

  m_dt = dt;

  m_pwmDes.setZero(m_robot_util->m_nbJoints);
  m_q.setZero(m_robot_util->m_nbJoints + 6);
  m_dq.setZero(m_robot_util->m_nbJoints);

  resetIntegral();

  m_initSucceeded = true;
}

void PositionController::resetIntegral() {
  m_e_integral.setZero(m_robot_util->m_nbJoints);
}

/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */

DEFINE_SIGNAL_OUT_FUNCTION(pwmDes, dynamicgraph::Vector) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot compute signal pwmDes before initialization!");
    return s;
  }

  getProfiler().start(PROFILE_PWM_DES_COMPUTATION);
  {
    const VectorN& Kp = m_KpSIN(iter);                // n
    const VectorN& Kd = m_KdSIN(iter);                // n
    const VectorN6& q = m_base6d_encodersSIN(iter);   // n+6
    const VectorN& dq = m_jointsVelocitiesSIN(iter);  // n
    const VectorN& qRef = m_qRefSIN(iter);            // n
    const VectorN& dqRef = m_dqRefSIN(iter);          // n

    assert(q.size() == static_cast<Eigen::VectorXd::Index>(
                           m_robot_util->m_nbJoints + 6) &&
           "Unexpected size of signal base6d_encoder");
    assert(dq.size() ==
               static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
           "Unexpected size of signal dq");
    assert(qRef.size() ==
               static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
           "Unexpected size of signal qRef");
    assert(dqRef.size() ==
               static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
           "Unexpected size of signal dqRef");
    assert(Kp.size() ==
               static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
           "Unexpected size of signal Kd");
    assert(Kd.size() ==
               static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
           "Unexpected size of signal Kd");

    m_pwmDes = Kp.cwiseProduct(qRef - q.tail(m_robot_util->m_nbJoints)) +
               Kd.cwiseProduct(dqRef - dq);

    if (s.size() !=
        static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
      s.resize(m_robot_util->m_nbJoints);
    s = m_pwmDes;
  }
  getProfiler().stop(PROFILE_PWM_DES_COMPUTATION);

  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(qError, dynamicgraph::Vector) {
  if (!m_initSucceeded) {
    SEND_MSG("Cannot compute signal qError before initialization!",
             MSG_TYPE_WARNING_STREAM);
    return s;
  }

  const VectorN6& q = m_base6d_encodersSIN(iter);  // n+6
  const VectorN& qRef = m_qRefSIN(iter);           // n
  assert(q.size() == static_cast<Eigen::VectorXd::Index>(
                         m_robot_util->m_nbJoints + 6) &&
         "Unexpected size of signal base6d_encoder");
  assert(qRef.size() ==
             static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
         "Unexpected size of signal qRef");

  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
    s.resize(m_robot_util->m_nbJoints);
  s = qRef - q.tail(m_robot_util->m_nbJoints);

  return s;
}

/* --- COMMANDS ---------------------------------------------------------- */

/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */

void PositionController::display(std::ostream& os) const {
  os << "PositionController " << getName();
  try {
    getProfiler().report_all(3, os);
  } catch (ExceptionSignal e) {
  }
}
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2015, Andrea Del Prete, LAAS-CNRS
3			*
4			*/
5
6			#include <dynamic-graph/factory.h>
7
8			#include <sot/core/debug.hh>
9			#include <sot/core/stop-watch.hh>
10			#include <sot/torque_control/commands-helper.hh>
11			#include <sot/torque_control/position-controller.hh>
12
13			namespace dynamicgraph {
14			namespace sot {
15			namespace torque_control {
16			namespace dynamicgraph = ::dynamicgraph;
17			using namespace dynamicgraph;
18			using namespace dynamicgraph::command;
19			using namespace std;
20			// Size to be aligned "-------------------------------------------------------"
21			#define PROFILE_PWM_DES_COMPUTATION \
22			"PositionController: desired pwm computation "
23
24			#define GAIN_SIGNALS m_KpSIN << m_KdSIN << m_KiSIN
25			#define REF_JOINT_SIGNALS m_qRefSIN << m_dqRefSIN
26			#define STATE_SIGNALS m_base6d_encodersSIN << m_jointsVelocitiesSIN
27
28			#define INPUT_SIGNALS STATE_SIGNALS << REF_JOINT_SIGNALS << GAIN_SIGNALS
29
30			#define OUTPUT_SIGNALS m_pwmDesSOUT << m_qErrorSOUT
31
32			/// Define EntityClassName here rather than in the header file
33			/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
34			typedef PositionController EntityClassName;
35			typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN;
36			typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6;
37
38			/* --- DG FACTORY ---------------------------------------------------- */
39			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PositionController, "PositionController");
40
41			/* ------------------------------------------------------------------- */
42			/* --- CONSTRUCTION -------------------------------------------------- */
43			/* ------------------------------------------------------------------- */
44			PositionController::PositionController(const std::string& name)
45			: Entity(name),
46			CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
47			CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector),
48			CONSTRUCT_SIGNAL_IN(qRef, dynamicgraph::Vector),
49			CONSTRUCT_SIGNAL_IN(dqRef, dynamicgraph::Vector),
50			CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector),
51			CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector),
52			CONSTRUCT_SIGNAL_IN(Ki, dynamicgraph::Vector),
53			CONSTRUCT_SIGNAL_OUT(pwmDes, dynamicgraph::Vector, INPUT_SIGNALS),
54			CONSTRUCT_SIGNAL_OUT(qError, dynamicgraph::Vector,
55			m_base6d_encodersSIN << m_qRefSIN),
56			m_robot_util(RefVoidRobotUtil()),
57			m_initSucceeded(false) {
58			Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
59
60			/* Commands. */
61			addCommand("init", makeCommandVoid2(
62			*this, &PositionController::init,
63			docCommandVoid2("Initialize the entity.",
64			"Time period in seconds (double)",
65			"Reference to the robot (string)")));
66			addCommand("resetIntegral",
67			makeCommandVoid0(*this, &PositionController::resetIntegral,
68			docCommandVoid0("Reset the integral.")));
69			}
70
71			void PositionController::init(const double& dt, const std::string& robotRef) {
72			if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
73			if (!m_base6d_encodersSIN.isPlugged())
74			return SEND_MSG("Init failed: signal base6d_encoders is not plugged",
75			MSG_TYPE_ERROR);
76			if (!m_jointsVelocitiesSIN.isPlugged())
77			return SEND_MSG("Init failed: signal jointsVelocities is not plugged",
78			MSG_TYPE_ERROR);
79			if (!m_qRefSIN.isPlugged())
80			return SEND_MSG("Init failed: signal qRef is not plugged", MSG_TYPE_ERROR);
81			if (!m_dqRefSIN.isPlugged())
82			return SEND_MSG("Init failed: signal dqRef is not plugged", MSG_TYPE_ERROR);
83			if (!m_KpSIN.isPlugged())
84			return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
85			if (!m_KdSIN.isPlugged())
86			return SEND_MSG("Init failed: signal Kd is not plugged", MSG_TYPE_ERROR);
87			if (!m_KiSIN.isPlugged())
88			return SEND_MSG("Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
89
90			/* Retrieve m_robot_util informations */
91			std::string localName(robotRef);
92			if (isNameInRobotUtil(localName))
93			m_robot_util = getRobotUtil(localName);
94			else {
95			SEND_MSG("You should have an entity controller manager initialized before",
96			MSG_TYPE_ERROR);
97			return;
98			}
99
100			m_dt = dt;
101
102			m_pwmDes.setZero(m_robot_util->m_nbJoints);
103			m_q.setZero(m_robot_util->m_nbJoints + 6);
104			m_dq.setZero(m_robot_util->m_nbJoints);
105
106			resetIntegral();
107
108			m_initSucceeded = true;
109			}
110
111			void PositionController::resetIntegral() {
112			m_e_integral.setZero(m_robot_util->m_nbJoints);
113			}
114
115			/* ------------------------------------------------------------------- */
116			/* --- SIGNALS ------------------------------------------------------- */
117			/* ------------------------------------------------------------------- */
118
119			DEFINE_SIGNAL_OUT_FUNCTION(pwmDes, dynamicgraph::Vector) {
120			if (!m_initSucceeded) {
121			SEND_WARNING_STREAM_MSG(
122			"Cannot compute signal pwmDes before initialization!");
123			return s;
124			}
125
126			getProfiler().start(PROFILE_PWM_DES_COMPUTATION);
127			{
128			const VectorN& Kp = m_KpSIN(iter); // n
129			const VectorN& Kd = m_KdSIN(iter); // n
130			const VectorN6& q = m_base6d_encodersSIN(iter); // n+6
131			const VectorN& dq = m_jointsVelocitiesSIN(iter); // n
132			const VectorN& qRef = m_qRefSIN(iter); // n
133			const VectorN& dqRef = m_dqRefSIN(iter); // n
134
135			assert(q.size() == static_cast<Eigen::VectorXd::Index>(
136			m_robot_util->m_nbJoints + 6) &&
137			"Unexpected size of signal base6d_encoder");
138			assert(dq.size() ==
139			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
140			"Unexpected size of signal dq");
141			assert(qRef.size() ==
142			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
143			"Unexpected size of signal qRef");
144			assert(dqRef.size() ==
145			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
146			"Unexpected size of signal dqRef");
147			assert(Kp.size() ==
148			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
149			"Unexpected size of signal Kd");
150			assert(Kd.size() ==
151			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
152			"Unexpected size of signal Kd");
153
154			m_pwmDes = Kp.cwiseProduct(qRef - q.tail(m_robot_util->m_nbJoints)) +
155			Kd.cwiseProduct(dqRef - dq);
156
157			if (s.size() !=
158			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
159			s.resize(m_robot_util->m_nbJoints);
160			s = m_pwmDes;
161			}
162			getProfiler().stop(PROFILE_PWM_DES_COMPUTATION);
163
164			return s;
165			}
166
167			DEFINE_SIGNAL_OUT_FUNCTION(qError, dynamicgraph::Vector) {
168			if (!m_initSucceeded) {
169			SEND_MSG("Cannot compute signal qError before initialization!",
170			MSG_TYPE_WARNING_STREAM);
171			return s;
172			}
173
174			const VectorN6& q = m_base6d_encodersSIN(iter); // n+6
175			const VectorN& qRef = m_qRefSIN(iter); // n
176			assert(q.size() == static_cast<Eigen::VectorXd::Index>(
177			m_robot_util->m_nbJoints + 6) &&
178			"Unexpected size of signal base6d_encoder");
179			assert(qRef.size() ==
180			static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
181			"Unexpected size of signal qRef");
182
183			if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
184			s.resize(m_robot_util->m_nbJoints);
185			s = qRef - q.tail(m_robot_util->m_nbJoints);
186
187			return s;
188			}
189
190			/* --- COMMANDS ---------------------------------------------------------- */
191
192			/* ------------------------------------------------------------------- */
193			/* --- ENTITY -------------------------------------------------------- */
194			/* ------------------------------------------------------------------- */
195
196			void PositionController::display(std::ostream& os) const {
197			os << "PositionController " << getName();
198			try {
199			getProfiler().report_all(3, os);
200			} catch (ExceptionSignal e) {
201			}
202			}
203			} // namespace torque_control
204			} // namespace sot
205			} // namespace dynamicgraph