formulation.hpp

Go to the documentation of this file.

//
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#ifndef __tsid_python_HQPOutput_hpp__
#define __tsid_python_HQPOutput_hpp__

#include "tsid/bindings/python/fwd.hpp"

#include <pinocchio/bindings/python/utils/deprecation.hpp>

#include "tsid/formulations/inverse-dynamics-formulation-acc-force.hpp"
#include "tsid/bindings/python/solvers/HQPData.hpp"
#include "tsid/contacts/contact-6d.hpp"
#include "tsid/contacts/contact-point.hpp"
#include "tsid/tasks/task-joint-posture.hpp"
#include "tsid/tasks/task-se3-equality.hpp"
#include "tsid/tasks/task-com-equality.hpp"
#include "tsid/tasks/task-cop-equality.hpp"
#include "tsid/tasks/task-actuation-bounds.hpp"
#include "tsid/tasks/task-joint-bounds.hpp"
#include "tsid/tasks/task-joint-posVelAcc-bounds.hpp"
#include "tsid/tasks/task-angular-momentum-equality.hpp"

namespace tsid {
namespace python {
namespace bp = boost::python;

template <typename T>
struct InvDynPythonVisitor
    : public boost::python::def_visitor<InvDynPythonVisitor<T> > {
  template <class PyClass>

  void visit(PyClass& cl) const {
    cl.def(bp::init<std::string, robots::RobotWrapper&, bool>(
               (bp::args("name", "robot", "verbose"))))
        .def("data", &InvDynPythonVisitor::data)
        .add_property("nVar", &T::nVar)
        .add_property("nEq", &T::nEq)
        .add_property("nIn", &T::nIn)

        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_SE3,
             bp::args("task", "weight", "priorityLevel", "transition duration"))
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_COM,
             bp::args("task", "weight", "priorityLevel", "transition duration"))
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint,
             bp::args("task", "weight", "priorityLevel", "transition duration"))
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds,
             bp::args("task", "weight", "priorityLevel", "transition duration"))
        .def("addMotionTask",
             &InvDynPythonVisitor::addMotionTask_JointPosVelAccBounds,
             bp::args("task", "weight", "priorityLevel", "transition duration"))
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM,
             bp::args("task", "weight", "priorityLevel", "transition duration"))
        .def("addForceTask", &InvDynPythonVisitor::addForceTask_COP,
             bp::args("task", "weight", "priorityLevel", "transition duration"))
        .def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds,
             bp::args("task", "weight", "priorityLevel", "transition duration"))
        .def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight,
             bp::args("task_name", "weight"))
        .def("updateRigidContactWeights",
             &InvDynPythonVisitor::updateRigidContactWeights,
             bp::args("contact_name", "force_regularization_weight"))
        .def("updateRigidContactWeights",
             &InvDynPythonVisitor::updateRigidContactWeightsWithMotionWeight,
             bp::args("contact_name", "force_regularization_weight",
                      "motion_weight"))
        .def("addRigidContact",
             &InvDynPythonVisitor::addRigidContact6dDeprecated,
             bp::args("contact"),
             pinocchio::python::deprecated_function<>(
                 "Method addRigidContact(ContactBase) is deprecated. You "
                 "should use addRigidContact(ContactBase, double) instead"))
        .def("addRigidContact", &InvDynPythonVisitor::addRigidContact6d,
             bp::args("contact", "force_reg_weight"))
        .def("addRigidContact",
             &InvDynPythonVisitor::addRigidContact6dWithPriorityLevel,
             bp::args("contact", "force_reg_weight", "motion_weight",
                      "priority_level"))
        .def("addRigidContact", &InvDynPythonVisitor::addRigidContactPoint,
             bp::args("contact", "force_reg_weight"))
        .def("addRigidContact",
             &InvDynPythonVisitor::addRigidContactPointWithPriorityLevel,
             bp::args("contact", "force_reg_weight", "motion_weight",
                      "priority_level"))
        .def("removeTask", &InvDynPythonVisitor::removeTask,
             bp::args("task_name", "duration"))
        .def("removeRigidContact", &InvDynPythonVisitor::removeRigidContact,
             bp::args("contact_name", "duration"))
        .def("removeFromHqpData", &InvDynPythonVisitor::removeFromHqpData,
             bp::args("constraint_name"))
        .def("computeProblemData", &InvDynPythonVisitor::computeProblemData,
             bp::args("time", "q", "v"))

        .def("getActuatorForces", &InvDynPythonVisitor::getActuatorForces,
             bp::args("HQPOutput"))
        .def("getAccelerations", &InvDynPythonVisitor::getAccelerations,
             bp::args("HQPOutput"))
        .def("getContactForces", &InvDynPythonVisitor::getContactForces,
             bp::args("HQPOutput"))
        .def("checkContact", &InvDynPythonVisitor::checkContact,
             bp::args("name", "HQPOutput"))
        .def("getContactForce", &InvDynPythonVisitor::getContactForce,
             bp::args("name", "HQPOutput"));
  }
  static pinocchio::Data data(T& self) {
    pinocchio::Data data = self.data();
    return data;
  }
  static bool addMotionTask_SE3(T& self, tasks::TaskSE3Equality& task,
                                double weight, unsigned int priorityLevel,
                                double transition_duration) {
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
  }
  static bool addMotionTask_COM(T& self, tasks::TaskComEquality& task,
                                double weight, unsigned int priorityLevel,
                                double transition_duration) {
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
  }
  static bool addMotionTask_Joint(T& self, tasks::TaskJointPosture& task,
                                  double weight, unsigned int priorityLevel,
                                  double transition_duration) {
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
  }
  static bool addMotionTask_JointBounds(T& self, tasks::TaskJointBounds& task,
                                        double weight,
                                        unsigned int priorityLevel,
                                        double transition_duration) {
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
  }
  static bool addMotionTask_JointPosVelAccBounds(
      T& self, tasks::TaskJointPosVelAccBounds& task, double weight,
      unsigned int priorityLevel, double transition_duration) {
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
  }
  static bool addMotionTask_AM(T& self, tasks::TaskAMEquality& task,
                               double weight, unsigned int priorityLevel,
                               double transition_duration) {
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
  }
  static bool addForceTask_COP(T& self, tasks::TaskCopEquality& task,
                               double weight, unsigned int priorityLevel,
                               double transition_duration) {
    return self.addForceTask(task, weight, priorityLevel, transition_duration);
  }
  static bool addActuationTask_Bounds(T& self, tasks::TaskActuationBounds& task,
                                      double weight, unsigned int priorityLevel,
                                      double transition_duration) {
    return self.addActuationTask(task, weight, priorityLevel,
                                 transition_duration);
  }
  static bool updateTaskWeight(T& self, const std::string& task_name,
                               double weight) {
    return self.updateTaskWeight(task_name, weight);
  }
  static bool updateRigidContactWeights(T& self,
                                        const std::string& contact_name,
                                        double force_regularization_weight) {
    return self.updateRigidContactWeights(contact_name,
                                          force_regularization_weight);
  }
  static bool updateRigidContactWeightsWithMotionWeight(
      T& self, const std::string& contact_name,
      double force_regularization_weight, double motion_weight) {
    return self.updateRigidContactWeights(
        contact_name, force_regularization_weight, motion_weight);
  }
  static bool addRigidContact6dDeprecated(T& self,
                                          contacts::Contact6d& contact) {
    return self.addRigidContact(contact, 1e-5);
  }
  static bool addRigidContact6d(T& self, contacts::Contact6d& contact,
                                double force_regularization_weight) {
    return self.addRigidContact(contact, force_regularization_weight);
  }
  static bool addRigidContact6dWithPriorityLevel(
      T& self, contacts::Contact6d& contact, double force_regularization_weight,
      double motion_weight, const bool priority_level) {
    return self.addRigidContact(contact, force_regularization_weight,
                                motion_weight, priority_level);
  }
  static bool addRigidContactPoint(T& self, contacts::ContactPoint& contact,
                                   double force_regularization_weight) {
    return self.addRigidContact(contact, force_regularization_weight);
  }
  static bool addRigidContactPointWithPriorityLevel(
      T& self, contacts::ContactPoint& contact,
      double force_regularization_weight, double motion_weight,
      const bool priority_level) {
    return self.addRigidContact(contact, force_regularization_weight,
                                motion_weight, priority_level);
  }
  static bool removeTask(T& self, const std::string& task_name,
                         double transition_duration) {
    return self.removeTask(task_name, transition_duration);
  }
  static bool removeRigidContact(T& self, const std::string& contactName,
                                 double transition_duration) {
    return self.removeRigidContact(contactName, transition_duration);
  }
  static bool removeFromHqpData(T& self, const std::string& constraintName) {
    return self.removeFromHqpData(constraintName);
  }
  static HQPDatas computeProblemData(T& self, double time,
                                     const Eigen::VectorXd& q,
                                     const Eigen::VectorXd& v) {
    HQPDatas Hqp;
    Hqp.set(self.computeProblemData(time, q, v));
    return Hqp;
  }
  static Eigen::VectorXd getActuatorForces(T& self,
                                           const solvers::HQPOutput& sol) {
    return self.getActuatorForces(sol);
  }
  static Eigen::VectorXd getAccelerations(T& self,
                                          const solvers::HQPOutput& sol) {
    return self.getAccelerations(sol);
  }
  static Eigen::VectorXd getContactForces(T& self,
                                          const solvers::HQPOutput& sol) {
    return self.getContactForces(sol);
  }
  static bool checkContact(T& self, const std::string& name,
                           const solvers::HQPOutput& sol) {
    tsid::math::Vector f = self.getContactForces(name, sol);
    if (f.size() > 0) return true;
    return false;
  }
  static Eigen::VectorXd getContactForce(T& self, const std::string& name,
                                         const solvers::HQPOutput& sol) {
    return self.getContactForces(name, sol);
  }

  static void expose(const std::string& class_name) {
    std::string doc = "InvDyn info.";
    bp::class_<T>(class_name.c_str(), doc.c_str(), bp::no_init)
        .def(InvDynPythonVisitor<T>());
  }
};
}  // namespace python
}  // namespace tsid

#endif  // ifndef __tsid_python_HQPOutput_hpp__