Commit a9f71a13 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][Python] Added a method to get neutral config from an srdf file + binded it

parent e98ba1ca
......@@ -126,12 +126,26 @@ namespace se3
nq += j.nq();
nv += j.nv();
neutralConfigurations.conservativeResize(nq);neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration();
effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = effort;
velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = velocity;
lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = lowPos;
upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>() = upPos;
// Ensure that limits are not inf
Eigen::VectorXd neutralConf((lowerPositionLimit.bottomRows<D::NQ>() + upperPositionLimit.bottomRows<D::NQ>())/2 );
neutralConfigurations.conservativeResize(nq);
if ( std::isfinite(neutralConf.norm()) )
{
neutralConfigurations.bottomRows<D::NQ>() = neutralConf;
}
else
{
assert( false && "One of the position limit is inf or NaN");
neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration();
}
addFrame((jointName!="")?jointName:random(8), idx, SE3::Identity(), JOINT);
return idx;
......
......@@ -109,6 +109,89 @@ namespace se3
#endif // ifdef WITH_HPP_FCL
///
/// \brief Get the neutral configuration of a given model associated to a SRDF file.
/// It throws if the SRDF file is incorrect.
///
/// \param[in] model The Model for which we want the neutral config
/// \param[in] filename The complete path to the SRDF file.
/// \param[in] verbose Verbosity mode.
///
/// \return The neutral configuration as an eigen vector
inline Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
{
const Eigen::VectorXd neutralConfig(model.nq);
// Check extension
const std::string extension = filename.substr(filename.find_last_of('.')+1);
if (extension != "srdf")
{
const std::string exception_message (filename + " does not have the right extension.");
throw std::invalid_argument(exception_message);
}
// Open file
std::ifstream srdf_stream(filename.c_str());
if (! srdf_stream.is_open())
{
const std::string exception_message (filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
// Read xml stream
using boost::property_tree::ptree;
ptree pt;
read_xml(srdf_stream, pt);
// Iterate over all tags directly children of robot
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
{
// if we encounter a tag group_state
if (v.first == "group_state")
{
const std::string name = v.second.get<std::string>("<xmlattr>.name");
std::cout << name << std::endl;
// Ensure that it is the half_sitting tag
if( name == "half_sitting")
{
// Iterate over all the joint tags
BOOST_FOREACH(const ptree::value_type & joint, v.second)
{
if (joint.first == "joint")
{
std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
double joint_config = joint.second.get<double>("<xmlattr>.value");
if (verbose)
{
std::cout << "(" << joint_name << " , " << joint_config << ")" << std::endl;
}
// Search in model the joint and its config id
Model::JointIndex joint_id = model.getJointId(joint_name);
const JointModel & joint = model.joints[joint_id];
if (joint_id != model.joints.size()) // != model.njoints
{
model.neutralConfigurations(joint.idx_q()) = joint_config; // joint with 1 dof
// model.neutralConfigurations.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof
}
else
{
if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
}
}
}
return neutralConfig;
}
}
} // BOOST_FOREACH
assert(false && "no half_sitting configuration found in the srdf file"); // Should we throw something here ?
return neutralConfig; // warning : uninitialized vector is returned
}
}
} // namespace se3
......
......@@ -37,6 +37,8 @@
#include "pinocchio/multibody/parser/lua.hpp"
#endif // #ifdef WITH_LUA
#include "pinocchio/multibody/parser/srdf.hpp"
namespace se3
{
namespace python
......@@ -122,6 +124,14 @@ namespace se3
}
#endif // #ifdef WITH_LUA
static Eigen::VectorXd getNeutralConfigurationFromSrdf(ModelHandler & model,
const std::string & filename,
bool verbose
)
{
return se3::srdf::getNeutralConfigurationFromSrdf(*model, filename, verbose);
}
/* --- Expose --------------------------------------------------------- */
static void expose();
}; // struct ParsersPythonVisitor
......@@ -173,6 +183,13 @@ namespace se3
"Parse the urdf file given in input and return a proper pinocchio model "
"(remember to create the corresponding data structure).");
#endif // #ifdef WITH_LUA
bp::def("getNeutralConfigurationFromSrdf",getNeutralConfigurationFromSrdf,
// static_cast <ModelHandler (*) ( const std::string &, bool)> (&ParsersPythonVisitor::getNeutralConfigurationFromSrdf),
bp::args("Model for which we want the neutral config","srdf filename (string)", "verbosity"
),
"Get the neutral configuration of a given model associated to a SRDF file");
}
}
......
Supports Markdown
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