Commit 21f723b9 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[ConvexShapeContact] Fix bug in indexing of pairs of contact surfaces.

parent 574ad149
......@@ -121,8 +121,8 @@ namespace hpp {
// Store a vector of explicit relative transforms corresponding to
// each pair (floor surface, object surface)
std::vector<RelativePosePtr_t> pose_;
// Number of floor surfaces
std::size_t nFloor_;
// Number of object surfaces
std::size_t nObjSurf_;
// shared pointer to itself
ConvexShapeContactWkPtr_t weak_;
}; // class ConvexShapeContact
......
......@@ -142,7 +142,7 @@ namespace hpp {
Eigen::RowBlockIndices inputIndices(inputConf());
vector_t q(f->inputSize()); q.fill(sqrt(-1));
inputIndices.lview(q) = qin;
RelativePosePtr_t relativePose(pose_[ifloor*nFloor_ + iobject]);
RelativePosePtr_t relativePose(pose_[ifloor*nObjSurf_ + iobject]);
Eigen::RowBlockIndices relPosInputIndices(relativePose->inputConf());
vector_t qinRelPose = relPosInputIndices.rview(q);
assert(!qinRelPose.hasNaN());
......@@ -164,7 +164,7 @@ namespace hpp {
Eigen::RowBlockIndices inputIndices(inputConf());
vector_t q(f->inputSize()); q.fill(sqrt(-1));
inputIndices.lview(q) = qin;
RelativePosePtr_t relativePose(pose_[ifloor*nFloor_ + iobject]);
RelativePosePtr_t relativePose(pose_[ifloor*nObjSurf_ + iobject]);
Eigen::RowBlockIndices relPosInputIndices(relativePose->inputConf());
vector_t qinRelPose = relPosInputIndices.rview(q);
assert(!qinRelPose.hasNaN());
......@@ -202,22 +202,22 @@ namespace hpp {
(f->contactConstraint()->floorContactSurfaces());
const ConvexShapes_t& os
(f->contactConstraint()->objectContactSurfaces());
nFloor_ = fs.size();
nObjSurf_ = os.size();
// Compute explicit relative poses
for (std::size_t j=0; j<fs.size(); ++j)
for (std::size_t ifloor=0; ifloor<fs.size(); ++ifloor)
{
// move floor surface along x to take into account margin.
Transform3f posInJoint(fs[j].positionInJoint());
Transform3f posInJoint(fs[ifloor].positionInJoint());
hppDout(info, "posInJoint" << posInJoint);
posInJoint.translation() -= margin * posInJoint.rotation().col(0);
hppDout(info, "posInJoint" << posInJoint);
for (std::size_t i=0; i<os.size(); ++i)
for (std::size_t iobject=0; iobject<os.size(); ++iobject)
{
// Create explicit relative pose for each combination
// (floor surface, object surface)
pose_.push_back(RelativePose::create
("",robot, fs[j].joint_, os[i].joint_,
posInJoint, os[i].positionInJoint(),
("",robot, fs[ifloor].joint_, os[iobject].joint_,
posInJoint, os[iobject].positionInJoint(),
EqualToZero << 3 * Equality << 2 * EqualToZero,
std::vector<bool>(6, true)));
}
......
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