added cone accessors and optimization method

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -225,6 +225,32 @@ module hpp
     /// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
     floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error);
 
+
+    /// returns the CWC of the robot at a given state
+    ///
+    /// \param stateId The considered state
+    /// \return H matrix, such that H w <= h. h is added as the last column
+    floatSeqSeq getContactCone(in unsigned short stateId) raises (Error);
+
+    /// returns the CWC of the robot between two states
+    ///
+    /// \param stateId The considered state
+    /// \return H matrix, such that H w <= h. h is added as the last column
+    floatSeqSeq getContactIntermediateCone(in unsigned short stateId) raises (Error);
+
+    /// Create a path for the root given
+    /// a set of 3d key points
+    /// The path is composed of n+1 linear interpolations
+    /// between the n positions.
+    /// The rotation is linearly interpolated as well,
+    /// between a start and a goal rotation. The resulting path
+    /// is added to the problem solver
+    /// \param positions array of positions
+    /// \param q1 quaternion of 1st rotation
+    /// \param q2 quaternion of 2nd rotation
+    void generateRootPath(in floatSeqSeq rootPositions, in floatSeq q1, in floatSeq q2) raises (Error);
+
+
     /// Provided a path has already been computed and interpolated, generate a continuous path
     /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
     /// Will fail if the index of the states do not exist

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -19,6 +19,7 @@
 from hpp.corbaserver.rbprm import Client as RbprmClient
 from hpp.corbaserver import Client as BasicClient
 import hpp.gepetto.blender.exportmotion as em
+from numpy import array
 
 ## Corba clients to the various servers
 #
@@ -234,13 +235,11 @@ class FullBody (object):
     def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0):
 		return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold)
 	
-	## Discretizes a path return by a motion planner into a discrete
-	# sequence of balanced, contact configurations and returns
-	# the sequence as an array of configurations
-	#
-    # \param stateId id of the first state
-    # \param pathId Id of the path to compute from
-    # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
+	## Provided a discrete contact sequence has already been computed, computes
+    # all the contact positions and normals for a given state, the next one, and the intermediate between them.
+    #
+    # \param stateId normalized step for generation along the path (ie the path has a length of 1).
+    # \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     def computeContactPoints(self, stateId):
 		rawdata = self.client.rbprm.rbprm.computeContactPoints(stateId)
 		return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
@@ -254,6 +253,51 @@ class FullBody (object):
     def interpolateConfigs(self, configs):
 		return self.client.rbprm.rbprm.interpolateConfigs(configs)
 		
+	## returns the CWC of the robot at a given state
+    #
+    # \param stateId The considered state
+    # \return H matrix and h column, such that H w <= h
+    def getContactCone(self, stateId):
+		H_h =  array(self.client.rbprm.rbprm.getContactCone(stateId))
+		# now decompose cone 
+		return H_h[:,:-1], H_h[:, -1]
+		
+	## returns the CWC of the robot  between two states
+    #
+    # \param stateId The first considered state
+    # \return H matrix and h column, such that H w <= h
+    def getContactIntermediateCone(self, stateId):
+		H_h =  array(self.client.rbprm.rbprm.getContactIntermediateCone(stateId))
+		# now decompose cone 
+		return H_h[:,:-1], H_h[:, -1]
+		
+	## Create a path for the root given
+    #  a set of 3d key points
+    #  The path is composed of n+1 linear interpolations
+    #  between the n positions.
+    #  The rotation is linearly interpolated as well,
+    #  between a start and a goal rotation. The resulting path
+    #  is added to the problem solver
+    #  \param positions array of positions
+    #  \param quat_1 quaternion of 1st rotation
+    #  \param quat_2 quaternion of 2nd rotation
+    def generateRootPath(self, positions, quat_1, quat_2):
+		return self.client.rbprm.rbprm.generateRootPath(positions, quat_1, quat_2)
+		
+	## Create a path for the root given
+    #  a set of 3d key points
+    #  The path is composed of n+1 linear interpolations
+    #  between the n positions.
+    #  The rotation is linearly interpolated as well,
+    #  between a start and a goal configuration. Assuming the robot is a
+    #  free-flyer, rotations are extracted automatically. The resulting path
+    #  is added to the problem solver
+    #  \param positions array of positions
+    #  \param configState1 configuration of 1st rotation
+    #  \param configState2 configuration of 2nd rotation
+    def generateRootPathStates(self, positions, configState1, configState2):
+		return self.client.rbprm.rbprm.generateRootPath(positions, configState1[3:7], configState2[3:7])
+		
 	## Given start and goal states
 	#  Provided a path has already been computed and interpolated, generate a continuous path
     #  between two indicated states. Will fail if the index of the states do not exist

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

