Skip to content

Commit

Permalink
Added tasks acceleration bindings
Browse files Browse the repository at this point in the history
  • Loading branch information
@EnricoMingo
EnricoMingo committed May 5, 2024
1 parent 805831e commit c2e9517
Show file tree
Hide file tree
Showing 6 changed files with 199 additions and 32 deletions.
4 changes: 4 additions & 0 deletions bindings/python/pyopensot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,10 @@ PYBIND11_MODULE(pyopensot, m) {

auto m_ta = m_t.def_submodule("acceleration");
pyAccelerationPostural(m_ta);
pyAccelerationCartesian(m_ta);
pyAccelerationAngularMomentum(m_ta);
pyAccelerationCoM(m_ta);
pyDynamicFeasibility(m_ta);

auto m_c = m.def_submodule("constraints");

Expand Down
138 changes: 137 additions & 1 deletion bindings/python/tasks/acceleration.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,10 @@
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <OpenSoT/tasks/acceleration/Postural.h>
#include <OpenSoT/tasks/acceleration/Cartesian.h>
#include <OpenSoT/tasks/acceleration/AngularMomentum.h>
#include <OpenSoT/tasks/acceleration/CoM.h>
#include <OpenSoT/tasks/acceleration/DynamicFeasibility.h>

namespace py = pybind11;
using namespace OpenSoT::tasks;
Expand All @@ -13,7 +17,6 @@ std::tuple<Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd> postural_get_refer
return std::make_tuple(qref, dqref, ddqref);
}


void pyAccelerationPostural(py::module& m) {
py::class_<acceleration::Postural, std::shared_ptr<acceleration::Postural>, OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "Postural")
.def(py::init<const XBot::ModelInterface&, AffineHelper, const std::string&>(),
Expand Down Expand Up @@ -41,3 +44,136 @@ void pyAccelerationPostural(py::module& m) {
.def("getKp", &acceleration::Postural::getKp)
.def("getKd", &acceleration::Postural::getKd);
}


std::tuple<Eigen::Affine3d, Eigen::Vector6d, Eigen::Vector6d> cartesian_get_reference_(const acceleration::Cartesian& cartesian)
{
Eigen::Affine3d pose_ref;
Eigen::Vector6d vel_ref, acc_ref;
cartesian.getReference(pose_ref, vel_ref, acc_ref);
return std::make_tuple(pose_ref, vel_ref, acc_ref);
}

Eigen::Affine3d cartesian_get_actual_pose_(const acceleration::Cartesian& cartesian)
{
Eigen::Affine3d actual_pose;
cartesian.getActualPose(actual_pose);
return actual_pose;
}

Eigen::Vector6d cartesian_get_actual_twist_(const acceleration::Cartesian& cartesian)
{
Eigen::Vector6d actual_twist;
cartesian.getActualTwist(actual_twist);
return actual_twist;
}

void pyAccelerationCartesian(py::module& m) {
py::class_<acceleration::Cartesian, std::shared_ptr<acceleration::Cartesian>, OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "Cartesian")
.def(py::init<const std::string, const XBot::ModelInterface&, const std::string&, const std::string&>())
.def(py::init<const std::string, const XBot::ModelInterface&, const std::string&, const std::string&, const AffineHelper&>())
.def("setGainType", &acceleration::Cartesian::setGainType)
.def("getGainType", &acceleration::Cartesian::getGainType)
.def("getBaseLink", &acceleration::Cartesian::getBaseLink)
.def("getDistalLink", &acceleration::Cartesian::getDistalLink)
.def("setReference", py::overload_cast<const Eigen::Affine3d&>(&acceleration::Cartesian::setReference))
.def("setReference", py::overload_cast<const Eigen::Affine3d&, const Eigen::Vector6d&>(&acceleration::Cartesian::setReference))
.def("setReference", py::overload_cast<const Eigen::Affine3d&, const Eigen::Vector6d&, const Eigen::Vector6d&>(&acceleration::Cartesian::setReference))
.def("setVirtualForce", &acceleration::Cartesian::setVirtualForce)
.def("getReference", cartesian_get_reference_)
.def("getCachedVelocityReference", &acceleration::Cartesian::getCachedVelocityReference)
.def("getCachedAccelerationReference", &acceleration::Cartesian::getCachedAccelerationReference)
.def("getCachedVirtualForceReference", &acceleration::Cartesian::getCachedVirtualForceReference)
.def("getActualPose", cartesian_get_actual_pose_)
.def("getActualTwist", cartesian_get_actual_twist_)
.def("reset", &acceleration::Cartesian::reset)
.def("setLambda", py::overload_cast<double, double>(&acceleration::Cartesian::setLambda))
.def("setLambda", py::overload_cast<double>(&acceleration::Cartesian::setLambda))
.def("getLambda2", &acceleration::Cartesian::getLambda2)
.def("setOrientationGain", &acceleration::Cartesian::setOrientationGain)
.def("getOrientationErrorGain", &acceleration::Cartesian::getOrientationErrorGain)
.def("baseLinkIsWorld", &acceleration::Cartesian::baseLinkIsWorld)
.def("setDistalLink", &acceleration::Cartesian::setDistalLink)
.def("setBaseLink", &acceleration::Cartesian::setBaseLink)
.def("setKp", &acceleration::Cartesian::setKp)
.def("setKd", &acceleration::Cartesian::setKd)
.def("setGains", &acceleration::Cartesian::setGains)
.def("getKp", &acceleration::Cartesian::getKp)
.def("getKd", &acceleration::Cartesian::getKd)
.def("getError", &acceleration::Cartesian::getError)
.def("getVelocityError", &acceleration::Cartesian::getVelocityError);
}

