Commit 9750072d authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[Minor fix] Remove trailing spaces

parent e8514ecb
......@@ -236,7 +236,7 @@ namespace dynamicgraph {
SE3 m_sole_M_ftSens; /// foot sole to F/T sensor transformation
pinocchio::FrameIndex m_right_foot_id;
pinocchio::FrameIndex m_right_foot_id;
pinocchio::FrameIndex m_left_foot_id;
pinocchio::FrameIndex m_IMU_body_id;
......
......@@ -106,9 +106,9 @@ namespace dynamicgraph
// m_fRightHandRefSIN)
// ,CONSTRUCT_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector, m_fLeftHandSIN <<
// m_fLeftHandRefSIN)
,m_firstIter(true)
,m_initSucceeded(false)
,m_useJacobianTranspose(true)
,m_firstIter(true)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
......@@ -370,6 +370,7 @@ namespace dynamicgraph
if(saturating)
SEND_INFO_STREAM_MSG("Saturate m_v_LF_int integral: "+toString(m_v_LF_int.transpose()));
s = v_des + m_v_LF_int;
return s;
}
// DEFINE_SIGNAL_OUT_FUNCTION(fRightHandError,dynamicgraph::Vector)
......@@ -439,7 +440,7 @@ namespace dynamicgraph
// cout<<"b = "<<toString(b,1)<<endl;
VectorXd tmp(A.cols());
int nzsv = A.nonzeroSingularValues();
int nzsv = static_cast<int>(A.nonzeroSingularValues());
tmp.noalias() = A.matrixU().leftCols(nzsv).adjoint() * b;
// cout<<"U^T*b = "<<toString(tmp,1)<<endl;
double sv, d2 = damping*damping;
......
......@@ -104,9 +104,11 @@ namespace dynamicgraph
#define INPUT_SIGNALS m_joint_positionsSIN << m_joint_velocitiesSIN << \
m_imu_quaternionSIN << m_forceLLEGSIN << m_forceRLEGSIN << m_dforceLLEGSIN << m_dforceRLEGSIN << \
m_w_lf_inSIN << m_w_rf_inSIN << m_K_fb_feet_posesSIN << m_lf_ref_xyzquatSIN << m_rf_ref_xyzquatSIN << m_accelerometerSIN << m_gyroscopeSIN
#define OUTPUT_SIGNALS m_qSOUT << m_vSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT << \
m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT << m_w_rf_filteredSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << \
m_v_acSOUT << m_a_acSOUT << m_v_kinSOUT << m_v_imuSOUT << m_v_gyrSOUT << m_v_flexSOUT
#define OUTPUT_SIGNALS m_qSOUT << m_vSOUT << m_v_kinSOUT << m_v_flexSOUT << m_v_imuSOUT << m_v_gyrSOUT << \
m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << m_a_acSOUT << m_v_acSOUT << \
m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT << \
m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT << m_w_rf_filteredSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
......@@ -151,14 +153,14 @@ namespace dynamicgraph
<<m_lf_ref_xyzquatSIN
<<m_rf_ref_xyzquatSIN)
,CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector, m_kinematics_computationsSINNER << m_accelerometerSIN << m_gyroscopeSIN << m_qSOUT << m_dforceLLEGSIN << m_dforceRLEGSIN)
,CONSTRUCT_SIGNAL_OUT(v_ac, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(a_ac, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(v_kin, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(v_flex, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(v_imu, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(v_gyr, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(v_kin, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector, m_qSOUT)
,CONSTRUCT_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector, m_qSOUT)
,CONSTRUCT_SIGNAL_OUT(a_ac, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(v_ac, dynamicgraph::Vector, m_vSOUT)
,CONSTRUCT_SIGNAL_OUT(q_lf, dynamicgraph::Vector, m_qSOUT)
,CONSTRUCT_SIGNAL_OUT(q_rf, dynamicgraph::Vector, m_qSOUT)
,CONSTRUCT_SIGNAL_OUT(q_imu, dynamicgraph::Vector, m_qSOUT<<m_imu_quaternionSIN)
......@@ -491,8 +493,10 @@ namespace dynamicgraph
SE3 dummy, dummy1, lfMff, rfMff;
m_oMrfs = SE3::Identity();
m_oMlfs = SE3::Identity();
kinematics_estimation(ftrf, m_K_rf, m_oMrfs, m_right_foot_id, rfMff, dummy, dummy1); //rfMff is obtain reading oMff becaused oMrfs is here set to Identity
kinematics_estimation(ftlf, m_K_lf, m_oMlfs, m_left_foot_id, lfMff, dummy, dummy1);
kinematics_estimation(ftrf, m_K_rf, m_oMrfs, static_cast<int>(m_right_foot_id),
rfMff, dummy, dummy1); //rfMff is obtain reading oMff becaused oMrfs is here set to Identity
kinematics_estimation(ftlf, m_K_lf, m_oMlfs, static_cast<int>(m_left_foot_id),
lfMff, dummy, dummy1);
// distance from ankle to ground
const Vector3 & ankle_2_sole_xyz = m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ;
......@@ -570,8 +574,8 @@ namespace dynamicgraph
const Eigen::VectorXd& qj= m_joint_positionsSIN(iter); //n+6
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(qj.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_positions");
assert(dq.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_velocities");
assert(qj.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* convert sot to pinocchio joint order */
m_robot_util->joints_sot_to_urdf(qj, m_q_pin.tail(m_robot_util->m_nbJoints));
......@@ -598,7 +602,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints+6)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & qj = m_joint_positionsSIN(iter); //n+6
......@@ -627,7 +631,7 @@ namespace dynamicgraph
wR = 0.0;
}
assert(qj.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_positions");
assert(qj.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
// if both weights are zero set them to a small positive value to avoid division by zero
if(wR==0.0 && wL==0.0)
......@@ -649,8 +653,8 @@ namespace dynamicgraph
getProfiler().start(PROFILE_BASE_POSITION_ESTIMATION);
{
SE3 oMlfa, oMrfa, lfsMff, rfsMff;
kinematics_estimation(ftrf, m_K_rf, m_oMrfs, m_right_foot_id, m_oMff_rf, oMrfa, rfsMff);
kinematics_estimation(ftlf, m_K_lf, m_oMlfs, m_left_foot_id, m_oMff_lf, oMlfa, lfsMff);
kinematics_estimation(ftrf, m_K_rf, m_oMrfs, static_cast<int>(m_right_foot_id), m_oMff_rf, oMrfa, rfsMff);
kinematics_estimation(ftlf, m_K_lf, m_oMlfs, static_cast<int>(m_left_foot_id), m_oMff_lf, oMlfa, lfsMff);
// get rpy
const SE3 ffMchest(m_data->oMf[m_IMU_body_id]); // transform between freeflyer and body attached to IMU sensor
......@@ -787,12 +791,11 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal lf_xyzquat before initialization!");
SEND_WARNING_STREAM_MSG("Cannot compute signal lf_xyzquat before initialization! iter"+toString(iter));
return s;
}
if(s.size()!=7)
s.resize(7);
const Eigen::VectorXd & q = m_qSOUT(iter);
s = m_oMlfs_xyzquat;
return s;
}
......@@ -801,12 +804,11 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal rf_xyzquat before initialization!");
SEND_WARNING_STREAM_MSG("Cannot compute signal rf_xyzquat before initialization! iter"+toString(iter));
return s;
}
if(s.size()!=7)
s.resize(7);
const Eigen::VectorXd & q = m_qSOUT(iter);
s = m_oMrfs_xyzquat;
return s;
}
......@@ -818,7 +820,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_lf before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints+6)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -835,7 +837,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_rf before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints+6)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -852,7 +854,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_imu before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints+6)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -966,7 +968,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints+6)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -977,10 +979,9 @@ namespace dynamicgraph
const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
const Eigen::Vector3d& acc_imu = m_accelerometerSIN(iter);
const Eigen::Vector3d& gyr_imu = m_gyroscopeSIN(iter);
const Eigen::Vector4d& quatIMU_vec = m_imu_quaternionSIN(iter);
const Vector6 & dftrf = m_dforceRLEGSIN(iter);
const Vector6 & dftlf = m_dforceLLEGSIN(iter);
assert(dq.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_velocities");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
// if the weights are not specified by the user through the input signals w_lf, w_rf
// then compute them
......@@ -1107,7 +1108,6 @@ namespace dynamicgraph
/* Here we could remove dc from gyrometer to remove bias*/ ///TODO
const Motion imuWimu(imuVimu,gyr_imu);
//const Frame & f_imu = m_model.frames[m_IMU_body_id];
const Motion ffWchest = m_data->v[f_imu.parent];
//const SE3 ffMchest(m_data->oMf[m_IMU_body_id]);
//const SE3 chestMimu(Matrix3::Identity(), +1.0*Vector3(-0.13, 0.0, 0.118)); ///TODO Read this transform from setable parameter /// TODO chesk the sign of the translation
const SE3 ffMimu = ffMchest * chestMimu;
......@@ -1224,4 +1224,3 @@ namespace dynamicgraph
} // namespace torquecontrol
} // namespace sot
} // namespace dynamicgraph
......@@ -103,8 +103,8 @@ namespace dynamicgraph
m_bemf_factorSIN)
,m_robot_util(RefVoidRobotUtil())
,m_initSucceeded(false)
,m_is_first_iter(true)
,m_emergency_stop_triggered(false)
,m_is_first_iter(true)
,m_iter(0)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
......@@ -115,7 +115,7 @@ namespace dynamicgraph
docCommandVoid3("Initialize the entity.",
"Time period in seconds (double)",
"Robot reference (string)",
"Number of iterations while control is disabled to calibrate current sensors (int)")));
"Number of iterations while control is disabled to calibrate current sensors (int)")));
addCommand("reset_integral",
makeCommandVoid0(*this, &CurrentController::reset_integral,
......@@ -206,7 +206,7 @@ namespace dynamicgraph
//s = s.cwiseProduct(in_out_gain);
// when estimating current offset set ctrl to zero
if(m_emergency_stop_triggered || m_iter<m_currentOffsetIters)
if(m_emergency_stop_triggered || m_iter<static_cast<int>(m_currentOffsetIters))
s.setZero();
return s;
......@@ -226,7 +226,7 @@ namespace dynamicgraph
const dynamicgraph::Vector& i_real = m_i_realSOUT(iter);
const dynamicgraph::Vector& in_out_gain = m_in_out_gainSIN(iter);
if(s.size()!=m_robot_util->m_nbJoints)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
......@@ -256,7 +256,7 @@ namespace dynamicgraph
}
// when estimating current offset set ctrl to zero
if(m_emergency_stop_triggered || m_iter<m_currentOffsetIters)
if(m_emergency_stop_triggered || m_iter<static_cast<int>(m_currentOffsetIters))
s.setZero();
return s;
......@@ -311,15 +311,15 @@ namespace dynamicgraph
// Compute current sensor offsets
if (m_currentOffsetIters > 0)
{
if(m_iter<m_currentOffsetIters)
if(m_iter<static_cast<int>(m_currentOffsetIters))
m_i_offsets_real += (currents-m_i_offsets_real)/(m_iter+1);
else if(m_iter==m_currentOffsetIters)
else if(m_iter==static_cast<int>(m_currentOffsetIters))
{
SEND_MSG("Current sensor offsets computed in "+toString(m_iter)+" iterations: "+toString(m_i_offsets_real), MSG_TYPE_INFO);
for(int i=0; i<s.size(); i++)
if(fabs(m_i_offsets_real(i))>0.6)
{
SEND_MSG("Current offset for joint "+m_robot_util->get_name_from_id(i)+
SEND_MSG("Current offset for joint "+m_robot_util->get_name_from_id(i)+
" is too large, suggesting that the sensor may be broken: "+toString(m_i_offsets_real(i)), MSG_TYPE_WARNING);
m_i_offsets_real(i) = 0.0;
}
......@@ -409,8 +409,7 @@ namespace dynamicgraph
}
catch (ExceptionSignal e) {}
}
} // namespace torquecontrol
} // namespace sot
} // namespace dynamicgraph
......@@ -41,7 +41,7 @@ namespace dynamicgraph
#define PROFILE_FREE_FLYER_VELOCITY_COMPUTATION "Free-flyer velocity computation"
#define INPUT_SIGNALS m_base6d_encodersSIN << m_joint_velocitiesSIN
#define OUTPUT_SIGNALS m_base6dFromFoot_encodersSOUT << m_freeflyer_aaSOUT << m_vSOUT
#define OUTPUT_SIGNALS m_freeflyer_aaSOUT << m_base6dFromFoot_encodersSOUT << m_vSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
......@@ -60,8 +60,8 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN( base6d_encoders, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN( joint_velocities, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector, INPUT_SIGNALS)
,CONSTRUCT_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector, m_kinematics_computationsSINNER)
,CONSTRUCT_SIGNAL_OUT(freeflyer_aa, dynamicgraph::Vector, m_base6dFromFoot_encodersSOUT)
,CONSTRUCT_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector, m_kinematics_computationsSINNER)
,CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector, m_kinematics_computationsSINNER)
,m_initSucceeded(false)
,m_model(0)
......@@ -111,7 +111,7 @@ namespace dynamicgraph
pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
pinocchio::JointModelFreeFlyer(),*m_model);
assert(m_model->nv == m_robot_util->m_nbJoints+6);
assert(m_model->nv == static_cast<int>(m_robot_util->m_nbJoints+6));
assert(m_model->existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
assert(m_model->existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
m_left_foot_id = m_model->getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
......@@ -147,8 +147,8 @@ namespace dynamicgraph
const Eigen::VectorXd& q= m_base6d_encodersSIN(iter); //n+6
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(q.size()==m_robot_util->m_nbJoints+6 && "Unexpected size of signal base6d_encoder");
assert(dq.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_velocities");
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* convert sot to pinocchio joint order */
m_robot_util->joints_sot_to_urdf(q.tail(m_robot_util->m_nbJoints), m_q_pin.tail(m_robot_util->m_nbJoints));
......@@ -168,7 +168,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal base6dFromFoot_encoders before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints+6)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -176,7 +176,7 @@ namespace dynamicgraph
getProfiler().start(PROFILE_FREE_FLYER_COMPUTATION);
{
const Eigen::VectorXd& q= m_base6d_encodersSIN(iter); //n+6
assert(q.size()==m_robot_util->m_nbJoints+6 && "Unexpected size of signal base6d_encoder");
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
/* Compute kinematic and return q with freeflyer */
const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
......@@ -232,7 +232,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints+6)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -240,7 +240,7 @@ namespace dynamicgraph
getProfiler().start(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
{
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(dq.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_velocities");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* Compute foot velocities */
const Frame & f_lf = m_model->frames[m_left_foot_id];
......@@ -283,4 +283,3 @@ namespace dynamicgraph
} // namespace torquecontrol
} // namespace sot
} // namespace dynamicgraph
......@@ -119,11 +119,13 @@ namespace dynamicgraph
<< m_qSIN \
<< m_vSIN \
<< m_wrench_baseSIN \
<< m_active_jointsSIN \
<< m_wrench_left_footSIN \
<< m_wrench_right_footSIN
<< m_wrench_right_footSIN \
<< m_active_jointsSIN
#define OUTPUT_SIGNALS m_tau_desSOUT \
<< m_MSOUT \
<< m_dv_desSOUT \
<< m_f_des_right_footSOUT \
<< m_f_des_left_footSOUT \
<< m_zmp_des_right_footSOUT \
......@@ -147,9 +149,7 @@ namespace dynamicgraph
<< m_right_foot_accSOUT \
<< m_left_foot_accSOUT \
<< m_right_foot_acc_desSOUT \
<< m_left_foot_acc_desSOUT \
<< m_dv_desSOUT \
<< m_MSOUT
<< m_left_foot_acc_desSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
......@@ -203,14 +203,13 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(w_base_orientation, double)
,CONSTRUCT_SIGNAL_IN(w_torques, double)
,CONSTRUCT_SIGNAL_IN(w_forces, double)
,CONSTRUCT_SIGNAL_IN(mu, double)
,CONSTRUCT_SIGNAL_IN(f_min, double)
,CONSTRUCT_SIGNAL_IN(f_max_right_foot, double)
,CONSTRUCT_SIGNAL_IN(f_max_left_foot, double)
,CONSTRUCT_SIGNAL_IN(dt_joint_pos_limits, double)
,CONSTRUCT_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(mu, double)
,CONSTRUCT_SIGNAL_IN(contact_points, dynamicgraph::Matrix)
,CONSTRUCT_SIGNAL_IN(contact_normal, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(f_min, double)
,CONSTRUCT_SIGNAL_IN(f_max_right_foot, double)
,CONSTRUCT_SIGNAL_IN(f_max_left_foot, double)
,CONSTRUCT_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(gear_ratios, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(tau_max, dynamicgraph::Vector)
......@@ -218,6 +217,7 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(q_max, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(dq_max, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(ddq_max, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(dt_joint_pos_limits, double)
,CONSTRUCT_SIGNAL_IN(tau_estimated, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector)
......@@ -226,6 +226,8 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS)
,CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector, m_f_des_right_footSOUT)
......@@ -242,28 +244,26 @@ namespace dynamicgraph
m_wrench_right_footSIN<<
m_zmp_left_footSOUT<<
m_zmp_right_footSOUT)
,CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN)
,m_t(0.0)
,m_initSucceeded(false)
,m_enabled(false)
,m_t(0.0)
,m_firstTime(true)
,m_timeLast(0)
,m_contactState(DOUBLE_SUPPORT)
,m_timeLast(0)
,m_robot_util(RefVoidRobotUtil())
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
......@@ -412,10 +412,10 @@ namespace dynamicgraph
const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0);
const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0);
assert(kp_posture.size()==m_robot_util->m_nbJoints);
assert(kd_posture.size()==m_robot_util->m_nbJoints);
assert(rotor_inertias_sot.size()==m_robot_util->m_nbJoints);
assert(gear_ratios_sot.size()==m_robot_util->m_nbJoints);
assert(kp_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(kd_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(rotor_inertias_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(gear_ratios_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
m_w_com = m_w_comSIN(0);
m_w_posture = m_w_postureSIN(0);
......@@ -1117,7 +1117,9 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal zmp before initialization!");
std::ostringstream oss("Cannot compute signal zmp before initialization!");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
if(s.size()!=2)
......@@ -1138,7 +1140,9 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal com before initialization!");
std::ostringstream oss("Cannot compute signal com before initialization! iter:");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
if(s.size()!=3)
......@@ -1152,7 +1156,9 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal com_vel before initialization!");
std::ostringstream oss("Cannot compute signal com_vel before initialization! iter:");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
if(s.size()!=3)
......@@ -1167,7 +1173,9 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal base_orientation before initialization!");
std::ostringstream oss("Cannot compute signal com_vel before initialization! iter:");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
/*
......@@ -1180,7 +1188,9 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal left_foot_pos before initialization!");
std::ostringstream oss("Cannot compute signal left_foot_pos before initialization! iter:");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
m_tau_desSOUT(iter);
......@@ -1195,7 +1205,9 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal right_foot_pos before initialization!");
std::ostringstream oss("Cannot compute signal rigt_foot_pos before initialization! iter:");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
m_tau_desSOUT(iter);
......@@ -1210,7 +1222,9 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal left_foot_vel before initialization!");
std::ostringstream oss("Cannot compute signal left_foot_vel before initialization!");
oss << iter;
SEND_WARNING_STREAM_MSG(oss.str());
return s;
}
pinocchio::Motion v;
......@@ -1313,4 +1327,3 @@ namespace dynamicgraph
} // namespace torquecontrol
} // namespace sot
} // namespace dynamicgraph
......@@ -151,12 +151,12 @@ namespace dynamicgraph
const VectorN& qRef = m_qRefSIN(iter); // n
const VectorN& dqRef = m_dqRefSIN(iter); // n
assert(q.size()==m_robot_util->m_nbJoints+6 && "Unexpected size of signal base6d_encoder");
assert(dq.size()==m_robot_util->m_nbJoints && "Unexpected size of signal dq");
assert(qRef.size()==m_robot_util->m_nbJoints && "Unexpected size of signal qRef");
assert(dqRef.size()==m_robot_util->m_nbJoints && "Unexpected size of signal dqRef");
assert(Kp.size()==m_robot_util->m_nbJoints && "Unexpected size of signal Kd");
assert(Kd.size()==m_robot_util->m_nbJoints && "Unexpected size of signal Kd");
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dq");
assert(qRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
assert(dqRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dqRef");
assert(Kp.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
assert(Kd.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
m_pwmDes = Kp.cwiseProduct(qRef-q.tail(m_robot_util->m_nbJoints)) + Kd.cwiseProduct(dqRef-dq);
......
Supports Markdown
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