Skip to content

Commit

Permalink
Moved acceleration::JointLImitsViability and tests to new API
Browse files Browse the repository at this point in the history
  • Loading branch information
@EnricoMingo
EnricoMingo committed Mar 24, 2024
1 parent 8a7cace commit 3bf98d4
Show file tree
Hide file tree
Showing 4 changed files with 53 additions and 56 deletions.
2 changes: 2 additions & 0 deletions include/OpenSoT/constraints/acceleration/JointLimitsViability.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@

GenericConstraint::Ptr _generic_constraint_internal;

Eigen::VectorXd _zeros;


/**
* @brief _dt
Expand Down
10 changes: 6 additions & 4 deletions src/constraints/acceleration/JointLimitsViability.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@ JointLimitsViability::JointLimitsViability(XBot::ModelInterface& robot,
throw std::runtime_error("_jointVelMax.size() != _jointLimitsMin.size()");
/* calling update to generate bounds */

_zeros = _robot.getNeutralQ();

_ddq_LB_pos.setZero(jointBoundMax.size());
_ddq_UB_pos.setZero(jointBoundMax.size());
_ddq_LB_via.setZero(jointBoundMax.size());
Expand Down Expand Up @@ -127,9 +129,9 @@ void JointLimitsViability::accBoundsFromPosLimits()

_ddq_M1 = -_qdot/dt;
_ddq_M2 = -(_qdot.array() * _qdot.array()) / (2.0*(_jointLimitsMax.array() - _q.array()));
_ddq_M3 = 2.0*(_jointLimitsMax - _q - dt*_qdot)/a;
_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_m3 = 2.0*(_jointLimitsMin - _q - dt*_qdot)/a;
_ddq_m3 = 2.0*(_jointLimitsMin - _robot.difference(_q, _zeros) - dt*_qdot)/a;


for(unsigned int it = 0; it < _jointLimitsMax.size(); ++it)
Expand Down Expand Up @@ -159,7 +161,7 @@ void JointLimitsViability::accBoundsFromViability()
double a = dt * dt;

_b_1 = dt*(2.0*_qdot + _jointAccMax*dt);
_c_1 = _qdot.array()*_qdot.array() - 2.0*_jointAccMax.array()*(_jointLimitsMax.array() - _q.array() - dt*_qdot.array());
_c_1 = _qdot.array()*_qdot.array() - 2.0*_jointAccMax.array()*(_jointLimitsMax.array() - _robot.difference(_q, _zeros).array() - dt*_qdot.array());
_ddq_1 = -_qdot/dt;
_delta_1 = _b_1.array()*_b_1.array() - 4.0*a*_c_1.array();

Expand All @@ -174,7 +176,7 @@ void JointLimitsViability::accBoundsFromViability()


_b_2 = dt*(2.0*_qdot - _jointAccMax*dt);
_c_2 = _qdot.array()*_qdot.array() - 2.0*_jointAccMax.array()*(_q.array() + dt*_qdot.array() - _jointLimitsMin.array());
_c_2 = _qdot.array()*_qdot.array() - 2.0*_jointAccMax.array()*(_robot.difference(_q, _zeros).array() + dt*_qdot.array() - _jointLimitsMin.array());
_delta_2 = _b_2.array()*_b_2.array() - 4.0*a*_c_2.array();


Expand Down
8 changes: 4 additions & 4 deletions tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,10 +243,10 @@ endif()
add_dependencies(testJointLimitsAccelerationBounds OpenSoT)
add_test(NAME OpenSoT_constraints_acceleration_JointLimits COMMAND testJointLimitsAccelerationBounds)

