diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index ff6f9e7d83fd9d96d64568058c960c591a5a02ce..ee084b67ce39026de33b7dfe6f45dbe7d78fcc62 100755 --- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl +++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl @@ -260,7 +260,8 @@ module hpp void addLimb(in string id, in string limb, in string effector, in floatSeq offset, in floatSeq normal, in double x, in double y, in unsigned long samples, in string heuristicName, in double resolution, in string contactType, in double disableEffectorCollision, - in double grasp,in floatSeq limbOffset) raises (Error); + in double grasp,in floatSeq limbOffset, + in string kinematicConstraintsPath, in double kinematicConstraintsMin) raises (Error); /// Add a limb not used for contact generation /// \param id user given name of the new limb diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py index fa143965ca58477838274df69f851aca252035c2..12e2d50cbc0cda603a42551d4f28823b2d0b1c93 100755 --- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py +++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py @@ -176,14 +176,17 @@ class FullBody (object): # This can be problematic in terms of performance. The default value is 3 cm. # \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF") # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones - def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False, grasp =False,limbOffset=[0,0,0]): + # \param kinematicConstraintsPath : path that point to the .obj file containing the kinematic constraints of the limb, + # if not set the default is "package://hpp-rbprm-corba/com_inequalities/"+name+"_com_constraints.obj" + # \param kinematicConstraintsMin : add an additionnal inequalities on the kinematicConstraints, of normal (0,0,1) and origin (0,0,kinematicConstraintsMin) + def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False, grasp =False,limbOffset=[0,0,0],kinematicConstraintsPath = "", kinematicConstraintsMin = 0.): boolValEff = 0. if(disableEffectorCollision): boolValEff = 1. graspv = 0. if(grasp): graspv = 1. - self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff,graspv,limbOffset) + self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff,graspv,limbOffset,kinematicConstraintsPath,kinematicConstraintsMin) self.limbNames += [limbId] ## Returns the configuration of a limb described by a sample diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index ed17baec382a5555ede17e87f7436cfc4faf1aa2..606a81a4947bbff2a8868cfd97b9e515270dab5a 100755 --- a/src/rbprmbuilder.impl.cc +++ b/src/rbprmbuilder.impl.cc @@ -1139,7 +1139,7 @@ namespace hpp { } void RbprmBuilder::addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y, - unsigned int samples, const char* heuristicName, double resolution, const char *contactType, double disableEffectorCollision, double grasp, const hpp::floatSeq &limbOffset) throw (hpp::Error) + unsigned int samples, const char* heuristicName, double resolution, const char *contactType, double disableEffectorCollision, double grasp, const hpp::floatSeq &limbOffset, const char *kinematicConstraintsPath, double kinematicConstraintsMin) throw (hpp::Error) { if(!fullBodyLoaded_) throw Error ("No full body robot was loaded"); @@ -1159,7 +1159,7 @@ namespace hpp { cType = hpp::rbprm::_3_DOF; } fullBody()->AddLimb(std::string(id), std::string(limb), std::string(effector), off,limbOff, norm, x, y, - problemSolver()->collisionObstacles(), samples,heuristicName,resolution,cType,disableEffectorCollision > 0.5, grasp > 0.5); + problemSolver()->collisionObstacles(), samples,heuristicName,resolution,cType,disableEffectorCollision > 0.5, grasp > 0.5,kinematicConstraintsPath,kinematicConstraintsMin); } catch(std::runtime_error& e) { diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh index 7e0a3bad6dcbe1f962dbb11d6965eaa95d55ccb3..c2f96172006c1ecd877e8673291414082d3ba0a0 100755 --- a/src/rbprmbuilder.impl.hh +++ b/src/rbprmbuilder.impl.hh @@ -244,7 +244,7 @@ namespace hpp { virtual void addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y, unsigned int samples, const char *heuristicName, double resolution, const char *contactType, - double disableEffectorCollision, double grasp,const hpp::floatSeq& limbOffset) throw (hpp::Error); + double disableEffectorCollision, double grasp,const hpp::floatSeq& limbOffset,const char* kinematicConstraintsPath, double kinematicConstraintsMin) throw (hpp::Error); virtual void addNonContactingLimb(const char* id, const char* limb, const char* effector, unsigned int samples) throw (hpp::Error); virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues,