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: ...@@ -72,7 +72,7 @@ class TsidBiped:
postureTask = tsid.TaskJointPosture("task-posture", robot) postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * conf.gain_vector) postureTask.setKp(conf.kp_posture * conf.gain_vector)
postureTask.setKd(2.0 * np.sqrt(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) formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name) self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name)
......
...@@ -49,7 +49,7 @@ namespace tsid ...@@ -49,7 +49,7 @@ namespace tsid
.def(bp::init<std::string, robots::RobotWrapper &> ((bp::arg("name"), bp::arg("robot")), "Default Constructor")) .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("dim", &Task::dim, "return dimension size")
.add_property("mask", bp::make_function(&TaskActuationBoundsPythonVisitor::getmask, bp::return_value_policy<bp::copy_const_reference>()), "Return mask") .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("setBounds", &TaskActuationBoundsPythonVisitor::setBounds, bp::args("lower", "upper"))
.def("compute", &TaskActuationBoundsPythonVisitor::compute, bp::args("t", "q", "v", "data")) .def("compute", &TaskActuationBoundsPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskActuationBoundsPythonVisitor::getConstraint) .def("getConstraint", &TaskActuationBoundsPythonVisitor::getConstraint)
......
...@@ -64,6 +64,8 @@ namespace tsid ...@@ -64,6 +64,8 @@ namespace tsid
.def("compute", &TaskCOMEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data")) .def("compute", &TaskCOMEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskCOMEqualityPythonVisitor::getConstraint) .def("getConstraint", &TaskCOMEqualityPythonVisitor::getConstraint)
.add_property("name", &TaskCOMEqualityPythonVisitor::name) .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){ static std::string name(TaskCOM & self){
...@@ -118,6 +120,12 @@ namespace tsid ...@@ -118,6 +120,12 @@ namespace tsid
static void setKd (TaskCOM & self, const::Eigen::VectorXd Kv){ static void setKd (TaskCOM & self, const::Eigen::VectorXd Kv){
return self.Kd(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) static void expose(const std::string & class_name)
{ {
std::string doc = "TaskCOMEqualityPythonVisitor info."; std::string doc = "TaskCOMEqualityPythonVisitor info.";
......
...@@ -50,8 +50,8 @@ namespace tsid ...@@ -50,8 +50,8 @@ namespace tsid
.add_property("dim", &TaskJoint::dim, "return dimension size") .add_property("dim", &TaskJoint::dim, "return dimension size")
.def("setReference", &TaskJointPosturePythonVisitor::setReference, bp::arg("ref")) .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("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") .add_property("mask", bp::make_function(&TaskJointPosturePythonVisitor::getmask, bp::return_value_policy<bp::copy_const_reference>()), "Return mask")
.def("mask", &TaskJointPosturePythonVisitor::setmask, bp::arg("mask")) .def("setMask", &TaskJointPosturePythonVisitor::setmask, bp::arg("mask"))
.def("getAcceleration", &TaskJointPosturePythonVisitor::getAcceleration, bp::arg("dv")) .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("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>())) .add_property("velocity_error", bp::make_function(&TaskJointPosturePythonVisitor::velocity_error, bp::return_value_policy<bp::copy_const_reference>()))
...@@ -88,10 +88,10 @@ namespace tsid ...@@ -88,10 +88,10 @@ namespace tsid
return self.getDesiredAcceleration(); return self.getDesiredAcceleration();
} }
static const Eigen::VectorXd & getmask(const TaskJoint & self){ static const Eigen::VectorXd & getmask(const TaskJoint & self){
return self.mask(); return self.getMask();
} }
static void setmask (TaskJoint & self, const Eigen::VectorXd mask){ 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){ static Eigen::VectorXd getAcceleration (TaskJoint & self, const Eigen::VectorXd dv){
return self.getAcceleration(dv); return self.getAcceleration(dv);
......
...@@ -62,6 +62,7 @@ namespace tsid ...@@ -62,6 +62,7 @@ namespace tsid
.def("setKp", &TaskSE3EqualityPythonVisitor::setKp, bp::arg("Kp")) .def("setKp", &TaskSE3EqualityPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &TaskSE3EqualityPythonVisitor::setKd, bp::arg("Kd")) .def("setKd", &TaskSE3EqualityPythonVisitor::setKd, bp::arg("Kd"))
.def("useLocalFrame", &TaskSE3EqualityPythonVisitor::useLocalFrame, bp::arg("local_frame")) .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("setMask", &TaskSE3EqualityPythonVisitor::setMask, bp::arg("mask"))
.def("compute", &TaskSE3EqualityPythonVisitor::compute, bp::args("t", "q", "v", "data")) .def("compute", &TaskSE3EqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskSE3EqualityPythonVisitor::getConstraint) .def("getConstraint", &TaskSE3EqualityPythonVisitor::getConstraint)
...@@ -124,6 +125,9 @@ namespace tsid ...@@ -124,6 +125,9 @@ namespace tsid
static void useLocalFrame (TaskSE3 & self, const bool local_frame) { static void useLocalFrame (TaskSE3 & self, const bool local_frame) {
self.useLocalFrame(local_frame); self.useLocalFrame(local_frame);
} }
static void getMask (TaskSE3 & self) {
self.getMask();
}
static void setMask (TaskSE3 & self, const::Eigen::VectorXd mask) { static void setMask (TaskSE3 & self, const::Eigen::VectorXd mask) {
self.setMask(mask); self.setMask(mask);
} }
......
...@@ -56,14 +56,12 @@ namespace tsid ...@@ -56,14 +56,12 @@ namespace tsid
const Vector & getVelocityLowerBounds() const; const Vector & getVelocityLowerBounds() const;
const Vector & getVelocityUpperBounds() const; const Vector & getVelocityUpperBounds() const;
// const Vector & mask() const; virtual void setMask(math::ConstRefVector mask);
// void mask(const Vector & mask);
protected: protected:
Vector m_v_lb, m_v_ub; Vector m_v_lb, m_v_ub;
Vector m_a_lb, m_a_ub; Vector m_a_lb, m_a_ub;
Vector m_ddq_max_due_to_vel, m_ddq_min_due_to_vel; Vector m_ddq_max_due_to_vel, m_ddq_min_due_to_vel;
// Vector m_mask;
ConstraintBound m_constraint; ConstraintBound m_constraint;
double m_dt; double m_dt;
int m_nv, m_na; int m_nv, m_na;
......
...@@ -21,6 +21,7 @@ ...@@ -21,6 +21,7 @@
#include <tsid/tasks/task-motion.hpp> #include <tsid/tasks/task-motion.hpp>
#include <tsid/math/constraint-bound.hpp> #include <tsid/math/constraint-bound.hpp>
#include <tsid/math/constraint-inequality.hpp> #include <tsid/math/constraint-inequality.hpp>
#include <tsid/deprecation.hpp>
/** This class has been implemented following : /** This class has been implemented following :
* Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time * Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time
...@@ -106,8 +107,9 @@ namespace tsid ...@@ -106,8 +107,9 @@ namespace tsid
*/ */
void computeAccLimits(ConstRefVector q,ConstRefVector dq,bool verbose=true); void computeAccLimits(ConstRefVector q,ConstRefVector dq,bool verbose=true);
const Vector & mask() const; DEPRECATED const Vector & mask() const; // deprecated
void mask(const Vector & mask); DEPRECATED void mask(const Vector & mask); // deprecated
virtual void setMask(math::ConstRefVector mask);
protected: protected:
......
...@@ -21,6 +21,7 @@ ...@@ -21,6 +21,7 @@
#include <tsid/tasks/task-motion.hpp> #include <tsid/tasks/task-motion.hpp>
#include <tsid/trajectories/trajectory-base.hpp> #include <tsid/trajectories/trajectory-base.hpp>
#include <tsid/math/constraint-equality.hpp> #include <tsid/math/constraint-equality.hpp>
#include <tsid/deprecation.hpp>
namespace tsid namespace tsid
{ {
...@@ -57,8 +58,9 @@ namespace tsid ...@@ -57,8 +58,9 @@ namespace tsid
const Vector & getDesiredAcceleration() const; const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const; Vector getAcceleration(ConstRefVector dv) const;
const Vector & mask() const; DEPRECATED const Vector & mask() const; // deprecated
void mask(const Vector & mask); DEPRECATED void mask(const Vector & mask); // deprecated
virtual void setMask(math::ConstRefVector mask);
const Vector & position_error() const; const Vector & position_error() const;
const Vector & velocity_error() const; const Vector & velocity_error() const;
......
...@@ -50,11 +50,12 @@ namespace tsid ...@@ -50,11 +50,12 @@ namespace tsid
virtual const Vector & velocity_ref() const; virtual const Vector & velocity_ref() const;
virtual void setMask(math::ConstRefVector mask); virtual void setMask(math::ConstRefVector mask);
virtual const Vector & getMask() const;
virtual bool hasMask(); virtual bool hasMask();
protected: protected:
math::Vector m_mask; Vector m_mask;
math::Vector m_dummy; Vector m_dummy;
trajectories::TrajectorySample TrajectorySample_dummy; trajectories::TrajectorySample TrajectorySample_dummy;
}; };
} }
......
...@@ -93,6 +93,11 @@ namespace tsid ...@@ -93,6 +93,11 @@ namespace tsid
return m_constraint; return m_constraint;
} }
void TaskJointBounds::setMask(ConstRefVector mask)
{
m_mask = mask;
}
const ConstraintBase & TaskJointBounds::compute(const double , const ConstraintBase & TaskJointBounds::compute(const double ,
ConstRefVector , ConstRefVector ,
ConstRefVector v, ConstRefVector v,
...@@ -106,6 +111,7 @@ namespace tsid ...@@ -106,6 +111,7 @@ namespace tsid
int offset = m_nv-m_na; int offset = m_nv-m_na;
for(int i=0; i<m_na; i++) 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.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)); m_constraint.lowerBound()(offset+i) = std::max(m_ddq_min_due_to_vel(i), m_a_lb(i));
} }
......
...@@ -118,6 +118,12 @@ namespace tsid ...@@ -118,6 +118,12 @@ namespace tsid
} }
void TaskJointPosVelAccBounds::mask(const Vector & m) 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()); assert(m.size()==m_robot.na());
m_mask = m; m_mask = m;
......
...@@ -44,6 +44,12 @@ namespace tsid ...@@ -44,6 +44,12 @@ namespace tsid
} }
void TaskJointPosture::mask(const Vector & m) 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()); assert(m.size()==m_robot.na());
m_mask = m; m_mask = m;
......
...@@ -40,6 +40,8 @@ namespace tsid ...@@ -40,6 +40,8 @@ namespace tsid
return m_mask.size() > 0; return m_mask.size() > 0;
} }
const Vector & TaskMotion::getMask() const { return m_mask; }
const TrajectorySample & TaskMotion::getReference() const { return TrajectorySample_dummy; } const TrajectorySample & TaskMotion::getReference() const { return TrajectorySample_dummy; }
const Vector & TaskMotion::getDesiredAcceleration() const { return m_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