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_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS "DistributeWrench: kinematics computations " 43 #define PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS "DistributeWrench: QP problem computations " 45 #define WEIGHT_SIGNALS m_wSumSIN << m_wNormSIN << m_wRatioSIN << m_wAnkleSIN 47 #define INPUT_SIGNALS m_wrenchDesSIN << m_qSIN << m_rhoSIN << m_phaseSIN << m_frictionCoefficientSIN << WEIGHT_SIGNALS 49 #define INNER_SIGNALS m_kinematics_computations << m_qp_computations 51 #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 68 , CONSTRUCT_SIGNAL_IN(rho, double)
69 , CONSTRUCT_SIGNAL_IN(phase, int)
70 , CONSTRUCT_SIGNAL_IN(frictionCoefficient, double)
71 , CONSTRUCT_SIGNAL_IN(wSum, double)
72 , CONSTRUCT_SIGNAL_IN(wNorm, double)
73 , CONSTRUCT_SIGNAL_IN(wRatio, double)
75 , CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
76 , CONSTRUCT_SIGNAL_INNER(qp_computations, int, m_wrenchDesSIN << m_rhoSIN << m_phaseSIN <<
WEIGHT_SIGNALS << m_kinematics_computationsSINNER)
85 , CONSTRUCT_SIGNAL_OUT(wrenchRef,
dynamicgraph::
Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
87 , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
88 , m_initSucceeded(false)
107 m_qp1.problem(6,0,16);
108 m_qp2.problem(12,0,34);
111 addCommand(
"init", makeCommandVoid1(*
this, &
DistributeWrench::init, docCommandVoid1(
"Initialize the entity.",
"Robot name")));
113 addCommand(
"set_right_foot_sizes",
115 docCommandVoid1(
"Set the size of the right foot (pos x, neg x, pos y, neg y)",
117 addCommand(
"set_left_foot_sizes",
119 docCommandVoid1(
"Set the size of the left foot (pos x, neg x, pos y, neg y)",
122 addCommand(
"getMinPressure", makeDirectGetter(*
this,&
m_eps, docDirectGetter(
"Get minimum pressure",
"double")));
123 addCommand(
"setMinPressure", makeDirectSetter(*
this,&
m_eps, docDirectSetter(
"Set minimum pressure",
"double")));
130 if(!m_wrenchDesSIN.isPlugged())
131 return SEND_MSG(
"Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
132 if(!m_qSIN.isPlugged())
133 return SEND_MSG(
"Init failed: signal q is not plugged", MSG_TYPE_ERROR);
136 return SEND_ERROR_STREAM_MSG(
"Init failed: left foot size is not initialized");
138 return SEND_ERROR_STREAM_MSG(
"Init failed: right foot size is not initialized");
143 std::string localName(robotName);
144 if (isNameInRobotUtil(localName))
151 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
155 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
158 catch (
const std::exception& e)
160 std::cout << e.what();
161 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
180 return SEND_MSG(
"Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
187 return SEND_MSG(
"Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
206 -Y, -X, -(X + Y) * mu, +mu, +mu, -1,
207 -Y, +X, -(X + Y) * mu, +mu, -mu, -1,
208 +Y, -X, -(X + Y) * mu, -mu, +mu, -1,
209 +Y, +X, -(X + Y) * mu, -mu, -mu, -1,
210 +Y, +X, -(X + Y) * mu, +mu, +mu, +1,
211 +Y, -X, -(X + Y) * mu, +mu, -mu, +1,
212 -Y, +X, -(X + Y) * mu, -mu, +mu, +1,
213 -Y, -X, -(X + Y) * mu, -mu, -mu, +1;
221 const double h = pose.translation()[2];
223 const double fx = wrench[0];
224 const double fy = wrench[1];
225 const double fz = wrench[2];
226 const double tx = wrench[3];
227 const double ty = wrench[4];
235 px = (- ty - fx*h)/fz;
236 py = ( tx - fy*h)/fz;
243 const double pz = 0.0;
261 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
265 const Eigen::VectorXd & q = m_qSIN(iter);
266 assert(q.size()==
m_model.nq &&
"Unexpected size of signal q");
288 Eigen::MatrixXd & Q =
m_Q2;
289 Eigen::VectorXd & C =
m_C2;
292 Q.topLeftCorner<6,6>().setIdentity();
293 Q.topRightCorner<6,6>().setIdentity();
294 Q.bottomLeftCorner<6,6>().setIdentity();
295 Q.bottomRightCorner<6,6>().setIdentity();
298 C.head<6>() = -wrenchDes;
299 C.tail<6>() = -wrenchDes;
304 Q.topLeftCorner<6,6>().noalias() += tmp.transpose() * tmp *
m_wNorm;
307 Q.bottomRightCorner<6,6>().noalias() += tmp.transpose() * tmp *
m_wNorm;
310 Eigen::Matrix<double,1,12> tmp2;
311 tmp2 << (1-rho) * (
m_contactLeft.inverse().toDualActionMatrix().row(2) ),
312 (-rho) * (
m_contactRight.inverse().toDualActionMatrix().row(2) );
314 Q.noalias() += tmp2.transpose() * tmp2 *
m_wRatio;
318 Eigen::MatrixXd & Aeq =
m_Aeq2;
320 Eigen::VectorXd & Beq =
m_Beq2;
329 Aineq.topRightCorner<16,6>().setZero();
330 Aineq.block<16,6>(16,0).setZero();
333 Aineq.block<1,6>(32,0) = -
m_contactLeft.inverse().toDualActionMatrix().row(2);
334 Aineq.block<1,6>(32,6).setZero();
335 Aineq.block<1,6>(33,0).setZero();
336 Aineq.block<1,6>(33,6) = -
m_contactRight.inverse().toDualActionMatrix().row(2);
344 const bool success =
m_qp2.solve(Q, C, Aeq, Beq, Aineq, Bineq);
355 const Eigen::VectorXd & result =
m_qp2.result();
363 Eigen::MatrixXd & Q =
m_Q1;
364 Eigen::VectorXd & C =
m_C1;
372 Eigen::MatrixXd & Aeq =
m_Aeq1;
374 Eigen::VectorXd & Beq =
m_Beq1;
391 const bool success =
m_qp1.solve(Q, C, Aeq, Beq, Aineq, Bineq);
402 const Eigen::VectorXd & result =
m_qp1.result();
419 SEND_WARNING_STREAM_MSG(
"Cannot compute signal qp_computations before initialization!");
423 const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
424 const int & dummy = m_kinematics_computationsSINNER(iter);
426 const int & phase = m_phaseSIN(iter);
428 const double & mu = m_frictionCoefficientSIN(iter);
430 assert(wrenchDes.size()==6 &&
"Unexpected size of signal wrenchDes");
436 const double & rho = m_rhoSIN(iter);
454 SEND_ERROR_STREAM_MSG(
"No solution to the QP problem!");
465 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchLeft before initialization!");
471 const int & dummy = m_qp_computationsSINNER(iter);
481 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRight before initialization!");
487 const int & dummy = m_qp_computationsSINNER(iter);
497 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ankleWrenchLeft before initialization!");
503 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
514 SEND_WARNING_STREAM_MSG(
"Cannot compute signal ankleWrenchRight before initialization!");
520 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
531 SEND_WARNING_STREAM_MSG(
"Cannot compute signal surfaceWrenchLeft before initialization!");
537 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
539 s =
m_contactLeft.actInv(pinocchio::Force(wrenchLeft)).toVector();
548 SEND_WARNING_STREAM_MSG(
"Cannot compute signal surfaceWrenchRight before initialization!");
554 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
556 s =
m_contactRight.actInv(pinocchio::Force(wrenchRight)).toVector();
565 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copLeft before initialization!");
571 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
588 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copRight before initialization!");
594 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
611 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRef before initialization!");
617 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
618 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
620 s = wrenchLeft + wrenchRight;
629 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpRef before initialization!");
635 const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
645 const double fz = wrenchRef[2];
646 const double tx = wrenchRef[3];
647 const double ty = wrenchRef[4];
662 const double pz = 0.0;
664 Eigen::Vector3d zmp(3);
690 os <<
"DistributeWrench " << getName();
693 getProfiler().report_all(3, os);
695 catch (ExceptionSignal e) {}
#define PROFILE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS
void saturateWrench(const Eigen::VectorXd &wrenchDes, const int phase, const double mu)
Eigen::Vector4d m_right_foot_sizes
sizes of the left foot (pos x, neg x, pos y, neg y)
void computeWrenchFaceMatrix(const double mu)
sizes of the left foot (pos x, neg x, pos y, neg y)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DistributeWrench(const std::string &name)
pinocchio::Data m_data
Pinocchio robot model.
AdmittanceControllerEndEffector EntityClassName
Eigen::Vector4d m_left_foot_sizes
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
pinocchio::SE3 m_contactRight
Eigen::Matrix< double, 16, 6 > m_wrenchFaceMatrix
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
pinocchio::FrameIndex m_left_foot_id
ankle to sole transformation
pinocchio::SE3 m_contactLeft
pinocchio::FrameIndex m_right_foot_id
Eigen::Matrix< double, 6, 1 > m_wrenchRight
void set_left_foot_sizes(const dynamicgraph::Vector &s)
RobotUtilShrPtr m_robot_util
Pinocchio robot data.
#define PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS
void distributeWrench(const Eigen::VectorXd &wrenchDes, const double rho, const double mu)
Eigen::QuadProgDense m_qp1
void init(const std::string &robotName)
Eigen::Matrix< double, 6, 1 > m_wrenchLeft
void set_right_foot_sizes(const dynamicgraph::Vector &s)
virtual void display(std::ostream &os) const
Eigen::QuadProgDense m_qp2
pinocchio::Model m_model
true if the entity has been successfully initialized
bool m_emergency_stop_triggered
pinocchio::SE3 m_ankle_M_sole
Eigen::Vector3d computeCoP(const dynamicgraph::Vector &wrench, const pinocchio::SE3 &pose) const
Eigen::Matrix< Scalar, 6, 1 > Vector6
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)