21 #include <sot/core/debug.hh> 22 #include <dynamic-graph/factory.h> 23 #include <dynamic-graph/all-commands.h> 25 #include <sot/core/stop-watch.hh> 27 #include <pinocchio/parsers/urdf.hpp> 28 #include <pinocchio/algorithm/kinematics.hpp> 29 #include <pinocchio/algorithm/frames.hpp> 35 namespace talos_balance
37 namespace dg = ::dynamicgraph;
42 #define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS "SimpleDistributeWrench: kinematics computations " 43 #define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_WRENCHES_COMPUTATIONS "SimpleDistributeWrench: wrenches computations " 45 #define INPUT_SIGNALS m_wrenchDesSIN << m_qSIN << m_rhoSIN << m_phaseSIN 47 #define INNER_SIGNALS m_kinematics_computations << m_wrenches 49 #define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_ankleWrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_ankleWrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT 57 "SimpleDistributeWrench");
66 , CONSTRUCT_SIGNAL_IN(rho, double)
67 , CONSTRUCT_SIGNAL_IN(phase, int)
68 , CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
69 , CONSTRUCT_SIGNAL_INNER(wrenches, int, m_wrenchDesSIN << m_rhoSIN << m_phaseSIN << m_kinematics_computationsSINNER)
76 , CONSTRUCT_SIGNAL_OUT(wrenchRef,
dynamicgraph::
Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
78 , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
79 , m_initSucceeded(false)
91 if(!m_wrenchDesSIN.isPlugged())
92 return SEND_MSG(
"Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
93 if(!m_qSIN.isPlugged())
94 return SEND_MSG(
"Init failed: signal q is not plugged", MSG_TYPE_ERROR);
99 std::string localName(robotName);
100 if (isNameInRobotUtil(localName))
107 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
111 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
114 catch (
const std::exception& e)
116 std::cout << e.what();
117 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
138 const double h = pose.translation()[2];
140 const double fx = wrench[0];
141 const double fy = wrench[1];
142 const double fz = wrench[2];
143 const double tx = wrench[3];
144 const double ty = wrench[4];
152 px = (- ty - fx*h)/fz;
153 py = ( tx - fy*h)/fz;
160 const double pz = 0.0;
178 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
182 const Eigen::VectorXd & q = m_qSIN(iter);
183 assert(q.size()==
m_model.nq &&
"Unexpected size of signal q");
201 Eigen::Vector3d forceLeft = wrenchDes.head<3>()/2;
202 Eigen::Vector3d forceRight = forceLeft;
203 forceLeft[2] = rho * wrenchDes[2];
204 forceRight[2] = (1-rho) * wrenchDes[2];
206 Eigen::Vector3d tauLeft =
m_contactLeft.translation().cross(forceLeft);
207 Eigen::Vector3d tauRight =
m_contactRight.translation().cross(forceRight);
209 Eigen::Vector3d tauResidual = (wrenchDes.tail<3>() - tauLeft - tauRight)/2;
210 tauLeft += tauResidual;
211 tauRight += tauResidual;
228 const Eigen::VectorXd & result = wrenchDes;
246 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenches before initialization!");
250 const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
251 const int & dummy = m_kinematics_computationsSINNER(iter);
253 const int & phase = m_phaseSIN(iter);
255 assert(wrenchDes.size()==6 &&
"Unexpected size of signal wrenchDes");
263 const double & rho = m_rhoSIN(iter);
276 SEND_WARNING_STREAM_MSG(
"Error in wrench distribution!");
287 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchLeft before initialization!");
293 m_wrenchesSINNER(iter);
302 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRight before initialization!");
308 m_wrenchesSINNER(iter);
317 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ankleWrenchLeft before initialization!");
323 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
334 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ankleWrenchRight before initialization!");
340 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
351 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copLeft before initialization!");
357 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
368 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copRight before initialization!");
374 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
385 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRef before initialization!");
391 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
392 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
394 s = wrenchLeft + wrenchRight;
403 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpRef before initialization!");
409 const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
413 const double fz = wrenchRef[2];
414 const double tx = wrenchRef[3];
415 const double ty = wrenchRef[4];
430 const double pz = 0.0;
432 Eigen::Vector3d zmp(3);
459 os <<
"SimpleDistributeWrench " << getName();
462 getProfiler().report_all(3, os);
464 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< double, 6, 1 > m_wrenchLeft
Eigen::Matrix< double, 6, 1 > m_wrenchRight
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
pinocchio::Data m_data
Pinocchio robot model.
pinocchio::SE3 m_contactRight
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS
Eigen::Vector3d computeCoP(const dynamicgraph::Vector &wrench, const pinocchio::SE3 &pose) const
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
virtual void display(std::ostream &os) const
pinocchio::FrameIndex m_right_foot_id
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleDistributeWrench(const std::string &name)
pinocchio::SE3 m_contactLeft
RobotUtilShrPtr m_robot_util
Pinocchio robot data.
bool distributeWrench(const Eigen::VectorXd &wrenchDes, const double rho)
pinocchio::FrameIndex m_left_foot_id
ankle to sole transformation
bool saturateWrench(const Eigen::VectorXd &wrenchDes, const int phase)
bool m_emergency_stop_triggered
pinocchio::SE3 m_ankle_M_sole
Eigen::Matrix< Scalar, 6, 1 > Vector6
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)