Commit 295f8709 authored by NoelieRamuzat's avatar NoelieRamuzat Committed by Noëlie RAMUZAT
Browse files

[SimpleInvDyn] Add waist and contact6D tasks to CoM and Posture ones

Add outputs (q_des, v_des, u) and enum on output control modes to use the entity in torque or velocity.
q_des and v_des are the integration of the desired acceleration computed by TSID
u is the result of the desired torque computed + PD on posture des.
Works with talos-torque-control package for sinusoid traj on the CoM in velocity control.
parent d463e324
......@@ -397,6 +397,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) {
const Vector6& dx_waist_ref = m_waist_ref_velSIN(iter);
const Vector6& ddx_waist_ref = m_waist_ref_accSIN(iter);
const VectorN& q_ref = m_posture_ref_posSIN(iter);
// std::cout << "q_ref.size() = " << q_ref.size() << std::endl;
// std::cout << "m_robot_util->m_nbJoints = " << static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) << std::endl;
assert(q_ref.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
const VectorN& dq_ref = m_posture_ref_velSIN(iter);
assert(dq_ref.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment