Unverified Commit 0edce218 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by GitHub
Browse files

Merge pull request #157 from florent-lamiraux/pr

[ConvexShapeContact] Fix bug in indexing of pairs of contact surfaces
parents 574ad149 07daea16
......@@ -162,12 +162,11 @@ namespace hpp {
///
/// Currently only supports the case when all the joints for floors are the same
/// and all the joints for objects involved are the same
/// \param robot the robot the constraints are applied on,
/// \return pair of joints whose relative pose determines the value
/// of the contact function, arranged in order of increasing joint index,
/// or a pair of empty shared pointers.
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
(DeviceConstPtr_t /*robot*/) const
{
if ((floorConvexShapes_.size() == 0) || (objectConvexShapes_.size() == 0)) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
......@@ -310,14 +309,13 @@ namespace hpp {
///
/// Currently only supports the case when all the joints for floors are the same
/// and all the joints for objects involved are the same
/// \param robot the robot the constraints are applied on,
/// \return pair of joints whose relative pose determines the value
/// of the contact function, arranged in order of increasing joint index,
/// or a pair of empty shared pointers.
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
(DeviceConstPtr_t /*robot*/) const
{
return sibling_->dependsOnRelPoseBetween(robot);
return sibling_->dependsOnRelPoseBetween(nullptr);
};
protected:
......@@ -379,14 +377,13 @@ namespace hpp {
///
/// Currently only supports the case when all the joints for floors are the same
/// and all the joints for objects involved are the same
/// \param robot the robot the constraints are applied on,
/// \return pair of joints whose relative pose determines the value
/// of the contact function, arranged in order of increasing joint index,
/// or a pair of empty shared pointers.
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
(DeviceConstPtr_t /*robot*/) const
{
return constraint_->dependsOnRelPoseBetween(robot);
return constraint_->dependsOnRelPoseBetween(nullptr);
}
protected:
......
......@@ -122,9 +122,11 @@ namespace hpp {
_f != functions_.end(); ++_f) {
const DifferentiableFunction& f = **_f;
f.impl_compute(result_ [i], arg);
assert(hpp::pinocchio::checkNormalized(result_[i]));
result.vector ().segment(row, f.outputSize()) =
result_ [i].vector ();
row += f.outputSize(); ++i;
assert(hpp::pinocchio::checkNormalized(result));
}
}
void impl_jacobian (matrixOut_t jacobian, ConfigurationIn_t arg) const
......
......@@ -194,13 +194,15 @@ namespace hpp {
///
/// This method is useful to know whether an instance of Implicit constrains
/// the relative pose between two joints.
/// \param robot the robot the constraints are applied on,
/// \return the pair of joints involved, arranged in order of increasing
/// joint index, or a pair of empty shared pointers.
/// \note if absolute pose (relative pose with respect to "universe"),
/// "universe" is returned as empty shared pointer
/// \note
/// \li if absolute pose (relative pose with respect to "universe"),
/// "universe" is returned as empty shared pointer
/// \li child class reimplementing this may require a valid "robot"
/// argument, which the constraints are applied on.
virtual std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
(DeviceConstPtr_t /*robot*/) const
{
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
};
......@@ -235,7 +237,7 @@ namespace hpp {
virtual void impl_jacobian (matrixOut_t jacobian,
vectorIn_t arg) const = 0;
virtual bool isEqual(const DifferentiableFunction& other) const
virtual bool isEqual(const DifferentiableFunction& other) const
{
if (name_ != other.name_)
return false;
......
......@@ -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
......
......@@ -95,7 +95,7 @@ namespace hpp {
const ImplicitFunction& castother = dynamic_cast<const ImplicitFunction&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (robot_ != castother.robot_)
return false;
if (inputToOutput_ != castother.inputToOutput_)
......@@ -120,6 +120,7 @@ namespace hpp {
/// \li q_out is a vector corresponding to only 1 joint
/// \li q_in is an empty vector (since f is constant and specifies
/// the whole or part of the pose of the joint)
///
/// \param robot the robot the constraints are applied on,
/// \return pair of pointers to the lock joint and its parent joint,
/// arranged in order of increasing joint index, or a pair of empty
......
......@@ -89,13 +89,12 @@ namespace hpp {
///
/// This method is useful to know whether an instance of Implicit constrains
/// the relative pose between two joints.
/// \param robot the robot the constraints are applied on,
/// \return the pair of joints involved, arranged in order of increasing
/// joint index, or a pair of empty shared pointers.
/// \note if absolute pose (relative pose with respect to "universe"),
/// "universe" is returned as empty shared pointer
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const {
(DeviceConstPtr_t /*robot*/) const {
JointConstPtr_t j1 = joint1();
JointConstPtr_t j2 = joint2();
size_type index1 = (j1? j1->index(): 0);
......@@ -151,7 +150,7 @@ namespace hpp {
const RelativeTransformation& castother = dynamic_cast<const RelativeTransformation&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (robot_ != castother.robot_)
return false;
if (parentJoint_ != castother.parentJoint_)
......@@ -174,7 +173,7 @@ namespace hpp {
return false;
if (F1inJ1_invF2inJ2_ != castother.F1inJ1_invF2inJ2_)
return false;
return true;
}
......
......@@ -128,7 +128,7 @@ namespace hpp {
/// Used to model contact surfaces for manipulation applications
typedef std::vector<vector3_t> Shape_t;
typedef std::pair <JointPtr_t, Shape_t> JointAndShape_t;
typedef std::list <JointAndShape_t> JointAndShapes_t;
typedef std::vector <JointAndShape_t> JointAndShapes_t;
typedef shared_ptr<ConvexShapeContact>
ConvexShapeContactPtr_t;
typedef shared_ptr<ConvexShapeContactComplement>
......
......@@ -305,13 +305,12 @@ namespace hpp {
///
/// This method is useful to know whether an instance of Implicit constrains
/// the relative pose between two joints.
/// \param robot the robot the constraints are applied on,
/// \return the pair of joints involved, arranged in order of increasing
/// joint index, or a pair of empty shared pointers. or a pair of empty shared pointers.
/// \note if absolute pose (relative pose with respect to "universe"),
/// "universe" is returned as empty shared pointer
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
(DeviceConstPtr_t /*robot*/) const
{
JointConstPtr_t j1 = joint1();
JointConstPtr_t j2 = joint2();
......@@ -361,7 +360,7 @@ namespace hpp {
return false;
if (mask_ != castother.mask_)
return false;
return true;
}
......
......@@ -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)));
}
......
......@@ -315,6 +315,7 @@ namespace hpp {
std::pair<JointConstPtr_t, JointConstPtr_t> ImplicitFunction::dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
{
assert (robot);
// check that this is a constant function
if (inputToOutput_->inputSize() != 0) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
......
......@@ -219,6 +219,7 @@ namespace hpp {
compute<IsRelative, (bool)ComputePosition, (bool)ComputeOrientation, (bool)OutputR3xSO3>::error (data);
result.vector() = Vindices_.rview (data.value);
assert(hpp::pinocchio::checkNormalized(result));
}
template <int _Options>
......
......@@ -319,12 +319,17 @@ namespace hpp {
template <bool compileTimeRel /* false */, bool ori /* false */> struct relativeTransform {
template <bool runtimeRel, bool pos, bool ose3> static inline void run (GTDataV<runtimeRel, pos, false, ose3>& d)
{
using hpp::pinocchio::LiegroupElement;
using hpp::pinocchio::LiegroupSpace;
// There is no joint1
const Transform3f& M2 = d.M2 ();
d.value.noalias() = M2.act (d.model.F2inJ2.translation());
assert(hpp::pinocchio::checkNormalized(LiegroupElement(d.value,LiegroupSpace::R3xSO3())));
if (!d.model.t1isZero) d.value.noalias() -= d.model.F1inJ1.translation();
assert(hpp::pinocchio::checkNormalized(LiegroupElement(d.value,LiegroupSpace::R3xSO3())));
if (!d.model.R1isID)
d.value.applyOnTheLeft(d.model.F1inJ1.rotation().transpose());
assert(hpp::pinocchio::checkNormalized(LiegroupElement(d.value,LiegroupSpace::R3xSO3())));
}
};
template <> struct relativeTransform<false, true> {
......@@ -375,8 +380,13 @@ namespace hpp {
{
static inline void error (GTDataV<rel, pos, ori, ose3>& d)
{
using hpp::pinocchio::LiegroupElement;
using hpp::pinocchio::LiegroupSpace;
relativeTransform<rel, ori>::run (d);
unary<ori>::log(d);
if (ose3)
assert(hpp::pinocchio::checkNormalized(LiegroupElement
(d.value, LiegroupSpace::R3xSO3())));
}
static inline void jacobian (GTDataJ<rel, pos, ori, ose3>& d,
......
......@@ -27,7 +27,7 @@
// DAMAGE.
#include <hpp/constraints/solver/by-substitution.hh>
#include <hpp/pinocchio/liegroup-element.hh>
#include <boost/serialization/nvp.hpp>
#include <hpp/util/serialization.hh>
......@@ -266,7 +266,9 @@ namespace hpp {
projectVectorOnKernel (from, OM_, OP_);
assert (hpp::pinocchio::checkNormalized(M));
Lge_t P (O + OP_);
assert (hpp::pinocchio::checkNormalized(P));
saturate_->saturate (P.vector (), result, saturation_);
}
......
......@@ -637,6 +637,8 @@ namespace hpp {
Data& d = datas_[i];
f.value (d.output, config);
assert (hpp::pinocchio::checkNormalized(d.output));
assert (hpp::pinocchio::checkNormalized(d.rightHandSide));
d.error = d.output - d.rightHandSide;
constraints.setInactiveRowsToZero(d.error);
if (ComputeJac) {
......
......@@ -154,15 +154,15 @@ BOOST_AUTO_TEST_CASE(convexShapeContact)
BOOST_CHECK(M > sqrt(2));
// check that the two joints involved can be retrieved correctly
std::pair<JointConstPtr_t, JointConstPtr_t> joints =
f->dependsOnRelPoseBetween(robot);
f->dependsOnRelPoseBetween(nullptr);
BOOST_CHECK_EQUAL (Joint::index(joints.first), Joint::index(j1));
BOOST_CHECK_EQUAL (Joint::index(joints.second), Joint::index(j2));
joints = f->contactConstraint()->dependsOnRelPoseBetween(robot);
joints = f->contactConstraint()->dependsOnRelPoseBetween(nullptr);
BOOST_CHECK_EQUAL (Joint::index(joints.first), Joint::index(j1));
BOOST_CHECK_EQUAL (Joint::index(joints.second), Joint::index(j2));
joints = f->complement()->dependsOnRelPoseBetween(robot);
joints = f->complement()->dependsOnRelPoseBetween(nullptr);
BOOST_CHECK_EQUAL (Joint::index(joints.first), Joint::index(j1));
BOOST_CHECK_EQUAL (Joint::index(joints.second), Joint::index(j2));
// box 1 above box 2: surfaces in contact are
......
......@@ -433,7 +433,7 @@ BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
ImplicitPtr_t constraint;
function = Orientation::create ("Orientation" , device, ee2, tf2);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
// constraint does not fully constrain the relative pose
......@@ -446,7 +446,7 @@ BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
BOOST_CHECK (!jointsConstrained.second);
function = Position::create ("Position" , device, ee2, tf2, tf1);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
// constraint does not fully constrain the relative pose
......@@ -459,7 +459,7 @@ BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
BOOST_CHECK (!jointsConstrained.second);
function = Transformation::create ("Transformation" , device, ee1, tf1);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee1->index());
// constraint does not fully constrain the relative pose
......@@ -479,7 +479,7 @@ BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
BOOST_CHECK_EQUAL (jointsConstrained.second->index(), ee1->index());
function = RelativeOrientation::create ("RelativeOrientation" , device, ee1, ee2, tf1);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK_EQUAL (joints.first->index(), ee1->index());
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
// constraint does not fully constrain the relative pose
......@@ -491,7 +491,7 @@ BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
BOOST_CHECK (!jointsConstrained.second);
function = RelativePosition::create ("RelativePosition" , device, ee1, ee2, tf1, tf2);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK_EQUAL (joints.first->index(), ee1->index());
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
// constraint does not fully constrain the relative pose
......@@ -503,7 +503,7 @@ BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
BOOST_CHECK (!jointsConstrained.second);
function = RelativeTransformation::create ("RelativeTransformation", device, ee1, ee2, tf1, tf2);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK_EQUAL (joints.first->index(), ee1->index());
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
// constraint does not fully constrain the relative pose
......@@ -523,18 +523,18 @@ BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
BOOST_CHECK_EQUAL (jointsConstrained.second->index(), ee2->index());
function = RelativeOrientation::create ("RelativeOrientation" , device, ee1, JointPtr_t(), tf1);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee1->index());
function = RelativePosition::create ("RelativePosition" , device, ee1, JointPtr_t(), tf1, tf2);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee1->index());
function = RelativeTransformationR3xSO3::create (
"RelativeTransformationR3xSO3", device, ee1, JointPtr_t(), tf1, tf2);
joints = function->dependsOnRelPoseBetween (device);
joints = function->dependsOnRelPoseBetween (nullptr);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee1->index());
// constraint fully constrains the relative pose
......
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