Commit c5d08cee authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add task for actuation bounds

parent b51917a2
......@@ -21,7 +21,7 @@
#include "tsid/bindings/python/tasks/task-com-equality.hpp"
#include "tsid/bindings/python/tasks/task-se3-equality.hpp"
#include "tsid/bindings/python/tasks/task-joint-posture.hpp"
#include "tsid/bindings/python/tasks/task-actuation-bounds.hpp"
namespace tsid
......@@ -31,12 +31,14 @@ namespace tsid
void exposeTaskComEquality();
void exposeTaskSE3Equality();
void exposeTaskJointPosture();
void exposeTaskActuationBounds();
inline void exposeTasks()
{
exposeTaskComEquality();
exposeTaskSE3Equality();
exposeTaskJointPosture();
exposeTaskActuationBounds();
}
} // namespace python
......
//
// Copyright (c) 2018 CNRS
//
// 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/>.
//
#include "tsid/bindings/python/tasks/task-actuation-bounds.hpp"
#include "tsid/bindings/python/tasks/expose-tasks.hpp"
namespace tsid
{
namespace python
{
void exposeTaskActuationBounds()
{
TaskActuationBoundsPythonVisitor<tsid::tasks::TaskActuationBounds>::expose("TaskActuationBounds");
}
}
}
//
// Copyright (c) 2018 CNRS
//
// 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_task_actuation_bounds_hpp__
#define __tsid_python_task_actuation_bounds_hpp__
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "tsid/tasks/task-actuation-bounds.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/math/constraint-inequality.hpp"
#include "tsid/math/constraint-base.hpp"
namespace tsid
{
namespace python
{
namespace bp = boost::python;
template<typename Task>
struct TaskActuationBoundsPythonVisitor
: public boost::python::def_visitor< TaskActuationBoundsPythonVisitor<Task> >
{
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string, robots::RobotWrapper &> ((bp::arg("name"), bp::arg("robot")), "Default Constructor"))
.add_property("dim", &Task::dim, "return dimension size")
.add_property("mask", bp::make_function(&TaskActuationBoundsPythonVisitor::getmask, bp::return_value_policy<bp::copy_const_reference>()), "Return mask")
.def("mask", &TaskActuationBoundsPythonVisitor::setmask, bp::arg("mask"))
.def("setBounds", &TaskActuationBoundsPythonVisitor::setBounds, bp::args("lower", "upper"))
.def("compute", &TaskActuationBoundsPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskActuationBoundsPythonVisitor::getConstraint)
.add_property("getLowerBounds", bp::make_function(&TaskActuationBoundsPythonVisitor::getLowerBounds, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("getUpperBounds", bp::make_function(&TaskActuationBoundsPythonVisitor::getUpperBounds, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("name", &TaskActuationBoundsPythonVisitor::name)
;
}
static std::string name(Task & self){
std::string name = self.name();
return name;
}
static math::ConstraintInequality compute(Task & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.compute(t, q, v, data);
math::ConstraintInequality cons(self.getConstraint().name(), self.getConstraint().matrix(),
self.getConstraint().lowerBound(), self.getConstraint().upperBound());
return cons;
}
static math::ConstraintInequality getConstraint(const Task & self){
math::ConstraintInequality cons(self.getConstraint().name(), self.getConstraint().matrix(),
self.getConstraint().lowerBound(), self.getConstraint().upperBound());
return cons;
}
static const Eigen::VectorXd & getmask(const Task & self){
return self.mask();
}
static void setmask (Task & self, const Eigen::VectorXd mask){
return self.mask(mask);
}
static const Eigen::VectorXd & getLowerBounds (const Task & self){
return self.getLowerBounds();
}
static const Eigen::VectorXd & getUpperBounds (const Task & self){
return self.getUpperBounds();
}
static void setBounds (Task & self, const Eigen::VectorXd lower, const Eigen::VectorXd upper){
return self.setBounds(lower, upper);
}
static void expose(const std::string & class_name)
{
std::string doc = "Task info.";
bp::class_<Task>(class_name.c_str(),
doc.c_str(),
bp::no_init)
.def(TaskActuationBoundsPythonVisitor<Task>());
}
};
}
}
#endif // ifndef __tsid_python_task_actuation_bounds_hpp__
......@@ -15,15 +15,56 @@
// <http://www.gnu.org/licenses/>.
//
#ifndef __invdyn_tasks_fwd_hpp__
#define __invdyn_tasks_fwd_hpp__
#ifndef __invdyn_task_actuation_bounds_hpp__
#define __invdyn_task_actuation_bounds_hpp__
#include <tsid/tasks/task-actuation.hpp>
#include <tsid/trajectories/trajectory-base.hpp>
#include <tsid/math/constraint-inequality.hpp>
namespace tsid
{
namespace tasks
{
class TaskActuationBounds : public TaskActuation
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index;
typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector;
typedef math::VectorXi VectorXi;
typedef math::ConstraintInequality ConstraintInequality;
typedef pinocchio::Data Data;
TaskActuationBounds(const std::string & name,
RobotWrapper & robot);
int dim() const;
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
const Data & data);
const ConstraintBase & getConstraint() const;
void setBounds(ConstRefVector lower, ConstRefVector upper);
const Vector & getLowerBounds() const;
const Vector & getUpperBounds() const;
const Vector & mask() const;
void mask(const Vector & mask);
protected:
Vector m_mask;
VectorXi m_activeAxes;
ConstraintInequality m_constraint;
};
}
}
#endif // ifndef __invdyn_tasks_fwd_hpp__
#endif // ifndef __invdyn_task_actuation_bounds_hpp__
......@@ -15,11 +15,80 @@
// <http://www.gnu.org/licenses/>.
//
#include <tsid/tasks/task-actuation-bounds.hpp>
#include "tsid/robots/robot-wrapper.hpp"
namespace tsid
{
namespace tasks
{
using namespace math;
using namespace trajectories;
using namespace pinocchio;
TaskActuationBounds::TaskActuationBounds(const std::string & name,
RobotWrapper & robot):
TaskActuation(name, robot),
m_constraint(name, robot.na(), robot.na())
{
Vector m = Vector::Ones(robot.na());
mask(m);
}
const Vector & TaskActuationBounds::mask() const
{
return m_mask;
}
void TaskActuationBounds::mask(const Vector & m)
{
assert(m.size()==m_robot.na());
m_mask = m;
const Vector::Index dim = static_cast<Vector::Index>(m.sum());
Matrix S = Matrix::Zero(dim, m_robot.na());
m_activeAxes.resize(dim);
unsigned int j=0;
for(unsigned int i=0; i<m.size(); i++)
if(m(i)!=0.0)
{
assert(m(i)==1.0);
S(j,i) = 1.0;
m_activeAxes(j) = i;
j++;
}
m_constraint.resize((unsigned int)dim, m_robot.na());
m_constraint.setMatrix(S);
}
int TaskActuationBounds::dim() const
{
return (int)m_mask.sum();
}
const Vector & TaskActuationBounds::getLowerBounds() const { return m_constraint.lowerBound(); }
const Vector & TaskActuationBounds::getUpperBounds() const { return m_constraint.upperBound(); }
void TaskActuationBounds::setBounds(ConstRefVector lower, ConstRefVector upper)
{
assert(lower.size()==dim());
assert(upper.size()==dim());
m_constraint.setLowerBound(lower);
m_constraint.setUpperBound(upper);
}
const ConstraintBase & TaskActuationBounds::getConstraint() const
{
return m_constraint;
}
const ConstraintBase & TaskActuationBounds::compute(const double ,
ConstRefVector ,
ConstRefVector ,
const Data & )
{
return m_constraint;
}
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment