19 #include <sot/core/debug.hh> 20 #include <dynamic-graph/factory.h> 21 #include <dynamic-graph/command-bind.h> 23 #include <dynamic-graph/all-commands.h> 24 #include <sot/core/stop-watch.hh> 30 namespace talos_balance
32 namespace dg = ::dynamicgraph;
37 #define PROFILE_SIMPLEZMPESTIMATOR_ZMP_COMPUTATION "SimpleZmpEstimator: zmp computation " 38 #define PROFILE_SIMPLEZMPESTIMATOR_COPLEFT_COMPUTATION "SimpleZmpEstimator: copLeft computation " 39 #define PROFILE_SIMPLEZMPESTIMATOR_COPRIGHT_COMPUTATION "SimpleZmpEstimator: copRight computation " 41 #define INPUT_SIGNALS m_wrenchLeftSIN << m_wrenchRightSIN << m_poseLeftSIN << m_poseRightSIN 43 #define OUTPUT_SIGNALS m_copLeftSOUT << m_copRightSOUT << m_zmpSOUT << m_emergencyStopSOUT 51 "SimpleZmpEstimator");
60 , CONSTRUCT_SIGNAL_IN(poseLeft, MatrixHomogeneous)
61 , CONSTRUCT_SIGNAL_IN(poseRight, MatrixHomogeneous)
62 , CONSTRUCT_SIGNAL_OUT(copLeft,
dynamicgraph::
Vector, m_wrenchLeftSIN << m_poseLeftSIN)
63 , CONSTRUCT_SIGNAL_OUT(copRight,
dynamicgraph::
Vector, m_wrenchRightSIN << m_poseRightSIN)
64 , CONSTRUCT_SIGNAL_OUT(zmp,
dynamicgraph::
Vector, m_wrenchLeftSIN << m_wrenchRightSIN << m_copLeftSOUT << m_copRightSOUT)
65 , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpSOUT)
67 , m_initSucceeded(false)
73 addCommand(
"getForceThreshold", makeDirectGetter(*
this,&
m_eps, docDirectGetter(
"Get force threshold",
"double")));
74 addCommand(
"setForceThreshold", makeDirectSetter(*
this,&
m_eps, docDirectSetter(
"Set force threshold",
"double")));
79 if(!m_wrenchLeftSIN.isPlugged())
80 return SEND_MSG(
"Init failed: signal wrenchLeft is not plugged", MSG_TYPE_ERROR);
81 if(!m_poseLeftSIN.isPlugged())
82 return SEND_MSG(
"Init failed: signal poseLeft is not plugged", MSG_TYPE_ERROR);
83 if(!m_wrenchRightSIN.isPlugged())
84 return SEND_MSG(
"Init failed: signal wrenchRight is not plugged", MSG_TYPE_ERROR);
85 if(!m_poseRightSIN.isPlugged())
86 return SEND_MSG(
"Init failed: signal poseRight is not plugged", MSG_TYPE_ERROR);
98 const double h = pose(2,3);
100 const double fx = wrench[0];
101 const double fy = wrench[1];
102 const double fz = wrench[2];
103 const double tx = wrench[3];
104 const double ty = wrench[4];
109 px = (- ty - fx*h)/fz;
110 py = ( tx - fy*h)/fz;
117 const double pz = - h;
119 Eigen::Vector3d copLocal;
124 Eigen::Vector3d cop = pose.linear()*copLocal + pose.translation();
133 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copLeft before initialization!");
142 const MatrixHomogeneous & poseLeft = m_poseLeftSIN(iter);
144 assert(wrenchLeft.size()==6 &&
"Unexpected size of signal wrenchLeft");
157 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copRight before initialization!");
166 const MatrixHomogeneous & poseRight = m_poseRightSIN(iter);
168 assert(wrenchRight.size()==6 &&
"Unexpected size of signal wrenchRight");
182 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp before initialization!");
196 assert(wrenchLeft.size()==6 &&
"Unexpected size of signal wrenchLeft");
197 assert(wrenchRight.size()==6 &&
"Unexpected size of signal wrenchRight");
199 const double fzLeft = wrenchLeft[2];
200 const double fzRight = wrenchRight[2];
202 const double e2 =
m_eps/2;
203 const double fz = fzLeft + fzRight;
204 if(fzLeft >= e2 && fzRight < e2)
209 else if(fzLeft < e2 && fzRight >= e2)
217 s = ( copLeft*fzLeft + copRight*fzRight ) / fz;
222 SEND_WARNING_STREAM_MSG(
"Foot forces on the z-axis are both zero!");
247 os <<
"SimpleZmpEstimator " << getName();
250 getProfiler().report_all(3, os);
252 catch (ExceptionSignal e) {}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleZmpEstimator(const std::string &name, const double &eps=1.0)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_SIMPLEZMPESTIMATOR_COPRIGHT_COMPUTATION
#define PROFILE_SIMPLEZMPESTIMATOR_COPLEFT_COMPUTATION
virtual void display(std::ostream &os) const
bool m_emergency_stop_triggered
true if the entity has been successfully initialized
#define PROFILE_SIMPLEZMPESTIMATOR_ZMP_COMPUTATION
Eigen::Vector3d computeCoP(const dynamicgraph::Vector &wrench, const MatrixHomogeneous &pose) const
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)