Use Explicit relative transformation in Handle::createGrasp.

src/handle.cc

@@ -33,6 +33,7 @@
 #include <hpp/constraints/relative-transformation.hh>
 
 #include <hpp/core/numerical-constraint.hh>
+#include <hpp/core/explicit-relative-transformation.hh>
 
 namespace hpp {
   namespace manipulation {
@@ -52,163 +53,17 @@ namespace hpp {
       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
-
     NumericalConstraintPtr_t Handle::createGrasp
     (const GripperPtr_t& gripper) const
     {
       using boost::assign::list_of;
+      using core::ExplicitRelativeTransformation;
       // 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);
+	return ExplicitRelativeTransformation::create
+	  ("Explicit_relative_transform_" + name () + "_" + gripper->name (),
+	   gripper->joint ()->robot (), gripper->joint (), joint (),
+	   gripper->objectPositionInJoint (), localPosition());
       }
       std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(true);
       return NumericalConstraintPtr_t