added isLimbInContact method

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -317,6 +317,17 @@ module hpp
     /// \return projected configuration
     floatSeq projectToCom(in double state, in floatSeq targetCom) raises (Error);
 
+    /// \param limb name of the limb for which the request aplies
+    /// \param state1 current state considered
+    /// \return whether the limb is in contact at this state
+    short isLimbInContact(in string limbname, in double state1) raises (Error);
+
+    /// Is limb in contact during the motion from the current state to the next one
+    /// \param limb name of the limb for which the request aplies
+    /// \param state1 current state considered
+    /// \return whether the limb is in contact at this state
+    short isLimbInContactIntermediary(in string limbname, in double state1) raises (Error);
+
     /// Saves the last computed states by the function interpolate in a filename.
     /// Raises an error if interpolate has not been called, or the file could not be opened.
     /// \param filename name of the file used to save the contacts.

src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py

@@ -35,16 +35,24 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
 	global constraintsLoaded
 	As = [];	bs = []
 	fullBody.setCurrentConfig(config)
+	contacts = []
 	for i, v in limbsCOMConstraints.iteritems():
 		if not constraintsComLoaded.has_key(i):
 			constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
+		print "inter", interm
+		print "intermed", fullBody.isLimbInContactIntermediary(i, state)
+		print "inter", fullBody.isLimbInContact(i, state)
 		contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state))
 		if contact:
 			ineq = constraintsComLoaded[i]
 			qEffector = fullBody.getJointPosition(v['effector'])
-			tr = quaternion_matrix(qEffector[3:7])
+			tr = quaternion_matrix(qEffector[3:7])			
+			tr[:3,3] = np.array(qEffector[0:3])
 			ineq_r = rotate_inequalities(ineq, tr)
 			As.append(ineq_r.A); bs.append(ineq_r.b);
+			print 'contact', v['effector']
+			contacts.append(v['effector'])
+	print 'contacts', contacts
 	return [np.vstack(As), np.hstack(bs)]
 		
 

src/rbprmbuilder.impl.cc

@@ -693,6 +693,12 @@ namespace hpp {
             }
             bool success;
             State intermediaryState = intermediary(lastStatesComputed_[stateId], lastStatesComputed_[stateId+1],stateId,success);
+            for(std::map<std::string, fcl::Vec3f>::const_iterator cit = intermediaryState.contactPositions_.begin();
+                cit != intermediaryState.contactPositions_.end(); ++cit)
+            {
+                std::cout << cit->first << std::endl;
+                std::cout << "WTF " << std::endl;
+            }
             if(!success)
             {
                 throw std::runtime_error ("No contact breaks, hence no intermediate state from state " + std::string(""+(stateId)));