From c6ca9ec9449c0b05c932d6ae1df35bc435daa889 Mon Sep 17 00:00:00 2001
From: stevet <stevetonneau@gotmail.fr>
Date: Tue, 20 Aug 2019 09:07:45 +0200
Subject: [PATCH] [Feat] add clone state method, projectroot can now take
offset, added helper method to move limbs into non contacting
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 11 ++++++-
.../sandbox/anymal_one_step/config.py | 2 +-
.../scenarios/sandbox/anymal_one_step/demo.py | 14 +++++++-
.../scenarios/sandbox/anymal_one_step/rund.sh | 6 ++++
.../sandbox/anymal_one_step/setup_one_step.py | 6 ++--
src/CMakeLists.txt | 1 +
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 12 ++++---
src/hpp/corbaserver/rbprm/rbprmstate.py | 17 ++++++++--
src/hpp/corbaserver/rbprm/state_alg.py | 10 ++++--
src/rbprmbuilder.impl.cc | 32 +++++++++++++++++--
src/rbprmbuilder.impl.hh | 5 +--
11 files changed, 98 insertions(+), 18 deletions(-)
create mode 100644 script/scenarios/sandbox/anymal_one_step/rund.sh
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index bafa1eb2..1b195340 100644
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -154,11 +154,18 @@ module hpp
/// \param com target com
double projectStateToCOM(in unsigned short stateId, in floatSeq com, in unsigned short max_num_sample) raises (Error);
+ /// Project a state into a COM
+ ///
+ /// \param stateId target state
+ /// \param com target com
+ short cloneState(in unsigned short stateId) raises (Error);
+
/// Project a state into a given root position and orientation
///
/// \param stateId target state
/// \param root the root configuration (size 7)
- double projectStateToRoot(in unsigned short stateId, in floatSeq root) raises (Error);
+ /// \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
+ double projectStateToRoot(in unsigned short stateId, in floatSeq root, in floatSeq offset) raises (Error);
/// Project a state into a COM
@@ -799,6 +806,8 @@ module hpp
floatSeqSeq getPathAsBezier(in unsigned short pathId)raises (Error);
+ boolean toggleNonContactingLimb(in string limbname)raises (Error);
+
boolean areKinematicsConstraintsVerified(in floatSeq point)raises (Error);
boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);
diff --git a/script/scenarios/sandbox/anymal_one_step/config.py b/script/scenarios/sandbox/anymal_one_step/config.py
index 78f86158..5213a32b 100644
--- a/script/scenarios/sandbox/anymal_one_step/config.py
+++ b/script/scenarios/sandbox/anymal_one_step/config.py
@@ -1,4 +1,4 @@
SCENE="multicontact/ground"
INIT_CONFIG_ROOT = [0,0,0.465,0,0,0,1]
INIT_CONFIG_WB = None
-ROOT_BOUNDS = [-20,20, -20, 20, 0.4, 0.5]
+ROOT_BOUNDS = [-20,20, -20, 20, 0.1, 0.6]
diff --git a/script/scenarios/sandbox/anymal_one_step/demo.py b/script/scenarios/sandbox/anymal_one_step/demo.py
index d220cdc6..543ce44e 100644
--- a/script/scenarios/sandbox/anymal_one_step/demo.py
+++ b/script/scenarios/sandbox/anymal_one_step/demo.py
@@ -12,13 +12,14 @@ n_goal = q_init[:7][:]
n_goal[0] += 2
n_goal[1] += 1
n_goal[3:7] = [0.,0.,0.7071,0.7071]
+#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
states, configs = fsp.goToQuasiStatic(initState,n_goal, displayGuidePath = True)
#equivalent to
# fsp.goToQuasiStatic(self, initState, n_goal, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False)
#display computed States:
-sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
+#~ sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
s = states[-1] #last state computed
@@ -29,3 +30,14 @@ viewer(s.q())
#some helpers:
s.q() # configuration associated to state
#~ initState. # configuration associated to state
+
+from hpp.corbaserver.rbprm import rbprmstate
+target = sos.fullBody.getJointPosition(sos.fullBody.prongFrontId)[:3]
+target[2] = 0.
+
+
+s = rbprmstate.StateHelper.cloneState(states[-1])[0]
+fb = sos.fullBody
+
+#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
+#~ rbprmstate.StateHelper.addNewContact(s,sos.fullBody.prongFrontId,target,[0.,0.,1.])
diff --git a/script/scenarios/sandbox/anymal_one_step/rund.sh b/script/scenarios/sandbox/anymal_one_step/rund.sh
new file mode 100644
index 00000000..afd0965d
--- /dev/null
+++ b/script/scenarios/sandbox/anymal_one_step/rund.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+
+gepetto-gui &
+ipython -i --no-confirm-exit ./$1
+
+pkill -f 'gepetto-gui'
diff --git a/script/scenarios/sandbox/anymal_one_step/setup_one_step.py b/script/scenarios/sandbox/anymal_one_step/setup_one_step.py
index 641eae3c..a36f42ff 100644
--- a/script/scenarios/sandbox/anymal_one_step/setup_one_step.py
+++ b/script/scenarios/sandbox/anymal_one_step/setup_one_step.py
@@ -27,6 +27,7 @@ rbprmBuilder.setJointBounds ("root_joint", root_bounds)
rbprmBuilder.setFilter(rbprmBuilder.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
+#~ rbprmBuilder.setAffordanceFilter("front_prong", ['Support'])
# We also bound the rotations of the torso. (z, y, x)
#~ rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
@@ -103,7 +104,8 @@ pp = PathPlayer (v)
###### WHOLE BODY INIT
-from hpp.corbaserver.rbprm.anymal import Robot
+#~ from hpp.corbaserver.rbprm.anymal import Robot
+from hpp.corbaserver.rbprm.anymal_prong import Robot
#~ from hpp.gepetto import Viewer
from tools.display_tools import *
import time
@@ -195,7 +197,7 @@ fullBody.setCurrentConfig (q_init)
v(q_init)
-v.addLandmark('anymal/base_0',0.3)
+#~ v.addLandmark('anymal/base_0',0.3)
v(q_init)
#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 07500b95..b407e930 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -93,6 +93,7 @@ INSTALL(
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/generateROMs.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/com_constraints.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/plot_analytics.py
+ ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/affordance_centroids.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 0d95a5b4..6b91394f 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -290,7 +290,7 @@ class FullBody (Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName,configuration)[0]
- cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
+ cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
return cl.setStartStateId(sId)
@@ -308,7 +308,7 @@ class FullBody (Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName,configuration)[0]
- cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
+ cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
return cl.setEndStateId(sId)
## Initialize the first state of the path interpolation
@@ -745,9 +745,10 @@ class FullBody (Robot):
# and update the state configuration.
# \param state index of first state.
# \param root : root configuration (size 7)
+ # \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
# \return whether the projection was successful
- def projectStateToRoot(self, state, root):
- return self.clientRbprm.rbprm.projectStateToRoot(state, root) > 0
+ def projectStateToRoot(self, state, root, offset = [0,0,0.]):
+ return self.clientRbprm.rbprm.projectStateToRoot(state, root, offset) > 0
## Project a given state into a given COM position
# and update the state configuration.
@@ -990,4 +991,7 @@ class FullBody (Robot):
def isDynamicallyReachableFromState(self,stateFrom,stateTo,addPathPerPhase = False,timings=[],numPointsPerPhases=5):
return self.clientRbprm.rbprm.isDynamicallyReachableFromState(stateFrom,stateTo,addPathPerPhase,timings,numPointsPerPhases)
+
+ def toggleNonContactingLimb(self,limbName):
+ return self.clientRbprm.rbprm.toggleNonContactingLimb(limbName)
diff --git a/src/hpp/corbaserver/rbprm/rbprmstate.py b/src/hpp/corbaserver/rbprm/rbprmstate.py
index 6c8dce9f..3d98d684 100644
--- a/src/hpp/corbaserver/rbprm/rbprmstate.py
+++ b/src/hpp/corbaserver/rbprm/rbprmstate.py
@@ -141,8 +141,14 @@ class State (object):
else:
return self.fullBody.projectStateToCOM(self.sId, targetCom,maxNumSample) > 0
- def projectToRoot(self,targetRoot):
- return self.fullBody.projectStateToRoot(self.sId,targetRoot) > 0
+ ## Project a state into a given root position and orientation
+ #
+ # \param stateId target state
+ # \param root the root configuration (size 7)
+ # \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
+ # \return Whether the projection has been successful or not
+ def projectToRoot(self,targetRoot, offset = [0., 0., 0.]):
+ return self.fullBody.projectStateToRoot(self.sId,targetRoot, offset) > 0
def getComConstraint(self, limbsCOMConstraints, exceptList = []):
return get_com_constraint(self.fullBody, self.sId, self.cl.getConfigAtState(self.sId), limbsCOMConstraints, interm = self.isIntermediate, exceptList = exceptList)
@@ -181,6 +187,13 @@ class StateHelper(object):
if(sId != -1):
return State(state.fullBody, sId = sId), True
return state, False
+
+ @staticmethod
+ def cloneState(state):
+ sId = state.cl.cloneState(state.sId)
+ if(sId != -1):
+ return State(state.fullBody, sId = sId), True
+ return state, False
## tries to remove a new contact from a state
## if the limb is already in contact, replace the
diff --git a/src/hpp/corbaserver/rbprm/state_alg.py b/src/hpp/corbaserver/rbprm/state_alg.py
index 62ef1cfb..ed640da1 100644
--- a/src/hpp/corbaserver/rbprm/state_alg.py
+++ b/src/hpp/corbaserver/rbprm/state_alg.py
@@ -18,9 +18,13 @@
from __future__ import print_function
from hpp.corbaserver.rbprm.rbprmstate import State
-from lp_find_point import find_valid_c_cwc, find_valid_c_cwc_qp, lp_ineq_4D
-from hpp.corbaserver.rbprm.tools.com_constraints import *
-from CWC_methods import compute_CWC, is_stable
+try:
+ from hpp.corbaserver.rbprm.tools.com_constraints import *
+ from CWC_methods import compute_CWC, is_stable
+ from lp_find_point import find_valid_c_cwc, find_valid_c_cwc_qp, lp_ineq_4D
+except:
+ print "WARNING: in state_alg, some optinal dependencies were not found"
+ pass
## algorithmic methods on state
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index c758c030..24e1c60f 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -841,21 +841,40 @@ namespace hpp {
return (CORBA::Short)(lastStatesComputed_.size()-1);
}
+ CORBA::Short RbprmBuilder::cloneState(unsigned short stateId) throw (hpp::Error)
+ {
+ try
+ {
+ if(lastStatesComputed_.size() <= stateId){
+ throw std::runtime_error ("Can't clone state: invalid state id : "+std::string(""+stateId)+" number of state = "+std::string(""+lastStatesComputed_.size()));
+ }
+ State newState = lastStatesComputed_[stateId];
+ lastStatesComputed_.push_back(newState);
+ lastStatesComputedTime_.push_back(std::make_pair(-1., newState));
+ return (CORBA::Short)(lastStatesComputed_.size()-1);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error)
{
pinocchio::Configuration_t com_target = dofArrayToConfig (3, com);
return projectStateToCOMEigen(stateId, com_target, max_num_sample);
}
- double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error)
+ double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root, const hpp::floatSeq& offset) throw (hpp::Error)
{
pinocchio::Configuration_t root_target = dofArrayToConfig (7, root);
+ pinocchio::Configuration_t offset_target = dofArrayToConfig (3, offset);
if(lastStatesComputed_.size() <= stateId)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
State s = lastStatesComputed_[stateId];
- projection::ProjectionReport rep = projection::projectToRootConfiguration(fullBody(),root_target,s);
+ projection::ProjectionReport rep = projection::projectToRootConfiguration(fullBody(),root_target,s,offset_target);
double success = 0.;
if(rep.success_){
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
@@ -3461,6 +3480,15 @@ namespace hpp {
return limbsNames;
}
+ bool RbprmBuilder::toggleNonContactingLimb(const char* limbName)throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_){
+ throw std::runtime_error ("fullBody not loaded");
+ }
+ const std::string limb (limbName);
+ return fullBody()->toggleNonContactingLimb(limb);
+ }
+
bool RbprmBuilder::areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error){
if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded");
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 917922a0..f448602a 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -339,7 +339,8 @@ namespace hpp {
double projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error);
- virtual double projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error);
+ virtual CORBA::Short cloneState(unsigned short stateId) throw (hpp::Error);
+ virtual double projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root, const hpp::floatSeq& offset) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
@@ -366,7 +367,7 @@ namespace hpp {
virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
virtual hpp::floatSeqSeq* getPathAsBezier(unsigned short pathId)throw (hpp::Error);
-
+ virtual bool toggleNonContactingLimb(const char* limbName)throw (hpp::Error);
virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error);
virtual hpp::floatSeq *isReachableFromState(unsigned short stateFrom,unsigned short stateTo,const bool useIntermediateState)throw (hpp::Error);
--
GitLab