Merge branch '4.0-devel' into 4.0-devel_ros2

Author: EnricoMingo

src/constraints/acceleration/JointLimitsViability.cpp

@@ -128,9 +128,9 @@ void JointLimitsViability::accBoundsFromPosLimits()
     double a = dt * dt;
 
     _ddq_M1 = -_qdot/dt;
-    _ddq_M2 = -(_qdot.array() * _qdot.array()) / (2.0*(_jointLimitsMax.array() - _q.array()));
+    _ddq_M2 = -(_qdot.array() * _qdot.array()) / (2.0*(_jointLimitsMax.array() - _robot.difference(_q, _zeros).array()));
     _ddq_M3 = 2.0*(_jointLimitsMax - _robot.difference(_q, _zeros) - dt*_qdot)/a;
-    _ddq_m2 = (_qdot.array()*_qdot.array())/(2.0*(_q.array() - _jointLimitsMin.array()));
+    _ddq_m2 = (_qdot.array()*_qdot.array())/(2.0*(_robot.difference(_q, _zeros).array() - _jointLimitsMin.array()));
     _ddq_m3 = 2.0*(_jointLimitsMin - _robot.difference(_q, _zeros) - dt*_qdot)/a;
 
 

tests/constraints/TestSubConstraint.cpp

@@ -133,7 +133,8 @@ TEST_F(TestSubConstraint, testSubInequalityConstraint)
 
     EXPECT_EQ(sub_vcom->getbLowerBound(), this->_vcom_constraint->getbLowerBound().segment(0, indices.size()));
     EXPECT_EQ(sub_vcom->getbUpperBound(), this->_vcom_constraint->getbUpperBound().segment(0, indices.size()));
-    EXPECT_EQ(sub_vcom->getAineq(), this->_vcom_constraint->getAineq().block(0,0,2,2));
+    std::cout<<this->_vcom_constraint->getAineq().block(0,0,2,2)<<std::endl;
+    EXPECT_EQ(sub_vcom->getAineq(), this->_vcom_constraint->getAineq().topRows(2));
 
     this->q = _model_ptr->generateRandomQ();
 
@@ -156,7 +157,7 @@ TEST_F(TestSubConstraint, testSubInequalityConstraint)
 
     EXPECT_EQ(sub_vcom->getbLowerBound(), this->_vcom_constraint->getbLowerBound().segment(0, indices.size()));
     EXPECT_EQ(sub_vcom->getbUpperBound(), this->_vcom_constraint->getbUpperBound().segment(0, indices.size()));
-    EXPECT_EQ(sub_vcom->getAineq(), this->_vcom_constraint->getAineq().block(0,0,2,2));
+    EXPECT_EQ(sub_vcom->getAineq(), this->_vcom_constraint->getAineq().topRows(2));
 
 }
 

tests/solvers/TestQPOases.cpp

@@ -622,7 +622,7 @@ TEST_F(testiHQP, testContructor1Problem)
     bounds_list.push_back(joint_limits);
 
     OpenSoT::constraints::Aggregated::Ptr bounds(
-                new OpenSoT::constraints::Aggregated(bounds_list, q.size()));
+        new OpenSoT::constraints::Aggregated(bounds_list, _model_ptr->getNv()));
 
     OpenSoT::solvers::iHQP::Stack stack_of_tasks;
     stack_of_tasks.push_back(postural_task);