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 INPUT_SIGNALS m_wrenchDesSIN << m_qSIN 47 #define INNER_SIGNALS m_kinematics_computations << m_qp_computations 49 #define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT 66 , CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
67 , CONSTRUCT_SIGNAL_INNER(qp_computations, int, m_wrenchDesSIN << m_kinematics_computationsSINNER)
72 , CONSTRUCT_SIGNAL_OUT(wrenchRef,
dynamicgraph::
Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
74 , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
75 , m_initSucceeded(false)
82 addCommand(
"init", makeCommandVoid1(*
this, &
DistributeWrench::init, docCommandVoid1(
"Initialize the entity.",
"Robot name")));
87 if(!m_wrenchDesSIN.isPlugged())
88 return SEND_MSG(
"Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
89 if(!m_qSIN.isPlugged())
90 return SEND_MSG(
"Init failed: signal q is not plugged", MSG_TYPE_ERROR);
95 std::string localName(robotName);
96 if (isNameInRobotUtil(localName))
103 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
107 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
110 catch (
const std::exception& e)
112 std::cout << e.what();
113 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
127 m_qp2.problem(12,6,0);
135 dg::Vector wrench = pose.actInv(pinocchio::Force(wrenchGlobal)).toVector();
137 const double h = pose.translation()[2];
139 const double fx = wrench[0];
140 const double fy = wrench[1];
141 const double fz = wrench[2];
142 const double tx = wrench[3];
143 const double ty = wrench[4];
151 px = (- ty - fx*h)/fz;
152 py = ( tx - fy*h)/fz;
159 const double pz = 0.0;
177 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
181 const Eigen::VectorXd & q = m_qSIN(iter);
182 assert(q.size()==
m_model.nq &&
"Unexpected size of signal q");
200 SEND_WARNING_STREAM_MSG(
"Cannot compute signal qp_computations before initialization!");
204 const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
205 m_kinematics_computationsSINNER(iter);
206 assert(wrenchDes.size()==6 &&
"Unexpected size of signal q");
211 Eigen::MatrixXd Q(12,12);
214 Eigen::VectorXd C(12);
217 Eigen::MatrixXd Aeq(6,12);
218 Aeq.block<6,6>(0,0).setIdentity();
219 Aeq.block<6,6>(0,6).setIdentity();
221 Eigen::VectorXd Beq(6);
224 Eigen::MatrixXd Aineq(0,12);
226 Eigen::VectorXd Bineq(0);
228 bool success =
m_qp2.solve(Q, C, Aeq, Beq, Aineq, Bineq);
234 SEND_WARNING_STREAM_MSG(
"No solution to the QP problem!");
238 Eigen::VectorXd result =
m_qp2.result();
252 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchLeft before initialization!");
256 m_qp_computationsSINNER(iter);
265 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRight before initialization!");
269 m_qp_computationsSINNER(iter);
278 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copLeft before initialization!");
282 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
296 SEND_WARNING_STREAM_MSG(
"Cannot compute signal copRight before initialization!");
300 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
313 SEND_WARNING_STREAM_MSG(
"Cannot compute signal wrenchRef before initialization!");
317 const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
318 const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
320 s = wrenchLeft + wrenchRight;
329 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmpRef before initialization!");
333 const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
337 const double fz = wrenchRef[2];
338 const double tx = wrenchRef[3];
339 const double ty = wrenchRef[4];
354 const double pz = 0.0;
382 os <<
"DistributeWrench " << getName();
385 getProfiler().report_all(3, os);
387 catch (ExceptionSignal e) {}
#define PROFILE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS
dynamicgraph::Vector computeCoP(const dynamicgraph::Vector &wrench, const pinocchio::SE3 &pose) const
dynamicgraph::Vector m_wrenchLeft
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
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
dynamicgraph::Vector m_wrenchRight
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
pinocchio::FrameIndex m_left_foot_id
ankle to F/T sensor transformation
pinocchio::FrameIndex m_right_foot_id
RobotUtilShrPtr m_robot_util
Pinocchio robot data.
#define PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS
pinocchio::SE3 m_ankle_M_ftSens
void init(const std::string &robotName)
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
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)