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

Merge branch 'contact_and_point' of git://github.com/jviereck/tsid into jviereck-contact_and_point

parents db076b48 b23a73e1
Subproject commit dc8b946d456d2c41ad12b819111b005148c68031
Subproject commit 083fa2cb0fa4ad594926d9bb3d246075e62ce9ee
......@@ -122,6 +122,7 @@ SET(${PROJECT_NAME}_CONTACTS_HEADERS
include/tsid/contacts/fwd.hpp
include/tsid/contacts/contact-base.hpp
include/tsid/contacts/contact-6d.hpp
include/tsid/contacts/contact-point.hpp
)
SET(${PROJECT_NAME}_TRAJECTORIES_HEADERS
......
import libtsid_pywrap as tsid
from libtsid_pywrap import *
from tsid.libtsid_pywrap import *
//
// 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/>.
//
#include "tsid/bindings/python/contacts/contact-6d.hpp"
#include "tsid/bindings/python/contacts/expose-contact.hpp"
namespace tsid
{
namespace python
{
void exposeContact6d()
{
Contact6DPythonVisitor<tsid::contacts::Contact6d>::expose("Contact6d");
}
}
}
#ifndef __tsid_python_contact_hpp__
#define __tsid_python_contact_hpp__
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -23,6 +15,14 @@
// <http://www.gnu.org/licenses/>.
//
#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>
#include "tsid/contacts/contact-6d.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/math/constraint-inequality.hpp"
......@@ -37,8 +37,8 @@ namespace tsid
namespace bp = boost::python;
template<typename Contact6d>
struct ContactPythonVisitor
: public boost::python::def_visitor< ContactPythonVisitor<Contact6d> >
struct Contact6DPythonVisitor
: public boost::python::def_visitor< Contact6DPythonVisitor<Contact6d> >
{
template<class PyClass>
......@@ -49,32 +49,32 @@ namespace tsid
.def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::MatrixXd, Eigen::VectorXd, double, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactPoint"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce"), bp::arg("regWeight")), "Default Constructor"))
.add_property("n_motion", &Contact6d::n_motion, "return number of motion")
.add_property("n_force", &Contact6d::n_force, "return number of force")
.add_property("name", &ContactPythonVisitor::name, "return name")
.def("computeMotionTask", &ContactPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
.def("computeForceTask", &ContactPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
.def("computeForceRegularizationTask", &ContactPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))
.add_property("name", &Contact6DPythonVisitor::name, "return name")
.def("computeMotionTask", &Contact6DPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
.def("computeForceTask", &Contact6DPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
.def("computeForceRegularizationTask", &Contact6DPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))
.add_property("getForceGeneratorMatrix", bp::make_function(&ContactPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("getForceGeneratorMatrix", bp::make_function(&Contact6DPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("getForceRegularizationWeight", &Contact6d::getForceRegularizationWeight, "return force reg weight")
.def("getNormalForce", &ContactPythonVisitor::getNormalForce, bp::arg("vec"))
.def("getNormalForce", &Contact6DPythonVisitor::getNormalForce, bp::arg("vec"))
.add_property("getMinNormalForce", &Contact6d::getMinNormalForce)
.add_property("getMaxNormalForce", &Contact6d::getMaxNormalForce)
.add_property("Kp", bp::make_function(&ContactPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kd", bp::make_function(&ContactPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.def("setKp", &ContactPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &ContactPythonVisitor::setKd, bp::arg("Kd"))
.add_property("Kp", bp::make_function(&Contact6DPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kd", bp::make_function(&Contact6DPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.def("setKp", &Contact6DPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &Contact6DPythonVisitor::setKd, bp::arg("Kd"))
.def("setContactPoints", &ContactPythonVisitor::setContactPoints, bp::args("vec"))
.def("setContactNormal", &ContactPythonVisitor::setContactNormal, bp::args("vec"))
.def("setFrictionCoefficient", &ContactPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
.def("setMinNormalForce", &ContactPythonVisitor::setMinNormalForce, bp::args("min_force"))
.def("setMaxNormalForce", &ContactPythonVisitor::setMaxNormalForce, bp::args("max_force"))
.def("setRegularizationTaskWeight", &ContactPythonVisitor::setRegularizationTaskWeight, bp::args("double"))
.def("setReference", &ContactPythonVisitor::setReference, bp::args("SE3"))
.def("setForceReference", &ContactPythonVisitor::setForceReference, bp::args("f_vec"))
.def("setRegularizationTaskWeightVector", &ContactPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
.def("setContactPoints", &Contact6DPythonVisitor::setContactPoints, bp::args("vec"))
.def("setContactNormal", &Contact6DPythonVisitor::setContactNormal, bp::args("vec"))
.def("setFrictionCoefficient", &Contact6DPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
.def("setMinNormalForce", &Contact6DPythonVisitor::setMinNormalForce, bp::args("min_force"))
.def("setMaxNormalForce", &Contact6DPythonVisitor::setMaxNormalForce, bp::args("max_force"))
.def("setRegularizationTaskWeight", &Contact6DPythonVisitor::setRegularizationTaskWeight, bp::args("double"))
.def("setReference", &Contact6DPythonVisitor::setReference, bp::args("SE3"))
.def("setForceReference", &Contact6DPythonVisitor::setForceReference, bp::args("f_vec"))
.def("setRegularizationTaskWeightVector", &Contact6DPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
;
}
static std::string name(Contact6d & self){
......@@ -150,7 +150,7 @@ namespace tsid
bp::class_<Contact6d>(class_name.c_str(),
doc.c_str(),
bp::no_init)
.def(ContactPythonVisitor<Contact6d>());
.def(Contact6DPythonVisitor<Contact6d>());
}
};
}
......
......@@ -15,16 +15,16 @@
// <http://www.gnu.org/licenses/>.
//
#include "tsid/bindings/python/contacts/contact.hpp"
#include "tsid/bindings/python/contacts/contact-point.hpp"
#include "tsid/bindings/python/contacts/expose-contact.hpp"
namespace tsid
{
namespace python
{
void exposeContact6d()
void exposeContactPoint()
{
ContactPythonVisitor<tsid::contacts::Contact6d>::expose("Contact6d");
ContactPointPythonVisitor<tsid::contacts::ContactPoint>::expose("ContactPoint");
}
}
}
//
// 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_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>
#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"
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
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -18,19 +18,22 @@
#ifndef __tsid_python_expose_contact_hpp__
#define __tsid_python_expose_contact_hpp__
#include "tsid/bindings/python/contacts/contact.hpp"
#include "tsid/bindings/python/contacts/contact-6d.hpp"
#include "tsid/bindings/python/contacts/contact-point.hpp"
namespace tsid
{
namespace python
{
void exposeContact6d();
void exposeContactPoint();
inline void exposeContact()
{
exposeContact6d();
exposeContactPoint();
}
} // namespace python
} // namespace tsid
#endif // ifndef __tsid_python_expose_contact_hpp__
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -26,6 +26,7 @@
#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"
......@@ -56,7 +57,8 @@ namespace tsid
.def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact, bp::args("contact"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact6d, bp::args("contact"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContactPoint, bp::args("contact"))
.def("removeTask", &InvDynPythonVisitor::removeTask, bp::args("task_name", "duration"))
.def("removeRigidContact", &InvDynPythonVisitor::removeRigidContact, bp::args("contact_name", "duration"))
.def("computeProblemData", &InvDynPythonVisitor::computeProblemData, bp::args("time", "q", "v"))
......@@ -84,7 +86,10 @@ namespace tsid
static bool updateTaskWeight(T& self, const std::string & task_name, double weight){
return self.updateTaskWeight(task_name, weight);
}
static bool addRigidContact(T& self, contacts::Contact6d & contact){
static bool addRigidContact6d(T& self, contacts::Contact6d & contact){
return self.addRigidContact(contact);
}
static bool addRigidContactPoint(T& self, contacts::ContactPoint & contact){
return self.addRigidContact(contact);
}
static bool removeTask(T& self, const std::string & task_name, double transition_duration){
......
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -40,7 +40,7 @@ namespace tsid
void visit(PyClass& cl) const
{
cl
.def(bp::init<>("Defulat Constructor"))
.def(bp::init<>("Default Constructor"))
.def("print_all", &T::print)
.def("append", &T::append_eq, bp::arg("data"))
.def("append", &T::append_ineq, bp::arg("data"))
......
......@@ -23,6 +23,7 @@
#include "tsid/bindings/python/tasks/task-joint-posture.hpp"
namespace tsid
{
namespace python
......
Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
Subproject commit a921f1ead10e551804da70d6080a95dba1e673bd
//
// Copyright (c) 2017 CNRS
// Copyright (c) 2017 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -31,7 +31,7 @@ namespace tsid
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::ConstRefMatrix ConstRefMatrix;
typedef math::ConstRefVector ConstRefVector;
typedef math::Matrix3x Matrix3x;
......
//
// Copyright (c) 2017 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 __invdyn_contact_point_hpp__
#define __invdyn_contact_point_hpp__
#include "tsid/contacts/contact-base.hpp"
#include "tsid/tasks/task-se3-equality.hpp"
#include "tsid/math/constraint-inequality.hpp"
#include "tsid/math/constraint-equality.hpp"
namespace tsid
{
namespace contacts
{
class ContactPoint : public ContactBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::ConstRefMatrix ConstRefMatrix;
typedef math::ConstRefVector ConstRefVector;
typedef math::Matrix3x Matrix3x;
typedef math::Vector6 Vector6;
typedef math::Vector3 Vector3;
typedef math::Vector Vector;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality;
typedef se3::SE3 SE3;
ContactPoint(const std::string & name,
RobotWrapper & robot,
const std::string & frameName,
ConstRefVector contactNormal,
const double frictionCoefficient,
const double minNormalForce,
const double maxNormalForce,
const double regularizationTaskWeight);
/// Return the number of motion constraints
virtual unsigned int n_motion() const;
/// Return the number of force variables
virtual unsigned int n_force() const;
virtual const ConstraintBase & computeMotionTask(const double t,
ConstRefVector q,
ConstRefVector v,
const Data & data);
virtual const ConstraintInequality & computeForceTask(const double t,
ConstRefVector q,
ConstRefVector v,
const Data & data);
virtual const Matrix & getForceGeneratorMatrix();
virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
ConstRefVector q,
ConstRefVector v,
const Data & data);
const TaskMotion & getMotionTask() const;
const ConstraintBase & getMotionConstraint() const;
const ConstraintInequality & getForceConstraint() const;
const ConstraintEquality & getForceRegularizationTask() const;
double getForceRegularizationWeight() const;
double getNormalForce(ConstRefVector f) const;
double getMinNormalForce() const;
double getMaxNormalForce() const;
const Vector & Kp() const;
const Vector & Kd() const;
void Kp(ConstRefVector Kp);
void Kd(ConstRefVector Kp);
bool setContactNormal(ConstRefVector contactNormal);
bool setFrictionCoefficient(const double frictionCoefficient);
bool setMinNormalForce(const double minNormalForce);
bool setMaxNormalForce(const double maxNormalForce);
bool setRegularizationTaskWeight(const double w);
void setReference(const SE3 & ref);
void setForceReference(ConstRefVector & f_ref);
void setRegularizationTaskWeightVector(ConstRefVector & w);
/**
* @brief Specifies if properties of the contact point and motion task
* are expressed in the local or local world oriented frame. The contact
* forces, contact normal and contact coefficients are interpreted in
* the specified frame.
*
* @param local_frame If true, use the local frame, otherwise use the
* local world oriented
*/
void useLocalFrame(bool local_frame);
protected:
void updateForceInequalityConstraints();
void updateForceRegularizationTask();
void updateForceGeneratorMatrix();
TaskSE3Equality m_motionTask;
ConstraintInequality m_forceInequality;
ConstraintEquality m_forceRegTask;
Vector3 m_contactNormal;
Vector3 m_fRef;
Vector3 m_weightForceRegTask;
double m_mu;
double m_fMin;
double m_fMax;
double m_regularizationTaskWeight;
Matrix m_forceGenMat;
};
}
}
#endif // ifndef __invdyn_contact_6d_hpp__
//
// Copyright (c) 2017 CNRS
// Copyright (c) 2017 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -111,7 +111,7 @@ namespace tsid
bool updateTaskWeight(const std::string & task_name,