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]