Commit a10fc436 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added contact types: 3 dof for hyq, 6 for hrp2

parent e48e2f36
......@@ -155,8 +155,10 @@ module hpp
/// \param samples number of samples to generate for the limb (a typical value is 10000)
/// \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")
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) raises (Error);
in double x, in double y, in unsigned short samples, in string heuristicName,
in double resolution, in string contactType) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
......
......@@ -26,14 +26,14 @@ r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
#~ AFTER loading obstacles
cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
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, "manipulability", 0.1)
fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
......@@ -41,7 +41,7 @@ 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, "manipulability", 0.05)
fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
......@@ -49,7 +49,7 @@ 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, "manipulability", 0.05)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
......@@ -57,7 +57,7 @@ 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)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
......
......@@ -27,13 +27,14 @@ r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
#~ AFTER loading obstacles
cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
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)
fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.1,cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
......@@ -41,7 +42,7 @@ 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)
fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "backward", 0.05,cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
......@@ -49,7 +50,7 @@ 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)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "backward", 0.05,cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
......@@ -57,7 +58,7 @@ 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)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05,cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
......
......@@ -110,8 +110,9 @@ class FullBody (object):
# structure to perform efficient proximity requests. The resulution of the octree, in meters, specifies the size
# 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.
def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution):
self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution)
# \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)
## Returns the configuration of a limb described by a sample
#
......
......@@ -353,7 +353,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) throw (hpp::Error)
unsigned short samples, const char* heuristicName, double resolution, const char *contactType) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
......@@ -365,7 +365,12 @@ namespace hpp {
off[i] = offset[i];
norm[i] = normal[i];
}
fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y, problemSolver_->collisionObstacles(), samples,heuristicName,resolution);
ContactType cType = hpp::rbprm::_6_DOF;
if(std::string(contactType) == "_3_DOF")
{
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);
}
catch(std::runtime_error& e)
{
......
......@@ -110,7 +110,7 @@ namespace hpp {
const hpp::floatSeq& direction) 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) throw (hpp::Error);
unsigned short samples, const char *heuristicName, double resolution, const char *contactType) 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