diff --git a/script/tutorial_3.py b/script/tutorial_3.py index d2b206c0b8ac91f32b159070c0cb166ff1e6983c..735307f9165137719dd188803402f7b40aa14ce8 100644 --- a/script/tutorial_3.py +++ b/script/tutorial_3.py @@ -1,6 +1,6 @@ # Import libraries and load robots. {{{1 -# Import. {{{2 +# Import. from math import sqrt from hpp.gepetto import PathPlayer from hpp.corbaserver.manipulation.pr2 import Robot @@ -10,9 +10,8 @@ from hpp.gepetto.manipulation import ViewerFactory from hpp.corbaserver import loadServerPlugin loadServerPlugin ("corbaserver", "manipulation-corba.so") Client ().problem.resetProblem () -# 2}}} -# Load PR2 and a box to be manipulated. {{{2 +# Load PR2 and a box to be manipulated. class Box (object): rootJointType = 'freeflyer' packageName = 'hpp_tutorial' @@ -39,19 +38,15 @@ vf.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("pr2/root_joint", [-5,-2,-5.2,-2.7] ) robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5]) -# 2}}} -# 1}}} +# Initialization. -# Initialization. {{{1 - -# Set parameters. {{{2 +# Set parameters. # robot.client.basic.problem.resetRoadmap () ps.setErrorThreshold (1e-3) ps.setMaxIterProjection (40) -# 2}}} -# Generate initial and goal configuration. {{{2 +# Generate initial and goal configuration. q_init = robot.getCurrentConfig () rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint'] q_init [rank] = 0.5 @@ -70,15 +65,13 @@ q_goal = q_init [::] q_goal [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['box/root_joint'] q_goal [rank:rank+3] = [-2.5, -4.5, 0.746] -# 2}}} -# Create the constraints. {{{2 +# Create the constraints. locklhand = ['l_l_finger','l_r_finger']; 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,]) -# 2}}} -# Create the constraint graph. {{{2 +# Create the constraint graph. # Define the set of grippers used for manipulation grippers = [ "pr2/l_gripper", ] # Define the set of objects that can be manipulated @@ -104,20 +97,17 @@ cg.addConstraints (graph = True, constraints = Constraints \ (numConstraints = locklhand)) cg.initialize () -# 2}}} - ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) -# print ps.solve() +# uncomment to solve +# ps.solve() -# ps.setTargetState (cg.nodes["pr2/l_gripper grasps box/handle2"]) -# print ps.solve() - -# 1}}} +# Path optimization uncomment to optimize +# +# ps.loadPlugin('manipulation-spline-gradient-based.so') +# ps.addPathOptimizer('SplineGradientBased_bezier1') +# ps.optimizePath(0) +# display in gepetto-gui # v = vf.createViewer () -# v (q_init_proj) -# pp = PathPlayer (v) - -# vim: foldmethod=marker foldlevel=1 diff --git a/srdf/box.srdf b/srdf/box.srdf index f8b2218f111d3bf48c8fb7a541ae62f0b13e4f4f..42553e4272b4101f00b52fc45f727abacfb10a71 100644 --- a/srdf/box.srdf +++ b/srdf/box.srdf @@ -1,12 +1,12 @@ <?xml version="1.0"?> <robot name="box"> <handle name="handle" clearance="0.025"> - <position> 0 0 0 1 0 0 0 </position> + <position xyz="0 0 0" wxyz="1 0 0 0" /> <link name="base_link"/> </handle> <handle name="handle2" clearance="0.025"> - <position> 0 0 0 - 0 0 0.7071067811865476 0.7071067811865476 </position> + <position xyz="0 0 0" + wxyz="0 0 0.7071067811865476 0.7071067811865476" /> <link name="base_link"/> </handle> <contact name="box_surface">