Commit 9a95781d authored by andreadelprete's avatar andreadelprete
Browse files

[adm-ctrl] Fix bug in assert

parent 4e566d56
......@@ -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)
......
......@@ -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);
......
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