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_surfaceWrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_ankleWrenchRightSOUT << m_surfaceWrenchRightSOUT << 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)
78 , CONSTRUCT_SIGNAL_OUT(wrenchRef,
dynamicgraph::
Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
80 , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
81 , m_initSucceeded(false)
93 if(!m_wrenchDesSIN.isPlugged())
94 return SEND_MSG(
"Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
95 if(!m_qSIN.isPlugged())
96 return SEND_MSG(
"Init failed: signal q is not plugged", MSG_TYPE_ERROR);
101 std::string localName(robotName);
102 if (isNameInRobotUtil(localName))
109 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
113 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
116 catch (
const std::exception& e)
118 std::cout << e.what();
119 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
140 const double h = pose.translation()[2];
142 const double fx = wrench[0];
143 const double fy = wrench[1];
144 const double fz = wrench[2];
145 const double tx = wrench[3];
146 const double ty = wrench[4];
154 px = (- ty - fx*h)/fz;
155 py = ( tx - fy*h)/fz;
162 const double pz = 0.0;
180 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
184 const Eigen::VectorXd & q = m_qSIN(iter);
185 assert(q.size()==
m_model.nq &&
"Unexpected size of signal q");
203 Eigen::Vector3d forceLeft = wrenchDes.head<3>()/2;
204 Eigen::Vector3d forceRight = forceLeft;
205 forceLeft[2] = rho * wrenchDes[2];
206 forceRight[2] = (1-rho) * wrenchDes[2];
208 Eigen::Vector3d tauLeft =
m_contactLeft.translation().cross(forceLeft);
209 Eigen::Vector3d tauRight =
m_contactRight.translation().cross(forceRight);
211 Eigen::Vector3d tauResidual = (wrenchDes.tail<3>() - tauLeft - tauRight)/2;
212 tauLeft += tauResidual;
213 tauRight += tauResidual;
218 const bool success =
true;
224 const bool success =
true;
228 const Eigen::VectorXd & result = wrenchDes;
244 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenches before initialization!");
248 const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
249 const int & dummy = m_kinematics_computationsSINNER(iter);
251 const int & phase = m_phaseSIN(iter);
253 assert(wrenchDes.size()==6 &&
"Unexpected size of signal wrenchDes");
259 const double & rho = m_rhoSIN(iter);
272 SEND_WARNING_STREAM_MSG(
"Error in wrench distribution!");
283 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchLeft before initialization!");
289 const int & dummy = m_wrenchesSINNER(iter);
299 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRight before initialization!");
305 const int & dummy = m_wrenchesSINNER(iter);
315 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ankleWrenchLeft before initialization!");
321 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
332 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ankleWrenchRight before initialization!");
338 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
349 SEND_WARNING_STREAM_MSG(
"Cannot compute signal surfaceWrenchLeft before initialization!");
355 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
357 s =
m_contactLeft.actInv(pinocchio::Force(wrenchLeft)).toVector();
366 SEND_WARNING_STREAM_MSG(
"Cannot compute signal surfaceWrenchRight before initialization!");
372 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
374 s =
m_contactRight.actInv(pinocchio::Force(wrenchRight)).toVector();
383 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copLeft before initialization!");
389 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
406 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copRight before initialization!");
412 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
429 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRef before initialization!");
435 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
436 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
438 s = wrenchLeft + wrenchRight;
447 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpRef before initialization!");
453 const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
463 const double fz = wrenchRef[2];
464 const double tx = wrenchRef[3];
465 const double ty = wrenchRef[4];
480 const double pz = 0.0;
482 Eigen::Vector3d zmp(3);
509 os <<
"SimpleDistributeWrench " << getName();
512 getProfiler().report_all(3, os);
514 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
void saturateWrench(const Eigen::VectorXd &wrenchDes, const int phase)
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
void distributeWrench(const Eigen::VectorXd &wrenchDes, const double rho)
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.
pinocchio::FrameIndex m_left_foot_id
ankle to sole transformation
bool m_emergency_stop_triggered
pinocchio::SE3 m_ankle_M_sole
Eigen::Matrix< Scalar, 6, 1 > Vector6
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)