@@ -4,23 +4,30 @@ import numpy as np
 def __get_com(robot, config):
 	save = robot.getCurrentConfig()
 	robot.setCurrentConfig(config)
-	com = robot.getCenterOfMass()	
+	com = robot.getCenterOfMass()
+	com = config[0:3]	 #assimilate root and com
 	robot.setCurrentConfig(save)
 	return com
 
-def gen_trajectory(fullBody, states, state_id, mu = 1, reduce_ineq = True):
+def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
 	init_com = __get_com(fullBody, states[state_id])
 	end_com = __get_com(fullBody, states[state_id+1])
 	p, N = fullBody.computeContactPoints(state_id)
 	mass = fullBody.getMass()
 	t_end_phases = [0]
-	[t_end_phases.append(t_end_phases[-1]+0.5) for _ in range(len(p))]
-	print t_end_phases
+	[t_end_phases.append(t_end_phases[-1]+1) for _ in range(len(p))]
 	dt = 0.1
-	return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, mu, mass, 9.81, reduce_ineq)
+	cones = None
+	if(computeCones):
+		cones = [fullBody.getContactCone(state_id)[0]]
+		if(len(p) > 1):
+			cones.append(fullBody.getContactIntermediateCone(state_id)[0])
+		if(len(p) > len(cones)):
+			cones.append(fullBody.getContactCone(state_id+1)[0])
+	return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, mu, mass, 9.81, reduce_ineq)
 
-def draw_trajectory(fullBody, states, state_id, mu = 1, reduce_ineq = True ):
-	var_final, params = gen_trajectory(fullBody, states, state_id, mu , reduce_ineq)
+def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
+	var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , reduce_ineq)
 	p, N = fullBody.computeContactPoints(state_id)
 	print var_final
 	import numpy as np
@@ -49,3 +56,4 @@ def draw_trajectory(fullBody, states, state_id, mu = 1, reduce_ineq = True ):
 	ax.set_zlabel('Z Label')
 
 	plt.show()
+	return var_final, params

src/rbprmbuilder.impl.cc

@@ -27,6 +27,7 @@
 #include "hpp/rbprm/stability/stability.hh"
 #include "hpp/rbprm/sampling/sample-db.hh"
 #include "hpp/model/urdf/util.hh"
+#include "hpp/core/straight-path.hh"
 #include <fstream>
 
 
@@ -250,15 +251,10 @@ namespace hpp {
         return cit->second[sampleId];
     }
 
