Merge remote-tracking branch 'origin/joseph'

cmake @ 59b1f75f

-Subproject commit e31b581cd45f102d2b9b28e182f096e4b51696a7
+Subproject commit 59b1f75fc529cc5474c4154d29b1a7e28f1feee3

script/test_constraints.py → script/balance_constraints_success_statistics.py

@@ -22,8 +22,7 @@ balanceConstraints = [constraintName + "/relative-com",
                       constraintName + "/orientation-left-foot",
                       constraintName + "/position-left-foot"]
 
-for a in ["x","y","z"]:
-  robot.setJointBounds ("base_joint_"+a, [-4,4])
+robot.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
 
 def testConstraint (constraints, nbIter = 100):
   success = 0

script/test_graph_two_hands.py

 # vim: foldmethod=marker foldlevel=2
 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
 
+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
-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.loadObjectModel ('box', 'freeflyer', 'hpp_tutorial', 'box', '', '')
-robot.buildCompositeRobot ('pr2-box', ['pr2', 'box'])
-robot.client.manipulation.robot.loadEnvironmentModel ("iai_maps", "kitchen_area", '', '', "")
+r.loadObjectModel (Box, 'box')
+r.buildCompositeRobot (['pr2', 'box'])
+r.loadEnvironmentModel (Environment, "kitchen_area")
 robot.setJointBounds ("box/base_joint_xyz", [-3,-2,-5,-3,0.7,1])
 # 3}}}
 
-from hpp_ros.manipulation import ScenePublisher
-r = ScenePublisher (robot)
-
 # Define configurations. {{{3
 q_init = robot.getCurrentConfig ()
 rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint']
@@ -33,13 +51,11 @@ q_init [rank:rank+3] = [-2.5, -3.6, 0.76]
 rank = robot.rankInConfiguration ['box/base_joint_SO3']
 c = sqrt (2) / 2
 q_init [rank:rank+4] = [c, 0, c, 0]
-r (q_init)
 
 rank = robot.rankInConfiguration ['box/base_joint_xyz']
 q_goal [rank:rank+3] = [-2.5, -4.4, 0.76]
 rank = robot.rankInConfiguration ['box/base_joint_SO3']
 q_goal [rank:rank+4] = [c, 0, -c, 0]
-r (q_goal)
 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]
@@ -49,16 +65,14 @@ robot.client.basic.problem.resetRoadmap ()
 robot.client.basic.problem.selectPathOptimizer ('None')
 robot.client.basic.problem.setErrorThreshold (1e-3)
 robot.client.basic.problem.setMaxIterations (40)
-
-from hpp.corbaserver.manipulation import ProblemSolver
-p = ProblemSolver (robot)
+ps.selectPathProjector ('Progressive', 0.2)
 
 # Create constraints. {{{3
 from hpp.corbaserver.manipulation import ConstraintGraph
 cg = ConstraintGraph (robot, 'graph')
 
 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['all'] = robot.getJointNames ()
@@ -69,23 +83,24 @@ for n in jointNames['all']:
     jointNames['pr2'].append (n)
   if not n.startswith ("pr2/l_gripper"):
     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 ('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'];
-p.createLockedDofConstraint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', 0.5, 0, 0)
-p.createLockedDofConstraint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', 0.5, 0, 0)
+ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5])
+ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5])
 
 lockrhand = ['r_l_finger','r_r_finger'];
-p.createLockedDofConstraint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', 0.5, 0, 0)
-p.createLockedDofConstraint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', 0.5, 0, 0)
+ps.createLockedJoint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', [0.5])
+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'];
 
@@ -173,13 +188,13 @@ cg.client.graph.setLevelSetConstraints (cg.edges['r_keep_grasp_ls'], [], lockbox
 cg.setConstraints (graph = True, lockDof = lockhands)
 # 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]:
   raise Exception ('Init configuration could not be projected.')
 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]:
   raise Exception ('Goal configuration could not be projected.')
 q_goal_proj = res [1]
-p.setInitialConfig (q_init_proj)
-p.addGoalConfig (q_goal_proj)
+ps.setInitialConfig (q_init_proj)
+ps.addGoalConfig (q_goal_proj)

script/test_simple_graph.py

@@ -60,19 +60,20 @@ for n in jointNames['all']:
 graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', jointNames ['hrp2'])
 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']
-ps.createLockedJointConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
-ps.createLockedJointConstraint \
+ps.createLockedJoint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
+ps.createLockedJoint \
     ('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
-ps.createLockedJointConstraint \
+ps.createLockedJoint \
     ('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
-ps.createLockedJointConstraint \
+ps.createLockedJoint \
     ('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
-ps.createLockedJointConstraint \
+ps.createLockedJoint \
     ('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
-ps.createLockedJointConstraint \
+ps.createLockedJoint \
     ('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
 
 ps.createStaticStabilityConstraints ("balance", q_init)