# ADD_EXECUTABLE(testJointLimitsAccelerationViabilityBounds constraints/acceleration/TestJointLimitsViability.cpp)
# TARGET_LINK_LIBRARIES(testJointLimitsAccelerationViabilityBounds ${TestLibs})
# add_dependencies(testJointLimitsAccelerationViabilityBounds OpenSoT)
# add_test(NAME OpenSoT_constraints_acceleration_viability_JointLimits COMMAND testJointLimitsAccelerationViabilityBounds)
ADD_EXECUTABLE(testJointLimitsAccelerationViabilityBounds constraints/acceleration/TestJointLimitsViability.cpp)
TARGET_LINK_LIBRARIES(testJointLimitsAccelerationViabilityBounds ${TestLibs})
add_dependencies(testJointLimitsAccelerationViabilityBounds OpenSoT)
add_test(NAME OpenSoT_constraints_acceleration_viability_JointLimits COMMAND testJointLimitsAccelerationViabilityBounds)

ADD_EXECUTABLE(testJointLimitsAccelerationECBFBounds constraints/acceleration/TestJointLimitsECBF.cpp)
TARGET_LINK_LIBRARIES(testJointLimitsAccelerationECBFBounds ${TestLibs})
Expand Down
89 changes: 41 additions & 48 deletions tests/constraints/acceleration/TestJointLimitsViability.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,34 +9,24 @@
#include <cmath>
#include <OpenSoT/solvers/iHQP.h>
#include <matlogger2/matlogger2.h>
#include "../../common.h"

std::string relative_path = OPENSOT_TEST_PATH "configs/coman/configs/config_coman_RBDL.yaml";
std::string _path_to_cfg = relative_path;

