Unverified Commit 844f1f48 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #97 from jmirabel/updates

Add missing implementation of interpolate.
parents db21fa91 19235565
......@@ -41,7 +41,7 @@ namespace hpp {
class CenterOfMassComputation
{
public:
typedef std::vector <JointIndex> JointRootIndexes_t;
typedef std::vector <std::size_t> JointRootIndexes_t;
/// \cond
// This fixes an alignment issue of ::pinocchio::Data::hg
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......
......@@ -50,7 +50,7 @@ namespace hpp {
class Frame;
class AbstractDevice;
class DeviceSync;
class DeviceData;
struct DeviceData;
enum Request_t {COLLISION, DISTANCE};
enum InOutType { INNER, OUTER };
......
......@@ -100,7 +100,7 @@ namespace hpp {
GripperPtr_t clone () const;
virtual std::ostream& print (std::ostream& os) const;
std::ostream& print (std::ostream& os) const;
protected:
/// Constructor
......
......@@ -201,7 +201,7 @@ namespace hpp {
/// \}
/// Display joint
virtual std::ostream& display (std::ostream& os) const;
std::ostream& display (std::ostream& os) const;
/// Get configuration space of joint
LiegroupSpacePtr_t configurationSpace () const;
......
......@@ -119,7 +119,7 @@ namespace hpp {
assert (0 <= i && i < nbInnerObjects());
DevicePtr_t device = devicePtr.lock();
return CollisionObjectPtr_t(new CollisionObject(device,
device->geomData().innerObjects[jointIndex][i]));
device->geomData().innerObjects[jointIndex][(std::size_t)i]));
}
value_type Body::radius () const
......@@ -144,7 +144,7 @@ namespace hpp {
assert (0 <= i && i < nbOuterObjects());
DevicePtr_t device = devicePtr.lock();
return CollisionObjectPtr_t(new CollisionObject(device,
device->geomData().outerObjects[jointIndex][i]));
device->geomData().outerObjects[jointIndex][(std::size_t)i]));
}
} // namespace pinocchio
} // namespace hpp
......
......@@ -63,13 +63,13 @@ namespace hpp {
}
// Nullify non-subtree com and mass.
int root = 0;
for(JointIndex jid=1; std::size_t(jid)<model.joints.size(); ++jid )
std::size_t root = 0;
for(std::size_t jid=1; jid<model.joints.size(); ++jid )
{
const JointIndex& rootId = roots_[root];
const std::size_t& rootId = roots_[root];
if(jid == rootId)
{
jid = data.lastChild[rootId];
jid = (std::size_t)data.lastChild[rootId];
root ++;
}
else
......@@ -88,10 +88,10 @@ namespace hpp {
// Nasty cast below, from (u-long) size_t to int.
for( int root=int(roots_.size()-1); root>=0; --root )
{
JointIndex rootId = roots_[root];
std::size_t rootId = roots_[(std::size_t)root];
// Backward loop on descendents of joint rootId.
for( JointIndex jid = data.lastChild[rootId];jid>=rootId;--jid )
for( JointIndex jid = (JointIndex)data.lastChild[rootId];jid>=rootId;--jid )
{
if(computeJac)
Pass::run(model.joints[jid],data.joints[jid],
......@@ -111,7 +111,7 @@ namespace hpp {
// Backward loop on ancestors of joint rootId
JointIndex jid = model.parents[rootId]; // loop variable
rootId = (root>0) ? roots_[root-1] : 0; // root of previous subtree in roots_
rootId = (root>0) ? roots_[(std::size_t)(root-1)] : 0; // root of previous subtree in roots_
while (jid>rootId) // stop when meeting the next subtree
{
const JointIndex & parent = model.parents[jid];
......
......@@ -142,6 +142,12 @@ namespace hpp {
ConfigurationIn_t q1,
const value_type& u,
ConfigurationOut_t result);
template void interpolate<RnxSOnLieGroupMap> (const DevicePtr_t& robot,
ConfigurationIn_t q0,
ConfigurationIn_t q1,
const value_type& u,
ConfigurationOut_t result);
// TODO remove me. This is kept for backward compatibility
template void interpolate< ::pinocchio::LieGroupMap> (const DevicePtr_t& robot,
ConfigurationIn_t q0,
......
......@@ -67,7 +67,7 @@ namespace hpp {
, weakPtr_()
, datas_ ()
{
numberDeviceData(other.datas_.size());
numberDeviceData(other.numberDeviceData());
}
Device::~Device ()
......@@ -80,8 +80,8 @@ namespace hpp {
// Delete current device datas
datas_.clear();
// Create new device datas
std::vector<DeviceData*> datas (s);
for (size_type i = 0; i < s; ++i) datas[i] = new DeviceData (d_);
std::vector<DeviceData*> datas ((std::size_t)s);
for (std::size_t i = 0; i < (std::size_t)s; ++i) datas[i] = new DeviceData (d_);
datas_.push_back (datas.begin(), datas.end());
}
......@@ -148,7 +148,7 @@ namespace hpp {
d().geomData_ = GeomDataPtr_t( new GeomData(geomModel()) );
::pinocchio::computeBodyRadius(model(),geomModel(),geomData());
d().invalidate();
numberDeviceData(datas_.size());
numberDeviceData(numberDeviceData());
}
void Device::controlComputation (const Computation_t& flag)
......@@ -158,7 +158,7 @@ namespace hpp {
// It should be done in another function (like controlComputations)
// as it might be a desired behaviour to have different computation options
// in different DeviceData.
numberDeviceData(datas_.size());
numberDeviceData(numberDeviceData());
}
/* ---------------------------------------------------------------------- */
......@@ -183,7 +183,7 @@ namespace hpp {
JointPtr_t Device::jointAt (const size_type& i) const
{
assert (i < nbJoints());
return Joint::create(weakPtr_.lock(),i+1);
return Joint::create(weakPtr_.lock(),JointIndex(i+1));
}
JointPtr_t Device::
......@@ -276,7 +276,7 @@ namespace hpp {
d_.currentConfiguration_ = neutralConfiguration();
d_.currentVelocity_ = vector_t::Zero(numberDof());
d_.currentAcceleration_ = vector_t::Zero(numberDof());
d_.jointJacobians_.resize (model().njoints);
d_.jointJacobians_.resize ((std::size_t)model().njoints);
configSpace_ = LiegroupSpace::empty();
const Model& m (model());
......@@ -285,7 +285,7 @@ namespace hpp {
if (extraConfigSpace_.dimension() > 0)
*configSpace_ *= LiegroupSpace::create (extraConfigSpace_.dimension());
numberDeviceData(datas_.size());
numberDeviceData(numberDeviceData());
}
LiegroupSpacePtr_t Device::
......@@ -335,7 +335,7 @@ namespace hpp {
CollisionObjectPtr_t Device::objectAt (const size_type& i) const
{
assert (i < nbObjects());
return CollisionObjectPtr_t (new CollisionObject(weakPtr_.lock(),i));
return CollisionObjectPtr_t (new CollisionObject(weakPtr_.lock(),(GeomIndex)i));
}
bool Device::collisionTest (const bool stopAtFirstCollision)
......@@ -377,7 +377,7 @@ namespace hpp {
bool initializeAABB,
fcl::AABB& aabb)
{
typedef typename LieGroupTpl::operation<JointModel>::type LG_t;
typedef typename RnxSOnLieGroupMap::operation<JointModel>::type LG_t;
/*
if (LG_t::NT == 0) {
aabb.min_.setZero();
......
......@@ -33,7 +33,7 @@ namespace hpp {
ConfigurationIn_t bounds,
Configuration_t& out)
{
::hpp::pinocchio::LieGroupTpl::template operation<JointModel>::type
::hpp::pinocchio::RnxSOnLieGroupMap::template operation<JointModel>::type
::setBound(bounds,
jmodel.jointConfigSelector(out));
}
......
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