[adm-ctrl] Fix bug in assert

scripts/python_commands.txt

@@ -18,10 +18,10 @@ create_topic(robot.ros, robot.traj_gen.q, 'q_ref');
 create_topic(robot.ros, robot.device.robotVelocity, 'v');
 create_topic(robot.ros, robot.base_estimator.v, 'v_est', robot, robot.base_estimator);
 create_topic(robot.ros, robot.inv_dyn.zmp_des, 'zmp_des');
-create_topic(robot.ros, robot.inv_dyn.zmp_ref, 'zmp_ref');
+#create_topic(robot.ros, robot.inv_dyn.zmp_ref, 'zmp_ref');
 create_topic(robot.ros, robot.inv_dyn.zmp, 'zmp');
-create_topic(robot.ros, robot.inv_dyn.f_ref_right_foot, 'f_ref_right_foot');
-create_topic(robot.ros, robot.inv_dyn.f_ref_left_foot, 'f_ref_left_foot');
+#create_topic(robot.ros, robot.inv_dyn.f_ref_right_foot, 'f_ref_right_foot');
+#create_topic(robot.ros, robot.inv_dyn.f_ref_left_foot, 'f_ref_left_foot');
 create_topic(robot.ros, robot.inv_dyn.f_des_right_foot, 'f_des_right_foot');
 create_topic(robot.ros, robot.inv_dyn.f_des_left_foot, 'f_des_left_foot');
 create_topic(robot.ros, robot.inv_dyn.zmp_left_foot, 'zmp_left_foot');
@@ -49,7 +49,8 @@ create_topic(robot.ros, robot.inv_dyn.dq_admittance, 'dq_adm');
 
 #robot.torque_ctrl.velLeakingRate.value = 30*(0.,)
 #robot.torque_ctrl.reset_ddq_integral();
-ent.ctrl_manager.setCtrlMode('all','torque')
+#ent.ctrl_manager.setCtrlMode('all','torque')
+robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap','torque')
 robot.inv_dyn.dq_admittance.recompute(0)
 robot.base_estimator.set_imu_weight(0.0);
 plug(robot.encoders.sout,       robot.base_estimator.joint_positions)

src/admittance-controller.cpp

@@ -235,9 +235,9 @@ namespace dynamicgraph
           //const Vector& qMask            = m_controlledJointsSIN(iter); // n
           //const Eigen::Vector4d& damping = m_dampingSIN(iter);          // 4
 
-          assert(q.size()==m_nj     && "Unexpected size of signal encoder");
-          assert(dq.size()==m_nj    && "Unexpected size of signal dq");
-          assert(qMask.size()==m_nj && "Unexpected size of signal controlledJoints");
+          assert(q_sot.size()==m_nj     && "Unexpected size of signal encoder");
+//          assert(dq.size()==m_nj    && "Unexpected size of signal dq");
+//          assert(qMask.size()==m_nj && "Unexpected size of signal controlledJoints");
 
           Eigen::Vector6d v_des_RF = -kp.cwiseProduct(e_f_RF);
           Eigen::Vector6d v_des_LF = -kp.cwiseProduct(e_f_LF);