Commit 3adcaf4d authored by Steve Tonneau's avatar Steve Tonneau
Browse files

allowing to disable end effector collision

parent 3ff807ca
......@@ -178,9 +178,10 @@ module hpp
/// \param heuristicName heuristic used to bias sample selection
/// \param resolution resolution of the octree used to store the samples (a typical value is 0.01 meters)
/// \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
/// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
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 short samples, in string heuristicName,
in double resolution, in string contactType) raises (Error);
in double resolution, in string contactType, in double disableEffectorCollision) raises (Error);
/// Specifies a subchain of the robot as a limb, which can be used to create contacts.
/// A limb must consist in a simple kinematic chain, where every node has only one child
......@@ -188,7 +189,9 @@ module hpp
/// \param id user given name of the new limb
/// \param heuristicName heuristic used to bias sample selection
/// \param loadValues whether other values computed for the limb database should be loaded
void addLimbDatabase(in string databasepath, in string id, in string heuristicName, in double loadValues) raises (Error);
/// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
void addLimbDatabase(in string databasepath, in string id, in string heuristicName, in double loadValues,
in double disableEffectorCollision) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
......
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import ground_crouch_hyq_path as tp
......@@ -26,45 +27,43 @@ r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
#~ AFTER loading obstacles
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_3_DOF"
# string identifying the limb
rLegId = 'rfleg'
# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfoot = 'rf_foot_joint'
rLegOffset = [0.,0,0.]
rLegNormal = [0,1,0]
rLegx = 0.02; rLegy = 0.02
fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.1,cType)
# Specifying the distance between last joint and contact surface
offset = [0.,-0.021,0.]
# Specifying the contact surface direction when the limb is in rest pose
normal = [0,1,0]
# Specifying the rectangular contact surface length
legx = 0.02; legy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
lLegOffset = [0,0,0]
lLegNormal = [0,1,0]
lLegx = 0.02; lLegy = 0.02
fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "backward", 0.05,cType)
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
rArmOffset = [0.,0,-0.]
rArmNormal = [0,1,0]
rArmx = 0.02; rArmy = 0.02
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "backward", 0.05,cType)
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
lArmOffset = [0.,0,-0.]
lArmNormal = [0,1,0]
lArmx = 0.02; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05,cType)
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])
q_0 = fullBody.getCurrentConfig();
......@@ -84,9 +83,9 @@ r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
i = 0;
r (configs[i]); i=i+1; i-1
q0 = configs[2]
q0 = fullBody.generateContacts(q0, [0,0,1])
r(q0)
#~ q0 = configs[2]
#~ q0 = fullBody.generateContacts(q0, [0,0,1])
#~ r(q0)
c = fullBody.getContactSamplesIds("rfleg",q_init, [0,0,1])
r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
#~ r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
......@@ -30,9 +30,7 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
ps.addPathOptimizer("RandomShortcut")
......@@ -44,6 +42,8 @@ ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "groundcrouch", "planning")
#~ ps.solve ()
t = ps.solve ()
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
......
......@@ -45,7 +45,8 @@ rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1]
rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05)
#disabling collision for hook
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
#~ AFTER loading obstacles
......
......@@ -101,11 +101,15 @@ class FullBody (object):
# The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
# \param heuristicName: name of the selected heuristic for configuration evaluation
# \param loadValues: whether values computed, other than the static ones, should be loaded in memory
def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True):
# \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
boolVal = 0.
boolValEff = 0.
if(loadValues):
boolVal = 1.
self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal)
if(disableEffectorCollision):
boolValEff = 1.
self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff)
## Add a limb to the model
#
......@@ -124,8 +128,12 @@ class FullBody (object):
# of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact.
# 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")
def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF"):
self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType)
# \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):
boolValEff = 0.
if(disableEffectorCollision):
boolValEff = 1.
self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff)
## Returns the configuration of a limb described by a sample
#
......
......@@ -451,7 +451,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 short samples, const char* heuristicName, double resolution, const char *contactType) throw (hpp::Error)
unsigned short samples, const char* heuristicName, double resolution, const char *contactType, double disableEffectorCollision) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
......@@ -468,7 +468,8 @@ namespace hpp {
{
cType = hpp::rbprm::_3_DOF;
}
fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y, problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType);
fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y,
problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType,disableEffectorCollision > 0.5);
}
catch(std::runtime_error& e)
{
......@@ -477,14 +478,15 @@ namespace hpp {
}
void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues) throw (hpp::Error)
void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues, double disableEffectorCollision) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
try
{
std::string fileName(databasePath);
fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName, loadValues > 0.5);
fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName, loadValues > 0.5,
disableEffectorCollision > 0.5);
}
catch(std::runtime_error& e)
{
......
......@@ -118,8 +118,10 @@ namespace hpp {
double octreeNodeId) throw (hpp::Error);
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 short samples, const char *heuristicName, double resolution, const char *contactType) throw (hpp::Error);
virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues) throw (hpp::Error);
unsigned short samples, const char *heuristicName, double resolution, const char *contactType,
double disableEffectorCollision) throw (hpp::Error);
virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues,
double disableEffectorCollision) throw (hpp::Error);
virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) 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