std::tuple<Eigen::Vector3d, Eigen::Vector3d> angular_momentum_get_reference(const acceleration::AngularMomentum& am)
{
Eigen::Vector3d l_ref, dl_ref;
am.getReference(l_ref, dl_ref);
return std::make_tuple(l_ref, dl_ref);
}

void pyAccelerationAngularMomentum(py::module& m) {
py::class_<acceleration::AngularMomentum, std::shared_ptr<acceleration::AngularMomentum>, OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "AngularMomentum")
.def(py::init<XBot::ModelInterface&, const AffineHelper&>())
.def("setReference", py::overload_cast<const Eigen::Vector3d&>(&acceleration::AngularMomentum::setReference))
.def("setReference", py::overload_cast<const Eigen::Vector3d&, const Eigen::Vector3d&>(&acceleration::AngularMomentum::setReference))
.def("setMomentumGain", &acceleration::AngularMomentum::setMomentumGain)
.def("getReference", angular_momentum_get_reference)
.def("getBaseLink", &acceleration::AngularMomentum::getBaseLink)
.def("getDistalLink", &acceleration::AngularMomentum::getDistalLink)
.def("reset", &acceleration::AngularMomentum::reset);
}

std::tuple<Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d> com_get_reference_(const acceleration::CoM& com)
{
Eigen::Vector3d pos_ref, vel_ref, acc_ref;
com.getReference(pos_ref, vel_ref, acc_ref);
return std::make_tuple(pos_ref, vel_ref, acc_ref);
}

Eigen::Vector3d com_get_actual_pos(const acceleration::CoM& com)
{
Eigen::Vector3d actual_pos;
com.getActualPose(actual_pos);
return actual_pos;
}

Eigen::Vector3d com_get_pos_error(const acceleration::CoM& com)
{
Eigen::Vector3d pos_error;
com.getPosError(pos_error);
return pos_error;
}

void pyAccelerationCoM(py::module& m) {
py::class_<acceleration::CoM, std::shared_ptr<acceleration::CoM>, OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "CoM")
.def(py::init<const XBot::ModelInterface&>())
.def(py::init<const XBot::ModelInterface&, const AffineHelper&>())
.def("getBaseLink", &acceleration::CoM::getBaseLink)
.def("getDistalLink", &acceleration::CoM::getDistalLink)
.def("setReference", py::overload_cast<const Eigen::Vector3d&>(&acceleration::CoM::setReference))
.def("setReference", py::overload_cast<const Eigen::Vector3d&, const Eigen::Vector3d&>(&acceleration::CoM::setReference))
.def("getReference", com_get_reference_)
.def("getActualPose", com_get_actual_pos)
.def("getPosError", com_get_pos_error)
.def("reset", &acceleration::CoM::reset)
.def("setLambda", py::overload_cast<double>(&acceleration::CoM::setLambda))
.def("setLambda", py::overload_cast<double, double>(&acceleration::CoM::setLambda))
.def("getLambda2", &acceleration::CoM::getLambda2)
.def("getCachedVelocityReference", &acceleration::CoM::getCachedVelocityReference)
.def("getCachedAccelerationReference", &acceleration::CoM::getCachedAccelerationReference)
.def("getKp", &acceleration::CoM::getKp)
.def("getKd", &acceleration::CoM::getKd)
.def("setKp", &acceleration::CoM::setKp)
.def("setKd", &acceleration::CoM::setKd)
.def("setGains", &acceleration::CoM::setGains);
}

