From 84b6dab746ceebddf68bf6af6ab1e04819884b14 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Tue, 20 Feb 2018 16:56:44 +0100
Subject: [PATCH] [contact generation] add method generateEndEffectorBezier
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 11 +++++++++
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 11 +++++++++
src/rbprmbuilder.impl.cc | 27 ++++++++++++++++++++++
src/rbprmbuilder.impl.hh | 4 ++++
4 files changed, 53 insertions(+)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 7cfdc41..2b87bf2 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -551,6 +551,17 @@ module hpp
in unsigned short numOptimizations,
in Names_t trackedEffectors) raises (Error);
+
+
+ /// compute and add a trajectory for the end effector between the 2 states
+ /// represented as a bezier curve.
+ /// Do not check the kinematic feasability of this trajectory
+ /// \param state1 index of first state.
+ /// \param state2 index of end state.
+ /// \param rootPositions com positions to track
+ short generateEndEffectorBezier(in double state1,in double state2,
+ in unsigned short comTraj) raises (Error);
+
/// Project a given state into a given COM position
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the state does not exist.
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 848b9ae..d350b8a 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -665,6 +665,17 @@ class FullBody (object):
# \return id of the root path computed
def effectorRRTFromPath(self, state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim = 10, trackedEffectors = []):
return self.client.rbprm.rbprm.effectorRRTFromPath(state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim, trackedEffectors)
+
+
+ # compute and add a trajectory for the end effector between the 2 states
+ # represented as a bezier curve.
+ # Do not check the kinematic feasability of this trajectory
+ # \param state1 index of first state.
+ # \param state2 index of end state.
+ # \param rootPositions com positions to track
+ def generateEndEffectorBezier(self, state1, state2, comPos):
+ return self.client.rbprm.rbprm.generateEndEffectorBezier(state1, state2, comPos)
+
## Project a given state into a given COM position
# between two indicated states. The states do not need to be consecutive, but increasing in Id.
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index d4ed8d0..70043a4 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -2410,6 +2410,33 @@ assert(s2 == s1 +1);
}
}
+ CORBA::Short RbprmBuilder::generateEndEffectorBezier(double state1, double state2,
+ unsigned short cT)throw (hpp::Error){
+ try
+ {
+ hppDout(notice,"Begin generateEndEffectorBezier");
+ std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
+ hppDout(notice,"index first state = "<<s1<<" ; index second state : "<<s2);
+ if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
+ {
+ throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
+ }
+ const core::PathVectors_t& paths = problemSolver()->paths();
+ if(paths.size() -1 < cT)
+ {
+ throw std::runtime_error("in generateEndEffectorBezier, at least one com trajectory is not present in problem solver");
+ }
+ State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
+ hppDout(notice,"start generateEndEffectorBezier");
+ interpolation::generateEndEffectorBezier(fullBody(),problemSolver(),paths[cT],state1,state2);
+ return problemSolver()->paths().size()-1;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
hpp::floatSeq* RbprmBuilder::projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error)
{
try
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 428ad32..53a67ca 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -313,6 +313,10 @@ namespace hpp {
unsigned short comTraj3,
unsigned short numOptimizations,
const hpp::Names_t& trackedEffectors) throw (hpp::Error);
+
+ virtual CORBA::Short generateEndEffectorBezier(double state1, double state2,
+ unsigned short cT) throw (hpp::Error);
+
virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error);
virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
--
GitLab