-    model::Configuration_t dofArrayToConfig (const model::DevicePtr_t& robot,
+    model::Configuration_t dofArrayToConfig (const std::size_t& deviceDim,
       const hpp::floatSeq& dofArray)
     {
         std::size_t configDim = (std::size_t)dofArray.length();
-        // Get robot
-        if (!robot) {
-            throw hpp::Error ("No robot in problem solver.");
-        }
-        std::size_t deviceDim = robot->configSize ();
         // Fill dof vector with dof array.
         model::Configuration_t config; config.resize (configDim);
         for (std::size_t iDof = 0; iDof < configDim; iDof++) {
@@ -273,18 +269,30 @@ namespace hpp {
         return config;
     }
 
-    std::vector<model::Configuration_t> doubleDofArrayToConfig (const model::DevicePtr_t& robot,
+    model::Configuration_t dofArrayToConfig (const model::DevicePtr_t& robot,
+      const hpp::floatSeq& dofArray)
+    {
+        return dofArrayToConfig(robot->configSize(), dofArray);
+    }
+
+    T_Configuration doubleDofArrayToConfig (const std::size_t& deviceDim,
       const hpp::floatSeqSeq& doubleDofArray)
     {
         std::size_t configsDim = (std::size_t)doubleDofArray.length();
-        std::vector<model::Configuration_t> res;
+        T_Configuration res;
         for (_CORBA_ULong iConfig = 0; iConfig < configsDim; iConfig++)
         {
-            res.push_back(dofArrayToConfig(robot, doubleDofArray[iConfig]));
+            res.push_back(dofArrayToConfig(deviceDim, doubleDofArray[iConfig]));
         }
         return res;
     }
 
+    T_Configuration doubleDofArrayToConfig (const model::DevicePtr_t& robot,
+      const hpp::floatSeqSeq& doubleDofArray)
+    {
+        return doubleDofArrayToConfig(robot->configSize(), doubleDofArray);
+    }
+
     std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
     {
         std::vector<std::string> res;
@@ -577,7 +585,7 @@ namespace hpp {
             {
                 throw std::runtime_error ("End state not initialized, can not interpolate ");
             }
-            std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
+            T_Configuration configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
             const affMap_t &affMap = problemSolver_->map
                         <std::vector<boost::shared_ptr<model::CollisionObject> > > ();
             if (affMap.empty ())
@@ -616,6 +624,142 @@ namespace hpp {
         }
     }
 
+    hpp::floatSeqSeq* contactCone(RbPrmFullBodyPtr_t& fullBody, State& state)
+    {
+        hpp::floatSeqSeq *res;
+        res = new hpp::floatSeqSeq ();
+
+        std::pair<stability::MatrixXX, stability::VectorX> cone = stability::ComputeCentroidalCone(fullBody, state);
+        res->length ((_CORBA_ULong)cone.first.rows());
+        _CORBA_ULong size = (_CORBA_ULong) cone.first.cols()+1;
+        for(int i=0; i < cone.first.rows(); ++i)
+        {
+            double* dofArray = hpp::floatSeq::allocbuf(size);
+            hpp::floatSeq floats (size, size, dofArray, true);
+            //convert the row in dofseq
+            for (int j=0 ; j < cone.first.cols() ; ++j)
+            {
+                dofArray[j] = cone.first(i,j);
+            }
+            dofArray[size-1] =cone.second[i];
+            (*res) [(_CORBA_ULong)i] = floats;
+        }
+        return res;
+    }
+
+    hpp::floatSeqSeq* RbprmBuilder::getContactCone(unsigned short stateId) throw (hpp::Error)
+    {
+        try
+        {
+            if(lastStatesComputed_.size() <= stateId)
+            {
+                throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
+            }
+            return contactCone(fullBody_, lastStatesComputed_[stateId]);
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
+    State intermediary(const State& firstState, const State& thirdState, unsigned short& cId, bool& success)
+    {
+        success = false;
+        std::vector<std::string> breaks;
+        thirdState.contactBreaks(firstState, breaks);
+        if(breaks.size() > 1)
+        {
+            throw std::runtime_error ("too many contact breaks between states" + std::string(""+cId) +
+                                      "and " + std::string(""+(cId + 1)));
+        }
+        if(breaks.size() == 1)
+        {
+            State intermediary(firstState);
+            intermediary.RemoveContact(*breaks.begin());
+            success = true;
+        }
+        return firstState;
+    }
+
+    hpp::floatSeqSeq* RbprmBuilder::getContactIntermediateCone(unsigned short stateId) throw (hpp::Error)
+    {
+        try
+        {
+            if(lastStatesComputed_.size() <= stateId+1)
+            {
+                throw std::runtime_error ("Unexisting state " + std::string(""+(stateId+1)));
+            }
+            bool success;
+            State intermediaryState = intermediary(lastStatesComputed_[stateId], lastStatesComputed_[stateId+1],stateId,success);
+            if(!success)
+            {
+                throw std::runtime_error ("No contact breaks, hence no intermediate state from state " + std::string(""+(stateId)));
+            }
+            return contactCone(fullBody_,intermediaryState);
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
+    core::PathPtr_t makePath(DevicePtr_t device,
+                             const ProblemPtr_t& problem,
+                             model::ConfigurationIn_t q1,
+                             model::ConfigurationIn_t q2)
+    {
+        return StraightPath::create(device, q1, q2, (*problem->distance()) (q1, q2));
+    }
+
+    model::Configuration_t addRotation(CIT_Configuration& pit, const model::value_type& u,
+                              model::ConfigurationIn_t q1, model::ConfigurationIn_t q2,
+                              model::ConfigurationIn_t ref)
+    {
+        model::Configuration_t res = ref;
+        res.head(3) = *pit;
+        res.segment<4>(3) = tools::interpolate(q1, q2, u);
+        return res;
+    }
+
+    core::PathVectorPtr_t addRotations(const T_Configuration& positions,
+                                       model::ConfigurationIn_t q1, model::ConfigurationIn_t q2,
+                                       model::ConfigurationIn_t ref, DevicePtr_t device,
+                                       const ProblemPtr_t& problem)
+    {
+        core::PathVectorPtr_t res = core::PathVector::create(device->configSize(), device->numberDof());
+        model::value_type size_step = 1 /(model::value_type)(positions.size());
+        model::value_type u = 0.;
+        CIT_Configuration pit = positions.begin();
+        model::Configuration_t previous = addRotation(pit, 0., q1, q2, ref), current;
+        ++pit;
+        for(;pit != positions.end()-1; ++pit, u+=size_step)
+        {
+            current = addRotation(pit, u, q1, q2, ref);
+            res->appendPath(makePath(device,problem, previous, current));
+            previous = current;
+        }
+        // last configuration is exactly q2
+        current = addRotation(pit, 1., q1, q2, ref);
+        res->appendPath(makePath(device,problem, previous, current));
+        return res;
+    }
+
+    void RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions,
+                          const hpp::floatSeq& q1Seq, const hpp::floatSeq& q2Seq) throw (hpp::Error)
+    {
+        try
+        {
+            model::Configuration_t q1 = dofArrayToConfig(4, q1Seq), q2 = dofArrayToConfig(4, q2Seq);
+            T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
+            problemSolver_->addPath(addRotations(positions,q1,q2,fullBody_->device_->currentConfiguration(),
+                                                 fullBody_->device_,problemSolver_->problem()));
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
 
     typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
     typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Rotation;
@@ -661,18 +805,12 @@ namespace hpp {
         const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1];
         std::vector<std::vector<fcl::Vec3f> > allStates;
         allStates.push_back(computeRectangleContact(fullBody_, firstState));
-        std::vector<std::string> breaks, creations;
-        thirdState.contactBreaks(firstState, breaks);
-        if(breaks.size() > 1)
+        std::vector<std::string> creations;
+        bool success(false);
+        State intermediaryState = intermediary(firstState, thirdState, cId, success);
+        if(success)
         {
-            throw std::runtime_error ("too many contact breaks between states" + std::string(""+cId) +
-                                      "and " + std::string(""+(cId + 1)));
-        }
-        if(breaks.size() == 1)
-        {
-            State intermediary(firstState);
-            intermediary.RemoveContact(*breaks.begin());
-            allStates.push_back(computeRectangleContact(fullBody_, intermediary));
+            allStates.push_back(computeRectangleContact(fullBody_, intermediaryState));
         }
         thirdState.contactCreations(firstState, creations);
         if(creations.size() == 1)

src/rbprmbuilder.impl.hh

@@ -142,6 +142,10 @@ namespace hpp {
         virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
         virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error);
         virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId) throw (hpp::Error);
+        virtual void generateRootPath(const hpp::floatSeqSeq& rootPositions,
+                                      const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
         virtual void interpolateBetweenStates(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
         virtual void interpolateBetweenStatesFromPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
         virtual void saveComputedStates(const char* filepath) throw (hpp::Error);