Commit 5d5c74a4 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

cleaning script files - still need to clean meshes

parent b211682c
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk'
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", [-1, 1, -0.5, 0.5, 0, 5])
rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_goal = rbprmBuilder.getCurrentConfig ();
#~ rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init = rbprmBuilder.getCurrentConfig ();
#~ q_init = [-0.15, -0.2, 0.6, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]
q_init = [-0.3, -0.2, 0.6, 1, 0.0, 0., 0.0]; r (q_init)
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_goal = [0, 0, 2, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]; r (q_goal)
#~ q_goal = [0.2, -0.2, 1.6, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]; r (q_goal)
q_goal = [0.1, -0.2, 1.6, 0.98877107793604235, 0.0, 0.14943813247359924, 0.0]
ps.addPathOptimizer("RandomShortcut")
#~ ps.addPathOptimizer("GradientBased")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.1)
r.loadObstacleModel (packageName, "chair", "planning")
ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (0)
pp (1)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
#!/usr/bin/env python
import pygame
import time
pygame.init()
pygame.joystick.init()
pygame.event.pump()
if (pygame.joystick.get_count() > 0):
print "found gamepad! : " + pygame.joystick.Joystick(0).get_name()
my_joystick = pygame.joystick.Joystick(0)
my_joystick.init()
for i in range(100):
pygame.event.pump()
time.sleep(0.1)
x=my_joystick.get_axis(0)
print x
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'box'
urdfNameRom = 'box_rom'
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, -1, 1.5])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:3] = [0, -0.5, 0.3]
q_goal = [0, -0.5, -0.2, -0.501544,0.431183, 0.662926, -0.350804]
r (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "scene", "car")
ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~
pp (0)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
packageName = "hpp_tutorial"
meshPackageName = "pr2_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "pr2"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1, 2, -2, 0, -1, 1.5])
fullBody.addLimb("r_shoulder_pan_joint", 5000, 0.001)
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
q_init = fullBody.getCurrentConfig ()
q_init [0:3] = [-0.6, -0.5, -0.4]
r (q_init)
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,1,0])
r (q_init)
fullBody.getContactSamplesIds(rLeg, q_init, [0,1,0])
#~ q_init = fullBody.getSample('r_shoulder_pan_joint', 1)
#~ r (q_init)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
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", [-5, 5, -5, 5, -5, 5])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
#~ AFTER loading obstacles
rLegId = '1rLeg'
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, 20000, 0.01)
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, 20000, 0.01)
fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
fullBody.client.basic.robot.setJointConfig('LLEG_JOINT3',[1.5])
fullBody.client.basic.robot.setJointConfig('RLEG_JOINT3',[1.5])
q_init = fullBody.getCurrentConfig ()
q_init [0:3] = [0, -0.5, 0.6]
r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [1, -0.5, 0.6]
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
r(q_goal)
ps.solve ()
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,-1])
r (q_init)
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,-1])
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
#~
pp (0)
fullBody.setStartState(q_init,[rLegId,lLegId])
fullBody.setEndState(q_goal,[rLegId])
configs = fullBody.interpolate(0.1)
i = 1;
r (configs[i]); i=i+1; i-1
#~ pp.toFile(0, "/home/stonneau/test.txt")
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import climb_path_hrp2 as tp
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,4, -1, 1, 1, 2.5])
fullBody.setJointBounds ("base_joint_xyz", [-1, 1, -0.5, 0.5, 0, 5])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "climb", "contact")
#~ AFTER loading obstacles
rLegId = '7rLeg'
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, 20000, 0.1)
lLegId = '8lLeg'
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, 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, 0.01)
#~ 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, 20000, 0.01)
q_0 = fullBody.getCurrentConfig (); r (q_0)
q_init = fullBody.getCurrentConfig (); r (q_init)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
q_init = fullBody.generateContacts(q_init, [0,0,-1])
r (q_init)
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,-1])
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
#~
configs = fullBody.interpolate(0.1)
#~ configs2 = fullBody.interpolate(0.05)
i = 0;
r (configs[i]); i=i+1; i-1
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
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", [-5, 5, -5, 5, -5, 5])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
#~ AFTER loading obstacles
rLegId = '7rLeg'
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, 20000, 0.01)
lLegId = '8lLeg'
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, 20000, 0.01)
rarmId = '3RKarm'
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, 5000, 0.01)
#~ AFTER loading obstacles
larmId = '4RKarm'
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, 5000, 0.01)
rLegId = '5RKnee'
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(rLegId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 5000, 0.01)
lLegId = '6LKnee'
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(lLegId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 5000, 0.01)
#~
q_0 = fullBody.getCurrentConfig (); r (q_0)
fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[1])
fullBody.client.basic.robot.setJointConfig('LLEG_JOINT3',[1.5])
fullBody.client.basic.robot.setJointConfig('RLEG_JOINT3',[1.5])
#~
fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
q_init = fullBody.getCurrentConfig (); r (q_init)
q_init [0:3] = [0, -0.5, 0.3]; fullBody.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [0.4, -0.5, 0.3]
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
r(q_goal)
ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
#~ pp (0)
q_init = fullBody.generateContacts(q_init, [0,0,-1])
r (q_init)
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,-1])
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
#~
configs = fullBody.interpolate(0.05)
i = 0;
r (configs[i]); i=i+1; i-1
#~ configs = fullBody.getContactSamplesIds(rLeg, q_init, [0,1,0])
#~ i = 0
#~ q_init = fullBody.getSample(rLeg, int(configs[i])); i = i+1;r(q_init)
#~
#~ fullBody.setCurrentConfig (q_init)
#~ q_init = fullBody.generateContacts(q_init, [-0.1,0,1]) ; r(q_init)
#~ r (q_init)
#~
#~ q_goal = q_init [::]
#~ q_goal [0:3] = [1, -0.5, 0.6]
#~ r (q_0)
#~ fullBody.setCurrentConfig (q_0)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT3',[1.5])
#~ fullBody.client.basic.robot.setJointConfig('RLEG_JOINT3',[1.5])
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
#~ q_init = fullBody.getCurrentConfig (); r (q_init)
#~ q_init [0:3] = [0, -0.5, 0.2]; fullBody.setCurrentConfig (q_init); r (q_init)
#~ q_init = fullBody.generateContacts(q_init, [-0.1,0,1]) ; r(q_init)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
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", [-5, 5, -5, 5, -5, 5])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( fullBody )
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("GradientBased")
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "scene_wall", "car")
#~ AFTER loading obstacles
rLegId = '7rLeg'
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, 20000, 0.01)
lLegId = '8lLeg'
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, 20000, 0.01)
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, 0.01)
#~ 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, 20000, 0.01)
#~ rLegId = '5RKnee'
#~ 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(rLegId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 5000, 0.01)
#~ lLegId = '6LKnee'
#~ 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(lLegId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 5000, 0.01)
#~
q_0 = fullBody.getCurrentConfig (); r (q_0)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT3',[1.5])
#~ fullBody.client.basic.robot.setJointConfig('RLEG_JOINT3',[1.5])
#~
fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
q_init = fullBody.getCurrentConfig (); r (q_init)
q_init [0:3] = [-0.1, 0, 0.3]; fullBody.setCurrentConfig (q_init); r (q_init)
q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0)
q_goal = q_init [::]
#~ q_goal [0:3] = [0.2, -0.5, 0.3]
q_goal = [-0.58,
0.0,
0.6,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.8165569353186153,
0.01904481617296847,
0.5757001393867748,
0.0349066,
-0.5759795881447421,
-0.8458895038625474,
0.0,
0.999644764477483,
-0.27941899714496077,
0.7887881502725462,
-0.3842797425576481,
-0.8266261474212095,
-0.7322957359271716,
0.0,
-0.06084265166231969,
5.531954151015379e-05,
0.07882462690145475,
0.9267587094966957,
0.5652483098600809,
-0.06099150171806746,
-0.2825554499271455,
0.0026828522588598323,
0.07421013928536811,
0.9328970238734515,
0.5644449731454055,
-0.2825451621281345]