diff --git a/profile/profile.py b/profile/profile.py index f17277cd259837099f46b2d88e145e3375804745..d6f69d81475583692953a4d8a5396bb0da8111e2 100755 --- a/profile/profile.py +++ b/profile/profile.py @@ -7,9 +7,9 @@ import datetime import time #~ scenarios = ['standing_hrp2'] -scenarios = ['downSlope_hrp2'] +scenarios = ['wall_hrp2'] #~ scenarios = ['stair_bauzil_hrp2'] -n_trials = 10 +n_trials = 50 stats = ['balance','collision','ik'] stats_optim = ['time_cwc','com_traj'] diff --git a/profile/scenarios/hrp2_wall_path.py b/profile/scenarios/hrp2_wall_path.py new file mode 100644 index 0000000000000000000000000000000000000000..2e0086c704a869f136b9d0e8e12bff06496485c0 --- /dev/null +++ b/profile/scenarios/hrp2_wall_path.py @@ -0,0 +1,126 @@ +from hpp.corbaserver.rbprm.rbprmbuilder import Builder +from hpp.gepetto import Viewer +from hpp.corbaserver import Client +from hpp.corbaserver.robot import Robot as Parent +from hpp.corbaserver.rbprm.problem_solver import ProblemSolver + + +class Robot (Parent): + rootJointType = 'freeflyer' + packageName = 'hpp-rbprm-corba' + meshPackageName = 'hpp-rbprm-corba' + # URDF file describing the trunk of the robot HyQ + urdfName = 'hrp2_trunk_flexible' + urdfSuffix = "" + srdfSuffix = "" + def __init__ (self, robotName, load = True): + Parent.__init__ (self, robotName, self.rootJointType, load) + self.tf_root = "base_footprint" + self.client.basic = Client () + self.load = load + + +rootJointType = 'freeflyer' +packageName = 'hpp-rbprm-corba' +meshPackageName = 'hpp-rbprm-corba' +urdfName = 'hrp2_trunk_flexible' +urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'] +urdfSuffix = "" +srdfSuffix = "" + +rbprmBuilder = Builder () + +rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) +rbprmBuilder.setJointBounds ("base_joint_xyz", [0,3, -2, 0, 0.3, 1]) +rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom']) +rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean']) +rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',]) +rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support']) +rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1]) +vMax = 1; +aMax = 10; +extraDof = 6 +rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof) +rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0]) +indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace() + +#~ from hpp.corbaserver.rbprm. import ProblemSolver + + +ps = ProblemSolver( rbprmBuilder ) +ps.client.problem.setParameter("aMax",aMax) +ps.client.problem.setParameter("vMax",vMax) +ps.client.problem.setParameter("sizeFootX",0.24) +ps.client.problem.setParameter("sizeFootY",0.14) +r = Viewer (ps) + + +from hpp.corbaserver.affordance.affordance import AffordanceTool +afftool = AffordanceTool () +afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) +afftool.setAffordanceConfig('Lean', [0.5, 0.03, 0.00005]) +afftool.loadObstacleModel (packageName, "roomWall", "planning", r) +r.addLandmark(r.sceneName,1) +afftool.visualiseAffordances('Support', r,r.color.brown) +afftool.visualiseAffordances('Lean', r, r.color.blue) + + +q_init = rbprmBuilder.getCurrentConfig (); +#q_init[0:3] = [2.1, -1, 0.58]; +#q_init[0:3] = [2, -1, 0.58]; +q_init[0:3] = [1.85, -1, 0.58]; +q_init[3:7] = [0.7071,0,0,0.7071] +#q_init[indexECS:indexECS+3] = [2,0,0] +q_init[indexECS:indexECS+3] = [1,0,0] +rbprmBuilder.setCurrentConfig (q_init); r (q_init) + + +q_goal = q_init [::] +q_goal [0:3] = [1.2, -1, 0.58]; +q_goal[indexECS:indexECS+3] = [-1,0,0] + + +#r(q_goal) + + +ps.setInitialConfig (q_init) +ps.addGoalConfig (q_goal) +# Choosing RBPRM shooter and path validation methods. +ps.client.problem.selectConFigurationShooter("RbprmShooter") +ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) +# Choosing kinodynamic methods : +ps.selectSteeringMethod("RBPRMKinodynamic") +ps.selectDistance("KinodynamicDistance") +ps.selectPathPlanner("DynamicPlanner") + + +t = ps.solve () +if isinstance(t, list): + t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] +f = open('log.txt', 'a') +f.write("path computation " + str(t) + "\n") +f.close() + + +# Playing the computed path +from hpp.gepetto import PathPlayer +pp = PathPlayer (rbprmBuilder.client.basic, r) +pp.dt=1./30. +#r.client.gui.removeFromGroup("rm0",r.sceneName) +pp.displayVelocityPath(0) +pp.speed=0.5 +#pp(0) + + + + + +q_far = q_init[::] +q_far[2] = -3 +r(q_far) + +#~ pp(0) + + + + diff --git a/profile/scenarios/wall_hrp2_interp.py b/profile/scenarios/wall_hrp2_interp.py new file mode 100644 index 0000000000000000000000000000000000000000..554a1a529f968f8acf79ac244b3929b262e22b80 --- /dev/null +++ b/profile/scenarios/wall_hrp2_interp.py @@ -0,0 +1,171 @@ +from hpp.corbaserver.rbprm.rbprmbuilder import Builder +from hpp.corbaserver.rbprm.rbprmfullbody import FullBody +from hpp.gepetto import Viewer + +import hrp2_wall_path as tp +import time + + + +packageName = "hrp2_14_description" +meshPackageName = "hrp2_14_description" +rootJointType = "freeflyer" +## +# Information to retrieve urdf and srdf files. +urdfName = "hrp2_14" +urdfSuffix = "_reduced" +srdfSuffix = "" + +fullBody = FullBody () + +fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) +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] +rArmx = 0.024; rArmy = 0.024 +#disabling collision for hook +fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 50000, "jointlimits", 0.05, "_6_DOF", True) + +""" + +#~ AFTER loading obstacles +larmId = '4Larm' +larm = 'LARM_JOINT0' +lHand = 'LARM_JOINT5' +lArmOffset = [-0.05,-0.050,-0.050] +lArmNormal = [1,0,0] +lArmx = 0.024; lArmy = 0.024 +#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05) + +rKneeId = '0RKnee' +rLeg = 'RLEG_JOINT0' +rKnee = 'RLEG_JOINT3' +rLegOffset = [0.105,0.055,0.017] +rLegNormal = [-1,0,0] +rLegx = 0.05; rLegy = 0.05 +#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01) +#~ +lKneeId = '1LKnee' +lLeg = 'LLEG_JOINT0' +lKnee = 'LLEG_JOINT3' +lLegOffset = [0.105,0.055,0.017] +lLegNormal = [-1,0,0] +lLegx = 0.05; lLegy = 0.05 +#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01) + #~ + +fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) +fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) + +#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1]) +#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1]) + + +""" + + +fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) +fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) +fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True) + +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, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init) +fullBody.setCurrentConfig (q_init) + +configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace() + +q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.0001)[0:7] # use this to get the correct orientation +q_init[2] = 0.648702 +q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:7] +dir_init = tp.ps.configAtParam(0,0.01)[tp.indexECS:tp.indexECS+3] +acc_init = tp.ps.configAtParam(0,0.01)[tp.indexECS+3:tp.indexECS+6] +dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[tp.indexECS:tp.indexECS+3] +acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[tp.indexECS+3:tp.indexECS+6] + + + +fullBody.setStaticStability(False) +# Randomly generating a contact configuration at q_init +fullBody.setCurrentConfig (q_init) +r(q_init) +#~ q_init = fullBody.generateContacts(q_init,dir_init,acc_init) +r(q_init) + +# Randomly generating a contact configuration at q_end +fullBody.setCurrentConfig (q_goal) +q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal) + +# copy extraconfig for start and init configurations +q_init[configSize:configSize+3] = dir_init[::] +q_init[configSize+3:configSize+6] = acc_init[::] +q_goal[configSize:configSize+3] = dir_goal[::] +q_goal[configSize+3:configSize+6] = acc_goal[::] +# specifying the full body configurations as start and goal state of the problem + +r(q_init) + + +#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) +fullBody.setStartState(q_init,[rLegId,lLegId]) +fullBody.setEndState(q_goal,[rLegId,lLegId]) + + +""" + +from hpp.gepetto import PathPlayer +from fullBodyPlayerHrp2 import Player +pp = PathPlayer (fullBody.client.basic, r) +""" + +#~ configs = fullBody.interpolate(0.08,pathId=0,robustnessTreshold = 1, filterStates = False) +#~ configs = fullBody.interpolate(0.04,pathId=0,robustnessTreshold = 1, filterStates = True) +configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 0, filterStates = True) + + + + +print "number of configs :", len(configs) + + + + +""" +player = Player(fullBody,pp,tp,configs,use_window =1,draw=False,optim_effector=False,use_velocity=True,pathId= 0) + +#player.displayContactPlan() + + + +player.interpolate(0, 17) +""" + diff --git a/script/dynamic/hrp2_wall_path.py b/script/dynamic/hrp2_wall_path.py index e1f6028969d00a83cb7384a25aa675f799ffc5b6..de06bdf40d606235e7e0e9f3113a4cd374242649 100644 --- a/script/dynamic/hrp2_wall_path.py +++ b/script/dynamic/hrp2_wall_path.py @@ -67,9 +67,11 @@ afftool.visualiseAffordances('Lean', r, r.color.blue) q_init = rbprmBuilder.getCurrentConfig (); #q_init[0:3] = [2.1, -1, 0.58]; -q_init[0:3] = [2, -1, 0.58]; +#q_init[0:3] = [2, -1, 0.58]; +q_init[0:3] = [1.85, -1, 0.58]; q_init[3:7] = [0.7071,0,0,0.7071] -q_init[indexECS:indexECS+3] = [2,0,0] +#q_init[indexECS:indexECS+3] = [2,0,0] +q_init[indexECS:indexECS+3] = [1,0,0] rbprmBuilder.setCurrentConfig (q_init); r (q_init) @@ -120,7 +122,7 @@ q_far = q_init[::] q_far[2] = -3 r(q_far) - +#~ pp(0) diff --git a/script/dynamic/log.txt b/script/dynamic/log.txt index 10fe89dec59835c8a7a132d9667a122c5a09b574..f2d01a3330e5fe34a9219f78da5ca6046b269693 100644 --- a/script/dynamic/log.txt +++ b/script/dynamic/log.txt @@ -2418,3 +2418,18 @@ planner succeeded: 1 unstable contact: 2 path computation 887965 path computation 2632519 +contact: 5 +no contact: 3 +planner failed: 1 +contact: 8 +no contact: 5 +planner failed: 2 + +*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) *** +collision 0.07 0.19 0.66 348.35 1855 +complete generation 13143.06 13143.06 13143.06 13143.06 1 +ik 0.09 0.31 1.17 1448.63 4741 +test balance 0.15 0.20 0.27 3.33 17 +contact: 19 +no contact: 11 +planner succeeded: 1 diff --git a/script/dynamic/wall_hrp2_interp.py b/script/dynamic/wall_hrp2_interp.py index b8b8c665781d12db8c2b18f4920a17429c1a386d..554a1a529f968f8acf79ac244b3929b262e22b80 100644 --- a/script/dynamic/wall_hrp2_interp.py +++ b/script/dynamic/wall_hrp2_interp.py @@ -23,22 +23,26 @@ 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, "EFORT_Normal", 0.1) +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, "EFORT_Normal", 0.1) +fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "manipulability", 0.1) + rarmId = 'hrp2_rarm_rom' rarm = 'RARM_JOINT0' @@ -47,7 +51,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, "EFORT_Normal", 0.05, "_6_DOF", True) +fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 50000, "jointlimits", 0.05, "_6_DOF", True) """ @@ -77,8 +81,8 @@ lLegx = 0.05; lLegy = 0.05 #~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01) #~ -#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) -#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) +fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) +fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) #~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1]) #~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1]) @@ -87,6 +91,10 @@ lLegx = 0.05; lLegy = 0.05 """ +fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) +fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) +fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True) + q_0 = fullBody.getCurrentConfig(); #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,) @@ -96,7 +104,8 @@ fullBody.setCurrentConfig (q_init) configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace() -q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation +q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.0001)[0:7] # use this to get the correct orientation +q_init[2] = 0.648702 q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:7] dir_init = tp.ps.configAtParam(0,0.01)[tp.indexECS:tp.indexECS+3] acc_init = tp.ps.configAtParam(0,0.01)[tp.indexECS+3:tp.indexECS+6] @@ -109,7 +118,7 @@ fullBody.setStaticStability(False) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig (q_init) r(q_init) -q_init = fullBody.generateContacts(q_init,dir_init,acc_init) +#~ q_init = fullBody.generateContacts(q_init,dir_init,acc_init) r(q_init) # Randomly generating a contact configuration at q_end @@ -126,23 +135,37 @@ q_goal[configSize+3:configSize+6] = acc_goal[::] r(q_init) -fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) +#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) +fullBody.setStartState(q_init,[rLegId,lLegId]) fullBody.setEndState(q_goal,[rLegId,lLegId]) +""" + +from hpp.gepetto import PathPlayer +from fullBodyPlayerHrp2 import Player +pp = PathPlayer (fullBody.client.basic, r) +""" + +#~ configs = fullBody.interpolate(0.08,pathId=0,robustnessTreshold = 1, filterStates = False) +#~ configs = fullBody.interpolate(0.04,pathId=0,robustnessTreshold = 1, filterStates = True) +configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 0, filterStates = True) + + + -configs = fullBody.interpolate(0.08,pathId=0,robustnessTreshold = 1, filterStates = True) print "number of configs :", len(configs) -from hpp.gepetto import PathPlayer -pp = PathPlayer (fullBody.client.basic, r) -from fullBodyPlayerHrp2 import Player -player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=True,pathId = 0) +""" +player = Player(fullBody,pp,tp,configs,use_window =1,draw=False,optim_effector=False,use_velocity=True,pathId= 0) -player.displayContactPlan() +#player.displayContactPlan() +player.interpolate(0, 17) +""" +