void pyDynamicFeasibility(py::module& m) {
py::class_<acceleration::DynamicFeasibility, std::shared_ptr<acceleration::DynamicFeasibility>, OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "DynamicFeasibility")
.def(py::init<const std::string&, const XBot::ModelInterface&, const AffineHelper&,
const std::vector<AffineHelper>&, const std::vector<std::string>&>())
.def("enableContact", &acceleration::DynamicFeasibility::enableContact)
.def("disableContact", &acceleration::DynamicFeasibility::disableContact)
.def("getEnabledContacts", &acceleration::DynamicFeasibility::getEnabledContacts)
.def("checkTask", &acceleration::DynamicFeasibility::checkTask);
}
27 changes: 14 additions & 13 deletions include/OpenSoT/tasks/acceleration/Cartesian.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,20 +100,20 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
void setVirtualForce(const Eigen::Vector6d& virtual_force_ref);


void getReference(Eigen::Affine3d& ref);
void getReference(KDL::Frame& ref);
void getReference(Eigen::Affine3d& ref) const;
void getReference(KDL::Frame& ref) const;

void getReference(Eigen::Affine3d& desiredPose,
Eigen::Vector6d& desiredTwist);
Eigen::Vector6d& desiredTwist) const;
void getReference(KDL::Frame& desiredPose,
KDL::Twist& desiredTwist);
KDL::Twist& desiredTwist) const;

void getReference(Eigen::Affine3d& desiredPose,
Eigen::Vector6d& desiredTwist,
Eigen::Vector6d& desiredAcceleration);
Eigen::Vector6d& desiredAcceleration) const;
void getReference(KDL::Frame& desiredPose,
KDL::Twist& desiredTwist,
KDL::Twist& desiredAcceleration);
KDL::Twist& desiredAcceleration) const;

/**
* @brief getCachedVelocityReference can be used to get Velocity reference after update(), it will reset
Expand All @@ -136,19 +136,16 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
*/
const Eigen::Vector6d& getCachedVirtualForceReference() const;


void getActualPose(Eigen::Affine3d& actual);
const Eigen::Affine3d& getActualPose() const;
void getActualPose(Eigen::Affine3d& actual) const;
void getActualPose(KDL::Frame& actual);

void getActualTwist(Eigen::Vector6d& actual);
const Eigen::Vector6d& getActualTwist() const;
void getActualTwist(Eigen::Vector6d& actual) const;
void getActualTwist(KDL::Twist& actual);

bool reset();

virtual void _update();

virtual void _log(XBot::MatLogger2::Ptr logger);

void setLambda(double lambda1, double lambda2);
virtual void setLambda(double lambda);

Expand Down Expand Up @@ -282,6 +279,10 @@ namespace OpenSoT { namespace tasks { namespace acceleration {

Eigen::MatrixXd _tmpMatrixXd;
Eigen::MatrixXd _Bi;

virtual void _update();
virtual void _log(XBot::MatLogger2::Ptr logger);

};

} } }
Expand Down
17 changes: 10 additions & 7 deletions include/OpenSoT/tasks/acceleration/CoM.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,22 +81,20 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
* @brief getReference
* @param ref position reference
*/
void getReference(Eigen::Vector3d& ref);
void getReference(Eigen::Vector3d& ref) const;
void getReference(Eigen::Vector3d& pos_ref, Eigen::Vector3d& vel_ref) const;
void getReference(Eigen::Vector3d& pos_ref, Eigen::Vector3d& vel_ref, Eigen::Vector3d& acc_ref) const;

/**
* @brief getActualPose
* @param actual pose of the CoM
*/
void getActualPose(Eigen::Vector3d& actual);
void getActualPose(Eigen::Vector3d& actual) const;

void getPosError(Eigen::Vector3d& error);
void getPosError(Eigen::Vector3d& error) const;

bool reset() override;

virtual void _update();

virtual void _log(XBot::MatLogger2::Ptr logger);

/**
* @brief setLambda set position and velocity gains of the feedback errors
* @param lambda1 postion gain
Expand Down Expand Up @@ -171,6 +169,11 @@ namespace OpenSoT { namespace tasks { namespace acceleration {
*/
void resetReference();

virtual void _update();

virtual void _log(XBot::MatLogger2::Ptr logger);



};

Expand Down
26 changes: 18 additions & 8 deletions src/tasks/acceleration/Cartesian.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,33 +312,33 @@ void Cartesian::_log(XBot::MatLogger2::Ptr logger)
logger->add(getTaskID() + "_lambda2", _lambda2);
}

