Commit e31d4f3d authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[parsers/srdf] parse rotor mass and gear params from srdf

parent 9243a5ca
......@@ -98,6 +98,14 @@ namespace se3
make_getter(&Model::neutralConfiguration, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::neutralConfiguration, bp::return_value_policy<bp::return_by_value>()),
"Joint's neutral configurations.")
.add_property("rotorMass",
make_getter(&Model::rotorMass, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::rotorMass, bp::return_value_policy<bp::return_by_value>()),
"Joint rotor masses.")
.add_property("rotorGearRatio",
make_getter(&Model::rotorGearRatio, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::rotorGearRatio, bp::return_value_policy<bp::return_by_value>()),
"Joint rotor gear ratios.")
.add_property("effortLimit",
make_getter(&Model::effortLimit, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::effortLimit, bp::return_value_policy<bp::return_by_value>()),
......
......@@ -130,6 +130,15 @@ namespace se3
return se3::srdf::getNeutralConfigurationFromSrdf(model, filename, verbose);
}
static bool loadRotorParamsFromSrdf(Model & model,
const std::string & filename,
bool verbose
)
{
return se3::srdf::loadRotorParamsFromSrdf(model, filename, verbose);
}
/* --- Expose --------------------------------------------------------- */
static void expose();
}; // struct ParsersPythonVisitor
......@@ -191,6 +200,9 @@ namespace se3
),
"Get the neutral configuration of a given model associated to a SRDF file");
bp::def("loadRotorParamsFromSrdf",loadRotorParamsFromSrdf,
bp::args("Model for which we are loading the rotor parameters","srdf filename (string)", "verbosity"),
"Load the rotor parameters of a given model from an SRDF file. Results are stored in model.rotorMass and model.rotorGearRatio.");
}
}
......
......@@ -62,6 +62,65 @@
<joint name="RWristYaw" value="0" />
<joint name="TrunkYaw" value="0" />
</group_state>
<rotor_params>
<joint name="HeadPitch" mass="1.0" gear_ratio="0.0" />
<joint name="HeadRoll" mass="0.0" gear_ratio="1.0" />
<joint name="LAnklePitch" mass="0.0" gear_ratio="0.0" />
<joint name="LAnkleRoll" mass="0.0" gear_ratio="0.0" />
<joint name="LElbowRoll" mass="0.0" gear_ratio="0.0" />
<joint name="LElbowYaw" mass="0.0" gear_ratio="0.0" />
<joint name="LFinger12" mass="0.0" gear_ratio="0.0" />
<joint name="LFinger13" mass="0.0" gear_ratio="0.0" />
<joint name="LFinger21" mass="0.0" gear_ratio="0.0" />
<joint name="LFinger22" mass="0.0" gear_ratio="0.0" />
<joint name="LFinger23" mass="0.0" gear_ratio="0.0" />
<joint name="LFinger31" mass="0.0" gear_ratio="0.0" />
<joint name="LFinger32" mass="0.0" gear_ratio="0.0" />
<joint name="LFinger33" mass="0.0" gear_ratio="0.0" />
<joint name="LHand" mass="0.0" gear_ratio="0.0" />
<joint name="LHipPitch" mass="0.0" gear_ratio="0.0" />
<joint name="LHipRoll" mass="0.0" gear_ratio="0.0" />
<joint name="LHipYaw" mass="0.0" gear_ratio="0.0" />
<joint name="LKneePitch" mass="0.0" gear_ratio="0.0" />
<joint name="LShoulderPitch" mass="0.0" gear_ratio="0.0" />
<joint name="LShoulderYaw" mass="0.0" gear_ratio="0.0" />
<joint name="LThumb1" mass="0.0" gear_ratio="0.0" />
<joint name="LThumb2" mass="0.0" gear_ratio="0.0" />
<joint name="LThumb3" mass="0.0" gear_ratio="0.0" />
<joint name="LWristPitch" mass="0.0" gear_ratio="0.0" />
<joint name="LWristRoll" mass="0.0" gear_ratio="0.0" />
<joint name="LWristYaw" mass="0.0" gear_ratio="0.0" />
<joint name="NeckPitch" mass="0.0" gear_ratio="0.0" />
<joint name="NeckYaw" mass="0.0" gear_ratio="0.0" />
<joint name="RAnklePitch" mass="0.0" gear_ratio="0.0" />
<joint name="RAnkleRoll" mass="0.0" gear_ratio="0.0" />
<joint name="RElbowRoll" mass="0.0" gear_ratio="0.0" />
<joint name="RElbowYaw" mass="0.0" gear_ratio="0.0" />
<joint name="RFinger12" mass="0.0" gear_ratio="0.0" />
<joint name="RFinger13" mass="0.0" gear_ratio="0.0" />
<joint name="RFinger21" mass="0.0" gear_ratio="0.0" />
<joint name="RFinger22" mass="0.0" gear_ratio="0.0" />
<joint name="RFinger23" mass="0.0" gear_ratio="0.0" />
<joint name="RFinger31" mass="0.0" gear_ratio="0.0" />
<joint name="RFinger32" mass="0.0" gear_ratio="0.0" />
<joint name="RFinger33" mass="0.0" gear_ratio="0.0" />
<joint name="RHand" mass="0.0" gear_ratio="0.0" />
<joint name="RHipPitch" mass="0.0" gear_ratio="0.0" />
<joint name="RHipRoll" mass="0.0" gear_ratio="0.0" />
<joint name="RHipYaw" mass="0.0" gear_ratio="0.0" />
<joint name="RKneePitch" mass="0.0" gear_ratio="0.0" />
<joint name="RShoulderPitch" mass="0.0" gear_ratio="0.0" />
<joint name="RShoulderYaw" mass="0.0" gear_ratio="0.0" />
<joint name="RThumb1" mass="0.0" gear_ratio="0.0" />
<joint name="RThumb2" mass="0.0" gear_ratio="0.0" />
<joint name="RThumb3" mass="0.0" gear_ratio="0.0" />
<joint name="RWristPitch" mass="0.0" gear_ratio="0.0" />
<joint name="RWristRoll" mass="0.0" gear_ratio="0.0" />
<joint name="RWristYaw" mass="0.0" gear_ratio="0.0" />
<joint name="TrunkYaw" mass="0.0" gear_ratio="0.0" />
</rotor_params>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="HeadRollLink" link2="LHipPitchLink" reason="Never" />
<disable_collisions link1="HeadRollLink" link2="LKneePitchLink" reason="Never" />
......
......@@ -78,6 +78,12 @@ namespace se3
/// \brief Vector of joint's neutral configurations
Eigen::VectorXd neutralConfiguration;
/// \brief Vector of rotor masses
Eigen::VectorXd rotorMass;
/// \brief Vector of rotor gear ratios
Eigen::VectorXd rotorGearRatio;
/// \brief Vector of maximal joint torques
Eigen::VectorXd effortLimit;
/// \brief Vector of maximal joint velocities
......
......@@ -94,6 +94,12 @@ namespace se3
neutralConfiguration.conservativeResize(nq);
neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
rotorMass.conservativeResize(nv);
rotorGearRatio.conservativeResize(nv);
rotorMass.tail(joint_model.nv()).setZero();
rotorGearRatio.tail(joint_model.nv()).setZero();
// Init and add joint index to its parent subtrees.
subtrees.push_back(IndexVector(1));
......
......@@ -141,7 +141,73 @@ namespace se3
}
#endif // ifdef WITH_HPP_FCL
bool loadRotorParamsFromSrdf(Model & model,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
{
// 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 rotor_params
if (v.first == "rotor_params")
{
// 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 rotor_mass = joint.second.get<double>("<xmlattr>.mass");
double rotor_gr = joint.second.get<double>("<xmlattr>.gear_ratio");
if (verbose)
{
std::cout << "(" << joint_name << " , " <<
rotor_mass << " , " << rotor_gr << ")" << std::endl;
}
// Search in model the joint and its config id
Model::JointIndex joint_id = model.getJointId(joint_name);
if (joint_id != model.joints.size()) // != model.njoints
{
const JointModel & joint = model.joints[joint_id];
assert(joint.nv()==1);
model.rotorMass(joint.idx_v()) = rotor_mass;
model.rotorGearRatio(joint.idx_v()) = rotor_gr; // joint with 1 dof
}
else
{
if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
}
}
}
return true;
}
}
assert(false && "no rotor params found in the srdf file");
return false; // warning : uninitialized vector is returned
}
Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
......
......@@ -71,6 +71,21 @@ namespace se3
Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
const std::string & filename,
const bool verbose = false) throw (std::invalid_argument);
///
/// \brief Load the rotor params 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 rotor parmeters
/// \param[in] filename The complete path to the SRDF file.
/// \param[in] verbose Verbosity mode.
///
/// \return Boolean whether it loads or not.
bool loadRotorParamsFromSrdf(Model & model,
const std::string & filename,
const bool verbose) throw (std::invalid_argument);
}
} // namespace se3
......
......@@ -68,5 +68,21 @@ BOOST_AUTO_TEST_CASE(readNeutralConfig)
BOOST_CHECK(q.size() == model.nq);
BOOST_CHECK(!q.isZero());
}
BOOST_AUTO_TEST_CASE(readRotorParams)
{
using namespace se3::urdf;
using namespace se3::srdf;
const string model_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
const string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/srdf/romeo_collision.srdf";
Model model;
buildModel(model_filename, model);
loadRotorParamsFromSrdf(model,srdf_filename,false);
BOOST_CHECK(model.rotorMass(model.joints[model.getJointId("HeadPitch")].idx_v())==1.0);
BOOST_CHECK(model.rotorGearRatio(model.joints[model.getJointId("HeadRoll")].idx_v())==1.0);
}
BOOST_AUTO_TEST_SUITE_END()
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