namespace {

class testJointLimitsViability : public ::testing::Test {
class testJointLimitsViability : public TestBase {
protected:
testJointLimitsViability():
testJointLimitsViability(): TestBase("coman_floating_base"),
p_test(0.001, M_PI, 20.),
p_dp(0.01, 2., 12.)
{

_model_ptr = XBot::ModelInterface::getModel(_path_to_cfg);

if(_model_ptr)
std::cout<<"pointer address: "<<_model_ptr.get()<<std::endl;
else
std::cout<<"pointer is NULL "<<_model_ptr.get()<<std::endl;

logger = XBot::MatLogger2::MakeLogger("/tmp/testJointLimitsViability_acceleration");
logger->set_buffer_mode(XBot::VariableBuffer::Mode::circular_buffer);

}

void createStack()
{
qdot.setZero(_model_ptr->getJointNum());
qdot.setZero(_model_ptr->getNv());
_model_ptr->setJointVelocity(qdot);


Expand All @@ -46,22 +36,22 @@ class testJointLimitsViability : public ::testing::Test {

_model_ptr->update();

qddot = OpenSoT::AffineHelper::Identity(_model_ptr->getJointNum());
qddot = OpenSoT::AffineHelper::Identity(_model_ptr->getNv());


acc_lims.setOnes(_model_ptr->getJointNum());
acc_lims.setOnes(_model_ptr->getNv());
acc_lims *= p_test._qddot_max; //p_dp._qddot_max; //20.;

dT = p_test._dt; //p_dp._dt; //0.001;
qdotMax = p_test._qdot_max; //p_dp._qdot_max; //M_PI;
Eigen::VectorXd vel_lims;
vel_lims.setOnes(_model_ptr->getJointNum());
vel_lims.setOnes(_model_ptr->getNv());
vel_lims *= qdotMax;

_model_ptr->getJointLimits(qmin, qmax);
jointLimits = std::make_shared<OpenSoT::constraints::acceleration::JointLimitsViability>(
*_model_ptr, qddot, qmax, qmin, vel_lims, acc_lims, dT);
jointLimits->setPStepAheadPredictor(10.);
jointLimits->setPStepAheadPredictor(2.);

jointAccelerationLimits = std::make_shared<OpenSoT::constraints::GenericConstraint>(
"acceleration_limits", acc_lims, -acc_lims, acc_lims.size());
Expand Down Expand Up @@ -96,11 +86,13 @@ class testJointLimitsViability : public ::testing::Test {
void checkConstraints(const Eigen::VectorXd& qddot, double EPS)
{

Eigen::VectorXd _q = _model_ptr->difference(q, _model_ptr->getNeutralQ());

for(unsigned int i = 0; i < qddot.size(); ++i)
{
double x = qddot[i];
double x_min = -acc_lims[i];
double x_max = acc_lims[i];
double x_min = jointAccelerationLimits->getLowerBound()[i];
double x_max = jointAccelerationLimits->getUpperBound()[i];

EXPECT_TRUE( (x-x_min >= -EPS) && (x-x_max <= EPS) )<<
"joint acc violated @i:"<<i<<" "<<x_min<<" <= "<<x<<" <= "<<x_max<<std::endl;
Expand All @@ -111,38 +103,36 @@ class testJointLimitsViability : public ::testing::Test {
EXPECT_TRUE( (x-x_min >= -EPS) && (x-x_max <= EPS) )<<
"joint vel violated @i:"<<i<<" "<<x_min<<" <= "<<x<<" <= "<<x_max<<std::endl;

EXPECT_TRUE( (q[i]-qmin[i] >= -EPS) && (q[i]-qmax[i] <= EPS) )<<
"joint limits @i:"<<i<<" "<<qmin[i]<<" <= "<<q[i]<<" <= "<<qmax[i]<<std::endl;
EXPECT_TRUE( (_q[i]-qmin[i] >= -EPS) && (_q[i]-qmax[i] <= EPS) )<<
"joint limits @i:"<<i<<" "<<qmin[i]<<" <= "<<_q[i]<<" <= "<<qmax[i]<<std::endl;

}
}

Eigen::VectorXd getGoodInitialPosition(XBot::ModelInterface::Ptr _model) {
Eigen::VectorXd _q(_model->getJointNum());
_q.setZero(_q.size());
Eigen::VectorXd _q = _model_ptr->getNeutralQ();

_q[_model->getDofIndex("RHipSag")] = -25.0*M_PI/180.0;
_q[_model->getDofIndex("RKneeSag")] = 50.0*M_PI/180.0;
_q[_model->getDofIndex("RAnkSag")] = -25.0*M_PI/180.0;
_q[_model->getQIndex("RHipSag")] = -25.0*M_PI/180.0;
_q[_model->getQIndex("RKneeSag")] = 50.0*M_PI/180.0;
_q[_model->getQIndex("RAnkSag")] = -25.0*M_PI/180.0;

_q[_model->getDofIndex("LHipSag")] = -25.0*M_PI/180.0;
_q[_model->getDofIndex("LKneeSag")] = 50.0*M_PI/180.0;
_q[_model->getDofIndex("LAnkSag")] = -25.0*M_PI/180.0;
_q[_model->getQIndex("LHipSag")] = -25.0*M_PI/180.0;
_q[_model->getQIndex("LKneeSag")] = 50.0*M_PI/180.0;
_q[_model->getQIndex("LAnkSag")] = -25.0*M_PI/180.0;

_q[_model->getDofIndex("LShSag")] = 20.0*M_PI/180.0;
_q[_model->getDofIndex("LShLat")] = 20.0*M_PI/180.0;
_q[_model->getDofIndex("LShYaw")] = -15.0*M_PI/180.0;
_q[_model->getDofIndex("LElbj")] = -80.0*M_PI/180.0;
_q[_model->getQIndex("LShSag")] = 20.0*M_PI/180.0;
_q[_model->getQIndex("LShLat")] = 20.0*M_PI/180.0;
_q[_model->getQIndex("LShYaw")] = -15.0*M_PI/180.0;
_q[_model->getQIndex("LElbj")] = -80.0*M_PI/180.0;

_q[_model->getDofIndex("RShSag")] = 20.0*M_PI/180.0;
_q[_model->getDofIndex("RShLat")] = -20.0*M_PI/180.0;
_q[_model->getDofIndex("RShYaw")] = 15.0*M_PI/180.0;
_q[_model->getDofIndex("RElbj")] = -80.0*M_PI/180.0;
_q[_model->getQIndex("RShSag")] = 20.0*M_PI/180.0;
_q[_model->getQIndex("RShLat")] = -20.0*M_PI/180.0;
_q[_model->getQIndex("RShYaw")] = 15.0*M_PI/180.0;
_q[_model->getQIndex("RElbj")] = -80.0*M_PI/180.0;

return _q;
}

XBot::ModelInterface::Ptr _model_ptr;

OpenSoT::constraints::acceleration::JointLimitsViability::Ptr jointLimits;
OpenSoT::constraints::GenericConstraint::Ptr jointAccelerationLimits;
OpenSoT::constraints::acceleration::VelocityLimits::Ptr jointVelocityLimits;
Expand Down Expand Up @@ -190,8 +180,7 @@ class testJointLimitsViability : public ::testing::Test {
TEST_F(testJointLimitsViability, testBoundsWithTrajectory) {
this->createStack();

Eigen::VectorXd qref(this->q.size());
qref.setZero();
Eigen::VectorXd qref = _model_ptr->getNeutralQ();

this->postural->setReference(qref);

Expand All @@ -209,7 +198,7 @@ TEST_F(testJointLimitsViability, testBoundsWithTrajectory) {
ASSERT_TRUE(this->solver->solve(qddot));


this->q += this->qdot*this->dT + 0.5*qddot*this->dT*this->dT;
this->q = _model_ptr->sum(this->q, this->qdot*this->dT + 0.5*qddot*this->dT*this->dT);
this->qdot += qddot*this->dT;

// this->logger->add("qddot", qddot);
Expand Down Expand Up @@ -242,6 +231,9 @@ TEST_F(testJointLimitsViability, testBoundsWithTrajectory) {
qref.setOnes(qref.size());
qref *= 3.* std::sin(i*this->dT);
qref += q0;

qref.segment(3,4) = qref.segment(3,4)/qref.segment(3,4).norm();

this->postural->setReference(qref);

this->_model_ptr->setJointVelocity(this->qdot);
Expand All @@ -255,7 +247,7 @@ TEST_F(testJointLimitsViability, testBoundsWithTrajectory) {
ASSERT_TRUE(this->solver->solve(qddot));


this->q += this->qdot*this->dT + 0.5*qddot*this->dT*this->dT;
this->q = _model_ptr->sum(this->q, this->qdot*this->dT + 0.5*qddot*this->dT*this->dT);
this->qdot += qddot*this->dT;

this->logger->add("qddot", qddot);
Expand All @@ -278,8 +270,7 @@ TEST_F(testJointLimitsViability, testBoundsWithTrajectory) {
TEST_F(testJointLimitsViability, testBoundsWithRegulation) {
this->createStack();

Eigen::VectorXd qref(this->q.size());
qref.setZero();
Eigen::VectorXd qref = _model_ptr->getNeutralQ();

this->postural->setReference(qref);

Expand All @@ -296,7 +287,7 @@ TEST_F(testJointLimitsViability, testBoundsWithRegulation) {
ASSERT_TRUE(this->solver->solve(qddot));


this->q += this->qdot*this->dT + 0.5*qddot*this->dT*this->dT;
this->q = _model_ptr->sum(this->q, this->qdot*this->dT + 0.5*qddot*this->dT*this->dT);
this->qdot += qddot*this->dT;

this->checkConstraints(qddot, 1e-4);
Expand Down Expand Up @@ -326,6 +317,8 @@ TEST_F(testJointLimitsViability, testBoundsWithRegulation) {
else
qref *= -3.;

qref.segment(3,4) = qref.segment(3,4)/qref.segment(3,4).norm();

this->postural->setReference(qref);

this->_model_ptr->setJointVelocity(this->qdot);
Expand All @@ -339,7 +332,7 @@ TEST_F(testJointLimitsViability, testBoundsWithRegulation) {
ASSERT_TRUE(this->solver->solve(qddot));


this->q += this->qdot*this->dT + 0.5*qddot*this->dT*this->dT;
this->q = _model_ptr->sum(this->q, this->qdot*this->dT + 0.5*qddot*this->dT*this->dT);
this->qdot += qddot*this->dT;

logger->add("qddot", qddot);
Expand Down

0 comments on commit 3bf98d4

Please sign in to comment.