From 2548f17abbabb7ce184e387b8c751b81d9ae5293 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Thu, 21 Dec 2017 11:56:07 +0100
Subject: [PATCH] getEffectorTrajectory now return a list of curves waypoints
(a list of waypoints for each curves)
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 7 ++-
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 9 +++-
src/rbprmbuilder.impl.cc | 61 +++++++++++++++-------
src/rbprmbuilder.impl.hh | 2 +-
4 files changed, 54 insertions(+), 25 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 3d63f56..b4052b4 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -22,8 +22,10 @@
module hpp
{
+ typedef sequence<floatSeqSeq> floatSeqSeqSeq;
module corbaserver {
module rbprm {
+
interface RbprmBuilder
{
/// Create a Device for the ROM of the robot
@@ -680,9 +682,10 @@ module hpp
/// Return the waypoints of the bezier curve for a given pathIndex and effector name
/// \param pathId : index of the path, the same index as the wholeBody path stored in problem-solver
/// \param effectorName : the name of the desired effector (Joint name)
- /// \return the waypoints of the bezier curve followed by this end effector
+ /// \return A list with the length of the curve at the first index
+ /// and then the waypoints of the bezier curve followed by this end effector
/// Throw an error if there is no trajectory computed for the given id/name
- floatSeqSeq getEffectorTrajectoryWaypoints(in unsigned short pathId,in string effectorName) raises (Error);
+ floatSeqSeqSeq getEffectorTrajectoryWaypoints(in unsigned short pathId,in string effectorName) raises (Error);
}; // interface Robot
}; // module rbprm
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 286dfe9..2d16647 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -1014,8 +1014,13 @@ class FullBody (object):
# Throw an error if there is no trajectory computed for the given id/name
def getEffectorTrajectory(self,pathId,effectorName):
wp = self.client.rbprm.rbprm.getEffectorTrajectoryWaypoints(pathId,effectorName)
- waypoints=matrix(wp).transpose()
- return bezier(waypoints)
+ if len(wp[0]) == 1: # length is provided :
+ waypoints = matrix(wp[1:]).transpose()
+ curve = bezier(waypoints,wp[0][0])
+ else :
+ waypoints=matrix(wp).transpose()
+ curve = bezier(waypoints)
+ return curve
## return the contacts variation between two states
# \param stateIdFrom : index of the first state
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 0bd3ce0..35c6e0d 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -17,7 +17,7 @@
//#include <hpp/fcl/math/transform.h>
#include <hpp/util/debug.hh>
-#include "hpp/corbaserver/rbprm/rbprmbuilder.hh"
+#include <hpp/corbaserver/rbprm/rbprmbuilder.hh>
#include "rbprmbuilder.impl.hh"
#include "hpp/rbprm/rbprm-device.hh"
#include "hpp/rbprm/rbprm-validation.hh"
@@ -72,6 +72,18 @@ namespace hpp {
// NOTHING
}
+
+ hpp::floatSeq vectorToFloatseq (const hpp::core::vector_t& input)
+ {
+ CORBA::ULong size = (CORBA::ULong) input.size ();
+ double* dofArray = hpp::floatSeq::allocbuf(size);
+ hpp::floatSeq floats (size, size, dofArray, true);
+ for (std::size_t i=0; i<size; ++i) {
+ dofArray [i] = input [i];
+ }
+ return floats;
+ }
+
void RbprmBuilder::loadRobotRomModel(const char* robotName,
const char* rootJointType,
const char* packageName,
@@ -1461,31 +1473,40 @@ namespace hpp {
}
}
- hpp::floatSeqSeq* RbprmBuilder::getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error){
+ hpp::floatSeqSeqSeq* RbprmBuilder::getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error){
try{
if(!fullBodyLoaded_)
throw std::runtime_error ("No Fullbody loaded");
- bezier_Ptr curve;
- if(! fullBody()->getEffectorTrajectory(pathId,effectorName,curve))
+ std::vector<bezier_Ptr> curvesVector;
+ if(! fullBody()->getEffectorTrajectory(pathId,effectorName,curvesVector))
throw std::runtime_error ("There is no trajectory stored for path Id = "+ boost::lexical_cast<std::string>(pathId) +" and end effector name = "+std::string(effectorName));
- const bezier_t::t_point_t waypoints = curve->waypoints();
- // build a floatSeqSeq from the waypoints :
- hpp::floatSeqSeq *res;
- res = new hpp::floatSeqSeq ();
- res->length ((_CORBA_ULong)waypoints.size());
- std::size_t i=0;
- for(bezier_t::t_point_t::const_iterator wit = waypoints.begin(); wit != waypoints.end(); ++wit,++i)
- {
- const bezier_t::point_t position = *wit;
- _CORBA_ULong size = (_CORBA_ULong) position.size ();
- double* dofArray = hpp::floatSeq::allocbuf(size);
- hpp::floatSeq floats (size, size, dofArray, true);
- //convert the config in dofseq
- for(std::size_t h = 0; h<position.size(); ++h)
+ // 3 dimensionnal array : first index is the curve, second index is the wp of the curve, third index is the coordinate of each wp
+ hpp::floatSeqSeqSeq *res;
+ res = new hpp::floatSeqSeqSeq();
+ res->length((_CORBA_ULong) curvesVector.size());
+ size_t curveId = 0;
+ for(std::vector<bezier_Ptr>::const_iterator cit=curvesVector.begin() ; cit != curvesVector.end();++cit,curveId++){
+ const bezier_t::t_point_t waypoints = (*cit)->waypoints();
+ // build a floatSeqSeq from the waypoints :
+ hpp::floatSeqSeq *curveWp;
+ curveWp = new hpp::floatSeqSeq ();
+ _CORBA_ULong size = (_CORBA_ULong)waypoints.size()+1;
+ curveWp->length (size); // +1 because the first value is the length (time)
+ { // add the time at the first index :
+ double* dofArray = hpp::floatSeq::allocbuf(1);
+ hpp::floatSeq floats (1, 1, dofArray, true);
+ dofArray[0] = (*cit)->max();
+ (*curveWp) [(_CORBA_ULong)0] = floats; // Always assume the curve start at 0. There isn't any ways to create it otherwise in python
+ }
+ // now add the waypoints :
+ std::size_t i=1;
+ for(bezier_t::t_point_t::const_iterator wit = waypoints.begin(); wit != waypoints.end(); ++wit,++i)
{
- dofArray[h] = position[h];
+
+ const bezier_t::point_t position = *wit;
+ (*curveWp) [(_CORBA_ULong)i] = vectorToFloatseq(position);
}
- (*res) [(_CORBA_ULong)i] = floats;
+ (*res)[(_CORBA_ULong)curveId] = (*curveWp);
}
return res;
}
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 39fffd9..5f48468 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -335,7 +335,7 @@ namespace hpp {
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);
- virtual hpp::floatSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
+ virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
void selectFullBody (const char* name) throw (hpp::Error)
--
GitLab