void Cartesian::getReference(Eigen::Affine3d& ref)
void Cartesian::getReference(Eigen::Affine3d& ref) const
{
ref = _pose_ref;
}

void Cartesian::getReference(KDL::Frame& ref)
void Cartesian::getReference(KDL::Frame& ref) const
{
tf::transformEigenToKDL(_pose_ref, ref);
}

void Cartesian::getReference(Eigen::Affine3d& desiredPose,
Eigen::Vector6d& desiredTwist)
Eigen::Vector6d& desiredTwist) const
{
desiredPose = _pose_ref;
desiredTwist = _vel_ref;
}

void Cartesian::getReference(KDL::Frame& desiredPose,
KDL::Twist& desiredTwist)
KDL::Twist& desiredTwist) const
{
tf::transformEigenToKDL(_pose_ref, desiredPose);
tf::twistEigenToKDL(_vel_ref, desiredTwist);
}

void Cartesian::getReference(Eigen::Affine3d& desiredPose,
Eigen::Vector6d& desiredTwist,
Eigen::Vector6d& desiredAcceleration)
Eigen::Vector6d& desiredAcceleration) const
{
desiredPose = _pose_ref;
desiredTwist = _vel_ref;
Expand All @@ -347,15 +347,15 @@ void Cartesian::getReference(Eigen::Affine3d& desiredPose,

void Cartesian::getReference(KDL::Frame& desiredPose,
KDL::Twist& desiredTwist,
KDL::Twist& desiredAcceleration)
KDL::Twist& desiredAcceleration) const
{
tf::transformEigenToKDL(_pose_ref, desiredPose);
tf::twistEigenToKDL(_vel_ref, desiredTwist);
tf::twistEigenToKDL(_acc_ref, desiredAcceleration);
}


void Cartesian::getActualPose(Eigen::Affine3d& actual)
void Cartesian::getActualPose(Eigen::Affine3d& actual) const
{
actual = _pose_current;
}
Expand All @@ -365,7 +365,12 @@ void Cartesian::getActualPose(KDL::Frame& actual)
tf::transformEigenToKDL(_pose_current, actual);
}

void Cartesian::getActualTwist(Eigen::Vector6d& actual)
const Eigen::Affine3d& Cartesian::getActualPose() const
{
return _pose_current;
}

void Cartesian::getActualTwist(Eigen::Vector6d& actual) const
{
actual = _vel_current;
}
Expand All @@ -375,6 +380,11 @@ void Cartesian::getActualTwist(KDL::Twist& actual)
tf::twistEigenToKDL(_vel_current, actual);
}

const Eigen::Vector6d& Cartesian::getActualTwist() const
{
return _vel_current;
}

const bool Cartesian::baseLinkIsWorld() const
{
return _base_link == world_name;
Expand Down
19 changes: 16 additions & 3 deletions src/tasks/acceleration/CoM.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,17 +179,30 @@ void OpenSoT::tasks::acceleration::CoM::_log(XBot::MatLogger2::Ptr logger)
logger->add(getTaskID() + "_lambda2", _lambda2);
}

void OpenSoT::tasks::acceleration::CoM::getReference(Eigen::Vector3d& ref)
void OpenSoT::tasks::acceleration::CoM::getReference(Eigen::Vector3d& ref) const
{
ref = _pose_ref;
}

void OpenSoT::tasks::acceleration::CoM::getActualPose(Eigen::Vector3d& actual)
void OpenSoT::tasks::acceleration::CoM::getReference(Eigen::Vector3d& pos_ref, Eigen::Vector3d& vel_ref) const
{
pos_ref = _pose_ref;
vel_ref = _vel_ref;
}

void OpenSoT::tasks::acceleration::CoM::getReference(Eigen::Vector3d& pos_ref, Eigen::Vector3d& vel_ref, Eigen::Vector3d& acc_ref) const
{
pos_ref = _pose_ref;
vel_ref = _vel_ref;
acc_ref = _acc_ref;
}

void OpenSoT::tasks::acceleration::CoM::getActualPose(Eigen::Vector3d& actual) const
{
actual = _pose_current;
}

void OpenSoT::tasks::acceleration::CoM::getPosError(Eigen::Vector3d& error)
void OpenSoT::tasks::acceleration::CoM::getPosError(Eigen::Vector3d& error) const
{
error = _pose_error;
}
Expand Down

0 comments on commit c2e9517

Please sign in to comment.