Commit 9e58434d authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Adding parameter server access with buildReducedModel.

As this stage the main test is not working.
parent 1b1dab11
......@@ -189,9 +189,13 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
@{
*/
/*! \brief Build the pattern generator interface from a Urdf
and SRDF file. */
bool buildModel(void);
/*! \brief Build the pattern generator interface from the parameter
"/robot_description" and the informations in
"/robot/specifificities/feet/[right|left]/[size|anklePosition]*/
bool buildPGI(void);
/*! \brief Build the reduced model. */
bool buildReducedModel(void);
/*! \brief Initialize the state of the robot. */
bool InitState(void);
......
......@@ -640,10 +640,11 @@ bool PatternGenerator::InitState(void) {
return true;
}
bool PatternGenerator::buildModel(void) {
bool ok = true;
bool PatternGenerator::buildReducedModel(void) {
// Name of the parameter
std::string lparameter_name("robot_description");
std::string lparameter_name("/robot_description");
// Model of the robot inside a string.
std::string lrobot_description;
......@@ -662,12 +663,18 @@ bool PatternGenerator::buildModel(void) {
<< "The robot with name " << model_name
<< " was not found !";
throw std::invalid_argument(oss.str());
ok=false;
return false;
}
// Then build a complete robot model.
lrobot_description = aRobotUtil->get_parameter<string>(lparameter_name);
try {
// Then build a complete robot model.
lrobot_description = aRobotUtil->get_parameter<string>(lparameter_name);
} catch (...) {
SOT_THROW ExceptionPatternGenerator(
ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
"Error while getting parameter " + lparameter_name + " for the WPG.");
return false;
}
pinocchio::Model lrobotModel;
pinocchio::urdf::buildModelFromXML(lrobot_description,
......@@ -685,7 +692,8 @@ bool PatternGenerator::buildModel(void) {
for(auto it : list_of_joints_to_lock_by_name)
{
const std::string & joint_name = it;
if(m_robotModel.existJointName(joint_name)) // do not consider joint that are not in the model
if(m_robotModel.existJointName(joint_name))
// do not consider joint that are not in the model
list_of_joints_to_lock_by_id.
push_back(m_robotModel.getJointId(joint_name));
}
......@@ -693,49 +701,65 @@ bool PatternGenerator::buildModel(void) {
list_of_joints_to_lock_by_id,
q_neutral);
return true;
}
bool PatternGenerator::buildPGI(void) {
bool ok = true;
// Build the reduced model of the robot
buildReducedModel();
// Creating the humanoid robot.
m_PR = new pg::PinocchioRobot();
m_PR->initializeRobotModelAndData(&m_robotModel, m_robotData);
// Read xml/srdf stream
std::ifstream srdf_stream(m_srdfFile.c_str());
using boost::property_tree::ptree;
ptree pt;
try {
read_xml(srdf_stream, pt);
// Initialize the Right Foot
pg::PRFoot aFoot;
string path = "robot.specificities.feet.right.size";
string path = "/robot/specificities/feet/right/size";
BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
aFoot.soleHeight = v.second.get<double>("height");
aFoot.soleWidth = v.second.get<double>("width");
aFoot.soleDepth = v.second.get<double>("depth");
} // BOOST_FOREACH
path = "robot.specificities.feet.right.anklePosition";
}
path = "/robot/specificities/feet/right/anklePosition";
BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
aFoot.anklePosition(0) = v.second.get<double>("x");
aFoot.anklePosition(1) = v.second.get<double>("y");
aFoot.anklePosition(2) = v.second.get<double>("z");
} // BOOST_FOREACH
}
pinocchio::FrameIndex ra = m_robotModel.getFrameId("r_ankle");
aFoot.associatedAnkle = m_robotModel.frames.at(ra).parent;
m_PR->initializeRightFoot(aFoot);
// Initialize the Left Foot
path = "robot.specificities.feet.left.size";
path = "/robot/specificities/feet/left/size";
BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
aFoot.soleHeight = v.second.get<double>("height");
aFoot.soleWidth = v.second.get<double>("width");
aFoot.soleDepth = v.second.get<double>("depth");
} // BOOST_FOREACH
path = "robot.specificities.feet.left.anklePosition";
path = "/robot/specificities/feet/left/anklePosition";
BOOST_FOREACH (const ptree::value_type &v, pt.get_child(path.c_str())) {
aFoot.anklePosition(0) = v.second.get<double>("x");
aFoot.anklePosition(1) = v.second.get<double>("y");
aFoot.anklePosition(2) = v.second.get<double>("z");
} // BOOST_FOREACH
pinocchio::FrameIndex la = m_robotModel.getFrameId("l_ankle");
aFoot.associatedAnkle = m_robotModel.frames.at(la).parent;
m_PR->initializeLeftFoot(aFoot);
} catch (...) {
cerr << "problem while reading the srdf file. File corrupted?" << endl;
ok = false;
......@@ -1682,8 +1706,9 @@ void PatternGenerator::initCommands(void) {
"buildModel",
makeCommandVoid0(
*this,
(void (PatternGenerator::*)(void)) & PatternGenerator::buildModel,
docCommandVoid0("From the files, parse and build.")));
(void (PatternGenerator::*)(void)) & PatternGenerator::buildPGI,
docCommandVoid0("From the files, parse and build the robot model and"
" the Walking Pattern Generator.")));
addCommand(
"initState",
makeCommandVoid0(
......
......@@ -5,17 +5,7 @@
ADD_DEFINITIONS(-DDEBUG=2)
ADD_UNIT_TEST(main_test main.cc)
TARGET_LINK_LIBRARIES(main_test
jrl-walkgen::jrl-walkgen
sot-core::sot-core
next-step
next-step-pg-sot
step-checker
step-computer-joystick
step-observer
step-queue
step-time-line
which-foot-upper
pg
pg-manager
)
/*
* Copyright
*/
#include <iostream>
#include <string>
#include <sstream>
#include <algorithm>
#include <iterator>
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
#define BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
#endif
#ifdef BOOST_MPL_LIMIT_LIST_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
#undef BOOST_MPL_LIMIT_LIST_SIZE
#define BOOST_MPL_LIMIT_LIST_SIZE_PUSH
#endif
#include <boost/property_tree/ptree.hpp>
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#endif
#ifdef BOOST_MPL_LIMIT_LIST_SIZE_PUSH
#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
#endif
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <sot/pattern-generator/pg.h>
#include <sot/core/debug.hh>
#include <sot/core/robot-utils.hh>
int main(int, char**) {
using namespace std;
dynamicgraph::sot::PatternGenerator aPG;
std::string aRobotURDF(
"/opt/openrobots/share/talos_data/urdf/talos_reduced_wpg.urdf");
std::string aRobotSRDF(
"/opt/openrobots/share/talos_data/srdf/talos_wpg.srdf");
// Get environment variable CMAKE_PREFIX_PATH
const string s_cmake_prefix_path = getenv( "CMAKE_PREFIX_PATH" );
// Read the various paths
vector<string> paths;
boost::split(paths, s_cmake_prefix_path, boost::is_any_of(":;"));
// Search talos_reduced_wpg.urdf
string filename="";
for (auto test_path : paths)
{
filename = test_path +
string("/share/talos_data/urdf/talos_reduced_wpg.urdf");
if ( boost::filesystem::exists(filename))
break;
}
// If not found fails
if (filename.size()==0)
{
cerr << "Unable to find talos_reduced_wpg.urdf" << endl;
exit(-1);
}
// Otherwise read the file
ifstream talos_reduced_wpg_file(filename);
ostringstream oss;
oss << talos_reduced_wpg_file.rdbuf();
// Name of the parameter
string lparameter_name("/robot_description");
// Model of the robot inside a string.
string lrobot_description = oss.str();
std::shared_ptr<std::vector<std::string>>
alist_of_robots = dynamicgraph::sot::getListOfRobots();
unsigned int idx = 0;
for (auto an_it_of_robot : *alist_of_robots)
{
std::cout << idx++ << " " << an_it_of_robot << std::endl;
}
// Reading the parameter.
string model_name("robot");
// Search for the robot util related to robot_name.
dynamicgraph::sot::RobotUtilShrPtr aRobotUtil =
dynamicgraph::sot::getRobotUtil(model_name);
std::cout << dynamicgraph::sot::RefVoidRobotUtil() << std::endl;
std::cout << aRobotUtil.get() << std::endl;
if (aRobotUtil == dynamicgraph::sot::RefVoidRobotUtil())
aRobotUtil = dynamicgraph::sot::createRobotUtil(model_name);
std::cout << aRobotUtil.get() << std::endl;
// Then build the complete robot model.
aRobotUtil->set_parameter<string>(lparameter_name,lrobot_description);
aPG.setURDFFile(aRobotURDF);
aPG.setSRDFFile(aRobotSRDF);
// Build the pattern generator
aPG.buildPGI();
aPG.buildModel();
// aPG.InitState();
aPG.pgCommandLine(":samplingperiod 0.005");
aPG.pgCommandLine(":previewcontroltime 1.6");
......
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