def loadRobot(packageName,meshPackageName,rootJointType,urdfName,urdfSuffix,srdfSuffix, limbId, limbRoot, limbEffector):
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [0,0,0,0,0,0])
limbOffset = [0,0,0] #inutile ici
limbNormal = [0,1,0] #inutile ici
limbx = 0.09; limby = 0.05 #inutile ici
fullBody.addLimb(limbId,limbRoot,'',limbOffset,limbNormal, limbx, limby, 1000, "manipulability", 0.1)
return fullBody
-1.5, 0.5, -1, 0, 0, 0, 0, 1.4, -1.2, 1.6, 2.1, 0.4, 0, 0.0, -1.4, -1.2,
-1.6, 2.1, 0.4, 0.0, 0.0
]
fullBody.setPose(extending, "extending")
fullBody.setPose(flexion, "flexion")
fullBody.setPose(q_contact_takeoff, "takeoffContact")
rLegId = 'RFoot'
lLegId = 'LFoot'
rarmId = 'RHand'
larmId = 'LHand'
nbSamples = 50000
x = 0.03
y = 0.08
fullBody.addLimb(rLegId, 'RThigh_rx', 'SpidermanRFootSphere', [0, 0, 0],
[0, 0, 1], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
fullBody.addLimb(lLegId, 'LThigh_rx', 'SpidermanLFootSphere', [0, 0, 0],
[0, 0, 1], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
fullBody.addLimb(rarmId, 'RHumerus_rx', 'SpidermanRHandSphere', [0, 0, 0],
[0, -1, 0], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
fullBody.addLimb(larmId, 'LHumerus_rx', 'SpidermanLHandSphere', [0, 0, 0],
[0, 1, 0], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
print("Limbs added to fullbody")
confsize = len(tp.q11)
### TEST OF CONTACT CREATION FOR INTERMEDIATE CONFIG, NOT USED FOR INTERPOLATION
q_tmp = q_contact_takeoff[::]
q_tmp[0:confsize - tp.ecsSize] = tp.q_cube[0:confsize - tp.ecsSize]
rr(q_tmp)
fullBody.setCurrentConfig(q_tmp)
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
ps = ProblemSolver( fullBody )
r = Viewer (ps)
n_samples = 10000
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, n_samples, "manipulability", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, n_samples, "manipulability", 0.1)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1]
rArmNormal = [0,0,1]
rArmOffset = [-0.045,-0.01,-0.085]
rArmNormal = [1,0,0]
legx = 0.02; legy = 0.02 # remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm). def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False): fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision) lLegId = 'lhleg' rarmId = 'rhleg' larmId = 'lfleg' #~ addLimbDb(rLegId, "static") #~ addLimbDb(lLegId, "static") #~ addLimbDb(rarmId, "static") #~ addLimbDb(larmId, "static") fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType) lLegId = 'lhleg' lLeg = 'lh_haa_joint' lfoot = 'lh_foot_joint' fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType) #~ rarmId = 'rhleg' rarm = 'rh_haa_joint' rHand = 'rh_foot_joint' fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType) larmId = 'lfleg' larm = 'lf_haa_joint' lHand = 'lf_foot_joint' fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
cType = "_3_DOF"
# string identifying the limb
rfLegId = 'rfleg'
# First joint of the limb, as in urdf file
rfLeg = 'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfFoot = 'rf_foot_joint'
# 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(rfLegId, rfLeg, rfFoot, offset, normal, legx, legy, nbSamples,
"manipulability", 0.05, cType)
fullBody.runLimbSampleAnalysis(rfLegId, "jointLimitsDistance", True)
lhLegId = 'lhleg'
lhLeg = 'lh_haa_joint'
lhFoot = 'lh_foot_joint'
fullBody.addLimb(lhLegId, lhLeg, lhFoot, offset, normal, legx, legy, nbSamples,
"manipulability", 0.05, cType)
fullBody.runLimbSampleAnalysis(lhLegId, "jointLimitsDistance", True)
lfLegId = 'lfleg'
lfLeg = 'lf_haa_joint'
lfFoot = 'lf_foot_joint'
fullBody.addLimb(lfLegId, lfLeg, lfFoot, offset, normal, legx, legy, nbSamples,
"manipulability", 0.05, cType)
fullBody.runLimbSampleAnalysis(lfLegId, "jointLimitsDistance", True)
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,1, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "EFORT", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "EFORT", 0.1)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "random", 0.05)
qfar=q_ref[::]
qfar[2] = -5
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
lLegId = 'hrp2_lleg_rom'
tStart = time.time()
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.0955]
rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,0,-0.0955]
lLegLimbOffset=[0,0,0.035]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
fullBody.setReferenceConfig (q_ref)
## Add arms (not used for contact) :
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,2, -2, 1, 0.5, 2.5])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "static", 0.03)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "static", 0.03)
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.045,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
ps.client.problem.setParameter("aMax", omniORB.any.to_any(tp.aMax))
ps.client.problem.setParameter("aMaxZ", omniORB.any.to_any(1.))
ps.client.problem.setParameter("vMax", omniORB.any.to_any(tp.vMax))
ps.client.problem.setParameter("friction", tp.mu)
r = tp.Viewer(ps, viewerClient=tp.r.client)
#~ AFTER loading obstacles
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, lLegNormal, lLegx, lLegy, 50000,
"manipulability", 0.01, "_6_DOF")
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
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,
100000, "manipulability", 0.01, "_6_DOF", True)
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.105]
lLegId = 'hrp2_lleg_rom'
rarmId = 'hrp2_rarm_rom'
larmId = 'hrp2_larm_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.105]
rLegLimbOffset = [0, 0, -0.035] #0.035
rLegNormal = [0, 0, 1]
rLegx = 0.09
rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(rLegId,
rLeg,
'',
rLegOffset,
rLegNormal,
rLegx,
rLegy,
100000,
"manipulability",
0.01,
"_6_DOF",
limbOffset=rLegLimbOffset)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, 0, -0.105]
lLegLimbOffset = [0, 0, 0.035]
lLegNormal = [0, 0, 1]
lLegx = 0.09
lLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "forward", 0.03)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "forward", 0.03)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
confsize = len(tp.q_init)
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer(ps)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [
0,
-0.105,
0,
]
rLegNormal = [0, 1, 0]
rLegx = 0.09
rLegy = 0.05
fullBody.addLimb(rLegId, rLeg, '', rLegOffset, rLegNormal, rLegx, rLegy, 10000,
"static", 0.03)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, -0.105, 0]
lLegNormal = [0, 1, 0]
lLegx = 0.09
lLegy = 0.05
fullBody.addLimb(lLegId, lLeg, '', lLegOffset, rLegNormal, lLegx, lLegy, 10000,
"static", 0.03)
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.045, -0.01, -0.085]
rArmNormal = [1, 0, 0]
nbSamples = 50000 ps = ProblemSolver(fullBody) rootName = "base_joint_xyz" # ~ AFTER loading obstacles rmId = "rm" rm = "j_c1_rm" rme = "j_tibia_rm" rmOffset = [0, 0, 0] rmNormal = [0, 1, 0] rmx = 0.02 rmy = 0.02 fullBody.addLimb(rmId, rm, rme, rmOffset, rmNormal, rmx, rmy, nbSamples, "EFORT", 0.01) rrId = "rr" rr = "j_c1_rr" rre = "j_tibia_rr" rrOffset = [0, 0, 0] rrNormal = [0, 1, 0] rrx = 0.02 rry = 0.02 fullBody.addLimb(rrId, rr, rre, rrOffset, rrNormal, rrx, rry, nbSamples, "EFORT", 0.01) rfId = "rf" rf = "j_c1_rf" rfe = "j_tibia_rf" rfOffset = [0, 0, 0] rfNormal = [0, 1, 0]
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'
# 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'
fullBody.addLimb(lLegId, lLeg, lfoot, offset, normal, legx, legy, nbSamples,
"backward", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId, rarm, rHand, offset, normal, legx, legy, nbSamples,
"backward", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.0955]
rLegLimbOffset = [0, 0, -0.035] #0.035
rLegNormal = [0, 0, 1]
rLegx = 0.09
rLegy = 0.05
fullBody.addLimb(
rLegId,
rLeg,
'',
rLegOffset,
rLegNormal,
rLegx,
rLegy,
100000,
"fixedStep1",
0.01,
"_6_DOF",
limbOffset=rLegLimbOffset,
kinematicConstraintsPath=
"package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",
kinematicConstraintsMin=0.)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, 0, -0.0955]
lLegLimbOffset = [0, 0, 0.035]
lLegNormal = [0, 0, 1]
lLegx = 0.09
offset = [0., -0.021, 0.]
normal = [0, 1, 0]
legx = 0.02
legy = 0.02
def addLimbDb(limbId,
heuristicName,
loadValues=True,
disableEffectorCollision=False):
fullBody.addLimbDatabase(str(db_dir + limbId + '.db'), limbId,
heuristicName, loadValues,
disableEffectorCollision)
fullBody.addLimb(rLegId, rLeg, rfoot, offset, normal, legx, legy, nbSamples,
"jointlimits", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId, lLeg, lfoot, offset, normal, legx, legy, nbSamples,
"jointlimits", 0.05, cType)
#~
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId, rarm, rHand, offset, normal, legx, legy, nbSamples,
"jointlimits", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
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'
# Specifying the distance between last joint and contact surface
rLegOffset = [0., 0, 0.]
# Specifying the contact surface direction when the limb is in rest pose
rLegNormal = [0, 1, 0]
# Specifying the rectangular contact surface length
rLegx = 0.02
rLegy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
fullBody.addLimb(rLegId, rLeg, rfoot, rLegOffset, rLegNormal, rLegx, rLegy,
nbSamples, "manipulability", 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, "manipulability", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
rArmOffset = [0., 0, -0.]
normal = [0, 1, 0] #hyq needs [0,1,0], also for anymal
legx = 0.03
legy = 0.03
def addLimbDb(limbId,
heuristicName,
loadValues=True,
disableEffectorCollision=False):
fullBody.addLimbDatabase(str(db_dir + limbId + '.db'), limbId,
heuristicName, loadValues,
disableEffectorCollision)
#fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
fullBody.addLimb(rLegId, rLeg, rfoot, offset, normal, legx, legy, nbSamples,
"manipulability", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'LH_HAA'
lfoot = 'LH_MOUNT_TO_FOOT'
fullBody.addLimb(lLegId, lLeg, lfoot, offset, normal, legx, legy, nbSamples,
"manipulability", 0.1, cType)
#~
rarmId = 'rhleg'
rarm = 'RH_HAA'
rHand = 'RH_MOUNT_TO_FOOT'
fullBody.addLimb(rarmId, rarm, rHand, offset, normal, legx, legy, nbSamples,
"manipulability", 0.1, cType)
larmId = 'lfleg'
larm = 'LF_HAA'
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
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)
"""
r.addLandmark(r.sceneName,1)
r.addLandmark('talos/gripper_left_inner_single_link',0.3)
r.addLandmark('talos/gripper_right_inner_single_link',0.3)
r.addLandmark('talos/left_sole_link',0.3)
r.addLandmark('talos/right_sole_link',0.3)
"""
rLegId = "rleg"
rLeg = "leg_right_1_joint"
rfoot = "leg_right_sole_fix_joint"
rLegOffset = [0, 0, 0.01]
rLegNormal = [0, 0, 1]
rLegx = 0.1
rLegy = 0.06
fullBody.addLimb(rLegId, rLeg, rfoot, rLegOffset, rLegNormal, rLegx, rLegy,
nbSamples, "EFORT", 0.01)
lLegId = "lleg"
lLeg = "leg_left_1_joint"
lfoot = "leg_left_sole_fix_joint"
lLegOffset = [0, 0, 0.01]
lLegNormal = [0, 0, 1]
lLegx = 0.1
lLegy = 0.06
fullBody.addLimb(lLegId, lLeg, lfoot, lLegOffset, rLegNormal, lLegx, lLegy,
nbSamples, "EFORT", 0.01)
rarmId = "rarm"
rarm = "arm_right_1_joint"
rHand = "gripper_right_inner_single_joint"
rArmOffset = [0, 0, 0.1]
ps.client.problem.setParameter("friction", tp.mu)
r = tp.Viewer(ps,
viewerClient=tp.r.client,
displayArrows=True,
displayCoM=True)
#~ AFTER loading obstacles
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, lLegNormal, lLegx, lLegy, 50000,
"manipulability", 0.01, "_6_DOF")
"""
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
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, 100000, "manipulability", 0.01, "_6_DOF", True)
"""
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.105]
r = Viewer(ps)
n_samples = 10000
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [
0,
-0.105,
0,
]
rLegNormal = [0, 1, 0]
rLegx = 0.09
rLegy = 0.05
fullBody.addLimb(rLegId, rLeg, '', rLegOffset, rLegNormal, rLegx, rLegy,
n_samples, "manipulability", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, -0.105, 0]
lLegNormal = [0, 1, 0]
lLegx = 0.09
lLegy = 0.05
fullBody.addLimb(lLegId, lLeg, '', lLegOffset, rLegNormal, lLegx, lLegy,
n_samples, "manipulability", 0.1)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0, 0, -0.1]
rArmNormal = [0, 0, 1]
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.105]
rLegLimbOffset = [0, 0, -0.035] #0.035
rLegNormal = [0, 0, 1]
rLegx = 0.09
rLegy = 0.05
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody.addLimb(
rLegId,
rLeg,
'',
rLegOffset,
rLegNormal,
rLegx,
rLegy,
100000,
"fixedStep08",
0.01,
"_6_DOF",
limbOffset=rLegLimbOffset,
kinematicConstraintsPath=
"package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj")
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, 0, -0.105]
lLegLimbOffset = [0, 0, 0.035]
lLegNormal = [0, 0, 1]
lLegx = 0.09
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "ground", "contact")
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "manipulability", 0.05)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000,"manipulability", 0.03)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
q_0 = fullBody.getCurrentConfig();
q_init = [
0.0, 0.0, 0.648702, 1.0, 0.0, 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
pp.speed = 0.6
q_0 = fullBody.getCurrentConfig()
rr(q_0)
rLegId = 'RFoot'
lLegId = 'LFoot'
rarmId = 'RHand'
larmId = 'LHand'
rfoot = 'SpidermanRFootSphere'
lfoot = 'SpidermanLFootSphere'
lHand = 'SpidermanLHandSphere'
rHand = 'SpidermanRHandSphere'
nbSamples = 50000
x = 0.03
y = 0.08
fullBody.addLimb(rLegId, 'RThigh_rx', 'SpidermanRFootSphere', [0, 0, 0],
[0, 0, 1], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
fullBody.addLimb(lLegId, 'LThigh_rx', 'SpidermanLFootSphere', [0, 0, 0],
[0, 0, 1], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
#~ fullBody.addLimb(rarmId,'RHumerus_rx','SpidermanRHandSphere',[0,0,0],[0,-1,0], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF")
#~ fullBody.addLimb(larmId,'LHumerus_rx','SpidermanLHandSphere',[0,0,0],[0,1,0], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF")
#~
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
limbsCOMConstraints = {
rLegId: {
'file': "spiderman/RL_com.ineq",
'effector': rfoot
},
lLegId: {
'file': "spiderman/LL_com.ineq",
tStart = time.time()
rLegOffset = [0, 0, -0.0955]
rLegLimbOffset = [0, 0, -0.035] #0.035
rLegNormal = [0, 0, 1]
rLegx = 0.09
rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(
rLegId,
rleg,
'',
rLegOffset,
rLegNormal,
rLegx,
rLegy,
100000,
"fixedStep06",
0.01,
"_6_DOF",
kinematicConstraintsPath=
"package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",
limbOffset=rLegLimbOffset,
kinematicConstraintsMin=0.4)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLegOffset = [0, 0, -0.0955]
lLegLimbOffset = [0, 0, 0.035]
lLegNormal = [0, 0, 1]
lLegx = 0.09
lLegy = 0.05
"""
fullBody.setCurrentConfig(q_init)
qfar = q_ref[::]
qfar[2] = -5
tStart = time.time()
# generate databases :
nbSamples = 50000
rLegOffset = MRsole_offset.translation.transpose().tolist()[0]
rLegOffset[2] += 0.006
rLegNormal = [0, 0, 1]
rLegx = 0.1
rLegy = 0.06
fullBody.addLimb(rLegId, rleg, rfoot, rLegOffset, rLegNormal, rLegx, rLegy,
nbSamples, "fixedStep06", 0.01)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset = MLsole_offset.translation.transpose().tolist()[0]
lLegOffset[2] += 0.006
lLegNormal = [0, 0, 1]
lLegx = 0.1
lLegy = 0.06
fullBody.addLimb(lLegId, lleg, lfoot, lLegOffset, rLegNormal, lLegx, lLegy,
nbSamples, "fixedStep06", 0.01)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
"""
rArmOffset = [0,0,0.1]
rArmNormal = [0,0,1]
fullBody.setJointBounds ("base_joint_xyz", [-5.5,5.5, -2.5, 2.5, 0.55, 0.6])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver( fullBody )
ps.client.problem.setParameter("aMax",omniORB.any.to_any(tp.aMax))
ps.client.problem.setParameter("vMax",omniORB.any.to_any(tp.vMax))
r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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, "forward", 0.1)
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, "forward", 0.1)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
lLegId = 'hrp2_lleg_rom'
tStart = time.time()
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.105]
rLegLimbOffset = [0, 0, -0.035] #0.035
rLegNormal = [0, 0, 1]
rLegx = 0.09
rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(rLegId,
rLeg,
'',
rLegOffset,
rLegNormal,
rLegx,
rLegy,
100000,
"manipulability",
0.01,
"_6_DOF",
limbOffset=rLegLimbOffset)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, 0, -0.105]
lLegLimbOffset = [0, 0, 0.035]
lLegNormal = [0, 0, 1]
lLegx = 0.09
lLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody.setJointBounds ("base_joint_xyz", [-2,4, 0.5, 1.5, 0.3, 1.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver( fullBody )
ps.client.problem.setParameter("aMax",omniORB.any.to_any(tp.aMax))
ps.client.problem.setParameter("vMax",omniORB.any.to_any(tp.vMax))
r = tp.Viewer (ps,viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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, "forward", 0.1)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "dynamic", 0.1)
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, "forward", 0.1)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "dynamic", 0.1)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual) cType = "_6_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_Z' # 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, "manipulability", 0.05, cType) lLegId = 'lhleg' lLeg = 'lh_haa_joint' lfoot = 'lh_foot_Z' fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType) rarmId = 'rhleg' rarm = 'rh_haa_joint' rHand = 'rh_foot_Z' fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType) larmId = 'lfleg' larm = 'lf_haa_joint' lHand = 'lf_foot_Z' fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer(ps)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [
0,
-0.105,
0,
]
rLegNormal = [0, 1, 0]
rLegx = 0.09
rLegy = 0.05
fullBody.addLimb(rLegId, rLeg, '', rLegOffset, rLegNormal, rLegx, rLegy, 10000,
"manipulability", 0.03)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, -0.105, 0]
lLegNormal = [0, 1, 0]
lLegx = 0.09
lLegy = 0.05
fullBody.addLimb(lLegId, lLeg, '', lLegOffset, rLegNormal, lLegx, lLegy, 10000,
"manipulability", 0.03)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05, -0.050, -0.050]
rArmNormal = [1, 0, 0]
fullBody.setJointBounds("base_joint_xyz", [0, 3, -2, 0, 0.3, 1])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver(fullBody)
ps.client.problem.setParameter("aMax", tp.aMax)
ps.client.problem.setParameter("vMax", tp.vMax)
r = tp.Viewer(ps, viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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, 20000,
"manipulability", 0.1)
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, 20000,
"manipulability", 0.1)
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0, 0, -0.1]
rArmNormal = [0, 0, 1]
fullBody.setJointBounds("base_joint_xyz", [-2, 5, 0, 2, 0.45, 1.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver(fullBody)
ps.client.problem.setParameter("aMax", tp.aMax)
ps.client.problem.setParameter("vMax", tp.vMax)
r = tp.Viewer(ps, viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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, 20000,
"EFORT_Normal", 0.1)
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, 20000,
"EFORT_Normal", 0.1)
q_0 = fullBody.getCurrentConfig()
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
q_init = [
0.1, -0.82, 0.648702, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.261799388,
# 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' # Specifying the distance between last joint and contact surface rLegOffset = [0.,0,0.] # Specifying the contact surface direction when the limb is in rest pose rLegNormal = [0,1,0] # Specifying the rectangular contact surface length rLegx = 0.02; rLegy = 0.02 # remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm). fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 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, "manipulability", 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
lLegId = 'hrp2_lleg_rom'
tStart = time.time()
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.105]
rLegLimbOffset = [0, 0, -0.06] #0.035
rLegNormal = [0, 0, 1]
rLegx = 0.09
rLegy = 0.05
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody.addLimb(rLegId,
rLeg,
'',
rLegOffset,
rLegNormal,
rLegx,
rLegy,
50000,
"dynamicWalk",
0.05,
"_6_DOF",
limbOffset=rLegLimbOffset)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, 0, -0.105]
lLegLimbOffset = [0, 0, 0.06]
lLegNormal = [0, 0, 1]
lLegx = 0.09
lLegy = 0.05
#fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1,"_6_DOF")
tStart = time.time()
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.105]
rLegLimbOffset = [0, 0, -0.035] #0.035
rLegNormal = [0, 0, 1]
rLegx = 0.09
rLegy = 0.05
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody.addLimb(rLegId,
rLeg,
'',
rLegOffset,
rLegNormal,
rLegx,
rLegy,
100000,
"fixedStep06",
0.01,
"_6_DOF",
limbOffset=rLegLimbOffset,
kinematicConstraintsMin=0.5)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg = 'LLEG_JOINT0'
lLegOffset = [0, 0, -0.105]
lLegLimbOffset = [0, 0, 0.035]
lLegNormal = [0, 0, 1]
lLegx = 0.09
lLegy = 0.05
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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, 10000, "manipulability", 0.1)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegx = 0.09; lLegy = 0.05
lLegOffset = [0,0,-0.105]
lLegNormal = [0,0,1]
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
#~ AFTER loading obstacles
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [0,0,-0.1075]
lArmNormal = [0,0,1]
lArmx = 0.024; lArmy = 0.024
tStart = time.time()
# generate databases :
nbSamples = 50000
rLegOffset = MRsole_offset.translation.transpose().tolist()[0]
rLegOffset[2] += 0.006
rLegNormal = [0, 0, 1]
rLegx = 0.1
rLegy = 0.06
fullBody.addLimb(rLegId,
rleg,
rfoot,
rLegOffset,
rLegNormal,
rLegx,
rLegy,
nbSamples,
"static",
0.01,
kinematicConstraintsMin=0.4)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset = MLsole_offset.translation.transpose().tolist()[0]
lLegOffset[2] += 0.006
lLegNormal = [0, 0, 1]
lLegx = 0.1
lLegy = 0.06
fullBody.addLimb(lLegId,
lleg,
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, 0, 2, 0.45, 1.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver( fullBody )
ps.client.problem.setParameter("aMax",tp.aMax)
ps.client.problem.setParameter("vMax",tp.vMax)
r = tp.Viewer (ps,viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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)
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)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,2, -2, 1, 0.5, 2.5])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.03, "_6_DOF")
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.115,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03, "_6_DOF")
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.040,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver nbSamples = 50000 ps = ProblemSolver( fullBody ) rootName = 'base_joint_xyz' #~ AFTER loading obstacles 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, "EFORT", 0.01) 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, "EFORT", 0.01) rarmId = 'rhleg' rarm = 'rh_haa_joint' rHand = 'rh_foot_joint' rArmOffset = [0,0,0] rArmNormal = [1,0,0] rArmx = 0.02; rArmy = 0.02
nbSamples = 1 ps = ProblemSolver( fullBody ) rLegId = 'RFoot' lLegId = 'LFoot' rarmId = 'RHand' larmId = 'LHand' rfoot = 'SpidermanRFootSphere' lfoot = 'SpidermanLFootSphere' lHand = 'SpidermanLHandSphere' rHand = 'SpidermanRHandSphere' nbSamples = 50000; x = 0.03; y = 0.08 fullBody.addLimb(rLegId,'RThigh_rx','SpidermanRFootSphere',[0,0,0],[0,0,1], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF") fullBody.addLimb(lLegId,'LThigh_rx','SpidermanLFootSphere',[0,0,0],[0,0,1], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF") fullBody.addLimb(rarmId,'RHumerus_rx','SpidermanRHandSphere',[0,0,0],[0,-1,0], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF") fullBody.addLimb(larmId,'LHumerus_rx','SpidermanLHandSphere',[0,0,0],[0,1,0], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF") fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) #make sure this is 0 q_0 = fullBody.getCurrentConfig () zeroConf = [0,0,0, 1, 0, 0, 0] q_0[0:7] = zeroConf fullBody.setCurrentConfig (q_0) effectors = [rfoot, lfoot, lHand, rHand]
