Commit fd4bbe70 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

addNewContact now take an optional argument "rotation" to force the orientation of the end effector

parent 8b4cd89d
......@@ -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
......
......@@ -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
......
......@@ -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);
}
}
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment