19 #include <sot/core/debug.hh> 20 #include <dynamic-graph/factory.h> 21 #include <dynamic-graph/all-commands.h> 23 #include <sot/core/stop-watch.hh> 25 #include <pinocchio/parsers/urdf.hpp> 26 #include <pinocchio/algorithm/kinematics.hpp> 27 #include <pinocchio/algorithm/frames.hpp> 33 namespace talos_balance
35 namespace dg = ::dynamicgraph;
40 #define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS "SimpleDistributeWrench: kinematics computations " 41 #define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_WRENCHES_COMPUTATIONS "SimpleDistributeWrench: wrenches computations " 43 #define INPUT_SIGNALS m_wrenchDesSIN << m_qSIN << m_rhoSIN 45 #define INNER_SIGNALS m_kinematics_computations << m_wrenches 47 #define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_wrenchRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT 55 "SimpleDistributeWrench");
64 , CONSTRUCT_SIGNAL_IN(rho, double)
65 , CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
66 , CONSTRUCT_SIGNAL_INNER(wrenches, int, m_wrenchDesSIN << m_kinematics_computationsSINNER)
69 , CONSTRUCT_SIGNAL_OUT(wrenchRef,
dynamicgraph::
Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
71 , m_initSucceeded(false)
83 if(!m_wrenchDesSIN.isPlugged())
84 return SEND_MSG(
"Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
85 if(!m_qSIN.isPlugged())
86 return SEND_MSG(
"Init failed: signal q is not plugged", MSG_TYPE_ERROR);
91 std::string localName(robotName);
92 if (isNameInRobotUtil(localName))
99 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
103 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
106 catch (
const std::exception& e)
108 std::cout << e.what();
109 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
124 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
128 const Eigen::VectorXd & q = m_qSIN(iter);
129 assert(q.size()==
m_model.nq &&
"Unexpected size of signal q");
144 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenches before initialization!");
148 const double rho_m = 0.1;
149 const double rho_M = 1.0 - rho_m;
151 const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
153 double rho = m_rhoSIN.isPlugged() ? m_rhoSIN(iter) : 0.5;
159 m_kinematics_computationsSINNER(iter);
161 assert(wrenchDes.size()==6 &&
"Unexpected size of signal wrenchDes");
181 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchLeft before initialization!");
185 m_wrenchesSINNER(iter);
194 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRight before initialization!");
198 m_wrenchesSINNER(iter);
207 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRef before initialization!");
211 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
212 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
214 s = wrenchLeft + wrenchRight;
223 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpRef before initialization!");
227 const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
229 const double fx = wrenchRef[0];
230 const double fy = wrenchRef[1];
231 const double fz = wrenchRef[2];
232 const double tx = wrenchRef[3];
233 const double ty = wrenchRef[4];
248 const double pz = 0.0;
269 os <<
"SimpleDistributeWrench " << getName();
272 getProfiler().report_all(3, os);
274 catch (ExceptionSignal e) {}
void init(const std::string &robotName)
pinocchio::Model m_model
true if the entity has been successfully initialized
#define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_WRENCHES_COMPUTATIONS
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
pinocchio::Data m_data
Pinocchio robot model.
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
dynamicgraph::Vector m_wrenchLeft
dynamicgraph::Vector m_wrenchRight
virtual void display(std::ostream &os) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleDistributeWrench(const std::string &name)
RobotUtilShrPtr m_robot_util
Pinocchio robot data.
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)