def makeRobotProblemAndViewerFactory(corbaclient):
robot = Robot("dev",
"talos",
rootJointType="freeflyer",
client=corbaclient)
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver(robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory(ps)
vf.loadObjectModel(Object, "box")
robot.setJointBounds("box/root_joint", [-1, 1, -1, 1, 0, 2])
vf.loadEnvironmentModel(Table, "table")
return robot, ps, vf
0.20387672048126043, 0.9007626913161502, -0.39038645767349395,
0.31725226129015516, 1.5475253831101246, -0.0104572058777634,
0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06,
1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969,
0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0
]
q_init = q_goal[::]
q_init[r + 3:r + 7] = [0, 0, 1, 0]
n = 'romeo/l_hand grasps placard/low'
res, q_init, err = cg.applyNodeConstraints(n, q_init)
if not res: raise RuntimeError("Failed to project initial configuration.")
res, q_goal, err = cg.applyNodeConstraints(n, q_goal)
if not res: raise RuntimeError("Failed to project initial configuration.")
ps.selectPathProjector("Progressive", .05)
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
N = 20
for i in range(N):
ps.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
t1 = dt.datetime.now()
ps.solve()
t2 = dt.datetime.now()
totalTime += t2 - t1
print(t2 - t1)
#q_init [rank:rank+4] = [c, 0, c, 0]
q_init[rank:rank + 7] = [-2.5, -3.6, 0.76, 0, c, 0, c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
rank = robot.rankInConfiguration['box/root_joint']
q_goal[rank:rank + 7] = [-2.5, -4.4, 0.76, 0, -c, 0, c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
#q_goal [rank:rank+4] = [c, 0, -c, 0]
del c
# 3}}}
robot.client.basic.problem.resetRoadmap()
robot.client.basic.problem.setErrorThreshold(1e-3)
robot.client.basic.problem.setMaxIterProjection(40)
ps.selectPathProjector('Progressive', 0.2)
# Create constraints. {{{3
robot.client.manipulation.problem.createPlacementConstraint \
('box_placement', ['box/box_surface',],
['kitchen_area/pancake_table_table_top',])
jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['pr2'] = list()
jointNames['allButPR2LeftArm'] = list()
for n in jointNames['all']:
if n.startswith("pr2"):
jointNames['pr2'].append(n)
if not n.startswith("pr2/l_gripper"):
rank = robot.rankInConfiguration ['box1/base_joint_xyz']
q1 = q_init[:]
q1[rank:rank+7] = [ -0.14391940018238783, 1.092383723903555, 0.7460100959532432, 0.44542854851747, 0.5489390643072047, 0.4452361192941091, -0.5495672023684386]
q2 = q0[:]
q2[rank:rank+3] = [ -0.545, 0.268, 0.7460100959532432]
rank = robot.rankInConfiguration ['box2/base_joint_xyz']
q1[rank:rank+7] = [ -0.14391940018238783, 1.092383723903555, 0.7460100959532432, 0.44542854851747, 0.5489390643072047, 0.4452361192941091, -0.5495672023684386]
q2[rank:rank+3] = [ -0.445, 0.268, 0.7460100959532432]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (20)
#ps.selectPathValidation ('Graph-Discretized', 0.05)
ps.selectPathProjector ('Progressive', 0.1)
# ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['baxter'] = list ()
jointNames['baxterRightSide'] = list ()
jointNames['baxterLeftSide'] = list ()
jointNames['box1'] = list ()
jointNames['box2'] = list ()
for n in jointNames['all']:
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'cup'
urdfSuffix = ""
srdfSuffix = ""
handles = [ "box/top", "box/bottom" ]
robot = Robot ('dev', 'talos', rootJointType = "freeflyer")
robot. leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds ("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver (robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterProjection (40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory (ps)
Object = Box
vf.loadObjectModel (Object, 'box')
robot.setJointBounds ("box/root_joint", [-1, 1, -1, 1, 0, 2])
qq = [-0.7671778026566639, 0.0073267002287253635, 1.0035168727631776, -0.003341673452654457, -0.021566597515109524, 0.0002183620894239602, 0.9997618053357284, -0.00020053128587844821, -0.0021365695604276275, -0.4415951773681094, 0.9659230706255528, -0.48119003672520416, 0.007109157982067145, -0.00020095991543181877, -0.002126639473414498, -0.4382848597339842, 0.9589221865248464, -0.4774994711722908, 0.007099218648561522, 0.0, 0.025235347910697536, -0.06985947194357875, 0.0, -0.12446173084176845, -1.5808415926365578, 0.014333078135875619, -0.0806417043955706, -0.37124401660668394, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25955282922987977, -0.1618313202464181, 0.002447883426630002, -0.5149037074691503, -0.00010703274362664899, 0.0008742582163227642, 0.10168585913285667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.785398163397, 0.32250041955468695, -0.2569883469655496, 0.18577095561452217, 1.164388709412583, 0.0694401264431558, 0.5475575114527793, -0.11842286843715424, 0.8254301089264399]
half_sitting = [
-0.74,0,1.0192720229567027,0,0,0,1, # root_joint
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left
cg.addConstraints (graph = True, constraints = commonConstraints)
cg.initialize ()
# Define initial and final configurations
q_goal = [-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]
q_init = q_goal [::]
q_init [r+3:r+7] = [0, 0, 1, 0]
n = 'romeo/l_hand grasps placard/low'
res, q_init, err = cg.applyNodeConstraints (n, q_init)
if not res: raise RuntimeError ("Failed to project initial configuration.")
res, q_goal, err = cg.applyNodeConstraints (n, q_goal)
if not res: raise RuntimeError ("Failed to project initial configuration.")
ps.selectPathProjector ("Progressive", .05)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
robot.setCurrentConfig (robot.getInitialConfig ())
q_init = robot.getHandConfig ("both", "open")
rank = robot.rankInConfiguration ['romeo/base_joint_xyz']
q_init [rank:rank+7] = [-3.5,-3.7, 0.877, 1, 0, 0, 0]
rank = robot.rankInConfiguration ['cup/base_joint_xyz']
q_init [rank:rank+7] = [-4.8, -4.6, 0.91,0,sqrt(2)/2,sqrt(2)/2,0]
q_goal = q_init [::]
q_goal [rank:rank+7] = [-4.8, -3.35, 0.9, 0,sqrt(2)/2,sqrt(2)/2,0]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
# ps.selectPathProjector ('Progressive', 0.2)
ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['romeo'] = list ()
jointNames['romeoWithoutLeftArm'] = list ()
jointNames['romeoWithoutRightArm'] = list ()
jointNames['kitchen_area'] = list ()
jointNames['cup'] = list ()
for n in jointNames['all']:
if n.startswith ("romeo"):
robot.setCurrentConfig(robot.getInitialConfig())
q_init = robot.getHandConfig("both", "open")
rank = robot.rankInConfiguration['romeo/base_joint_xyz']
q_init[rank:rank + 7] = [-3.5, -3.7, 0.877, 1, 0, 0, 0]
rank = robot.rankInConfiguration['cup/base_joint_xyz']
q_init[rank:rank + 7] = [-4.8, -4.6, 0.91, 0, sqrt(2) / 2, sqrt(2) / 2, 0]
q_goal = q_init[::]
q_goal[rank:rank + 7] = [-4.8, -3.35, 0.9, 0, sqrt(2) / 2, sqrt(2) / 2, 0]
# 3}}}
robot.client.basic.problem.resetRoadmap()
robot.client.basic.problem.setErrorThreshold(1e-3)
robot.client.basic.problem.setMaxIterations(40)
# ps.selectPathProjector ('Progressive', 0.2)
ps.selectPathProjector('Global', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph(robot, 'graph')
# Create passive DOF lists {{{4
jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['romeo'] = list()
jointNames['romeoWithoutLeftArm'] = list()
jointNames['romeoWithoutRightArm'] = list()
jointNames['kitchen_area'] = list()
jointNames['cup'] = list()
for n in jointNames['all']:
if n.startswith("romeo"):
urdfSuffix = ""
srdfSuffix = ""
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_manipulation'
# Load HRP2 and a screwgun {{{3
robot = Robot ('hrp2-screw', 'hrp2')
ps = ProblemSolver (robot)
ps.selectPathPlanner ("M-RRT")
vf = ViewerFactory (ps)
vf.loadObjectModel (ScrewGun, 'screw_gun')
for d in ["hrp2", "screw_gun"]:
robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
ps.selectPathProjector ("Progressive", 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::]
# Set initial position of screw-driver
# q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0.5, -0.5, 0]
# Open left hand
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
q_goal = q_init [::]
