Commit e3bd2d6f authored by Julian Viereck's avatar Julian Viereck
Browse files

Adding mask for MotionTask and use it in task-se3-equality. Removes the

task-point-equality class.
parent 388a32d8
#ifndef __tsid_python_contact_6d_hpp__
#define __tsid_python_contact_6d_hpp__
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
//
// 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/contacts/contact-point.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/math/constraint-inequality.hpp"
#include "tsid/math/constraint-equality.hpp"
#include "tsid/math/constraint-base.hpp"
#include "tsid/tasks/task-point-equality.hpp"
namespace tsid
{
namespace python
{
namespace bp = boost::python;
template<typename ContactPoint>
struct ContactPointPythonVisitor
: public boost::python::def_visitor< ContactPointPythonVisitor<ContactPoint> >
{
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::VectorXd, double, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce"), bp::arg("regWeight")), "Default Constructor"))
.add_property("n_motion", &ContactPoint::n_motion, "return number of motion")
.add_property("n_force", &ContactPoint::n_force, "return number of force")
.add_property("name", &ContactPointPythonVisitor::name, "return name")
.def("computeMotionTask", &ContactPointPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
.def("computeForceTask", &ContactPointPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
.def("computeForceRegularizationTask", &ContactPointPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))
.add_property("getForceGeneratorMatrix", bp::make_function(&ContactPointPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("getForceRegularizationWeight", &ContactPoint::getForceRegularizationWeight, "return force reg weight")
.def("getNormalForce", &ContactPointPythonVisitor::getNormalForce, bp::arg("vec"))
.add_property("getMinNormalForce", &ContactPoint::getMinNormalForce)
.add_property("getMaxNormalForce", &ContactPoint::getMaxNormalForce)
.add_property("Kp", bp::make_function(&ContactPointPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kd", bp::make_function(&ContactPointPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.def("setKp", &ContactPointPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &ContactPointPythonVisitor::setKd, bp::arg("Kd"))
.def("setContactNormal", &ContactPointPythonVisitor::setContactNormal, bp::args("vec"))
.def("setFrictionCoefficient", &ContactPointPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
.def("setMinNormalForce", &ContactPointPythonVisitor::setMinNormalForce, bp::args("min_force"))
.def("setMaxNormalForce", &ContactPointPythonVisitor::setMaxNormalForce, bp::args("max_force"))
.def("setRegularizationTaskWeight", &ContactPointPythonVisitor::setRegularizationTaskWeight, bp::args("double"))
.def("setReference", &ContactPointPythonVisitor::setReference, bp::args("SE3"))
.def("setForceReference", &ContactPointPythonVisitor::setForceReference, bp::args("f_vec"))
.def("setRegularizationTaskWeightVector", &ContactPointPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
;
}
static std::string name(ContactPoint & self){
std::string name = self.name();
return name;
}
static math::ConstraintEquality computeMotionTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
self.computeMotionTask(t, q, v, data);
math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
return cons;
}
static math::ConstraintInequality computeForceTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
self.computeForceTask(t, q, v, data);
math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
return cons;
}
static math::ConstraintEquality computeForceRegularizationTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
self.computeForceRegularizationTask(t, q, v, data);
math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
return cons;
}
static const Eigen::MatrixXd & getForceGeneratorMatrix(ContactPoint & self){
return self.getForceGeneratorMatrix();
}
static const Eigen::VectorXd & Kp (ContactPoint & self){
return self.Kp();
}
static const Eigen::VectorXd & Kd (ContactPoint & self){
return self.Kd();
}
static void setKp (ContactPoint & self, const::Eigen::VectorXd Kp){
return self.Kp(Kp);
}
static void setKd (ContactPoint & self, const::Eigen::VectorXd Kd){
return self.Kd(Kd);
}
static bool setContactPoints (ContactPoint & self, const::Eigen::MatrixXd contactpoints){
return self.setContactPoints(contactpoints);
}
static bool setContactNormal (ContactPoint & self, const::Eigen::VectorXd contactNormal){
return self.setContactNormal(contactNormal);
}
static bool setFrictionCoefficient (ContactPoint & self, const double frictionCoefficient){
return self.setFrictionCoefficient(frictionCoefficient);
}
static bool setMinNormalForce (ContactPoint & self, const double minNormalForce){
return self.setMinNormalForce(minNormalForce);
}
static bool setMaxNormalForce (ContactPoint & self, const double maxNormalForce){
return self.setMaxNormalForce(maxNormalForce);
}
static bool setRegularizationTaskWeight (ContactPoint & self, const double w){
return self.setRegularizationTaskWeight(w);
}
static void setReference(ContactPoint & self, const se3::SE3 & ref){
self.setReference(ref);
}
static void setForceReference(ContactPoint & self, const::Eigen::VectorXd f_ref){
self.setForceReference(f_ref);
}
static void setRegularizationTaskWeightVector(ContactPoint & self, const::Eigen::VectorXd w){
self.setRegularizationTaskWeightVector(w);
}
static double getNormalForce(ContactPoint & self, Eigen::VectorXd f){
return self.getNormalForce(f);
}
static void expose(const std::string & class_name)
{
std::string doc = "ContactPoint info.";
bp::class_<ContactPoint>(class_name.c_str(),
doc.c_str(),
bp::no_init)
.def(ContactPointPythonVisitor<ContactPoint>());
}
};
}
}
#endif // ifndef __tsid_python_contact_hpp__
\ No newline at end of file
......@@ -20,7 +20,6 @@
#include "tsid/bindings/python/tasks/task-com-equality.hpp"
#include "tsid/bindings/python/tasks/task-se3-equality.hpp"
#include "tsid/bindings/python/tasks/task-point-equality.hpp"
#include "tsid/bindings/python/tasks/task-joint-posture.hpp"
......@@ -31,14 +30,12 @@ namespace tsid
{
void exposeTaskComEquality();
void exposeTaskSE3Equality();
void exposeTaskPointEquality();
void exposeTaskJointPosture();
inline void exposeTasks()
{
exposeTaskComEquality();
exposeTaskSE3Equality();
exposeTaskPointEquality();
exposeTaskJointPosture();
}
......
//
// 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-point-equality.hpp"
#include "tsid/bindings/python/tasks/expose-tasks.hpp"
namespace tsid
{
namespace python
{
void exposeTaskPointEquality()
{
TaskPointEqualityPythonVisitor<tsid::tasks::TaskPointEquality>::expose("TaskPointEquality");
}
}
}
//
// 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_se3_hpp__
#define __tsid_python_task_se3_hpp__
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "tsid/tasks/task-point-equality.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/trajectories/trajectory-base.hpp"
#include "tsid/math/constraint-equality.hpp"
#include "tsid/math/constraint-base.hpp"
namespace tsid
{
namespace python
{
namespace bp = boost::python;
template<typename TaskSE3>
struct TaskPointEqualityPythonVisitor
: public boost::python::def_visitor< TaskPointEqualityPythonVisitor<TaskSE3> >
{
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string, robots::RobotWrapper &, std::string> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename")), "Default Constructor"))
.add_property("dim", &TaskSE3::dim, "return dimension size")
.def("setReference", &TaskPointEqualityPythonVisitor::setReference, bp::arg("ref"))
.add_property("getDesiredAcceleration", bp::make_function(&TaskPointEqualityPythonVisitor::getDesiredAcceleration, bp::return_value_policy<bp::copy_const_reference>()), "Return Acc_desired")
.def("getAcceleration", &TaskPointEqualityPythonVisitor::getAcceleration, bp::arg("dv"))
.add_property("position_error", bp::make_function(&TaskPointEqualityPythonVisitor::position_error, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("velocity_error", bp::make_function(&TaskPointEqualityPythonVisitor::velocity_error, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("position", bp::make_function(&TaskPointEqualityPythonVisitor::position, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("velocity", bp::make_function(&TaskPointEqualityPythonVisitor::velocity, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("position_ref", bp::make_function(&TaskPointEqualityPythonVisitor::position_ref, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("velocity_ref", bp::make_function(&TaskPointEqualityPythonVisitor::velocity_ref, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kp", bp::make_function(&TaskPointEqualityPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kd", bp::make_function(&TaskPointEqualityPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.def("setKp", &TaskPointEqualityPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &TaskPointEqualityPythonVisitor::setKd, bp::arg("Kd"))
.def("compute", &TaskPointEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskPointEqualityPythonVisitor::getConstraint)
.add_property("frame_id", &TaskSE3::frame_id, "frame id return")
.add_property("name", &TaskPointEqualityPythonVisitor::name)
;
}
static std::string name(TaskSE3 & self){
std::string name = self.name();
return name;
}
static math::ConstraintEquality compute(TaskSE3 & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
self.compute(t, q, v, data);
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
return cons;
}
static math::ConstraintEquality getConstraint(const TaskSE3 & self){
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
return cons;
}
static void setReference(TaskSE3 & self, trajectories::TrajectorySample & ref){
self.setReference(ref);
}
static const Eigen::VectorXd & getDesiredAcceleration(const TaskSE3 & self){
return self.getDesiredAcceleration();
}
static Eigen::VectorXd getAcceleration (TaskSE3 & self, const Eigen::VectorXd dv){
return self.getAcceleration(dv);
}
static const Eigen::VectorXd & position_error(const TaskSE3 & self){
return self.position_error();
}
static const Eigen::VectorXd & velocity_error(const TaskSE3 & self){
return self.velocity_error();
}
static const Eigen::VectorXd & position (const TaskSE3 & self){
return self.position();
}
static const Eigen::VectorXd & velocity (const TaskSE3 & self){
return self.velocity();
}
static const Eigen::VectorXd & position_ref (const TaskSE3 & self){
return self.position_ref();
}
static const Eigen::VectorXd & velocity_ref (const TaskSE3 & self){
return self.velocity_ref();
}
static const Eigen::VectorXd & Kp (TaskSE3 & self){
return self.Kp();
}
static const Eigen::VectorXd & Kd (TaskSE3 & self){
return self.Kd();
}
static void setKp (TaskSE3 & self, const::Eigen::VectorXd Kp){
return self.Kp(Kp);
}
static void setKd (TaskSE3 & self, const::Eigen::VectorXd Kv){
return self.Kd(Kv);
}
static Eigen::VectorXd frame_id (TaskSE3 & self){
return self.frame_id();
}
static void expose(const std::string & class_name)
{
std::string doc = "TaskPoint info.";
bp::class_<TaskSE3>(class_name.c_str(),
doc.c_str(),
bp::no_init)
.def(TaskPointEqualityPythonVisitor<TaskSE3>());
// bp::register_ptr_to_python< boost::shared_ptr<math::ConstraintBase> >();
}
};
}
}
#endif // ifndef __tsid_python_task_se3_hpp__
\ No newline at end of file
......@@ -19,7 +19,7 @@
#define __invdyn_contact_point_hpp__
#include "tsid/contacts/contact-base.hpp"
#include "tsid/tasks/task-point-equality.hpp"
#include "tsid/tasks/task-se3-equality.hpp"
#include "tsid/math/constraint-inequality.hpp"
#include "tsid/math/constraint-equality.hpp"
......@@ -39,7 +39,7 @@ namespace tsid
typedef math::Vector3 Vector3;
typedef math::Vector Vector;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskPointEquality TaskPointEquality;
typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality;
typedef se3::SE3 SE3;
......@@ -107,7 +107,7 @@ namespace tsid
void updateForceRegularizationTask();
void updateForceGeneratorMatrix();
TaskPointEquality m_motionTask;
TaskSE3Equality m_motionTask;
ConstraintInequality m_forceInequality;
ConstraintEquality m_forceRegTask;
Vector3 m_contactNormal;
......
......@@ -49,6 +49,11 @@ namespace tsid
virtual const Vector & position_ref() const = 0;
virtual const Vector & velocity_ref() const = 0;
virtual void setMask(math::ConstRefVector mask);
virtual bool hasMask();
protected:
math::Vector m_mask;
};
}
}
......
//
// Copyright (c) 2017 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 __invdyn_task_point_equality_hpp__
#define __invdyn_task_point_equality_hpp__
#include "tsid/tasks/task-motion.hpp"
#include "tsid/trajectories/trajectory-base.hpp"
#include "tsid/math/constraint-equality.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
namespace tsid
{
namespace tasks
{
class TaskPointEquality : public TaskMotion
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index;
typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector;
typedef math::ConstraintEquality ConstraintEquality;
typedef se3::Data Data;
typedef se3::Data::Matrix6x Matrix6x;
typedef se3::Motion Motion;
typedef se3::SE3 SE3;
TaskPointEquality(const std::string & name,
RobotWrapper & robot,
const std::string & frameName);
int dim() const;
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
const Data & data);
const ConstraintBase & getConstraint() const;
void setReference(TrajectorySample & ref);
const TrajectorySample & getReference() const;
const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const;
const Vector & position_error() const;
const Vector & velocity_error() const;
const Vector & position() const;
const Vector & velocity() const;
const Vector & position_ref() const;
const Vector & velocity_ref() const;
const Vector & Kp() const;
const Vector & Kd() const;
void Kp(ConstRefVector Kp);
void Kd(ConstRefVector Kp);
Index frame_id() const;
protected:
std::string m_frame_name;
Index m_frame_id;
Motion m_p_error, m_v_error;
Vector m_p_error_vec, m_v_error_vec;
Vector m_p, m_v;
Vector m_p_ref, m_v_ref_vec;
Motion m_v_ref, m_a_ref;
SE3 m_M_ref, m_wMl;
Vector m_Kp;
Vector m_Kd;
Vector m_a_des;
Motion m_drift;
Matrix6x m_J;
ConstraintEquality m_constraint;
TrajectorySample m_ref;
};
}
}
#endif // ifndef __invdyn_task_se3_equality_hpp__
......@@ -37,7 +37,6 @@ SET(${LIBRARY_NAME}_TASKS_SOURCES
tasks/task-joint-posture.cpp
tasks/task-motion.cpp
tasks/task-se3-equality.cpp
tasks/task-point-equality.cpp
)
SET(${LIBRARY_NAME}_CONTACTS_SOURCES
......
......@@ -23,8 +23,17 @@ namespace tsid
{
TaskMotion::TaskMotion(const std::string & name,
RobotWrapper & robot):
TaskBase(name, robot)
TaskBase(name, robot), m_mask(0)
{}
void TaskMotion::setMask(math::ConstRefVector mask)
{
m_mask = mask;
}
bool TaskMotion::hasMask()
{
return m_mask.size() > 0;
}
}
}
//
// Copyright (c) 2017 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/math/utils.hpp"
#include "tsid/tasks/task-point-equality.hpp"
#include "tsid/robots/robot-wrapper.hpp"
namespace tsid
{
namespace tasks
{
using namespace math;
using namespace trajectories;
using namespace se3;
TaskPointEquality::TaskPointEquality(const std::string & name,
RobotWrapper & robot,
const std::string & frameName):
TaskMotion(name, robot),
m_frame_name(frameName),
m_constraint(name, 3, robot.nv()),
m_ref(12, 6)
{
assert(m_robot.model().existFrame(frameName));
m_frame_id = m_robot.model().getFrameId(frameName);
m_v_ref.setZero();
m_a_ref.setZero();
m_M_ref.setIdentity();
m_wMl.setIdentity();
m_p_error_vec.setZero(3);
m_v_error_vec.setZero(3);
m_p.resize(12);
m_v.resize(6);
m_p_ref.resize(12);
m_v_ref_vec.resize(6);
m_Kp.setZero(3);
m_Kd.setZero(3);
m_a_des.setZero(3);
m_J.setZero(6, robot.nv());
}
int TaskPointEquality::dim() const
{
//return self._mask.sum ()
return 6;
}
const Vector & TaskPointEquality::Kp() const { return m_Kp; }
const Vector & TaskPointEquality::Kd() const { return m_Kd; }