Commit d9679694 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[pg.cpp] Read the model from parameter-serverq

parent bfd3a805
......@@ -527,53 +527,6 @@ bool PatternGenerator::InitState(void) {
for (unsigned i = 0; i < res.size(); i++) res(i) = pos(i + 6);
// lWaistPosition(0) = 0.0;
// lWaistPosition(1) = 0.0;
// lWaistPosition(2) = 1.019272;
// lWaistPosition(3) = 0.0;
// lWaistPosition(4) = 0.0;
// lWaistPosition(5) = 0.0;
// res(18) = 0.0;
// res(19) = -0.000285;
// res(20) = -0.4113544;
// res(21) = 0.8593958;
// res(22) = -0.4480414;
// res(23) = 0.000285;
// res(24) = 0.0;
// res(25) = -0.000285;
// res(26) = -0.4113544;
// res(27) = 0.8593958;
// res(28) = -0.4480414;
// res(29) = 0.000285;
// res(30) = 0.0;
// res(31) = 0.006761;
// res(0) = 0.0;
// res(1) = 0.0;
// res(2) = 0.0;
// res(3) = 0.0;
// res(4) = 0.0;
// res(5) = 0.0;
// res(6) = 0.1;
// res(14) = 0.05;
// res(7) = 0.0;
// res(8) = 0.0;
// res(9) = 0.0;
// res(10) = 0.0;
// res(11) = 0.0;
// res(12) = 0.0;
// res(13) = 0.1;
// res(15) = 0.05;
// res(16) = 0.0;
// res(17) = 0.0;
Vector lZMPPrevious = ZMPPreviousControllerSIN(m_LocalTime);
for (unsigned int i = 0; i < 3; i++) m_ZMPPrevious[i] = lZMPPrevious(i);
} else {
......@@ -697,9 +650,36 @@ bool PatternGenerator::InitState(void) {
bool PatternGenerator::buildModel(void) {
bool ok = true;
// Parsing the file.
pinocchio::urdf::buildModel(m_urdfFile, pinocchio::JointModelFreeFlyer(),
m_robotModel);
// Name of the parameter
std::string lparameter_name("robot_description");
// Model of the robot inside a string.
std::string lrobot_description;
// Reading the parameter.
std::string model_name("robot");
// Search for the robot util related to robot_name.
sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);
// If does not exist then it is created.
if (aRobotUtil == sot::RefVoidRobotUtil())
{
ostringstream oss;
oss << __FILE__
<< " PatternGenerator::buildModel "
<< "The robot with name " << model_name
<< " was not found !";
throw std::invalid_arguments(oss.str());
ok=false;
return false;
}
// Then set the robot model.
aRobotUtil->get_parameter(parameter_name,lrobot_description);
pinocchio::urdf::buildModelFromXML(lrobot_description,
pinocchio::JointModelFreeFlyer(),
m_robotModel);
m_robotData = new pinocchio::Data(m_robotModel);
// Creating the humanoid robot.
......
Markdown is supported
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