Skip to content

Commit

Permalink
Fixed floating b ase contact test and tasks. Removed state from const…
Browse files Browse the repository at this point in the history 
…ructor of Cartesian task
  • Loading branch information
@EnricoMingo
EnricoMingo committed Feb 8, 2024
1 parent 520134d commit b7231b7
Show file tree
Hide file tree
Showing 12 changed files with 88 additions and 82 deletions.
10 changes: 5 additions & 5 deletions examples/cpp/coman_ik.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -400,11 +400,11 @@ int main(int argc, char **argv)
model_ptr->update();

using namespace OpenSoT::tasks::velocity;
auto r_sole = std::make_shared<Cartesian>("r_sole", q, *model_ptr.get(), "r_sole", "world");
auto l_sole = std::make_shared<Cartesian>("l_sole", q, *model_ptr.get(), "l_sole", "world");
auto l_wrist = std::make_shared<Cartesian>("l_wrist", q, *model_ptr.get(), "l_wrist", "world");
auto r_sole = std::make_shared<Cartesian>("r_sole", *model_ptr.get(), "r_sole", "world");
auto l_sole = std::make_shared<Cartesian>("l_sole", *model_ptr.get(), "l_sole", "world");
auto l_wrist = std::make_shared<Cartesian>("l_wrist", *model_ptr.get(), "l_wrist", "world");
l_wrist->setLambda(0.1);
auto r_wrist = std::make_shared<Cartesian>("r_wrist", q, *model_ptr.get(), TCP_frame, "world");
auto r_wrist = std::make_shared<Cartesian>("r_wrist", *model_ptr.get(), TCP_frame, "world");
r_wrist->setLambda(0.1);
auto com = std::make_shared<CoM>(*model_ptr.get());
com->setLambda(0.1);
Expand All @@ -420,7 +420,7 @@ int main(int argc, char **argv)
auto joint_limits = std::make_shared<JointLimits>(*model_ptr, qmax, qmin);

double dT = 0.01;
auto vel_limits = std::make_shared<VelocityLimits>(dqlim, dT);
auto vel_limits = std::make_shared<VelocityLimits>(*model_ptr, dqlim, dT);

OpenSoT::AutoStack::Ptr stack;
std::cout<<"Creating stack "<<stack_priority<<std::endl;
Expand Down
10 changes: 5 additions & 5 deletions examples/cpp/coman_ik_hcod.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -359,11 +359,11 @@ int main(int argc, char **argv)
model_ptr->update();

using namespace OpenSoT::tasks::velocity;
auto r_sole = std::make_shared<Cartesian>("r_sole", q, *model_ptr.get(), "r_sole", "world");
auto l_sole = std::make_shared<Cartesian>("l_sole", q, *model_ptr.get(), "l_sole", "world");
auto l_wrist = std::make_shared<Cartesian>("l_wrist", q, *model_ptr.get(), "l_wrist", "world");
auto r_sole = std::make_shared<Cartesian>("r_sole", *model_ptr.get(), "r_sole", "world");
auto l_sole = std::make_shared<Cartesian>("l_sole", *model_ptr.get(), "l_sole", "world");
auto l_wrist = std::make_shared<Cartesian>("l_wrist", *model_ptr.get(), "l_wrist", "world");
l_wrist->setLambda(0.1);
auto r_wrist = std::make_shared<Cartesian>("r_wrist", q, *model_ptr.get(), TCP_frame, "world");
auto r_wrist = std::make_shared<Cartesian>("r_wrist", *model_ptr.get(), TCP_frame, "world");
r_wrist->setLambda(0.1);
auto com = std::make_shared<CoM>(*model_ptr.get());
com->setLambda(0.1);
Expand All @@ -379,7 +379,7 @@ int main(int argc, char **argv)
auto joint_limits = std::make_shared<JointLimits>(*model_ptr, qmax, qmin);

double dT = 0.01;
auto vel_limits = std::make_shared<VelocityLimits>(dqlim, dT);
auto vel_limits = std::make_shared<VelocityLimits>(*model_ptr, dqlim, dT);

OpenSoT::AutoStack::Ptr stack;
std::cout<<"Creating stack "<<stack_priority<<std::endl;
Expand Down
4 changes: 2 additions & 2 deletions examples/cpp/panda_ik.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -320,7 +320,7 @@ int main(int argc, char **argv)
model_ptr->update();

