From cebf310560e845c9642d35e6654c9b42ad4c44f6 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 20 Oct 2015 16:43:36 +0200
Subject: [PATCH] In Handle::createGrasp, create an explicit numerical
constraint
---
include/hpp/manipulation/axial-handle.hh | 12 +-
include/hpp/manipulation/fwd.hh | 6 +
include/hpp/manipulation/handle.hh | 14 +-
src/axial-handle.cc | 62 +++++---
src/handle.cc | 171 +++++++++++++++++++++++
5 files changed, 233 insertions(+), 32 deletions(-)
diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index 12bc4dc..5f72287 100644
--- a/include/hpp/manipulation/axial-handle.hh
+++ b/include/hpp/manipulation/axial-handle.hh
@@ -50,7 +50,7 @@ namespace hpp {
/// \param grasp object containing the grasp information
/// \return the constraint of relative transformation between the handle and
/// the gripper. The rotation around x is not constrained.
- virtual DifferentiableFunctionPtr_t createGrasp
+ virtual NumericalConstraintPtr_t createGrasp
(const GripperPtr_t& gripper) const;
/// Create constraint that acts on the non-constrained axis of the
@@ -58,7 +58,7 @@ namespace hpp {
/// \param gripper object containing the gripper information
/// \return a relative orientation constraint between the handle and
/// the gripper. Only the rotation around the x-axis is constrained.
- virtual DifferentiableFunctionPtr_t createGraspComplement
+ virtual NumericalConstraintPtr_t createGraspComplement
(const GripperPtr_t& gripper) const;
/// Create constraint corresponding to a pregrasping task.
@@ -66,18 +66,20 @@ namespace hpp {
/// \return the constraint of relative transformation between the handle and
/// the gripper.
/// \note The translation along x-axis and the rotation around z-axis are not constrained.
- virtual DifferentiableFunctionPtr_t createPreGrasp
+ virtual NumericalConstraintPtr_t createPreGrasp
(const GripperPtr_t& gripper) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createPreGrasp.
/// \param gripper object containing the gripper information
/// \param shift the target value along the x-axis
+ /// \param width width of the interval of freedom of gripper along x-axis.
/// \return the constraint of relative position between the handle and
/// the gripper.
/// \note The translation along x-axis and the rotation around z-axis are constrained.
- virtual DifferentiableFunctionPtr_t createPreGraspComplement
- (const GripperPtr_t& gripper, const value_type& shift) const;
+ virtual NumericalConstraintPtr_t createPreGraspComplement
+ (const GripperPtr_t& gripper, const value_type& shift,
+ const value_type& width) const;
virtual std::ostream& print (std::ostream& os) const;
protected:
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 521b263..7b17ee9 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -101,6 +101,12 @@ namespace hpp {
typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
typedef core::ValidationReport ValidationReport;
typedef core::PathValidationReportPtr_t PathValidationReportPtr_t;
+ typedef core::matrix_t matrix_t;
+ typedef core::matrixIn_t matrixIn_t;
+ typedef core::matrixOut_t matrixOut_t;
+ typedef core::size_type size_type;
+ typedef core::value_type value_type;
+ typedef core::vector3_t vector3_t;
typedef std::list < NumericalConstraintPtr_t > NumericalConstraints_t;
typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t;
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index ee1da30..b3b4597 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/include/hpp/manipulation/handle.hh
@@ -88,14 +88,14 @@ namespace hpp {
/// \return the constraint of relative transformation between the handle and
/// the gripper.
/// \note The 6 DOFs of the relative transformation are constrained.
- virtual DifferentiableFunctionPtr_t createGrasp
+ virtual NumericalConstraintPtr_t createGrasp
(const GripperPtr_t& gripper) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createGrasp.
/// \param gripper object containing the gripper information
/// \return a constraints that is not doing anything.
- virtual DifferentiableFunctionPtr_t createGraspComplement
+ virtual NumericalConstraintPtr_t createGraspComplement
(const GripperPtr_t& gripper) const;
/// Create constraint corresponding to a pregrasping task.
@@ -104,21 +104,23 @@ namespace hpp {
/// the gripper.
/// \note Only 5 DOFs of the relative transformation between the handle and the gripper
/// are constrained. The translation along x-axis is not constrained.
- virtual DifferentiableFunctionPtr_t createPreGrasp
+ virtual NumericalConstraintPtr_t createPreGrasp
(const GripperPtr_t& gripper) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createPreGrasp.
/// \param gripper object containing the gripper information
/// \param shift the target value along the x-axis
+ /// \param width width of the interval of freedom of gripper along x-axis.
/// \return the constraint of relative position between the handle and
/// the gripper.
/// \note Only the x-axis of the relative transformation between the handle and the gripper
/// is constrained.
- virtual DifferentiableFunctionPtr_t createPreGraspComplement
- (const GripperPtr_t& gripper, const value_type& shift) const;
+ virtual NumericalConstraintPtr_t createPreGraspComplement
+ (const GripperPtr_t& gripper, const value_type& shift,
+ const value_type& width) const;
- static DifferentiableFunctionPtr_t createGrasp
+ static NumericalConstraintPtr_t createGrasp
(const GripperPtr_t& gripper,const HandlePtr_t& handle)
{
return handle->createGrasp(gripper);
diff --git a/src/axial-handle.cc b/src/axial-handle.cc
index e2cd42a..a16baef 100644
--- a/src/axial-handle.cc
+++ b/src/axial-handle.cc
@@ -28,56 +28,76 @@
#include <hpp/constraints/relative-transformation.hh>
+#include <hpp/core/numerical-constraint.hh>
+
namespace hpp {
namespace manipulation {
- DifferentiableFunctionPtr_t AxialHandle::createGrasp
+ NumericalConstraintPtr_t AxialHandle::createGrasp
(const GripperPtr_t& gripper) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
- return RelativeTransformation::create
- ("Transformation_(1,1,1,1,1,1)_" + name () + "_" + gripper->name (),
- gripper->joint()->robot(), gripper->joint (), joint (),
- gripper->objectPositionInJoint (), localPosition(), mask);
+ return NumericalConstraintPtr_t
+ (NumericalConstraint::create (RelativeTransformation::create
+ ("Transformation_(1,1,1,1,1,1)_" + name ()
+ + "_" + gripper->name (),
+ gripper->joint()->robot(),
+ gripper->joint (), joint (),
+ gripper->objectPositionInJoint (),
+ localPosition(), mask)));
}
- DifferentiableFunctionPtr_t AxialHandle::createGraspComplement
+ NumericalConstraintPtr_t AxialHandle::createGraspComplement
(const GripperPtr_t& gripper) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(false)(false)(false)(true)
(false);
- return RelativeTransformation::create
- ("Transformation_(0,0,0,0,0,0)_" + name () + "_" + gripper->name (),
- gripper->joint()->robot(), gripper->joint (), joint (),
- gripper->objectPositionInJoint (), localPosition(), mask);
+ return NumericalConstraintPtr_t
+ (NumericalConstraint::create (RelativeTransformation::create
+ ("Transformation_(0,0,0,0,0,0)_" + name ()
+ + "_" + gripper->name (),
+ gripper->joint()->robot(),
+ gripper->joint (), joint (),
+ gripper->objectPositionInJoint (),
+ localPosition(), mask)));
}
- DifferentiableFunctionPtr_t AxialHandle::createPreGrasp
+ NumericalConstraintPtr_t AxialHandle::createPreGrasp
(const GripperPtr_t& gripper) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(true)(true)(true)(true)(false);
- return RelativeTransformation::create
- ("Transformation_(0,1,1,1,1,1)_" + name () + "_" + gripper->name (),
- gripper->joint()->robot(), gripper->joint (), joint (),
- gripper->objectPositionInJoint (), localPosition(), mask);
+ return NumericalConstraintPtr_t
+ (NumericalConstraint::create (RelativeTransformation::create
+ ("Transformation_(0,1,1,1,1,1)_" + name ()
+ + "_" + gripper->name (),
+ gripper->joint()->robot(),
+ gripper->joint (), joint (),
+ gripper->objectPositionInJoint (),
+ localPosition(), mask)));
}
- DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement
- (const GripperPtr_t& gripper, const value_type& shift) const
+ NumericalConstraintPtr_t AxialHandle::createPreGraspComplement
+ (const GripperPtr_t& gripper, const value_type& shift,
+ const value_type& width) const
{
using boost::assign::list_of;
+ using core::DoubleInequality;
std::vector <bool> mask = list_of (true)(false)(false)(false)(true)
(false);
Transform3f transform (gripper->objectPositionInJoint ().getRotation (),
gripper->objectPositionInJoint ().getTranslation ()
+ fcl::Vec3f (shift,0,0));
- return RelativeTransformation::create
- ("Transformation_(1,0,0,0,0,0)_" + name () + "_" + gripper->name (),
- gripper->joint()->robot(), gripper->joint (), joint (),
- transform, localPosition(), mask);
+ return NumericalConstraintPtr_t
+ (NumericalConstraint::create (RelativeTransformation::create
+ ("Transformation_(1,0,0,0,0,0)_" + name ()
+ + "_" + gripper->name (),
+ gripper->joint()->robot(),
+ gripper->joint (), joint (),
+ transform, localPosition(), mask),
+ DoubleInequality::create (width)));
}
HandlePtr_t AxialHandle::clone () const
diff --git a/src/handle.cc b/src/handle.cc
index add5ed0..325ee0f 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -18,6 +18,8 @@
// <http://www.gnu.org/licenses/>.
#include <hpp/util/debug.hh>
+#include <hpp/model/device.hh>
+#include <hpp/core/explicit-numerical-constraint.hh>
#include <hpp/manipulation/handle.hh>
#include <boost/assign/list_of.hpp>
@@ -33,10 +35,179 @@
namespace hpp {
namespace manipulation {
+ using core::ExplicitNumericalConstraint;
+ using constraints::DifferentiableFunction;
+ using core::SizeInterval_t;
+ using core::SizeIntervals_t;
+
+ bool isHandleOnR3SO3 (const Handle& handle)
+ {
+ if ((dynamic_cast <model::JointSO3*> (handle.joint ())) &&
+ (dynamic_cast <model::JointTranslation <3>* >
+ (handle.joint ()->parentJoint ()))) {
+ return true;
+ }
+ return false;
+ }
+
+ HPP_PREDEF_CLASS (ObjectPositionWrtRobot);
+ typedef boost::shared_ptr <ObjectPositionWrtRobot>
+ ObjectPositionWrtRobotPtr_t;
+ class ObjectPositionWrtRobot : public DifferentiableFunction
+ {
+ public:
+ typedef Eigen::Matrix <value_type, 3, Eigen::Dynamic> HalfJacobian_t;
+ virtual ~ObjectPositionWrtRobot ()
+ {
+ }
+
+ static ObjectPositionWrtRobotPtr_t create (const GripperPtr_t& gripper,
+ const Handle& handle,
+ const std::string& name)
+ {
+ return ObjectPositionWrtRobotPtr_t (new ObjectPositionWrtRobot
+ (gripper, handle, name));
+ }
+ protected:
+ ObjectPositionWrtRobot (const GripperPtr_t& gripper,
+ const Handle& handle,
+ const std::string& name) :
+ DifferentiableFunction (gripper->joint ()->robot ()->configSize () - 7,
+ gripper->joint ()->robot ()->numberDof () - 6,
+ 7, 6, name),
+ robot_ (gripper->joint ()->robot ()),
+ objectJoint_ (handle.joint ()->parentJoint ()),
+ gripperJoint_ (gripper->joint ()),
+ robotConfigVar_ (),
+ robotDofVar_ (), objectInGripperJoint_ (), configuration_ ()
+ {
+ configuration_.resize (robot_->configSize ());
+ // Extract configuration parameters relative to robot without object
+ if (objectJoint_->rankInConfiguration () != 0) {
+ robotConfigVar_.push_back (SizeInterval_t
+ (0, objectJoint_->rankInConfiguration ()));
+ }
+ if (robot_->configSize () != objectJoint_->rankInConfiguration () + 7) {
+ robotConfigVar_.push_back (SizeInterval_t
+ (objectJoint_->rankInConfiguration () + 7,
+ robot_->configSize () -
+ objectJoint_->rankInConfiguration ()
+ - 7));
+ }
+ if (objectJoint_->rankInVelocity () != 0) {
+ robotDofVar_.push_back (SizeInterval_t
+ (0, objectJoint_->rankInVelocity ()));
+ }
+ if (robot_->numberDof () != objectJoint_->rankInVelocity () + 6) {
+ robotDofVar_.push_back (SizeInterval_t
+ (objectJoint_->rankInVelocity () + 6,
+ robot_->numberDof () -
+ objectJoint_->rankInVelocity () - 6));
+ }
+ // Compute position of object in gripper joint frame
+ objectInGripperJoint_ = gripper->objectPositionInJoint () *
+ inverse (handle.localPosition ());
+
+ GOcross_.setZero ();
+ Jgj_v_.resize (3, robot_->numberDof () - 6); Jgj_v_.setZero ();
+ Jgj_omega_.resize (3, robot_->numberDof () - 6); Jgj_omega_.setZero ();
+ }
+
+ void computeRobotForwardKinematics (vectorIn_t argument) const
+ {
+ // Be sure to initialize all configuration variables
+ configuration_ = robot_->neutralConfiguration ();
+ size_type index = 0;
+ for (SizeIntervals_t::const_iterator it = robotConfigVar_.begin ();
+ it != robotConfigVar_.end (); ++it) {
+ configuration_.segment (it->first, it->second) =
+ argument.segment (index, it->second);
+ index += it->second;
+ }
+ robot_->currentConfiguration (configuration_);
+ robot_->computeForwardKinematics ();
+ }
+
+ virtual void impl_compute (vectorOut_t result, vectorIn_t argument) const
+ {
+ computeRobotForwardKinematics (argument);
+ objectPosition_ = gripperJoint_->currentTransformation () *
+ objectInGripperJoint_;
+ result [0] = objectPosition_.getTranslation () [0];
+ result [1] = objectPosition_.getTranslation () [1];
+ result [2] = objectPosition_.getTranslation () [2];
+ result [3] = objectPosition_.getQuatRotation ().getW ();
+ result [4] = objectPosition_.getQuatRotation ().getX ();
+ result [5] = objectPosition_.getQuatRotation ().getY ();
+ result [6] = objectPosition_.getQuatRotation ().getZ ();
+ }
+
+ virtual void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const
+ {
+ computeRobotForwardKinematics (arg);
+ vector3_t GO (gripperJoint_->currentTransformation ().transform
+ (objectInGripperJoint_.getTranslation ()));
+ GOcross_ (0,1) = -GO [2]; GOcross_ (1,0) = GO [2];
+ GOcross_ (0,2) = GO [1]; GOcross_ (2,0) = -GO [1];
+ GOcross_ (1,2) = -GO [0]; GOcross_ (2,1) = GO [0];
+ // Fill Jacobian without columns relative to object position
+ size_type index = 0;
+ for (SizeIntervals_t::const_iterator it = robotDofVar_.begin ();
+ it != robotDofVar_.end (); ++it) {
+ Jgj_v_.block (0, index, 3, it->second) =
+ gripperJoint_->jacobian (). block (0, it->first, 3, it->second);
+ Jgj_omega_.block (0, index, 3, it->second) =
+ gripperJoint_->jacobian (). block (3, it->first, 3, it->second);
+ index += it->second;
+ }
+ jacobian.topRows <3> () = Jgj_v_ - GOcross_ * Jgj_omega_;
+ jacobian.bottomRows <3> () = Jgj_omega_;
+ }
+
+ private:
+ model::DevicePtr_t robot_;
+ // First joint of the freeflying object, should be a TranslationJoint <3>
+ JointPtr_t objectJoint_;
+ // Joint that holds the gripper
+ JointPtr_t gripperJoint_;
+ // Set of configuration variables corresponding to composite robot minus
+ // object.
+ SizeIntervals_t robotConfigVar_;
+ // Set of degrees of freedom corresponding to composite robot minus
+ // object.
+ SizeIntervals_t robotDofVar_;
+ // Position of object in gripper joint frame
+ Transform3f objectInGripperJoint_;
+ // Configuration of the composite robot
+ mutable Configuration_t configuration_;
+ // Object position
+ mutable Transform3f objectPosition_;
+ // Jacobian of gripper joint without columns relative to object
+ mutable HalfJacobian_t Jgj_v_;
+ mutable HalfJacobian_t Jgj_omega_;
+ mutable Eigen::Matrix<value_type, 3, 3> GOcross_;
+ }; // class ObjectPositionWrtRobot
+
DifferentiableFunctionPtr_t Handle::createGrasp
(const GripperPtr_t& gripper) const
{
using boost::assign::list_of;
+ // If handle is on a freeflying object, create an explicit constraint
+ if (isHandleOnR3SO3 (*this)) {
+ JointPtr_t objectJoint (joint ()->parentJoint ());
+ SizeIntervals_t outputConf, outputVelocity;
+ outputConf.push_back (SizeInterval_t
+ (objectJoint->rankInConfiguration (), 7));
+ outputVelocity.push_back (SizeInterval_t
+ (objectJoint->rankInVelocity (), 6));
+ DifferentiableFunctionPtr_t objectPositionWrtRobotConf
+ (ObjectPositionWrtRobot::create (gripper, *this,
+ "Explicit transformation_" + name ()
+ + "_" + gripper->name ()));
+ return core::ExplicitNumericalConstraint::create
+ (joint ()->robot (), objectPositionWrtRobotConf, outputConf,
+ outputVelocity);
+ }
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(true);
return RelativeTransformation::create
("Transformation_(1,1,1,1,1,1)_" + name () + "_" + gripper->name (),
--
GitLab