Skip to content

Commit

Permalink
Ported test. Removed position from convex hull constraint constructor…
Browse files Browse the repository at this point in the history 
…. Fixed bug in convex_hull_utils
  • Loading branch information
@EnricoMingo
EnricoMingo committed Feb 9, 2024
1 parent 240dc92 commit 654b911
Show file tree
Hide file tree
Showing 5 changed files with 67 additions and 104 deletions.
3 changes: 1 addition & 2 deletions include/OpenSoT/constraints/velocity/ConvexHull.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,8 +61,7 @@
* @param robot the robot model, with floating base link set on the support foot
* @param safetyMargin the margin, in [m], of the bounds margins
*/
ConvexHull( const Eigen::VectorXd& x,
XBot::ModelInterface& robot,
ConvexHull( XBot::ModelInterface& robot,
const std::list<std::string>& links_in_contact,
const double safetyMargin = BOUND_SCALING);

Expand Down
6 changes: 3 additions & 3 deletions src/constraints/velocity/ConvexHull.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,7 @@

using namespace OpenSoT::constraints::velocity;

ConvexHull::ConvexHull(const Eigen::VectorXd& x,
XBot::ModelInterface& robot,
ConvexHull::ConvexHull(XBot::ModelInterface& robot,
const std::list<std::string>& links_in_contact,
const double safetyMargin) :
Constraint("convex_hull", robot.getNv()),
Expand All @@ -36,7 +35,7 @@ ConvexHull::ConvexHull(const Eigen::VectorXd& x,
_bUpperBound.resize(links_in_contact.size());
_bLowerBound.resize(links_in_contact.size());
_bLowerBound = -1.0e20*_bLowerBound.setOnes(_bUpperBound.size());
this->update(x);
this->update(Eigen::VectorXd(0));
}

void ConvexHull::update(const Eigen::VectorXd &x) {
Expand All @@ -60,6 +59,7 @@ bool ConvexHull::getConvexHull(std::vector<Eigen::Vector3d> &ch)
_points.clear();
// get support polygon points w.r.t. COM
if(_convex_hull->getSupportPolygonPoints(_points,_links_in_contact,_robot,"COM")){

if(_points.size() > 2)
{
_tmp_ch.clear();
Expand Down
2 changes: 2 additions & 0 deletions src/utils/convex_hull_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ convex_hull::convex_hull():
{
_pointCloud = make_shared< pcl::PointCloud<pcl::PointXYZ> >();
_projectedPointCloud = make_shared< pcl::PointCloud<pcl::PointXYZ> >();

world_T_CoM.setIdentity();
}

convex_hull::~convex_hull()
Expand Down
12 changes: 6 additions & 6 deletions tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,17 +78,17 @@ add_dependencies(testTask OpenSoT)
add_test(NAME OpenSoT_tasks_Task COMMAND testTask)


# if(${PCL_FOUND} AND ${moveit_core_FOUND})
# ADD_EXECUTABLE(testAggregatedConstraint constraints/TestAggregated.cpp)
# TARGET_LINK_LIBRARIES(testAggregatedConstraint ${TestLibs})
# add_dependencies(testAggregatedConstraint OpenSoT)
# add_test(NAME OpenSoT_constraints_Aggregated COMMAND testAggregatedConstraint)
if(${PCL_FOUND} AND ${moveit_core_FOUND})
ADD_EXECUTABLE(testAggregatedConstraint constraints/TestAggregated.cpp)
TARGET_LINK_LIBRARIES(testAggregatedConstraint ${TestLibs})
add_dependencies(testAggregatedConstraint OpenSoT)
add_test(NAME OpenSoT_constraints_Aggregated COMMAND testAggregatedConstraint)

# ADD_EXECUTABLE(testAggregatedTask tasks/TestAggregated.cpp)
# TARGET_LINK_LIBRARIES(testAggregatedTask ${TestLibs})
# add_dependencies(testAggregatedTask OpenSoT)
# add_test(NAME OpenSoT_task_Aggregated COMMAND testAggregatedTask)
# endif()
endif()


#ADD_EXECUTABLE(testl1HQP solvers/Testl1HQP.cpp)
Expand Down
148 changes: 55 additions & 93 deletions tests/constraints/TestAggregated.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,20 +6,18 @@
#include <OpenSoT/constraints/velocity/ConvexHull.h>
#include <string>
#include <xbot2_interface/xbotinterface2.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 {

// The fixture for testing class Foo.
class testAggregated : public ::testing::Test {
class testAggregated : public TestBase {
protected:
// You can remove any or all of the following functions if its body
// is empty.

testAggregated() {
testAggregated() : TestBase("coman_floating_base") {
// You can do set-up work for each test here.
}

Expand All @@ -43,88 +41,54 @@ class testAggregated : public ::testing::Test {
// Objects declared here can be used by all tests in the test case for Foo.
};

void initializeIfNeeded()
{
static bool is_initialized = false;

if(!is_initialized) {
time_t seed = time(NULL);
seed48((unsigned short*)(&seed));
srand((unsigned int)(seed));

is_initialized = true;
}

}

double getRandomAngle()
{
initializeIfNeeded();
return drand48()*2.0*M_PI-M_PI;
}

double getRandomAngle(const double min, const double max)
{
initializeIfNeeded();
assert(min <= max);
if(min < -M_PI || max > M_PI)
return getRandomAngle();

return (double)rand()/RAND_MAX * (max-min) + min;
}

Eigen::VectorXd getRandomAngles(const Eigen::VectorXd &min,
const Eigen::VectorXd &max,
const int size)
{
initializeIfNeeded();
Eigen::VectorXd q(size);
assert(min.size() >= size);
assert(max.size() >= size);
for(unsigned int i = 0; i < size; ++i)
q(i) = getRandomAngle(min[i],max[i]);
return q;
}

// Tests that the Foo::Bar() method does Abc.
TEST_F(testAggregated, AggregatedWorks) {
using namespace OpenSoT::constraints;
std::list<Aggregated::ConstraintPtr> constraints;
const unsigned int nJ = 6;
const double dT = 0.1;
const double qDotMax = 0.5;

Eigen::VectorXd q = _model_ptr->getNeutralQ();
_model_ptr->setJointPosition(q);
_model_ptr->update();

constraints.push_back( Aggregated::ConstraintPtr(
new velocity::VelocityLimits(qDotMax,dT,nJ)
new velocity::VelocityLimits(*_model_ptr, qDotMax,dT)
)
);

Eigen::VectorXd q(nJ); q.setZero(nJ);
Eigen::VectorXd q_next(nJ); q_next.setConstant(nJ, M_PI - 0.01);

Eigen::MatrixXd A(nJ,nJ); A.setIdentity(nJ,nJ);
Eigen::VectorXd bUpperBound(nJ); bUpperBound.setConstant(nJ, M_PI);
Eigen::VectorXd bLowerBound(nJ); bLowerBound.setZero(nJ);
Eigen::VectorXd q_next = _model_ptr->generateRandomQ();

Eigen::MatrixXd A(_model_ptr->getNv(),_model_ptr->getNv()); A.setIdentity();
Eigen::VectorXd bUpperBound(_model_ptr->getNv()); bUpperBound.setConstant(_model_ptr->getNv(), M_PI);
Eigen::VectorXd bLowerBound(_model_ptr->getNv()); bLowerBound.setZero();
constraints.push_back(Aggregated::ConstraintPtr(
new BilateralConstraint(A,bUpperBound,bLowerBound)));

constraints.push_back(Aggregated::ConstraintPtr(
new velocity::JointLimits(q,bUpperBound,bLowerBound)));
Aggregated::ConstraintPtr aggregated(new Aggregated(constraints, q));
new velocity::JointLimits(*_model_ptr,bUpperBound,bLowerBound)));
Aggregated::ConstraintPtr aggregated(new Aggregated(constraints, _model_ptr->getNv()));

/* we should mash joint limits and velocity limits in one */
EXPECT_TRUE(aggregated->getLowerBound().size() == nJ);
EXPECT_TRUE(aggregated->getUpperBound().size() == nJ);
EXPECT_TRUE(aggregated->getLowerBound().size() == _model_ptr->getNv());
EXPECT_TRUE(aggregated->getUpperBound().size() == _model_ptr->getNv());
/* we have a BilateralConstraint... */
EXPECT_TRUE(aggregated->getAineq().rows() == nJ);
EXPECT_TRUE(aggregated->getbLowerBound().size() == nJ);
EXPECT_TRUE(aggregated->getbUpperBound().size() == nJ);
EXPECT_TRUE(aggregated->getAineq().rows() == _model_ptr->getNv());
EXPECT_TRUE(aggregated->getbLowerBound().size() == _model_ptr->getNv());
EXPECT_TRUE(aggregated->getbUpperBound().size() == _model_ptr->getNv());
/* and no equality constraint */
EXPECT_TRUE(aggregated->getAeq().rows() == 0);
EXPECT_TRUE(aggregated->getbeq().size() == 0);

Eigen::VectorXd oldLowerBound = aggregated->getLowerBound();
Eigen::VectorXd oldUpperBound = aggregated->getUpperBound();
aggregated->update(q_next);

_model_ptr->setJointPosition(q_next);
_model_ptr->update();
aggregated->update(Eigen::VectorXd(0));

Eigen::VectorXd newLowerBound = aggregated->getLowerBound();
Eigen::VectorXd newUpperBound = aggregated->getUpperBound();
EXPECT_FALSE(oldLowerBound == newLowerBound);
Expand All @@ -133,17 +97,11 @@ TEST_F(testAggregated, AggregatedWorks) {

TEST_F(testAggregated, UnilateralToBilateralWorks) {

XBot::ModelInterface::Ptr _model_ptr;
_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;
Eigen::VectorXd q = _model_ptr->getNeutralQ();


Eigen::VectorXd q;
q.setZero(_model_ptr->getJointNum());
_model_ptr->setJointPosition(q);
_model_ptr->update();

std::list<std::string> _links_in_contact;
_links_in_contact.push_back("l_foot_lower_left_link");
Expand All @@ -156,15 +114,15 @@ TEST_F(testAggregated, UnilateralToBilateralWorks) {
_links_in_contact.push_back("r_foot_upper_right_link");

OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>::ConstraintPtr convexHull(
new OpenSoT::constraints::velocity::ConvexHull(q,*(_model_ptr.get()), _links_in_contact));
new OpenSoT::constraints::velocity::ConvexHull(*_model_ptr.get(), _links_in_contact));

std::list<OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>::ConstraintPtr> constraints;

constraints.push_back(convexHull);

OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>::ConstraintPtr aggregated(
new OpenSoT::constraints::Aggregated(constraints,
_model_ptr->getJointNum()));
_model_ptr->getNv()));

EXPECT_TRUE(aggregated->getbLowerBound().rows() == aggregated->getbUpperBound().rows()) <<
"bLowerBound:" << aggregated->getbLowerBound() << std::endl <<
Expand All @@ -173,58 +131,62 @@ TEST_F(testAggregated, UnilateralToBilateralWorks) {

}

/// TODO implement
/**
* @todo implement
*/
TEST_F(testAggregated, EqualityToInequalityWorks) {
}

TEST_F(testAggregated, MultipleAggregationdWork) {
using namespace OpenSoT::constraints;
std::list<Aggregated::ConstraintPtr> constraints;
const unsigned int nJ = 6;
const double dT = 1;
const double qDotMax = 50;
constraints.push_back( Aggregated::ConstraintPtr(
new velocity::VelocityLimits(qDotMax,dT,nJ)));
const double dT = 1.;
const double qDotMax = 50.;

Eigen::VectorXd q = _model_ptr->getNeutralQ();
_model_ptr->setJointPosition(q);
_model_ptr->update();

Eigen::VectorXd q(nJ);
q = getRandomAngles(Eigen::VectorXd::Constant(nJ, -M_PI),
Eigen::VectorXd::Constant(nJ, M_PI), nJ);
constraints.push_back( Aggregated::ConstraintPtr(
new velocity::VelocityLimits(*_model_ptr, qDotMax,dT)
)
);

Eigen::MatrixXd A;
A.setZero(nJ,nJ);
Eigen::VectorXd bUpperBound(nJ);
bUpperBound<<bUpperBound.setOnes(nJ)*M_PI;
A.setZero(_model_ptr->getNv(),_model_ptr->getNv());
Eigen::VectorXd bUpperBound(_model_ptr->getNv());
bUpperBound<<bUpperBound.setOnes(_model_ptr->getNv())*M_PI;
Eigen::VectorXd bLowerBound;
bLowerBound.setZero(nJ);
bLowerBound.setZero(_model_ptr->getNv());
constraints.push_back(Aggregated::ConstraintPtr(
new BilateralConstraint(A, bUpperBound, bLowerBound)));

Aggregated::ConstraintPtr aggregated(new Aggregated(constraints, q));
Aggregated::ConstraintPtr aggregated(new Aggregated(constraints, _model_ptr->getNv()));

constraints.push_back( Aggregated::ConstraintPtr(
new velocity::VelocityLimits(qDotMax/2,dT,nJ)));
new velocity::VelocityLimits(*_model_ptr, qDotMax/2,dT)));

for(typename std::list<Aggregated::ConstraintPtr>::iterator i = constraints.begin();
i != constraints.end(); ++i) {
Aggregated::ConstraintPtr b(*i);
if(b->getLowerBound().size() > 0)
std::cout << b->getLowerBound() << std::endl;
}
Aggregated::ConstraintPtr aggregated2(new Aggregated(constraints, q));
Aggregated::ConstraintPtr aggregated2(new Aggregated(constraints, _model_ptr->getNv()));

constraints.push_back(aggregated);
constraints.push_back(aggregated2);

Aggregated::ConstraintPtr aggregated3(new Aggregated(constraints, q));
Aggregated::ConstraintPtr aggregated3(new Aggregated(constraints, _model_ptr->getNv()));

Aggregated::ConstraintPtr aggregated4(new Aggregated(aggregated2,
aggregated3, q.size()));
aggregated3, _model_ptr->getNv()));

Eigen::VectorXd qq(q.size());
Eigen::VectorXd qq(_model_ptr->getNv());
qq<<qq.setOnes()*-0.1;
Aggregated::ConstraintPtr aggregated5(new Aggregated(aggregated4,
Aggregated::ConstraintPtr(
new velocity::JointLimits(q,qq,qq)), q.size()));
new velocity::JointLimits(*_model_ptr,qq,qq)), _model_ptr->getNv()));


/* testing constraints stay put */
Expand Down

0 comments on commit 654b911

Please sign in to comment.