6 #include <dynamic-graph/factory.h> 7 #include <sot/core/debug.hh> 10 #include <sot/core/stop-watch.hh> 20 #define PROFILE_PWM_DES_COMPUTATION "PositionController: desired pwm computation " 22 #define GAIN_SIGNALS m_KpSIN << m_KdSIN << m_KiSIN 23 #define REF_JOINT_SIGNALS m_qRefSIN << m_dqRefSIN 24 #define STATE_SIGNALS m_base6d_encodersSIN << m_jointsVelocitiesSIN 26 #define INPUT_SIGNALS STATE_SIGNALS << REF_JOINT_SIGNALS << GAIN_SIGNALS 28 #define OUTPUT_SIGNALS m_pwmDesSOUT << m_qErrorSOUT 33 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN;
34 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN6;
44 CONSTRUCT_SIGNAL_IN(base6d_encoders,
dynamicgraph::Vector),
45 CONSTRUCT_SIGNAL_IN(jointsVelocities,
dynamicgraph::Vector),
52 CONSTRUCT_SIGNAL_OUT(qError,
dynamicgraph::Vector, m_base6d_encodersSIN << m_qRefSIN),
53 m_robot_util(RefVoidRobotUtil()),
54 m_initSucceeded(false) {
59 docCommandVoid2(
"Initialize the entity.",
"Time period in seconds (double)",
60 "Reference to the robot (string)")));
61 addCommand(
"resetIntegral",
66 if (dt <= 0.0)
return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
67 if (!m_base6d_encodersSIN.isPlugged())
68 return SEND_MSG(
"Init failed: signal base6d_encoders is not plugged", MSG_TYPE_ERROR);
69 if (!m_jointsVelocitiesSIN.isPlugged())
70 return SEND_MSG(
"Init failed: signal jointsVelocities is not plugged", MSG_TYPE_ERROR);
71 if (!m_qRefSIN.isPlugged())
return SEND_MSG(
"Init failed: signal qRef is not plugged", MSG_TYPE_ERROR);
72 if (!m_dqRefSIN.isPlugged())
return SEND_MSG(
"Init failed: signal dqRef is not plugged", MSG_TYPE_ERROR);
73 if (!m_KpSIN.isPlugged())
return SEND_MSG(
"Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
74 if (!m_KdSIN.isPlugged())
return SEND_MSG(
"Init failed: signal Kd is not plugged", MSG_TYPE_ERROR);
75 if (!m_KiSIN.isPlugged())
return SEND_MSG(
"Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
78 std::string localName(robotRef);
79 if (isNameInRobotUtil(localName))
82 SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
104 if (!m_initSucceeded) {
105 SEND_WARNING_STREAM_MSG(
"Cannot compute signal pwmDes before initialization!");
111 const VectorN& Kp = m_KpSIN(iter);
112 const VectorN& Kd = m_KdSIN(iter);
113 const VectorN6& q = m_base6d_encodersSIN(iter);
114 const VectorN& dq = m_jointsVelocitiesSIN(iter);
115 const VectorN& qRef = m_qRefSIN(iter);
116 const VectorN& dqRef = m_dqRefSIN(iter);
118 assert(q.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6) &&
119 "Unexpected size of signal base6d_encoder");
120 assert(dq.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
121 "Unexpected size of signal dq");
122 assert(qRef.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
123 "Unexpected size of signal qRef");
124 assert(dqRef.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
125 "Unexpected size of signal dqRef");
126 assert(Kp.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
127 "Unexpected size of signal Kd");
128 assert(Kd.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
129 "Unexpected size of signal Kd");
131 m_pwmDes = Kp.cwiseProduct(qRef - q.tail(m_robot_util->m_nbJoints)) + Kd.cwiseProduct(dqRef - dq);
133 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
142 if (!m_initSucceeded) {
143 SEND_MSG(
"Cannot compute signal qError before initialization!", MSG_TYPE_WARNING_STREAM);
147 const VectorN6& q = m_base6d_encodersSIN(iter);
148 const VectorN& qRef = m_qRefSIN(iter);
149 assert(q.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6) &&
150 "Unexpected size of signal base6d_encoder");
151 assert(qRef.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
152 "Unexpected size of signal qRef");
154 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
155 s = qRef - q.tail(m_robot_util->m_nbJoints);
167 os <<
"PositionController " << getName();
169 getProfiler().report_all(3, os);
170 }
catch (ExceptionSignal e) {
RobotUtilShrPtr m_robot_util
Eigen::VectorXd m_pwmDes
Robot Util.
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
#define PROFILE_PWM_DES_COMPUTATION
void init(const double &dt, const std::string &robotRef)
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
virtual void display(std::ostream &os) const
qRef-q
Eigen::VectorXd m_e_integral
control loop time period
AdmittanceController EntityClassName
double m_dt
true if the entity has been successfully initialized
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PositionController(const std::string &name)