test_simple_graph.py 3.9 KB
Newer Older
1
# vim: foldmethod=marker foldlevel=2
2
from hpp.corbaserver.manipulation.hrp2 import Robot
3
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
4
from hpp.gepetto.manipulation import Viewer
5

6
7
8
9
10
11
12
13
14
class ScrewGun (object):
  rootJointType = 'freeflyer'
  packageName = 'airbus_environment'
  meshPackageName = 'airbus_environment'
  urdfName = 'screw_gun'
  urdfSuffix = "_massless"
  srdfSuffix = ""

Robot.urdfSuffix = ''
15
Robot.srdfSuffix = '_manipulation'
16

17
# Load HRP2 and a screwgun {{{3
18
robot = Robot ('hrp2-screw', 'hrp2')
19
robot.client.basic.problem.selectPathPlanner ("M-RRT")
20
21
22
r = Viewer (robot)
r.loadObjectModel (ScrewGun, 'screw_gun')
r.buildCompositeRobot (['hrp2', 'screw_gun'])
23
for d in ["hrp2", "screw_gun"]:
24
  robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
25

26
27
28
29
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
30
# 3}}}
31

32
# Define configurations {{{3
33
34
35
36
37
38
39
40
41
42
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]
# 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 [::]
q_goal [-7:] = [2, -1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
43

44
# 3}}}
45

46
# Generate constraints {{{3
47
p = ProblemSolver (robot)
48
graph = ConstraintGraph (robot, 'graph')
49
50
51
52
53
54
55
56
57
58
59
60


jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['hrp2'] = list ()
jointNames['allButHRP2LeftArm'] = list ()
for n in jointNames['all']:
  if n.startswith ("hrp2"):
    jointNames['hrp2'].append (n)
  if not n.startswith ("hrp2/LARM"):
    jointNames['allButHRP2LeftArm'].append (n)

61
62
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', jointNames ['hrp2'])
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
63

64
lockscrewgun = p.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock', parametric = True)
65

66
locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
67
68
69
70
71
72
73
74
p.createLockedDofConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , q_init [ilh], 0, 0)
p.createLockedDofConstraint ('lhand_0', 'hrp2/LHAND_JOINT0', q_init [ilh+1], 0, 0)
p.createLockedDofConstraint ('lhand_1', 'hrp2/LHAND_JOINT1', q_init [ilh+2], 0, 0)
p.createLockedDofConstraint ('lhand_2', 'hrp2/LHAND_JOINT2', q_init [ilh+3], 0, 0)
p.createLockedDofConstraint ('lhand_3', 'hrp2/LHAND_JOINT3', q_init [ilh+4], 0, 0)
p.createLockedDofConstraint ('lhand_4', 'hrp2/LHAND_JOINT4', q_init [ilh+5], 0, 0)

p.createStaticStabilityConstraints ("balance", q_init)
75
76
77
78
# 3}}}

# Create the graph of constraints {{{3
graph.createNode (["screwgun", "free"])
79

80
graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1)
81

82
graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5)
83

84
graph.createEdge ('free', 'free', 'move_free', 5)
85

86
87
88
89
90
91
92
93
94
95
96
97
98
99
graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10)
graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5)

graph.setConstraints (node='screwgun', grasp='l_grasp')
graph.setConstraints (edge='move_free', lockDof = lockscrewgun)
graph.setConstraints (edge='ungrasp_e0', lockDof = lockscrewgun)
graph.setConstraints (node='ungrasp_n0', pregrasp = 'l_pregrasp')
graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun)
graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun)
graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp')
graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun)
graph.client.graph.setLevelSetConstraints (graph.edges['keep_grasp_ls'], [], lockscrewgun)
graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=p.balanceConstraints ())
# 3}}}
100

101
102
103
104
105
p.setInitialConfig (q_init)
p.addGoalConfig (q_goal)

from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client.basic, r)