using namespace OpenSoT::tasks::velocity;
auto TCP = std::make_shared<Cartesian>("TCP", q_init, *model_ptr.get(), TCP_frame, "world");
auto TCP = std::make_shared<Cartesian>("TCP", *model_ptr.get(), TCP_frame, "world");
TCP->setLambda(0.1);

Eigen::VectorXd zeros = q_init;
Expand All @@ -336,7 +336,7 @@ int main(int argc, char **argv)
auto joint_limits = std::make_shared<JointLimits>(*model_ptr, qmax, qmin);

double dT = 0.01;
auto vel_limits = std::make_shared<VelocityLimits>(dqlim, dT);
auto vel_limits = std::make_shared<VelocityLimits>(*model_ptr, dqlim, dT);

std::list<unsigned int> position_ids = {0, 1, 2};
OpenSoT::AutoStack::Ptr stack;
Expand Down
4 changes: 2 additions & 2 deletions examples/cpp/panda_ik_hcod.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -277,7 +277,7 @@ int main(int argc, char **argv)
model_ptr->update();

using namespace OpenSoT::tasks::velocity;
auto TCP = std::make_shared<Cartesian>("TCP", q_init, *model_ptr.get(), TCP_frame, "world");
auto TCP = std::make_shared<Cartesian>("TCP", *model_ptr.get(), TCP_frame, "world");
TCP->setLambda(0.1);

Eigen::VectorXd zeros = q_init;
Expand All @@ -293,7 +293,7 @@ int main(int argc, char **argv)
auto joint_limits = std::make_shared<JointLimits>(*model_ptr, qmax, qmin);

double dT = 0.01;
auto vel_limits = std::make_shared<VelocityLimits>(dqlim, dT);
auto vel_limits = std::make_shared<VelocityLimits>(*model_ptr, dqlim, dT);

