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

[demo] stairs multicontact

parent d9742281
......@@ -342,6 +342,8 @@ install(FILES
install(FILES
data/com_inequalities/LLEG_JOINT0_com_constraints.obj
data/com_inequalities/RLEG_JOINT0_com_constraints.obj
data/com_inequalities/RARM_JOINT0_com_constraints.obj
data/com_inequalities/LARM_JOINT0_com_constraints.obj
data/com_inequalities/LLEG_JOINT0_com_constraints.dae
data/com_inequalities/RLEG_JOINT0_com_constraints.dae
data/com_inequalities/LLEG_LINK5_Kinematics.dae
......
......@@ -37,14 +37,14 @@ rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "manipulability", 0.1)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "manipulability", 0.01,"_6_DOF")
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,0,-0.105]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "manipulability", 0.1)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "manipulability", 0.01,"_6_DOF")
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
......@@ -53,7 +53,7 @@ rArmOffset = [0,0,-0.1]
rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
#disabling collision for hook
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 50000, "manipulability", 0.05, "_6_DOF", False)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 100000, "manipulability", 0.01, "_6_DOF", True)
"""
......@@ -121,6 +121,8 @@ fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
q_init[2] = q_init[2]+0.02
q_goal[2] = q_goal[2]+0.02
q_init[0:3]=[0.28994563306701016,-0.82,0.6191688248477717]
fullBody.setStaticStability(True)
# Randomly generating a contact configuration at q_init
......
......@@ -70,7 +70,7 @@ r.addLandmark(r.sceneName,1)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r)
afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r,reduceSizes=[0.1,0])
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
......
......@@ -436,7 +436,7 @@ class FullBody (object):
# \param limb name of the limb for which the request aplies
# \return all limbs in contact at this state
def getLimbsInContact(self, limbNames, state1):
return [limbName for limbName in limbNames if self.isLimbInContact(limbname, state1)]
return [limbName for limbName in limbNames if self.isLimbInContact(limbName, state1)]
##
#
......@@ -444,7 +444,7 @@ class FullBody (object):
# \param limb name of the limb for which the request aplies
# \return all limbs in contact at this state
def getLimbsInContactIntermediary(self, limbNames, state1):
return [limbName for limbName in limbNames if self.isLimbInContactIntermediary(limbname, state1)]
return [limbName for limbName in limbNames if self.isLimbInContactIntermediary(limbName, state1)]
## returns the CWC of the robot at a given state
#
......
Markdown is supported
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