From fd4bbe70d39d1e1e491dab75904bb7c61dba701b Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Sun, 15 Sep 2019 16:36:17 +0200
Subject: [PATCH] addNewContact now take an optional argument "rotation" to
force the orientation of the end effector
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 3 ++-
src/hpp/corbaserver/rbprm/rbprmstate.py | 5 +++--
src/rbprmbuilder.impl.cc | 15 +++++++++++----
src/rbprmbuilder.impl.hh | 2 +-
4 files changed, 17 insertions(+), 8 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index e89b649b..f3c31b55 100644
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -778,8 +778,9 @@ module hpp
/// \param max_num_sample number of times it will try to randomly sample a configuration before failing
/// \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
/// This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
+ /// \param rotation : desired rotation of the end-effector in contact, expressed as Quaternion (x,y,z,w). If different from zero, the normal is ignored. Otherwise the rotation is automatically computed from the normal (with one axis of freedom)
/// \return (success,NState) whether the creation was successful, as well as the new state
- short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal, in unsigned short max_num_sample, in boolean lockOtherJoints) raises (Error);
+ short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal, in unsigned short max_num_sample, in boolean lockOtherJoints,in floatSeq rotation) raises (Error);
/// removes a contact from the state
/// if the limb is not already in contact, does nothing
diff --git a/src/hpp/corbaserver/rbprm/rbprmstate.py b/src/hpp/corbaserver/rbprm/rbprmstate.py
index 3d98d684..22ebb58f 100644
--- a/src/hpp/corbaserver/rbprm/rbprmstate.py
+++ b/src/hpp/corbaserver/rbprm/rbprmstate.py
@@ -179,11 +179,12 @@ class StateHelper(object):
# \param n 3d normal of the contact location center
# \param max_num_sample number of times it will try to randomly sample a configuration before failing
# \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
+ # \param rotation : desired rotation of the end-effector in contact, expressed as Quaternion (x,y,z,w). If different from zero, the normal is ignored. Otherwise the rotation is automatically computed from the normal (with one axis of freedom)
# This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
# \return (State, success) whether the creation was successful, as well as the new state
@staticmethod
- def addNewContact(state, limbName, p, n, num_max_sample = 0, lockOtherJoints= False):
- sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample,lockOtherJoints)
+ def addNewContact(state, limbName, p, n, num_max_sample = 0, lockOtherJoints= False,rotation = [0,0,0,0]):
+ sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample,lockOtherJoints,rotation)
if(sId != -1):
return State(state.fullBody, sId = sId), True
return state, False
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 1be3a48d..dfd38800 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -3501,7 +3501,7 @@ namespace hpp {
CORBA::Short RbprmBuilder::addNewContact(unsigned short stateId, const char* limbName,
- const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints) throw (hpp::Error)
+ const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints,const hpp::floatSeq& rotation) throw (hpp::Error)
{
try
{
@@ -3514,8 +3514,15 @@ namespace hpp {
fcl::Vec3f p; for(int i =0; i<3; ++i) p[i] = config[i];
config = dofArrayToConfig (std::size_t(3), normal);
fcl::Vec3f n; for(int i =0; i<3; ++i) n[i] = config[i];
-
- projection::ProjectionReport rep = projection::projectStateToObstacle(fullBody(),limb, fullBody()->GetLimbs().at(limb), ns, n,p,lockOtherJoints);
+ fcl::Matrix3f rotationMatrix;
+ pinocchio::Configuration_t q = dofArrayToConfig(std::size_t(4), rotation);
+ if(q.isZero(1e-9)){
+ rotationMatrix = fcl::Matrix3f::Zero();
+ }else{
+ rotationMatrix = (fcl::Quaternion3f(q[3],q[0],q[1],q[2])).matrix();
+ }
+ hppDout(notice,"addNewContact, rotation = \n"<<rotationMatrix);
+ projection::ProjectionReport rep = projection::projectStateToObstacle(fullBody(),limb, fullBody()->GetLimbs().at(limb), ns, n,p,lockOtherJoints,rotationMatrix);
hppDout(notice,"Projection State to obstacle success : "<<rep.success_);
hppDout(notice,"report status : "<<rep.status_);
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
@@ -3531,7 +3538,7 @@ namespace hpp {
{
shooter->shoot(ns.configuration_);
ns.configuration_.head<7>() = head;
- rep = projection::projectStateToObstacle(fullBody(),limb, fullBody()->GetLimbs().at(limb), ns, n,p);
+ rep = projection::projectStateToObstacle(fullBody(),limb, fullBody()->GetLimbs().at(limb), ns, n,p,false,rotationMatrix);
rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
}
}
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 75f06905..7caf882f 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -363,7 +363,7 @@ namespace hpp {
virtual Names_t* getAllLimbsNames()throw (hpp::Error);
virtual CORBA::Short addNewContact(unsigned short stateId, const char* limbName,
- const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints) throw (hpp::Error);
+ const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints, const floatSeq &rotation) throw (hpp::Error);
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);
--
GitLab