Skip to content

Commit

Permalink
Ported TestCoM. Fixed constructors in Cartesian, Com and POstural. Fi…
Browse files Browse the repository at this point in the history 
…xed Postural
  • Loading branch information
@EnricoMingo
EnricoMingo committed Feb 21, 2024
1 parent 93d137b commit d0f3078
Show file tree
Hide file tree
Showing 8 changed files with 55 additions and 63 deletions.
1 change: 0 additions & 1 deletion include/OpenSoT/tasks/acceleration/Cartesian.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
typedef std::shared_ptr<Cartesian> Ptr;

Cartesian(const std::string task_id,
const Eigen::VectorXd& x,
const XBot::ModelInterface& robot,
const std::string& distal_link,
const std::string& base_link
Expand Down
3 changes: 1 addition & 2 deletions include/OpenSoT/tasks/acceleration/CoM.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,8 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
/**
* @brief CoM
* @param robot the robot model
* @param x vector of size of joints of the robot (not used anymore)
*/
CoM(const XBot::ModelInterface& robot, const Eigen::VectorXd& x);
CoM(const XBot::ModelInterface& robot);

/**
* @brief CoM
Expand Down
2 changes: 1 addition & 1 deletion include/OpenSoT/tasks/acceleration/Postural.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
typedef std::shared_ptr<Postural> Ptr;

Postural(const XBot::ModelInterface& robot,
AffineHelper qddot = AffineHelper(), const std::string task_id = "Postural");
AffineHelper qddot, const std::string task_id = "Postural");

Postural(const XBot::ModelInterface& robot,
const std::string task_id = "Postural");
Expand Down
1 change: 0 additions & 1 deletion src/tasks/acceleration/Cartesian.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@ using namespace OpenSoT::tasks::acceleration;
const std::string Cartesian::world_name = "world";

Cartesian::Cartesian(const std::string task_id,
const Eigen::VectorXd& x,
const XBot::ModelInterface& robot,
const std::string& distal_link,
const std::string& base_link
Expand Down
2 changes: 1 addition & 1 deletion src/tasks/acceleration/CoM.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ using XBot::Logger;

const std::string OpenSoT::tasks::acceleration::CoM::world_name = "world";

OpenSoT::tasks::acceleration::CoM::CoM(const XBot::ModelInterface& robot, const Eigen::VectorXd& x):
OpenSoT::tasks::acceleration::CoM::CoM(const XBot::ModelInterface& robot):
Task< Eigen::MatrixXd, Eigen::VectorXd >("CoM", robot.getNv()),
_robot(robot),
_distal_link("CoM"),
Expand Down
8 changes: 4 additions & 4 deletions src/tasks/acceleration/Postural.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,8 @@ OpenSoT::tasks::acceleration::Postural::Postural(

_A.setZero(_x_size, _x_size);

_Kp.setIdentity(_qref.size(), _qref.size());
_Kd.setIdentity(_qref.size(), _qref.size());
_Kp.setIdentity(robot.getNv(), robot.getNv());
_Kd.setIdentity(robot.getNv(), robot.getNv());

setLambda(10.0);
setWeight(Eigen::MatrixXd::Identity(_x_size, _x_size));
Expand Down Expand Up @@ -161,8 +161,8 @@ void OpenSoT::tasks::acceleration::Postural::_update(const Eigen::VectorXd& x)
_A = _postural_task.getM();
_b = - _postural_task.getq();

_qdot_ref.setZero(_qref.size());
_qddot_ref.setZero(_qref.size());
_qdot_ref.setZero();
_qddot_ref.setZero();
}

void OpenSoT::tasks::acceleration::Postural::_log(XBot::MatLogger2::Ptr logger)
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 @@ -273,10 +273,10 @@ add_test(NAME OpenSoT_task_velocity_Postural COMMAND testPosturalVelocityTask)
# add_dependencies(testCartesianAccelerationTask OpenSoT)
# add_test(NAME OpenSoT_task_acceleration_Cartesian COMMAND testCartesianAccelerationTask)

# ADD_EXECUTABLE(testCoMAccelerationTask tasks/acceleration/TestCoM.cpp)
# TARGET_LINK_LIBRARIES(testCoMAccelerationTask ${TestLibs})
# add_dependencies(testCoMAccelerationTask OpenSoT)
# add_test(NAME OpenSoT_task_acceleration_com COMMAND testCoMAccelerationTask)
ADD_EXECUTABLE(testCoMAccelerationTask tasks/acceleration/TestCoM.cpp)
TARGET_LINK_LIBRARIES(testCoMAccelerationTask ${TestLibs})
add_dependencies(testCoMAccelerationTask OpenSoT)
add_test(NAME OpenSoT_task_acceleration_com COMMAND testCoMAccelerationTask)

ADD_EXECUTABLE(testContactFloatingBaseTask tasks/floating_base/TestContact.cpp)
TARGET_LINK_LIBRARIES(testContactFloatingBaseTask ${TestLibs})
Expand Down
93 changes: 44 additions & 49 deletions tests/tasks/acceleration/TestCoM.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,54 +9,50 @@
#include <OpenSoT/solvers/eHQP.h>
#include <OpenSoT/constraints/velocity/VelocityLimits.h>
#include <xbot2_interface/logger.h>

std::string relative_path = OPENSOT_TEST_PATH "configs/coman/configs/config_coman_floating_base.yaml";
std::string _path_to_cfg = relative_path;
#include "../../common.h"

namespace{

class testCoMTask: public ::testing::Test
class testCoMTask: public TestBase
{
protected:

testCoMTask()
testCoMTask() : TestBase("coman_floating_base")
{
_model = XBot::ModelInterface::getModel(_path_to_cfg);
_q.setZero(_model->getJointNum());
_q = _model_ptr->getNeutralQ();
setGoodInitialPosition();
_dq.setZero(_model->getJointNum());
_dq.setZero(_model_ptr->getNv());

_model->setJointPosition(_q);
_model->setJointVelocity(_dq);
_model->update();
_model_ptr->setJointPosition(_q);
_model_ptr->setJointVelocity(_dq);
_model_ptr->update();

KDL::Frame l_sole_T_Waist;
_model->getPose("Waist", "l_sole", l_sole_T_Waist);
Eigen::Affine3d l_sole_T_Waist;
_model_ptr->getPose("Waist", "l_sole", l_sole_T_Waist);

l_sole_T_Waist.p.x(0.0);
l_sole_T_Waist.p.y(0.0);
l_sole_T_Waist.translation()[0] = 0.;
l_sole_T_Waist.translation()[1] = 0.;

this->setWorld(l_sole_T_Waist, _q);


com.reset(
new OpenSoT::tasks::acceleration::CoM(*_model, _q));
com.reset(new OpenSoT::tasks::acceleration::CoM(*_model_ptr));
com->getReference(_com_ref);
std::cout<<"_com_initial: \n"<<_com_ref.matrix()<<std::endl;
l_sole.reset(
new OpenSoT::tasks::acceleration::Cartesian("l_sole",_q, *_model, "l_sole", "world"));
new OpenSoT::tasks::acceleration::Cartesian("l_sole", *_model_ptr, "l_sole", "world"));
l_sole->getReference(_l_sole_ref);
std::cout<<"_l_sole_initial: \n"<<_l_sole_ref.matrix()<<std::endl;
r_sole.reset(
new OpenSoT::tasks::acceleration::Cartesian("r_sole",_q, *_model, "r_sole", "world"));
new OpenSoT::tasks::acceleration::Cartesian("r_sole", *_model_ptr, "r_sole", "world"));
r_sole->getReference(_r_sole_ref);
std::cout<<"_r_sole_initial: \n"<<_r_sole_ref.matrix()<<std::endl;
OpenSoT::tasks::acceleration::Postural::Ptr postural(new
OpenSoT::tasks::acceleration::Postural(*_model,_q.size()));
OpenSoT::tasks::acceleration::Postural(*_model_ptr));


OpenSoT::AffineHelper var = OpenSoT::AffineHelper::Identity(_q.size());
Eigen::VectorXd ddq_max = 20.*Eigen::VectorXd::Ones(_q.size());
OpenSoT::AffineHelper var = OpenSoT::AffineHelper::Identity(_model_ptr->getNv());
Eigen::VectorXd ddq_max = 20.*Eigen::VectorXd::Ones(_model_ptr->getNv());
Eigen::VectorXd ddq_min = -ddq_max;
acc_lims.reset(
new OpenSoT::constraints::GenericConstraint("acc_lims", var, ddq_max, ddq_min,
Expand All @@ -67,7 +63,7 @@ class testCoMTask: public ::testing::Test


autostack = ((l_sole + r_sole)/(com)/(postural))<<acc_lims;
autostack->update(_q);
autostack->update(Eigen::VectorXd(0));

iHQP.reset(new OpenSoT::solvers::iHQP(autostack->getStack(), autostack->getBounds()));

Expand All @@ -85,36 +81,35 @@ class testCoMTask: public ::testing::Test

}

void setWorld(const KDL::Frame& l_sole_T_Waist, Eigen::VectorXd& q)
void setWorld(const Eigen::Affine3d& l_sole_T_Waist, Eigen::VectorXd& q)
{
_model->setFloatingBasePose(l_sole_T_Waist);
_model->update();
_model->getJointPosition(q);
_model_ptr->setFloatingBasePose(l_sole_T_Waist);
_model_ptr->update();
_model_ptr->getJointPosition(q);
}

void setGoodInitialPosition() {
_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_ptr->getQIndex("RHipSag")] = -25.0*M_PI/180.0;
_q[_model_ptr->getQIndex("RKneeSag")] = 50.0*M_PI/180.0;
_q[_model_ptr->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_ptr->getQIndex("LHipSag")] = -25.0*M_PI/180.0;
_q[_model_ptr->getQIndex("LKneeSag")] = 50.0*M_PI/180.0;
_q[_model_ptr->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_ptr->getQIndex("LShSag")] = 20.0*M_PI/180.0;
_q[_model_ptr->getQIndex("LShLat")] = 20.0*M_PI/180.0;
_q[_model_ptr->getQIndex("LShYaw")] = -15.0*M_PI/180.0;
_q[_model_ptr->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_ptr->getQIndex("RShSag")] = 20.0*M_PI/180.0;
_q[_model_ptr->getQIndex("RShLat")] = -20.0*M_PI/180.0;
_q[_model_ptr->getQIndex("RShYaw")] = 15.0*M_PI/180.0;
_q[_model_ptr->getQIndex("RElbj")] = -80.0*M_PI/180.0;

}

Eigen::VectorXd _q, _dq;
XBot::ModelInterface::Ptr _model;
Eigen::Affine3d _l_sole_ref, _r_sole_ref;
Eigen::Vector3d _com_ref;
OpenSoT::tasks::acceleration::Cartesian::Ptr l_sole, r_sole;
Expand All @@ -133,23 +128,23 @@ TEST_F(testCoMTask, testCoMTask_)
this->com->setReference(_com_ref);

double dt = 0.001;
Eigen::VectorXd ddq(_q.size());
ddq.setZero(ddq.size());
Eigen::VectorXd ddq(_model_ptr->getNv());
ddq.setZero();
std::cout<<"q: "<<_q<<std::endl;
for(unsigned int i = 0; i < 1000; ++i)
{
this->_model->setJointPosition(_q);
this->_model->setJointVelocity(_dq);
this->_model->update();
this->_model_ptr->setJointPosition(_q);
this->_model_ptr->setJointVelocity(_dq);
this->_model_ptr->update();

this->autostack->update(_q);
this->autostack->update(Eigen::VectorXd(0));


if(!(iHQP->solve(ddq)))
std::cout<<"CAN NOT SOLVE"<<std::endl;

_dq += ddq*dt;
_q += _dq*dt + 0.5*ddq*dt*dt;
_q = _model_ptr->sum(_q, _dq*dt + 0.5*ddq*dt*dt);
}

Eigen::Affine3d r_actual, l_actual;
Expand Down

0 comments on commit d0f3078

Please # to comment.