12 #include <sot/core/debug.hh> 13 #include <dynamic-graph/factory.h> 14 #include <dynamic-graph/all-commands.h> 15 #include <sot/core/stop-watch.hh> 21 namespace talos_balance
23 namespace dg = ::dynamicgraph;
28 #define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_WFORCE_COMPUTATION \ 29 "AdmittanceControllerEndEffector: w_force computation " 31 #define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_WDQ_COMPUTATION \ 32 "AdmittanceControllerEndEffector: w_dq computation " 34 #define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_DQ_COMPUTATION \ 35 "AdmittanceControllerEndEffector: dq computation " 37 #define INPUT_SIGNALS m_KpSIN << m_KdSIN << m_dqSaturationSIN << m_forceSIN << m_w_forceDesSIN << m_qSIN 39 #define INNER_SIGNALS m_w_forceSINNER << m_w_dqSINNER 41 #define OUTPUT_SIGNALS m_dqSOUT 49 "AdmittanceControllerEndEffector");
65 m_initSucceeded(false),
75 addCommand(
"init", makeCommandVoid3(*
this,
77 docCommandVoid3(
"Initialize the entity.",
80 "end Effector Joint Name")));
81 addCommand(
"resetDq", makeCommandVoid0(*
this,
83 docCommandVoid0(
"resetDq")));
87 const std::string &sensorFrameName,
88 const std::string &endEffectorName)
90 if (!m_dqSaturationSIN.isPlugged())
91 return SEND_MSG(
"Init failed: signal dqSaturation is not plugged", MSG_TYPE_ERROR);
92 if (!m_KpSIN.isPlugged())
93 return SEND_MSG(
"Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
94 if (!m_KdSIN.isPlugged())
95 return SEND_MSG(
"Init failed: signal Kd is not plugged", MSG_TYPE_ERROR);
96 if (!m_forceSIN.isPlugged())
97 return SEND_MSG(
"Init failed: signal force is not plugged", MSG_TYPE_ERROR);
98 if (!m_w_forceDesSIN.isPlugged())
99 return SEND_MSG(
"Init failed: signal w_forceDes is not plugged", MSG_TYPE_ERROR);
100 if (!m_qSIN.isPlugged())
101 return SEND_MSG(
"Init failed: signal q is not plugged", MSG_TYPE_ERROR);
110 std::string localName(
"robot");
111 if (isNameInRobotUtil(localName))
114 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
118 SEND_MSG(
"You should have a robotUtil pointer initialized before", MSG_TYPE_ERROR);
122 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
128 catch (
const std::exception &e)
130 std::cout << e.what();
131 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename,
152 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_force before initialization!");
160 const Vector &force = m_forceSIN(iter);
161 const Vector &q = m_qSIN(iter);
162 assert(force.size() ==
m_n &&
"Unexpected size of signal force");
163 assert(q.size() ==
m_model.nq &&
"Unexpected size of signal q");
169 s = sensorPlacement.act(pinocchio::Force(force)).toVector();
180 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_dq before initialization!");
188 const Vector &w_forceDes = m_w_forceDesSIN(iter);
189 const Vector &w_force = m_w_forceSINNER(iter);
190 const Vector &Kp = m_KpSIN(iter);
191 const Vector &Kd = m_KdSIN(iter);
192 const Vector &dqSaturation = m_dqSaturationSIN(iter);
193 assert(w_force.size() ==
m_n &&
"Unexpected size of signal force");
194 assert(w_forceDes.size() ==
m_n &&
"Unexpected size of signal w_forceDes");
195 assert(Kp.size() ==
m_n &&
"Unexpected size of signal Kp");
196 assert(Kd.size() ==
m_n &&
"Unexpected size of signal Kd");
197 assert(dqSaturation.size() ==
m_n &&
"Unexpected size of signal dqSaturation");
199 m_w_dq = m_w_dq +
m_dt * (Kp.cwiseProduct(w_forceDes - w_force)) - Kd.cwiseProduct(m_w_dq);
201 for (
int i = 0; i <
m_n; i++)
203 if (m_w_dq[i] > dqSaturation[i])
204 m_w_dq[i] = dqSaturation[i];
205 if (m_w_dq[i] < -dqSaturation[i])
206 m_w_dq[i] = -dqSaturation[i];
220 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dq before initialization!");
228 const Vector &w_dq = m_w_dqSINNER(iter);
229 assert(w_dq.size() ==
m_n &&
"Unexpected size of signal w_dq");
234 s = placement.actInv(pinocchio::Motion(w_dq)).toVector();
248 os <<
"AdmittanceControllerEndEffector " << getName();
251 getProfiler().report_all(3, os);
253 catch (ExceptionSignal e)
dynamicgraph::Vector m_w_dq
Internal state.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
RobotUtilShrPtr m_robot_util
Robot Util instance to get the sensor frame.
void init(const double &dt, const std::string &sensorFrameName, const std::string &endeffectorName)
Initialize the entity.
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AdmittanceControllerEndEffector(const std::string &name)
bool m_initSucceeded
True if the entity has been successfully initialized.
pinocchio::Data * m_data
Pinocchio robot data.
#define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_WFORCE_COMPUTATION
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
void resetDq()
Reset the velocity.
Admittance controller for an upper body end-effector (right or left wrist)
#define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_WDQ_COMPUTATION
int m_n
Dimension of the force signals and of the output.
virtual void display(std::ostream &os) const
pinocchio::FrameIndex m_sensorFrameId
Id of the force sensor frame.
#define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_DQ_COMPUTATION
double m_dt
Time step of the control.
pinocchio::JointIndex m_endEffectorId
Id of the joint of the end-effector.
pinocchio::Model m_model
Pinocchio robot model.
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)