Commit c86b64c8 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Split haveJointTrajectory in two, add haveJointsDerivativesTrajectories

parent bcef57d3
......@@ -135,7 +135,11 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
"If this effector was in contact in the previous phase,"
"it check that the trajectory start at the previous contact placement.")
.def("haveJointsTrajectories", &CS::haveJointsTrajectories,
"Check that a q, dq and ddq trajectories are defined for each phases.\n"
"Check that a q trajectory is defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveJointsDerivativesTrajectories", &CS::haveJointsDerivativesTrajectories,
"Check that a dq and ddq trajectories are defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveTorquesTrajectories", &CS::haveTorquesTrajectories,
......
......@@ -628,8 +628,9 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
return true;
}
/**
* @brief haveJointsTrajectories Check that a q, dq and ddq trajectories are defined for each phases
* @brief haveJointsTrajectories Check that a q trajectory is defined for each phases
* Also check that the time interval of this trajectories matches the one of the phase
* and that the trajectories start and end and the correct values defined in each phase
* @return
......@@ -643,6 +644,38 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout<<"joint position trajectory not defined for phase : "<<i<<std::endl;
return false;
}
if(phase.m_q->min() != phase.timeInitial()){
std::cout<<"joint trajectory do not start at t_init for phase : "<<i<<std::endl;
return false;
}
if(phase.m_q->max() != phase.timeFinal()){
std::cout<<"joint trajectory do not end at t_final for phase : "<<i<<std::endl;
return false;
}
if(!(*phase.m_q)(phase.m_q->min()).isApprox(phase.m_q_init) ){
std::cout<<"joint trajectory do not start at q_init for phase : "<<i<<std::endl;
return false;
}
if(!(*phase.m_q)(phase.m_q->max()).isApprox(phase.m_q_final) ){
std::cout<<"joint trajectory do not end at q_final for phase : "<<i<<std::endl;
return false;
}
++i;
}
return true;
}
/**
* @brief haveJointsDerivativesTrajectories Check that a dq and ddq trajectories are defined for each phases
* Also check that the time interval of this trajectories matches the one of the phase
* and that the trajectories start and end and the correct values defined in each phase
* @return
*/
bool haveJointsDerivativesTrajectories() const{
if(!(haveTimings()))
return false;
size_t i = 0;
for(const ContactPhase& phase : m_contact_phases){
if(!phase.m_dq){
std::cout<<"joint velocity trajectory not defined for phase : "<<i<<std::endl;
return false;
......@@ -651,10 +684,6 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout<<"joint acceleration trajectory not defined for phase : "<<i<<std::endl;
return false;
}
if(phase.m_q->min() != phase.timeInitial()){
std::cout<<"joint trajectory do not start at t_init for phase : "<<i<<std::endl;
return false;
}
if(phase.m_dq->min() != phase.timeInitial()){
std::cout<<"joint velocity trajectory do not start at t_init for phase : "<<i<<std::endl;
return false;
......@@ -663,10 +692,6 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout<<"joint acceleration trajectory do not start at t_init for phase : "<<i<<std::endl;
return false;
}
if(phase.m_q->max() != phase.timeFinal()){
std::cout<<"joint trajectory do not end at t_final for phase : "<<i<<std::endl;
return false;
}
if(phase.m_dq->max() != phase.timeFinal()){
std::cout<<"joint velocity trajectory do not end at t_final for phase : "<<i<<std::endl;
return false;
......@@ -675,19 +700,13 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout<<"joint acceleration trajectory do not end at t_final for phase : "<<i<<std::endl;
return false;
}
if(!(*phase.m_q)(phase.m_q->min()).isApprox(phase.m_q_init) ){
std::cout<<"joint trajectory do not start at q_init for phase : "<<i<<std::endl;
return false;
}
if(!(*phase.m_q)(phase.m_q->max()).isApprox(phase.m_q_final) ){
std::cout<<"joint trajectory do not end at q_final for phase : "<<i<<std::endl;
return false;
}
++i;
}
return true;
}
/**
* @brief haveJointsTrajectories Check that a joint torque trajectories are defined for each phases
* Also check that the time interval of this trajectories matches the one of the phase
......
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