Commit 59a75dc5 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'devel' into release/4.11.1

parents c70e11b3 99bf9db6
......@@ -167,6 +167,10 @@ template <typename T>
Vector6d convertVelocity(const MatrixHomogeneous &M,
const MatrixHomogeneous &Mdes,
const Vector &faNufafbDes);
template <> const std::string FeaturePose<SE3Representation>::CLASS_NAME;
template <> const std::string FeaturePose<R3xSO3Representation>::CLASS_NAME;
#if __cplusplus >= 201103L
extern template class SOT_CORE_DLLAPI FeaturePose<SE3Representation>;
extern template class SOT_CORE_DLLAPI FeaturePose<R3xSO3Representation>;
......
......@@ -99,9 +99,31 @@ public:
/// @}
/// \name Commands related to the model
/// @{
template <typename Type>
void setParameter(const std::string &ParameterName,
const std::string &ParameterValue);
std::string getParameter(const std::string &ParameterName);
const Type &ParameterValue) {
if (!m_initSucceeded) {
DYNAMIC_GRAPH_ENTITY_WARNING(*this)
<< "Cannot set parameter " << ParameterName << " to "
<< ParameterValue << " before initialization!\n";
return;
}
m_robot_util->set_parameter<Type>(ParameterName, ParameterValue);
}
template <typename Type> Type getParameter(const std::string &ParameterName) {
if (!m_initSucceeded) {
DYNAMIC_GRAPH_ENTITY_WARNING(*this)
<< "Cannot get parameter " << ParameterName
<< " before initialization!\n";
Type ParameterValue;
return ParameterValue;
}
return m_robot_util->get_parameter<Type>(ParameterName);
}
/// @}
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
......
......@@ -11,6 +11,39 @@
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
This happen to be not working when including the boost property_tree
library. For this reason if defined, the current value of
BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
Once the property_tree included the pinocchio value of this variable is
restored.
*/
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#define UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
#endif
#ifdef BOOST_MPL_LIMIT_LIST_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
#define UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
#undef BOOST_MPL_LIMIT_LIST_SIZE
#endif
#include <boost/property_tree/ptree.hpp>
#ifdef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#undef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
#endif
#ifdef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
#undef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
#endif
#include "boost/assign.hpp"
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/logger.h>
......@@ -32,6 +65,25 @@ struct SOT_CORE_EXPORT JointLimits {
typedef Eigen::VectorXd::Index Index;
class SOT_CORE_EXPORT ExtractJointMimics {
public:
/// Constructor
ExtractJointMimics(std::string &robot_model);
/// Get mimic joints.
const std::vector<std::string> &get_mimic_joints();
private:
void go_through(boost::property_tree::ptree &pt, int level, int stage);
// Create empty property tree object
boost::property_tree::ptree tree_;
std::vector<std::string> mimic_joints_;
std::string current_joint_name_;
void go_through_full();
};
struct SOT_CORE_EXPORT ForceLimits {
Eigen::VectorXd upper;
Eigen::VectorXd lower;
......@@ -217,8 +269,22 @@ public:
If parameter_name already exists the value is overwritten.
If not it is inserted.
*/
template <typename Type>
void set_parameter(const std::string &parameter_name,
const std::string &parameter_value);
const Type &parameter_value) {
try {
typedef boost::property_tree::ptree::path_type path;
path apath(parameter_name, '/');
property_tree_.put<Type>(apath, parameter_value);
} catch (const boost::property_tree::ptree_error &e) {
std::ostringstream oss;
oss << "Robot utils: parameter path is invalid " << '\n'
<< " for set_parameter(" << parameter_name << ")\n"
<< e.what() << std::endl;
sendMsg(oss.str(), MSG_TYPE_ERROR);
return;
}
}
/** \brief Get a parameter of type string.
If parameter_name already exists the value is overwritten.
......@@ -226,19 +292,38 @@ public:
@param parameter_name: Name of the parameter
Return false if the parameter is not found.
*/
const std::string &get_parameter(const std::string &parameter_name);
template <typename Type>
Type get_parameter(const std::string &parameter_name) {
try {
boost::property_tree::ptree::path_type apath(parameter_name, '/');
const Type &res = property_tree_.get<Type>(apath);
return res;
} catch (const boost::property_tree::ptree_error &e) {
std::ostringstream oss;
oss << "Robot utils: parameter path is invalid " << '\n'
<< " for get_parameter(" << parameter_name << ")\n"
<< e.what() << std::endl;
sendMsg(oss.str(), MSG_TYPE_ERROR);
}
}
/** @} */
/** Access to property tree directly */
boost::property_tree::ptree &get_property_tree();
protected:
Logger logger_;
/** \brief Map of the parameters: map of strings. */
std::map<std::string, std::string> parameters_strings_;
/** \brief Property tree */
boost::property_tree::ptree property_tree_;
}; // struct RobotUtil
/// Accessors - This should be changed to RobotUtilPtrShared
typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
RobotUtilShrPtr RefVoidRobotUtil();
RobotUtilShrPtr getRobotUtil(std::string &robotName);
......
......@@ -7,7 +7,8 @@ from functools import reduce
from dynamic_graph import plug
from dynamic_graph.entity import Entity
from dynamic_graph.signal_base import SignalBase
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
from dynamic_graph.sot.core.feature_point6d_relative \
import FeaturePoint6dRelative
# Identity matrix of order 4
I4 = reduce(lambda m, i: m + (i * (0., ) + (1., ) + (3 - i) * (0., ), ),
......
......@@ -10,10 +10,11 @@ from .operator import (
Compose_R_and_T, ConvolutionTemporal, HomoToMatrix, HomoToRotation,
HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo,
Inverse_of_matrixrotation, Inverse_of_matrixtwist, Inverse_of_unitquat,
MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion, PoseQuatToMatrixHomo,
MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo,
MatrixToQuaternion, MatrixToRPY, MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo,
MatrixTranspose, Multiply_double_vector, Multiply_matrix_vector,
MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
PoseQuatToMatrixHomo, MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta,
MatrixToHomo, MatrixToQuaternion, MatrixToRPY, MatrixToUTheta,
MatrixHomoToSE3Vector, SE3VectorToMatrixHomo, MatrixTranspose,
Multiply_double_vector, Multiply_matrix_vector,
Multiply_matrixTwist_vector, Multiply_matrixHomo_vector,
Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo,
Multiply_of_matrixrotation, Multiply_of_matrixtwist,
......
......@@ -7,7 +7,8 @@ from dynamic_graph.sot.core.task import Task
def toFlags(arr):
"""
Convert an array of boolean to a /flag/ format, type 001010110, in little indian
Convert an array of boolean to a /flag/ format, type 001010110,
in little indian
(reverse order, first bool of the list will be the [01] of extrem right).
"""
lres = [0] * (max(arr) + 1)
......
......@@ -34,7 +34,7 @@ class MetaTaskCom(object):
self.featureDes.errorIN.value = v
# --- HELPER FUNCTIONS ---------------------------------------------------------
# --- HELPER FUNCTIONS --------------------------------------------------------
def setGain(gain, val):
if val is not None:
if isinstance(val, int) or isinstance(val, float):
......
from dynamic_graph import plug
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d
# TODO: this function is imported from meta_tasks_kine in several places, whereas it is defined in meta_tasks
# TODO: this function is imported from meta_tasks_kine in several places,
# whereas it is defined in meta_tasks
from dynamic_graph.sot.core.meta_tasks import gotoNd # noqa
from dynamic_graph.sot.core.meta_tasks import MetaTaskCom
from dynamic_graph.sot.core.task import Task
......
......@@ -99,7 +99,7 @@ class MetaTaskKine6dRel(MetaTask6d):
self.opPointModifBase.activ = True
# --- HELPER FUNCTIONS ---------------------------------------------------------
# --- HELPER FUNCTIONS --------------------------------------------------------
def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
......@@ -141,20 +141,27 @@ Documentation
Inherited from MetaTask6d.
The aim of this MetaTask is to give a simple and immediate interface to implement a relative task between two
operational points of the robot. The new variable "opPointBase" represents in fact the second operational point (the
The aim of this MetaTask is to give a simple and immediate interface to
implement a relative task between two
operational points of the robot. The new variable "opPointBase" represents
in fact the second operational point (the
first is inherited from the father class).
It's been decided to reuse (so not to redefine) all methodes from MetaTask6d related to the opPoint to implement the
behaviour of one of the two points (called "Other" from now on) and to reimplement in a intuitive way the same
It's been decided to reuse (so not to redefine) all methodes from MetaTask6d
related to the opPoint to implement the
behaviour of one of the two points (called "Other" from now on) and to
reimplement in a intuitive way the same
functions for the second point ("Ref").
Utilization
It should be noticed that both feature and reference are defined as a couple of signals, while normally it would be
enough define the reference as one signal that represents the diplacement between the two positions. Nevertheless this
redundant approach allows to a very intuitive and safe usage of the class because the references can be set just using
It should be noticed that both feature and reference are defined as a
couple of signals, while normally it would be
enough define the reference as one signal that represents the diplacement
between the two positions. Nevertheless this
redundant approach allows to a very intuitive and safe usage of the class
because the references can be set just using
the current position of the two operational points.
For this reason all the goTo functions have been redefined.
......
# This class embeds a given loop function (generally by using a decorator) into a
# dedicated thread, whose execution can be stoped or run on demand from the
# This class embeds a given loop function (generally by using a decorator) into
# a dedicated thread, whose execution can be stoped or run on demand from the
# script thread.
#
# To use the previous class, a 'loop' function has to be defined.
......
......@@ -55,7 +55,8 @@ def addRobotViewer(robot, **args):
if small:
if verbose:
print('using a small robot')
RobotClass.stateFullSize = lambda x: stateFullSize(x, small_extra * (0.0, ))
RobotClass.stateFullSize = lambda x: \
stateFullSize(x, small_extra * (0.0, ))
else:
RobotClass.stateFullSize = stateFullSize
......
......@@ -20,7 +20,7 @@ namespace dg = ::dynamicgraph;
#define REGISTER_UNARY_OP(OpType, name) \
template <> \
const std::string UnaryOp<OpType>::CLASS_NAME = std::string(#name); \
Entity *regFunction_##name(const std::string &objname) { \
Entity *regFunction_##name(const std::string &objname) { \
return new UnaryOp<OpType>(objname); \
} \
EntityRegisterer regObj_##name(std::string(#name), &regFunction_##name)
......@@ -66,7 +66,8 @@ struct HomogeneousMatrixToSE3Vector
res.head<3>() = M.translation();
res.segment(3, 3) = M.linear().row(0);
res.segment(6, 3) = M.linear().row(1);
res.segment(9, 3) = M.linear().row(2); }
res.segment(9, 3) = M.linear().row(2);
}
};
REGISTER_UNARY_OP(HomogeneousMatrixToSE3Vector, MatrixHomoToSE3Vector);
......@@ -116,7 +117,7 @@ REGISTER_UNARY_OP(UThetaToQuaternion, UThetaToQuaternion);
#define REGISTER_BINARY_OP(OpType, name) \
template <> \
const std::string BinaryOp<OpType>::CLASS_NAME = std::string(#name); \
Entity *regFunction_##name(const std::string &objname) { \
Entity *regFunction_##name(const std::string &objname) { \
return new BinaryOp<OpType>(objname); \
} \
EntityRegisterer regObj_##name(std::string(#name), &regFunction_##name)
......@@ -154,7 +155,7 @@ REGISTER_BINARY_OP(WeightedAdder<double>, WeightAdd_of_double);
#define REGISTER_VARIADIC_OP(OpType, name) \
template <> \
const std::string VariadicOp<OpType>::CLASS_NAME = std::string(#name); \
Entity *regFunction_##name(const std::string &objname) { \
Entity *regFunction_##name(const std::string &objname) { \
return new VariadicOp<OpType>(objname); \
} \
EntityRegisterer regObj_##name(std::string(#name), &regFunction_##name)
......
......@@ -14,6 +14,23 @@
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
This happen to be not working when including the boost property_tree
library. For this reason if defined, the current value of
BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
Once the property_tree included the pinocchio value of this variable is
restored.
*/
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
#include <boost/property_tree/ptree.hpp>
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#else
#include <boost/property_tree/ptree.hpp>
#endif
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
#include <iostream>
......@@ -128,20 +145,67 @@ ParameterServer::ParameterServer(const std::string &name)
*this, &ParameterServer::displayRobotUtil,
docCommandVoid0("Display the current robot util data set.")));
addCommand(
"setParameterBool",
makeCommandVoid2(
*this, &ParameterServer::setParameter<bool>,
docCommandVoid2("Set a parameter named ParameterName to value "
"ParameterValue (string format).",
"(string) ParameterName", "(bool) ParameterValue")));
addCommand(
"setParameterInt",
makeCommandVoid2(
*this, &ParameterServer::setParameter<int>,
docCommandVoid2("Set a parameter named ParameterName to value "
"ParameterValue (string format).",
"(string) ParameterName", "(int) ParameterValue")));
addCommand("setParameterDbl",
makeCommandVoid2(
*this, &ParameterServer::setParameter<double>,
docCommandVoid2("Set a parameter named ParameterName to value "
"ParameterValue (string format).",
"(string) ParameterName",
"(double) ParameterValue")));
addCommand("setParameter",
makeCommandVoid2(
*this, &ParameterServer::setParameter,
*this, &ParameterServer::setParameter<std::string>,
docCommandVoid2("Set a parameter named ParameterName to value "
"ParameterValue (string format).",
"(string) ParameterName",
"(string) ParameterValue")));
addCommand("getParameter", makeCommandReturnType1(
*this, &ParameterServer::getParameter,
docCommandReturnType1<std::string>(
"Return the parameter value for parameter"
addCommand(
"getParameter",
makeCommandReturnType1(*this, &ParameterServer::getParameter<std::string>,
docCommandReturnType1<std::string>(
"Return the parameter value for parameter"
" named ParameterName.",
"(string) ParameterName")));
addCommand(
"getParameterInt",
makeCommandReturnType1(
*this, &ParameterServer::getParameter<int>,
docCommandReturnType1<int>("Return the parameter value for parameter"
" named ParameterName.",
"(string) ParameterName")));
"(int) ParameterName")));
addCommand(
"getParameterDbl",
makeCommandReturnType1(*this, &ParameterServer::getParameter<double>,
docCommandReturnType1<double>(
"Return the parameter value for parameter"
" named ParameterName.",
"(double) ParameterName")));
addCommand(
"getParameterBool",
makeCommandReturnType1(
*this, &ParameterServer::getParameter<bool>,
docCommandReturnType1<bool>("Return the parameter value for parameter"
" named ParameterName.",
"(string) ParameterName")));
}
void ParameterServer::init_simple(const double &dt) {
......@@ -161,8 +225,11 @@ void ParameterServer::init_simple(const double &dt) {
if (listOfRobots->size() == 1)
localName = (*listOfRobots)[0];
else {
std::ostringstream oss;
oss << "No robot registered in the parameter server list";
oss << " listOfRobots->size: " << listOfRobots->size();
throw ExceptionTools(ExceptionTools::ErrorCodeEnum::PARAMETER_SERVER,
"No robot registered in the parameter server list");
oss.str());
}
if (!isNameInRobotUtil(localName)) {
......@@ -340,30 +407,6 @@ bool ParameterServer::isJointInRange(unsigned int id, double q) {
return true;
}
void ParameterServer::setParameter(const std::string &ParameterName,
const std::string &ParameterValue) {
if (!m_initSucceeded) {
SEND_WARNING_STREAM_MSG("Cannot set parameter " + ParameterName + " to " +
ParameterValue + " before initialization!");
return;
}
m_robot_util->set_parameter(ParameterName, ParameterValue);
}
std::string ParameterServer::getParameter(const std::string &ParameterName) {
std::string ParameterValue("");
if (!m_initSucceeded) {
SEND_WARNING_STREAM_MSG("Cannot get parameter " + ParameterName +
" before initialization!");
return ParameterValue;
}
ParameterValue = m_robot_util->get_parameter(ParameterName);
return ParameterValue;
}
/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
......
......@@ -4,6 +4,14 @@
* This file is part of sot-core.
* See license file.
*/
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
#include <boost/property_tree/ptree.hpp>
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#else
#include <boost/property_tree/ptree.hpp>
#endif
#include <boost/python.hpp>
#include <boost/python/suite/indexing/map_indexing_suite.hpp>
......
......@@ -5,26 +5,107 @@
*
*/
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
This happen to be not working when including the boost property_tree
library. For this reason if defined, the current value of
BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
Once the property_tree included the pinocchio value of this variable is
restored.
*/
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#else
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#endif
#include <dynamic-graph/factory.h>
#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/robot-utils.hh>
#include <sstream>
namespace dynamicgraph {
namespace sot {
namespace dg = ::dynamicgraph;
namespace pt = boost::property_tree;
using namespace dg;
using namespace dg::command;
RobotUtil VoidRobotUtil;
ForceLimits VoidForceLimits;
JointLimits VoidJointLimits;
Index VoidIndex(-1);
RobotUtilShrPtr RefVoidRobotUtil() {
return boost::make_shared<RobotUtil>(VoidRobotUtil);
return std::shared_ptr<RobotUtil>(nullptr);
}
ExtractJointMimics::ExtractJointMimics(std::string &robot_model) {
// Parsing the model from a string.
std::istringstream iss(robot_model);
/// Read the XML file in the property tree.
boost::property_tree::read_xml(iss, tree_);
/// Start the recursive parsing.
go_through_full();
}
const std::vector<std::string> &ExtractJointMimics::get_mimic_joints() {
return mimic_joints_;
}
void ExtractJointMimics::go_through_full() {
/// Root of the recursive parsing.
current_joint_name_ = "";
go_through(tree_, 0, 0);
}
void ExtractJointMimics::go_through(pt::ptree &pt, int level, int stage) {
/// If pt is empty (i.e. this is a leaf)
if (pt.empty()) {
/// and this is a name of a joint (stage == 3) update the
/// curret_joint_name_ variable.
if (stage == 3)
current_joint_name_ = pt.data();
} else {
/// This is not a leaf
for (auto pos : pt) {
int new_stage = stage;
/// But this is joint
if (pos.first == "joint")
/// the continue the exploration.
new_stage = 1;
else if (pos.first == "<xmlattr>") {
/// we are exploring the xml attributes of a joint
/// -> continue the exploration
if (stage == 1)
new_stage = 2;
}
/// The xml attribute of the joint is the name
/// next leaf is the name we are possibly looking for
else if (pos.first == "name") {
if (stage == 2)
new_stage = 3;
}
/// The exploration of the tree tracback on the joint
/// and find that this is a mimic joint.
else if (pos.first == "mimic") {
if (stage == 1)
/// Save the current name of the joint
/// in mimic_joints.
mimic_joints_.push_back(current_joint_name_);
} else
new_stage = 0;
/// Explore the subtree of the XML robot description.
go_through(pos.second, level + 1, new_stage);
}
}
}
void ForceLimits::display(std::ostream &os) const {
......@@ -359,6 +440,8 @@ void RobotUtil::display(std::ostream &os) const {
os << "(" << it->first << "," << it->second << ") ";
}
os << std::endl;
boost::