Commit 03006f06 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Merge remote-tracking branch 'origin/devel' into pinocchio_v2

parents a860f3bf d58c318a
......@@ -19,6 +19,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
SET(CXX_DISABLE_WERROR TRUE)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/hpp.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/ros.cmake)
INCLUDE(cmake/apple.cmake)
......@@ -27,14 +28,14 @@ INCLUDE(cmake/test.cmake)
SET(PROJECT_NAME hpp-pinocchio)
SET(PROJECT_DESCRIPTION "Wrapping of the kinematic/dynamic chain Pinocchio for HPP.")
SET(PROJECT_URL "https://github.com/humanoid-path-planner/hpp-pinocchio")
OPTION(INSTALL_DOCUMENTATION "Generate and install the documentation" ON)
OPTION(INSTALL_DOCUMENTATION "Generate and install the documentation" ON)
# Handle APPLE Cmake policy
IF(APPLE)
APPLY_DEFAULT_APPLE_CONFIGURATION()
ENDIF(APPLE)
SETUP_PROJECT()
SETUP_HPP_PROJECT()
# Activate hpp-util logging if requested
SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
......@@ -76,8 +77,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/pinocchio/joint-collection.hh
include/hpp/pinocchio/frame.hh
include/hpp/pinocchio/body.hh
include/hpp/pinocchio/fake-container.hh
include/hpp/pinocchio/device-object-vector.hh
include/hpp/pinocchio/gripper.hh
include/hpp/pinocchio/configuration.hh
include/hpp/pinocchio/collision-object.hh
......@@ -103,4 +102,4 @@ ADD_SUBDIRECTORY(tests)
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
SETUP_PROJECT_FINALIZE()
SETUP_HPP_PROJECT_FINALIZE()
Subproject commit 52d25e05c3b5dfd70c79b3e75787fdc78c6f695e
Subproject commit c396102629bb04aad462124ef5b04e140deb1f5b
......@@ -5,6 +5,4 @@ FILE_PATTERNS = *.cc *.hh *.hxx \
INPUT = @CMAKE_SOURCE_DIR@/include \
@CMAKE_SOURCE_DIR@/doc
GENERATE_TREEVIEW = NO
TAGFILES = @PINOCCHIO_DOXYGENDOCDIR@/pinocchio.doxytag=@PINOCCHIO_DOXYGENDOCDIR@
PREDEFINED = HPP_PINOCCHIO_PARSED_BY_DOXYGEN
......@@ -26,7 +26,6 @@
# include <hpp/pinocchio/config.hh>
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/device-object-vector.hh>
namespace hpp {
namespace pinocchio {
......@@ -64,9 +63,6 @@ namespace hpp {
/// \name Inner/outer objects
/// \{
/// \deprecated Use nbInnerObjects and innerObjectAt instead
const ObjectVector_t& innerObjects () const HPP_PINOCCHIO_DEPRECATED { return innerObjects_; }
/// Number of inner objects.
size_type nbInnerObjects () const;
......@@ -79,9 +75,6 @@ namespace hpp {
/// the body to the origin of the joint that holds the body.
value_type radius () const;
/// \deprecated Use nbOuterObjects and outerObjectAt instead
const ObjectVector_t& outerObjects () const HPP_PINOCCHIO_DEPRECATED { return outerObjects_; }
/// Number of outer objects.
size_type nbOuterObjects () const;
......@@ -114,7 +107,6 @@ namespace hpp {
JointIndex jointIndex;
mutable FrameIndex frameIndex; // In pinocchio, bodies are stored as frames of type BODY.
mutable bool frameIndexSet;
ObjectVector innerObjects_,outerObjects_;
}; // class Body
} // namespace pinocchio
} // namespace hpp
......
//
// Copyright (c) 2016 CNRS
// Author: NMansard from Florent Lamiraux
//
//
// 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/>.
#ifndef HPP_PINOCCHIO_OBJECT_ITERATOR_HH
#define HPP_PINOCCHIO_OBJECT_ITERATOR_HH
# include <vector>
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/config.hh>
# include <hpp/pinocchio/deprecated.hh>
# include <hpp/pinocchio/fake-container.hh>
namespace hpp {
namespace pinocchio {
/// Iterator over all inner objects of a Device.
/// \deprecated Use Device::nbObjects and Device::objectAt
struct DeviceObjectVector
: public FakeContainer<CollisionObjectPtr_t,CollisionObjectConstPtr_t>
{
DeviceObjectVector(DeviceWkPtr_t device)
: FakeContainer<CollisionObjectPtr_t,CollisionObjectConstPtr_t>(device) {}
DeviceObjectVector() {}
virtual CollisionObjectPtr_t at(const size_type i) ;
virtual CollisionObjectConstPtr_t at(const size_type i) const ;
virtual size_type size() const ;
void selfAssert(size_type i = 0) const;
} HPP_PINOCCHIO_DEPRECATED; // struct DeviceObjectVector
/* --- CONTAINER -------------------------------------------------------- */
struct ObjectVector
: public FakeContainer<CollisionObjectPtr_t,CollisionObjectConstPtr_t>
{
JointIndex jointIndex;
InOutType inOutType;
ObjectVector(DeviceWkPtr_t device,const JointIndex i, InOutType inout)
: FakeContainer<CollisionObjectPtr_t,CollisionObjectConstPtr_t>(device)
, jointIndex(i), inOutType(inout) {}
ObjectVector() {}
virtual CollisionObjectPtr_t at(const size_type i) ;
virtual CollisionObjectConstPtr_t at(const size_type i) const ;
virtual size_type size() const ;
void selfAssert(size_type i = 0) const;
private:
typedef std::vector<GeomIndex> GeomIndexList;
const GeomIndexList & geometries() const;
} HPP_PINOCCHIO_DEPRECATED;
/** Fake std::vector<Joint>, used to comply with the actual structure of hpp::model.
*
* You can use it for the following loop:
* for (JointVector_t::const_iterator it = jv.begin ();
* it != jv.end (); ++it)
* cout << (*it)->name;
*/
struct JointVector
: public FakeContainer<JointPtr_t,JointConstPtr_t>
{
JointVector(DeviceWkPtr_t device) : FakeContainer<JointPtr_t,JointConstPtr_t>(device) {}
JointVector() {}
virtual ~JointVector() {}
virtual JointPtr_t at(const size_type i) ;
virtual JointConstPtr_t at(const size_type i) const ;
virtual size_type size() const ;
virtual size_type iend() const ;
void selfAssert(size_type i = 0) const;
} HPP_PINOCCHIO_DEPRECATED;
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_OBJECT_ITERATOR_HH
//
// Copyright (c) 2016 CNRS
// Author: NMansard from Florent Lamiraux
//
//
// 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/>.
#ifndef HPP_PINOCCHIO_FAKECONTAINER_HH
# define HPP_PINOCCHIO_FAKECONTAINER_HH
# include <hpp/pinocchio/config.hh>
# include <hpp/pinocchio/fwd.hh>
namespace hpp {
namespace pinocchio {
template< typename T_,typename Tconst_ >
struct HPP_PINOCCHIO_DLLAPI FakeContainer
{
typedef T_ T;
typedef Tconst_ Tconst;
virtual ~FakeContainer() {}
virtual T at(const size_type) = 0;
virtual Tconst at(const size_type) const = 0;
virtual size_type size() const = 0;
virtual size_type ibegin() const { return 0; }
virtual size_type iend () const { return size(); }
DeviceWkPtr_t deviceWkPtr_;
FakeContainer( DeviceWkPtr_t device ) : deviceWkPtr_(device) {}
FakeContainer() : deviceWkPtr_() {}
struct iterator
{
FakeContainer & ref;
size_type idx;
iterator(FakeContainer & ref,size_type idx) : ref(ref),idx(idx) {}
iterator& operator++ () { ++idx; return *this; }
iterator operator++ (int) { iterator copy = *this; idx++; return copy; }
iterator& operator-- () { --idx; return *this; }
iterator operator-- (int) { iterator copy = *this; idx--; return copy; }
T operator* () { return ref.at(idx); }
bool operator== (const iterator & i2) { return idx==i2.idx; }
bool operator!= (const iterator & i2) { return idx!=i2.idx; };
};
struct const_iterator
{
const FakeContainer & ref;
size_type idx;
const_iterator(const FakeContainer & ref,size_type idx) : ref(ref),idx(idx) {}
const_iterator(const iterator & it) : ref(it.ref),idx(it.idx) {}
const_iterator& operator++ () { ++idx; return *this; }
const_iterator operator++ (int) { const_iterator copy = *this; idx++; return copy; }
const_iterator& operator-- () { --idx; return *this; }
const_iterator operator-- (int) { const_iterator copy = *this; idx--; return copy; }
Tconst operator* () { return ref.at(idx); }
bool operator== (const const_iterator & i2){ return idx==i2.idx; }
bool operator!= (const const_iterator & i2){ return idx!=i2.idx; }
};
iterator begin() { return iterator (*this,ibegin() ); }
iterator end() { return iterator (*this,iend() ); }
iterator rbegin() { return iterator (*this,iend()-1 ); }
iterator rend() { return iterator (*this,ibegin()-1); }
const_iterator begin() const { return const_iterator(*this,ibegin() ); }
const_iterator end() const { return const_iterator(*this,iend() ); }
const_iterator rbegin() const { return const_iterator(*this,iend()-1 ); }
const_iterator rend() const { return const_iterator(*this,ibegin()-1); }
T operator[](const size_type idx) { return at(idx); }
Tconst operator[](const size_type idx) const { return at(idx); }
DevicePtr_t device () const { assert(!deviceWkPtr_.expired()); return deviceWkPtr_.lock(); }
};
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_FAKECONTAINER_HH
......@@ -70,8 +70,7 @@ namespace hpp {
bool isFixed () const;
/// Returns the joint associated to this frame
// /// \throws std::logic_error is isFixed() returns true.
Joint joint () const;
JointPtr_t joint () const;
/// Get the parent frame (if any).
/// \warning the parent joint of the universe is the universe itself.
......@@ -85,6 +84,9 @@ namespace hpp {
return children_;
}
/// Get (constant) placement of frame in parent joint
const Transform3f& positionInParentJoint () const;
/// Get (constant) placement of frame in parent frame
Transform3f positionInParentFrame () const;
......
......@@ -128,7 +128,11 @@ namespace hpp {
/// Weak pointer to itself
GripperWkPtr_t weakPtr_;
}; // class Gripper
std::ostream& operator<< (std::ostream& os, const Gripper& gripper);
inline std::ostream& operator<< (std::ostream& os, const Gripper& gripper)
{
return gripper.print (os);
}
} // namespace pinocchio
} // namespace hpp
......
......@@ -49,6 +49,10 @@ namespace hpp {
/// \name Construction and copy and destruction
/// \{
/// Create a new joint
/// Returns a null pointer if indexInJointList is 0.
static JointPtr_t create (DeviceWkPtr_t device, JointIndex indexInJointList );
/// Constructor
/// \param device pointer on the device the joint is belonging to.
/// \param indexInJointList index of the joint, i.e. joint = device.model.joints[index]
......@@ -184,9 +188,9 @@ namespace hpp {
// -----------------------------------------------------------------------
/// Access robot owning the object
DeviceConstPtr_t robot () const { selfAssert(); return devicePtr.lock();}
DeviceConstPtr_t robot () const { return devicePtr.lock();}
/// Access robot owning the object
DevicePtr_t robot () { selfAssert(); return devicePtr.lock();}
DevicePtr_t robot () { return devicePtr.lock();}
/// \name Body linked to the joint
/// \{
......
......@@ -31,7 +31,19 @@ namespace hpp {
/// \addtogroup liegroup
/// \{
#ifdef HPP_PINOCCHIO_PARSED_BY_DOXYGEN
/// Elementary Lie groups
/// A boost variant with the following classes:
/// \li \f$\mathbf{R}^n\f$, where \f$n\f$ is either 1, 2, 3 or dynamic,
/// \li \f$\mathbf{R}^n \times SO(n) \f$, where \f$n\f$ is either 2 or 3,
/// \li \f$SO(n) \f$, where \f$n\f$ is either 2 or 3,
/// \li \f$SE(n) \f$, where \f$n\f$ is either 2 or 3.
/// \sa hpp::pinocchio::liegroup::VectorSpaceOperation,
/// hpp::pinocchio::liegroup::CartesianProductOperation,
/// hpp::pinocchio::liegroup::SpecialOrthogonalOperation,
/// hpp::pinocchio::liegroup::SpecialEuclideanOperation,
typedef ABoostVariant LiegroupType;
#else
typedef boost::variant <liegroup::VectorSpaceOperation
<Eigen::Dynamic, false>,
liegroup::VectorSpaceOperation <1, true>,
......@@ -50,6 +62,7 @@ namespace hpp {
liegroup::SpecialEuclideanOperation <2>,
liegroup::SpecialEuclideanOperation <3> >
LiegroupType;
#endif
enum DerivativeProduct {
DerivativeTimesInput,
......
......@@ -51,6 +51,7 @@ namespace hpp {
template<typename JointModel> struct operation {};
};
/// \cond
//---------------- RnxSOnLieGroupMap -------------------------------//
// JointModelRevolute, JointModelRevoluteUnbounded, JointModelRevoluteUnaligned
template<typename Scalar, int Options, int Axis>
......@@ -155,6 +156,7 @@ namespace hpp {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelPlanarTpl<Scalar, Options> > {
typedef liegroup::SpecialEuclideanOperation<2> type;
};
/// \endcond
} // namespace pinocchio
} // namespace hpp
......
......@@ -26,6 +26,7 @@
namespace hpp {
namespace pinocchio {
namespace liegroup {
/// \cond
namespace details {
template <bool Test> struct assign_if {
template <class D1, class D2> static void run(
......@@ -42,6 +43,7 @@ namespace hpp {
}
};
}
/// \endcond
template<int Size, bool rot>
struct VectorSpaceOperation : public ::pinocchio::VectorSpaceOperationTpl<Size, value_type>
......
......@@ -25,6 +25,8 @@
# include <boost/thread/mutex.hpp>
# include <boost/thread/condition_variable.hpp>
# include <hpp/pinocchio/config.hh>
namespace hpp {
namespace pinocchio {
......
......@@ -29,7 +29,6 @@ SET(LIBRARY_SOURCES
frame.cc
collision-object.cc
body.cc
device-object-vector.cc
gripper.cc
center-of-mass-computation.cc
configuration.cc
......
......@@ -36,8 +36,6 @@ namespace hpp {
Body::
Body (DeviceWkPtr_t device, JointIndex joint)
: devicePtr(device),jointIndex(joint) ,frameIndexSet(false)
, innerObjects_(device,joint,INNER)
, outerObjects_(device,joint,OUTER)
{
selfAssert();
}
......@@ -88,7 +86,7 @@ namespace hpp {
JointPtr_t Body::joint () const
{
selfAssert();
return JointPtr_t( new Joint(devicePtr,jointIndex) );
return Joint::create (devicePtr,jointIndex);
}
......
......@@ -84,13 +84,13 @@ namespace hpp {
JointPtr_t CollisionObject::joint ()
{
if (!devicePtr) return JointPtr_t();
return JointPtr_t (new Joint(devicePtr,jointIndex_));
return Joint::create (devicePtr,jointIndex_);
}
JointConstPtr_t CollisionObject::joint () const
{
if (!devicePtr) return JointConstPtr_t();
return JointConstPtr_t(new Joint(devicePtr,jointIndex_));
return Joint::create (devicePtr,jointIndex_);
}
const Transform3f& CollisionObject::
......
......@@ -131,7 +131,8 @@ namespace hpp {
const value_type& u,
ConfigurationOut_t result)
{
result = ::pinocchio::interpolate<LieGroup>(robot->model(), q0, q1, u);
const Model& model = robot->model();
result.head(model.nq) = ::pinocchio::interpolate<LieGroup>(model, q0, q1, u);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = u * q1.tail (dim) + (1-u) * q0.tail (dim);
}
......@@ -161,7 +162,8 @@ namespace hpp {
void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result)
{
result = ::pinocchio::difference<LieGroup> (robot->model(), q2, q1);
const Model& model = robot->model();
result.head(model.nv) = ::pinocchio::difference<LieGroup> (model, q2, q1);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = q1.tail (dim) - q2.tail (dim);
}
......
//
// Copyright (c) 2016 CNRS
// Author: NMansard from Florent Lamiraux
//
//
// 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/>.
# include <hpp/pinocchio/device-object-vector.hh>
# include <pinocchio/multibody/geometry.hpp>
# include <hpp/pinocchio/joint.hh>
# include <hpp/pinocchio/joint-collection.hh>
# include <hpp/pinocchio/device.hh>
# include <hpp/pinocchio/collision-object.hh>
# define UNUSED(x) (void)(x)
namespace hpp {
namespace pinocchio {
CollisionObjectPtr_t DeviceObjectVector::at(const size_type i)
{
selfAssert();
return CollisionObjectPtr_t (new CollisionObject(device(),i));
}
CollisionObjectConstPtr_t DeviceObjectVector::at(const size_type i) const
{
selfAssert();
return CollisionObjectConstPtr_t (new CollisionObject(device(),i));
}
size_type DeviceObjectVector::size() const
{ return device()->geomModel().ngeoms; }
void DeviceObjectVector::selfAssert(size_type i) const
{
assert(device());
assert(i<size());
UNUSED(i);
}
/* --- ObjectVector --------------------------------------------------------- */
CollisionObjectPtr_t ObjectVector::at(const size_type i)
{
return CollisionObjectPtr_t(new CollisionObject(device(), geometries()[i]));
}
CollisionObjectConstPtr_t ObjectVector::at(const size_type i) const
{
return CollisionObjectConstPtr_t(new CollisionObject(device(), geometries()[i]));
}
size_type ObjectVector::size() const
{
return geometries().size();
}
void ObjectVector::selfAssert(size_type i) const
{
assert(device());
assert(std::size_t(jointIndex)<device()->model().joints.size());
assert(i<size());
UNUSED(i);
}
const ObjectVector::GeomIndexList & ObjectVector::geometries() const
{
if(inOutType==INNER) return device()->geomData().innerObjects[jointIndex];
else return device()->geomData().outerObjects[jointIndex];
}
/* --- JointVector --------------------------------------------------------- */
/* Access to pinocchio index + 1 because pinocchio first joint is the universe. */
JointPtr_t JointVector::at(const size_type i)
{ selfAssert(i); return JointPtr_t(new Joint(device(),i+1)); }
/* Access to pinocchio index + 1 because pinocchio first joint is the universe. */
JointConstPtr_t JointVector::at(const size_type i) const
{ selfAssert(i); return JointConstPtr_t(new Joint(device(),i+1)); }
size_type JointVector::size() const
{ return device()->model().joints.size() - 1; }
size_type JointVector::iend() const
{ return size(); }
void JointVector::selfAssert(size_type i) const
{
assert(device());
assert(i>=ibegin());
assert(i<iend());
UNUSED(i);
}
} // namespace pinocchio
} // namespace hpp
......@@ -140,8 +140,6 @@ namespace hpp {
d_.data_ = DataPtr_t( new Data(model()) );
// We assume that model is now complete and state can be resized.
resizeState();
d_.invalidate();
numberDeviceData(datas_.size());
}
void Device::
......@@ -159,7 +157,7 @@ namespace hpp {
JointPtr_t Device::rootJoint () const
{
return JointPtr_t( new Joint(weakPtr_.lock(),1) );
return Joint::create(weakPtr_.lock(),1);
}
Frame Device::rootFrame () const
......@@ -175,7 +173,7 @@ namespace hpp {
JointPtr_t Device::jointAt (const size_type& i) const
{
assert (i < nbJoints());
return JointPtr_t(new Joint(weakPtr_.lock(),i+1));
return Joint::create(weakPtr_.lock(),i+1);
}