addNewContact now take an optional argument "rotation" to force the orientation of the end effector

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -778,8 +778,9 @@ module hpp
     /// \param max_num_sample number of times it will try to randomly sample a configuration before failing
     /// \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
     ///                           This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
+    /// \param rotation : desired rotation of the end-effector in contact, expressed as Quaternion (x,y,z,w). If different from zero, the normal is ignored. Otherwise the rotation is automatically computed from the normal (with one axis of freedom)
     /// \return (success,NState) whether the creation was successful, as well as the new state
-    short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal, in unsigned short max_num_sample, in boolean lockOtherJoints) raises (Error);
+    short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal, in unsigned short max_num_sample, in boolean lockOtherJoints,in floatSeq rotation) raises (Error);
 
     /// removes a contact from the state
     /// if the limb is not already in contact, does nothing

src/hpp/corbaserver/rbprm/rbprmstate.py

@@ -179,11 +179,12 @@ class StateHelper(object):
     # \param n 3d normal of the contact location center
     # \param max_num_sample number of times it will try to randomly sample a configuration before failing
     # \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
+    # \param rotation : desired rotation of the end-effector in contact, expressed as Quaternion (x,y,z,w). If different from zero, the normal is ignored. Otherwise the rotation is automatically computed from the normal (with one axis of freedom)
     #                This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
     # \return (State, success) whether the creation was successful, as well as the new state
     @staticmethod
-    def addNewContact(state, limbName, p, n, num_max_sample = 0, lockOtherJoints= False):
-        sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample,lockOtherJoints)
+    def addNewContact(state, limbName, p, n, num_max_sample = 0, lockOtherJoints= False,rotation = [0,0,0,0]):
+        sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample,lockOtherJoints,rotation)
         if(sId != -1):
             return State(state.fullBody, sId = sId), True
         return state, False

src/rbprmbuilder.impl.cc

@@ -3501,7 +3501,7 @@ namespace hpp {
 
 
     CORBA::Short RbprmBuilder::addNewContact(unsigned short stateId, const char* limbName,
-                                        const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints) throw (hpp::Error)
+                                        const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints,const hpp::floatSeq& rotation) throw (hpp::Error)
     {
         try
         {
@@ -3514,8 +3514,15 @@ namespace hpp {
             fcl::Vec3f p; for(int i =0; i<3; ++i) p[i] = config[i];
             config = dofArrayToConfig (std::size_t(3), normal);
             fcl::Vec3f n; for(int i =0; i<3; ++i) n[i] = config[i];
-
-            projection::ProjectionReport rep = projection::projectStateToObstacle(fullBody(),limb, fullBody()->GetLimbs().at(limb), ns, n,p,lockOtherJoints);
+            fcl::Matrix3f rotationMatrix;
+            pinocchio::Configuration_t q = dofArrayToConfig(std::size_t(4), rotation);
+            if(q.isZero(1e-9)){
+              rotationMatrix = fcl::Matrix3f::Zero();
+            }else{
+              rotationMatrix = (fcl::Quaternion3f(q[3],q[0],q[1],q[2])).matrix();
+            }
+            hppDout(notice,"addNewContact, rotation = \n"<<rotationMatrix);
+            projection::ProjectionReport rep = projection::projectStateToObstacle(fullBody(),limb, fullBody()->GetLimbs().at(limb), ns, n,p,lockOtherJoints,rotationMatrix);
             hppDout(notice,"Projection State to obstacle success : "<<rep.success_);
             hppDout(notice,"report status : "<<rep.status_);
             ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
@@ -3531,7 +3538,7 @@ namespace hpp {
                 {
                     shooter->shoot(ns.configuration_);
                     ns.configuration_.head<7>() = head;
-                    rep = projection::projectStateToObstacle(fullBody(),limb, fullBody()->GetLimbs().at(limb), ns, n,p);
+                    rep = projection::projectStateToObstacle(fullBody(),limb, fullBody()->GetLimbs().at(limb), ns, n,p,false,rotationMatrix);
                     rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
                 }
             }

src/rbprmbuilder.impl.hh

@@ -363,7 +363,7 @@ namespace hpp {
 
         virtual Names_t* getAllLimbsNames()throw (hpp::Error);
         virtual CORBA::Short addNewContact(unsigned short stateId, const char* limbName,
-                                            const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints) throw (hpp::Error);
+                                            const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints, const floatSeq &rotation) throw (hpp::Error);
         virtual CORBA::Short removeContact(unsigned short stateId, const char* limbName) throw (hpp::Error);
         virtual hpp::floatSeq* computeTargetTransform(const char* limbName, const hpp::floatSeq& configuration, const hpp::floatSeq& p, const hpp::floatSeq& n) throw (hpp::Error);
         virtual Names_t* getEffectorsTrajectoriesNames(unsigned short pathId)throw (hpp::Error);