std::list<unsigned int> position_ids = {0, 1, 2};
OpenSoT::AutoStack::Ptr stack;
Expand Down
2 changes: 1 addition & 1 deletion include/OpenSoT/tasks/floating_base/Contact.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ namespace OpenSoT{
private:
std::string _link_in_contact;
XBot::ModelInterface& _robot;
Eigen::MatrixXd _J, Jrot;
Eigen::MatrixXd _J, _Jrot;
Eigen::MatrixXd _Jcontact;
Eigen::VectorXd _dqm;
Eigen::MatrixXd _contact_matrix;
Expand Down
1 change: 0 additions & 1 deletion include/OpenSoT/tasks/velocity/Cartesian.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,6 @@
* @param base_link the name of the base link as expressed in the robot urdf. Can be set to "world"
*/
Cartesian(std::string task_id,
const Eigen::VectorXd& x,
XBot::ModelInterface &robot,
const std::string& distal_link,
const std::string& base_link);
Expand Down
2 changes: 1 addition & 1 deletion include/OpenSoT/tasks/velocity/Manipulability.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ namespace OpenSoT {

_robot->syncFrom(_model);

_CartesianTask = Cartesian::Ptr(new Cartesian(CartesianTask->getTaskID(), q,
_CartesianTask = Cartesian::Ptr(new Cartesian(CartesianTask->getTaskID(),
*(_robot.get()), CartesianTask->getDistalLink(), CartesianTask->getBaseLink()));
}

Expand Down
5 changes: 3 additions & 2 deletions src/tasks/floating_base/Contact.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,9 @@ OpenSoT::tasks::floating_base::Contact::~Contact()
void OpenSoT::tasks::floating_base::Contact::_update(const Eigen::VectorXd &x)
{
_robot.getJacobian(_link_in_contact, _J);
XBot::Utils::rotate(_J, _robot.getPose(_link_in_contact).linear().transpose(), Jrot);
_Jcontact = _contact_matrix*_J;
_Jrot.resize(_J.rows(), _J.cols());
XBot::Utils::rotate(_J, _robot.getPose(_link_in_contact).linear().transpose(), _Jrot);
_Jcontact = _contact_matrix*_Jrot;
_robot.getJointVelocity(_dqm);

Eigen::Affine3d w_T_c;
Expand Down
3 changes: 1 addition & 2 deletions src/tasks/velocity/Cartesian.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@
using namespace OpenSoT::tasks::velocity;

Cartesian::Cartesian(std::string task_id,
const Eigen::VectorXd& x,
XBot::ModelInterface &robot,
const std::string& distal_link,
const std::string& base_link) :
Expand Down Expand Up @@ -52,7 +51,7 @@ Cartesian::Cartesian(std::string task_id,
assert(this->_distal_link_index != _base_link_index);

/* first update. Setting desired pose equal to the actual pose */
this->_update(x);
this->_update(Eigen::VectorXd(0));

_W.setIdentity(_A.rows(), _A.rows());

Expand Down
2 changes: 1 addition & 1 deletion src/tasks/velocity/Gaze.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ Gaze::Gaze(std::string task_id,
std::string distal_link) :
Task(task_id, robot.getNv()),
_distal_link(distal_link),
_cartesian_task(new Cartesian(task_id, x, robot, _distal_link, base_link)),
_cartesian_task(new Cartesian(task_id, robot, _distal_link, base_link)),
_subtask(new SubTask(_cartesian_task, Indices::range(4,5))),
_robot(robot), _tmp_vector(3), _bl_T_gaze_kdl(),
_gaze_goal()
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 @@ -293,10 +293,10 @@ add_test(NAME OpenSoT_task_velocity_Postural COMMAND testPosturalVelocityTask)
# 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})
# add_dependencies(testContactFloatingBaseTask OpenSoT)
# add_test(NAME OpenSoT_task_floating_base_Postural COMMAND testContactFloatingBaseTask)
ADD_EXECUTABLE(testContactFloatingBaseTask tasks/floating_base/TestContact.cpp)
TARGET_LINK_LIBRARIES(testContactFloatingBaseTask ${TestLibs})
add_dependencies(testContactFloatingBaseTask OpenSoT)
add_test(NAME OpenSoT_task_floating_base_Postural COMMAND testContactFloatingBaseTask)

# ADD_EXECUTABLE(testCartesianVelocityTask tasks/velocity/TestCartesian.cpp)
# TARGET_LINK_LIBRARIES(testCartesianVelocityTask ${TestLibs})
Expand Down
119 changes: 63 additions & 56 deletions tests/tasks/floating_base/TestContact.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,49 +10,48 @@
#include <xbot2_interface/xbotinterface2.h>
#include <OpenSoT/constraints/GenericConstraint.h>
#include <OpenSoT/utils/Affine.h>
#include "../../common.h"


using namespace OpenSoT::tasks::velocity;
using namespace OpenSoT::constraints::velocity;

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

namespace name {

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

testPosturalTask()
testPosturalTask() : TestBase("coman_floating_base")
{
dT = 0.002;

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.0;
l_sole_T_Waist.translation()[1] = 0.0;

this->setWorld(l_sole_T_Waist, q);

l_sole.reset(new Cartesian("l_sole", q, *model, "l_sole", "world"));
r_sole.reset(new Cartesian("r_sole", q, *model, "r_sole", "world"));
com.reset(new CoM(q, *model));
postural.reset(new Postural(q));
vel_lims.reset(new VelocityLimits(M_PI, dT, q.size()));
l_sole.reset(new Cartesian("l_sole", *_model_ptr, "l_sole", "world"));
r_sole.reset(new Cartesian("r_sole", *_model_ptr, "r_sole", "world"));
com.reset(new CoM(*_model_ptr));
postural.reset(new Postural(*_model_ptr));
vel_lims.reset(new VelocityLimits(*_model_ptr, M_PI, dT));

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

ik_solver.reset(
new OpenSoT::solvers::iHQP(autostack->getStack(), autostack->getBounds(), 1e6));
Expand All @@ -72,32 +71,31 @@ class testPosturalTask: 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->getDofIndex("RHipSag")] = -25.0*M_PI/180.0;
q[_model_ptr->getDofIndex("RKneeSag")] = 50.0*M_PI/180.0;
q[_model_ptr->getDofIndex("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->getDofIndex("LHipSag")] = -25.0*M_PI/180.0;
q[_model_ptr->getDofIndex("LKneeSag")] = 50.0*M_PI/180.0;
q[_model_ptr->getDofIndex("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->getDofIndex("LShSag")] = 20.0*M_PI/180.0;
q[_model_ptr->getDofIndex("LShLat")] = 20.0*M_PI/180.0;
q[_model_ptr->getDofIndex("LShYaw")] = -15.0*M_PI/180.0;
q[_model_ptr->getDofIndex("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->getDofIndex("RShSag")] = 20.0*M_PI/180.0;
q[_model_ptr->getDofIndex("RShLat")] = -20.0*M_PI/180.0;
q[_model_ptr->getDofIndex("RShYaw")] = 15.0*M_PI/180.0;
q[_model_ptr->getDofIndex("RElbj")] = -80.0*M_PI/180.0;

}

Expand All @@ -108,7 +106,6 @@ class testPosturalTask: public ::testing::Test
OpenSoT::AutoStack::Ptr autostack;
OpenSoT::solvers::iHQP::Ptr ik_solver;

XBot::ModelInterface::Ptr model;

Eigen::VectorXd q, dq;

Expand All @@ -121,7 +118,7 @@ TEST_F(testPosturalTask, floating_base_open_loop)
Eigen::VectorXd qm = this->q;
Eigen::VectorXd dqm = this->dq;

XBot::ModelInterface::Ptr robot = XBot::ModelInterface::getModel(_path_to_cfg);
auto robot = GetTestModel("coman_floating_base");
robot->setJointPosition(qm);
robot->setJointVelocity(dqm/this->dT);
robot->update();
Expand All @@ -146,17 +143,17 @@ TEST_F(testPosturalTask, floating_base_open_loop)
OpenSoT::AutoStack::Ptr autostack_fb(
new OpenSoT::AutoStack((l_sole_fb + r_sole_fb)));
autostack_fb<<fb_lims;
autostack_fb->update(qm);
autostack_fb->update(Eigen::VectorXd(0));

OpenSoT::solvers::iHQP::Ptr solver_fb(
new OpenSoT::solvers::iHQP(autostack_fb->getStack(), autostack_fb->getBounds(), 0.));
new OpenSoT::solvers::iHQP(autostack_fb->getStack(), autostack_fb->getBounds(), 1e6));

Eigen::Vector3d com_ref = com->getActualPosition();
com_ref[2] -= 0.1;
com->setReference(com_ref);

Eigen::VectorXd dQ(6); dQ.setZero(6);
Eigen::VectorXd Q = qm.segment(0,6);
Eigen::VectorXd Q = qm.segment(0,7);
std::cout<<"Q: "<<Q.transpose()<<std::endl;
for(unsigned int i = 0; i < 1000; ++i)
{
Expand All @@ -166,34 +163,44 @@ TEST_F(testPosturalTask, floating_base_open_loop)
robot->setJointVelocity(dqm/dT);
robot->update();

autostack_fb->update(qm);
autostack_fb->update(Eigen::VectorXd(0));
if(!solver_fb->solve(dQ)){
std::cout<<"solver ik can not solve!"<<std::endl;
dQ.setZero(dq.size());}
Q += dQ*dT;

model->setJointPosition(q);
model->setJointVelocity(dq/dT);
model->update();

autostack->update(q);
Eigen::VectorXd Qfull(q.size()); Qfull.setZero();
Eigen::VectorXd dQfull(dq.size()); dQfull.setZero();
Qfull.segment(0,7) = Q;
dQfull.segment(0,6) = dQ;

Qfull = robot->sum(Qfull, dQfull*dT);

Q = Qfull.segment(0,7);
dQ = dQfull.segment(0,6);

_model_ptr->setJointPosition(q);
_model_ptr->setJointVelocity(dq/dT);
_model_ptr->update();

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

if(!ik_solver->solve(dq)){
std::cout<<"solver ik can not solve!"<<std::endl;
dq.setZero(dq.size());}
q += dq;
q = _model_ptr->sum(q, dq);
}

for(unsigned int i = 0 ; i < 3; ++i)
EXPECT_NEAR(com->getActualPosition()[i], com_ref[i], 1e-6);
EXPECT_NEAR(com->getActualPosition()[i], com_ref[i], 1e-4);

std::cout<<"q final fb model: \n"<<q.segment(0,6).transpose()<<std::endl;
std::cout<<"q final fb model: \n"<<q.segment(0,7).transpose()<<std::endl;
std::cout<<"Q final fb robot: \n"<<Q.transpose()<<std::endl;

std::cout<<"error: \n"<<(q.segment(0,6) - Q).array().abs()<<std::endl;
std::cout<<"error: \n"<<(q.segment(0,7) - Q).array().abs()<<std::endl;

for(unsigned int i = 0; i < 6; ++i)
EXPECT_NEAR(Q[i], q.segment(0,6)[i], 1e-3);
for(unsigned int i = 0; i < 7; ++i)
EXPECT_NEAR(Q[i], q.segment(0,7)[i], 1e-3);
}

}
Expand Down

0 comments on commit b7231b7

Please sign in to comment.