Commit 6a460d28 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[C++] Standardize names of methods to get and set mask in motion tasks. Add...

[C++] Standardize names of methods to get and set mask in motion tasks. Add python bindings for setting/getting masks in TaskCOM and TaskSE3Equality.
parent 5cd270b3
Pipeline #12648 passed with stage
in 33 minutes and 14 seconds
......@@ -72,7 +72,7 @@ class TsidBiped:
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * conf.gain_vector)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture * conf.gain_vector))
postureTask.mask(conf.masks_posture)
postureTask.setMask(conf.masks_posture)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name)
......
......@@ -49,7 +49,7 @@ namespace tsid
.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("setMask", &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)
......
......@@ -64,6 +64,8 @@ namespace tsid
.def("compute", &TaskCOMEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskCOMEqualityPythonVisitor::getConstraint)
.add_property("name", &TaskCOMEqualityPythonVisitor::name)
.add_property("mask", bp::make_function(&TaskCOMEqualityPythonVisitor::getmask, bp::return_value_policy<bp::copy_const_reference>()), "Return mask")
.def("setMask", &TaskCOMEqualityPythonVisitor::setmask, bp::arg("mask"))
;
}
static std::string name(TaskCOM & self){
......@@ -118,6 +120,12 @@ namespace tsid
static void setKd (TaskCOM & self, const::Eigen::VectorXd Kv){
return self.Kd(Kv);
}
static const Eigen::VectorXd & getmask(const TaskCOM & self){
return self.getMask();
}
static void setmask (TaskCOM & self, const Eigen::VectorXd mask){
return self.setMask(mask);
}
static void expose(const std::string & class_name)
{
std::string doc = "TaskCOMEqualityPythonVisitor info.";
......
......@@ -50,8 +50,8 @@ namespace tsid
.add_property("dim", &TaskJoint::dim, "return dimension size")
.def("setReference", &TaskJointPosturePythonVisitor::setReference, bp::arg("ref"))
.add_property("getDesiredAcceleration", bp::make_function(&TaskJointPosturePythonVisitor::getDesiredAcceleration, bp::return_value_policy<bp::copy_const_reference>()), "Return Acc_desired")
.add_property("mask", bp::make_function(&TaskJointPosturePythonVisitor::getmask, bp::return_value_policy<bp::copy_const_reference>()), "Return Acc_desired")
.def("mask", &TaskJointPosturePythonVisitor::setmask, bp::arg("mask"))
.add_property("mask", bp::make_function(&TaskJointPosturePythonVisitor::getmask, bp::return_value_policy<bp::copy_const_reference>()), "Return mask")
.def("setMask", &TaskJointPosturePythonVisitor::setmask, bp::arg("mask"))
.def("getAcceleration", &TaskJointPosturePythonVisitor::getAcceleration, bp::arg("dv"))
.add_property("position_error", bp::make_function(&TaskJointPosturePythonVisitor::position_error, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("velocity_error", bp::make_function(&TaskJointPosturePythonVisitor::velocity_error, bp::return_value_policy<bp::copy_const_reference>()))
......@@ -88,10 +88,10 @@ namespace tsid
return self.getDesiredAcceleration();
}
static const Eigen::VectorXd & getmask(const TaskJoint & self){
return self.mask();
return self.getMask();
}
static void setmask (TaskJoint & self, const Eigen::VectorXd mask){
return self.mask(mask);
return self.setMask(mask);
}
static Eigen::VectorXd getAcceleration (TaskJoint & self, const Eigen::VectorXd dv){
return self.getAcceleration(dv);
......
......@@ -62,6 +62,7 @@ namespace tsid
.def("setKp", &TaskSE3EqualityPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &TaskSE3EqualityPythonVisitor::setKd, bp::arg("Kd"))
.def("useLocalFrame", &TaskSE3EqualityPythonVisitor::useLocalFrame, bp::arg("local_frame"))
.add_property("mask", bp::make_function(&TaskSE3EqualityPythonVisitor::getMask, bp::return_value_policy<bp::copy_const_reference>()), "Return mask")
.def("setMask", &TaskSE3EqualityPythonVisitor::setMask, bp::arg("mask"))
.def("compute", &TaskSE3EqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskSE3EqualityPythonVisitor::getConstraint)
......@@ -124,6 +125,9 @@ namespace tsid
static void useLocalFrame (TaskSE3 & self, const bool local_frame) {
self.useLocalFrame(local_frame);
}
static void getMask (TaskSE3 & self) {
self.getMask();
}
static void setMask (TaskSE3 & self, const::Eigen::VectorXd mask) {
self.setMask(mask);
}
......
......@@ -56,14 +56,12 @@ namespace tsid
const Vector & getVelocityLowerBounds() const;
const Vector & getVelocityUpperBounds() const;
// const Vector & mask() const;
// void mask(const Vector & mask);
virtual void setMask(math::ConstRefVector mask);
protected:
Vector m_v_lb, m_v_ub;
Vector m_a_lb, m_a_ub;
Vector m_ddq_max_due_to_vel, m_ddq_min_due_to_vel;
// Vector m_mask;
ConstraintBound m_constraint;
double m_dt;
int m_nv, m_na;
......
......@@ -21,6 +21,7 @@
#include <tsid/tasks/task-motion.hpp>
#include <tsid/math/constraint-bound.hpp>
#include <tsid/math/constraint-inequality.hpp>
#include <tsid/deprecation.hpp>
/** This class has been implemented following :
* Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time
......@@ -106,8 +107,9 @@ namespace tsid
*/
void computeAccLimits(ConstRefVector q,ConstRefVector dq,bool verbose=true);
const Vector & mask() const;
void mask(const Vector & mask);
DEPRECATED const Vector & mask() const; // deprecated
DEPRECATED void mask(const Vector & mask); // deprecated
virtual void setMask(math::ConstRefVector mask);
protected:
......
......@@ -21,6 +21,7 @@
#include <tsid/tasks/task-motion.hpp>
#include <tsid/trajectories/trajectory-base.hpp>
#include <tsid/math/constraint-equality.hpp>
#include <tsid/deprecation.hpp>
namespace tsid
{
......@@ -57,8 +58,9 @@ namespace tsid
const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const;
const Vector & mask() const;
void mask(const Vector & mask);
DEPRECATED const Vector & mask() const; // deprecated
DEPRECATED void mask(const Vector & mask); // deprecated
virtual void setMask(math::ConstRefVector mask);
const Vector & position_error() const;
const Vector & velocity_error() const;
......
......@@ -50,11 +50,12 @@ namespace tsid
virtual const Vector & velocity_ref() const;
virtual void setMask(math::ConstRefVector mask);
virtual const Vector & getMask() const;
virtual bool hasMask();
protected:
math::Vector m_mask;
math::Vector m_dummy;
Vector m_mask;
Vector m_dummy;
trajectories::TrajectorySample TrajectorySample_dummy;
};
}
......
......@@ -93,6 +93,11 @@ namespace tsid
return m_constraint;
}
void TaskJointBounds::setMask(ConstRefVector mask)
{
m_mask = mask;
}
const ConstraintBase & TaskJointBounds::compute(const double ,
ConstRefVector ,
ConstRefVector v,
......@@ -106,6 +111,7 @@ namespace tsid
int offset = m_nv-m_na;
for(int i=0; i<m_na; i++)
{
// TODO: use mask here
m_constraint.upperBound()(offset+i) = std::min(m_ddq_max_due_to_vel(i), m_a_ub(i));
m_constraint.lowerBound()(offset+i) = std::max(m_ddq_min_due_to_vel(i), m_a_lb(i));
}
......
......@@ -118,6 +118,12 @@ namespace tsid
}
void TaskJointPosVelAccBounds::mask(const Vector & m)
{
// std::cerr<<"The method TaskJointPosVelAccBounds::mask is deprecated. Use TaskJointPosVelAccBounds::setMask instead.\n";
return setMask(m);
}
void TaskJointPosVelAccBounds::setMask(ConstRefVector m)
{
assert(m.size()==m_robot.na());
m_mask = m;
......
......@@ -44,6 +44,12 @@ namespace tsid
}
void TaskJointPosture::mask(const Vector & m)
{
// std::cerr<<"The method TaskJointPosture::mask is deprecated. Use TaskJointPosture::setMask instead.\n";
return setMask(m);
}
void TaskJointPosture::setMask(ConstRefVector m)
{
assert(m.size()==m_robot.na());
m_mask = m;
......
......@@ -40,6 +40,8 @@ namespace tsid
return m_mask.size() > 0;
}
const Vector & TaskMotion::getMask() const { return m_mask; }
const TrajectorySample & TaskMotion::getReference() const { return TrajectorySample_dummy; }
const Vector & TaskMotion::getDesiredAcceleration() const { return m_dummy; }
......
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