Commit 0474cb69 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Store URDF mimic joints in Device.

parent 0c386559
......@@ -117,6 +117,18 @@ namespace hpp {
/// \name Joints
/// \{
/// This struct defines a linear relation between two joint values.
///
/// The configuration of \c JointConstraint::joint is expected to be \f$
/// q_{joint} = multiplier * q_{reference} + offset\f$.
/// where \f$q_{reference}\f$ is the configuration of
/// JointLinearConstraint::reference joint.
struct JointLinearConstraint
{
value_type offset, multiplier;
JointPtr_t joint, reference;
};
/// Get root joint
JointPtr_t rootJoint () const;
......@@ -171,6 +183,14 @@ namespace hpp {
/// Get the neutral configuration
Configuration_t neutralConfiguration () const;
/// Add a joint constraint
void addJointConstraint (JointLinearConstraint constraint);
const std::vector<JointLinearConstraint>& jointConstraints () const
{
return jointConstraints_;
}
/// \}
// -----------------------------------------------------------------------
/// \name Extra configuration space
......@@ -313,6 +333,8 @@ namespace hpp {
LiegroupSpacePtr_t configSpace_;
// Extra configuration space
ExtraConfigSpace extraConfigSpace_;
// Joint linear constraints
std::vector<JointLinearConstraint> jointConstraints_;
DeviceWkPtr_t weakPtr_;
private:
......
......@@ -310,12 +310,29 @@ namespace hpp {
return n;
}
void Device::
addJointConstraint (JointLinearConstraint constraint)
{
assert (constraint.joint && constraint.reference);
assert (constraint.joint ->configSize() == 1);
assert (constraint.reference->configSize() == 1);
jointConstraints_.push_back (constraint);
}
std::ostream& Device::
print (std::ostream& os) const
{
for (size_type i = 0; i < nbJoints(); ++i)
jointAt(i)->display(os);
return os;
os << model() << iendl;
if (jointConstraints_.size()>0)
os << "Joint linear constraints:" << incindent;
for (std::size_t i = 0; i < jointConstraints_.size(); ++i)
os << iendl
<< jointConstraints_[i].joint->name() << " = "
<< jointConstraints_[i].multiplier << " * "
<< jointConstraints_[i].reference->name() << " + "
<< jointConstraints_[i].offset;
return os << decindent << std::endl;
}
/* ---------------------------------------------------------------------- */
......
......@@ -471,32 +471,6 @@ namespace hpp {
os << "Anchor joint\\n";
os << "Current transformation: " << currentTransformation();
os << "\\n";
/*hpp::model::BodyPtr_t body = linkedBody();
if (body) {
const matrix3_t& I = body->inertiaMatrix();
os << "Attached body: " << body->name () << "\\n";
os << "Mass of the attached body: " << body->mass() << "\\n";
os << "Local center of mass:" << body->localCenterOfMass() << "\\n";
os << "Inertia matrix:" << "\\n";
os << I (0,0) << "\t" << I (0,1) << "\t" << I (0,2) << "\\n"
<< I (1,0) << "\t" << I (1,1) << "\t" << I (1,2) << "\\n"
<< I (2,0) << "\t" << I (2,1) << "\t" << I (2,2) << "\\n";
os << "geometric objects" << "\\n";
const hpp::model::ObjectVector_t& colObjects =
body->innerObjects (hpp::model::COLLISION);
for (hpp::model::ObjectVector_t::const_iterator it =
colObjects.begin (); it != colObjects.end (); ++it) {
os << "name: " << (*it)->name () << "\\n";
os << "position in joint:" << "\\n";
const fcl::Transform3f& local ((*it)->positionInJointFrame ());
displayTransform3f (os, local); os << "\\n";
os << "position :" << "\\n";
const fcl::Transform3f& global ((*it)->fcl ()->getTransform ());
displayTransform3f (os, global);
}
} else {
os << "No body";
}*/
os << "\"]" << std::endl;
for (unsigned int iChild=0; iChild < numberChildJoints (); iChild++)
{
......
......@@ -258,6 +258,21 @@ namespace hpp {
robot->createData();
robot->createGeomData();
// Build mimic joint table.
typedef std::map<std::string, PINOCCHIO_URDF_SHARED_PTR(::urdf::Joint) > UrdfJointMap_t;
for (UrdfJointMap_t::const_iterator _joint = urdfTree->joints_.begin();
_joint != urdfTree->joints_.end(); ++_joint) {
const PINOCCHIO_URDF_SHARED_PTR(::urdf::Joint)& joint = _joint->second;
if (joint && joint->mimic) {
Device::JointLinearConstraint constraint;
constraint.joint = robot->getJointByName (prefix + joint->name);
constraint.reference = robot->getJointByName (prefix + joint->mimic->joint_name);
constraint.multiplier = joint->mimic->multiplier;
constraint.offset = joint->mimic->offset;
robot->addJointConstraint (constraint);
}
}
hppDout (notice, "Finished parsing SRDF file.");
}
......
......@@ -61,6 +61,7 @@ ENDIF(ROMEO_DESCRIPTION_FOUND)
ADD_TESTCASE(tconfiguration FALSE)
ADD_TESTCASE(device FALSE)
ADD_TESTCASE(urdf FALSE)
ADD_TESTCASE(liegroup-element FALSE)
ADD_TESTCASE(print FALSE)
ADD_TESTCASE(joint-bounds FALSE)
......
// Copyright (c) 2019, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.
#define BOOST_TEST_MODULE urdf
#include <boost/test/unit_test.hpp>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/urdf/util.hh>
using namespace hpp::pinocchio;
BOOST_AUTO_TEST_CASE (mimic_joint)
{
std::string urdf (
"<robot name='test'>"
"<link name='base_link'/>"
"<link name='link1'/>"
"<joint name='joint1' type='revolute'>"
" <parent link='base_link'/>"
" <child link='link1'/>"
" <limit effort='30' velocity='1.0' />"
"</joint>"
"<link name='link2'/>"
"<joint name='joint2' type='revolute'>"
" <parent link='base_link'/>"
" <child link='link2'/>"
" <mimic joint='joint1' multiplier='-1.' offset='0.5' />"
" <limit effort='30' velocity='1.0' />"
"</joint>"
"</robot>");
DevicePtr_t robot = Device::create ("test");
urdf::loadModelFromString (robot, 0, "test", "anchor", urdf, "");
BOOST_REQUIRE(robot);
BOOST_REQUIRE_EQUAL (robot->jointConstraints().size(), 1);
BOOST_CHECK_EQUAL (robot->jointConstraints()[0].multiplier, -1.);
BOOST_CHECK_EQUAL (robot->jointConstraints()[0].offset , 0.5);
}
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