Update script/manipulation.py to topic/pinocchio branch.

script/manipulation.py

@@ -5,11 +5,13 @@ from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripp
 
 vf.loadEnvironmentModel (Ground, 'ground')
 vf.loadObjectModel (Pokeball, 'pokeball')
-robot.setJointBounds ('pokeball/base_joint_xyz', [-.4,.4,-.4,.4,-.1,1.])
+robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,1.,
+                                              -1.0001, 1.0001,-1.0001, 1.0001,
+                                              -1.0001, 1.0001,-1.0001, 1.0001,])
 
 r = vf.createViewer ()
 
-q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 1, 0, 0, 0]
+q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1]
 r (q1)
 
 ## Create constraint graph
@@ -17,7 +19,7 @@ graph = ConstraintGraph (robot, 'graph')
 
 ## Create constraint of relative position of the ball in the gripper when ball
 ## is grasped
-ballInGripper = [0, 0, 0.055, 0, -sqrt (2)/2, -sqrt (2)/2, 0]
+ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5]
 ps.createTransformationConstraint ('grasp', gripperName, ballName,
                                    ballInGripper, 6*[True,])
 
@@ -33,9 +35,10 @@ graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp')
 
 ## Create transformation constraint : ball is in horizontal plane with free
 ## rotation around z
-ps.createTransformationConstraint ('placement', ballName, '', [0,0,0.025,1,0,0,0], [False, False, True, True, True, False,])
+
+ps.createTransformationConstraint ('placement', '', ballName, [0,0,0.025,0, 0, 0, 1], [False, False, True, True, True, False,])
 #  Create complement constraint
-ps.createTransformationConstraint ('placement/complement', ballName, '', [0,0,0.025,1,0,0,0], [True, True, False, False, False, True,])
+ps.createTransformationConstraint ('placement/complement', '', ballName, [0,0,0.025,0, 0, 0, 1], [True, True, False, False, False, True,])
 
 ps.setConstantRightHandSide ('placement', True)
 ps.setConstantRightHandSide ('placement/complement', False)

script/manipulation/common.py

@@ -37,5 +37,5 @@ robot.client.basic.problem.setMaxIterations (40)
 
 ps = ProblemSolver (robot)
 vf = ViewerFactory (ps)
-gripperName = 'ur5/wrist_3_link-tool0_fixed_joint'
-ballName = 'pokeball/base_joint_SO3'
+gripperName = 'ur5/wrist_3_joint'
+ballName = 'pokeball/root_joint'

script/manipulation_box.py

@@ -5,17 +5,19 @@ from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripp
 
 vf.loadEnvironmentModel (Ground, 'ground')
 vf.loadEnvironmentModel (Box, 'box')
-vf.moveObstacle ('box/base_link_0', [0.3+0.04, 0, 0.04, 1, 0, 0, 0])
-vf.moveObstacle ('box/base_link_1', [0.3-0.04, 0, 0.04, 1, 0, 0, 0])
-vf.moveObstacle ('box/base_link_2', [0.3, 0.04, 0.04, 1, 0, 0, 0])
-vf.moveObstacle ('box/base_link_3', [0.3, -0.04, 0.04, 1, 0, 0, 0])
+vf.moveObstacle ('box/base_link_0', [0.3+0.04, 0, 0.04, 0, 0, 0, 1])
+vf.moveObstacle ('box/base_link_1', [0.3-0.04, 0, 0.04, 0, 0, 0, 1])
+vf.moveObstacle ('box/base_link_2', [0.3, 0.04, 0.04, 0, 0, 0, 1])
+vf.moveObstacle ('box/base_link_3', [0.3, -0.04, 0.04, 0, 0, 0, 1])
 
 vf.loadObjectModel (Pokeball, 'pokeball')
-robot.setJointBounds ('pokeball/base_joint_xyz', [-.4,.4,-.4,.4,-.1,1.])
+robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,1.,
+                                              -1.0001, 1.0001,-1.0001, 1.0001,
+                                              -1.0001, 1.0001,-1.0001, 1.0001,])
 
 r = vf.createViewer ()
 
-q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 1, 0, 0, 0]
+q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1]
 r (q1)
 
 ## Create graph
@@ -31,7 +33,7 @@ res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
 
 ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
-ps.selectPathValidation ("Dichotomy", 0)
+ps.selectPathValidation ("Discretized", 0.01)
 ps.selectPathProjector ("Progressive", 0.1)
 
 pp = PathPlayer (ps.client.basic, r)