Commit 9b77c6ed authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Merge remote-tracking branch 'origin/joseph'

parents 5fed9624 aabfe903
Subproject commit e31b581cd45f102d2b9b28e182f096e4b51696a7 Subproject commit 59b1f75fc529cc5474c4154d29b1a7e28f1feee3
...@@ -22,8 +22,7 @@ balanceConstraints = [constraintName + "/relative-com", ...@@ -22,8 +22,7 @@ balanceConstraints = [constraintName + "/relative-com",
constraintName + "/orientation-left-foot", constraintName + "/orientation-left-foot",
constraintName + "/position-left-foot"] constraintName + "/position-left-foot"]
for a in ["x","y","z"]: robot.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
robot.setJointBounds ("base_joint_"+a, [-4,4])
def testConstraint (constraints, nbIter = 100): def testConstraint (constraints, nbIter = 100):
success = 0 success = 0
......
# vim: foldmethod=marker foldlevel=2 # vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.pr2 import Robot from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver
from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer
from math import sqrt from math import sqrt
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3 # Load robot and object. {{{3
robot = Robot ('pr2', rootJointType = "anchor") robot = Robot ('pr2-box', 'pr2', rootJointType = "anchor")
ps = ProblemSolver (robot)
r = ViewerFactory (ps)
robot.client.manipulation.robot.setRootJointPosition ('pr2', (-3.2, -4, 0, 1, 0, 0, 0)) robot.client.manipulation.robot.setRootJointPosition ('pr2', (-3.2, -4, 0, 1, 0, 0, 0))
robot.loadObjectModel ('box', 'freeflyer', 'hpp_tutorial', 'box', '', '') r.loadObjectModel (Box, 'box')
robot.buildCompositeRobot ('pr2-box', ['pr2', 'box']) r.buildCompositeRobot (['pr2', 'box'])
robot.client.manipulation.robot.loadEnvironmentModel ("iai_maps", "kitchen_area", '', '', "") r.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("box/base_joint_xyz", [-3,-2,-5,-3,0.7,1]) robot.setJointBounds ("box/base_joint_xyz", [-3,-2,-5,-3,0.7,1])
# 3}}} # 3}}}
from hpp_ros.manipulation import ScenePublisher
r = ScenePublisher (robot)
# Define configurations. {{{3 # Define configurations. {{{3
q_init = robot.getCurrentConfig () q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint'] rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint']
...@@ -33,13 +51,11 @@ q_init [rank:rank+3] = [-2.5, -3.6, 0.76] ...@@ -33,13 +51,11 @@ q_init [rank:rank+3] = [-2.5, -3.6, 0.76]
rank = robot.rankInConfiguration ['box/base_joint_SO3'] rank = robot.rankInConfiguration ['box/base_joint_SO3']
c = sqrt (2) / 2 c = sqrt (2) / 2
q_init [rank:rank+4] = [c, 0, c, 0] q_init [rank:rank+4] = [c, 0, c, 0]
r (q_init)
rank = robot.rankInConfiguration ['box/base_joint_xyz'] rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_goal [rank:rank+3] = [-2.5, -4.4, 0.76] q_goal [rank:rank+3] = [-2.5, -4.4, 0.76]
rank = robot.rankInConfiguration ['box/base_joint_SO3'] rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_goal [rank:rank+4] = [c, 0, -c, 0] q_goal [rank:rank+4] = [c, 0, -c, 0]
r (q_goal)
del c del c
q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836] q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836]
...@@ -49,16 +65,14 @@ robot.client.basic.problem.resetRoadmap () ...@@ -49,16 +65,14 @@ robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None') robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40) robot.client.basic.problem.setMaxIterations (40)
ps.selectPathProjector ('Progressive', 0.2)
from hpp.corbaserver.manipulation import ProblemSolver
p = ProblemSolver (robot)
# Create constraints. {{{3 # Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph') cg = ConstraintGraph (robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint ( robot.client.manipulation.problem.createPlacementConstraint (
'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top') 'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'kitchen_area/pancake_table_table_top')
jointNames = dict () jointNames = dict ()
jointNames['all'] = robot.getJointNames () jointNames['all'] = robot.getJointNames ()
...@@ -69,23 +83,24 @@ for n in jointNames['all']: ...@@ -69,23 +83,24 @@ for n in jointNames['all']:
jointNames['pr2'].append (n) jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"): if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n) jointNames['allButPR2LeftArm'].append (n)
ps.addPassiveDofs ('pr2', jointNames ['pr2'])
cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', jointNames['pr2']) cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', 'pr2')
cg.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2', jointNames['pr2']) cg.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2', 'pr2')
cg.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle') cg.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2') cg.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2')
lockbox = p.lockFreeFlyerJoint ('box/base_joint', 'box_lock', parametric = True) lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock', compType = 'Equality')
locklhand = ['l_l_finger','l_r_finger']; locklhand = ['l_l_finger','l_r_finger'];
p.createLockedDofConstraint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', 0.5, 0, 0) ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5])
p.createLockedDofConstraint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', 0.5, 0, 0) ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5])
lockrhand = ['r_l_finger','r_r_finger']; lockrhand = ['r_l_finger','r_r_finger'];
p.createLockedDofConstraint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', 0.5, 0, 0) ps.createLockedJoint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', [0.5])
p.createLockedDofConstraint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', 0.5, 0, 0) ps.createLockedJoint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', [0.5])
lockhands = ['l_l_finger','l_r_finger','r_l_finger','r_r_finger']; lockhands = ['l_l_finger','l_r_finger','r_l_finger','r_r_finger'];
...@@ -173,13 +188,13 @@ cg.client.graph.setLevelSetConstraints (cg.edges['r_keep_grasp_ls'], [], lockbox ...@@ -173,13 +188,13 @@ cg.client.graph.setLevelSetConstraints (cg.edges['r_keep_grasp_ls'], [], lockbox
cg.setConstraints (graph = True, lockDof = lockhands) cg.setConstraints (graph = True, lockDof = lockhands)
# 3}}} # 3}}}
res = p.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init) res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]: if not res[0]:
raise Exception ('Init configuration could not be projected.') raise Exception ('Init configuration could not be projected.')
q_init_proj = res [1] q_init_proj = res [1]
res = p.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal) res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
if not res[0]: if not res[0]:
raise Exception ('Goal configuration could not be projected.') raise Exception ('Goal configuration could not be projected.')
q_goal_proj = res [1] q_goal_proj = res [1]
p.setInitialConfig (q_init_proj) ps.setInitialConfig (q_init_proj)
p.addGoalConfig (q_goal_proj) ps.addGoalConfig (q_goal_proj)
...@@ -60,19 +60,20 @@ for n in jointNames['all']: ...@@ -60,19 +60,20 @@ for n in jointNames['all']:
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', jointNames ['hrp2']) graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', jointNames ['hrp2'])
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2') graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock') lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock',
compType = 'Equality')
locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4'] locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
ps.createLockedJointConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],]) ps.createLockedJoint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
ps.createLockedJointConstraint \ ps.createLockedJoint \
('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],]) ('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
ps.createLockedJointConstraint \ ps.createLockedJoint \
('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],]) ('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJointConstraint \ ps.createLockedJoint \
('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],]) ('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJointConstraint \ ps.createLockedJoint \
('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],]) ('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJointConstraint \ ps.createLockedJoint \
('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],]) ('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
ps.createStaticStabilityConstraints ("balance", q_init) ps.createStaticStabilityConstraints ("balance", q_init)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment